Tesseract
Motion Planning Environment
Loading...
Searching...
No Matches
iiwa7_ikfast_solver.hpp
Go to the documentation of this file.
1// clang-format off
2
23#include <tesseract_common/macros.h>
24TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
25#ifndef IKFAST_HAS_LIBRARY
26#define IKFAST_HAS_LIBRARY
27#define IKFAST_NO_MAIN
28#endif
29#include <tesseract_kinematics/ikfast/external/ikfast.h> // found inside share/openrave-X.Y/python/ikfast.h
30using namespace ikfast;
31
32// check if the included ikfast version matches what this file was compiled with
33#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
34IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 61);
35
36#include <cmath>
37#include <vector>
38#include <limits>
39#include <algorithm>
40#include <complex>
41
42#ifndef IKFAST_ASSERT
43#include <stdexcept>
44#include <sstream>
45#include <iostream>
46
47#ifdef _MSC_VER
48#ifndef __PRETTY_FUNCTION__
49#define __PRETTY_FUNCTION__ __FUNCDNAME__
50#endif
51#endif
52
53#ifndef __PRETTY_FUNCTION__
54#define __PRETTY_FUNCTION__ __func__
55#endif
56
57#define IKFAST_ASSERT(b) \
58 { \
59 if (!(b)) \
60 { \
61 std::stringstream ss; \
62 ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " << __PRETTY_FUNCTION__ << ": Assertion '" \
63 << #b << "' failed"; \
64 throw std::runtime_error(ss.str()); \
65 } \
66 }
67
68#endif
69
70#if defined(_MSC_VER)
71#define IKFAST_ALIGNED16(x) __declspec(align(16)) x
72#else
73#define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
74#endif
75
76#define IK2PI ((IkReal)6.28318530717959)
77#define IKPI ((IkReal)3.14159265358979)
78#define IKPI_2 ((IkReal)1.57079632679490)
79
80#ifdef _MSC_VER
81#ifndef isnan
82#define isnan _isnan
83#endif
84#ifndef isinf
85#define isinf _isinf
86#endif
87//#ifndef isfinite
88//#define isfinite _isfinite
89//#endif
90#endif // _MSC_VER
91
92// lapack routines
93extern "C" {
94void dgetrf_(const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
95void zgetrf_(const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
96void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
97void dgesv_(const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
98void dgetrs_(const char* trans,
99 const int* n,
100 const int* nrhs,
101 double* a,
102 const int* lda,
103 int* ipiv,
104 double* b,
105 const int* ldb,
106 int* info);
107void dgeev_(const char* jobvl,
108 const char* jobvr,
109 const int* n,
110 double* a,
111 const int* lda,
112 double* wr,
113 double* wi,
114 double* vl,
115 const int* ldvl,
116 double* vr,
117 const int* ldvr,
118 double* work,
119 const int* lwork,
120 int* info);
121}
122
123using namespace std; // necessary to get std math routines
124
125#ifdef IKFAST_NAMESPACE
126namespace IKFAST_NAMESPACE
127{
128#endif
129
130inline float IKabs(float f) { return fabsf(f); }
131inline double IKabs(double f) { return fabs(f); }
132
133inline float IKsqr(float f) { return f * f; }
134inline double IKsqr(double f) { return f * f; }
135
136inline float IKlog(float f) { return logf(f); }
137inline double IKlog(double f) { return log(f); }
138
139// allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
140#ifndef IKFAST_SINCOS_THRESH
141#define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
142#endif
143
144// used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and
145// evaluation
146#ifndef IKFAST_ATAN2_MAGTHRESH
147#define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
148#endif
149
150// minimum distance of separate solutions
151#ifndef IKFAST_SOLUTION_THRESH
152#define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
153#endif
154
155// there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold speicfies by how much they
156// can deviate
157#ifndef IKFAST_EVALCOND_THRESH
158#define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
159#endif
160
161inline float IKasin(float f)
162{
163 IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
164 // wrong with the solver
165 if (f <= -1)
166 return float(-IKPI_2);
167 else if (f >= 1)
168 return float(IKPI_2);
169 return asinf(f);
170}
171inline double IKasin(double f)
172{
173 IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
174 // wrong with the solver
175 if (f <= -1)
176 return -IKPI_2;
177 else if (f >= 1)
178 return IKPI_2;
179 return asin(f);
180}
181
182// return positive value in [0,y)
183inline float IKfmod(float x, float y)
184{
185 while (x < 0)
186 {
187 x += y;
188 }
189 return fmodf(x, y);
190}
191
192// return positive value in [0,y)
193inline double IKfmod(double x, double y)
194{
195 while (x < 0)
196 {
197 x += y;
198 }
199 return fmod(x, y);
200}
201
202inline float IKacos(float f)
203{
204 IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
205 // wrong with the solver
206 if (f <= -1)
207 return float(IKPI);
208 else if (f >= 1)
209 return float(0);
210 return acosf(f);
211}
212inline double IKacos(double f)
213{
214 IKFAST_ASSERT(f > -1 - IKFAST_SINCOS_THRESH && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
215 // wrong with the solver
216 if (f <= -1)
217 return IKPI;
218 else if (f >= 1)
219 return 0;
220 return acos(f);
221}
222inline float IKsin(float f) { return sinf(f); }
223inline double IKsin(double f) { return sin(f); }
224inline float IKcos(float f) { return cosf(f); }
225inline double IKcos(double f) { return cos(f); }
226inline float IKtan(float f) { return tanf(f); }
227inline double IKtan(double f) { return tan(f); }
228inline float IKsqrt(float f)
229{
230 if (f <= 0.0f)
231 return 0.0f;
232 return sqrtf(f);
233}
234inline double IKsqrt(double f)
235{
236 if (f <= 0.0)
237 return 0.0;
238 return sqrt(f);
239}
240inline float IKatan2Simple(float fy, float fx) { return atan2f(fy, fx); }
241inline float IKatan2(float fy, float fx)
242{
243 if (isnan(fy))
244 {
245 IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
246 return float(IKPI_2);
247 }
248 else if (isnan(fx))
249 {
250 return 0;
251 }
252 return atan2f(fy, fx);
253}
254inline double IKatan2Simple(double fy, double fx) { return atan2(fy, fx); }
255inline double IKatan2(double fy, double fx)
256{
257 if (isnan(fy))
258 {
259 IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
260 return IKPI_2;
261 }
262 else if (isnan(fx))
263 {
264 return 0;
265 }
266 return atan2(fy, fx);
267}
268
269template <typename T>
270struct CheckValue
271{
272 T value;
273 bool valid;
274};
275
276template <typename T>
277inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
278{
279 CheckValue<T> ret;
280 ret.valid = false;
281 ret.value = 0;
282 if (!isnan(fy) && !isnan(fx))
283 {
284 if (IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH)
285 {
286 ret.value = IKatan2Simple(fy, fx);
287 ret.valid = true;
288 }
289 }
290 return ret;
291}
292
293inline float IKsign(float f)
294{
295 if (f > 0)
296 {
297 return float(1);
298 }
299 else if (f < 0)
300 {
301 return float(-1);
302 }
303 return 0;
304}
305
306inline double IKsign(double f)
307{
308 if (f > 0)
309 {
310 return 1.0;
311 }
312 else if (f < 0)
313 {
314 return -1.0;
315 }
316 return 0;
317}
318
319template <typename T>
320inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
321{
322 CheckValue<T> ret;
323 ret.valid = true;
324 if (n == 0)
325 {
326 ret.value = 1.0;
327 return ret;
328 }
329 else if (n == 1)
330 {
331 ret.value = f;
332 return ret;
333 }
334 else if (n < 0)
335 {
336 if (f == 0)
337 {
338 ret.valid = false;
339 ret.value = (T)1.0e30;
340 return ret;
341 }
342 if (n == -1)
343 {
344 ret.value = T(1.0) / f;
345 return ret;
346 }
347 }
348
349 int num = n > 0 ? n : -n;
350 if (num == 2)
351 {
352 ret.value = f * f;
353 }
354 else if (num == 3)
355 {
356 ret.value = f * f * f;
357 }
358 else
359 {
360 ret.value = 1.0;
361 while (num > 0)
362 {
363 if (num & 1)
364 {
365 ret.value *= f;
366 }
367 num >>= 1;
368 f *= f;
369 }
370 }
371
372 if (n < 0)
373 {
374 ret.value = T(1.0) / ret.value;
375 }
376 return ret;
377}
378
381IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
382{
383 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,
384 x24, x25, x26, x27, x28, x29, x30, x31, x32, x33, x34, x35, x36, x37, x38, x39, x40, x41, x42, x43, x44, x45, x46,
385 x47, x48, x49, x50, x51, x52, x53, x54, x55, x56, x57, x58, x59, x60, x61, x62, x63, x64, x65, x66, x67, x68;
386 x0 = IKcos(j[0]);
387 x1 = IKcos(j[1]);
388 x2 = IKcos(j[2]);
389 x3 = IKsin(j[0]);
390 x4 = IKsin(j[2]);
391 x5 = IKcos(j[3]);
392 x6 = IKsin(j[1]);
393 x7 = IKsin(j[3]);
394 x8 = IKcos(j[4]);
395 x9 = IKsin(j[4]);
396 x10 = IKcos(j[6]);
397 x11 = IKsin(j[6]);
398 x12 = IKsin(j[5]);
399 x13 = IKcos(j[5]);
400 x14 = ((0.4) * x3);
401 x15 = ((0.15085) * x5);
402 x16 = ((1.0) * x5);
403 x17 = ((1.0) * x8);
404 x18 = ((1.0) * x7);
405 x19 = ((1.0) * x4);
406 x20 = ((1.0) * x13);
407 x21 = ((1.0) * x1);
408 x22 = ((0.15085) * x7);
409 x23 = ((0.15085) * x8);
410 x24 = ((0.4) * x5);
411 x25 = ((0.4) * x2);
412 x26 = ((1.0) * x9);
413 x27 = ((0.15085) * x9);
414 x28 = (x0 * x6);
415 x29 = (x0 * x4);
416 x30 = ((-1.0) * x9);
417 x31 = (x5 * x6);
418 x32 = (x6 * x9);
419 x33 = (x2 * x3);
420 x34 = ((-1.0) * x8);
421 x35 = (x6 * x7);
422 x36 = (x1 * x2);
423 x37 = (x0 * x2);
424 x38 = (x2 * x6);
425 x39 = (x3 * x6);
426 x40 = (x19 * x3);
427 x41 = (x16 * x38);
428 x42 = (x19 * x32);
429 x43 = ((((-1.0) * x40)) + ((x0 * x36)));
430 x44 = (x29 + ((x1 * x33)));
431 x45 = (((x1 * x5)) + ((x2 * x35)));
432 x46 = (x40 + (((-1.0) * x21 * x37)));
433 x47 = ((((-1.0) * x37)) + ((x1 * x40)));
434 x48 = ((((-1.0) * x1 * x18)) + x41);
435 x49 = (((x0 * x1 * x19)) + (((1.0) * x33)));
436 x50 = ((((-1.0) * x0 * x19)) + (((-1.0) * x21 * x33)));
437 x51 = (x12 * x45);
438 x52 = (x43 * x5);
439 x53 = (x48 * x8);
440 x54 = (x47 * x9);
441 x55 = (x49 * x9);
442 x56 = (x50 * x7);
443 x57 = (x52 + ((x28 * x7)));
444 x58 = (((x3 * x35)) + ((x44 * x5)));
445 x59 = (((x46 * x7)) + ((x28 * x5)));
446 x60 = ((((-1.0) * x42)) + x53);
447 x61 = (((x26 * (((((-1.0) * x1 * x7)) + x41)))) + ((x17 * x4 * x6)));
448 x62 = ((((-1.0) * x18 * x28)) + (((-1.0) * x16 * x43)));
449 x63 = (((x3 * x31)) + x56);
450 x64 = ((((-1.0) * x18 * x39)) + (((-1.0) * x16 * x44)));
451 x65 = (x12 * x59);
452 x66 = (x62 * x8);
453 x67 = (x12 * x63);
454 x68 = (x54 + ((x64 * x8)));
455 eerot[0] =
456 ((((-1.0) * x10 *
457 (((((1.0) * x65)) + (((1.0) * x13 * ((((x8 * (((((-1.0) * x28 * x7)) + (((-1.0) * x52)))))) + x55)))))))) +
458 ((x11 * (((((-1.0) * x17 * x49)) + (((-1.0) * x26 * x57)))))));
459 eerot[1] = (((x11 * ((((x13 * ((x55 + x66)))) + x65)))) + ((x10 * ((((x34 * x49)) + ((x30 * x57)))))));
460 eerot[2] = (((x13 * x59)) + ((x12 * (((((-1.0) * x17 * x62)) + (((-1.0) * x26 * x49)))))));
461 eetrans[0] = (((x7 * (((((-1.0) * x0 * x1 * x25)) + ((x14 * x4)))))) + ((x13 * ((((x22 * x46)) + ((x15 * x28)))))) +
462 (((0.4) * x28)) + ((x12 * (((((-1.0) * x27 * x49)) + (((-1.0) * x23 * x62)))))) + ((x24 * x28)));
463 eerot[3] = (((x10 * (((((-1.0) * x20 * x68)) + (((-1.0) * x67)))))) + ((x11 * ((((x34 * x47)) + ((x30 * x58)))))));
464 eerot[4] = (((x10 * (((((-1.0) * x17 * x47)) + (((-1.0) * x26 * x58)))))) + ((x11 * ((((x13 * x68)) + x67)))));
465 eerot[5] = (((x13 * x63)) + ((x12 * (((((-1.0) * x17 * x64)) + (((-1.0) * x26 * x47)))))));
466 eetrans[1] = (((x13 * ((((x15 * x39)) + ((x22 * x50)))))) + ((x7 * (((((-1.0) * x14 * x36)) + (((-0.4) * x29)))))) +
467 ((x14 * x6)) + ((x14 * x31)) + ((x12 * (((((-1.0) * x27 * x47)) + (((-1.0) * x23 * x64)))))));
468 eerot[6] = (((x11 * x61)) + ((x10 * (((((-1.0) * x20 * x60)) + (((-1.0) * x51)))))));
469 eerot[7] = (((x11 * ((((x13 * x60)) + x51)))) + ((x10 * x61)));
470 eerot[8] = (((x12 * (((((-1.0) * x17 * x48)) + x42)))) + ((x13 * x45)));
471 eetrans[2] = ((0.34) + ((x1 * x24)) + (((0.4) * x1)) + ((x12 * ((((x27 * x4 * x6)) + (((-1.0) * x23 * x48)))))) +
472 ((x13 * ((((x1 * x15)) + ((x22 * x38)))))) + ((x25 * x35)));
473}
474
475IKFAST_API int GetNumFreeParameters() { return 1; }
476IKFAST_API int* GetFreeParameters()
477{
478 static int freeparams[] = { 4 };
479 return freeparams;
480}
481IKFAST_API int GetNumJoints() { return 7; }
482
483IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
484
485IKFAST_API int GetIkType() { return 0x67000001; }
486
487class IKSolver
488{
489public:
490 IkReal j0, cj0, sj0, htj0, j0mul, j1, cj1, sj1, htj1, j1mul, j2, cj2, sj2, htj2, j2mul, j3, cj3, sj3, htj3, j3mul, j5,
491 cj5, sj5, htj5, j5mul, j6, cj6, sj6, htj6, j6mul, j4, cj4, sj4, htj4, new_r00, r00, rxp0_0, new_r01, r01, rxp0_1,
492 new_r02, r02, rxp0_2, new_r10, r10, rxp1_0, new_r11, r11, rxp1_1, new_r12, r12, rxp1_2, new_r20, r20, rxp2_0,
493 new_r21, r21, rxp2_1, new_r22, r22, rxp2_2, new_px, px, npx, new_py, py, npy, new_pz, pz, npz, pp;
494 unsigned char _ij0[2], _nj0, _ij1[2], _nj1, _ij2[2], _nj2, _ij3[2], _nj3, _ij5[2], _nj5, _ij6[2], _nj6, _ij4[2], _nj4;
495
496 IkReal j100, cj100, sj100;
497 unsigned char _ij100[2], _nj100;
498 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions)
499 {
500 j0 = numeric_limits<IkReal>::quiet_NaN();
501 _ij0[0] = -1;
502 _ij0[1] = -1;
503 _nj0 = -1;
504 j1 = numeric_limits<IkReal>::quiet_NaN();
505 _ij1[0] = -1;
506 _ij1[1] = -1;
507 _nj1 = -1;
508 j2 = numeric_limits<IkReal>::quiet_NaN();
509 _ij2[0] = -1;
510 _ij2[1] = -1;
511 _nj2 = -1;
512 j3 = numeric_limits<IkReal>::quiet_NaN();
513 _ij3[0] = -1;
514 _ij3[1] = -1;
515 _nj3 = -1;
516 j5 = numeric_limits<IkReal>::quiet_NaN();
517 _ij5[0] = -1;
518 _ij5[1] = -1;
519 _nj5 = -1;
520 j6 = numeric_limits<IkReal>::quiet_NaN();
521 _ij6[0] = -1;
522 _ij6[1] = -1;
523 _nj6 = -1;
524 _ij4[0] = -1;
525 _ij4[1] = -1;
526 _nj4 = 0;
527 for (int dummyiter = 0; dummyiter < 1; ++dummyiter)
528 {
529 solutions.Clear();
530 j4 = pfree[0];
531 cj4 = cos(pfree[0]);
532 sj4 = sin(pfree[0]), htj4 = tan(pfree[0] * 0.5);
533 r00 = eerot[0 * 3 + 0];
534 r01 = eerot[0 * 3 + 1];
535 r02 = eerot[0 * 3 + 2];
536 r10 = eerot[1 * 3 + 0];
537 r11 = eerot[1 * 3 + 1];
538 r12 = eerot[1 * 3 + 2];
539 r20 = eerot[2 * 3 + 0];
540 r21 = eerot[2 * 3 + 1];
541 r22 = eerot[2 * 3 + 2];
542 px = eetrans[0];
543 py = eetrans[1];
544 pz = eetrans[2];
545
546 new_r00 = ((-1.0) * r00);
547 new_r01 = ((-1.0) * r01);
548 new_r02 = r02;
549 new_px = (px + (((-0.15085) * r02)));
550 new_r10 = ((-1.0) * r10);
551 new_r11 = ((-1.0) * r11);
552 new_r12 = r12;
553 new_py = (py + (((-0.15085) * r12)));
554 new_r20 = ((-1.0) * r20);
555 new_r21 = ((-1.0) * r21);
556 new_r22 = r22;
557 new_pz = ((-0.34) + pz + (((-0.15085) * r22)));
558 r00 = new_r00;
559 r01 = new_r01;
560 r02 = new_r02;
561 r10 = new_r10;
562 r11 = new_r11;
563 r12 = new_r12;
564 r20 = new_r20;
565 r21 = new_r21;
566 r22 = new_r22;
567 px = new_px;
568 py = new_py;
569 pz = new_pz;
570 IkReal x69 = ((1.0) * px);
571 IkReal x70 = ((1.0) * pz);
572 IkReal x71 = ((1.0) * py);
573 pp = ((px * px) + (py * py) + (pz * pz));
574 npx = (((px * r00)) + ((py * r10)) + ((pz * r20)));
575 npy = (((px * r01)) + ((py * r11)) + ((pz * r21)));
576 npz = (((px * r02)) + ((py * r12)) + ((pz * r22)));
577 rxp0_0 = ((((-1.0) * r20 * x71)) + ((pz * r10)));
578 rxp0_1 = (((px * r20)) + (((-1.0) * r00 * x70)));
579 rxp0_2 = ((((-1.0) * r10 * x69)) + ((py * r00)));
580 rxp1_0 = ((((-1.0) * r21 * x71)) + ((pz * r11)));
581 rxp1_1 = (((px * r21)) + (((-1.0) * r01 * x70)));
582 rxp1_2 = ((((-1.0) * r11 * x69)) + ((py * r01)));
583 rxp2_0 = ((((-1.0) * r22 * x71)) + ((pz * r12)));
584 rxp2_1 = (((px * r22)) + (((-1.0) * r02 * x70)));
585 rxp2_2 = ((((-1.0) * r12 * x69)) + ((py * r02)));
586 {
587 IkReal j3array[2], cj3array[2], sj3array[2];
588 bool j3valid[2] = { false };
589 _nj3 = 2;
590 cj3array[0] = ((-1.0) + (((3.125) * pp)));
591 if (cj3array[0] >= -1 - IKFAST_SINCOS_THRESH && cj3array[0] <= 1 + IKFAST_SINCOS_THRESH)
592 {
593 j3valid[0] = j3valid[1] = true;
594 j3array[0] = IKacos(cj3array[0]);
595 sj3array[0] = IKsin(j3array[0]);
596 cj3array[1] = cj3array[0];
597 j3array[1] = -j3array[0];
598 sj3array[1] = -sj3array[0];
599 }
600 else if (isnan(cj3array[0]))
601 {
602 // probably any value will work
603 j3valid[0] = true;
604 cj3array[0] = 1;
605 sj3array[0] = 0;
606 j3array[0] = 0;
607 }
608 for (int ij3 = 0; ij3 < 2; ++ij3)
609 {
610 if (!j3valid[ij3])
611 {
612 continue;
613 }
614 _ij3[0] = ij3;
615 _ij3[1] = -1;
616 for (int iij3 = ij3 + 1; iij3 < 2; ++iij3)
617 {
618 if (j3valid[iij3] && IKabs(cj3array[ij3] - cj3array[iij3]) < IKFAST_SOLUTION_THRESH &&
619 IKabs(sj3array[ij3] - sj3array[iij3]) < IKFAST_SOLUTION_THRESH)
620 {
621 j3valid[iij3] = false;
622 _ij3[1] = iij3;
623 break;
624 }
625 }
626 j3 = j3array[ij3];
627 cj3 = cj3array[ij3];
628 sj3 = sj3array[ij3];
629
630 {
631 IkReal j6eval[2];
632 j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
633 j6eval[1] = ((npx * npx) + (npy * npy));
634 if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000)
635 {
636 {
637 IkReal j5eval[2];
638 j5eval[0] = ((1.0) + (cj3 * cj3) + (((2.0) * cj3)) + (((cj4 * cj4) * (sj3 * sj3))));
639 j5eval[1] = ((((2.5) * (IKabs(((0.4) + (((0.4) * cj3))))))) + (IKabs((cj4 * sj3))));
640 if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000)
641 {
642 {
643 IkReal evalcond[2];
644 bool bgotonextstatement = true;
645 do
646 {
647 evalcond[0] =
648 ((-3.14159265358979) +
649 (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j3)))), 6.28318530717959)));
650 evalcond[1] = pp;
651 if (IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000)
652 {
653 bgotonextstatement = false;
654 {
655 IkReal j6eval[1];
656 sj3 = 0;
657 cj3 = -1.0;
658 j3 = 3.14159265358979;
659 j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
660 if (IKabs(j6eval[0]) < 0.0000010000000000)
661 {
662 continue; // no branches [j5, j6]
663 }
664 else
665 {
666 {
667 IkReal j6array[2], cj6array[2], sj6array[2];
668 bool j6valid[2] = { false };
669 _nj6 = 2;
670 CheckValue<IkReal> x73 =
671 IKatan2WithCheck(IkReal(((-1.0) * npx)), IkReal(npy), IKFAST_ATAN2_MAGTHRESH);
672 if (!x73.valid)
673 {
674 continue;
675 }
676 IkReal x72 = x73.value;
677 j6array[0] = ((-1.0) * x72);
678 sj6array[0] = IKsin(j6array[0]);
679 cj6array[0] = IKcos(j6array[0]);
680 j6array[1] = ((3.14159265358979) + (((-1.0) * x72)));
681 sj6array[1] = IKsin(j6array[1]);
682 cj6array[1] = IKcos(j6array[1]);
683 if (j6array[0] > IKPI)
684 {
685 j6array[0] -= IK2PI;
686 }
687 else if (j6array[0] < -IKPI)
688 {
689 j6array[0] += IK2PI;
690 }
691 j6valid[0] = true;
692 if (j6array[1] > IKPI)
693 {
694 j6array[1] -= IK2PI;
695 }
696 else if (j6array[1] < -IKPI)
697 {
698 j6array[1] += IK2PI;
699 }
700 j6valid[1] = true;
701 for (int ij6 = 0; ij6 < 2; ++ij6)
702 {
703 if (!j6valid[ij6])
704 {
705 continue;
706 }
707 _ij6[0] = ij6;
708 _ij6[1] = -1;
709 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
710 {
711 if (j6valid[iij6] && IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
712 IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
713 {
714 j6valid[iij6] = false;
715 _ij6[1] = iij6;
716 break;
717 }
718 }
719 j6 = j6array[ij6];
720 cj6 = cj6array[ij6];
721 sj6 = sj6array[ij6];
722 {
723 IkReal evalcond[1];
724 evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) + (((-1.0) * npx * (IKsin(j6)))));
725 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
726 {
727 continue;
728 }
729 }
730
731 {
732 IkReal j5eval[1];
733 sj3 = 0;
734 cj3 = -1.0;
735 j3 = 3.14159265358979;
736 j5eval[0] = IKabs((((cj6 * npx)) + (((-1.0) * npy * sj6))));
737 if (IKabs(j5eval[0]) < 0.0000000100000000)
738 {
739 continue; // no branches [j5]
740 }
741 else
742 {
743 IkReal op[2 + 1], zeror[2];
744 int numroots;
745 IkReal x74 = (npy * sj6);
746 IkReal x75 = (cj6 * npx);
747 op[0] = (x75 + (((-1.0) * x74)));
748 op[1] = 0;
749 op[2] = (x74 + (((-1.0) * x75)));
750 polyroots2(op, zeror, numroots);
751 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
752 int numsolutions = 0;
753 for (int ij5 = 0; ij5 < numroots; ++ij5)
754 {
755 IkReal htj5 = zeror[ij5];
756 tempj5array[0] = ((2.0) * (atan(htj5)));
757 for (int kj5 = 0; kj5 < 1; ++kj5)
758 {
759 j5array[numsolutions] = tempj5array[kj5];
760 if (j5array[numsolutions] > IKPI)
761 {
762 j5array[numsolutions] -= IK2PI;
763 }
764 else if (j5array[numsolutions] < -IKPI)
765 {
766 j5array[numsolutions] += IK2PI;
767 }
768 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
769 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
770 numsolutions++;
771 }
772 }
773 bool j5valid[2] = { true, true };
774 _nj5 = 2;
775 for (int ij5 = 0; ij5 < numsolutions; ++ij5)
776 {
777 if (!j5valid[ij5])
778 {
779 continue;
780 }
781 j5 = j5array[ij5];
782 cj5 = cj5array[ij5];
783 sj5 = sj5array[ij5];
784 htj5 = IKtan(j5 / 2);
785
786 _ij5[0] = ij5;
787 _ij5[1] = -1;
788 for (int iij5 = ij5 + 1; iij5 < numsolutions; ++iij5)
789 {
790 if (j5valid[iij5] &&
791 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
792 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
793 {
794 j5valid[iij5] = false;
795 _ij5[1] = iij5;
796 break;
797 }
798 }
799 rotationfunction0(solutions);
800 }
801 }
802 }
803 }
804 }
805 }
806 }
807 }
808 } while (0);
809 if (bgotonextstatement)
810 {
811 bool bgotonextstatement = true;
812 do
813 {
814 if (1)
815 {
816 bgotonextstatement = false;
817 continue; // branch miss [j5, j6]
818 }
819 } while (0);
820 if (bgotonextstatement)
821 {
822 }
823 }
824 }
825 }
826 else
827 {
828 {
829 IkReal j5array[2], cj5array[2], sj5array[2];
830 bool j5valid[2] = { false };
831 _nj5 = 2;
832 IkReal x1007 = ((0.4) + (((0.4) * cj3)));
833 CheckValue<IkReal> x1010 =
834 IKatan2WithCheck(IkReal(x1007), IkReal(((0.4) * cj4 * sj3)), IKFAST_ATAN2_MAGTHRESH);
835 if (!x1010.valid)
836 {
837 continue;
838 }
839 IkReal x1008 = ((1.0) * (x1010.value));
840 if ((((((0.16) * (cj4 * cj4) * (sj3 * sj3))) + (x1007 * x1007))) < -0.00001)
841 continue;
842 CheckValue<IkReal> x1011 = IKPowWithIntegerCheck(
843 IKabs(IKsqrt(((((0.16) * (cj4 * cj4) * (sj3 * sj3))) + (x1007 * x1007)))), -1);
844 if (!x1011.valid)
845 {
846 continue;
847 }
848 if (((npz * (x1011.value))) < -1 - IKFAST_SINCOS_THRESH ||
849 ((npz * (x1011.value))) > 1 + IKFAST_SINCOS_THRESH)
850 continue;
851 IkReal x1009 = IKasin((npz * (x1011.value)));
852 j5array[0] = (x1009 + (((-1.0) * x1008)));
853 sj5array[0] = IKsin(j5array[0]);
854 cj5array[0] = IKcos(j5array[0]);
855 j5array[1] = ((3.14159265358979) + (((-1.0) * x1009)) + (((-1.0) * x1008)));
856 sj5array[1] = IKsin(j5array[1]);
857 cj5array[1] = IKcos(j5array[1]);
858 if (j5array[0] > IKPI)
859 {
860 j5array[0] -= IK2PI;
861 }
862 else if (j5array[0] < -IKPI)
863 {
864 j5array[0] += IK2PI;
865 }
866 j5valid[0] = true;
867 if (j5array[1] > IKPI)
868 {
869 j5array[1] -= IK2PI;
870 }
871 else if (j5array[1] < -IKPI)
872 {
873 j5array[1] += IK2PI;
874 }
875 j5valid[1] = true;
876 for (int ij5 = 0; ij5 < 2; ++ij5)
877 {
878 if (!j5valid[ij5])
879 {
880 continue;
881 }
882 _ij5[0] = ij5;
883 _ij5[1] = -1;
884 for (int iij5 = ij5 + 1; iij5 < 2; ++iij5)
885 {
886 if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
887 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
888 {
889 j5valid[iij5] = false;
890 _ij5[1] = iij5;
891 break;
892 }
893 }
894 j5 = j5array[ij5];
895 cj5 = cj5array[ij5];
896 sj5 = sj5array[ij5];
897
898 {
899 IkReal j6eval[3];
900 IkReal x1012 = npy * npy;
901 IkReal x1013 = npx * npx;
902 IkReal x1014 = (cj5 * sj4);
903 IkReal x1015 = (cj5 * x1013);
904 IkReal x1016 = (cj5 * x1012);
905 IkReal x1017 = ((5.0) * npz * sj5);
906 IkReal x1018 = ((2.0) * npy * sj3);
907 IkReal x1019 = ((2.0) * npx * sj3);
908 j6eval[0] = (x1015 + x1016);
909 j6eval[1] = ((IKabs((((x1014 * x1019)) + ((cj4 * x1018)) + (((-1.0) * npy * x1017))))) +
910 (IKabs((((x1014 * x1018)) + (((-1.0) * cj4 * x1019)) + ((npx * x1017))))));
911 j6eval[2] = IKsign(((((5.0) * x1016)) + (((5.0) * x1015))));
912 if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 ||
913 IKabs(j6eval[2]) < 0.0000010000000000)
914 {
915 {
916 IkReal j6eval[3];
917 IkReal x1020 = npy * npy;
918 IkReal x1021 = npx * npx;
919 IkReal x1022 = ((2.0) * cj3);
920 IkReal x1023 = ((5.0) * sj5);
921 IkReal x1024 = ((2.0) * npx);
922 IkReal x1025 = ((2.0) * npy);
923 IkReal x1026 = ((5.0) * cj5 * npz);
924 IkReal x1027 = (sj3 * sj4 * sj5);
925 j6eval[0] = (((sj5 * x1021)) + ((sj5 * x1020)));
926 j6eval[1] = IKsign((((x1021 * x1023)) + ((x1020 * x1023))));
927 j6eval[2] =
928 ((IKabs((x1024 + ((npx * x1022)) + (((-1.0) * npx * x1026)) + ((x1025 * x1027))))) +
929 (IKabs((((npy * x1026)) + (((-1.0) * x1025)) + (((-1.0) * npy * x1022)) +
930 ((x1024 * x1027))))));
931 if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 ||
932 IKabs(j6eval[2]) < 0.0000010000000000)
933 {
934 {
935 IkReal j6eval[3];
936 IkReal x1028 = npx * npx;
937 IkReal x1029 = npy * npy;
938 IkReal x1030 = ((2.0) * sj5);
939 IkReal x1031 = ((2.0) * sj3);
940 IkReal x1032 = (cj4 * cj5);
941 j6eval[0] = (x1029 + x1028);
942 j6eval[1] = ((IKabs(((((-1.0) * npx * x1031 * x1032)) + ((npy * sj4 * x1031)) +
943 ((npx * x1030)) + ((cj3 * npx * x1030))))) +
944 (IKabs((((npx * sj4 * x1031)) + ((npy * x1031 * x1032)) +
945 (((-1.0) * cj3 * npy * x1030)) + (((-1.0) * npy * x1030))))));
946 j6eval[2] = IKsign(((((5.0) * x1028)) + (((5.0) * x1029))));
947 if (IKabs(j6eval[0]) < 0.0000010000000000 || IKabs(j6eval[1]) < 0.0000010000000000 ||
948 IKabs(j6eval[2]) < 0.0000010000000000)
949 {
950 {
951 IkReal evalcond[2];
952 bool bgotonextstatement = true;
953 do
954 {
955 evalcond[0] = ((-3.14159265358979) +
956 (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j3)))),
957 6.28318530717959)));
958 evalcond[1] = pp;
959 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
960 IKabs(evalcond[1]) < 0.0000050000000000)
961 {
962 bgotonextstatement = false;
963 {
964 IkReal j6eval[1];
965 sj3 = 0;
966 cj3 = -1.0;
967 j3 = 3.14159265358979;
968 j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
969 if (IKabs(j6eval[0]) < 0.0000010000000000)
970 {
971 {
972 IkReal j6eval[1];
973 sj3 = 0;
974 cj3 = -1.0;
975 j3 = 3.14159265358979;
976 j6eval[0] = ((IKabs((npx * sj5))) + (IKabs((npy * sj5))));
977 if (IKabs(j6eval[0]) < 0.0000010000000000)
978 {
979 {
980 IkReal j6eval[1];
981 sj3 = 0;
982 cj3 = -1.0;
983 j3 = 3.14159265358979;
984 j6eval[0] = ((IKabs((cj5 * npx))) + (IKabs((cj5 * npy))));
985 if (IKabs(j6eval[0]) < 0.0000010000000000)
986 {
987 {
988 IkReal evalcond[1];
989 bool bgotonextstatement = true;
990 do
991 {
992 evalcond[0] = ((-3.14159265358979) +
993 (IKfmod(((3.14159265358979) +
994 (IKabs(((-1.5707963267949) + j5)))),
995 6.28318530717959)));
996 if (IKabs(evalcond[0]) < 0.0000050000000000)
997 {
998 bgotonextstatement = false;
999 {
1000 IkReal j6eval[1];
1001 sj3 = 0;
1002 cj3 = -1.0;
1003 j3 = 3.14159265358979;
1004 sj5 = 1.0;
1005 cj5 = 0;
1006 j5 = 1.5707963267949;
1007 j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1008 if (IKabs(j6eval[0]) < 0.0000010000000000)
1009 {
1010 continue; // no branches [j6]
1011 }
1012 else
1013 {
1014 {
1015 IkReal j6array[2], cj6array[2], sj6array[2];
1016 bool j6valid[2] = { false };
1017 _nj6 = 2;
1018 CheckValue<IkReal> x1034 =
1019 IKatan2WithCheck(IkReal(((-1.0) * npx)),
1020 IkReal(npy),
1021 IKFAST_ATAN2_MAGTHRESH);
1022 if (!x1034.valid)
1023 {
1024 continue;
1025 }
1026 IkReal x1033 = x1034.value;
1027 j6array[0] = ((-1.0) * x1033);
1028 sj6array[0] = IKsin(j6array[0]);
1029 cj6array[0] = IKcos(j6array[0]);
1030 j6array[1] = ((3.14159265358979) + (((-1.0) * x1033)));
1031 sj6array[1] = IKsin(j6array[1]);
1032 cj6array[1] = IKcos(j6array[1]);
1033 if (j6array[0] > IKPI)
1034 {
1035 j6array[0] -= IK2PI;
1036 }
1037 else if (j6array[0] < -IKPI)
1038 {
1039 j6array[0] += IK2PI;
1040 }
1041 j6valid[0] = true;
1042 if (j6array[1] > IKPI)
1043 {
1044 j6array[1] -= IK2PI;
1045 }
1046 else if (j6array[1] < -IKPI)
1047 {
1048 j6array[1] += IK2PI;
1049 }
1050 j6valid[1] = true;
1051 for (int ij6 = 0; ij6 < 2; ++ij6)
1052 {
1053 if (!j6valid[ij6])
1054 {
1055 continue;
1056 }
1057 _ij6[0] = ij6;
1058 _ij6[1] = -1;
1059 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1060 {
1061 if (j6valid[iij6] &&
1062 IKabs(cj6array[ij6] - cj6array[iij6]) <
1063 IKFAST_SOLUTION_THRESH &&
1064 IKabs(sj6array[ij6] - sj6array[iij6]) <
1065 IKFAST_SOLUTION_THRESH)
1066 {
1067 j6valid[iij6] = false;
1068 _ij6[1] = iij6;
1069 break;
1070 }
1071 }
1072 j6 = j6array[ij6];
1073 cj6 = cj6array[ij6];
1074 sj6 = sj6array[ij6];
1075 {
1076 IkReal evalcond[1];
1077 evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1078 (((-1.0) * npx * (IKsin(j6)))));
1079 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1080 {
1081 continue;
1082 }
1083 }
1084
1085 rotationfunction0(solutions);
1086 }
1087 }
1088 }
1089 }
1090 }
1091 } while (0);
1092 if (bgotonextstatement)
1093 {
1094 bool bgotonextstatement = true;
1095 do
1096 {
1097 evalcond[0] = ((-3.14159265358979) +
1098 (IKfmod(((3.14159265358979) +
1099 (IKabs(((1.5707963267949) + j5)))),
1100 6.28318530717959)));
1101 if (IKabs(evalcond[0]) < 0.0000050000000000)
1102 {
1103 bgotonextstatement = false;
1104 {
1105 IkReal j6eval[1];
1106 sj3 = 0;
1107 cj3 = -1.0;
1108 j3 = 3.14159265358979;
1109 sj5 = -1.0;
1110 cj5 = 0;
1111 j5 = -1.5707963267949;
1112 j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1113 if (IKabs(j6eval[0]) < 0.0000010000000000)
1114 {
1115 continue; // no branches [j6]
1116 }
1117 else
1118 {
1119 {
1120 IkReal j6array[2], cj6array[2], sj6array[2];
1121 bool j6valid[2] = { false };
1122 _nj6 = 2;
1123 CheckValue<IkReal> x1036 =
1124 IKatan2WithCheck(IkReal(npx),
1125 IkReal(((-1.0) * npy)),
1126 IKFAST_ATAN2_MAGTHRESH);
1127 if (!x1036.valid)
1128 {
1129 continue;
1130 }
1131 IkReal x1035 = x1036.value;
1132 j6array[0] = ((-1.0) * x1035);
1133 sj6array[0] = IKsin(j6array[0]);
1134 cj6array[0] = IKcos(j6array[0]);
1135 j6array[1] =
1136 ((3.14159265358979) + (((-1.0) * x1035)));
1137 sj6array[1] = IKsin(j6array[1]);
1138 cj6array[1] = IKcos(j6array[1]);
1139 if (j6array[0] > IKPI)
1140 {
1141 j6array[0] -= IK2PI;
1142 }
1143 else if (j6array[0] < -IKPI)
1144 {
1145 j6array[0] += IK2PI;
1146 }
1147 j6valid[0] = true;
1148 if (j6array[1] > IKPI)
1149 {
1150 j6array[1] -= IK2PI;
1151 }
1152 else if (j6array[1] < -IKPI)
1153 {
1154 j6array[1] += IK2PI;
1155 }
1156 j6valid[1] = true;
1157 for (int ij6 = 0; ij6 < 2; ++ij6)
1158 {
1159 if (!j6valid[ij6])
1160 {
1161 continue;
1162 }
1163 _ij6[0] = ij6;
1164 _ij6[1] = -1;
1165 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1166 {
1167 if (j6valid[iij6] &&
1168 IKabs(cj6array[ij6] - cj6array[iij6]) <
1169 IKFAST_SOLUTION_THRESH &&
1170 IKabs(sj6array[ij6] - sj6array[iij6]) <
1171 IKFAST_SOLUTION_THRESH)
1172 {
1173 j6valid[iij6] = false;
1174 _ij6[1] = iij6;
1175 break;
1176 }
1177 }
1178 j6 = j6array[ij6];
1179 cj6 = cj6array[ij6];
1180 sj6 = sj6array[ij6];
1181 {
1182 IkReal evalcond[1];
1183 evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1184 (((-1.0) * npx * (IKsin(j6)))));
1185 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1186 {
1187 continue;
1188 }
1189 }
1190
1191 rotationfunction0(solutions);
1192 }
1193 }
1194 }
1195 }
1196 }
1197 } while (0);
1198 if (bgotonextstatement)
1199 {
1200 bool bgotonextstatement = true;
1201 do
1202 {
1203 evalcond[0] = ((-3.14159265358979) +
1204 (IKfmod(((3.14159265358979) + (IKabs(j5))),
1205 6.28318530717959)));
1206 if (IKabs(evalcond[0]) < 0.0000050000000000)
1207 {
1208 bgotonextstatement = false;
1209 {
1210 IkReal j6eval[1];
1211 sj3 = 0;
1212 cj3 = -1.0;
1213 j3 = 3.14159265358979;
1214 sj5 = 0;
1215 cj5 = 1.0;
1216 j5 = 0;
1217 j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1218 if (IKabs(j6eval[0]) < 0.0000010000000000)
1219 {
1220 continue; // no branches [j6]
1221 }
1222 else
1223 {
1224 {
1225 IkReal j6array[2], cj6array[2], sj6array[2];
1226 bool j6valid[2] = { false };
1227 _nj6 = 2;
1228 CheckValue<IkReal> x1038 =
1229 IKatan2WithCheck(IkReal(((-1.0) * npx)),
1230 IkReal(npy),
1231 IKFAST_ATAN2_MAGTHRESH);
1232 if (!x1038.valid)
1233 {
1234 continue;
1235 }
1236 IkReal x1037 = x1038.value;
1237 j6array[0] = ((-1.0) * x1037);
1238 sj6array[0] = IKsin(j6array[0]);
1239 cj6array[0] = IKcos(j6array[0]);
1240 j6array[1] =
1241 ((3.14159265358979) + (((-1.0) * x1037)));
1242 sj6array[1] = IKsin(j6array[1]);
1243 cj6array[1] = IKcos(j6array[1]);
1244 if (j6array[0] > IKPI)
1245 {
1246 j6array[0] -= IK2PI;
1247 }
1248 else if (j6array[0] < -IKPI)
1249 {
1250 j6array[0] += IK2PI;
1251 }
1252 j6valid[0] = true;
1253 if (j6array[1] > IKPI)
1254 {
1255 j6array[1] -= IK2PI;
1256 }
1257 else if (j6array[1] < -IKPI)
1258 {
1259 j6array[1] += IK2PI;
1260 }
1261 j6valid[1] = true;
1262 for (int ij6 = 0; ij6 < 2; ++ij6)
1263 {
1264 if (!j6valid[ij6])
1265 {
1266 continue;
1267 }
1268 _ij6[0] = ij6;
1269 _ij6[1] = -1;
1270 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1271 {
1272 if (j6valid[iij6] &&
1273 IKabs(cj6array[ij6] - cj6array[iij6]) <
1274 IKFAST_SOLUTION_THRESH &&
1275 IKabs(sj6array[ij6] - sj6array[iij6]) <
1276 IKFAST_SOLUTION_THRESH)
1277 {
1278 j6valid[iij6] = false;
1279 _ij6[1] = iij6;
1280 break;
1281 }
1282 }
1283 j6 = j6array[ij6];
1284 cj6 = cj6array[ij6];
1285 sj6 = sj6array[ij6];
1286 {
1287 IkReal evalcond[1];
1288 evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1289 (((-1.0) * npx * (IKsin(j6)))));
1290 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1291 {
1292 continue;
1293 }
1294 }
1295
1296 rotationfunction0(solutions);
1297 }
1298 }
1299 }
1300 }
1301 }
1302 } while (0);
1303 if (bgotonextstatement)
1304 {
1305 bool bgotonextstatement = true;
1306 do
1307 {
1308 evalcond[0] =
1309 ((-3.14159265358979) +
1310 (IKfmod(((3.14159265358979) +
1311 (IKabs(((-3.14159265358979) + j5)))),
1312 6.28318530717959)));
1313 if (IKabs(evalcond[0]) < 0.0000050000000000)
1314 {
1315 bgotonextstatement = false;
1316 {
1317 IkReal j6eval[1];
1318 sj3 = 0;
1319 cj3 = -1.0;
1320 j3 = 3.14159265358979;
1321 sj5 = 0;
1322 cj5 = -1.0;
1323 j5 = 3.14159265358979;
1324 j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1325 if (IKabs(j6eval[0]) < 0.0000010000000000)
1326 {
1327 continue; // no branches [j6]
1328 }
1329 else
1330 {
1331 {
1332 IkReal j6array[2], cj6array[2], sj6array[2];
1333 bool j6valid[2] = { false };
1334 _nj6 = 2;
1335 CheckValue<IkReal> x1040 =
1336 IKatan2WithCheck(IkReal(npx),
1337 IkReal(((-1.0) * npy)),
1338 IKFAST_ATAN2_MAGTHRESH);
1339 if (!x1040.valid)
1340 {
1341 continue;
1342 }
1343 IkReal x1039 = x1040.value;
1344 j6array[0] = ((-1.0) * x1039);
1345 sj6array[0] = IKsin(j6array[0]);
1346 cj6array[0] = IKcos(j6array[0]);
1347 j6array[1] =
1348 ((3.14159265358979) + (((-1.0) * x1039)));
1349 sj6array[1] = IKsin(j6array[1]);
1350 cj6array[1] = IKcos(j6array[1]);
1351 if (j6array[0] > IKPI)
1352 {
1353 j6array[0] -= IK2PI;
1354 }
1355 else if (j6array[0] < -IKPI)
1356 {
1357 j6array[0] += IK2PI;
1358 }
1359 j6valid[0] = true;
1360 if (j6array[1] > IKPI)
1361 {
1362 j6array[1] -= IK2PI;
1363 }
1364 else if (j6array[1] < -IKPI)
1365 {
1366 j6array[1] += IK2PI;
1367 }
1368 j6valid[1] = true;
1369 for (int ij6 = 0; ij6 < 2; ++ij6)
1370 {
1371 if (!j6valid[ij6])
1372 {
1373 continue;
1374 }
1375 _ij6[0] = ij6;
1376 _ij6[1] = -1;
1377 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1378 {
1379 if (j6valid[iij6] &&
1380 IKabs(cj6array[ij6] - cj6array[iij6]) <
1381 IKFAST_SOLUTION_THRESH &&
1382 IKabs(sj6array[ij6] - sj6array[iij6]) <
1383 IKFAST_SOLUTION_THRESH)
1384 {
1385 j6valid[iij6] = false;
1386 _ij6[1] = iij6;
1387 break;
1388 }
1389 }
1390 j6 = j6array[ij6];
1391 cj6 = cj6array[ij6];
1392 sj6 = sj6array[ij6];
1393 {
1394 IkReal evalcond[1];
1395 evalcond[0] =
1396 ((((-1.0) * npy * (IKcos(j6)))) +
1397 (((-1.0) * npx * (IKsin(j6)))));
1398 if (IKabs(evalcond[0]) >
1399 IKFAST_EVALCOND_THRESH)
1400 {
1401 continue;
1402 }
1403 }
1404
1405 rotationfunction0(solutions);
1406 }
1407 }
1408 }
1409 }
1410 }
1411 } while (0);
1412 if (bgotonextstatement)
1413 {
1414 bool bgotonextstatement = true;
1415 do
1416 {
1417 if (1)
1418 {
1419 bgotonextstatement = false;
1420 continue; // branch miss [j6]
1421 }
1422 } while (0);
1423 if (bgotonextstatement)
1424 {
1425 }
1426 }
1427 }
1428 }
1429 }
1430 }
1431 }
1432 else
1433 {
1434 {
1435 IkReal j6array[2], cj6array[2], sj6array[2];
1436 bool j6valid[2] = { false };
1437 _nj6 = 2;
1438 CheckValue<IkReal> x1042 =
1439 IKatan2WithCheck(IkReal(((-1.0) * cj5 * npx)),
1440 IkReal((cj5 * npy)),
1441 IKFAST_ATAN2_MAGTHRESH);
1442 if (!x1042.valid)
1443 {
1444 continue;
1445 }
1446 IkReal x1041 = x1042.value;
1447 j6array[0] = ((-1.0) * x1041);
1448 sj6array[0] = IKsin(j6array[0]);
1449 cj6array[0] = IKcos(j6array[0]);
1450 j6array[1] = ((3.14159265358979) + (((-1.0) * x1041)));
1451 sj6array[1] = IKsin(j6array[1]);
1452 cj6array[1] = IKcos(j6array[1]);
1453 if (j6array[0] > IKPI)
1454 {
1455 j6array[0] -= IK2PI;
1456 }
1457 else if (j6array[0] < -IKPI)
1458 {
1459 j6array[0] += IK2PI;
1460 }
1461 j6valid[0] = true;
1462 if (j6array[1] > IKPI)
1463 {
1464 j6array[1] -= IK2PI;
1465 }
1466 else if (j6array[1] < -IKPI)
1467 {
1468 j6array[1] += IK2PI;
1469 }
1470 j6valid[1] = true;
1471 for (int ij6 = 0; ij6 < 2; ++ij6)
1472 {
1473 if (!j6valid[ij6])
1474 {
1475 continue;
1476 }
1477 _ij6[0] = ij6;
1478 _ij6[1] = -1;
1479 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1480 {
1481 if (j6valid[iij6] &&
1482 IKabs(cj6array[ij6] - cj6array[iij6]) <
1483 IKFAST_SOLUTION_THRESH &&
1484 IKabs(sj6array[ij6] - sj6array[iij6]) <
1485 IKFAST_SOLUTION_THRESH)
1486 {
1487 j6valid[iij6] = false;
1488 _ij6[1] = iij6;
1489 break;
1490 }
1491 }
1492 j6 = j6array[ij6];
1493 cj6 = cj6array[ij6];
1494 sj6 = sj6array[ij6];
1495 {
1496 IkReal evalcond[3];
1497 IkReal x1043 = IKsin(j6);
1498 IkReal x1044 = IKcos(j6);
1499 IkReal x1045 = ((1.0) * npx);
1500 IkReal x1046 = (npy * x1043);
1501 evalcond[0] = (x1046 + (((-1.0) * x1044 * x1045)));
1502 evalcond[1] =
1503 ((((-1.0) * x1043 * x1045)) + (((-1.0) * npy * x1044)));
1504 evalcond[2] =
1505 (((sj5 * x1046)) + (((-1.0) * sj5 * x1044 * x1045)));
1506 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1507 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
1508 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH)
1509 {
1510 continue;
1511 }
1512 }
1513
1514 rotationfunction0(solutions);
1515 }
1516 }
1517 }
1518 }
1519 }
1520 else
1521 {
1522 {
1523 IkReal j6array[2], cj6array[2], sj6array[2];
1524 bool j6valid[2] = { false };
1525 _nj6 = 2;
1526 CheckValue<IkReal> x1048 =
1527 IKatan2WithCheck(IkReal(((-1.0) * npx * sj5)),
1528 IkReal((npy * sj5)),
1529 IKFAST_ATAN2_MAGTHRESH);
1530 if (!x1048.valid)
1531 {
1532 continue;
1533 }
1534 IkReal x1047 = x1048.value;
1535 j6array[0] = ((-1.0) * x1047);
1536 sj6array[0] = IKsin(j6array[0]);
1537 cj6array[0] = IKcos(j6array[0]);
1538 j6array[1] = ((3.14159265358979) + (((-1.0) * x1047)));
1539 sj6array[1] = IKsin(j6array[1]);
1540 cj6array[1] = IKcos(j6array[1]);
1541 if (j6array[0] > IKPI)
1542 {
1543 j6array[0] -= IK2PI;
1544 }
1545 else if (j6array[0] < -IKPI)
1546 {
1547 j6array[0] += IK2PI;
1548 }
1549 j6valid[0] = true;
1550 if (j6array[1] > IKPI)
1551 {
1552 j6array[1] -= IK2PI;
1553 }
1554 else if (j6array[1] < -IKPI)
1555 {
1556 j6array[1] += IK2PI;
1557 }
1558 j6valid[1] = true;
1559 for (int ij6 = 0; ij6 < 2; ++ij6)
1560 {
1561 if (!j6valid[ij6])
1562 {
1563 continue;
1564 }
1565 _ij6[0] = ij6;
1566 _ij6[1] = -1;
1567 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1568 {
1569 if (j6valid[iij6] &&
1570 IKabs(cj6array[ij6] - cj6array[iij6]) <
1571 IKFAST_SOLUTION_THRESH &&
1572 IKabs(sj6array[ij6] - sj6array[iij6]) <
1573 IKFAST_SOLUTION_THRESH)
1574 {
1575 j6valid[iij6] = false;
1576 _ij6[1] = iij6;
1577 break;
1578 }
1579 }
1580 j6 = j6array[ij6];
1581 cj6 = cj6array[ij6];
1582 sj6 = sj6array[ij6];
1583 {
1584 IkReal evalcond[3];
1585 IkReal x1049 = IKsin(j6);
1586 IkReal x1050 = IKcos(j6);
1587 IkReal x1051 = ((1.0) * npx);
1588 IkReal x1052 = (npy * x1049);
1589 evalcond[0] = (x1052 + (((-1.0) * x1050 * x1051)));
1590 evalcond[1] =
1591 ((((-1.0) * npy * x1050)) + (((-1.0) * x1049 * x1051)));
1592 evalcond[2] =
1593 ((((-1.0) * cj5 * x1050 * x1051)) + ((cj5 * x1052)));
1594 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1595 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
1596 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH)
1597 {
1598 continue;
1599 }
1600 }
1601
1602 rotationfunction0(solutions);
1603 }
1604 }
1605 }
1606 }
1607 }
1608 else
1609 {
1610 {
1611 IkReal j6array[2], cj6array[2], sj6array[2];
1612 bool j6valid[2] = { false };
1613 _nj6 = 2;
1614 CheckValue<IkReal> x1054 = IKatan2WithCheck(
1615 IkReal(((-1.0) * npx)), IkReal(npy), IKFAST_ATAN2_MAGTHRESH);
1616 if (!x1054.valid)
1617 {
1618 continue;
1619 }
1620 IkReal x1053 = x1054.value;
1621 j6array[0] = ((-1.0) * x1053);
1622 sj6array[0] = IKsin(j6array[0]);
1623 cj6array[0] = IKcos(j6array[0]);
1624 j6array[1] = ((3.14159265358979) + (((-1.0) * x1053)));
1625 sj6array[1] = IKsin(j6array[1]);
1626 cj6array[1] = IKcos(j6array[1]);
1627 if (j6array[0] > IKPI)
1628 {
1629 j6array[0] -= IK2PI;
1630 }
1631 else if (j6array[0] < -IKPI)
1632 {
1633 j6array[0] += IK2PI;
1634 }
1635 j6valid[0] = true;
1636 if (j6array[1] > IKPI)
1637 {
1638 j6array[1] -= IK2PI;
1639 }
1640 else if (j6array[1] < -IKPI)
1641 {
1642 j6array[1] += IK2PI;
1643 }
1644 j6valid[1] = true;
1645 for (int ij6 = 0; ij6 < 2; ++ij6)
1646 {
1647 if (!j6valid[ij6])
1648 {
1649 continue;
1650 }
1651 _ij6[0] = ij6;
1652 _ij6[1] = -1;
1653 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1654 {
1655 if (j6valid[iij6] &&
1656 IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
1657 IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
1658 {
1659 j6valid[iij6] = false;
1660 _ij6[1] = iij6;
1661 break;
1662 }
1663 }
1664 j6 = j6array[ij6];
1665 cj6 = cj6array[ij6];
1666 sj6 = sj6array[ij6];
1667 {
1668 IkReal evalcond[3];
1669 IkReal x1055 = IKsin(j6);
1670 IkReal x1056 = IKcos(j6);
1671 IkReal x1057 = ((1.0) * npx);
1672 IkReal x1058 = (npy * x1055);
1673 evalcond[0] = ((((-1.0) * npy * x1056)) + (((-1.0) * x1055 * x1057)));
1674 evalcond[1] = (((sj5 * x1058)) + (((-1.0) * sj5 * x1056 * x1057)));
1675 evalcond[2] = ((((-1.0) * cj5 * x1056 * x1057)) + ((cj5 * x1058)));
1676 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1677 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
1678 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH)
1679 {
1680 continue;
1681 }
1682 }
1683
1684 rotationfunction0(solutions);
1685 }
1686 }
1687 }
1688 }
1689 }
1690 } while (0);
1691 if (bgotonextstatement)
1692 {
1693 bool bgotonextstatement = true;
1694 do
1695 {
1696 evalcond[0] = ((-3.14159265358979) +
1697 (IKfmod(((3.14159265358979) + (IKabs(j5))), 6.28318530717959)));
1698 if (IKabs(evalcond[0]) < 0.0000050000000000)
1699 {
1700 bgotonextstatement = false;
1701 {
1702 IkReal j6eval[3];
1703 sj5 = 0;
1704 cj5 = 1.0;
1705 j5 = 0;
1706 IkReal x1059 = npx * npx;
1707 IkReal x1060 = npy * npy;
1708 IkReal x1061 = (npy * sj3);
1709 IkReal x1062 = (npx * sj3);
1710 j6eval[0] = (x1059 + x1060);
1711 j6eval[1] = ((IKabs(((((-1.0) * cj4 * x1062)) + ((sj4 * x1061))))) +
1712 (IKabs((((cj4 * x1061)) + ((sj4 * x1062))))));
1713 j6eval[2] = IKsign(((((5.0) * x1060)) + (((5.0) * x1059))));
1714 if (IKabs(j6eval[0]) < 0.0000010000000000 ||
1715 IKabs(j6eval[1]) < 0.0000010000000000 ||
1716 IKabs(j6eval[2]) < 0.0000010000000000)
1717 {
1718 {
1719 IkReal evalcond[1];
1720 bool bgotonextstatement = true;
1721 do
1722 {
1723 evalcond[0] =
1724 ((-3.14159265358979) +
1725 (IKfmod(((3.14159265358979) + (IKabs(j3))), 6.28318530717959)));
1726 if (IKabs(evalcond[0]) < 0.0000050000000000)
1727 {
1728 bgotonextstatement = false;
1729 {
1730 IkReal j6eval[1];
1731 sj5 = 0;
1732 cj5 = 1.0;
1733 j5 = 0;
1734 sj3 = 0;
1735 cj3 = 1.0;
1736 j3 = 0;
1737 j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1738 if (IKabs(j6eval[0]) < 0.0000010000000000)
1739 {
1740 continue; // no branches [j6]
1741 }
1742 else
1743 {
1744 {
1745 IkReal j6array[2], cj6array[2], sj6array[2];
1746 bool j6valid[2] = { false };
1747 _nj6 = 2;
1748 CheckValue<IkReal> x1064 =
1749 IKatan2WithCheck(IkReal(((-1.0) * npx)),
1750 IkReal(npy),
1751 IKFAST_ATAN2_MAGTHRESH);
1752 if (!x1064.valid)
1753 {
1754 continue;
1755 }
1756 IkReal x1063 = x1064.value;
1757 j6array[0] = ((-1.0) * x1063);
1758 sj6array[0] = IKsin(j6array[0]);
1759 cj6array[0] = IKcos(j6array[0]);
1760 j6array[1] = ((3.14159265358979) + (((-1.0) * x1063)));
1761 sj6array[1] = IKsin(j6array[1]);
1762 cj6array[1] = IKcos(j6array[1]);
1763 if (j6array[0] > IKPI)
1764 {
1765 j6array[0] -= IK2PI;
1766 }
1767 else if (j6array[0] < -IKPI)
1768 {
1769 j6array[0] += IK2PI;
1770 }
1771 j6valid[0] = true;
1772 if (j6array[1] > IKPI)
1773 {
1774 j6array[1] -= IK2PI;
1775 }
1776 else if (j6array[1] < -IKPI)
1777 {
1778 j6array[1] += IK2PI;
1779 }
1780 j6valid[1] = true;
1781 for (int ij6 = 0; ij6 < 2; ++ij6)
1782 {
1783 if (!j6valid[ij6])
1784 {
1785 continue;
1786 }
1787 _ij6[0] = ij6;
1788 _ij6[1] = -1;
1789 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
1790 {
1791 if (j6valid[iij6] &&
1792 IKabs(cj6array[ij6] - cj6array[iij6]) <
1793 IKFAST_SOLUTION_THRESH &&
1794 IKabs(sj6array[ij6] - sj6array[iij6]) <
1795 IKFAST_SOLUTION_THRESH)
1796 {
1797 j6valid[iij6] = false;
1798 _ij6[1] = iij6;
1799 break;
1800 }
1801 }
1802 j6 = j6array[ij6];
1803 cj6 = cj6array[ij6];
1804 sj6 = sj6array[ij6];
1805 {
1806 IkReal evalcond[1];
1807 evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
1808 (((-1.0) * npx * (IKsin(j6)))));
1809 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
1810 {
1811 continue;
1812 }
1813 }
1814
1815 rotationfunction0(solutions);
1816 }
1817 }
1818 }
1819 }
1820 }
1821 } while (0);
1822 if (bgotonextstatement)
1823 {
1824 bool bgotonextstatement = true;
1825 do
1826 {
1827 if (1)
1828 {
1829 bgotonextstatement = false;
1830 continue; // branch miss [j6]
1831 }
1832 } while (0);
1833 if (bgotonextstatement)
1834 {
1835 }
1836 }
1837 }
1838 }
1839 else
1840 {
1841 {
1842 IkReal j6array[1], cj6array[1], sj6array[1];
1843 bool j6valid[1] = { false };
1844 _nj6 = 1;
1845 IkReal x1065 = ((2.0) * sj3);
1846 CheckValue<IkReal> x1066 = IKatan2WithCheck(
1847 IkReal((((cj4 * npy * x1065)) + ((npx * sj4 * x1065)))),
1848 IkReal(((((-1.0) * cj4 * npx * x1065)) + ((npy * sj4 * x1065)))),
1849 IKFAST_ATAN2_MAGTHRESH);
1850 if (!x1066.valid)
1851 {
1852 continue;
1853 }
1854 CheckValue<IkReal> x1067 = IKPowWithIntegerCheck(
1855 IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))), -1);
1856 if (!x1067.valid)
1857 {
1858 continue;
1859 }
1860 j6array[0] = ((-1.5707963267949) + (x1066.value) +
1861 (((1.5707963267949) * (x1067.value))));
1862 sj6array[0] = IKsin(j6array[0]);
1863 cj6array[0] = IKcos(j6array[0]);
1864 if (j6array[0] > IKPI)
1865 {
1866 j6array[0] -= IK2PI;
1867 }
1868 else if (j6array[0] < -IKPI)
1869 {
1870 j6array[0] += IK2PI;
1871 }
1872 j6valid[0] = true;
1873 for (int ij6 = 0; ij6 < 1; ++ij6)
1874 {
1875 if (!j6valid[ij6])
1876 {
1877 continue;
1878 }
1879 _ij6[0] = ij6;
1880 _ij6[1] = -1;
1881 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
1882 {
1883 if (j6valid[iij6] &&
1884 IKabs(cj6array[ij6] - cj6array[iij6]) <
1885 IKFAST_SOLUTION_THRESH &&
1886 IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
1887 {
1888 j6valid[iij6] = false;
1889 _ij6[1] = iij6;
1890 break;
1891 }
1892 }
1893 j6 = j6array[ij6];
1894 cj6 = cj6array[ij6];
1895 sj6 = sj6array[ij6];
1896 {
1897 IkReal evalcond[2];
1898 IkReal x1068 = IKsin(j6);
1899 IkReal x1069 = IKcos(j6);
1900 IkReal x1070 = ((0.4) * sj3);
1901 IkReal x1071 = ((1.0) * x1069);
1902 evalcond[0] = ((((-1.0) * npx * x1071)) + (((-1.0) * cj4 * x1070)) +
1903 ((npy * x1068)));
1904 evalcond[1] = ((((-1.0) * npy * x1071)) + (((-1.0) * npx * x1068)) +
1905 ((sj4 * x1070)));
1906 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
1907 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
1908 {
1909 continue;
1910 }
1911 }
1912
1913 rotationfunction0(solutions);
1914 }
1915 }
1916 }
1917 }
1918 }
1919 } while (0);
1920 if (bgotonextstatement)
1921 {
1922 bool bgotonextstatement = true;
1923 do
1924 {
1925 evalcond[0] =
1926 ((-3.14159265358979) +
1927 (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j5)))),
1928 6.28318530717959)));
1929 if (IKabs(evalcond[0]) < 0.0000050000000000)
1930 {
1931 bgotonextstatement = false;
1932 {
1933 IkReal j6eval[3];
1934 sj5 = 0;
1935 cj5 = -1.0;
1936 j5 = 3.14159265358979;
1937 IkReal x1072 = npx * npx;
1938 IkReal x1073 = npy * npy;
1939 IkReal x1074 = (cj4 * sj3);
1940 IkReal x1075 = (sj3 * sj4);
1941 j6eval[0] = (x1072 + x1073);
1942 j6eval[1] = ((IKabs((((npx * x1074)) + ((npy * x1075))))) +
1943 (IKabs(((((-1.0) * npy * x1074)) + ((npx * x1075))))));
1944 j6eval[2] = IKsign(((((5.0) * x1073)) + (((5.0) * x1072))));
1945 if (IKabs(j6eval[0]) < 0.0000010000000000 ||
1946 IKabs(j6eval[1]) < 0.0000010000000000 ||
1947 IKabs(j6eval[2]) < 0.0000010000000000)
1948 {
1949 {
1950 IkReal evalcond[1];
1951 bool bgotonextstatement = true;
1952 do
1953 {
1954 evalcond[0] = ((-3.14159265358979) +
1955 (IKfmod(((3.14159265358979) + (IKabs(j3))),
1956 6.28318530717959)));
1957 if (IKabs(evalcond[0]) < 0.0000050000000000)
1958 {
1959 bgotonextstatement = false;
1960 {
1961 IkReal j6eval[1];
1962 sj5 = 0;
1963 cj5 = -1.0;
1964 j5 = 3.14159265358979;
1965 sj3 = 0;
1966 cj3 = 1.0;
1967 j3 = 0;
1968 j6eval[0] = ((IKabs(npy)) + (IKabs(npx)));
1969 if (IKabs(j6eval[0]) < 0.0000010000000000)
1970 {
1971 continue; // no branches [j6]
1972 }
1973 else
1974 {
1975 {
1976 IkReal j6array[2], cj6array[2], sj6array[2];
1977 bool j6valid[2] = { false };
1978 _nj6 = 2;
1979 CheckValue<IkReal> x1077 =
1980 IKatan2WithCheck(IkReal(npx),
1981 IkReal(((-1.0) * npy)),
1982 IKFAST_ATAN2_MAGTHRESH);
1983 if (!x1077.valid)
1984 {
1985 continue;
1986 }
1987 IkReal x1076 = x1077.value;
1988 j6array[0] = ((-1.0) * x1076);
1989 sj6array[0] = IKsin(j6array[0]);
1990 cj6array[0] = IKcos(j6array[0]);
1991 j6array[1] = ((3.14159265358979) + (((-1.0) * x1076)));
1992 sj6array[1] = IKsin(j6array[1]);
1993 cj6array[1] = IKcos(j6array[1]);
1994 if (j6array[0] > IKPI)
1995 {
1996 j6array[0] -= IK2PI;
1997 }
1998 else if (j6array[0] < -IKPI)
1999 {
2000 j6array[0] += IK2PI;
2001 }
2002 j6valid[0] = true;
2003 if (j6array[1] > IKPI)
2004 {
2005 j6array[1] -= IK2PI;
2006 }
2007 else if (j6array[1] < -IKPI)
2008 {
2009 j6array[1] += IK2PI;
2010 }
2011 j6valid[1] = true;
2012 for (int ij6 = 0; ij6 < 2; ++ij6)
2013 {
2014 if (!j6valid[ij6])
2015 {
2016 continue;
2017 }
2018 _ij6[0] = ij6;
2019 _ij6[1] = -1;
2020 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
2021 {
2022 if (j6valid[iij6] &&
2023 IKabs(cj6array[ij6] - cj6array[iij6]) <
2024 IKFAST_SOLUTION_THRESH &&
2025 IKabs(sj6array[ij6] - sj6array[iij6]) <
2026 IKFAST_SOLUTION_THRESH)
2027 {
2028 j6valid[iij6] = false;
2029 _ij6[1] = iij6;
2030 break;
2031 }
2032 }
2033 j6 = j6array[ij6];
2034 cj6 = cj6array[ij6];
2035 sj6 = sj6array[ij6];
2036 {
2037 IkReal evalcond[1];
2038 evalcond[0] = ((((-1.0) * npy * (IKcos(j6)))) +
2039 (((-1.0) * npx * (IKsin(j6)))));
2040 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
2041 {
2042 continue;
2043 }
2044 }
2045
2046 rotationfunction0(solutions);
2047 }
2048 }
2049 }
2050 }
2051 }
2052 } while (0);
2053 if (bgotonextstatement)
2054 {
2055 bool bgotonextstatement = true;
2056 do
2057 {
2058 if (1)
2059 {
2060 bgotonextstatement = false;
2061 continue; // branch miss [j6]
2062 }
2063 } while (0);
2064 if (bgotonextstatement)
2065 {
2066 }
2067 }
2068 }
2069 }
2070 else
2071 {
2072 {
2073 IkReal j6array[1], cj6array[1], sj6array[1];
2074 bool j6valid[1] = { false };
2075 _nj6 = 1;
2076 IkReal x1078 = ((2.0) * sj3);
2077 CheckValue<IkReal> x1079 = IKPowWithIntegerCheck(
2078 IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))), -1);
2079 if (!x1079.valid)
2080 {
2081 continue;
2082 }
2083 CheckValue<IkReal> x1080 = IKatan2WithCheck(
2084 IkReal(((((-1.0) * cj4 * npy * x1078)) + ((npx * sj4 * x1078)))),
2085 IkReal((((cj4 * npx * x1078)) + ((npy * sj4 * x1078)))),
2086 IKFAST_ATAN2_MAGTHRESH);
2087 if (!x1080.valid)
2088 {
2089 continue;
2090 }
2091 j6array[0] = ((-1.5707963267949) +
2092 (((1.5707963267949) * (x1079.value))) + (x1080.value));
2093 sj6array[0] = IKsin(j6array[0]);
2094 cj6array[0] = IKcos(j6array[0]);
2095 if (j6array[0] > IKPI)
2096 {
2097 j6array[0] -= IK2PI;
2098 }
2099 else if (j6array[0] < -IKPI)
2100 {
2101 j6array[0] += IK2PI;
2102 }
2103 j6valid[0] = true;
2104 for (int ij6 = 0; ij6 < 1; ++ij6)
2105 {
2106 if (!j6valid[ij6])
2107 {
2108 continue;
2109 }
2110 _ij6[0] = ij6;
2111 _ij6[1] = -1;
2112 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2113 {
2114 if (j6valid[iij6] &&
2115 IKabs(cj6array[ij6] - cj6array[iij6]) <
2116 IKFAST_SOLUTION_THRESH &&
2117 IKabs(sj6array[ij6] - sj6array[iij6]) <
2118 IKFAST_SOLUTION_THRESH)
2119 {
2120 j6valid[iij6] = false;
2121 _ij6[1] = iij6;
2122 break;
2123 }
2124 }
2125 j6 = j6array[ij6];
2126 cj6 = cj6array[ij6];
2127 sj6 = sj6array[ij6];
2128 {
2129 IkReal evalcond[2];
2130 IkReal x1081 = IKsin(j6);
2131 IkReal x1082 = IKcos(j6);
2132 IkReal x1083 = ((0.4) * sj3);
2133 IkReal x1084 = ((1.0) * npy);
2134 evalcond[0] = ((((-1.0) * x1081 * x1084)) + ((npx * x1082)) +
2135 (((-1.0) * cj4 * x1083)));
2136 evalcond[1] = ((((-1.0) * x1082 * x1084)) + ((sj4 * x1083)) +
2137 (((-1.0) * npx * x1081)));
2138 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2139 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
2140 {
2141 continue;
2142 }
2143 }
2144
2145 rotationfunction0(solutions);
2146 }
2147 }
2148 }
2149 }
2150 }
2151 } while (0);
2152 if (bgotonextstatement)
2153 {
2154 bool bgotonextstatement = true;
2155 do
2156 {
2157 evalcond[0] =
2158 ((-3.14159265358979) +
2159 (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j5)))),
2160 6.28318530717959)));
2161 if (IKabs(evalcond[0]) < 0.0000050000000000)
2162 {
2163 bgotonextstatement = false;
2164 {
2165 IkReal j6eval[3];
2166 sj5 = 1.0;
2167 cj5 = 0;
2168 j5 = 1.5707963267949;
2169 IkReal x1085 = npx * npx;
2170 IkReal x1086 = npy * npy;
2171 IkReal x1087 = ((2.0) * cj3);
2172 IkReal x1088 = ((2.0) * npx);
2173 IkReal x1089 = (sj3 * sj4);
2174 IkReal x1090 = ((2.0) * npy);
2175 j6eval[0] = (x1086 + x1085);
2176 j6eval[1] = ((IKabs((x1088 + ((x1089 * x1090)) + ((npx * x1087))))) +
2177 (IKabs(((((-1.0) * x1090)) + (((-1.0) * npy * x1087)) +
2178 ((x1088 * x1089))))));
2179 j6eval[2] = IKsign(((((5.0) * x1085)) + (((5.0) * x1086))));
2180 if (IKabs(j6eval[0]) < 0.0000010000000000 ||
2181 IKabs(j6eval[1]) < 0.0000010000000000 ||
2182 IKabs(j6eval[2]) < 0.0000010000000000)
2183 {
2184 continue; // no branches [j6]
2185 }
2186 else
2187 {
2188 {
2189 IkReal j6array[1], cj6array[1], sj6array[1];
2190 bool j6valid[1] = { false };
2191 _nj6 = 1;
2192 IkReal x1091 = ((2.0) * cj3);
2193 IkReal x1092 = ((2.0) * npx);
2194 IkReal x1093 = (sj3 * sj4);
2195 IkReal x1094 = ((2.0) * npy);
2196 CheckValue<IkReal> x1095 = IKatan2WithCheck(
2197 IkReal(((((-1.0) * x1094)) + ((x1092 * x1093)) +
2198 (((-1.0) * npy * x1091)))),
2199 IkReal((((npx * x1091)) + x1092 + ((x1093 * x1094)))),
2200 IKFAST_ATAN2_MAGTHRESH);
2201 if (!x1095.valid)
2202 {
2203 continue;
2204 }
2205 CheckValue<IkReal> x1096 = IKPowWithIntegerCheck(
2206 IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))),
2207 -1);
2208 if (!x1096.valid)
2209 {
2210 continue;
2211 }
2212 j6array[0] = ((-1.5707963267949) + (x1095.value) +
2213 (((1.5707963267949) * (x1096.value))));
2214 sj6array[0] = IKsin(j6array[0]);
2215 cj6array[0] = IKcos(j6array[0]);
2216 if (j6array[0] > IKPI)
2217 {
2218 j6array[0] -= IK2PI;
2219 }
2220 else if (j6array[0] < -IKPI)
2221 {
2222 j6array[0] += IK2PI;
2223 }
2224 j6valid[0] = true;
2225 for (int ij6 = 0; ij6 < 1; ++ij6)
2226 {
2227 if (!j6valid[ij6])
2228 {
2229 continue;
2230 }
2231 _ij6[0] = ij6;
2232 _ij6[1] = -1;
2233 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2234 {
2235 if (j6valid[iij6] &&
2236 IKabs(cj6array[ij6] - cj6array[iij6]) <
2237 IKFAST_SOLUTION_THRESH &&
2238 IKabs(sj6array[ij6] - sj6array[iij6]) <
2239 IKFAST_SOLUTION_THRESH)
2240 {
2241 j6valid[iij6] = false;
2242 _ij6[1] = iij6;
2243 break;
2244 }
2245 }
2246 j6 = j6array[ij6];
2247 cj6 = cj6array[ij6];
2248 sj6 = sj6array[ij6];
2249 {
2250 IkReal evalcond[2];
2251 IkReal x1097 = IKsin(j6);
2252 IkReal x1098 = IKcos(j6);
2253 IkReal x1099 = ((1.0) * npx);
2254 evalcond[0] = ((0.4) + (((0.4) * cj3)) + ((npy * x1097)) +
2255 (((-1.0) * x1098 * x1099)));
2256 evalcond[1] =
2257 ((((-1.0) * x1097 * x1099)) + (((-1.0) * npy * x1098)) +
2258 (((0.4) * sj3 * sj4)));
2259 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2260 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
2261 {
2262 continue;
2263 }
2264 }
2265
2266 rotationfunction0(solutions);
2267 }
2268 }
2269 }
2270 }
2271 }
2272 } while (0);
2273 if (bgotonextstatement)
2274 {
2275 bool bgotonextstatement = true;
2276 do
2277 {
2278 evalcond[0] =
2279 ((-3.14159265358979) +
2280 (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j5)))),
2281 6.28318530717959)));
2282 if (IKabs(evalcond[0]) < 0.0000050000000000)
2283 {
2284 bgotonextstatement = false;
2285 {
2286 IkReal j6eval[3];
2287 sj5 = -1.0;
2288 cj5 = 0;
2289 j5 = -1.5707963267949;
2290 IkReal x1100 = npx * npx;
2291 IkReal x1101 = npy * npy;
2292 IkReal x1102 = ((2.0) * cj3);
2293 IkReal x1103 = ((2.0) * npx);
2294 IkReal x1104 = (sj3 * sj4);
2295 IkReal x1105 = ((2.0) * npy);
2296 j6eval[0] = (x1100 + x1101);
2297 j6eval[1] = ((IKabs(((((-1.0) * npx * x1102)) + ((x1104 * x1105)) +
2298 (((-1.0) * x1103))))) +
2299 (IKabs((x1105 + ((npy * x1102)) + ((x1103 * x1104))))));
2300 j6eval[2] = IKsign(((((5.0) * x1101)) + (((5.0) * x1100))));
2301 if (IKabs(j6eval[0]) < 0.0000010000000000 ||
2302 IKabs(j6eval[1]) < 0.0000010000000000 ||
2303 IKabs(j6eval[2]) < 0.0000010000000000)
2304 {
2305 continue; // no branches [j6]
2306 }
2307 else
2308 {
2309 {
2310 IkReal j6array[1], cj6array[1], sj6array[1];
2311 bool j6valid[1] = { false };
2312 _nj6 = 1;
2313 IkReal x1106 = ((2.0) * cj3);
2314 IkReal x1107 = ((2.0) * npx);
2315 IkReal x1108 = (sj3 * sj4);
2316 IkReal x1109 = ((2.0) * npy);
2317 CheckValue<IkReal> x1110 = IKPowWithIntegerCheck(
2318 IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))),
2319 -1);
2320 if (!x1110.valid)
2321 {
2322 continue;
2323 }
2324 CheckValue<IkReal> x1111 = IKatan2WithCheck(
2325 IkReal((x1109 + ((npy * x1106)) + ((x1107 * x1108)))),
2326 IkReal((((x1108 * x1109)) + (((-1.0) * npx * x1106)) +
2327 (((-1.0) * x1107)))),
2328 IKFAST_ATAN2_MAGTHRESH);
2329 if (!x1111.valid)
2330 {
2331 continue;
2332 }
2333 j6array[0] =
2334 ((-1.5707963267949) + (((1.5707963267949) * (x1110.value))) +
2335 (x1111.value));
2336 sj6array[0] = IKsin(j6array[0]);
2337 cj6array[0] = IKcos(j6array[0]);
2338 if (j6array[0] > IKPI)
2339 {
2340 j6array[0] -= IK2PI;
2341 }
2342 else if (j6array[0] < -IKPI)
2343 {
2344 j6array[0] += IK2PI;
2345 }
2346 j6valid[0] = true;
2347 for (int ij6 = 0; ij6 < 1; ++ij6)
2348 {
2349 if (!j6valid[ij6])
2350 {
2351 continue;
2352 }
2353 _ij6[0] = ij6;
2354 _ij6[1] = -1;
2355 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2356 {
2357 if (j6valid[iij6] &&
2358 IKabs(cj6array[ij6] - cj6array[iij6]) <
2359 IKFAST_SOLUTION_THRESH &&
2360 IKabs(sj6array[ij6] - sj6array[iij6]) <
2361 IKFAST_SOLUTION_THRESH)
2362 {
2363 j6valid[iij6] = false;
2364 _ij6[1] = iij6;
2365 break;
2366 }
2367 }
2368 j6 = j6array[ij6];
2369 cj6 = cj6array[ij6];
2370 sj6 = sj6array[ij6];
2371 {
2372 IkReal evalcond[2];
2373 IkReal x1112 = IKsin(j6);
2374 IkReal x1113 = IKcos(j6);
2375 IkReal x1114 = ((1.0) * x1112);
2376 evalcond[0] = ((0.4) + ((npx * x1113)) +
2377 (((-1.0) * npy * x1114)) + (((0.4) * cj3)));
2378 evalcond[1] =
2379 ((((-1.0) * npy * x1113)) + (((-1.0) * npx * x1114)) +
2380 (((0.4) * sj3 * sj4)));
2381 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2382 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
2383 {
2384 continue;
2385 }
2386 }
2387
2388 rotationfunction0(solutions);
2389 }
2390 }
2391 }
2392 }
2393 }
2394 } while (0);
2395 if (bgotonextstatement)
2396 {
2397 bool bgotonextstatement = true;
2398 do
2399 {
2400 if (1)
2401 {
2402 bgotonextstatement = false;
2403 continue; // branch miss [j6]
2404 }
2405 } while (0);
2406 if (bgotonextstatement)
2407 {
2408 }
2409 }
2410 }
2411 }
2412 }
2413 }
2414 }
2415 }
2416 else
2417 {
2418 {
2419 IkReal j6array[1], cj6array[1], sj6array[1];
2420 bool j6valid[1] = { false };
2421 _nj6 = 1;
2422 IkReal x1115 = ((2.0) * sj5);
2423 IkReal x1116 = ((2.0) * sj3);
2424 IkReal x1117 = (cj4 * cj5);
2425 CheckValue<IkReal> x1118 = IKatan2WithCheck(
2426 IkReal((((npy * x1116 * x1117)) + (((-1.0) * npy * x1115)) +
2427 ((npx * sj4 * x1116)) + (((-1.0) * cj3 * npy * x1115)))),
2428 IkReal((((npx * x1115)) + (((-1.0) * npx * x1116 * x1117)) +
2429 ((npy * sj4 * x1116)) + ((cj3 * npx * x1115)))),
2430 IKFAST_ATAN2_MAGTHRESH);
2431 if (!x1118.valid)
2432 {
2433 continue;
2434 }
2435 CheckValue<IkReal> x1119 = IKPowWithIntegerCheck(
2436 IKsign(((((5.0) * (npx * npx))) + (((5.0) * (npy * npy))))), -1);
2437 if (!x1119.valid)
2438 {
2439 continue;
2440 }
2441 j6array[0] =
2442 ((-1.5707963267949) + (x1118.value) + (((1.5707963267949) * (x1119.value))));
2443 sj6array[0] = IKsin(j6array[0]);
2444 cj6array[0] = IKcos(j6array[0]);
2445 if (j6array[0] > IKPI)
2446 {
2447 j6array[0] -= IK2PI;
2448 }
2449 else if (j6array[0] < -IKPI)
2450 {
2451 j6array[0] += IK2PI;
2452 }
2453 j6valid[0] = true;
2454 for (int ij6 = 0; ij6 < 1; ++ij6)
2455 {
2456 if (!j6valid[ij6])
2457 {
2458 continue;
2459 }
2460 _ij6[0] = ij6;
2461 _ij6[1] = -1;
2462 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2463 {
2464 if (j6valid[iij6] &&
2465 IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2466 IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2467 {
2468 j6valid[iij6] = false;
2469 _ij6[1] = iij6;
2470 break;
2471 }
2472 }
2473 j6 = j6array[ij6];
2474 cj6 = cj6array[ij6];
2475 sj6 = sj6array[ij6];
2476 {
2477 IkReal evalcond[4];
2478 IkReal x1120 = IKsin(j6);
2479 IkReal x1121 = IKcos(j6);
2480 IkReal x1122 = ((0.4) * sj3);
2481 IkReal x1123 = ((1.0) * npx);
2482 IkReal x1124 = ((0.4) * sj5);
2483 IkReal x1125 = (npy * x1120);
2484 evalcond[0] =
2485 ((((-1.0) * npy * x1121)) + ((sj4 * x1122)) + (((-1.0) * x1120 * x1123)));
2486 evalcond[1] = (((npz * sj5)) + (((-1.0) * cj4 * x1122)) +
2487 (((-1.0) * cj5 * x1121 * x1123)) + ((cj5 * x1125)));
2488 evalcond[2] = ((0.4) + (((0.4) * cj3)) + (((-1.0) * sj5 * x1121 * x1123)) +
2489 (((-1.0) * cj5 * npz)) + ((sj5 * x1125)));
2490 evalcond[3] = ((((-1.0) * cj4 * cj5 * x1122)) + x1124 + x1125 +
2491 (((-1.0) * x1121 * x1123)) + ((cj3 * x1124)));
2492 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2493 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
2494 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
2495 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
2496 {
2497 continue;
2498 }
2499 }
2500
2501 rotationfunction0(solutions);
2502 }
2503 }
2504 }
2505 }
2506 }
2507 else
2508 {
2509 {
2510 IkReal j6array[1], cj6array[1], sj6array[1];
2511 bool j6valid[1] = { false };
2512 _nj6 = 1;
2513 IkReal x1126 = ((2.0) * cj3);
2514 IkReal x1127 = ((5.0) * sj5);
2515 IkReal x1128 = ((2.0) * npx);
2516 IkReal x1129 = ((2.0) * npy);
2517 IkReal x1130 = ((5.0) * cj5 * npz);
2518 IkReal x1131 = (sj3 * sj4 * sj5);
2519 CheckValue<IkReal> x1132 = IKPowWithIntegerCheck(
2520 IKsign((((x1127 * (npy * npy))) + ((x1127 * (npx * npx))))), -1);
2521 if (!x1132.valid)
2522 {
2523 continue;
2524 }
2525 CheckValue<IkReal> x1133 = IKatan2WithCheck(
2526 IkReal(((((-1.0) * npy * x1126)) + ((npy * x1130)) + (((-1.0) * x1129)) +
2527 ((x1128 * x1131)))),
2528 IkReal((x1128 + ((npx * x1126)) + (((-1.0) * npx * x1130)) + ((x1129 * x1131)))),
2529 IKFAST_ATAN2_MAGTHRESH);
2530 if (!x1133.valid)
2531 {
2532 continue;
2533 }
2534 j6array[0] =
2535 ((-1.5707963267949) + (((1.5707963267949) * (x1132.value))) + (x1133.value));
2536 sj6array[0] = IKsin(j6array[0]);
2537 cj6array[0] = IKcos(j6array[0]);
2538 if (j6array[0] > IKPI)
2539 {
2540 j6array[0] -= IK2PI;
2541 }
2542 else if (j6array[0] < -IKPI)
2543 {
2544 j6array[0] += IK2PI;
2545 }
2546 j6valid[0] = true;
2547 for (int ij6 = 0; ij6 < 1; ++ij6)
2548 {
2549 if (!j6valid[ij6])
2550 {
2551 continue;
2552 }
2553 _ij6[0] = ij6;
2554 _ij6[1] = -1;
2555 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2556 {
2557 if (j6valid[iij6] &&
2558 IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2559 IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2560 {
2561 j6valid[iij6] = false;
2562 _ij6[1] = iij6;
2563 break;
2564 }
2565 }
2566 j6 = j6array[ij6];
2567 cj6 = cj6array[ij6];
2568 sj6 = sj6array[ij6];
2569 {
2570 IkReal evalcond[4];
2571 IkReal x1134 = IKsin(j6);
2572 IkReal x1135 = IKcos(j6);
2573 IkReal x1136 = ((0.4) * sj3);
2574 IkReal x1137 = ((1.0) * npx);
2575 IkReal x1138 = ((0.4) * sj5);
2576 IkReal x1139 = (npy * x1134);
2577 evalcond[0] =
2578 ((((-1.0) * npy * x1135)) + ((sj4 * x1136)) + (((-1.0) * x1134 * x1137)));
2579 evalcond[1] = ((((-1.0) * cj4 * x1136)) + ((npz * sj5)) +
2580 (((-1.0) * cj5 * x1135 * x1137)) + ((cj5 * x1139)));
2581 evalcond[2] = ((0.4) + (((-1.0) * sj5 * x1135 * x1137)) + (((0.4) * cj3)) +
2582 (((-1.0) * cj5 * npz)) + ((sj5 * x1139)));
2583 evalcond[3] = (x1139 + x1138 + ((cj3 * x1138)) + (((-1.0) * cj4 * cj5 * x1136)) +
2584 (((-1.0) * x1135 * x1137)));
2585 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2586 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
2587 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
2588 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
2589 {
2590 continue;
2591 }
2592 }
2593
2594 rotationfunction0(solutions);
2595 }
2596 }
2597 }
2598 }
2599 }
2600 else
2601 {
2602 {
2603 IkReal j6array[1], cj6array[1], sj6array[1];
2604 bool j6valid[1] = { false };
2605 _nj6 = 1;
2606 IkReal x1140 = (cj5 * sj4);
2607 IkReal x1141 = ((5.0) * cj5);
2608 IkReal x1142 = ((5.0) * npz * sj5);
2609 IkReal x1143 = ((2.0) * npy * sj3);
2610 IkReal x1144 = ((2.0) * npx * sj3);
2611 CheckValue<IkReal> x1145 = IKatan2WithCheck(
2612 IkReal((((x1140 * x1144)) + (((-1.0) * npy * x1142)) + ((cj4 * x1143)))),
2613 IkReal((((x1140 * x1143)) + (((-1.0) * cj4 * x1144)) + ((npx * x1142)))),
2614 IKFAST_ATAN2_MAGTHRESH);
2615 if (!x1145.valid)
2616 {
2617 continue;
2618 }
2619 CheckValue<IkReal> x1146 =
2620 IKPowWithIntegerCheck(IKsign((((x1141 * (npx * npx))) + ((x1141 * (npy * npy))))), -1);
2621 if (!x1146.valid)
2622 {
2623 continue;
2624 }
2625 j6array[0] = ((-1.5707963267949) + (x1145.value) + (((1.5707963267949) * (x1146.value))));
2626 sj6array[0] = IKsin(j6array[0]);
2627 cj6array[0] = IKcos(j6array[0]);
2628 if (j6array[0] > IKPI)
2629 {
2630 j6array[0] -= IK2PI;
2631 }
2632 else if (j6array[0] < -IKPI)
2633 {
2634 j6array[0] += IK2PI;
2635 }
2636 j6valid[0] = true;
2637 for (int ij6 = 0; ij6 < 1; ++ij6)
2638 {
2639 if (!j6valid[ij6])
2640 {
2641 continue;
2642 }
2643 _ij6[0] = ij6;
2644 _ij6[1] = -1;
2645 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
2646 {
2647 if (j6valid[iij6] && IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2648 IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2649 {
2650 j6valid[iij6] = false;
2651 _ij6[1] = iij6;
2652 break;
2653 }
2654 }
2655 j6 = j6array[ij6];
2656 cj6 = cj6array[ij6];
2657 sj6 = sj6array[ij6];
2658 {
2659 IkReal evalcond[4];
2660 IkReal x1147 = IKsin(j6);
2661 IkReal x1148 = IKcos(j6);
2662 IkReal x1149 = ((0.4) * sj3);
2663 IkReal x1150 = ((1.0) * npx);
2664 IkReal x1151 = ((0.4) * sj5);
2665 IkReal x1152 = (npy * x1147);
2666 evalcond[0] = (((sj4 * x1149)) + (((-1.0) * x1147 * x1150)) + (((-1.0) * npy * x1148)));
2667 evalcond[1] = (((npz * sj5)) + (((-1.0) * cj4 * x1149)) +
2668 (((-1.0) * cj5 * x1148 * x1150)) + ((cj5 * x1152)));
2669 evalcond[2] = ((0.4) + (((0.4) * cj3)) + ((sj5 * x1152)) + (((-1.0) * cj5 * npz)) +
2670 (((-1.0) * sj5 * x1148 * x1150)));
2671 evalcond[3] = (x1151 + x1152 + ((cj3 * x1151)) + (((-1.0) * x1148 * x1150)) +
2672 (((-1.0) * cj4 * cj5 * x1149)));
2673 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
2674 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
2675 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
2676 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
2677 {
2678 continue;
2679 }
2680 }
2681
2682 rotationfunction0(solutions);
2683 }
2684 }
2685 }
2686 }
2687 }
2688 }
2689 }
2690 }
2691 }
2692 else
2693 {
2694 {
2695 IkReal j6array[2], cj6array[2], sj6array[2];
2696 bool j6valid[2] = { false };
2697 _nj6 = 2;
2698 CheckValue<IkReal> x1155 =
2699 IKatan2WithCheck(IkReal(((-1.0) * npy)), IkReal(((-1.0) * npx)), IKFAST_ATAN2_MAGTHRESH);
2700 if (!x1155.valid)
2701 {
2702 continue;
2703 }
2704 IkReal x1153 = ((1.0) * (x1155.value));
2705 if ((((npx * npx) + (npy * npy))) < -0.00001)
2706 continue;
2707 CheckValue<IkReal> x1156 = IKPowWithIntegerCheck(IKabs(IKsqrt(((npx * npx) + (npy * npy)))), -1);
2708 if (!x1156.valid)
2709 {
2710 continue;
2711 }
2712 if ((((0.4) * sj3 * sj4 * (x1156.value))) < -1 - IKFAST_SINCOS_THRESH ||
2713 (((0.4) * sj3 * sj4 * (x1156.value))) > 1 + IKFAST_SINCOS_THRESH)
2714 continue;
2715 IkReal x1154 = IKasin(((0.4) * sj3 * sj4 * (x1156.value)));
2716 j6array[0] = ((((-1.0) * x1153)) + (((-1.0) * x1154)));
2717 sj6array[0] = IKsin(j6array[0]);
2718 cj6array[0] = IKcos(j6array[0]);
2719 j6array[1] = ((3.14159265358979) + x1154 + (((-1.0) * x1153)));
2720 sj6array[1] = IKsin(j6array[1]);
2721 cj6array[1] = IKcos(j6array[1]);
2722 if (j6array[0] > IKPI)
2723 {
2724 j6array[0] -= IK2PI;
2725 }
2726 else if (j6array[0] < -IKPI)
2727 {
2728 j6array[0] += IK2PI;
2729 }
2730 j6valid[0] = true;
2731 if (j6array[1] > IKPI)
2732 {
2733 j6array[1] -= IK2PI;
2734 }
2735 else if (j6array[1] < -IKPI)
2736 {
2737 j6array[1] += IK2PI;
2738 }
2739 j6valid[1] = true;
2740 for (int ij6 = 0; ij6 < 2; ++ij6)
2741 {
2742 if (!j6valid[ij6])
2743 {
2744 continue;
2745 }
2746 _ij6[0] = ij6;
2747 _ij6[1] = -1;
2748 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
2749 {
2750 if (j6valid[iij6] && IKabs(cj6array[ij6] - cj6array[iij6]) < IKFAST_SOLUTION_THRESH &&
2751 IKabs(sj6array[ij6] - sj6array[iij6]) < IKFAST_SOLUTION_THRESH)
2752 {
2753 j6valid[iij6] = false;
2754 _ij6[1] = iij6;
2755 break;
2756 }
2757 }
2758 j6 = j6array[ij6];
2759 cj6 = cj6array[ij6];
2760 sj6 = sj6array[ij6];
2761
2762 {
2763 IkReal j5eval[3];
2764 IkReal x1157 = (cj6 * npx);
2765 IkReal x1158 = (npy * sj6);
2766 IkReal x1159 = ((2.0) * cj3);
2767 IkReal x1160 = ((5.0) * npz);
2768 IkReal x1161 = ((0.8) * cj4 * sj3);
2769 IkReal x1162 = (cj4 * npz * sj3);
2770 j5eval[0] = (x1158 + x1162 + ((cj3 * x1158)) + (((-1.0) * cj3 * x1157)) + (((-1.0) * x1157)));
2771 j5eval[1] = ((IKabs(((-0.8) + (((-1.6) * cj3)) + ((npz * x1160)) + (((-0.8) * (cj3 * cj3)))))) +
2772 (IKabs((x1161 + ((cj3 * x1161)) + ((x1158 * x1160)) + (((-1.0) * x1157 * x1160))))));
2773 j5eval[2] = IKsign(((((-1.0) * x1157 * x1159)) + ((x1158 * x1159)) + (((2.0) * x1158)) +
2774 (((2.0) * x1162)) + (((-2.0) * x1157))));
2775 if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ||
2776 IKabs(j5eval[2]) < 0.0000010000000000)
2777 {
2778 {
2779 IkReal j5eval[3];
2780 IkReal x1163 = cj3 * cj3;
2781 IkReal x1164 = cj4 * cj4;
2782 IkReal x1165 = ((10.0) * cj3);
2783 IkReal x1166 = (npy * sj6);
2784 IkReal x1167 = (cj6 * npx);
2785 IkReal x1168 = ((4.0) * x1164);
2786 IkReal x1169 = ((10.0) * cj4 * sj3);
2787 j5eval[0] = ((1.0) + x1164 + x1163 + (((-1.0) * x1163 * x1164)) + (((2.0) * cj3)));
2788 j5eval[1] =
2789 IKsign(((4.0) + x1168 + (((8.0) * cj3)) + (((-1.0) * x1163 * x1168)) + (((4.0) * x1163))));
2790 j5eval[2] = ((IKabs((((x1165 * x1167)) + ((npz * x1169)) + (((10.0) * x1167)) +
2791 (((-10.0) * x1166)) + (((-1.0) * x1165 * x1166))))) +
2792 (IKabs((((x1166 * x1169)) + (((10.0) * npz)) + ((npz * x1165)) +
2793 (((-1.0) * x1167 * x1169))))));
2794 if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ||
2795 IKabs(j5eval[2]) < 0.0000010000000000)
2796 {
2797 {
2798 IkReal j5eval[3];
2799 IkReal x1170 = cj6 * cj6;
2800 IkReal x1171 = npy * npy;
2801 IkReal x1172 = npx * npx;
2802 IkReal x1173 = npz * npz;
2803 IkReal x1174 = ((2.0) * cj3);
2804 IkReal x1175 = (cj6 * npx);
2805 IkReal x1176 = ((2.0) * npz);
2806 IkReal x1177 = (cj4 * sj3);
2807 IkReal x1178 = ((2.0) * npy * sj6);
2808 IkReal x1179 = (x1170 * x1171);
2809 IkReal x1180 = (x1170 * x1172);
2810 j5eval[0] = (x1180 + x1173 + x1171 + (((-1.0) * x1175 * x1178)) + (((-1.0) * x1179)));
2811 j5eval[1] =
2812 ((IKabs((((x1174 * x1175)) + ((x1176 * x1177)) + (((2.0) * x1175)) +
2813 (((-1.0) * npy * sj6 * x1174)) + (((-1.0) * x1178))))) +
2814 (IKabs((x1176 + ((x1177 * x1178)) + ((npz * x1174)) + (((-2.0) * x1175 * x1177))))));
2815 j5eval[2] = IKsign(((((5.0) * x1180)) + (((-5.0) * x1179)) + (((5.0) * x1171)) +
2816 (((5.0) * x1173)) + (((-10.0) * npy * sj6 * x1175))));
2817 if (IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 ||
2818 IKabs(j5eval[2]) < 0.0000010000000000)
2819 {
2820 {
2821 IkReal evalcond[2];
2822 bool bgotonextstatement = true;
2823 do
2824 {
2825 evalcond[0] = ((-3.14159265358979) +
2826 (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j3)))),
2827 6.28318530717959)));
2828 evalcond[1] = pp;
2829 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
2830 IKabs(evalcond[1]) < 0.0000050000000000)
2831 {
2832 bgotonextstatement = false;
2833 {
2834 IkReal j5eval[1];
2835 sj3 = 0;
2836 cj3 = -1.0;
2837 j3 = 3.14159265358979;
2838 j5eval[0] = IKabs((((cj6 * npx)) + (((-1.0) * npy * sj6))));
2839 if (IKabs(j5eval[0]) < 0.0000000100000000)
2840 {
2841 continue; // no branches [j5]
2842 }
2843 else
2844 {
2845 IkReal op[2 + 1], zeror[2];
2846 int numroots;
2847 IkReal x1181 = (npy * sj6);
2848 IkReal x1182 = (cj6 * npx);
2849 op[0] = (x1182 + (((-1.0) * x1181)));
2850 op[1] = 0;
2851 op[2] = (x1181 + (((-1.0) * x1182)));
2852 polyroots2(op, zeror, numroots);
2853 IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
2854 int numsolutions = 0;
2855 for (int ij5 = 0; ij5 < numroots; ++ij5)
2856 {
2857 IkReal htj5 = zeror[ij5];
2858 tempj5array[0] = ((2.0) * (atan(htj5)));
2859 for (int kj5 = 0; kj5 < 1; ++kj5)
2860 {
2861 j5array[numsolutions] = tempj5array[kj5];
2862 if (j5array[numsolutions] > IKPI)
2863 {
2864 j5array[numsolutions] -= IK2PI;
2865 }
2866 else if (j5array[numsolutions] < -IKPI)
2867 {
2868 j5array[numsolutions] += IK2PI;
2869 }
2870 sj5array[numsolutions] = IKsin(j5array[numsolutions]);
2871 cj5array[numsolutions] = IKcos(j5array[numsolutions]);
2872 numsolutions++;
2873 }
2874 }
2875 bool j5valid[2] = { true, true };
2876 _nj5 = 2;
2877 for (int ij5 = 0; ij5 < numsolutions; ++ij5)
2878 {
2879 if (!j5valid[ij5])
2880 {
2881 continue;
2882 }
2883 j5 = j5array[ij5];
2884 cj5 = cj5array[ij5];
2885 sj5 = sj5array[ij5];
2886 htj5 = IKtan(j5 / 2);
2887
2888 _ij5[0] = ij5;
2889 _ij5[1] = -1;
2890 for (int iij5 = ij5 + 1; iij5 < numsolutions; ++iij5)
2891 {
2892 if (j5valid[iij5] &&
2893 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2894 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2895 {
2896 j5valid[iij5] = false;
2897 _ij5[1] = iij5;
2898 break;
2899 }
2900 }
2901 rotationfunction0(solutions);
2902 }
2903 }
2904 }
2905 }
2906 } while (0);
2907 if (bgotonextstatement)
2908 {
2909 bool bgotonextstatement = true;
2910 do
2911 {
2912 if (1)
2913 {
2914 bgotonextstatement = false;
2915 continue; // branch miss [j5]
2916 }
2917 } while (0);
2918 if (bgotonextstatement)
2919 {
2920 }
2921 }
2922 }
2923 }
2924 else
2925 {
2926 {
2927 IkReal j5array[1], cj5array[1], sj5array[1];
2928 bool j5valid[1] = { false };
2929 _nj5 = 1;
2930 IkReal x1183 = cj6 * cj6;
2931 IkReal x1184 = npy * npy;
2932 IkReal x1185 = (npy * sj6);
2933 IkReal x1186 = ((2.0) * cj3);
2934 IkReal x1187 = (cj6 * npx);
2935 IkReal x1188 = ((2.0) * npz);
2936 IkReal x1189 = (cj4 * sj3);
2937 IkReal x1190 = ((5.0) * x1184);
2938 CheckValue<IkReal> x1191 = IKPowWithIntegerCheck(
2939 IKsign((x1190 + (((-10.0) * x1185 * x1187)) + (((5.0) * (npz * npz))) +
2940 (((-1.0) * x1183 * x1190)) + (((5.0) * x1183 * (npx * npx))))),
2941 -1);
2942 if (!x1191.valid)
2943 {
2944 continue;
2945 }
2946 CheckValue<IkReal> x1192 =
2947 IKatan2WithCheck(IkReal((((x1186 * x1187)) + ((x1188 * x1189)) + (((2.0) * x1187)) +
2948 (((-2.0) * x1185)) + (((-1.0) * x1185 * x1186)))),
2949 IkReal(((((2.0) * x1185 * x1189)) + x1188 +
2950 (((-2.0) * x1187 * x1189)) + ((npz * x1186)))),
2951 IKFAST_ATAN2_MAGTHRESH);
2952 if (!x1192.valid)
2953 {
2954 continue;
2955 }
2956 j5array[0] =
2957 ((-1.5707963267949) + (((1.5707963267949) * (x1191.value))) + (x1192.value));
2958 sj5array[0] = IKsin(j5array[0]);
2959 cj5array[0] = IKcos(j5array[0]);
2960 if (j5array[0] > IKPI)
2961 {
2962 j5array[0] -= IK2PI;
2963 }
2964 else if (j5array[0] < -IKPI)
2965 {
2966 j5array[0] += IK2PI;
2967 }
2968 j5valid[0] = true;
2969 for (int ij5 = 0; ij5 < 1; ++ij5)
2970 {
2971 if (!j5valid[ij5])
2972 {
2973 continue;
2974 }
2975 _ij5[0] = ij5;
2976 _ij5[1] = -1;
2977 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
2978 {
2979 if (j5valid[iij5] &&
2980 IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
2981 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
2982 {
2983 j5valid[iij5] = false;
2984 _ij5[1] = iij5;
2985 break;
2986 }
2987 }
2988 j5 = j5array[ij5];
2989 cj5 = cj5array[ij5];
2990 sj5 = sj5array[ij5];
2991 {
2992 IkReal evalcond[4];
2993 IkReal x1193 = IKcos(j5);
2994 IkReal x1194 = IKsin(j5);
2995 IkReal x1195 = (npy * sj6);
2996 IkReal x1196 = ((1.0) * npz);
2997 IkReal x1197 = ((1.0) * cj6 * npx);
2998 IkReal x1198 = ((0.4) * cj4 * sj3);
2999 IkReal x1199 = ((0.4) * x1194);
3000 IkReal x1200 = ((0.4) * x1193);
3001 evalcond[0] = (x1200 + ((x1194 * x1198)) + (((-1.0) * x1196)) + ((cj3 * x1200)));
3002 evalcond[1] = ((((-1.0) * x1193 * x1197)) + (((-1.0) * x1198)) + ((npz * x1194)) +
3003 ((x1193 * x1195)));
3004 evalcond[2] = ((0.4) + (((-1.0) * x1193 * x1196)) + ((x1194 * x1195)) +
3005 (((-1.0) * x1194 * x1197)) + (((0.4) * cj3)));
3006 evalcond[3] = (x1199 + x1195 + (((-1.0) * x1193 * x1198)) + (((-1.0) * x1197)) +
3007 ((cj3 * x1199)));
3008 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3009 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3010 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3011 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3012 {
3013 continue;
3014 }
3015 }
3016
3017 rotationfunction0(solutions);
3018 }
3019 }
3020 }
3021 }
3022 }
3023 else
3024 {
3025 {
3026 IkReal j5array[1], cj5array[1], sj5array[1];
3027 bool j5valid[1] = { false };
3028 _nj5 = 1;
3029 IkReal x1201 = cj3 * cj3;
3030 IkReal x1202 = cj4 * cj4;
3031 IkReal x1203 = ((10.0) * cj3);
3032 IkReal x1204 = (npy * sj6);
3033 IkReal x1205 = (cj6 * npx);
3034 IkReal x1206 = ((10.0) * cj4 * sj3);
3035 IkReal x1207 = ((4.0) * x1202);
3036 CheckValue<IkReal> x1208 =
3037 IKPowWithIntegerCheck(IKsign(((4.0) + x1207 + (((8.0) * cj3)) + (((4.0) * x1201)) +
3038 (((-1.0) * x1201 * x1207)))),
3039 -1);
3040 if (!x1208.valid)
3041 {
3042 continue;
3043 }
3044 CheckValue<IkReal> x1209 =
3045 IKatan2WithCheck(IkReal(((((-10.0) * x1204)) + (((10.0) * x1205)) + ((npz * x1206)) +
3046 ((x1203 * x1205)) + (((-1.0) * x1203 * x1204)))),
3047 IkReal((((x1204 * x1206)) + (((10.0) * npz)) + ((npz * x1203)) +
3048 (((-1.0) * x1205 * x1206)))),
3049 IKFAST_ATAN2_MAGTHRESH);
3050 if (!x1209.valid)
3051 {
3052 continue;
3053 }
3054 j5array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x1208.value))) + (x1209.value));
3055 sj5array[0] = IKsin(j5array[0]);
3056 cj5array[0] = IKcos(j5array[0]);
3057 if (j5array[0] > IKPI)
3058 {
3059 j5array[0] -= IK2PI;
3060 }
3061 else if (j5array[0] < -IKPI)
3062 {
3063 j5array[0] += IK2PI;
3064 }
3065 j5valid[0] = true;
3066 for (int ij5 = 0; ij5 < 1; ++ij5)
3067 {
3068 if (!j5valid[ij5])
3069 {
3070 continue;
3071 }
3072 _ij5[0] = ij5;
3073 _ij5[1] = -1;
3074 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
3075 {
3076 if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
3077 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
3078 {
3079 j5valid[iij5] = false;
3080 _ij5[1] = iij5;
3081 break;
3082 }
3083 }
3084 j5 = j5array[ij5];
3085 cj5 = cj5array[ij5];
3086 sj5 = sj5array[ij5];
3087 {
3088 IkReal evalcond[4];
3089 IkReal x1210 = IKcos(j5);
3090 IkReal x1211 = IKsin(j5);
3091 IkReal x1212 = (npy * sj6);
3092 IkReal x1213 = ((1.0) * npz);
3093 IkReal x1214 = ((1.0) * cj6 * npx);
3094 IkReal x1215 = ((0.4) * cj4 * sj3);
3095 IkReal x1216 = ((0.4) * x1211);
3096 IkReal x1217 = ((0.4) * x1210);
3097 evalcond[0] = (x1217 + ((cj3 * x1217)) + ((x1211 * x1215)) + (((-1.0) * x1213)));
3098 evalcond[1] = (((x1210 * x1212)) + ((npz * x1211)) + (((-1.0) * x1210 * x1214)) +
3099 (((-1.0) * x1215)));
3100 evalcond[2] = ((0.4) + (((0.4) * cj3)) + (((-1.0) * x1210 * x1213)) +
3101 (((-1.0) * x1211 * x1214)) + ((x1211 * x1212)));
3102 evalcond[3] =
3103 (x1212 + x1216 + (((-1.0) * x1210 * x1215)) + ((cj3 * x1216)) + (((-1.0) * x1214)));
3104 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3105 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3106 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3107 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3108 {
3109 continue;
3110 }
3111 }
3112
3113 rotationfunction0(solutions);
3114 }
3115 }
3116 }
3117 }
3118 }
3119 else
3120 {
3121 {
3122 IkReal j5array[1], cj5array[1], sj5array[1];
3123 bool j5valid[1] = { false };
3124 _nj5 = 1;
3125 IkReal x1218 = (npy * sj6);
3126 IkReal x1219 = (cj6 * npx);
3127 IkReal x1220 = ((2.0) * cj3);
3128 IkReal x1221 = ((5.0) * npz);
3129 IkReal x1222 = ((0.8) * cj4 * sj3);
3130 CheckValue<IkReal> x1223 = IKatan2WithCheck(
3131 IkReal(((-0.8) + ((npz * x1221)) + (((-1.6) * cj3)) + (((-0.8) * (cj3 * cj3))))),
3132 IkReal((x1222 + ((cj3 * x1222)) + ((x1218 * x1221)) + (((-1.0) * x1219 * x1221)))),
3133 IKFAST_ATAN2_MAGTHRESH);
3134 if (!x1223.valid)
3135 {
3136 continue;
3137 }
3138 CheckValue<IkReal> x1224 = IKPowWithIntegerCheck(
3139 IKsign((((x1218 * x1220)) + (((2.0) * x1218)) + (((-1.0) * x1219 * x1220)) +
3140 (((-2.0) * x1219)) + (((2.0) * cj4 * npz * sj3)))),
3141 -1);
3142 if (!x1224.valid)
3143 {
3144 continue;
3145 }
3146 j5array[0] = ((-1.5707963267949) + (x1223.value) + (((1.5707963267949) * (x1224.value))));
3147 sj5array[0] = IKsin(j5array[0]);
3148 cj5array[0] = IKcos(j5array[0]);
3149 if (j5array[0] > IKPI)
3150 {
3151 j5array[0] -= IK2PI;
3152 }
3153 else if (j5array[0] < -IKPI)
3154 {
3155 j5array[0] += IK2PI;
3156 }
3157 j5valid[0] = true;
3158 for (int ij5 = 0; ij5 < 1; ++ij5)
3159 {
3160 if (!j5valid[ij5])
3161 {
3162 continue;
3163 }
3164 _ij5[0] = ij5;
3165 _ij5[1] = -1;
3166 for (int iij5 = ij5 + 1; iij5 < 1; ++iij5)
3167 {
3168 if (j5valid[iij5] && IKabs(cj5array[ij5] - cj5array[iij5]) < IKFAST_SOLUTION_THRESH &&
3169 IKabs(sj5array[ij5] - sj5array[iij5]) < IKFAST_SOLUTION_THRESH)
3170 {
3171 j5valid[iij5] = false;
3172 _ij5[1] = iij5;
3173 break;
3174 }
3175 }
3176 j5 = j5array[ij5];
3177 cj5 = cj5array[ij5];
3178 sj5 = sj5array[ij5];
3179 {
3180 IkReal evalcond[4];
3181 IkReal x1225 = IKcos(j5);
3182 IkReal x1226 = IKsin(j5);
3183 IkReal x1227 = (npy * sj6);
3184 IkReal x1228 = ((1.0) * npz);
3185 IkReal x1229 = ((1.0) * cj6 * npx);
3186 IkReal x1230 = ((0.4) * cj4 * sj3);
3187 IkReal x1231 = ((0.4) * x1226);
3188 IkReal x1232 = ((0.4) * x1225);
3189 evalcond[0] = (((cj3 * x1232)) + x1232 + (((-1.0) * x1228)) + ((x1226 * x1230)));
3190 evalcond[1] =
3191 (((npz * x1226)) + (((-1.0) * x1225 * x1229)) + (((-1.0) * x1230)) + ((x1225 * x1227)));
3192 evalcond[2] = ((0.4) + (((-1.0) * x1225 * x1228)) + (((0.4) * cj3)) +
3193 (((-1.0) * x1226 * x1229)) + ((x1226 * x1227)));
3194 evalcond[3] =
3195 (((cj3 * x1231)) + x1231 + x1227 + (((-1.0) * x1225 * x1230)) + (((-1.0) * x1229)));
3196 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3197 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3198 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3199 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3200 {
3201 continue;
3202 }
3203 }
3204
3205 rotationfunction0(solutions);
3206 }
3207 }
3208 }
3209 }
3210 }
3211 }
3212 }
3213 }
3214 }
3215 }
3216 }
3217 return solutions.GetNumSolutions() > 0;
3218 }
3219 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions)
3220 {
3221 for (int rotationiter = 0; rotationiter < 1; ++rotationiter)
3222 {
3223 IkReal x76 = (r02 * sj5);
3224 IkReal x77 = ((1.0) * cj5);
3225 IkReal x78 = ((1.0) * sj6);
3226 IkReal x79 = ((1.0) * cj4);
3227 IkReal x80 = ((1.0) * cj3);
3228 IkReal x81 = ((1.0) * sj4);
3229 IkReal x82 = ((1.0) * cj6);
3230 IkReal x83 = ((((-1.0) * r01 * x78)) + ((cj6 * r00)));
3231 IkReal x84 = (((cj6 * r10)) + (((-1.0) * r11 * x78)));
3232 IkReal x85 = ((((-1.0) * r21 * x78)) + ((cj6 * r20)));
3233 IkReal x86 = ((((-1.0) * r00 * x78)) + (((-1.0) * r01 * x82)));
3234 IkReal x87 = ((((-1.0) * r11 * x82)) + (((-1.0) * r10 * x78)));
3235 IkReal x88 = ((((-1.0) * r20 * x78)) + (((-1.0) * r21 * x82)));
3236 IkReal x89 = (((sj5 * x83)) + ((cj5 * r02)));
3237 IkReal x90 = ((((-1.0) * x77 * x83)) + x76);
3238 IkReal x91 = ((((-1.0) * x77 * x84)) + ((r12 * sj5)));
3239 IkReal x92 = (((cj5 * r12)) + ((sj5 * x84)));
3240 IkReal x93 = (((r22 * sj5)) + (((-1.0) * x77 * x85)));
3241 IkReal x94 = (((cj5 * r22)) + ((sj5 * x85)));
3242 IkReal x95 = (sj4 * x86);
3243 IkReal x96 = ((1.0) * x88);
3244 IkReal x97 = ((((-1.0) * x81 * x87)) + ((cj4 * x91)));
3245 IkReal x98 = ((((-1.0) * x81 * x88)) + ((cj4 * x93)));
3246 new_r00 = ((((-1.0) * x80 * ((((cj4 * (((((-1.0) * cj5 * x83)) + x76)))) + (((-1.0) * x95)))))) + ((sj3 * x89)));
3247 new_r01 = ((((-1.0) * x79 * x86)) + (((-1.0) * x81 * x90)));
3248 new_r02 = (((cj3 * x89)) + ((sj3 * (((((-1.0) * x81 * x86)) + ((cj4 * x90)))))));
3249 new_r10 = (((sj3 * x92)) + (((-1.0) * x80 * x97)));
3250 new_r11 = ((((-1.0) * x79 * x87)) + (((-1.0) * x81 * x91)));
3251 new_r12 = (((sj3 * x97)) + ((cj3 * x92)));
3252 new_r20 = (((sj3 * x94)) + (((-1.0) * x80 * x98)));
3253 new_r21 = ((((-1.0) * x79 * x88)) + (((-1.0) * x81 * x93)));
3254 new_r22 = (((sj3 * x98)) + ((cj3 * x94)));
3255 {
3256 IkReal j1array[2], cj1array[2], sj1array[2];
3257 bool j1valid[2] = { false };
3258 _nj1 = 2;
3259 cj1array[0] = new_r22;
3260 if (cj1array[0] >= -1 - IKFAST_SINCOS_THRESH && cj1array[0] <= 1 + IKFAST_SINCOS_THRESH)
3261 {
3262 j1valid[0] = j1valid[1] = true;
3263 j1array[0] = IKacos(cj1array[0]);
3264 sj1array[0] = IKsin(j1array[0]);
3265 cj1array[1] = cj1array[0];
3266 j1array[1] = -j1array[0];
3267 sj1array[1] = -sj1array[0];
3268 }
3269 else if (isnan(cj1array[0]))
3270 {
3271 // probably any value will work
3272 j1valid[0] = true;
3273 cj1array[0] = 1;
3274 sj1array[0] = 0;
3275 j1array[0] = 0;
3276 }
3277 for (int ij1 = 0; ij1 < 2; ++ij1)
3278 {
3279 if (!j1valid[ij1])
3280 {
3281 continue;
3282 }
3283 _ij1[0] = ij1;
3284 _ij1[1] = -1;
3285 for (int iij1 = ij1 + 1; iij1 < 2; ++iij1)
3286 {
3287 if (j1valid[iij1] && IKabs(cj1array[ij1] - cj1array[iij1]) < IKFAST_SOLUTION_THRESH &&
3288 IKabs(sj1array[ij1] - sj1array[iij1]) < IKFAST_SOLUTION_THRESH)
3289 {
3290 j1valid[iij1] = false;
3291 _ij1[1] = iij1;
3292 break;
3293 }
3294 }
3295 j1 = j1array[ij1];
3296 cj1 = cj1array[ij1];
3297 sj1 = sj1array[ij1];
3298
3299 {
3300 IkReal j0eval[3];
3301 j0eval[0] = sj1;
3302 j0eval[1] = ((IKabs(new_r12)) + (IKabs(new_r02)));
3303 j0eval[2] = IKsign(sj1);
3304 if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 ||
3305 IKabs(j0eval[2]) < 0.0000010000000000)
3306 {
3307 {
3308 IkReal j2eval[3];
3309 j2eval[0] = sj1;
3310 j2eval[1] = IKsign(sj1);
3311 j2eval[2] = ((IKabs(new_r20)) + (IKabs(new_r21)));
3312 if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
3313 IKabs(j2eval[2]) < 0.0000010000000000)
3314 {
3315 {
3316 IkReal j0eval[2];
3317 j0eval[0] = new_r12;
3318 j0eval[1] = sj1;
3319 if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000)
3320 {
3321 {
3322 IkReal evalcond[5];
3323 bool bgotonextstatement = true;
3324 do
3325 {
3326 evalcond[0] =
3327 ((-3.14159265358979) + (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
3328 evalcond[1] = new_r21;
3329 evalcond[2] = new_r02;
3330 evalcond[3] = new_r12;
3331 evalcond[4] = new_r20;
3332 if (IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 &&
3333 IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 &&
3334 IKabs(evalcond[4]) < 0.0000050000000000)
3335 {
3336 bgotonextstatement = false;
3337 IkReal j2mul = 1;
3338 j2 = 0;
3339 j0mul = -1.0;
3340 if (IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
3341 IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
3342 IKabs(IKsqr(((-1.0) * new_r10)) + IKsqr(((-1.0) * new_r00)) - 1) <=
3343 IKFAST_SINCOS_THRESH)
3344 continue;
3345 j0 = IKatan2(((-1.0) * new_r10), ((-1.0) * new_r00));
3346 {
3347 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3348 vinfos[0].jointtype = 1;
3349 vinfos[0].foffset = j0;
3350 vinfos[0].fmul = j0mul;
3351 vinfos[0].freeind = 0;
3352 vinfos[0].maxsolutions = 0;
3353 vinfos[1].jointtype = 1;
3354 vinfos[1].foffset = j1;
3355 vinfos[1].indices[0] = _ij1[0];
3356 vinfos[1].indices[1] = _ij1[1];
3357 vinfos[1].maxsolutions = _nj1;
3358 vinfos[2].jointtype = 1;
3359 vinfos[2].foffset = j2;
3360 vinfos[2].fmul = j2mul;
3361 vinfos[2].freeind = 0;
3362 vinfos[2].maxsolutions = 0;
3363 vinfos[3].jointtype = 1;
3364 vinfos[3].foffset = j3;
3365 vinfos[3].indices[0] = _ij3[0];
3366 vinfos[3].indices[1] = _ij3[1];
3367 vinfos[3].maxsolutions = _nj3;
3368 vinfos[4].jointtype = 1;
3369 vinfos[4].foffset = j4;
3370 vinfos[4].indices[0] = _ij4[0];
3371 vinfos[4].indices[1] = _ij4[1];
3372 vinfos[4].maxsolutions = _nj4;
3373 vinfos[5].jointtype = 1;
3374 vinfos[5].foffset = j5;
3375 vinfos[5].indices[0] = _ij5[0];
3376 vinfos[5].indices[1] = _ij5[1];
3377 vinfos[5].maxsolutions = _nj5;
3378 vinfos[6].jointtype = 1;
3379 vinfos[6].foffset = j6;
3380 vinfos[6].indices[0] = _ij6[0];
3381 vinfos[6].indices[1] = _ij6[1];
3382 vinfos[6].maxsolutions = _nj6;
3383 std::vector<int> vfree(1);
3384 vfree[0] = 2;
3385 solutions.AddSolution(vinfos, vfree);
3386 }
3387 }
3388 } while (0);
3389 if (bgotonextstatement)
3390 {
3391 bool bgotonextstatement = true;
3392 do
3393 {
3394 evalcond[0] = ((-3.14159265358979) +
3395 (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
3396 6.28318530717959)));
3397 evalcond[1] = new_r21;
3398 evalcond[2] = new_r02;
3399 evalcond[3] = new_r12;
3400 evalcond[4] = new_r20;
3401 if (IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 &&
3402 IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 &&
3403 IKabs(evalcond[4]) < 0.0000050000000000)
3404 {
3405 bgotonextstatement = false;
3406 IkReal j2mul = 1;
3407 j2 = 0;
3408 j0mul = 1.0;
3409 if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
3410 IKabs(((-1.0) * new_r11)) < IKFAST_ATAN2_MAGTHRESH &&
3411 IKabs(IKsqr(new_r10) + IKsqr(((-1.0) * new_r11)) - 1) <= IKFAST_SINCOS_THRESH)
3412 continue;
3413 j0 = IKatan2(new_r10, ((-1.0) * new_r11));
3414 {
3415 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3416 vinfos[0].jointtype = 1;
3417 vinfos[0].foffset = j0;
3418 vinfos[0].fmul = j0mul;
3419 vinfos[0].freeind = 0;
3420 vinfos[0].maxsolutions = 0;
3421 vinfos[1].jointtype = 1;
3422 vinfos[1].foffset = j1;
3423 vinfos[1].indices[0] = _ij1[0];
3424 vinfos[1].indices[1] = _ij1[1];
3425 vinfos[1].maxsolutions = _nj1;
3426 vinfos[2].jointtype = 1;
3427 vinfos[2].foffset = j2;
3428 vinfos[2].fmul = j2mul;
3429 vinfos[2].freeind = 0;
3430 vinfos[2].maxsolutions = 0;
3431 vinfos[3].jointtype = 1;
3432 vinfos[3].foffset = j3;
3433 vinfos[3].indices[0] = _ij3[0];
3434 vinfos[3].indices[1] = _ij3[1];
3435 vinfos[3].maxsolutions = _nj3;
3436 vinfos[4].jointtype = 1;
3437 vinfos[4].foffset = j4;
3438 vinfos[4].indices[0] = _ij4[0];
3439 vinfos[4].indices[1] = _ij4[1];
3440 vinfos[4].maxsolutions = _nj4;
3441 vinfos[5].jointtype = 1;
3442 vinfos[5].foffset = j5;
3443 vinfos[5].indices[0] = _ij5[0];
3444 vinfos[5].indices[1] = _ij5[1];
3445 vinfos[5].maxsolutions = _nj5;
3446 vinfos[6].jointtype = 1;
3447 vinfos[6].foffset = j6;
3448 vinfos[6].indices[0] = _ij6[0];
3449 vinfos[6].indices[1] = _ij6[1];
3450 vinfos[6].maxsolutions = _nj6;
3451 std::vector<int> vfree(1);
3452 vfree[0] = 2;
3453 solutions.AddSolution(vinfos, vfree);
3454 }
3455 }
3456 } while (0);
3457 if (bgotonextstatement)
3458 {
3459 bool bgotonextstatement = true;
3460 do
3461 {
3462 evalcond[0] = ((IKabs(new_r12)) + (IKabs(new_r02)));
3463 if (IKabs(evalcond[0]) < 0.0000050000000000)
3464 {
3465 bgotonextstatement = false;
3466 {
3467 IkReal j0eval[1];
3468 new_r02 = 0;
3469 new_r12 = 0;
3470 new_r20 = 0;
3471 new_r21 = 0;
3472 IkReal x99 = new_r22 * new_r22;
3473 IkReal x100 = ((16.0) * new_r10);
3474 IkReal x101 = ((16.0) * new_r01);
3475 IkReal x102 = ((16.0) * new_r22);
3476 IkReal x103 = ((8.0) * new_r11);
3477 IkReal x104 = ((8.0) * new_r00);
3478 IkReal x105 = (x100 * x99);
3479 IkReal x106 = (x101 * x99);
3480 j0eval[0] =
3481 ((IKabs(
3482 ((((16.0) * new_r00)) + (((-32.0) * new_r00 * x99)) + ((new_r11 * x102))))) +
3483 (IKabs(((((-1.0) * x105)) + x100))) + (IKabs(((((-1.0) * x100)) + x105))) +
3484 (IKabs(((((32.0) * new_r11)) + (((-1.0) * new_r00 * x102)) +
3485 (((-16.0) * new_r11 * x99))))) +
3486 (IKabs(((((-1.0) * x101)) + x106))) +
3487 (IKabs((((x103 * x99)) + (((-1.0) * new_r22 * x104))))) +
3488 (IKabs((((new_r22 * x103)) + (((-1.0) * x104))))) +
3489 (IKabs(((((-1.0) * x106)) + x101))));
3490 if (IKabs(j0eval[0]) < 0.0000000100000000)
3491 {
3492 continue; // no branches [j0, j2]
3493 }
3494 else
3495 {
3496 IkReal op[4 + 1], zeror[4];
3497 int numroots;
3498 IkReal j0evalpoly[1];
3499 IkReal x107 = new_r22 * new_r22;
3500 IkReal x108 = ((16.0) * new_r10);
3501 IkReal x109 = (new_r11 * new_r22);
3502 IkReal x110 = (x107 * x108);
3503 IkReal x111 = ((((8.0) * x109)) + (((-8.0) * new_r00)));
3504 op[0] = x111;
3505 op[1] = ((((-1.0) * x110)) + x108);
3506 op[2] = ((((16.0) * x109)) + (((-32.0) * new_r00 * x107)) + (((16.0) * new_r00)));
3507 op[3] = ((((-1.0) * x108)) + x110);
3508 op[4] = x111;
3509 polyroots4(op, zeror, numroots);
3510 IkReal j0array[4], cj0array[4], sj0array[4], tempj0array[1];
3511 int numsolutions = 0;
3512 for (int ij0 = 0; ij0 < numroots; ++ij0)
3513 {
3514 IkReal htj0 = zeror[ij0];
3515 tempj0array[0] = ((2.0) * (atan(htj0)));
3516 for (int kj0 = 0; kj0 < 1; ++kj0)
3517 {
3518 j0array[numsolutions] = tempj0array[kj0];
3519 if (j0array[numsolutions] > IKPI)
3520 {
3521 j0array[numsolutions] -= IK2PI;
3522 }
3523 else if (j0array[numsolutions] < -IKPI)
3524 {
3525 j0array[numsolutions] += IK2PI;
3526 }
3527 sj0array[numsolutions] = IKsin(j0array[numsolutions]);
3528 cj0array[numsolutions] = IKcos(j0array[numsolutions]);
3529 numsolutions++;
3530 }
3531 }
3532 bool j0valid[4] = { true, true, true, true };
3533 _nj0 = 4;
3534 for (int ij0 = 0; ij0 < numsolutions; ++ij0)
3535 {
3536 if (!j0valid[ij0])
3537 {
3538 continue;
3539 }
3540 j0 = j0array[ij0];
3541 cj0 = cj0array[ij0];
3542 sj0 = sj0array[ij0];
3543 htj0 = IKtan(j0 / 2);
3544
3545 IkReal x112 = ((16.0) * new_r01);
3546 IkReal x113 = new_r22 * new_r22;
3547 IkReal x114 = (new_r00 * new_r22);
3548 IkReal x115 = ((8.0) * x114);
3549 IkReal x116 = (new_r11 * x113);
3550 IkReal x117 = (x112 * x113);
3551 IkReal x118 = ((8.0) * x116);
3552 j0evalpoly[0] =
3553 (((htj0 * (((((-1.0) * x117)) + x112)))) +
3554 (((htj0 * htj0 * htj0) * (((((-1.0) * x112)) + x117)))) + (((-1.0) * x115)) +
3555 x118 +
3556 (((htj0 * htj0) *
3557 (((((32.0) * new_r11)) + (((-16.0) * x114)) + (((-16.0) * x116)))))) +
3558 (((htj0 * htj0 * htj0 * htj0) * (((((-1.0) * x115)) + x118)))));
3559 if (IKabs(j0evalpoly[0]) > 0.0000001000000000)
3560 {
3561 continue;
3562 }
3563 _ij0[0] = ij0;
3564 _ij0[1] = -1;
3565 for (int iij0 = ij0 + 1; iij0 < numsolutions; ++iij0)
3566 {
3567 if (j0valid[iij0] &&
3568 IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
3569 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
3570 {
3571 j0valid[iij0] = false;
3572 _ij0[1] = iij0;
3573 break;
3574 }
3575 }
3576 {
3577 IkReal j2eval[3];
3578 new_r02 = 0;
3579 new_r12 = 0;
3580 new_r20 = 0;
3581 new_r21 = 0;
3582 IkReal x119 = cj0 * cj0;
3583 IkReal x120 = new_r22 * new_r22;
3584 IkReal x121 = ((1.0) * cj0);
3585 IkReal x122 = (new_r22 * sj0);
3586 IkReal x123 = (x120 + x119 + (((-1.0) * x119 * x120)));
3587 j2eval[0] = x123;
3588 j2eval[1] =
3589 ((IKabs(((((-1.0) * new_r10 * x122)) + (((-1.0) * new_r11 * x121))))) +
3590 (IKabs(((((-1.0) * new_r10 * x121)) + ((new_r11 * x122))))));
3591 j2eval[2] = IKsign(x123);
3592 if (IKabs(j2eval[0]) < 0.0000010000000000 ||
3593 IKabs(j2eval[1]) < 0.0000010000000000 ||
3594 IKabs(j2eval[2]) < 0.0000010000000000)
3595 {
3596 {
3597 IkReal j2eval[1];
3598 new_r02 = 0;
3599 new_r12 = 0;
3600 new_r20 = 0;
3601 new_r21 = 0;
3602 j2eval[0] = new_r22;
3603 if (IKabs(j2eval[0]) < 0.0000010000000000)
3604 {
3605 {
3606 IkReal j2eval[1];
3607 new_r02 = 0;
3608 new_r12 = 0;
3609 new_r20 = 0;
3610 new_r21 = 0;
3611 j2eval[0] = cj0;
3612 if (IKabs(j2eval[0]) < 0.0000010000000000)
3613 {
3614 {
3615 IkReal evalcond[1];
3616 bool bgotonextstatement = true;
3617 do
3618 {
3619 evalcond[0] = ((-3.14159265358979) +
3620 (IKfmod(((3.14159265358979) +
3621 (IKabs(((-1.5707963267949) + j0)))),
3622 6.28318530717959)));
3623 if (IKabs(evalcond[0]) < 0.0000050000000000)
3624 {
3625 bgotonextstatement = false;
3626 {
3627 IkReal j2array[1], cj2array[1], sj2array[1];
3628 bool j2valid[1] = { false };
3629 _nj2 = 1;
3630 if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
3631 IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH &&
3632 IKabs(IKsqr(new_r00) + IKsqr(new_r01) - 1) <=
3633 IKFAST_SINCOS_THRESH)
3634 continue;
3635 j2array[0] = IKatan2(new_r00, new_r01);
3636 sj2array[0] = IKsin(j2array[0]);
3637 cj2array[0] = IKcos(j2array[0]);
3638 if (j2array[0] > IKPI)
3639 {
3640 j2array[0] -= IK2PI;
3641 }
3642 else if (j2array[0] < -IKPI)
3643 {
3644 j2array[0] += IK2PI;
3645 }
3646 j2valid[0] = true;
3647 for (int ij2 = 0; ij2 < 1; ++ij2)
3648 {
3649 if (!j2valid[ij2])
3650 {
3651 continue;
3652 }
3653 _ij2[0] = ij2;
3654 _ij2[1] = -1;
3655 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
3656 {
3657 if (j2valid[iij2] &&
3658 IKabs(cj2array[ij2] - cj2array[iij2]) <
3659 IKFAST_SOLUTION_THRESH &&
3660 IKabs(sj2array[ij2] - sj2array[iij2]) <
3661 IKFAST_SOLUTION_THRESH)
3662 {
3663 j2valid[iij2] = false;
3664 _ij2[1] = iij2;
3665 break;
3666 }
3667 }
3668 j2 = j2array[ij2];
3669 cj2 = cj2array[ij2];
3670 sj2 = sj2array[ij2];
3671 {
3672 IkReal evalcond[4];
3673 IkReal x124 = IKsin(j2);
3674 IkReal x125 = IKcos(j2);
3675 evalcond[0] = x125;
3676 evalcond[1] = ((-1.0) * x124);
3677 evalcond[2] = (x124 + (((-1.0) * new_r00)));
3678 evalcond[3] = (x125 + (((-1.0) * new_r01)));
3679 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3680 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3681 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3682 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3683 {
3684 continue;
3685 }
3686 }
3687
3688 {
3689 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3690 vinfos[0].jointtype = 1;
3691 vinfos[0].foffset = j0;
3692 vinfos[0].indices[0] = _ij0[0];
3693 vinfos[0].indices[1] = _ij0[1];
3694 vinfos[0].maxsolutions = _nj0;
3695 vinfos[1].jointtype = 1;
3696 vinfos[1].foffset = j1;
3697 vinfos[1].indices[0] = _ij1[0];
3698 vinfos[1].indices[1] = _ij1[1];
3699 vinfos[1].maxsolutions = _nj1;
3700 vinfos[2].jointtype = 1;
3701 vinfos[2].foffset = j2;
3702 vinfos[2].indices[0] = _ij2[0];
3703 vinfos[2].indices[1] = _ij2[1];
3704 vinfos[2].maxsolutions = _nj2;
3705 vinfos[3].jointtype = 1;
3706 vinfos[3].foffset = j3;
3707 vinfos[3].indices[0] = _ij3[0];
3708 vinfos[3].indices[1] = _ij3[1];
3709 vinfos[3].maxsolutions = _nj3;
3710 vinfos[4].jointtype = 1;
3711 vinfos[4].foffset = j4;
3712 vinfos[4].indices[0] = _ij4[0];
3713 vinfos[4].indices[1] = _ij4[1];
3714 vinfos[4].maxsolutions = _nj4;
3715 vinfos[5].jointtype = 1;
3716 vinfos[5].foffset = j5;
3717 vinfos[5].indices[0] = _ij5[0];
3718 vinfos[5].indices[1] = _ij5[1];
3719 vinfos[5].maxsolutions = _nj5;
3720 vinfos[6].jointtype = 1;
3721 vinfos[6].foffset = j6;
3722 vinfos[6].indices[0] = _ij6[0];
3723 vinfos[6].indices[1] = _ij6[1];
3724 vinfos[6].maxsolutions = _nj6;
3725 std::vector<int> vfree(0);
3726 solutions.AddSolution(vinfos, vfree);
3727 }
3728 }
3729 }
3730 }
3731 } while (0);
3732 if (bgotonextstatement)
3733 {
3734 bool bgotonextstatement = true;
3735 do
3736 {
3737 evalcond[0] = ((-3.14159265358979) +
3738 (IKfmod(((3.14159265358979) +
3739 (IKabs(((1.5707963267949) + j0)))),
3740 6.28318530717959)));
3741 if (IKabs(evalcond[0]) < 0.0000050000000000)
3742 {
3743 bgotonextstatement = false;
3744 {
3745 IkReal j2array[1], cj2array[1], sj2array[1];
3746 bool j2valid[1] = { false };
3747 _nj2 = 1;
3748 if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
3749 IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
3750 IKabs(IKsqr(((-1.0) * new_r00)) +
3751 IKsqr(((-1.0) * new_r01)) - 1) <=
3752 IKFAST_SINCOS_THRESH)
3753 continue;
3754 j2array[0] =
3755 IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
3756 sj2array[0] = IKsin(j2array[0]);
3757 cj2array[0] = IKcos(j2array[0]);
3758 if (j2array[0] > IKPI)
3759 {
3760 j2array[0] -= IK2PI;
3761 }
3762 else if (j2array[0] < -IKPI)
3763 {
3764 j2array[0] += IK2PI;
3765 }
3766 j2valid[0] = true;
3767 for (int ij2 = 0; ij2 < 1; ++ij2)
3768 {
3769 if (!j2valid[ij2])
3770 {
3771 continue;
3772 }
3773 _ij2[0] = ij2;
3774 _ij2[1] = -1;
3775 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
3776 {
3777 if (j2valid[iij2] &&
3778 IKabs(cj2array[ij2] - cj2array[iij2]) <
3779 IKFAST_SOLUTION_THRESH &&
3780 IKabs(sj2array[ij2] - sj2array[iij2]) <
3781 IKFAST_SOLUTION_THRESH)
3782 {
3783 j2valid[iij2] = false;
3784 _ij2[1] = iij2;
3785 break;
3786 }
3787 }
3788 j2 = j2array[ij2];
3789 cj2 = cj2array[ij2];
3790 sj2 = sj2array[ij2];
3791 {
3792 IkReal evalcond[4];
3793 IkReal x126 = IKcos(j2);
3794 IkReal x127 = IKsin(j2);
3795 evalcond[0] = x126;
3796 evalcond[1] = (x127 + new_r00);
3797 evalcond[2] = (x126 + new_r01);
3798 evalcond[3] = ((-1.0) * x127);
3799 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
3800 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
3801 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
3802 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH)
3803 {
3804 continue;
3805 }
3806 }
3807
3808 {
3809 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
3810 vinfos[0].jointtype = 1;
3811 vinfos[0].foffset = j0;
3812 vinfos[0].indices[0] = _ij0[0];
3813 vinfos[0].indices[1] = _ij0[1];
3814 vinfos[0].maxsolutions = _nj0;
3815 vinfos[1].jointtype = 1;
3816 vinfos[1].foffset = j1;
3817 vinfos[1].indices[0] = _ij1[0];
3818 vinfos[1].indices[1] = _ij1[1];
3819 vinfos[1].maxsolutions = _nj1;
3820 vinfos[2].jointtype = 1;
3821 vinfos[2].foffset = j2;
3822 vinfos[2].indices[0] = _ij2[0];
3823 vinfos[2].indices[1] = _ij2[1];
3824 vinfos[2].maxsolutions = _nj2;
3825 vinfos[3].jointtype = 1;
3826 vinfos[3].foffset = j3;
3827 vinfos[3].indices[0] = _ij3[0];
3828 vinfos[3].indices[1] = _ij3[1];
3829 vinfos[3].maxsolutions = _nj3;
3830 vinfos[4].jointtype = 1;
3831 vinfos[4].foffset = j4;
3832 vinfos[4].indices[0] = _ij4[0];
3833 vinfos[4].indices[1] = _ij4[1];
3834 vinfos[4].maxsolutions = _nj4;
3835 vinfos[5].jointtype = 1;
3836 vinfos[5].foffset = j5;
3837 vinfos[5].indices[0] = _ij5[0];
3838 vinfos[5].indices[1] = _ij5[1];
3839 vinfos[5].maxsolutions = _nj5;
3840 vinfos[6].jointtype = 1;
3841 vinfos[6].foffset = j6;
3842 vinfos[6].indices[0] = _ij6[0];
3843 vinfos[6].indices[1] = _ij6[1];
3844 vinfos[6].maxsolutions = _nj6;
3845 std::vector<int> vfree(0);
3846 solutions.AddSolution(vinfos, vfree);
3847 }
3848 }
3849 }
3850 }
3851 } while (0);
3852 if (bgotonextstatement)
3853 {
3854 bool bgotonextstatement = true;
3855 do
3856 {
3857 IkReal x128 = new_r22 * new_r22;
3858 CheckValue<IkReal> x129 =
3859 IKPowWithIntegerCheck(((1.0) + (((-1.0) * x128))), -1);
3860 if (!x129.valid)
3861 {
3862 continue;
3863 }
3864 if ((((-1.0) * x128 * (x129.value))) < -0.00001)
3865 continue;
3866 IkReal gconst12 = IKsqrt(((-1.0) * x128 * (x129.value)));
3867 evalcond[0] =
3868 ((-3.14159265358979) +
3869 (IKfmod(((3.14159265358979) +
3870 (IKabs((cj0 + (((-1.0) * gconst12))))) +
3871 (IKabs(((-1.0) + (IKsign(sj0)))))),
3872 6.28318530717959)));
3873 if (IKabs(evalcond[0]) < 0.0000050000000000)
3874 {
3875 bgotonextstatement = false;
3876 {
3877 IkReal j2eval[1];
3878 IkReal x130 = new_r22 * new_r22;
3879 new_r02 = 0;
3880 new_r12 = 0;
3881 new_r20 = 0;
3882 new_r21 = 0;
3883 if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
3884 -0.00001)
3885 continue;
3886 sj0 =
3887 IKsqrt(((1.0) + (((-1.0) * (gconst12 * gconst12)))));
3888 cj0 = gconst12;
3889 if ((gconst12) < -1 - IKFAST_SINCOS_THRESH ||
3890 (gconst12) > 1 + IKFAST_SINCOS_THRESH)
3891 continue;
3892 j0 = IKacos(gconst12);
3893 CheckValue<IkReal> x131 = IKPowWithIntegerCheck(
3894 ((1.0) + (((-1.0) * x130))), -1);
3895 if (!x131.valid)
3896 {
3897 continue;
3898 }
3899 if ((((-1.0) * x130 * (x131.value))) < -0.00001)
3900 continue;
3901 IkReal gconst12 = IKsqrt(((-1.0) * x130 * (x131.value)));
3902 j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
3903 if (IKabs(j2eval[0]) < 0.0000010000000000)
3904 {
3905 {
3906 IkReal j2array[1], cj2array[1], sj2array[1];
3907 bool j2valid[1] = { false };
3908 _nj2 = 1;
3909 CheckValue<IkReal> x132 =
3910 IKPowWithIntegerCheck(gconst12, -1);
3911 if (!x132.valid)
3912 {
3913 continue;
3914 }
3915 if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
3916 -0.00001)
3917 continue;
3918 if (IKabs(((-1.0) * new_r10 * (x132.value))) <
3919 IKFAST_ATAN2_MAGTHRESH &&
3920 IKabs((((new_r01 *
3921 (IKsqrt(((1.0) +
3922 (((-1.0) *
3923 (gconst12 * gconst12)))))))) +
3924 (((-1.0) * gconst12 * new_r11)))) <
3925 IKFAST_ATAN2_MAGTHRESH &&
3926 IKabs(IKsqr(((-1.0) * new_r10 * (x132.value))) +
3927 IKsqr((
3928 ((new_r01 *
3929 (IKsqrt(((1.0) + (((-1.0) *
3930 (gconst12 *
3931 gconst12)))))))) +
3932 (((-1.0) * gconst12 * new_r11)))) -
3933 1) <= IKFAST_SINCOS_THRESH)
3934 continue;
3935 j2array[0] = IKatan2(
3936 ((-1.0) * new_r10 * (x132.value)),
3937 (((new_r01 *
3938 (IKsqrt(((1.0) + (((-1.0) * (gconst12 *
3939 gconst12)))))))) +
3940 (((-1.0) * gconst12 * new_r11))));
3941 sj2array[0] = IKsin(j2array[0]);
3942 cj2array[0] = IKcos(j2array[0]);
3943 if (j2array[0] > IKPI)
3944 {
3945 j2array[0] -= IK2PI;
3946 }
3947 else if (j2array[0] < -IKPI)
3948 {
3949 j2array[0] += IK2PI;
3950 }
3951 j2valid[0] = true;
3952 for (int ij2 = 0; ij2 < 1; ++ij2)
3953 {
3954 if (!j2valid[ij2])
3955 {
3956 continue;
3957 }
3958 _ij2[0] = ij2;
3959 _ij2[1] = -1;
3960 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
3961 {
3962 if (j2valid[iij2] &&
3963 IKabs(cj2array[ij2] - cj2array[iij2]) <
3964 IKFAST_SOLUTION_THRESH &&
3965 IKabs(sj2array[ij2] - sj2array[iij2]) <
3966 IKFAST_SOLUTION_THRESH)
3967 {
3968 j2valid[iij2] = false;
3969 _ij2[1] = iij2;
3970 break;
3971 }
3972 }
3973 j2 = j2array[ij2];
3974 cj2 = cj2array[ij2];
3975 sj2 = sj2array[ij2];
3976 {
3977 IkReal evalcond[8];
3978 IkReal x133 = IKsin(j2);
3979 IkReal x134 = IKcos(j2);
3980 if ((((1.0) +
3981 (((-1.0) * (gconst12 * gconst12))))) <
3982 -0.00001)
3983 continue;
3984 IkReal x135 = IKsqrt(
3985 ((1.0) + (((-1.0) * (gconst12 * gconst12)))));
3986 IkReal x136 = ((1.0) * x135);
3987 evalcond[0] = x134;
3988 evalcond[1] = ((-1.0) * x133);
3989 evalcond[2] = (((gconst12 * x133)) + new_r10);
3990 evalcond[3] = (((gconst12 * x134)) + new_r11);
3991 evalcond[4] =
3992 ((((-1.0) * x133 * x136)) + new_r00);
3993 evalcond[5] =
3994 ((((-1.0) * x134 * x136)) + new_r01);
3995 evalcond[6] = ((((-1.0) * new_r00 * x136)) +
3996 x133 + ((gconst12 * new_r10)));
3997 evalcond[7] = ((((-1.0) * new_r01 * x136)) +
3998 x134 + ((gconst12 * new_r11)));
3999 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
4000 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
4001 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
4002 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
4003 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
4004 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
4005 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
4006 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4007 {
4008 continue;
4009 }
4010 }
4011
4012 {
4013 std::vector<IkSingleDOFSolutionBase<IkReal> >
4014 vinfos(7);
4015 vinfos[0].jointtype = 1;
4016 vinfos[0].foffset = j0;
4017 vinfos[0].indices[0] = _ij0[0];
4018 vinfos[0].indices[1] = _ij0[1];
4019 vinfos[0].maxsolutions = _nj0;
4020 vinfos[1].jointtype = 1;
4021 vinfos[1].foffset = j1;
4022 vinfos[1].indices[0] = _ij1[0];
4023 vinfos[1].indices[1] = _ij1[1];
4024 vinfos[1].maxsolutions = _nj1;
4025 vinfos[2].jointtype = 1;
4026 vinfos[2].foffset = j2;
4027 vinfos[2].indices[0] = _ij2[0];
4028 vinfos[2].indices[1] = _ij2[1];
4029 vinfos[2].maxsolutions = _nj2;
4030 vinfos[3].jointtype = 1;
4031 vinfos[3].foffset = j3;
4032 vinfos[3].indices[0] = _ij3[0];
4033 vinfos[3].indices[1] = _ij3[1];
4034 vinfos[3].maxsolutions = _nj3;
4035 vinfos[4].jointtype = 1;
4036 vinfos[4].foffset = j4;
4037 vinfos[4].indices[0] = _ij4[0];
4038 vinfos[4].indices[1] = _ij4[1];
4039 vinfos[4].maxsolutions = _nj4;
4040 vinfos[5].jointtype = 1;
4041 vinfos[5].foffset = j5;
4042 vinfos[5].indices[0] = _ij5[0];
4043 vinfos[5].indices[1] = _ij5[1];
4044 vinfos[5].maxsolutions = _nj5;
4045 vinfos[6].jointtype = 1;
4046 vinfos[6].foffset = j6;
4047 vinfos[6].indices[0] = _ij6[0];
4048 vinfos[6].indices[1] = _ij6[1];
4049 vinfos[6].maxsolutions = _nj6;
4050 std::vector<int> vfree(0);
4051 solutions.AddSolution(vinfos, vfree);
4052 }
4053 }
4054 }
4055 }
4056 else
4057 {
4058 {
4059 IkReal j2array[1], cj2array[1], sj2array[1];
4060 bool j2valid[1] = { false };
4061 _nj2 = 1;
4062 CheckValue<IkReal> x137 =
4063 IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
4064 IkReal(((-1.0) * new_r11)),
4065 IKFAST_ATAN2_MAGTHRESH);
4066 if (!x137.valid)
4067 {
4068 continue;
4069 }
4070 CheckValue<IkReal> x138 =
4071 IKPowWithIntegerCheck(IKsign(gconst12), -1);
4072 if (!x138.valid)
4073 {
4074 continue;
4075 }
4076 j2array[0] = ((-1.5707963267949) + (x137.value) +
4077 (((1.5707963267949) * (x138.value))));
4078 sj2array[0] = IKsin(j2array[0]);
4079 cj2array[0] = IKcos(j2array[0]);
4080 if (j2array[0] > IKPI)
4081 {
4082 j2array[0] -= IK2PI;
4083 }
4084 else if (j2array[0] < -IKPI)
4085 {
4086 j2array[0] += IK2PI;
4087 }
4088 j2valid[0] = true;
4089 for (int ij2 = 0; ij2 < 1; ++ij2)
4090 {
4091 if (!j2valid[ij2])
4092 {
4093 continue;
4094 }
4095 _ij2[0] = ij2;
4096 _ij2[1] = -1;
4097 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4098 {
4099 if (j2valid[iij2] &&
4100 IKabs(cj2array[ij2] - cj2array[iij2]) <
4101 IKFAST_SOLUTION_THRESH &&
4102 IKabs(sj2array[ij2] - sj2array[iij2]) <
4103 IKFAST_SOLUTION_THRESH)
4104 {
4105 j2valid[iij2] = false;
4106 _ij2[1] = iij2;
4107 break;
4108 }
4109 }
4110 j2 = j2array[ij2];
4111 cj2 = cj2array[ij2];
4112 sj2 = sj2array[ij2];
4113 {
4114 IkReal evalcond[8];
4115 IkReal x139 = IKsin(j2);
4116 IkReal x140 = IKcos(j2);
4117 if ((((1.0) +
4118 (((-1.0) * (gconst12 * gconst12))))) <
4119 -0.00001)
4120 continue;
4121 IkReal x141 = IKsqrt(
4122 ((1.0) + (((-1.0) * (gconst12 * gconst12)))));
4123 IkReal x142 = ((1.0) * x141);
4124 evalcond[0] = x140;
4125 evalcond[1] = ((-1.0) * x139);
4126 evalcond[2] = (((gconst12 * x139)) + new_r10);
4127 evalcond[3] = (((gconst12 * x140)) + new_r11);
4128 evalcond[4] =
4129 ((((-1.0) * x139 * x142)) + new_r00);
4130 evalcond[5] =
4131 ((((-1.0) * x140 * x142)) + new_r01);
4132 evalcond[6] =
4133 (x139 + (((-1.0) * new_r00 * x142)) +
4134 ((gconst12 * new_r10)));
4135 evalcond[7] = ((((-1.0) * new_r01 * x142)) +
4136 x140 + ((gconst12 * new_r11)));
4137 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
4138 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
4139 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
4140 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
4141 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
4142 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
4143 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
4144 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4145 {
4146 continue;
4147 }
4148 }
4149
4150 {
4151 std::vector<IkSingleDOFSolutionBase<IkReal> >
4152 vinfos(7);
4153 vinfos[0].jointtype = 1;
4154 vinfos[0].foffset = j0;
4155 vinfos[0].indices[0] = _ij0[0];
4156 vinfos[0].indices[1] = _ij0[1];
4157 vinfos[0].maxsolutions = _nj0;
4158 vinfos[1].jointtype = 1;
4159 vinfos[1].foffset = j1;
4160 vinfos[1].indices[0] = _ij1[0];
4161 vinfos[1].indices[1] = _ij1[1];
4162 vinfos[1].maxsolutions = _nj1;
4163 vinfos[2].jointtype = 1;
4164 vinfos[2].foffset = j2;
4165 vinfos[2].indices[0] = _ij2[0];
4166 vinfos[2].indices[1] = _ij2[1];
4167 vinfos[2].maxsolutions = _nj2;
4168 vinfos[3].jointtype = 1;
4169 vinfos[3].foffset = j3;
4170 vinfos[3].indices[0] = _ij3[0];
4171 vinfos[3].indices[1] = _ij3[1];
4172 vinfos[3].maxsolutions = _nj3;
4173 vinfos[4].jointtype = 1;
4174 vinfos[4].foffset = j4;
4175 vinfos[4].indices[0] = _ij4[0];
4176 vinfos[4].indices[1] = _ij4[1];
4177 vinfos[4].maxsolutions = _nj4;
4178 vinfos[5].jointtype = 1;
4179 vinfos[5].foffset = j5;
4180 vinfos[5].indices[0] = _ij5[0];
4181 vinfos[5].indices[1] = _ij5[1];
4182 vinfos[5].maxsolutions = _nj5;
4183 vinfos[6].jointtype = 1;
4184 vinfos[6].foffset = j6;
4185 vinfos[6].indices[0] = _ij6[0];
4186 vinfos[6].indices[1] = _ij6[1];
4187 vinfos[6].maxsolutions = _nj6;
4188 std::vector<int> vfree(0);
4189 solutions.AddSolution(vinfos, vfree);
4190 }
4191 }
4192 }
4193 }
4194 }
4195 }
4196 } while (0);
4197 if (bgotonextstatement)
4198 {
4199 bool bgotonextstatement = true;
4200 do
4201 {
4202 IkReal x143 = new_r22 * new_r22;
4203 CheckValue<IkReal> x144 =
4204 IKPowWithIntegerCheck(((1.0) + (((-1.0) * x143))), -1);
4205 if (!x144.valid)
4206 {
4207 continue;
4208 }
4209 if ((((-1.0) * x143 * (x144.value))) < -0.00001)
4210 continue;
4211 IkReal gconst12 = IKsqrt(((-1.0) * x143 * (x144.value)));
4212 evalcond[0] =
4213 ((-3.14159265358979) +
4214 (IKfmod(((3.14159265358979) +
4215 (IKabs((cj0 + (((-1.0) * gconst12))))) +
4216 (IKabs(((1.0) + (IKsign(sj0)))))),
4217 6.28318530717959)));
4218 if (IKabs(evalcond[0]) < 0.0000050000000000)
4219 {
4220 bgotonextstatement = false;
4221 {
4222 IkReal j2eval[1];
4223 IkReal x145 = new_r22 * new_r22;
4224 new_r02 = 0;
4225 new_r12 = 0;
4226 new_r20 = 0;
4227 new_r21 = 0;
4228 if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
4229 -0.00001)
4230 continue;
4231 sj0 = ((-1.0) *
4232 (IKsqrt(((1.0) +
4233 (((-1.0) * (gconst12 * gconst12)))))));
4234 cj0 = gconst12;
4235 if ((gconst12) < -1 - IKFAST_SINCOS_THRESH ||
4236 (gconst12) > 1 + IKFAST_SINCOS_THRESH)
4237 continue;
4238 j0 = ((-1.0) * (IKacos(gconst12)));
4239 CheckValue<IkReal> x146 = IKPowWithIntegerCheck(
4240 ((1.0) + (((-1.0) * x145))), -1);
4241 if (!x146.valid)
4242 {
4243 continue;
4244 }
4245 if ((((-1.0) * x145 * (x146.value))) < -0.00001)
4246 continue;
4247 IkReal gconst12 =
4248 IKsqrt(((-1.0) * x145 * (x146.value)));
4249 j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
4250 if (IKabs(j2eval[0]) < 0.0000010000000000)
4251 {
4252 {
4253 IkReal j2array[1], cj2array[1], sj2array[1];
4254 bool j2valid[1] = { false };
4255 _nj2 = 1;
4256 CheckValue<IkReal> x147 =
4257 IKPowWithIntegerCheck(gconst12, -1);
4258 if (!x147.valid)
4259 {
4260 continue;
4261 }
4262 if ((((1.0) + (((-1.0) * (gconst12 * gconst12))))) <
4263 -0.00001)
4264 continue;
4265 if (IKabs(((-1.0) * new_r10 * (x147.value))) <
4266 IKFAST_ATAN2_MAGTHRESH &&
4267 IKabs(((((-1.0) * new_r01 *
4268 (IKsqrt(((1.0) +
4269 (((-1.0) * (gconst12 *
4270 gconst12)))))))) +
4271 (((-1.0) * gconst12 * new_r11)))) <
4272 IKFAST_ATAN2_MAGTHRESH &&
4273 IKabs(IKsqr(((-1.0) * new_r10 * (x147.value))) +
4274 IKsqr(((((-1.0) * new_r01 *
4275 (IKsqrt(((1.0) +
4276 (((-1.0) *
4277 (gconst12 *
4278 gconst12)))))))) +
4279 (((-1.0) * gconst12 * new_r11)))) -
4280 1) <= IKFAST_SINCOS_THRESH)
4281 continue;
4282 j2array[0] = IKatan2(
4283 ((-1.0) * new_r10 * (x147.value)),
4284 ((((-1.0) * new_r01 *
4285 (IKsqrt((
4286 (1.0) +
4287 (((-1.0) * (gconst12 * gconst12)))))))) +
4288 (((-1.0) * gconst12 * new_r11))));
4289 sj2array[0] = IKsin(j2array[0]);
4290 cj2array[0] = IKcos(j2array[0]);
4291 if (j2array[0] > IKPI)
4292 {
4293 j2array[0] -= IK2PI;
4294 }
4295 else if (j2array[0] < -IKPI)
4296 {
4297 j2array[0] += IK2PI;
4298 }
4299 j2valid[0] = true;
4300 for (int ij2 = 0; ij2 < 1; ++ij2)
4301 {
4302 if (!j2valid[ij2])
4303 {
4304 continue;
4305 }
4306 _ij2[0] = ij2;
4307 _ij2[1] = -1;
4308 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4309 {
4310 if (j2valid[iij2] &&
4311 IKabs(cj2array[ij2] - cj2array[iij2]) <
4312 IKFAST_SOLUTION_THRESH &&
4313 IKabs(sj2array[ij2] - sj2array[iij2]) <
4314 IKFAST_SOLUTION_THRESH)
4315 {
4316 j2valid[iij2] = false;
4317 _ij2[1] = iij2;
4318 break;
4319 }
4320 }
4321 j2 = j2array[ij2];
4322 cj2 = cj2array[ij2];
4323 sj2 = sj2array[ij2];
4324 {
4325 IkReal evalcond[8];
4326 IkReal x148 = IKsin(j2);
4327 IkReal x149 = IKcos(j2);
4328 if ((((1.0) +
4329 (((-1.0) * (gconst12 * gconst12))))) <
4330 -0.00001)
4331 continue;
4332 IkReal x150 = IKsqrt(
4333 ((1.0) +
4334 (((-1.0) * (gconst12 * gconst12)))));
4335 evalcond[0] = x149;
4336 evalcond[1] = ((-1.0) * x148);
4337 evalcond[2] = (((gconst12 * x148)) + new_r10);
4338 evalcond[3] = (((gconst12 * x149)) + new_r11);
4339 evalcond[4] = (((x148 * x150)) + new_r00);
4340 evalcond[5] = (((x149 * x150)) + new_r01);
4341 evalcond[6] = (((new_r00 * x150)) + x148 +
4342 ((gconst12 * new_r10)));
4343 evalcond[7] = (((new_r01 * x150)) + x149 +
4344 ((gconst12 * new_r11)));
4345 if (IKabs(evalcond[0]) >
4346 IKFAST_EVALCOND_THRESH ||
4347 IKabs(evalcond[1]) >
4348 IKFAST_EVALCOND_THRESH ||
4349 IKabs(evalcond[2]) >
4350 IKFAST_EVALCOND_THRESH ||
4351 IKabs(evalcond[3]) >
4352 IKFAST_EVALCOND_THRESH ||
4353 IKabs(evalcond[4]) >
4354 IKFAST_EVALCOND_THRESH ||
4355 IKabs(evalcond[5]) >
4356 IKFAST_EVALCOND_THRESH ||
4357 IKabs(evalcond[6]) >
4358 IKFAST_EVALCOND_THRESH ||
4359 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4360 {
4361 continue;
4362 }
4363 }
4364
4365 {
4366 std::vector<IkSingleDOFSolutionBase<IkReal> >
4367 vinfos(7);
4368 vinfos[0].jointtype = 1;
4369 vinfos[0].foffset = j0;
4370 vinfos[0].indices[0] = _ij0[0];
4371 vinfos[0].indices[1] = _ij0[1];
4372 vinfos[0].maxsolutions = _nj0;
4373 vinfos[1].jointtype = 1;
4374 vinfos[1].foffset = j1;
4375 vinfos[1].indices[0] = _ij1[0];
4376 vinfos[1].indices[1] = _ij1[1];
4377 vinfos[1].maxsolutions = _nj1;
4378 vinfos[2].jointtype = 1;
4379 vinfos[2].foffset = j2;
4380 vinfos[2].indices[0] = _ij2[0];
4381 vinfos[2].indices[1] = _ij2[1];
4382 vinfos[2].maxsolutions = _nj2;
4383 vinfos[3].jointtype = 1;
4384 vinfos[3].foffset = j3;
4385 vinfos[3].indices[0] = _ij3[0];
4386 vinfos[3].indices[1] = _ij3[1];
4387 vinfos[3].maxsolutions = _nj3;
4388 vinfos[4].jointtype = 1;
4389 vinfos[4].foffset = j4;
4390 vinfos[4].indices[0] = _ij4[0];
4391 vinfos[4].indices[1] = _ij4[1];
4392 vinfos[4].maxsolutions = _nj4;
4393 vinfos[5].jointtype = 1;
4394 vinfos[5].foffset = j5;
4395 vinfos[5].indices[0] = _ij5[0];
4396 vinfos[5].indices[1] = _ij5[1];
4397 vinfos[5].maxsolutions = _nj5;
4398 vinfos[6].jointtype = 1;
4399 vinfos[6].foffset = j6;
4400 vinfos[6].indices[0] = _ij6[0];
4401 vinfos[6].indices[1] = _ij6[1];
4402 vinfos[6].maxsolutions = _nj6;
4403 std::vector<int> vfree(0);
4404 solutions.AddSolution(vinfos, vfree);
4405 }
4406 }
4407 }
4408 }
4409 else
4410 {
4411 {
4412 IkReal j2array[1], cj2array[1], sj2array[1];
4413 bool j2valid[1] = { false };
4414 _nj2 = 1;
4415 CheckValue<IkReal> x151 =
4416 IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
4417 IkReal(((-1.0) * new_r11)),
4418 IKFAST_ATAN2_MAGTHRESH);
4419 if (!x151.valid)
4420 {
4421 continue;
4422 }
4423 CheckValue<IkReal> x152 =
4424 IKPowWithIntegerCheck(IKsign(gconst12), -1);
4425 if (!x152.valid)
4426 {
4427 continue;
4428 }
4429 j2array[0] = ((-1.5707963267949) + (x151.value) +
4430 (((1.5707963267949) * (x152.value))));
4431 sj2array[0] = IKsin(j2array[0]);
4432 cj2array[0] = IKcos(j2array[0]);
4433 if (j2array[0] > IKPI)
4434 {
4435 j2array[0] -= IK2PI;
4436 }
4437 else if (j2array[0] < -IKPI)
4438 {
4439 j2array[0] += IK2PI;
4440 }
4441 j2valid[0] = true;
4442 for (int ij2 = 0; ij2 < 1; ++ij2)
4443 {
4444 if (!j2valid[ij2])
4445 {
4446 continue;
4447 }
4448 _ij2[0] = ij2;
4449 _ij2[1] = -1;
4450 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4451 {
4452 if (j2valid[iij2] &&
4453 IKabs(cj2array[ij2] - cj2array[iij2]) <
4454 IKFAST_SOLUTION_THRESH &&
4455 IKabs(sj2array[ij2] - sj2array[iij2]) <
4456 IKFAST_SOLUTION_THRESH)
4457 {
4458 j2valid[iij2] = false;
4459 _ij2[1] = iij2;
4460 break;
4461 }
4462 }
4463 j2 = j2array[ij2];
4464 cj2 = cj2array[ij2];
4465 sj2 = sj2array[ij2];
4466 {
4467 IkReal evalcond[8];
4468 IkReal x153 = IKsin(j2);
4469 IkReal x154 = IKcos(j2);
4470 if ((((1.0) +
4471 (((-1.0) * (gconst12 * gconst12))))) <
4472 -0.00001)
4473 continue;
4474 IkReal x155 = IKsqrt(
4475 ((1.0) +
4476 (((-1.0) * (gconst12 * gconst12)))));
4477 evalcond[0] = x154;
4478 evalcond[1] = ((-1.0) * x153);
4479 evalcond[2] = (((gconst12 * x153)) + new_r10);
4480 evalcond[3] = (((gconst12 * x154)) + new_r11);
4481 evalcond[4] = (((x153 * x155)) + new_r00);
4482 evalcond[5] = (((x154 * x155)) + new_r01);
4483 evalcond[6] = (((new_r00 * x155)) + x153 +
4484 ((gconst12 * new_r10)));
4485 evalcond[7] = (((new_r01 * x155)) + x154 +
4486 ((gconst12 * new_r11)));
4487 if (IKabs(evalcond[0]) >
4488 IKFAST_EVALCOND_THRESH ||
4489 IKabs(evalcond[1]) >
4490 IKFAST_EVALCOND_THRESH ||
4491 IKabs(evalcond[2]) >
4492 IKFAST_EVALCOND_THRESH ||
4493 IKabs(evalcond[3]) >
4494 IKFAST_EVALCOND_THRESH ||
4495 IKabs(evalcond[4]) >
4496 IKFAST_EVALCOND_THRESH ||
4497 IKabs(evalcond[5]) >
4498 IKFAST_EVALCOND_THRESH ||
4499 IKabs(evalcond[6]) >
4500 IKFAST_EVALCOND_THRESH ||
4501 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
4502 {
4503 continue;
4504 }
4505 }
4506
4507 {
4508 std::vector<IkSingleDOFSolutionBase<IkReal> >
4509 vinfos(7);
4510 vinfos[0].jointtype = 1;
4511 vinfos[0].foffset = j0;
4512 vinfos[0].indices[0] = _ij0[0];
4513 vinfos[0].indices[1] = _ij0[1];
4514 vinfos[0].maxsolutions = _nj0;
4515 vinfos[1].jointtype = 1;
4516 vinfos[1].foffset = j1;
4517 vinfos[1].indices[0] = _ij1[0];
4518 vinfos[1].indices[1] = _ij1[1];
4519 vinfos[1].maxsolutions = _nj1;
4520 vinfos[2].jointtype = 1;
4521 vinfos[2].foffset = j2;
4522 vinfos[2].indices[0] = _ij2[0];
4523 vinfos[2].indices[1] = _ij2[1];
4524 vinfos[2].maxsolutions = _nj2;
4525 vinfos[3].jointtype = 1;
4526 vinfos[3].foffset = j3;
4527 vinfos[3].indices[0] = _ij3[0];
4528 vinfos[3].indices[1] = _ij3[1];
4529 vinfos[3].maxsolutions = _nj3;
4530 vinfos[4].jointtype = 1;
4531 vinfos[4].foffset = j4;
4532 vinfos[4].indices[0] = _ij4[0];
4533 vinfos[4].indices[1] = _ij4[1];
4534 vinfos[4].maxsolutions = _nj4;
4535 vinfos[5].jointtype = 1;
4536 vinfos[5].foffset = j5;
4537 vinfos[5].indices[0] = _ij5[0];
4538 vinfos[5].indices[1] = _ij5[1];
4539 vinfos[5].maxsolutions = _nj5;
4540 vinfos[6].jointtype = 1;
4541 vinfos[6].foffset = j6;
4542 vinfos[6].indices[0] = _ij6[0];
4543 vinfos[6].indices[1] = _ij6[1];
4544 vinfos[6].maxsolutions = _nj6;
4545 std::vector<int> vfree(0);
4546 solutions.AddSolution(vinfos, vfree);
4547 }
4548 }
4549 }
4550 }
4551 }
4552 }
4553 } while (0);
4554 if (bgotonextstatement)
4555 {
4556 bool bgotonextstatement = true;
4557 do
4558 {
4559 IkReal x156 = new_r22 * new_r22;
4560 CheckValue<IkReal> x157 = IKPowWithIntegerCheck(
4561 ((1.0) + (((-1.0) * x156))), -1);
4562 if (!x157.valid)
4563 {
4564 continue;
4565 }
4566 if ((((-1.0) * x156 * (x157.value))) < -0.00001)
4567 continue;
4568 IkReal gconst13 =
4569 ((-1.0) * (IKsqrt(((-1.0) * x156 * (x157.value)))));
4570 evalcond[0] =
4571 ((-3.14159265358979) +
4572 (IKfmod(((3.14159265358979) +
4573 (IKabs((cj0 + (((-1.0) * gconst13))))) +
4574 (IKabs(((-1.0) + (IKsign(sj0)))))),
4575 6.28318530717959)));
4576 if (IKabs(evalcond[0]) < 0.0000050000000000)
4577 {
4578 bgotonextstatement = false;
4579 {
4580 IkReal j2eval[1];
4581 IkReal x158 = new_r22 * new_r22;
4582 new_r02 = 0;
4583 new_r12 = 0;
4584 new_r20 = 0;
4585 new_r21 = 0;
4586 if ((((1.0) + (((-1.0) * (gconst13 * gconst13))))) <
4587 -0.00001)
4588 continue;
4589 sj0 = IKsqrt(
4590 ((1.0) + (((-1.0) * (gconst13 * gconst13)))));
4591 cj0 = gconst13;
4592 if ((gconst13) < -1 - IKFAST_SINCOS_THRESH ||
4593 (gconst13) > 1 + IKFAST_SINCOS_THRESH)
4594 continue;
4595 j0 = IKacos(gconst13);
4596 CheckValue<IkReal> x159 = IKPowWithIntegerCheck(
4597 ((1.0) + (((-1.0) * x158))), -1);
4598 if (!x159.valid)
4599 {
4600 continue;
4601 }
4602 if ((((-1.0) * x158 * (x159.value))) < -0.00001)
4603 continue;
4604 IkReal gconst13 =
4605 ((-1.0) *
4606 (IKsqrt(((-1.0) * x158 * (x159.value)))));
4607 j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
4608 if (IKabs(j2eval[0]) < 0.0000010000000000)
4609 {
4610 {
4611 IkReal j2array[1], cj2array[1], sj2array[1];
4612 bool j2valid[1] = { false };
4613 _nj2 = 1;
4614 CheckValue<IkReal> x160 =
4615 IKPowWithIntegerCheck(gconst13, -1);
4616 if (!x160.valid)
4617 {
4618 continue;
4619 }
4620 if ((((1.0) +
4621 (((-1.0) * (gconst13 * gconst13))))) <
4622 -0.00001)
4623 continue;
4624 if (IKabs(((-1.0) * new_r10 * (x160.value))) <
4625 IKFAST_ATAN2_MAGTHRESH &&
4626 IKabs((
4627 (((-1.0) * gconst13 * new_r11)) +
4628 ((new_r01 *
4629 (IKsqrt(((1.0) + (((-1.0) *
4630 (gconst13 *
4631 gconst13)))))))))) <
4632 IKFAST_ATAN2_MAGTHRESH &&
4633 IKabs(
4634 IKsqr(((-1.0) * new_r10 * (x160.value))) +
4635 IKsqr(((((-1.0) * gconst13 * new_r11)) +
4636 ((new_r01 *
4637 (IKsqrt(((1.0) +
4638 (((-1.0) *
4639 (gconst13 *
4640 gconst13)))))))))) -
4641 1) <= IKFAST_SINCOS_THRESH)
4642 continue;
4643 j2array[0] = IKatan2(
4644 ((-1.0) * new_r10 * (x160.value)),
4645 ((((-1.0) * gconst13 * new_r11)) +
4646 ((new_r01 *
4647 (IKsqrt(((1.0) +
4648 (((-1.0) *
4649 (gconst13 * gconst13))))))))));
4650 sj2array[0] = IKsin(j2array[0]);
4651 cj2array[0] = IKcos(j2array[0]);
4652 if (j2array[0] > IKPI)
4653 {
4654 j2array[0] -= IK2PI;
4655 }
4656 else if (j2array[0] < -IKPI)
4657 {
4658 j2array[0] += IK2PI;
4659 }
4660 j2valid[0] = true;
4661 for (int ij2 = 0; ij2 < 1; ++ij2)
4662 {
4663 if (!j2valid[ij2])
4664 {
4665 continue;
4666 }
4667 _ij2[0] = ij2;
4668 _ij2[1] = -1;
4669 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4670 {
4671 if (j2valid[iij2] &&
4672 IKabs(cj2array[ij2] - cj2array[iij2]) <
4673 IKFAST_SOLUTION_THRESH &&
4674 IKabs(sj2array[ij2] - sj2array[iij2]) <
4675 IKFAST_SOLUTION_THRESH)
4676 {
4677 j2valid[iij2] = false;
4678 _ij2[1] = iij2;
4679 break;
4680 }
4681 }
4682 j2 = j2array[ij2];
4683 cj2 = cj2array[ij2];
4684 sj2 = sj2array[ij2];
4685 {
4686 IkReal evalcond[8];
4687 IkReal x161 = IKsin(j2);
4688 IkReal x162 = IKcos(j2);
4689 if ((((1.0) +
4690 (((-1.0) * (gconst13 * gconst13))))) <
4691 -0.00001)
4692 continue;
4693 IkReal x163 = IKsqrt(
4694 ((1.0) +
4695 (((-1.0) * (gconst13 * gconst13)))));
4696 IkReal x164 = ((1.0) * x163);
4697 evalcond[0] = x162;
4698 evalcond[1] = ((-1.0) * x161);
4699 evalcond[2] = (new_r10 + ((gconst13 * x161)));
4700 evalcond[3] = (new_r11 + ((gconst13 * x162)));
4701 evalcond[4] =
4702 ((((-1.0) * x161 * x164)) + new_r00);
4703 evalcond[5] =
4704 ((((-1.0) * x162 * x164)) + new_r01);
4705 evalcond[6] = ((((-1.0) * new_r00 * x164)) +
4706 x161 + ((gconst13 * new_r10)));
4707 evalcond[7] = (x162 + ((gconst13 * new_r11)) +
4708 (((-1.0) * new_r01 * x164)));
4709 if (IKabs(evalcond[0]) >
4710 IKFAST_EVALCOND_THRESH ||
4711 IKabs(evalcond[1]) >
4712 IKFAST_EVALCOND_THRESH ||
4713 IKabs(evalcond[2]) >
4714 IKFAST_EVALCOND_THRESH ||
4715 IKabs(evalcond[3]) >
4716 IKFAST_EVALCOND_THRESH ||
4717 IKabs(evalcond[4]) >
4718 IKFAST_EVALCOND_THRESH ||
4719 IKabs(evalcond[5]) >
4720 IKFAST_EVALCOND_THRESH ||
4721 IKabs(evalcond[6]) >
4722 IKFAST_EVALCOND_THRESH ||
4723 IKabs(evalcond[7]) >
4724 IKFAST_EVALCOND_THRESH)
4725 {
4726 continue;
4727 }
4728 }
4729
4730 {
4731 std::vector<IkSingleDOFSolutionBase<IkReal> >
4732 vinfos(7);
4733 vinfos[0].jointtype = 1;
4734 vinfos[0].foffset = j0;
4735 vinfos[0].indices[0] = _ij0[0];
4736 vinfos[0].indices[1] = _ij0[1];
4737 vinfos[0].maxsolutions = _nj0;
4738 vinfos[1].jointtype = 1;
4739 vinfos[1].foffset = j1;
4740 vinfos[1].indices[0] = _ij1[0];
4741 vinfos[1].indices[1] = _ij1[1];
4742 vinfos[1].maxsolutions = _nj1;
4743 vinfos[2].jointtype = 1;
4744 vinfos[2].foffset = j2;
4745 vinfos[2].indices[0] = _ij2[0];
4746 vinfos[2].indices[1] = _ij2[1];
4747 vinfos[2].maxsolutions = _nj2;
4748 vinfos[3].jointtype = 1;
4749 vinfos[3].foffset = j3;
4750 vinfos[3].indices[0] = _ij3[0];
4751 vinfos[3].indices[1] = _ij3[1];
4752 vinfos[3].maxsolutions = _nj3;
4753 vinfos[4].jointtype = 1;
4754 vinfos[4].foffset = j4;
4755 vinfos[4].indices[0] = _ij4[0];
4756 vinfos[4].indices[1] = _ij4[1];
4757 vinfos[4].maxsolutions = _nj4;
4758 vinfos[5].jointtype = 1;
4759 vinfos[5].foffset = j5;
4760 vinfos[5].indices[0] = _ij5[0];
4761 vinfos[5].indices[1] = _ij5[1];
4762 vinfos[5].maxsolutions = _nj5;
4763 vinfos[6].jointtype = 1;
4764 vinfos[6].foffset = j6;
4765 vinfos[6].indices[0] = _ij6[0];
4766 vinfos[6].indices[1] = _ij6[1];
4767 vinfos[6].maxsolutions = _nj6;
4768 std::vector<int> vfree(0);
4769 solutions.AddSolution(vinfos, vfree);
4770 }
4771 }
4772 }
4773 }
4774 else
4775 {
4776 {
4777 IkReal j2array[1], cj2array[1], sj2array[1];
4778 bool j2valid[1] = { false };
4779 _nj2 = 1;
4780 CheckValue<IkReal> x165 =
4781 IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
4782 IkReal(((-1.0) * new_r11)),
4783 IKFAST_ATAN2_MAGTHRESH);
4784 if (!x165.valid)
4785 {
4786 continue;
4787 }
4788 CheckValue<IkReal> x166 =
4789 IKPowWithIntegerCheck(IKsign(gconst13), -1);
4790 if (!x166.valid)
4791 {
4792 continue;
4793 }
4794 j2array[0] =
4795 ((-1.5707963267949) + (x165.value) +
4796 (((1.5707963267949) * (x166.value))));
4797 sj2array[0] = IKsin(j2array[0]);
4798 cj2array[0] = IKcos(j2array[0]);
4799 if (j2array[0] > IKPI)
4800 {
4801 j2array[0] -= IK2PI;
4802 }
4803 else if (j2array[0] < -IKPI)
4804 {
4805 j2array[0] += IK2PI;
4806 }
4807 j2valid[0] = true;
4808 for (int ij2 = 0; ij2 < 1; ++ij2)
4809 {
4810 if (!j2valid[ij2])
4811 {
4812 continue;
4813 }
4814 _ij2[0] = ij2;
4815 _ij2[1] = -1;
4816 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
4817 {
4818 if (j2valid[iij2] &&
4819 IKabs(cj2array[ij2] - cj2array[iij2]) <
4820 IKFAST_SOLUTION_THRESH &&
4821 IKabs(sj2array[ij2] - sj2array[iij2]) <
4822 IKFAST_SOLUTION_THRESH)
4823 {
4824 j2valid[iij2] = false;
4825 _ij2[1] = iij2;
4826 break;
4827 }
4828 }
4829 j2 = j2array[ij2];
4830 cj2 = cj2array[ij2];
4831 sj2 = sj2array[ij2];
4832 {
4833 IkReal evalcond[8];
4834 IkReal x167 = IKsin(j2);
4835 IkReal x168 = IKcos(j2);
4836 if ((((1.0) +
4837 (((-1.0) * (gconst13 * gconst13))))) <
4838 -0.00001)
4839 continue;
4840 IkReal x169 = IKsqrt(
4841 ((1.0) +
4842 (((-1.0) * (gconst13 * gconst13)))));
4843 IkReal x170 = ((1.0) * x169);
4844 evalcond[0] = x168;
4845 evalcond[1] = ((-1.0) * x167);
4846 evalcond[2] = (new_r10 + ((gconst13 * x167)));
4847 evalcond[3] = (new_r11 + ((gconst13 * x168)));
4848 evalcond[4] =
4849 (new_r00 + (((-1.0) * x167 * x170)));
4850 evalcond[5] =
4851 ((((-1.0) * x168 * x170)) + new_r01);
4852 evalcond[6] = (x167 + ((gconst13 * new_r10)) +
4853 (((-1.0) * new_r00 * x170)));
4854 evalcond[7] = (x168 + ((gconst13 * new_r11)) +
4855 (((-1.0) * new_r01 * x170)));
4856 if (IKabs(evalcond[0]) >
4857 IKFAST_EVALCOND_THRESH ||
4858 IKabs(evalcond[1]) >
4859 IKFAST_EVALCOND_THRESH ||
4860 IKabs(evalcond[2]) >
4861 IKFAST_EVALCOND_THRESH ||
4862 IKabs(evalcond[3]) >
4863 IKFAST_EVALCOND_THRESH ||
4864 IKabs(evalcond[4]) >
4865 IKFAST_EVALCOND_THRESH ||
4866 IKabs(evalcond[5]) >
4867 IKFAST_EVALCOND_THRESH ||
4868 IKabs(evalcond[6]) >
4869 IKFAST_EVALCOND_THRESH ||
4870 IKabs(evalcond[7]) >
4871 IKFAST_EVALCOND_THRESH)
4872 {
4873 continue;
4874 }
4875 }
4876
4877 {
4878 std::vector<IkSingleDOFSolutionBase<IkReal> >
4879 vinfos(7);
4880 vinfos[0].jointtype = 1;
4881 vinfos[0].foffset = j0;
4882 vinfos[0].indices[0] = _ij0[0];
4883 vinfos[0].indices[1] = _ij0[1];
4884 vinfos[0].maxsolutions = _nj0;
4885 vinfos[1].jointtype = 1;
4886 vinfos[1].foffset = j1;
4887 vinfos[1].indices[0] = _ij1[0];
4888 vinfos[1].indices[1] = _ij1[1];
4889 vinfos[1].maxsolutions = _nj1;
4890 vinfos[2].jointtype = 1;
4891 vinfos[2].foffset = j2;
4892 vinfos[2].indices[0] = _ij2[0];
4893 vinfos[2].indices[1] = _ij2[1];
4894 vinfos[2].maxsolutions = _nj2;
4895 vinfos[3].jointtype = 1;
4896 vinfos[3].foffset = j3;
4897 vinfos[3].indices[0] = _ij3[0];
4898 vinfos[3].indices[1] = _ij3[1];
4899 vinfos[3].maxsolutions = _nj3;
4900 vinfos[4].jointtype = 1;
4901 vinfos[4].foffset = j4;
4902 vinfos[4].indices[0] = _ij4[0];
4903 vinfos[4].indices[1] = _ij4[1];
4904 vinfos[4].maxsolutions = _nj4;
4905 vinfos[5].jointtype = 1;
4906 vinfos[5].foffset = j5;
4907 vinfos[5].indices[0] = _ij5[0];
4908 vinfos[5].indices[1] = _ij5[1];
4909 vinfos[5].maxsolutions = _nj5;
4910 vinfos[6].jointtype = 1;
4911 vinfos[6].foffset = j6;
4912 vinfos[6].indices[0] = _ij6[0];
4913 vinfos[6].indices[1] = _ij6[1];
4914 vinfos[6].maxsolutions = _nj6;
4915 std::vector<int> vfree(0);
4916 solutions.AddSolution(vinfos, vfree);
4917 }
4918 }
4919 }
4920 }
4921 }
4922 }
4923 } while (0);
4924 if (bgotonextstatement)
4925 {
4926 bool bgotonextstatement = true;
4927 do
4928 {
4929 IkReal x171 = new_r22 * new_r22;
4930 CheckValue<IkReal> x172 = IKPowWithIntegerCheck(
4931 ((1.0) + (((-1.0) * x171))), -1);
4932 if (!x172.valid)
4933 {
4934 continue;
4935 }
4936 if ((((-1.0) * x171 * (x172.value))) < -0.00001)
4937 continue;
4938 IkReal gconst13 =
4939 ((-1.0) * (IKsqrt(((-1.0) * x171 * (x172.value)))));
4940 evalcond[0] =
4941 ((-3.14159265358979) +
4942 (IKfmod(((3.14159265358979) +
4943 (IKabs(((1.0) + (IKsign(sj0))))) +
4944 (IKabs((cj0 + (((-1.0) * gconst13)))))),
4945 6.28318530717959)));
4946 if (IKabs(evalcond[0]) < 0.0000050000000000)
4947 {
4948 bgotonextstatement = false;
4949 {
4950 IkReal j2eval[1];
4951 IkReal x173 = new_r22 * new_r22;
4952 new_r02 = 0;
4953 new_r12 = 0;
4954 new_r20 = 0;
4955 new_r21 = 0;
4956 if ((((1.0) + (((-1.0) * (gconst13 * gconst13))))) <
4957 -0.00001)
4958 continue;
4959 sj0 = ((-1.0) *
4960 (IKsqrt(((1.0) + (((-1.0) * (gconst13 *
4961 gconst13)))))));
4962 cj0 = gconst13;
4963 if ((gconst13) < -1 - IKFAST_SINCOS_THRESH ||
4964 (gconst13) > 1 + IKFAST_SINCOS_THRESH)
4965 continue;
4966 j0 = ((-1.0) * (IKacos(gconst13)));
4967 CheckValue<IkReal> x174 = IKPowWithIntegerCheck(
4968 ((1.0) + (((-1.0) * x173))), -1);
4969 if (!x174.valid)
4970 {
4971 continue;
4972 }
4973 if ((((-1.0) * x173 * (x174.value))) < -0.00001)
4974 continue;
4975 IkReal gconst13 =
4976 ((-1.0) *
4977 (IKsqrt(((-1.0) * x173 * (x174.value)))));
4978 j2eval[0] = ((IKabs(new_r11)) + (IKabs(new_r10)));
4979 if (IKabs(j2eval[0]) < 0.0000010000000000)
4980 {
4981 {
4982 IkReal j2array[1], cj2array[1], sj2array[1];
4983 bool j2valid[1] = { false };
4984 _nj2 = 1;
4985 CheckValue<IkReal> x175 =
4986 IKPowWithIntegerCheck(gconst13, -1);
4987 if (!x175.valid)
4988 {
4989 continue;
4990 }
4991 if ((((1.0) +
4992 (((-1.0) * (gconst13 * gconst13))))) <
4993 -0.00001)
4994 continue;
4995 if (IKabs(((-1.0) * new_r10 * (x175.value))) <
4996 IKFAST_ATAN2_MAGTHRESH &&
4997 IKabs(((((-1.0) * gconst13 * new_r11)) +
4998 (((-1.0) * new_r01 *
4999 (IKsqrt(((1.0) +
5000 (((-1.0) *
5001 (gconst13 *
5002 gconst13)))))))))) <
5003 IKFAST_ATAN2_MAGTHRESH &&
5004 IKabs(IKsqr(((-1.0) * new_r10 *
5005 (x175.value))) +
5006 IKsqr((
5007 (((-1.0) * gconst13 * new_r11)) +
5008 (((-1.0) * new_r01 *
5009 (IKsqrt(((1.0) +
5010 (((-1.0) *
5011 (gconst13 *
5012 gconst13)))))))))) -
5013 1) <= IKFAST_SINCOS_THRESH)
5014 continue;
5015 j2array[0] = IKatan2(
5016 ((-1.0) * new_r10 * (x175.value)),
5017 ((((-1.0) * gconst13 * new_r11)) +
5018 (((-1.0) * new_r01 *
5019 (IKsqrt(((1.0) +
5020 (((-1.0) * (gconst13 *
5021 gconst13))))))))));
5022 sj2array[0] = IKsin(j2array[0]);
5023 cj2array[0] = IKcos(j2array[0]);
5024 if (j2array[0] > IKPI)
5025 {
5026 j2array[0] -= IK2PI;
5027 }
5028 else if (j2array[0] < -IKPI)
5029 {
5030 j2array[0] += IK2PI;
5031 }
5032 j2valid[0] = true;
5033 for (int ij2 = 0; ij2 < 1; ++ij2)
5034 {
5035 if (!j2valid[ij2])
5036 {
5037 continue;
5038 }
5039 _ij2[0] = ij2;
5040 _ij2[1] = -1;
5041 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5042 {
5043 if (j2valid[iij2] &&
5044 IKabs(cj2array[ij2] - cj2array[iij2]) <
5045 IKFAST_SOLUTION_THRESH &&
5046 IKabs(sj2array[ij2] - sj2array[iij2]) <
5047 IKFAST_SOLUTION_THRESH)
5048 {
5049 j2valid[iij2] = false;
5050 _ij2[1] = iij2;
5051 break;
5052 }
5053 }
5054 j2 = j2array[ij2];
5055 cj2 = cj2array[ij2];
5056 sj2 = sj2array[ij2];
5057 {
5058 IkReal evalcond[8];
5059 IkReal x176 = IKsin(j2);
5060 IkReal x177 = IKcos(j2);
5061 if ((((1.0) +
5062 (((-1.0) * (gconst13 * gconst13))))) <
5063 -0.00001)
5064 continue;
5065 IkReal x178 = IKsqrt(
5066 ((1.0) +
5067 (((-1.0) * (gconst13 * gconst13)))));
5068 evalcond[0] = x177;
5069 evalcond[1] = ((-1.0) * x176);
5070 evalcond[2] =
5071 (new_r10 + ((gconst13 * x176)));
5072 evalcond[3] =
5073 (new_r11 + ((gconst13 * x177)));
5074 evalcond[4] = (((x176 * x178)) + new_r00);
5075 evalcond[5] = (new_r01 + ((x177 * x178)));
5076 evalcond[6] = (((new_r00 * x178)) + x176 +
5077 ((gconst13 * new_r10)));
5078 evalcond[7] = (((new_r01 * x178)) + x177 +
5079 ((gconst13 * new_r11)));
5080 if (IKabs(evalcond[0]) >
5081 IKFAST_EVALCOND_THRESH ||
5082 IKabs(evalcond[1]) >
5083 IKFAST_EVALCOND_THRESH ||
5084 IKabs(evalcond[2]) >
5085 IKFAST_EVALCOND_THRESH ||
5086 IKabs(evalcond[3]) >
5087 IKFAST_EVALCOND_THRESH ||
5088 IKabs(evalcond[4]) >
5089 IKFAST_EVALCOND_THRESH ||
5090 IKabs(evalcond[5]) >
5091 IKFAST_EVALCOND_THRESH ||
5092 IKabs(evalcond[6]) >
5093 IKFAST_EVALCOND_THRESH ||
5094 IKabs(evalcond[7]) >
5095 IKFAST_EVALCOND_THRESH)
5096 {
5097 continue;
5098 }
5099 }
5100
5101 {
5102 std::vector<
5103 IkSingleDOFSolutionBase<IkReal> >
5104 vinfos(7);
5105 vinfos[0].jointtype = 1;
5106 vinfos[0].foffset = j0;
5107 vinfos[0].indices[0] = _ij0[0];
5108 vinfos[0].indices[1] = _ij0[1];
5109 vinfos[0].maxsolutions = _nj0;
5110 vinfos[1].jointtype = 1;
5111 vinfos[1].foffset = j1;
5112 vinfos[1].indices[0] = _ij1[0];
5113 vinfos[1].indices[1] = _ij1[1];
5114 vinfos[1].maxsolutions = _nj1;
5115 vinfos[2].jointtype = 1;
5116 vinfos[2].foffset = j2;
5117 vinfos[2].indices[0] = _ij2[0];
5118 vinfos[2].indices[1] = _ij2[1];
5119 vinfos[2].maxsolutions = _nj2;
5120 vinfos[3].jointtype = 1;
5121 vinfos[3].foffset = j3;
5122 vinfos[3].indices[0] = _ij3[0];
5123 vinfos[3].indices[1] = _ij3[1];
5124 vinfos[3].maxsolutions = _nj3;
5125 vinfos[4].jointtype = 1;
5126 vinfos[4].foffset = j4;
5127 vinfos[4].indices[0] = _ij4[0];
5128 vinfos[4].indices[1] = _ij4[1];
5129 vinfos[4].maxsolutions = _nj4;
5130 vinfos[5].jointtype = 1;
5131 vinfos[5].foffset = j5;
5132 vinfos[5].indices[0] = _ij5[0];
5133 vinfos[5].indices[1] = _ij5[1];
5134 vinfos[5].maxsolutions = _nj5;
5135 vinfos[6].jointtype = 1;
5136 vinfos[6].foffset = j6;
5137 vinfos[6].indices[0] = _ij6[0];
5138 vinfos[6].indices[1] = _ij6[1];
5139 vinfos[6].maxsolutions = _nj6;
5140 std::vector<int> vfree(0);
5141 solutions.AddSolution(vinfos, vfree);
5142 }
5143 }
5144 }
5145 }
5146 else
5147 {
5148 {
5149 IkReal j2array[1], cj2array[1], sj2array[1];
5150 bool j2valid[1] = { false };
5151 _nj2 = 1;
5152 CheckValue<IkReal> x179 =
5153 IKatan2WithCheck(IkReal(((-1.0) * new_r10)),
5154 IkReal(((-1.0) * new_r11)),
5155 IKFAST_ATAN2_MAGTHRESH);
5156 if (!x179.valid)
5157 {
5158 continue;
5159 }
5160 CheckValue<IkReal> x180 =
5161 IKPowWithIntegerCheck(IKsign(gconst13), -1);
5162 if (!x180.valid)
5163 {
5164 continue;
5165 }
5166 j2array[0] =
5167 ((-1.5707963267949) + (x179.value) +
5168 (((1.5707963267949) * (x180.value))));
5169 sj2array[0] = IKsin(j2array[0]);
5170 cj2array[0] = IKcos(j2array[0]);
5171 if (j2array[0] > IKPI)
5172 {
5173 j2array[0] -= IK2PI;
5174 }
5175 else if (j2array[0] < -IKPI)
5176 {
5177 j2array[0] += IK2PI;
5178 }
5179 j2valid[0] = true;
5180 for (int ij2 = 0; ij2 < 1; ++ij2)
5181 {
5182 if (!j2valid[ij2])
5183 {
5184 continue;
5185 }
5186 _ij2[0] = ij2;
5187 _ij2[1] = -1;
5188 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5189 {
5190 if (j2valid[iij2] &&
5191 IKabs(cj2array[ij2] - cj2array[iij2]) <
5192 IKFAST_SOLUTION_THRESH &&
5193 IKabs(sj2array[ij2] - sj2array[iij2]) <
5194 IKFAST_SOLUTION_THRESH)
5195 {
5196 j2valid[iij2] = false;
5197 _ij2[1] = iij2;
5198 break;
5199 }
5200 }
5201 j2 = j2array[ij2];
5202 cj2 = cj2array[ij2];
5203 sj2 = sj2array[ij2];
5204 {
5205 IkReal evalcond[8];
5206 IkReal x181 = IKsin(j2);
5207 IkReal x182 = IKcos(j2);
5208 if ((((1.0) +
5209 (((-1.0) * (gconst13 * gconst13))))) <
5210 -0.00001)
5211 continue;
5212 IkReal x183 = IKsqrt(
5213 ((1.0) +
5214 (((-1.0) * (gconst13 * gconst13)))));
5215 evalcond[0] = x182;
5216 evalcond[1] = ((-1.0) * x181);
5217 evalcond[2] =
5218 (((gconst13 * x181)) + new_r10);
5219 evalcond[3] =
5220 (((gconst13 * x182)) + new_r11);
5221 evalcond[4] = (new_r00 + ((x181 * x183)));
5222 evalcond[5] = (new_r01 + ((x182 * x183)));
5223 evalcond[6] = (((new_r00 * x183)) + x181 +
5224 ((gconst13 * new_r10)));
5225 evalcond[7] = (((new_r01 * x183)) + x182 +
5226 ((gconst13 * new_r11)));
5227 if (IKabs(evalcond[0]) >
5228 IKFAST_EVALCOND_THRESH ||
5229 IKabs(evalcond[1]) >
5230 IKFAST_EVALCOND_THRESH ||
5231 IKabs(evalcond[2]) >
5232 IKFAST_EVALCOND_THRESH ||
5233 IKabs(evalcond[3]) >
5234 IKFAST_EVALCOND_THRESH ||
5235 IKabs(evalcond[4]) >
5236 IKFAST_EVALCOND_THRESH ||
5237 IKabs(evalcond[5]) >
5238 IKFAST_EVALCOND_THRESH ||
5239 IKabs(evalcond[6]) >
5240 IKFAST_EVALCOND_THRESH ||
5241 IKabs(evalcond[7]) >
5242 IKFAST_EVALCOND_THRESH)
5243 {
5244 continue;
5245 }
5246 }
5247
5248 {
5249 std::vector<
5250 IkSingleDOFSolutionBase<IkReal> >
5251 vinfos(7);
5252 vinfos[0].jointtype = 1;
5253 vinfos[0].foffset = j0;
5254 vinfos[0].indices[0] = _ij0[0];
5255 vinfos[0].indices[1] = _ij0[1];
5256 vinfos[0].maxsolutions = _nj0;
5257 vinfos[1].jointtype = 1;
5258 vinfos[1].foffset = j1;
5259 vinfos[1].indices[0] = _ij1[0];
5260 vinfos[1].indices[1] = _ij1[1];
5261 vinfos[1].maxsolutions = _nj1;
5262 vinfos[2].jointtype = 1;
5263 vinfos[2].foffset = j2;
5264 vinfos[2].indices[0] = _ij2[0];
5265 vinfos[2].indices[1] = _ij2[1];
5266 vinfos[2].maxsolutions = _nj2;
5267 vinfos[3].jointtype = 1;
5268 vinfos[3].foffset = j3;
5269 vinfos[3].indices[0] = _ij3[0];
5270 vinfos[3].indices[1] = _ij3[1];
5271 vinfos[3].maxsolutions = _nj3;
5272 vinfos[4].jointtype = 1;
5273 vinfos[4].foffset = j4;
5274 vinfos[4].indices[0] = _ij4[0];
5275 vinfos[4].indices[1] = _ij4[1];
5276 vinfos[4].maxsolutions = _nj4;
5277 vinfos[5].jointtype = 1;
5278 vinfos[5].foffset = j5;
5279 vinfos[5].indices[0] = _ij5[0];
5280 vinfos[5].indices[1] = _ij5[1];
5281 vinfos[5].maxsolutions = _nj5;
5282 vinfos[6].jointtype = 1;
5283 vinfos[6].foffset = j6;
5284 vinfos[6].indices[0] = _ij6[0];
5285 vinfos[6].indices[1] = _ij6[1];
5286 vinfos[6].maxsolutions = _nj6;
5287 std::vector<int> vfree(0);
5288 solutions.AddSolution(vinfos, vfree);
5289 }
5290 }
5291 }
5292 }
5293 }
5294 }
5295 } while (0);
5296 if (bgotonextstatement)
5297 {
5298 bool bgotonextstatement = true;
5299 do
5300 {
5301 if (1)
5302 {
5303 bgotonextstatement = false;
5304 continue; // branch miss [j2]
5305 }
5306 } while (0);
5307 if (bgotonextstatement)
5308 {
5309 }
5310 }
5311 }
5312 }
5313 }
5314 }
5315 }
5316 }
5317 }
5318 else
5319 {
5320 {
5321 IkReal j2array[1], cj2array[1], sj2array[1];
5322 bool j2valid[1] = { false };
5323 _nj2 = 1;
5324 IkReal x184 = (new_r01 * new_r22);
5325 IkReal x185 = (cj0 * new_r11);
5326 CheckValue<IkReal> x186 = IKPowWithIntegerCheck(cj0, -1);
5327 if (!x186.valid)
5328 {
5329 continue;
5330 }
5331 if (IKabs(((x186.value) *
5332 ((((new_r22 * sj0 * x185)) + ((x184 * (cj0 * cj0))) +
5333 (((-1.0) * x184)) + (((-1.0) * new_r10)))))) <
5334 IKFAST_ATAN2_MAGTHRESH &&
5335 IKabs((((new_r01 * sj0)) + (((-1.0) * x185)))) <
5336 IKFAST_ATAN2_MAGTHRESH &&
5337 IKabs(
5338 IKsqr(((x186.value) *
5339 ((((new_r22 * sj0 * x185)) + ((x184 * (cj0 * cj0))) +
5340 (((-1.0) * x184)) + (((-1.0) * new_r10)))))) +
5341 IKsqr((((new_r01 * sj0)) + (((-1.0) * x185)))) - 1) <=
5342 IKFAST_SINCOS_THRESH)
5343 continue;
5344 j2array[0] =
5345 IKatan2(((x186.value) *
5346 ((((new_r22 * sj0 * x185)) + ((x184 * (cj0 * cj0))) +
5347 (((-1.0) * x184)) + (((-1.0) * new_r10))))),
5348 (((new_r01 * sj0)) + (((-1.0) * x185))));
5349 sj2array[0] = IKsin(j2array[0]);
5350 cj2array[0] = IKcos(j2array[0]);
5351 if (j2array[0] > IKPI)
5352 {
5353 j2array[0] -= IK2PI;
5354 }
5355 else if (j2array[0] < -IKPI)
5356 {
5357 j2array[0] += IK2PI;
5358 }
5359 j2valid[0] = true;
5360 for (int ij2 = 0; ij2 < 1; ++ij2)
5361 {
5362 if (!j2valid[ij2])
5363 {
5364 continue;
5365 }
5366 _ij2[0] = ij2;
5367 _ij2[1] = -1;
5368 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5369 {
5370 if (j2valid[iij2] &&
5371 IKabs(cj2array[ij2] - cj2array[iij2]) <
5372 IKFAST_SOLUTION_THRESH &&
5373 IKabs(sj2array[ij2] - sj2array[iij2]) <
5374 IKFAST_SOLUTION_THRESH)
5375 {
5376 j2valid[iij2] = false;
5377 _ij2[1] = iij2;
5378 break;
5379 }
5380 }
5381 j2 = j2array[ij2];
5382 cj2 = cj2array[ij2];
5383 sj2 = sj2array[ij2];
5384 {
5385 IkReal evalcond[10];
5386 IkReal x187 = IKcos(j2);
5387 IkReal x188 = IKsin(j2);
5388 IkReal x189 = (cj0 * new_r22);
5389 IkReal x190 = ((1.0) * sj0);
5390 IkReal x191 = (new_r22 * sj0);
5391 IkReal x192 = (new_r22 * x188);
5392 IkReal x193 = ((1.0) * x188);
5393 evalcond[0] =
5394 (x188 + (((-1.0) * new_r00 * x190)) + ((cj0 * new_r10)));
5395 evalcond[1] =
5396 (x187 + (((-1.0) * new_r01 * x190)) + ((cj0 * new_r11)));
5397 evalcond[2] = (((new_r10 * sj0)) + ((new_r22 * x187)) +
5398 ((cj0 * new_r00)));
5399 evalcond[3] = (((new_r10 * x191)) + ((new_r00 * x189)) + x187);
5400 evalcond[4] = (((cj0 * x188)) + ((x187 * x191)) + new_r10);
5401 evalcond[5] =
5402 (((new_r11 * sj0)) + (((-1.0) * x192)) + ((cj0 * new_r01)));
5403 evalcond[6] =
5404 ((((-1.0) * x188 * x190)) + ((x187 * x189)) + new_r00);
5405 evalcond[7] =
5406 (((cj0 * x187)) + (((-1.0) * x190 * x192)) + new_r11);
5407 evalcond[8] = (((new_r11 * x191)) + ((new_r01 * x189)) +
5408 (((-1.0) * x193)));
5409 evalcond[9] = ((((-1.0) * x189 * x193)) +
5410 (((-1.0) * x187 * x190)) + new_r01);
5411 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5412 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5413 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5414 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5415 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5416 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5417 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5418 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
5419 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
5420 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH)
5421 {
5422 continue;
5423 }
5424 }
5425
5426 {
5427 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5428 vinfos[0].jointtype = 1;
5429 vinfos[0].foffset = j0;
5430 vinfos[0].indices[0] = _ij0[0];
5431 vinfos[0].indices[1] = _ij0[1];
5432 vinfos[0].maxsolutions = _nj0;
5433 vinfos[1].jointtype = 1;
5434 vinfos[1].foffset = j1;
5435 vinfos[1].indices[0] = _ij1[0];
5436 vinfos[1].indices[1] = _ij1[1];
5437 vinfos[1].maxsolutions = _nj1;
5438 vinfos[2].jointtype = 1;
5439 vinfos[2].foffset = j2;
5440 vinfos[2].indices[0] = _ij2[0];
5441 vinfos[2].indices[1] = _ij2[1];
5442 vinfos[2].maxsolutions = _nj2;
5443 vinfos[3].jointtype = 1;
5444 vinfos[3].foffset = j3;
5445 vinfos[3].indices[0] = _ij3[0];
5446 vinfos[3].indices[1] = _ij3[1];
5447 vinfos[3].maxsolutions = _nj3;
5448 vinfos[4].jointtype = 1;
5449 vinfos[4].foffset = j4;
5450 vinfos[4].indices[0] = _ij4[0];
5451 vinfos[4].indices[1] = _ij4[1];
5452 vinfos[4].maxsolutions = _nj4;
5453 vinfos[5].jointtype = 1;
5454 vinfos[5].foffset = j5;
5455 vinfos[5].indices[0] = _ij5[0];
5456 vinfos[5].indices[1] = _ij5[1];
5457 vinfos[5].maxsolutions = _nj5;
5458 vinfos[6].jointtype = 1;
5459 vinfos[6].foffset = j6;
5460 vinfos[6].indices[0] = _ij6[0];
5461 vinfos[6].indices[1] = _ij6[1];
5462 vinfos[6].maxsolutions = _nj6;
5463 std::vector<int> vfree(0);
5464 solutions.AddSolution(vinfos, vfree);
5465 }
5466 }
5467 }
5468 }
5469 }
5470 }
5471 else
5472 {
5473 {
5474 IkReal j2array[1], cj2array[1], sj2array[1];
5475 bool j2valid[1] = { false };
5476 _nj2 = 1;
5477 IkReal x194 = ((1.0) * cj0);
5478 CheckValue<IkReal> x195 = IKPowWithIntegerCheck(new_r22, -1);
5479 if (!x195.valid)
5480 {
5481 continue;
5482 }
5483 if (IKabs((((new_r00 * sj0)) + (((-1.0) * new_r10 * x194)))) <
5484 IKFAST_ATAN2_MAGTHRESH &&
5485 IKabs(((x195.value) * (((((-1.0) * new_r10 * sj0)) +
5486 (((-1.0) * new_r00 * x194)))))) <
5487 IKFAST_ATAN2_MAGTHRESH &&
5488 IKabs(IKsqr((((new_r00 * sj0)) + (((-1.0) * new_r10 * x194)))) +
5489 IKsqr(((x195.value) * (((((-1.0) * new_r10 * sj0)) +
5490 (((-1.0) * new_r00 * x194)))))) -
5491 1) <= IKFAST_SINCOS_THRESH)
5492 continue;
5493 j2array[0] = IKatan2((((new_r00 * sj0)) + (((-1.0) * new_r10 * x194))),
5494 ((x195.value) * (((((-1.0) * new_r10 * sj0)) +
5495 (((-1.0) * new_r00 * x194))))));
5496 sj2array[0] = IKsin(j2array[0]);
5497 cj2array[0] = IKcos(j2array[0]);
5498 if (j2array[0] > IKPI)
5499 {
5500 j2array[0] -= IK2PI;
5501 }
5502 else if (j2array[0] < -IKPI)
5503 {
5504 j2array[0] += IK2PI;
5505 }
5506 j2valid[0] = true;
5507 for (int ij2 = 0; ij2 < 1; ++ij2)
5508 {
5509 if (!j2valid[ij2])
5510 {
5511 continue;
5512 }
5513 _ij2[0] = ij2;
5514 _ij2[1] = -1;
5515 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5516 {
5517 if (j2valid[iij2] &&
5518 IKabs(cj2array[ij2] - cj2array[iij2]) <
5519 IKFAST_SOLUTION_THRESH &&
5520 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
5521 {
5522 j2valid[iij2] = false;
5523 _ij2[1] = iij2;
5524 break;
5525 }
5526 }
5527 j2 = j2array[ij2];
5528 cj2 = cj2array[ij2];
5529 sj2 = sj2array[ij2];
5530 {
5531 IkReal evalcond[10];
5532 IkReal x196 = IKcos(j2);
5533 IkReal x197 = IKsin(j2);
5534 IkReal x198 = (cj0 * new_r22);
5535 IkReal x199 = ((1.0) * sj0);
5536 IkReal x200 = (new_r22 * sj0);
5537 IkReal x201 = (new_r22 * x197);
5538 IkReal x202 = ((1.0) * x197);
5539 evalcond[0] =
5540 (x197 + (((-1.0) * new_r00 * x199)) + ((cj0 * new_r10)));
5541 evalcond[1] =
5542 (x196 + (((-1.0) * new_r01 * x199)) + ((cj0 * new_r11)));
5543 evalcond[2] =
5544 (((new_r10 * sj0)) + ((new_r22 * x196)) + ((cj0 * new_r00)));
5545 evalcond[3] = (((new_r00 * x198)) + ((new_r10 * x200)) + x196);
5546 evalcond[4] = (((x196 * x200)) + ((cj0 * x197)) + new_r10);
5547 evalcond[5] =
5548 (((new_r11 * sj0)) + ((cj0 * new_r01)) + (((-1.0) * x201)));
5549 evalcond[6] =
5550 ((((-1.0) * x197 * x199)) + ((x196 * x198)) + new_r00);
5551 evalcond[7] = (((cj0 * x196)) + (((-1.0) * x199 * x201)) + new_r11);
5552 evalcond[8] =
5553 (((new_r01 * x198)) + ((new_r11 * x200)) + (((-1.0) * x202)));
5554 evalcond[9] =
5555 ((((-1.0) * x196 * x199)) + (((-1.0) * x198 * x202)) + new_r01);
5556 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5557 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5558 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5559 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5560 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5561 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5562 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5563 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
5564 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
5565 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH)
5566 {
5567 continue;
5568 }
5569 }
5570
5571 {
5572 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5573 vinfos[0].jointtype = 1;
5574 vinfos[0].foffset = j0;
5575 vinfos[0].indices[0] = _ij0[0];
5576 vinfos[0].indices[1] = _ij0[1];
5577 vinfos[0].maxsolutions = _nj0;
5578 vinfos[1].jointtype = 1;
5579 vinfos[1].foffset = j1;
5580 vinfos[1].indices[0] = _ij1[0];
5581 vinfos[1].indices[1] = _ij1[1];
5582 vinfos[1].maxsolutions = _nj1;
5583 vinfos[2].jointtype = 1;
5584 vinfos[2].foffset = j2;
5585 vinfos[2].indices[0] = _ij2[0];
5586 vinfos[2].indices[1] = _ij2[1];
5587 vinfos[2].maxsolutions = _nj2;
5588 vinfos[3].jointtype = 1;
5589 vinfos[3].foffset = j3;
5590 vinfos[3].indices[0] = _ij3[0];
5591 vinfos[3].indices[1] = _ij3[1];
5592 vinfos[3].maxsolutions = _nj3;
5593 vinfos[4].jointtype = 1;
5594 vinfos[4].foffset = j4;
5595 vinfos[4].indices[0] = _ij4[0];
5596 vinfos[4].indices[1] = _ij4[1];
5597 vinfos[4].maxsolutions = _nj4;
5598 vinfos[5].jointtype = 1;
5599 vinfos[5].foffset = j5;
5600 vinfos[5].indices[0] = _ij5[0];
5601 vinfos[5].indices[1] = _ij5[1];
5602 vinfos[5].maxsolutions = _nj5;
5603 vinfos[6].jointtype = 1;
5604 vinfos[6].foffset = j6;
5605 vinfos[6].indices[0] = _ij6[0];
5606 vinfos[6].indices[1] = _ij6[1];
5607 vinfos[6].maxsolutions = _nj6;
5608 std::vector<int> vfree(0);
5609 solutions.AddSolution(vinfos, vfree);
5610 }
5611 }
5612 }
5613 }
5614 }
5615 }
5616 else
5617 {
5618 {
5619 IkReal j2array[1], cj2array[1], sj2array[1];
5620 bool j2valid[1] = { false };
5621 _nj2 = 1;
5622 IkReal x203 = cj0 * cj0;
5623 IkReal x204 = new_r22 * new_r22;
5624 IkReal x205 = ((1.0) * cj0);
5625 IkReal x206 = (new_r22 * sj0);
5626 CheckValue<IkReal> x207 = IKatan2WithCheck(
5627 IkReal((((new_r11 * x206)) + (((-1.0) * new_r10 * x205)))),
5628 IkReal(((((-1.0) * new_r11 * x205)) + (((-1.0) * new_r10 * x206)))),
5629 IKFAST_ATAN2_MAGTHRESH);
5630 if (!x207.valid)
5631 {
5632 continue;
5633 }
5634 CheckValue<IkReal> x208 = IKPowWithIntegerCheck(
5635 IKsign((x204 + x203 + (((-1.0) * x203 * x204)))), -1);
5636 if (!x208.valid)
5637 {
5638 continue;
5639 }
5640 j2array[0] = ((-1.5707963267949) + (x207.value) +
5641 (((1.5707963267949) * (x208.value))));
5642 sj2array[0] = IKsin(j2array[0]);
5643 cj2array[0] = IKcos(j2array[0]);
5644 if (j2array[0] > IKPI)
5645 {
5646 j2array[0] -= IK2PI;
5647 }
5648 else if (j2array[0] < -IKPI)
5649 {
5650 j2array[0] += IK2PI;
5651 }
5652 j2valid[0] = true;
5653 for (int ij2 = 0; ij2 < 1; ++ij2)
5654 {
5655 if (!j2valid[ij2])
5656 {
5657 continue;
5658 }
5659 _ij2[0] = ij2;
5660 _ij2[1] = -1;
5661 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5662 {
5663 if (j2valid[iij2] &&
5664 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
5665 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
5666 {
5667 j2valid[iij2] = false;
5668 _ij2[1] = iij2;
5669 break;
5670 }
5671 }
5672 j2 = j2array[ij2];
5673 cj2 = cj2array[ij2];
5674 sj2 = sj2array[ij2];
5675 {
5676 IkReal evalcond[10];
5677 IkReal x209 = IKcos(j2);
5678 IkReal x210 = IKsin(j2);
5679 IkReal x211 = (cj0 * new_r22);
5680 IkReal x212 = ((1.0) * sj0);
5681 IkReal x213 = (new_r22 * sj0);
5682 IkReal x214 = (new_r22 * x210);
5683 IkReal x215 = ((1.0) * x210);
5684 evalcond[0] = (x210 + (((-1.0) * new_r00 * x212)) + ((cj0 * new_r10)));
5685 evalcond[1] = (x209 + (((-1.0) * new_r01 * x212)) + ((cj0 * new_r11)));
5686 evalcond[2] =
5687 (((new_r10 * sj0)) + ((new_r22 * x209)) + ((cj0 * new_r00)));
5688 evalcond[3] = (x209 + ((new_r00 * x211)) + ((new_r10 * x213)));
5689 evalcond[4] = (((cj0 * x210)) + ((x209 * x213)) + new_r10);
5690 evalcond[5] =
5691 (((new_r11 * sj0)) + (((-1.0) * x214)) + ((cj0 * new_r01)));
5692 evalcond[6] = (((x209 * x211)) + (((-1.0) * x210 * x212)) + new_r00);
5693 evalcond[7] = (((cj0 * x209)) + new_r11 + (((-1.0) * x212 * x214)));
5694 evalcond[8] =
5695 (((new_r01 * x211)) + (((-1.0) * x215)) + ((new_r11 * x213)));
5696 evalcond[9] =
5697 ((((-1.0) * x211 * x215)) + new_r01 + (((-1.0) * x209 * x212)));
5698 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5699 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5700 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5701 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5702 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5703 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5704 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5705 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
5706 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
5707 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH)
5708 {
5709 continue;
5710 }
5711 }
5712
5713 {
5714 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5715 vinfos[0].jointtype = 1;
5716 vinfos[0].foffset = j0;
5717 vinfos[0].indices[0] = _ij0[0];
5718 vinfos[0].indices[1] = _ij0[1];
5719 vinfos[0].maxsolutions = _nj0;
5720 vinfos[1].jointtype = 1;
5721 vinfos[1].foffset = j1;
5722 vinfos[1].indices[0] = _ij1[0];
5723 vinfos[1].indices[1] = _ij1[1];
5724 vinfos[1].maxsolutions = _nj1;
5725 vinfos[2].jointtype = 1;
5726 vinfos[2].foffset = j2;
5727 vinfos[2].indices[0] = _ij2[0];
5728 vinfos[2].indices[1] = _ij2[1];
5729 vinfos[2].maxsolutions = _nj2;
5730 vinfos[3].jointtype = 1;
5731 vinfos[3].foffset = j3;
5732 vinfos[3].indices[0] = _ij3[0];
5733 vinfos[3].indices[1] = _ij3[1];
5734 vinfos[3].maxsolutions = _nj3;
5735 vinfos[4].jointtype = 1;
5736 vinfos[4].foffset = j4;
5737 vinfos[4].indices[0] = _ij4[0];
5738 vinfos[4].indices[1] = _ij4[1];
5739 vinfos[4].maxsolutions = _nj4;
5740 vinfos[5].jointtype = 1;
5741 vinfos[5].foffset = j5;
5742 vinfos[5].indices[0] = _ij5[0];
5743 vinfos[5].indices[1] = _ij5[1];
5744 vinfos[5].maxsolutions = _nj5;
5745 vinfos[6].jointtype = 1;
5746 vinfos[6].foffset = j6;
5747 vinfos[6].indices[0] = _ij6[0];
5748 vinfos[6].indices[1] = _ij6[1];
5749 vinfos[6].maxsolutions = _nj6;
5750 std::vector<int> vfree(0);
5751 solutions.AddSolution(vinfos, vfree);
5752 }
5753 }
5754 }
5755 }
5756 }
5757 }
5758 }
5759 }
5760 }
5761 } while (0);
5762 if (bgotonextstatement)
5763 {
5764 bool bgotonextstatement = true;
5765 do
5766 {
5767 if (1)
5768 {
5769 bgotonextstatement = false;
5770 continue; // branch miss [j0, j2]
5771 }
5772 } while (0);
5773 if (bgotonextstatement)
5774 {
5775 }
5776 }
5777 }
5778 }
5779 }
5780 }
5781 else
5782 {
5783 {
5784 IkReal j0array[1], cj0array[1], sj0array[1];
5785 bool j0valid[1] = { false };
5786 _nj0 = 1;
5787 CheckValue<IkReal> x217 = IKPowWithIntegerCheck(sj1, -1);
5788 if (!x217.valid)
5789 {
5790 continue;
5791 }
5792 IkReal x216 = x217.value;
5793 CheckValue<IkReal> x218 = IKPowWithIntegerCheck(new_r12, -1);
5794 if (!x218.valid)
5795 {
5796 continue;
5797 }
5798 if (IKabs((x216 * (x218.value) *
5799 (((1.0) + (((-1.0) * (new_r02 * new_r02))) + (((-1.0) * (cj1 * cj1))))))) <
5800 IKFAST_ATAN2_MAGTHRESH &&
5801 IKabs((new_r02 * x216)) < IKFAST_ATAN2_MAGTHRESH &&
5802 IKabs(IKsqr((x216 * (x218.value) *
5803 (((1.0) + (((-1.0) * (new_r02 * new_r02))) + (((-1.0) * (cj1 * cj1))))))) +
5804 IKsqr((new_r02 * x216)) - 1) <= IKFAST_SINCOS_THRESH)
5805 continue;
5806 j0array[0] = IKatan2((x216 * (x218.value) *
5807 (((1.0) + (((-1.0) * (new_r02 * new_r02))) + (((-1.0) * (cj1 * cj1)))))),
5808 (new_r02 * x216));
5809 sj0array[0] = IKsin(j0array[0]);
5810 cj0array[0] = IKcos(j0array[0]);
5811 if (j0array[0] > IKPI)
5812 {
5813 j0array[0] -= IK2PI;
5814 }
5815 else if (j0array[0] < -IKPI)
5816 {
5817 j0array[0] += IK2PI;
5818 }
5819 j0valid[0] = true;
5820 for (int ij0 = 0; ij0 < 1; ++ij0)
5821 {
5822 if (!j0valid[ij0])
5823 {
5824 continue;
5825 }
5826 _ij0[0] = ij0;
5827 _ij0[1] = -1;
5828 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
5829 {
5830 if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
5831 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
5832 {
5833 j0valid[iij0] = false;
5834 _ij0[1] = iij0;
5835 break;
5836 }
5837 }
5838 j0 = j0array[ij0];
5839 cj0 = cj0array[ij0];
5840 sj0 = sj0array[ij0];
5841 {
5842 IkReal evalcond[8];
5843 IkReal x219 = IKcos(j0);
5844 IkReal x220 = IKsin(j0);
5845 IkReal x221 = ((1.0) * cj1);
5846 IkReal x222 = ((1.0) * sj1);
5847 IkReal x223 = (new_r12 * x220);
5848 IkReal x224 = (new_r02 * x219);
5849 evalcond[0] = ((((-1.0) * x219 * x222)) + new_r02);
5850 evalcond[1] = ((((-1.0) * x220 * x222)) + new_r12);
5851 evalcond[2] = (((new_r12 * x219)) + (((-1.0) * new_r02 * x220)));
5852 evalcond[3] = (x223 + x224 + (((-1.0) * x222)));
5853 evalcond[4] = (((cj1 * x224)) + ((cj1 * x223)) + (((-1.0) * new_r22 * x222)));
5854 evalcond[5] = ((((-1.0) * new_r10 * x220 * x222)) + (((-1.0) * new_r00 * x219 * x222)) +
5855 (((-1.0) * new_r20 * x221)));
5856 evalcond[6] = ((((-1.0) * new_r11 * x220 * x222)) + (((-1.0) * new_r01 * x219 * x222)) +
5857 (((-1.0) * new_r21 * x221)));
5858 evalcond[7] = ((1.0) + (((-1.0) * x222 * x223)) + (((-1.0) * x222 * x224)) +
5859 (((-1.0) * new_r22 * x221)));
5860 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5861 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5862 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5863 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5864 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5865 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5866 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5867 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
5868 {
5869 continue;
5870 }
5871 }
5872
5873 {
5874 IkReal j2eval[3];
5875 j2eval[0] = sj1;
5876 j2eval[1] = IKsign(sj1);
5877 j2eval[2] = ((IKabs(new_r20)) + (IKabs(new_r21)));
5878 if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
5879 IKabs(j2eval[2]) < 0.0000010000000000)
5880 {
5881 {
5882 IkReal j2eval[2];
5883 j2eval[0] = sj1;
5884 j2eval[1] = cj0;
5885 if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000)
5886 {
5887 {
5888 IkReal j2eval[3];
5889 j2eval[0] = sj1;
5890 j2eval[1] = cj1;
5891 j2eval[2] = sj0;
5892 if (IKabs(j2eval[0]) < 0.0000010000000000 ||
5893 IKabs(j2eval[1]) < 0.0000010000000000 || IKabs(j2eval[2]) < 0.0000010000000000)
5894 {
5895 {
5896 IkReal evalcond[5];
5897 bool bgotonextstatement = true;
5898 do
5899 {
5900 evalcond[0] =
5901 ((-3.14159265358979) +
5902 (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
5903 evalcond[1] = new_r21;
5904 evalcond[2] = new_r02;
5905 evalcond[3] = new_r12;
5906 evalcond[4] = new_r20;
5907 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
5908 IKabs(evalcond[1]) < 0.0000050000000000 &&
5909 IKabs(evalcond[2]) < 0.0000050000000000 &&
5910 IKabs(evalcond[3]) < 0.0000050000000000 &&
5911 IKabs(evalcond[4]) < 0.0000050000000000)
5912 {
5913 bgotonextstatement = false;
5914 {
5915 IkReal j2array[1], cj2array[1], sj2array[1];
5916 bool j2valid[1] = { false };
5917 _nj2 = 1;
5918 IkReal x225 = ((1.0) * cj0);
5919 if (IKabs(((((-1.0) * new_r10 * x225)) + ((new_r00 * sj0)))) <
5920 IKFAST_ATAN2_MAGTHRESH &&
5921 IKabs(((((-1.0) * new_r10 * sj0)) + (((-1.0) * new_r00 * x225)))) <
5922 IKFAST_ATAN2_MAGTHRESH &&
5923 IKabs(IKsqr(((((-1.0) * new_r10 * x225)) + ((new_r00 * sj0)))) +
5924 IKsqr(((((-1.0) * new_r10 * sj0)) +
5925 (((-1.0) * new_r00 * x225)))) -
5926 1) <= IKFAST_SINCOS_THRESH)
5927 continue;
5928 j2array[0] =
5929 IKatan2(((((-1.0) * new_r10 * x225)) + ((new_r00 * sj0))),
5930 ((((-1.0) * new_r10 * sj0)) + (((-1.0) * new_r00 * x225))));
5931 sj2array[0] = IKsin(j2array[0]);
5932 cj2array[0] = IKcos(j2array[0]);
5933 if (j2array[0] > IKPI)
5934 {
5935 j2array[0] -= IK2PI;
5936 }
5937 else if (j2array[0] < -IKPI)
5938 {
5939 j2array[0] += IK2PI;
5940 }
5941 j2valid[0] = true;
5942 for (int ij2 = 0; ij2 < 1; ++ij2)
5943 {
5944 if (!j2valid[ij2])
5945 {
5946 continue;
5947 }
5948 _ij2[0] = ij2;
5949 _ij2[1] = -1;
5950 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
5951 {
5952 if (j2valid[iij2] &&
5953 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
5954 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
5955 {
5956 j2valid[iij2] = false;
5957 _ij2[1] = iij2;
5958 break;
5959 }
5960 }
5961 j2 = j2array[ij2];
5962 cj2 = cj2array[ij2];
5963 sj2 = sj2array[ij2];
5964 {
5965 IkReal evalcond[8];
5966 IkReal x226 = IKcos(j2);
5967 IkReal x227 = IKsin(j2);
5968 IkReal x228 = ((1.0) * sj0);
5969 IkReal x229 = (cj0 * x226);
5970 IkReal x230 = (cj0 * x227);
5971 IkReal x231 = (x227 * x228);
5972 evalcond[0] = (((new_r10 * sj0)) + x226 + ((cj0 * new_r00)));
5973 evalcond[1] =
5974 (x227 + ((cj0 * new_r10)) + (((-1.0) * new_r00 * x228)));
5975 evalcond[2] =
5976 ((((-1.0) * new_r01 * x228)) + x226 + ((cj0 * new_r11)));
5977 evalcond[3] = (((sj0 * x226)) + x230 + new_r10);
5978 evalcond[4] =
5979 (((new_r11 * sj0)) + ((cj0 * new_r01)) + (((-1.0) * x227)));
5980 evalcond[5] = ((((-1.0) * x231)) + x229 + new_r00);
5981 evalcond[6] = ((((-1.0) * x231)) + x229 + new_r11);
5982 evalcond[7] =
5983 (new_r01 + (((-1.0) * x226 * x228)) + (((-1.0) * x230)));
5984 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
5985 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
5986 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
5987 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
5988 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
5989 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
5990 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
5991 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
5992 {
5993 continue;
5994 }
5995 }
5996
5997 {
5998 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
5999 vinfos[0].jointtype = 1;
6000 vinfos[0].foffset = j0;
6001 vinfos[0].indices[0] = _ij0[0];
6002 vinfos[0].indices[1] = _ij0[1];
6003 vinfos[0].maxsolutions = _nj0;
6004 vinfos[1].jointtype = 1;
6005 vinfos[1].foffset = j1;
6006 vinfos[1].indices[0] = _ij1[0];
6007 vinfos[1].indices[1] = _ij1[1];
6008 vinfos[1].maxsolutions = _nj1;
6009 vinfos[2].jointtype = 1;
6010 vinfos[2].foffset = j2;
6011 vinfos[2].indices[0] = _ij2[0];
6012 vinfos[2].indices[1] = _ij2[1];
6013 vinfos[2].maxsolutions = _nj2;
6014 vinfos[3].jointtype = 1;
6015 vinfos[3].foffset = j3;
6016 vinfos[3].indices[0] = _ij3[0];
6017 vinfos[3].indices[1] = _ij3[1];
6018 vinfos[3].maxsolutions = _nj3;
6019 vinfos[4].jointtype = 1;
6020 vinfos[4].foffset = j4;
6021 vinfos[4].indices[0] = _ij4[0];
6022 vinfos[4].indices[1] = _ij4[1];
6023 vinfos[4].maxsolutions = _nj4;
6024 vinfos[5].jointtype = 1;
6025 vinfos[5].foffset = j5;
6026 vinfos[5].indices[0] = _ij5[0];
6027 vinfos[5].indices[1] = _ij5[1];
6028 vinfos[5].maxsolutions = _nj5;
6029 vinfos[6].jointtype = 1;
6030 vinfos[6].foffset = j6;
6031 vinfos[6].indices[0] = _ij6[0];
6032 vinfos[6].indices[1] = _ij6[1];
6033 vinfos[6].maxsolutions = _nj6;
6034 std::vector<int> vfree(0);
6035 solutions.AddSolution(vinfos, vfree);
6036 }
6037 }
6038 }
6039 }
6040 } while (0);
6041 if (bgotonextstatement)
6042 {
6043 bool bgotonextstatement = true;
6044 do
6045 {
6046 evalcond[0] =
6047 ((-3.14159265358979) +
6048 (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
6049 6.28318530717959)));
6050 evalcond[1] = new_r21;
6051 evalcond[2] = new_r02;
6052 evalcond[3] = new_r12;
6053 evalcond[4] = new_r20;
6054 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6055 IKabs(evalcond[1]) < 0.0000050000000000 &&
6056 IKabs(evalcond[2]) < 0.0000050000000000 &&
6057 IKabs(evalcond[3]) < 0.0000050000000000 &&
6058 IKabs(evalcond[4]) < 0.0000050000000000)
6059 {
6060 bgotonextstatement = false;
6061 {
6062 IkReal j2array[1], cj2array[1], sj2array[1];
6063 bool j2valid[1] = { false };
6064 _nj2 = 1;
6065 IkReal x232 = ((1.0) * cj0);
6066 if (IKabs(((((-1.0) * new_r11 * sj0)) + (((-1.0) * new_r10 * x232)))) <
6067 IKFAST_ATAN2_MAGTHRESH &&
6068 IKabs((((new_r10 * sj0)) + (((-1.0) * new_r11 * x232)))) <
6069 IKFAST_ATAN2_MAGTHRESH &&
6070 IKabs(IKsqr(((((-1.0) * new_r11 * sj0)) +
6071 (((-1.0) * new_r10 * x232)))) +
6072 IKsqr((((new_r10 * sj0)) + (((-1.0) * new_r11 * x232)))) -
6073 1) <= IKFAST_SINCOS_THRESH)
6074 continue;
6075 j2array[0] =
6076 IKatan2(((((-1.0) * new_r11 * sj0)) + (((-1.0) * new_r10 * x232))),
6077 (((new_r10 * sj0)) + (((-1.0) * new_r11 * x232))));
6078 sj2array[0] = IKsin(j2array[0]);
6079 cj2array[0] = IKcos(j2array[0]);
6080 if (j2array[0] > IKPI)
6081 {
6082 j2array[0] -= IK2PI;
6083 }
6084 else if (j2array[0] < -IKPI)
6085 {
6086 j2array[0] += IK2PI;
6087 }
6088 j2valid[0] = true;
6089 for (int ij2 = 0; ij2 < 1; ++ij2)
6090 {
6091 if (!j2valid[ij2])
6092 {
6093 continue;
6094 }
6095 _ij2[0] = ij2;
6096 _ij2[1] = -1;
6097 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6098 {
6099 if (j2valid[iij2] &&
6100 IKabs(cj2array[ij2] - cj2array[iij2]) <
6101 IKFAST_SOLUTION_THRESH &&
6102 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
6103 {
6104 j2valid[iij2] = false;
6105 _ij2[1] = iij2;
6106 break;
6107 }
6108 }
6109 j2 = j2array[ij2];
6110 cj2 = cj2array[ij2];
6111 sj2 = sj2array[ij2];
6112 {
6113 IkReal evalcond[8];
6114 IkReal x233 = IKsin(j2);
6115 IkReal x234 = IKcos(j2);
6116 IkReal x235 = ((1.0) * sj0);
6117 IkReal x236 = (cj0 * x233);
6118 IkReal x237 = ((1.0) * x234);
6119 IkReal x238 = (x234 * x235);
6120 evalcond[0] = (((new_r11 * sj0)) + x233 + ((cj0 * new_r01)));
6121 evalcond[1] =
6122 (x233 + ((cj0 * new_r10)) + (((-1.0) * new_r00 * x235)));
6123 evalcond[2] =
6124 (x234 + (((-1.0) * new_r01 * x235)) + ((cj0 * new_r11)));
6125 evalcond[3] =
6126 (((new_r10 * sj0)) + (((-1.0) * x237)) + ((cj0 * new_r00)));
6127 evalcond[4] = (((sj0 * x233)) + new_r11 + ((cj0 * x234)));
6128 evalcond[5] = ((((-1.0) * x238)) + x236 + new_r10);
6129 evalcond[6] = ((((-1.0) * x238)) + x236 + new_r01);
6130 evalcond[7] =
6131 ((((-1.0) * x233 * x235)) + (((-1.0) * cj0 * x237)) + new_r00);
6132 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6133 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6134 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6135 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6136 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6137 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6138 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6139 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6140 {
6141 continue;
6142 }
6143 }
6144
6145 {
6146 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6147 vinfos[0].jointtype = 1;
6148 vinfos[0].foffset = j0;
6149 vinfos[0].indices[0] = _ij0[0];
6150 vinfos[0].indices[1] = _ij0[1];
6151 vinfos[0].maxsolutions = _nj0;
6152 vinfos[1].jointtype = 1;
6153 vinfos[1].foffset = j1;
6154 vinfos[1].indices[0] = _ij1[0];
6155 vinfos[1].indices[1] = _ij1[1];
6156 vinfos[1].maxsolutions = _nj1;
6157 vinfos[2].jointtype = 1;
6158 vinfos[2].foffset = j2;
6159 vinfos[2].indices[0] = _ij2[0];
6160 vinfos[2].indices[1] = _ij2[1];
6161 vinfos[2].maxsolutions = _nj2;
6162 vinfos[3].jointtype = 1;
6163 vinfos[3].foffset = j3;
6164 vinfos[3].indices[0] = _ij3[0];
6165 vinfos[3].indices[1] = _ij3[1];
6166 vinfos[3].maxsolutions = _nj3;
6167 vinfos[4].jointtype = 1;
6168 vinfos[4].foffset = j4;
6169 vinfos[4].indices[0] = _ij4[0];
6170 vinfos[4].indices[1] = _ij4[1];
6171 vinfos[4].maxsolutions = _nj4;
6172 vinfos[5].jointtype = 1;
6173 vinfos[5].foffset = j5;
6174 vinfos[5].indices[0] = _ij5[0];
6175 vinfos[5].indices[1] = _ij5[1];
6176 vinfos[5].maxsolutions = _nj5;
6177 vinfos[6].jointtype = 1;
6178 vinfos[6].foffset = j6;
6179 vinfos[6].indices[0] = _ij6[0];
6180 vinfos[6].indices[1] = _ij6[1];
6181 vinfos[6].maxsolutions = _nj6;
6182 std::vector<int> vfree(0);
6183 solutions.AddSolution(vinfos, vfree);
6184 }
6185 }
6186 }
6187 }
6188 } while (0);
6189 if (bgotonextstatement)
6190 {
6191 bool bgotonextstatement = true;
6192 do
6193 {
6194 evalcond[0] =
6195 ((-3.14159265358979) +
6196 (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j1)))),
6197 6.28318530717959)));
6198 evalcond[1] = new_r22;
6199 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6200 IKabs(evalcond[1]) < 0.0000050000000000)
6201 {
6202 bgotonextstatement = false;
6203 {
6204 IkReal j2array[1], cj2array[1], sj2array[1];
6205 bool j2valid[1] = { false };
6206 _nj2 = 1;
6207 if (IKabs(((-1.0) * new_r21)) < IKFAST_ATAN2_MAGTHRESH &&
6208 IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH &&
6209 IKabs(IKsqr(((-1.0) * new_r21)) + IKsqr(new_r20) - 1) <=
6210 IKFAST_SINCOS_THRESH)
6211 continue;
6212 j2array[0] = IKatan2(((-1.0) * new_r21), new_r20);
6213 sj2array[0] = IKsin(j2array[0]);
6214 cj2array[0] = IKcos(j2array[0]);
6215 if (j2array[0] > IKPI)
6216 {
6217 j2array[0] -= IK2PI;
6218 }
6219 else if (j2array[0] < -IKPI)
6220 {
6221 j2array[0] += IK2PI;
6222 }
6223 j2valid[0] = true;
6224 for (int ij2 = 0; ij2 < 1; ++ij2)
6225 {
6226 if (!j2valid[ij2])
6227 {
6228 continue;
6229 }
6230 _ij2[0] = ij2;
6231 _ij2[1] = -1;
6232 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6233 {
6234 if (j2valid[iij2] &&
6235 IKabs(cj2array[ij2] - cj2array[iij2]) <
6236 IKFAST_SOLUTION_THRESH &&
6237 IKabs(sj2array[ij2] - sj2array[iij2]) <
6238 IKFAST_SOLUTION_THRESH)
6239 {
6240 j2valid[iij2] = false;
6241 _ij2[1] = iij2;
6242 break;
6243 }
6244 }
6245 j2 = j2array[ij2];
6246 cj2 = cj2array[ij2];
6247 sj2 = sj2array[ij2];
6248 {
6249 IkReal evalcond[8];
6250 IkReal x239 = IKsin(j2);
6251 IkReal x240 = IKcos(j2);
6252 IkReal x241 = ((1.0) * sj0);
6253 evalcond[0] = (x239 + new_r21);
6254 evalcond[1] = ((((-1.0) * x240)) + new_r20);
6255 evalcond[2] = (((new_r02 * x239)) + new_r10);
6256 evalcond[3] = (((cj0 * x240)) + new_r11);
6257 evalcond[4] = (new_r00 + (((-1.0) * x239 * x241)));
6258 evalcond[5] = ((((-1.0) * x240 * x241)) + new_r01);
6259 evalcond[6] =
6260 ((((-1.0) * new_r00 * x241)) + x239 + ((cj0 * new_r10)));
6261 evalcond[7] =
6262 ((((-1.0) * new_r01 * x241)) + x240 + ((cj0 * new_r11)));
6263 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6264 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6265 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6266 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6267 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6268 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6269 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6270 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6271 {
6272 continue;
6273 }
6274 }
6275
6276 {
6277 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6278 vinfos[0].jointtype = 1;
6279 vinfos[0].foffset = j0;
6280 vinfos[0].indices[0] = _ij0[0];
6281 vinfos[0].indices[1] = _ij0[1];
6282 vinfos[0].maxsolutions = _nj0;
6283 vinfos[1].jointtype = 1;
6284 vinfos[1].foffset = j1;
6285 vinfos[1].indices[0] = _ij1[0];
6286 vinfos[1].indices[1] = _ij1[1];
6287 vinfos[1].maxsolutions = _nj1;
6288 vinfos[2].jointtype = 1;
6289 vinfos[2].foffset = j2;
6290 vinfos[2].indices[0] = _ij2[0];
6291 vinfos[2].indices[1] = _ij2[1];
6292 vinfos[2].maxsolutions = _nj2;
6293 vinfos[3].jointtype = 1;
6294 vinfos[3].foffset = j3;
6295 vinfos[3].indices[0] = _ij3[0];
6296 vinfos[3].indices[1] = _ij3[1];
6297 vinfos[3].maxsolutions = _nj3;
6298 vinfos[4].jointtype = 1;
6299 vinfos[4].foffset = j4;
6300 vinfos[4].indices[0] = _ij4[0];
6301 vinfos[4].indices[1] = _ij4[1];
6302 vinfos[4].maxsolutions = _nj4;
6303 vinfos[5].jointtype = 1;
6304 vinfos[5].foffset = j5;
6305 vinfos[5].indices[0] = _ij5[0];
6306 vinfos[5].indices[1] = _ij5[1];
6307 vinfos[5].maxsolutions = _nj5;
6308 vinfos[6].jointtype = 1;
6309 vinfos[6].foffset = j6;
6310 vinfos[6].indices[0] = _ij6[0];
6311 vinfos[6].indices[1] = _ij6[1];
6312 vinfos[6].maxsolutions = _nj6;
6313 std::vector<int> vfree(0);
6314 solutions.AddSolution(vinfos, vfree);
6315 }
6316 }
6317 }
6318 }
6319 } while (0);
6320 if (bgotonextstatement)
6321 {
6322 bool bgotonextstatement = true;
6323 do
6324 {
6325 evalcond[0] =
6326 ((-3.14159265358979) +
6327 (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j1)))),
6328 6.28318530717959)));
6329 evalcond[1] = new_r22;
6330 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6331 IKabs(evalcond[1]) < 0.0000050000000000)
6332 {
6333 bgotonextstatement = false;
6334 {
6335 IkReal j2array[1], cj2array[1], sj2array[1];
6336 bool j2valid[1] = { false };
6337 _nj2 = 1;
6338 if (IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH &&
6339 IKabs(((-1.0) * new_r20)) < IKFAST_ATAN2_MAGTHRESH &&
6340 IKabs(IKsqr(new_r21) + IKsqr(((-1.0) * new_r20)) - 1) <=
6341 IKFAST_SINCOS_THRESH)
6342 continue;
6343 j2array[0] = IKatan2(new_r21, ((-1.0) * new_r20));
6344 sj2array[0] = IKsin(j2array[0]);
6345 cj2array[0] = IKcos(j2array[0]);
6346 if (j2array[0] > IKPI)
6347 {
6348 j2array[0] -= IK2PI;
6349 }
6350 else if (j2array[0] < -IKPI)
6351 {
6352 j2array[0] += IK2PI;
6353 }
6354 j2valid[0] = true;
6355 for (int ij2 = 0; ij2 < 1; ++ij2)
6356 {
6357 if (!j2valid[ij2])
6358 {
6359 continue;
6360 }
6361 _ij2[0] = ij2;
6362 _ij2[1] = -1;
6363 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6364 {
6365 if (j2valid[iij2] &&
6366 IKabs(cj2array[ij2] - cj2array[iij2]) <
6367 IKFAST_SOLUTION_THRESH &&
6368 IKabs(sj2array[ij2] - sj2array[iij2]) <
6369 IKFAST_SOLUTION_THRESH)
6370 {
6371 j2valid[iij2] = false;
6372 _ij2[1] = iij2;
6373 break;
6374 }
6375 }
6376 j2 = j2array[ij2];
6377 cj2 = cj2array[ij2];
6378 sj2 = sj2array[ij2];
6379 {
6380 IkReal evalcond[8];
6381 IkReal x242 = IKcos(j2);
6382 IkReal x243 = IKsin(j2);
6383 IkReal x244 = ((1.0) * sj0);
6384 IkReal x245 = ((1.0) * x243);
6385 evalcond[0] = (x242 + new_r20);
6386 evalcond[1] = ((((-1.0) * x245)) + new_r21);
6387 evalcond[2] = (((cj0 * x242)) + new_r11);
6388 evalcond[3] = ((((-1.0) * new_r02 * x245)) + new_r10);
6389 evalcond[4] = ((((-1.0) * x243 * x244)) + new_r00);
6390 evalcond[5] = ((((-1.0) * x242 * x244)) + new_r01);
6391 evalcond[6] =
6392 ((((-1.0) * new_r00 * x244)) + x243 + ((cj0 * new_r10)));
6393 evalcond[7] =
6394 ((((-1.0) * new_r01 * x244)) + x242 + ((cj0 * new_r11)));
6395 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6396 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6397 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6398 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6399 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6400 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6401 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6402 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6403 {
6404 continue;
6405 }
6406 }
6407
6408 {
6409 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6410 vinfos[0].jointtype = 1;
6411 vinfos[0].foffset = j0;
6412 vinfos[0].indices[0] = _ij0[0];
6413 vinfos[0].indices[1] = _ij0[1];
6414 vinfos[0].maxsolutions = _nj0;
6415 vinfos[1].jointtype = 1;
6416 vinfos[1].foffset = j1;
6417 vinfos[1].indices[0] = _ij1[0];
6418 vinfos[1].indices[1] = _ij1[1];
6419 vinfos[1].maxsolutions = _nj1;
6420 vinfos[2].jointtype = 1;
6421 vinfos[2].foffset = j2;
6422 vinfos[2].indices[0] = _ij2[0];
6423 vinfos[2].indices[1] = _ij2[1];
6424 vinfos[2].maxsolutions = _nj2;
6425 vinfos[3].jointtype = 1;
6426 vinfos[3].foffset = j3;
6427 vinfos[3].indices[0] = _ij3[0];
6428 vinfos[3].indices[1] = _ij3[1];
6429 vinfos[3].maxsolutions = _nj3;
6430 vinfos[4].jointtype = 1;
6431 vinfos[4].foffset = j4;
6432 vinfos[4].indices[0] = _ij4[0];
6433 vinfos[4].indices[1] = _ij4[1];
6434 vinfos[4].maxsolutions = _nj4;
6435 vinfos[5].jointtype = 1;
6436 vinfos[5].foffset = j5;
6437 vinfos[5].indices[0] = _ij5[0];
6438 vinfos[5].indices[1] = _ij5[1];
6439 vinfos[5].maxsolutions = _nj5;
6440 vinfos[6].jointtype = 1;
6441 vinfos[6].foffset = j6;
6442 vinfos[6].indices[0] = _ij6[0];
6443 vinfos[6].indices[1] = _ij6[1];
6444 vinfos[6].maxsolutions = _nj6;
6445 std::vector<int> vfree(0);
6446 solutions.AddSolution(vinfos, vfree);
6447 }
6448 }
6449 }
6450 }
6451 } while (0);
6452 if (bgotonextstatement)
6453 {
6454 bool bgotonextstatement = true;
6455 do
6456 {
6457 evalcond[0] =
6458 ((-3.14159265358979) +
6459 (IKfmod(((3.14159265358979) + (IKabs(j0))), 6.28318530717959)));
6460 evalcond[1] = new_r12;
6461 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6462 IKabs(evalcond[1]) < 0.0000050000000000)
6463 {
6464 bgotonextstatement = false;
6465 {
6466 IkReal j2array[1], cj2array[1], sj2array[1];
6467 bool j2valid[1] = { false };
6468 _nj2 = 1;
6469 if (IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
6470 IKabs(((-1.0) * new_r11)) < IKFAST_ATAN2_MAGTHRESH &&
6471 IKabs(IKsqr(((-1.0) * new_r10)) + IKsqr(((-1.0) * new_r11)) -
6472 1) <= IKFAST_SINCOS_THRESH)
6473 continue;
6474 j2array[0] = IKatan2(((-1.0) * new_r10), ((-1.0) * new_r11));
6475 sj2array[0] = IKsin(j2array[0]);
6476 cj2array[0] = IKcos(j2array[0]);
6477 if (j2array[0] > IKPI)
6478 {
6479 j2array[0] -= IK2PI;
6480 }
6481 else if (j2array[0] < -IKPI)
6482 {
6483 j2array[0] += IK2PI;
6484 }
6485 j2valid[0] = true;
6486 for (int ij2 = 0; ij2 < 1; ++ij2)
6487 {
6488 if (!j2valid[ij2])
6489 {
6490 continue;
6491 }
6492 _ij2[0] = ij2;
6493 _ij2[1] = -1;
6494 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6495 {
6496 if (j2valid[iij2] &&
6497 IKabs(cj2array[ij2] - cj2array[iij2]) <
6498 IKFAST_SOLUTION_THRESH &&
6499 IKabs(sj2array[ij2] - sj2array[iij2]) <
6500 IKFAST_SOLUTION_THRESH)
6501 {
6502 j2valid[iij2] = false;
6503 _ij2[1] = iij2;
6504 break;
6505 }
6506 }
6507 j2 = j2array[ij2];
6508 cj2 = cj2array[ij2];
6509 sj2 = sj2array[ij2];
6510 {
6511 IkReal evalcond[8];
6512 IkReal x246 = IKsin(j2);
6513 IkReal x247 = IKcos(j2);
6514 IkReal x248 = ((1.0) * sj1);
6515 IkReal x249 = ((1.0) * x246);
6516 evalcond[0] = (x246 + new_r10);
6517 evalcond[1] = (x247 + new_r11);
6518 evalcond[2] = (new_r21 + ((sj1 * x246)));
6519 evalcond[3] = (((cj1 * x247)) + new_r00);
6520 evalcond[4] = (new_r20 + (((-1.0) * x247 * x248)));
6521 evalcond[5] = ((((-1.0) * cj1 * x249)) + new_r01);
6522 evalcond[6] =
6523 (((cj1 * new_r00)) + x247 + (((-1.0) * new_r20 * x248)));
6524 evalcond[7] = ((((-1.0) * new_r21 * x248)) +
6525 ((cj1 * new_r01)) + (((-1.0) * x249)));
6526 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6527 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6528 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6529 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6530 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6531 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6532 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6533 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6534 {
6535 continue;
6536 }
6537 }
6538
6539 {
6540 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6541 vinfos[0].jointtype = 1;
6542 vinfos[0].foffset = j0;
6543 vinfos[0].indices[0] = _ij0[0];
6544 vinfos[0].indices[1] = _ij0[1];
6545 vinfos[0].maxsolutions = _nj0;
6546 vinfos[1].jointtype = 1;
6547 vinfos[1].foffset = j1;
6548 vinfos[1].indices[0] = _ij1[0];
6549 vinfos[1].indices[1] = _ij1[1];
6550 vinfos[1].maxsolutions = _nj1;
6551 vinfos[2].jointtype = 1;
6552 vinfos[2].foffset = j2;
6553 vinfos[2].indices[0] = _ij2[0];
6554 vinfos[2].indices[1] = _ij2[1];
6555 vinfos[2].maxsolutions = _nj2;
6556 vinfos[3].jointtype = 1;
6557 vinfos[3].foffset = j3;
6558 vinfos[3].indices[0] = _ij3[0];
6559 vinfos[3].indices[1] = _ij3[1];
6560 vinfos[3].maxsolutions = _nj3;
6561 vinfos[4].jointtype = 1;
6562 vinfos[4].foffset = j4;
6563 vinfos[4].indices[0] = _ij4[0];
6564 vinfos[4].indices[1] = _ij4[1];
6565 vinfos[4].maxsolutions = _nj4;
6566 vinfos[5].jointtype = 1;
6567 vinfos[5].foffset = j5;
6568 vinfos[5].indices[0] = _ij5[0];
6569 vinfos[5].indices[1] = _ij5[1];
6570 vinfos[5].maxsolutions = _nj5;
6571 vinfos[6].jointtype = 1;
6572 vinfos[6].foffset = j6;
6573 vinfos[6].indices[0] = _ij6[0];
6574 vinfos[6].indices[1] = _ij6[1];
6575 vinfos[6].maxsolutions = _nj6;
6576 std::vector<int> vfree(0);
6577 solutions.AddSolution(vinfos, vfree);
6578 }
6579 }
6580 }
6581 }
6582 } while (0);
6583 if (bgotonextstatement)
6584 {
6585 bool bgotonextstatement = true;
6586 do
6587 {
6588 evalcond[0] = ((-3.14159265358979) +
6589 (IKfmod(((3.14159265358979) +
6590 (IKabs(((-3.14159265358979) + j0)))),
6591 6.28318530717959)));
6592 evalcond[1] = new_r12;
6593 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6594 IKabs(evalcond[1]) < 0.0000050000000000)
6595 {
6596 bgotonextstatement = false;
6597 {
6598 IkReal j2array[1], cj2array[1], sj2array[1];
6599 bool j2valid[1] = { false };
6600 _nj2 = 1;
6601 if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
6602 IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH &&
6603 IKabs(IKsqr(new_r10) + IKsqr(new_r11) - 1) <=
6604 IKFAST_SINCOS_THRESH)
6605 continue;
6606 j2array[0] = IKatan2(new_r10, new_r11);
6607 sj2array[0] = IKsin(j2array[0]);
6608 cj2array[0] = IKcos(j2array[0]);
6609 if (j2array[0] > IKPI)
6610 {
6611 j2array[0] -= IK2PI;
6612 }
6613 else if (j2array[0] < -IKPI)
6614 {
6615 j2array[0] += IK2PI;
6616 }
6617 j2valid[0] = true;
6618 for (int ij2 = 0; ij2 < 1; ++ij2)
6619 {
6620 if (!j2valid[ij2])
6621 {
6622 continue;
6623 }
6624 _ij2[0] = ij2;
6625 _ij2[1] = -1;
6626 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6627 {
6628 if (j2valid[iij2] &&
6629 IKabs(cj2array[ij2] - cj2array[iij2]) <
6630 IKFAST_SOLUTION_THRESH &&
6631 IKabs(sj2array[ij2] - sj2array[iij2]) <
6632 IKFAST_SOLUTION_THRESH)
6633 {
6634 j2valid[iij2] = false;
6635 _ij2[1] = iij2;
6636 break;
6637 }
6638 }
6639 j2 = j2array[ij2];
6640 cj2 = cj2array[ij2];
6641 sj2 = sj2array[ij2];
6642 {
6643 IkReal evalcond[8];
6644 IkReal x250 = IKsin(j2);
6645 IkReal x251 = IKcos(j2);
6646 IkReal x252 = ((1.0) * sj1);
6647 IkReal x253 = ((1.0) * new_r00);
6648 IkReal x254 = ((1.0) * new_r01);
6649 IkReal x255 = ((1.0) * x250);
6650 evalcond[0] = (((sj1 * x250)) + new_r21);
6651 evalcond[1] = (x250 + (((-1.0) * new_r10)));
6652 evalcond[2] = (x251 + (((-1.0) * new_r11)));
6653 evalcond[3] = ((((-1.0) * x251 * x252)) + new_r20);
6654 evalcond[4] = (((cj1 * x251)) + (((-1.0) * x253)));
6655 evalcond[5] = ((((-1.0) * cj1 * x255)) + (((-1.0) * x254)));
6656 evalcond[6] = (x251 + (((-1.0) * cj1 * x253)) +
6657 (((-1.0) * new_r20 * x252)));
6658 evalcond[7] = ((((-1.0) * new_r21 * x252)) +
6659 (((-1.0) * cj1 * x254)) + (((-1.0) * x255)));
6660 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6661 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6662 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6663 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6664 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6665 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6666 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6667 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6668 {
6669 continue;
6670 }
6671 }
6672
6673 {
6674 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6675 vinfos[0].jointtype = 1;
6676 vinfos[0].foffset = j0;
6677 vinfos[0].indices[0] = _ij0[0];
6678 vinfos[0].indices[1] = _ij0[1];
6679 vinfos[0].maxsolutions = _nj0;
6680 vinfos[1].jointtype = 1;
6681 vinfos[1].foffset = j1;
6682 vinfos[1].indices[0] = _ij1[0];
6683 vinfos[1].indices[1] = _ij1[1];
6684 vinfos[1].maxsolutions = _nj1;
6685 vinfos[2].jointtype = 1;
6686 vinfos[2].foffset = j2;
6687 vinfos[2].indices[0] = _ij2[0];
6688 vinfos[2].indices[1] = _ij2[1];
6689 vinfos[2].maxsolutions = _nj2;
6690 vinfos[3].jointtype = 1;
6691 vinfos[3].foffset = j3;
6692 vinfos[3].indices[0] = _ij3[0];
6693 vinfos[3].indices[1] = _ij3[1];
6694 vinfos[3].maxsolutions = _nj3;
6695 vinfos[4].jointtype = 1;
6696 vinfos[4].foffset = j4;
6697 vinfos[4].indices[0] = _ij4[0];
6698 vinfos[4].indices[1] = _ij4[1];
6699 vinfos[4].maxsolutions = _nj4;
6700 vinfos[5].jointtype = 1;
6701 vinfos[5].foffset = j5;
6702 vinfos[5].indices[0] = _ij5[0];
6703 vinfos[5].indices[1] = _ij5[1];
6704 vinfos[5].maxsolutions = _nj5;
6705 vinfos[6].jointtype = 1;
6706 vinfos[6].foffset = j6;
6707 vinfos[6].indices[0] = _ij6[0];
6708 vinfos[6].indices[1] = _ij6[1];
6709 vinfos[6].maxsolutions = _nj6;
6710 std::vector<int> vfree(0);
6711 solutions.AddSolution(vinfos, vfree);
6712 }
6713 }
6714 }
6715 }
6716 } while (0);
6717 if (bgotonextstatement)
6718 {
6719 bool bgotonextstatement = true;
6720 do
6721 {
6722 evalcond[0] = ((-3.14159265358979) +
6723 (IKfmod(((3.14159265358979) +
6724 (IKabs(((-1.5707963267949) + j0)))),
6725 6.28318530717959)));
6726 evalcond[1] = new_r02;
6727 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6728 IKabs(evalcond[1]) < 0.0000050000000000)
6729 {
6730 bgotonextstatement = false;
6731 {
6732 IkReal j2array[1], cj2array[1], sj2array[1];
6733 bool j2valid[1] = { false };
6734 _nj2 = 1;
6735 if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
6736 IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH &&
6737 IKabs(IKsqr(new_r00) + IKsqr(new_r01) - 1) <=
6738 IKFAST_SINCOS_THRESH)
6739 continue;
6740 j2array[0] = IKatan2(new_r00, new_r01);
6741 sj2array[0] = IKsin(j2array[0]);
6742 cj2array[0] = IKcos(j2array[0]);
6743 if (j2array[0] > IKPI)
6744 {
6745 j2array[0] -= IK2PI;
6746 }
6747 else if (j2array[0] < -IKPI)
6748 {
6749 j2array[0] += IK2PI;
6750 }
6751 j2valid[0] = true;
6752 for (int ij2 = 0; ij2 < 1; ++ij2)
6753 {
6754 if (!j2valid[ij2])
6755 {
6756 continue;
6757 }
6758 _ij2[0] = ij2;
6759 _ij2[1] = -1;
6760 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6761 {
6762 if (j2valid[iij2] &&
6763 IKabs(cj2array[ij2] - cj2array[iij2]) <
6764 IKFAST_SOLUTION_THRESH &&
6765 IKabs(sj2array[ij2] - sj2array[iij2]) <
6766 IKFAST_SOLUTION_THRESH)
6767 {
6768 j2valid[iij2] = false;
6769 _ij2[1] = iij2;
6770 break;
6771 }
6772 }
6773 j2 = j2array[ij2];
6774 cj2 = cj2array[ij2];
6775 sj2 = sj2array[ij2];
6776 {
6777 IkReal evalcond[8];
6778 IkReal x256 = IKsin(j2);
6779 IkReal x257 = IKcos(j2);
6780 IkReal x258 = ((1.0) * sj1);
6781 IkReal x259 = ((1.0) * x256);
6782 evalcond[0] = (((sj1 * x256)) + new_r21);
6783 evalcond[1] = (x256 + (((-1.0) * new_r00)));
6784 evalcond[2] = (x257 + (((-1.0) * new_r01)));
6785 evalcond[3] = (((cj1 * x257)) + new_r10);
6786 evalcond[4] = ((((-1.0) * x257 * x258)) + new_r20);
6787 evalcond[5] = ((((-1.0) * cj1 * x259)) + new_r11);
6788 evalcond[6] = (((cj1 * new_r10)) + x257 +
6789 (((-1.0) * new_r20 * x258)));
6790 evalcond[7] = ((((-1.0) * new_r21 * x258)) +
6791 ((cj1 * new_r11)) + (((-1.0) * x259)));
6792 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6793 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6794 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6795 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6796 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6797 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6798 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6799 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6800 {
6801 continue;
6802 }
6803 }
6804
6805 {
6806 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6807 vinfos[0].jointtype = 1;
6808 vinfos[0].foffset = j0;
6809 vinfos[0].indices[0] = _ij0[0];
6810 vinfos[0].indices[1] = _ij0[1];
6811 vinfos[0].maxsolutions = _nj0;
6812 vinfos[1].jointtype = 1;
6813 vinfos[1].foffset = j1;
6814 vinfos[1].indices[0] = _ij1[0];
6815 vinfos[1].indices[1] = _ij1[1];
6816 vinfos[1].maxsolutions = _nj1;
6817 vinfos[2].jointtype = 1;
6818 vinfos[2].foffset = j2;
6819 vinfos[2].indices[0] = _ij2[0];
6820 vinfos[2].indices[1] = _ij2[1];
6821 vinfos[2].maxsolutions = _nj2;
6822 vinfos[3].jointtype = 1;
6823 vinfos[3].foffset = j3;
6824 vinfos[3].indices[0] = _ij3[0];
6825 vinfos[3].indices[1] = _ij3[1];
6826 vinfos[3].maxsolutions = _nj3;
6827 vinfos[4].jointtype = 1;
6828 vinfos[4].foffset = j4;
6829 vinfos[4].indices[0] = _ij4[0];
6830 vinfos[4].indices[1] = _ij4[1];
6831 vinfos[4].maxsolutions = _nj4;
6832 vinfos[5].jointtype = 1;
6833 vinfos[5].foffset = j5;
6834 vinfos[5].indices[0] = _ij5[0];
6835 vinfos[5].indices[1] = _ij5[1];
6836 vinfos[5].maxsolutions = _nj5;
6837 vinfos[6].jointtype = 1;
6838 vinfos[6].foffset = j6;
6839 vinfos[6].indices[0] = _ij6[0];
6840 vinfos[6].indices[1] = _ij6[1];
6841 vinfos[6].maxsolutions = _nj6;
6842 std::vector<int> vfree(0);
6843 solutions.AddSolution(vinfos, vfree);
6844 }
6845 }
6846 }
6847 }
6848 } while (0);
6849 if (bgotonextstatement)
6850 {
6851 bool bgotonextstatement = true;
6852 do
6853 {
6854 evalcond[0] = ((-3.14159265358979) +
6855 (IKfmod(((3.14159265358979) +
6856 (IKabs(((1.5707963267949) + j0)))),
6857 6.28318530717959)));
6858 evalcond[1] = new_r02;
6859 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
6860 IKabs(evalcond[1]) < 0.0000050000000000)
6861 {
6862 bgotonextstatement = false;
6863 {
6864 IkReal j2array[1], cj2array[1], sj2array[1];
6865 bool j2valid[1] = { false };
6866 _nj2 = 1;
6867 if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
6868 IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
6869 IKabs(IKsqr(((-1.0) * new_r00)) +
6870 IKsqr(((-1.0) * new_r01)) - 1) <=
6871 IKFAST_SINCOS_THRESH)
6872 continue;
6873 j2array[0] =
6874 IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
6875 sj2array[0] = IKsin(j2array[0]);
6876 cj2array[0] = IKcos(j2array[0]);
6877 if (j2array[0] > IKPI)
6878 {
6879 j2array[0] -= IK2PI;
6880 }
6881 else if (j2array[0] < -IKPI)
6882 {
6883 j2array[0] += IK2PI;
6884 }
6885 j2valid[0] = true;
6886 for (int ij2 = 0; ij2 < 1; ++ij2)
6887 {
6888 if (!j2valid[ij2])
6889 {
6890 continue;
6891 }
6892 _ij2[0] = ij2;
6893 _ij2[1] = -1;
6894 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
6895 {
6896 if (j2valid[iij2] &&
6897 IKabs(cj2array[ij2] - cj2array[iij2]) <
6898 IKFAST_SOLUTION_THRESH &&
6899 IKabs(sj2array[ij2] - sj2array[iij2]) <
6900 IKFAST_SOLUTION_THRESH)
6901 {
6902 j2valid[iij2] = false;
6903 _ij2[1] = iij2;
6904 break;
6905 }
6906 }
6907 j2 = j2array[ij2];
6908 cj2 = cj2array[ij2];
6909 sj2 = sj2array[ij2];
6910 {
6911 IkReal evalcond[8];
6912 IkReal x260 = IKsin(j2);
6913 IkReal x261 = IKcos(j2);
6914 IkReal x262 = ((1.0) * new_r11);
6915 IkReal x263 = ((1.0) * sj1);
6916 IkReal x264 = ((1.0) * new_r10);
6917 IkReal x265 = ((1.0) * x260);
6918 evalcond[0] = (x260 + new_r00);
6919 evalcond[1] = (x261 + new_r01);
6920 evalcond[2] = (new_r21 + ((sj1 * x260)));
6921 evalcond[3] = ((((-1.0) * x261 * x263)) + new_r20);
6922 evalcond[4] = (((cj1 * x261)) + (((-1.0) * x264)));
6923 evalcond[5] =
6924 ((((-1.0) * cj1 * x265)) + (((-1.0) * x262)));
6925 evalcond[6] = ((((-1.0) * cj1 * x264)) + x261 +
6926 (((-1.0) * new_r20 * x263)));
6927 evalcond[7] =
6928 ((((-1.0) * cj1 * x262)) +
6929 (((-1.0) * new_r21 * x263)) + (((-1.0) * x265)));
6930 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
6931 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
6932 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
6933 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
6934 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
6935 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
6936 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
6937 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
6938 {
6939 continue;
6940 }
6941 }
6942
6943 {
6944 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
6945 vinfos[0].jointtype = 1;
6946 vinfos[0].foffset = j0;
6947 vinfos[0].indices[0] = _ij0[0];
6948 vinfos[0].indices[1] = _ij0[1];
6949 vinfos[0].maxsolutions = _nj0;
6950 vinfos[1].jointtype = 1;
6951 vinfos[1].foffset = j1;
6952 vinfos[1].indices[0] = _ij1[0];
6953 vinfos[1].indices[1] = _ij1[1];
6954 vinfos[1].maxsolutions = _nj1;
6955 vinfos[2].jointtype = 1;
6956 vinfos[2].foffset = j2;
6957 vinfos[2].indices[0] = _ij2[0];
6958 vinfos[2].indices[1] = _ij2[1];
6959 vinfos[2].maxsolutions = _nj2;
6960 vinfos[3].jointtype = 1;
6961 vinfos[3].foffset = j3;
6962 vinfos[3].indices[0] = _ij3[0];
6963 vinfos[3].indices[1] = _ij3[1];
6964 vinfos[3].maxsolutions = _nj3;
6965 vinfos[4].jointtype = 1;
6966 vinfos[4].foffset = j4;
6967 vinfos[4].indices[0] = _ij4[0];
6968 vinfos[4].indices[1] = _ij4[1];
6969 vinfos[4].maxsolutions = _nj4;
6970 vinfos[5].jointtype = 1;
6971 vinfos[5].foffset = j5;
6972 vinfos[5].indices[0] = _ij5[0];
6973 vinfos[5].indices[1] = _ij5[1];
6974 vinfos[5].maxsolutions = _nj5;
6975 vinfos[6].jointtype = 1;
6976 vinfos[6].foffset = j6;
6977 vinfos[6].indices[0] = _ij6[0];
6978 vinfos[6].indices[1] = _ij6[1];
6979 vinfos[6].maxsolutions = _nj6;
6980 std::vector<int> vfree(0);
6981 solutions.AddSolution(vinfos, vfree);
6982 }
6983 }
6984 }
6985 }
6986 } while (0);
6987 if (bgotonextstatement)
6988 {
6989 bool bgotonextstatement = true;
6990 do
6991 {
6992 evalcond[0] = ((IKabs(new_r20)) + (IKabs(new_r21)));
6993 if (IKabs(evalcond[0]) < 0.0000050000000000)
6994 {
6995 bgotonextstatement = false;
6996 {
6997 IkReal j2eval[1];
6998 new_r21 = 0;
6999 new_r20 = 0;
7000 new_r02 = 0;
7001 new_r12 = 0;
7002 j2eval[0] = IKabs(new_r22);
7003 if (IKabs(j2eval[0]) < 0.0000000100000000)
7004 {
7005 continue; // no branches [j2]
7006 }
7007 else
7008 {
7009 IkReal op[2 + 1], zeror[2];
7010 int numroots;
7011 op[0] = ((-1.0) * new_r22);
7012 op[1] = 0;
7013 op[2] = new_r22;
7014 polyroots2(op, zeror, numroots);
7015 IkReal j2array[2], cj2array[2], sj2array[2],
7016 tempj2array[1];
7017 int numsolutions = 0;
7018 for (int ij2 = 0; ij2 < numroots; ++ij2)
7019 {
7020 IkReal htj2 = zeror[ij2];
7021 tempj2array[0] = ((2.0) * (atan(htj2)));
7022 for (int kj2 = 0; kj2 < 1; ++kj2)
7023 {
7024 j2array[numsolutions] = tempj2array[kj2];
7025 if (j2array[numsolutions] > IKPI)
7026 {
7027 j2array[numsolutions] -= IK2PI;
7028 }
7029 else if (j2array[numsolutions] < -IKPI)
7030 {
7031 j2array[numsolutions] += IK2PI;
7032 }
7033 sj2array[numsolutions] =
7034 IKsin(j2array[numsolutions]);
7035 cj2array[numsolutions] =
7036 IKcos(j2array[numsolutions]);
7037 numsolutions++;
7038 }
7039 }
7040 bool j2valid[2] = { true, true };
7041 _nj2 = 2;
7042 for (int ij2 = 0; ij2 < numsolutions; ++ij2)
7043 {
7044 if (!j2valid[ij2])
7045 {
7046 continue;
7047 }
7048 j2 = j2array[ij2];
7049 cj2 = cj2array[ij2];
7050 sj2 = sj2array[ij2];
7051 htj2 = IKtan(j2 / 2);
7052
7053 _ij2[0] = ij2;
7054 _ij2[1] = -1;
7055 for (int iij2 = ij2 + 1; iij2 < numsolutions; ++iij2)
7056 {
7057 if (j2valid[iij2] &&
7058 IKabs(cj2array[ij2] - cj2array[iij2]) <
7059 IKFAST_SOLUTION_THRESH &&
7060 IKabs(sj2array[ij2] - sj2array[iij2]) <
7061 IKFAST_SOLUTION_THRESH)
7062 {
7063 j2valid[iij2] = false;
7064 _ij2[1] = iij2;
7065 break;
7066 }
7067 }
7068 {
7069 std::vector<IkSingleDOFSolutionBase<IkReal> >
7070 vinfos(7);
7071 vinfos[0].jointtype = 1;
7072 vinfos[0].foffset = j0;
7073 vinfos[0].indices[0] = _ij0[0];
7074 vinfos[0].indices[1] = _ij0[1];
7075 vinfos[0].maxsolutions = _nj0;
7076 vinfos[1].jointtype = 1;
7077 vinfos[1].foffset = j1;
7078 vinfos[1].indices[0] = _ij1[0];
7079 vinfos[1].indices[1] = _ij1[1];
7080 vinfos[1].maxsolutions = _nj1;
7081 vinfos[2].jointtype = 1;
7082 vinfos[2].foffset = j2;
7083 vinfos[2].indices[0] = _ij2[0];
7084 vinfos[2].indices[1] = _ij2[1];
7085 vinfos[2].maxsolutions = _nj2;
7086 vinfos[3].jointtype = 1;
7087 vinfos[3].foffset = j3;
7088 vinfos[3].indices[0] = _ij3[0];
7089 vinfos[3].indices[1] = _ij3[1];
7090 vinfos[3].maxsolutions = _nj3;
7091 vinfos[4].jointtype = 1;
7092 vinfos[4].foffset = j4;
7093 vinfos[4].indices[0] = _ij4[0];
7094 vinfos[4].indices[1] = _ij4[1];
7095 vinfos[4].maxsolutions = _nj4;
7096 vinfos[5].jointtype = 1;
7097 vinfos[5].foffset = j5;
7098 vinfos[5].indices[0] = _ij5[0];
7099 vinfos[5].indices[1] = _ij5[1];
7100 vinfos[5].maxsolutions = _nj5;
7101 vinfos[6].jointtype = 1;
7102 vinfos[6].foffset = j6;
7103 vinfos[6].indices[0] = _ij6[0];
7104 vinfos[6].indices[1] = _ij6[1];
7105 vinfos[6].maxsolutions = _nj6;
7106 std::vector<int> vfree(0);
7107 solutions.AddSolution(vinfos, vfree);
7108 }
7109 }
7110 }
7111 }
7112 }
7113 } while (0);
7114 if (bgotonextstatement)
7115 {
7116 bool bgotonextstatement = true;
7117 do
7118 {
7119 if (1)
7120 {
7121 bgotonextstatement = false;
7122 continue; // branch miss [j2]
7123 }
7124 } while (0);
7125 if (bgotonextstatement)
7126 {
7127 }
7128 }
7129 }
7130 }
7131 }
7132 }
7133 }
7134 }
7135 }
7136 }
7137 }
7138 }
7139 else
7140 {
7141 {
7142 IkReal j2array[1], cj2array[1], sj2array[1];
7143 bool j2valid[1] = { false };
7144 _nj2 = 1;
7145 CheckValue<IkReal> x267 = IKPowWithIntegerCheck(sj1, -1);
7146 if (!x267.valid)
7147 {
7148 continue;
7149 }
7150 IkReal x266 = x267.value;
7151 CheckValue<IkReal> x268 = IKPowWithIntegerCheck(cj1, -1);
7152 if (!x268.valid)
7153 {
7154 continue;
7155 }
7156 CheckValue<IkReal> x269 = IKPowWithIntegerCheck(sj0, -1);
7157 if (!x269.valid)
7158 {
7159 continue;
7160 }
7161 if (IKabs(((-1.0) * new_r21 * x266)) < IKFAST_ATAN2_MAGTHRESH &&
7162 IKabs((x266 * (x268.value) * (x269.value) *
7163 ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) <
7164 IKFAST_ATAN2_MAGTHRESH &&
7165 IKabs(IKsqr(((-1.0) * new_r21 * x266)) +
7166 IKsqr((x266 * (x268.value) * (x269.value) *
7167 ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) -
7168 1) <= IKFAST_SINCOS_THRESH)
7169 continue;
7170 j2array[0] = IKatan2(((-1.0) * new_r21 * x266),
7171 (x266 * (x268.value) * (x269.value) *
7172 ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1))))));
7173 sj2array[0] = IKsin(j2array[0]);
7174 cj2array[0] = IKcos(j2array[0]);
7175 if (j2array[0] > IKPI)
7176 {
7177 j2array[0] -= IK2PI;
7178 }
7179 else if (j2array[0] < -IKPI)
7180 {
7181 j2array[0] += IK2PI;
7182 }
7183 j2valid[0] = true;
7184 for (int ij2 = 0; ij2 < 1; ++ij2)
7185 {
7186 if (!j2valid[ij2])
7187 {
7188 continue;
7189 }
7190 _ij2[0] = ij2;
7191 _ij2[1] = -1;
7192 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7193 {
7194 if (j2valid[iij2] &&
7195 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7196 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7197 {
7198 j2valid[iij2] = false;
7199 _ij2[1] = iij2;
7200 break;
7201 }
7202 }
7203 j2 = j2array[ij2];
7204 cj2 = cj2array[ij2];
7205 sj2 = sj2array[ij2];
7206 {
7207 IkReal evalcond[12];
7208 IkReal x270 = IKsin(j2);
7209 IkReal x271 = IKcos(j2);
7210 IkReal x272 = ((1.0) * sj0);
7211 IkReal x273 = ((1.0) * sj1);
7212 IkReal x274 = (cj0 * new_r00);
7213 IkReal x275 = (cj1 * sj0);
7214 IkReal x276 = (cj0 * new_r01);
7215 IkReal x277 = (cj1 * x271);
7216 IkReal x278 = (cj1 * x270);
7217 evalcond[0] = (new_r21 + ((sj1 * x270)));
7218 evalcond[1] = ((((-1.0) * x271 * x273)) + new_r20);
7219 evalcond[2] = ((((-1.0) * new_r00 * x272)) + x270 + ((cj0 * new_r10)));
7220 evalcond[3] = ((((-1.0) * new_r01 * x272)) + x271 + ((cj0 * new_r11)));
7221 evalcond[4] = (((new_r10 * sj0)) + x277 + x274);
7222 evalcond[5] = (((x271 * x275)) + new_r10 + ((cj0 * x270)));
7223 evalcond[6] = ((((-1.0) * x278)) + ((new_r11 * sj0)) + x276);
7224 evalcond[7] = ((((-1.0) * x270 * x272)) + new_r00 + ((cj0 * x277)));
7225 evalcond[8] = ((((-1.0) * x272 * x278)) + new_r11 + ((cj0 * x271)));
7226 evalcond[9] =
7227 ((((-1.0) * cj0 * x278)) + (((-1.0) * x271 * x272)) + new_r01);
7228 evalcond[10] = (((cj1 * x274)) + x271 + (((-1.0) * new_r20 * x273)) +
7229 ((new_r10 * x275)));
7230 evalcond[11] = ((((-1.0) * x270)) + ((cj1 * x276)) +
7231 (((-1.0) * new_r21 * x273)) + ((new_r11 * x275)));
7232 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
7233 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
7234 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
7235 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
7236 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
7237 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
7238 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
7239 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
7240 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
7241 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
7242 IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
7243 IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
7244 {
7245 continue;
7246 }
7247 }
7248
7249 {
7250 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7251 vinfos[0].jointtype = 1;
7252 vinfos[0].foffset = j0;
7253 vinfos[0].indices[0] = _ij0[0];
7254 vinfos[0].indices[1] = _ij0[1];
7255 vinfos[0].maxsolutions = _nj0;
7256 vinfos[1].jointtype = 1;
7257 vinfos[1].foffset = j1;
7258 vinfos[1].indices[0] = _ij1[0];
7259 vinfos[1].indices[1] = _ij1[1];
7260 vinfos[1].maxsolutions = _nj1;
7261 vinfos[2].jointtype = 1;
7262 vinfos[2].foffset = j2;
7263 vinfos[2].indices[0] = _ij2[0];
7264 vinfos[2].indices[1] = _ij2[1];
7265 vinfos[2].maxsolutions = _nj2;
7266 vinfos[3].jointtype = 1;
7267 vinfos[3].foffset = j3;
7268 vinfos[3].indices[0] = _ij3[0];
7269 vinfos[3].indices[1] = _ij3[1];
7270 vinfos[3].maxsolutions = _nj3;
7271 vinfos[4].jointtype = 1;
7272 vinfos[4].foffset = j4;
7273 vinfos[4].indices[0] = _ij4[0];
7274 vinfos[4].indices[1] = _ij4[1];
7275 vinfos[4].maxsolutions = _nj4;
7276 vinfos[5].jointtype = 1;
7277 vinfos[5].foffset = j5;
7278 vinfos[5].indices[0] = _ij5[0];
7279 vinfos[5].indices[1] = _ij5[1];
7280 vinfos[5].maxsolutions = _nj5;
7281 vinfos[6].jointtype = 1;
7282 vinfos[6].foffset = j6;
7283 vinfos[6].indices[0] = _ij6[0];
7284 vinfos[6].indices[1] = _ij6[1];
7285 vinfos[6].maxsolutions = _nj6;
7286 std::vector<int> vfree(0);
7287 solutions.AddSolution(vinfos, vfree);
7288 }
7289 }
7290 }
7291 }
7292 }
7293 }
7294 else
7295 {
7296 {
7297 IkReal j2array[1], cj2array[1], sj2array[1];
7298 bool j2valid[1] = { false };
7299 _nj2 = 1;
7300 CheckValue<IkReal> x280 = IKPowWithIntegerCheck(sj1, -1);
7301 if (!x280.valid)
7302 {
7303 continue;
7304 }
7305 IkReal x279 = x280.value;
7306 CheckValue<IkReal> x281 = IKPowWithIntegerCheck(cj0, -1);
7307 if (!x281.valid)
7308 {
7309 continue;
7310 }
7311 if (IKabs(((-1.0) * new_r21 * x279)) < IKFAST_ATAN2_MAGTHRESH &&
7312 IKabs((x279 * (x281.value) *
7313 (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) <
7314 IKFAST_ATAN2_MAGTHRESH &&
7315 IKabs(
7316 IKsqr(((-1.0) * new_r21 * x279)) +
7317 IKsqr((x279 * (x281.value) *
7318 (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) -
7319 1) <= IKFAST_SINCOS_THRESH)
7320 continue;
7321 j2array[0] =
7322 IKatan2(((-1.0) * new_r21 * x279),
7323 (x279 * (x281.value) *
7324 (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1))))));
7325 sj2array[0] = IKsin(j2array[0]);
7326 cj2array[0] = IKcos(j2array[0]);
7327 if (j2array[0] > IKPI)
7328 {
7329 j2array[0] -= IK2PI;
7330 }
7331 else if (j2array[0] < -IKPI)
7332 {
7333 j2array[0] += IK2PI;
7334 }
7335 j2valid[0] = true;
7336 for (int ij2 = 0; ij2 < 1; ++ij2)
7337 {
7338 if (!j2valid[ij2])
7339 {
7340 continue;
7341 }
7342 _ij2[0] = ij2;
7343 _ij2[1] = -1;
7344 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7345 {
7346 if (j2valid[iij2] &&
7347 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7348 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7349 {
7350 j2valid[iij2] = false;
7351 _ij2[1] = iij2;
7352 break;
7353 }
7354 }
7355 j2 = j2array[ij2];
7356 cj2 = cj2array[ij2];
7357 sj2 = sj2array[ij2];
7358 {
7359 IkReal evalcond[12];
7360 IkReal x282 = IKsin(j2);
7361 IkReal x283 = IKcos(j2);
7362 IkReal x284 = ((1.0) * sj0);
7363 IkReal x285 = ((1.0) * sj1);
7364 IkReal x286 = (cj0 * new_r00);
7365 IkReal x287 = (cj1 * sj0);
7366 IkReal x288 = (cj0 * new_r01);
7367 IkReal x289 = (cj1 * x283);
7368 IkReal x290 = (cj1 * x282);
7369 evalcond[0] = (new_r21 + ((sj1 * x282)));
7370 evalcond[1] = (new_r20 + (((-1.0) * x283 * x285)));
7371 evalcond[2] = ((((-1.0) * new_r00 * x284)) + x282 + ((cj0 * new_r10)));
7372 evalcond[3] = (x283 + (((-1.0) * new_r01 * x284)) + ((cj0 * new_r11)));
7373 evalcond[4] = (((new_r10 * sj0)) + x289 + x286);
7374 evalcond[5] = (((cj0 * x282)) + new_r10 + ((x283 * x287)));
7375 evalcond[6] = ((((-1.0) * x290)) + ((new_r11 * sj0)) + x288);
7376 evalcond[7] = (((cj0 * x289)) + (((-1.0) * x282 * x284)) + new_r00);
7377 evalcond[8] = ((((-1.0) * x284 * x290)) + ((cj0 * x283)) + new_r11);
7378 evalcond[9] = ((((-1.0) * cj0 * x290)) + new_r01 + (((-1.0) * x283 * x284)));
7379 evalcond[10] =
7380 (x283 + (((-1.0) * new_r20 * x285)) + ((cj1 * x286)) + ((new_r10 * x287)));
7381 evalcond[11] = ((((-1.0) * x282)) + (((-1.0) * new_r21 * x285)) +
7382 ((new_r11 * x287)) + ((cj1 * x288)));
7383 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
7384 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
7385 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
7386 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
7387 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
7388 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
7389 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
7390 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
7391 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
7392 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
7393 IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
7394 IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
7395 {
7396 continue;
7397 }
7398 }
7399
7400 {
7401 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7402 vinfos[0].jointtype = 1;
7403 vinfos[0].foffset = j0;
7404 vinfos[0].indices[0] = _ij0[0];
7405 vinfos[0].indices[1] = _ij0[1];
7406 vinfos[0].maxsolutions = _nj0;
7407 vinfos[1].jointtype = 1;
7408 vinfos[1].foffset = j1;
7409 vinfos[1].indices[0] = _ij1[0];
7410 vinfos[1].indices[1] = _ij1[1];
7411 vinfos[1].maxsolutions = _nj1;
7412 vinfos[2].jointtype = 1;
7413 vinfos[2].foffset = j2;
7414 vinfos[2].indices[0] = _ij2[0];
7415 vinfos[2].indices[1] = _ij2[1];
7416 vinfos[2].maxsolutions = _nj2;
7417 vinfos[3].jointtype = 1;
7418 vinfos[3].foffset = j3;
7419 vinfos[3].indices[0] = _ij3[0];
7420 vinfos[3].indices[1] = _ij3[1];
7421 vinfos[3].maxsolutions = _nj3;
7422 vinfos[4].jointtype = 1;
7423 vinfos[4].foffset = j4;
7424 vinfos[4].indices[0] = _ij4[0];
7425 vinfos[4].indices[1] = _ij4[1];
7426 vinfos[4].maxsolutions = _nj4;
7427 vinfos[5].jointtype = 1;
7428 vinfos[5].foffset = j5;
7429 vinfos[5].indices[0] = _ij5[0];
7430 vinfos[5].indices[1] = _ij5[1];
7431 vinfos[5].maxsolutions = _nj5;
7432 vinfos[6].jointtype = 1;
7433 vinfos[6].foffset = j6;
7434 vinfos[6].indices[0] = _ij6[0];
7435 vinfos[6].indices[1] = _ij6[1];
7436 vinfos[6].maxsolutions = _nj6;
7437 std::vector<int> vfree(0);
7438 solutions.AddSolution(vinfos, vfree);
7439 }
7440 }
7441 }
7442 }
7443 }
7444 }
7445 else
7446 {
7447 {
7448 IkReal j2array[1], cj2array[1], sj2array[1];
7449 bool j2valid[1] = { false };
7450 _nj2 = 1;
7451 CheckValue<IkReal> x291 = IKatan2WithCheck(
7452 IkReal(((-1.0) * new_r21)), IkReal(new_r20), IKFAST_ATAN2_MAGTHRESH);
7453 if (!x291.valid)
7454 {
7455 continue;
7456 }
7457 CheckValue<IkReal> x292 = IKPowWithIntegerCheck(IKsign(sj1), -1);
7458 if (!x292.valid)
7459 {
7460 continue;
7461 }
7462 j2array[0] = ((-1.5707963267949) + (x291.value) + (((1.5707963267949) * (x292.value))));
7463 sj2array[0] = IKsin(j2array[0]);
7464 cj2array[0] = IKcos(j2array[0]);
7465 if (j2array[0] > IKPI)
7466 {
7467 j2array[0] -= IK2PI;
7468 }
7469 else if (j2array[0] < -IKPI)
7470 {
7471 j2array[0] += IK2PI;
7472 }
7473 j2valid[0] = true;
7474 for (int ij2 = 0; ij2 < 1; ++ij2)
7475 {
7476 if (!j2valid[ij2])
7477 {
7478 continue;
7479 }
7480 _ij2[0] = ij2;
7481 _ij2[1] = -1;
7482 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7483 {
7484 if (j2valid[iij2] &&
7485 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7486 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7487 {
7488 j2valid[iij2] = false;
7489 _ij2[1] = iij2;
7490 break;
7491 }
7492 }
7493 j2 = j2array[ij2];
7494 cj2 = cj2array[ij2];
7495 sj2 = sj2array[ij2];
7496 {
7497 IkReal evalcond[12];
7498 IkReal x293 = IKsin(j2);
7499 IkReal x294 = IKcos(j2);
7500 IkReal x295 = ((1.0) * sj0);
7501 IkReal x296 = ((1.0) * sj1);
7502 IkReal x297 = (cj0 * new_r00);
7503 IkReal x298 = (cj1 * sj0);
7504 IkReal x299 = (cj0 * new_r01);
7505 IkReal x300 = (cj1 * x294);
7506 IkReal x301 = (cj1 * x293);
7507 evalcond[0] = (((sj1 * x293)) + new_r21);
7508 evalcond[1] = (new_r20 + (((-1.0) * x294 * x296)));
7509 evalcond[2] = (x293 + ((cj0 * new_r10)) + (((-1.0) * new_r00 * x295)));
7510 evalcond[3] = ((((-1.0) * new_r01 * x295)) + x294 + ((cj0 * new_r11)));
7511 evalcond[4] = (((new_r10 * sj0)) + x300 + x297);
7512 evalcond[5] = (((x294 * x298)) + new_r10 + ((cj0 * x293)));
7513 evalcond[6] = (((new_r11 * sj0)) + (((-1.0) * x301)) + x299);
7514 evalcond[7] = ((((-1.0) * x293 * x295)) + ((cj0 * x300)) + new_r00);
7515 evalcond[8] = ((((-1.0) * x295 * x301)) + new_r11 + ((cj0 * x294)));
7516 evalcond[9] = ((((-1.0) * cj0 * x301)) + new_r01 + (((-1.0) * x294 * x295)));
7517 evalcond[10] =
7518 ((((-1.0) * new_r20 * x296)) + ((cj1 * x297)) + ((new_r10 * x298)) + x294);
7519 evalcond[11] = ((((-1.0) * x293)) + (((-1.0) * new_r21 * x296)) +
7520 ((new_r11 * x298)) + ((cj1 * x299)));
7521 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
7522 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
7523 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
7524 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
7525 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
7526 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
7527 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
7528 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
7529 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
7530 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
7531 IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
7532 IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
7533 {
7534 continue;
7535 }
7536 }
7537
7538 {
7539 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
7540 vinfos[0].jointtype = 1;
7541 vinfos[0].foffset = j0;
7542 vinfos[0].indices[0] = _ij0[0];
7543 vinfos[0].indices[1] = _ij0[1];
7544 vinfos[0].maxsolutions = _nj0;
7545 vinfos[1].jointtype = 1;
7546 vinfos[1].foffset = j1;
7547 vinfos[1].indices[0] = _ij1[0];
7548 vinfos[1].indices[1] = _ij1[1];
7549 vinfos[1].maxsolutions = _nj1;
7550 vinfos[2].jointtype = 1;
7551 vinfos[2].foffset = j2;
7552 vinfos[2].indices[0] = _ij2[0];
7553 vinfos[2].indices[1] = _ij2[1];
7554 vinfos[2].maxsolutions = _nj2;
7555 vinfos[3].jointtype = 1;
7556 vinfos[3].foffset = j3;
7557 vinfos[3].indices[0] = _ij3[0];
7558 vinfos[3].indices[1] = _ij3[1];
7559 vinfos[3].maxsolutions = _nj3;
7560 vinfos[4].jointtype = 1;
7561 vinfos[4].foffset = j4;
7562 vinfos[4].indices[0] = _ij4[0];
7563 vinfos[4].indices[1] = _ij4[1];
7564 vinfos[4].maxsolutions = _nj4;
7565 vinfos[5].jointtype = 1;
7566 vinfos[5].foffset = j5;
7567 vinfos[5].indices[0] = _ij5[0];
7568 vinfos[5].indices[1] = _ij5[1];
7569 vinfos[5].maxsolutions = _nj5;
7570 vinfos[6].jointtype = 1;
7571 vinfos[6].foffset = j6;
7572 vinfos[6].indices[0] = _ij6[0];
7573 vinfos[6].indices[1] = _ij6[1];
7574 vinfos[6].maxsolutions = _nj6;
7575 std::vector<int> vfree(0);
7576 solutions.AddSolution(vinfos, vfree);
7577 }
7578 }
7579 }
7580 }
7581 }
7582 }
7583 }
7584 }
7585 }
7586 }
7587 else
7588 {
7589 {
7590 IkReal j2array[1], cj2array[1], sj2array[1];
7591 bool j2valid[1] = { false };
7592 _nj2 = 1;
7593 CheckValue<IkReal> x302 =
7594 IKatan2WithCheck(IkReal(((-1.0) * new_r21)), IkReal(new_r20), IKFAST_ATAN2_MAGTHRESH);
7595 if (!x302.valid)
7596 {
7597 continue;
7598 }
7599 CheckValue<IkReal> x303 = IKPowWithIntegerCheck(IKsign(sj1), -1);
7600 if (!x303.valid)
7601 {
7602 continue;
7603 }
7604 j2array[0] = ((-1.5707963267949) + (x302.value) + (((1.5707963267949) * (x303.value))));
7605 sj2array[0] = IKsin(j2array[0]);
7606 cj2array[0] = IKcos(j2array[0]);
7607 if (j2array[0] > IKPI)
7608 {
7609 j2array[0] -= IK2PI;
7610 }
7611 else if (j2array[0] < -IKPI)
7612 {
7613 j2array[0] += IK2PI;
7614 }
7615 j2valid[0] = true;
7616 for (int ij2 = 0; ij2 < 1; ++ij2)
7617 {
7618 if (!j2valid[ij2])
7619 {
7620 continue;
7621 }
7622 _ij2[0] = ij2;
7623 _ij2[1] = -1;
7624 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
7625 {
7626 if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
7627 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
7628 {
7629 j2valid[iij2] = false;
7630 _ij2[1] = iij2;
7631 break;
7632 }
7633 }
7634 j2 = j2array[ij2];
7635 cj2 = cj2array[ij2];
7636 sj2 = sj2array[ij2];
7637 {
7638 IkReal evalcond[2];
7639 evalcond[0] = (((sj1 * (IKsin(j2)))) + new_r21);
7640 evalcond[1] = ((((-1.0) * sj1 * (IKcos(j2)))) + new_r20);
7641 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
7642 {
7643 continue;
7644 }
7645 }
7646
7647 {
7648 IkReal j0eval[3];
7649 j0eval[0] = sj1;
7650 j0eval[1] = ((IKabs(new_r12)) + (IKabs(new_r02)));
7651 j0eval[2] = IKsign(sj1);
7652 if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000 ||
7653 IKabs(j0eval[2]) < 0.0000010000000000)
7654 {
7655 {
7656 IkReal j0eval[2];
7657 j0eval[0] = new_r00;
7658 j0eval[1] = sj1;
7659 if (IKabs(j0eval[0]) < 0.0000010000000000 || IKabs(j0eval[1]) < 0.0000010000000000)
7660 {
7661 {
7662 IkReal evalcond[5];
7663 bool bgotonextstatement = true;
7664 do
7665 {
7666 evalcond[0] = ((-3.14159265358979) +
7667 (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
7668 evalcond[1] = new_r21;
7669 evalcond[2] = new_r02;
7670 evalcond[3] = new_r12;
7671 evalcond[4] = new_r20;
7672 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
7673 IKabs(evalcond[1]) < 0.0000050000000000 &&
7674 IKabs(evalcond[2]) < 0.0000050000000000 &&
7675 IKabs(evalcond[3]) < 0.0000050000000000 &&
7676 IKabs(evalcond[4]) < 0.0000050000000000)
7677 {
7678 bgotonextstatement = false;
7679 {
7680 IkReal j0eval[3];
7681 sj1 = 0;
7682 cj1 = 1.0;
7683 j1 = 0;
7684 IkReal x304 = ((1.0) * cj2);
7685 IkReal x305 = ((new_r10 * new_r10) + (new_r00 * new_r00));
7686 j0eval[0] = x305;
7687 j0eval[1] = ((IKabs((((new_r00 * sj2)) + (((-1.0) * new_r10 * x304))))) +
7688 (IKabs(((((-1.0) * new_r10 * sj2)) + (((-1.0) * new_r00 * x304))))));
7689 j0eval[2] = IKsign(x305);
7690 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7691 IKabs(j0eval[1]) < 0.0000010000000000 ||
7692 IKabs(j0eval[2]) < 0.0000010000000000)
7693 {
7694 {
7695 IkReal j0eval[3];
7696 sj1 = 0;
7697 cj1 = 1.0;
7698 j1 = 0;
7699 IkReal x306 = ((1.0) * cj2);
7700 IkReal x307 = (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
7701 j0eval[0] = x307;
7702 j0eval[1] =
7703 ((IKabs(((((-1.0) * new_r01 * x306)) + (((-1.0) * new_r10 * x306))))) +
7704 (IKabs((((cj2 * new_r00)) + (((-1.0) * new_r11 * x306))))));
7705 j0eval[2] = IKsign(x307);
7706 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7707 IKabs(j0eval[1]) < 0.0000010000000000 ||
7708 IKabs(j0eval[2]) < 0.0000010000000000)
7709 {
7710 {
7711 IkReal j0eval[3];
7712 sj1 = 0;
7713 cj1 = 1.0;
7714 j1 = 0;
7715 IkReal x308 = ((1.0) * new_r10);
7716 IkReal x309 = ((((-1.0) * sj2 * x308)) + ((cj2 * new_r00)));
7717 j0eval[0] = x309;
7718 j0eval[1] = IKsign(x309);
7719 j0eval[2] = ((IKabs(((((-1.0) * (cj2 * cj2))) + (new_r10 * new_r10)))) +
7720 (IKabs((((cj2 * sj2)) + (((-1.0) * new_r00 * x308))))));
7721 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7722 IKabs(j0eval[1]) < 0.0000010000000000 ||
7723 IKabs(j0eval[2]) < 0.0000010000000000)
7724 {
7725 {
7726 IkReal evalcond[1];
7727 bool bgotonextstatement = true;
7728 do
7729 {
7730 IkReal x312 = ((new_r10 * new_r10) + (new_r00 * new_r00));
7731 if (IKabs(x312) == 0)
7732 {
7733 continue;
7734 }
7735 IkReal x310 = pow(x312, -0.5);
7736 IkReal x311 = ((-1.0) * x310);
7737 CheckValue<IkReal> x313 =
7738 IKatan2WithCheck(IkReal(new_r00),
7739 IkReal(((-1.0) * new_r10)),
7740 IKFAST_ATAN2_MAGTHRESH);
7741 if (!x313.valid)
7742 {
7743 continue;
7744 }
7745 IkReal gconst0 = ((-1.0) * (x313.value));
7746 IkReal gconst1 = (new_r00 * x311);
7747 IkReal gconst2 = (new_r10 * x311);
7748 CheckValue<IkReal> x314 =
7749 IKatan2WithCheck(IkReal(new_r00),
7750 IkReal(((-1.0) * new_r10)),
7751 IKFAST_ATAN2_MAGTHRESH);
7752 if (!x314.valid)
7753 {
7754 continue;
7755 }
7756 evalcond[0] =
7757 ((-3.14159265358979) +
7758 (IKfmod(((3.14159265358979) + (IKabs(((x314.value) + j2)))),
7759 6.28318530717959)));
7760 if (IKabs(evalcond[0]) < 0.0000050000000000)
7761 {
7762 bgotonextstatement = false;
7763 {
7764 IkReal j0eval[2];
7765 CheckValue<IkReal> x318 =
7766 IKatan2WithCheck(IkReal(new_r00),
7767 IkReal(((-1.0) * new_r10)),
7768 IKFAST_ATAN2_MAGTHRESH);
7769 if (!x318.valid)
7770 {
7771 continue;
7772 }
7773 IkReal x315 = ((-1.0) * (x318.value));
7774 IkReal x316 = x310;
7775 IkReal x317 = ((-1.0) * x316);
7776 sj1 = 0;
7777 cj1 = 1.0;
7778 j1 = 0;
7779 sj2 = gconst1;
7780 cj2 = gconst2;
7781 j2 = x315;
7782 IkReal gconst0 = x315;
7783 IkReal gconst1 = (new_r00 * x317);
7784 IkReal gconst2 = (new_r10 * x317);
7785 IkReal x319 = ((new_r10 * new_r10) + (new_r00 * new_r00));
7786 j0eval[0] = x319;
7787 j0eval[1] = IKsign(x319);
7788 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7789 IKabs(j0eval[1]) < 0.0000010000000000)
7790 {
7791 {
7792 IkReal j0eval[3];
7793 CheckValue<IkReal> x323 =
7794 IKatan2WithCheck(IkReal(new_r00),
7795 IkReal(((-1.0) * new_r10)),
7796 IKFAST_ATAN2_MAGTHRESH);
7797 if (!x323.valid)
7798 {
7799 continue;
7800 }
7801 IkReal x320 = ((-1.0) * (x323.value));
7802 IkReal x321 = x310;
7803 IkReal x322 = ((-1.0) * x321);
7804 sj1 = 0;
7805 cj1 = 1.0;
7806 j1 = 0;
7807 sj2 = gconst1;
7808 cj2 = gconst2;
7809 j2 = x320;
7810 IkReal gconst0 = x320;
7811 IkReal gconst1 = (new_r00 * x322);
7812 IkReal gconst2 = (new_r10 * x322);
7813 IkReal x324 = new_r10 * new_r10;
7814 IkReal x325 =
7815 (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
7816 IkReal x326 = x310;
7817 IkReal x327 = (new_r10 * x326);
7818 j0eval[0] = x325;
7819 j0eval[1] = IKsign(x325);
7820 j0eval[2] =
7821 ((IKabs(((((-1.0) * new_r00 * x327)) +
7822 ((new_r11 * x327))))) +
7823 (IKabs((((new_r01 * x327)) + ((x324 * x326))))));
7824 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
7825 IKabs(j0eval[1]) < 0.0000010000000000 ||
7826 IKabs(j0eval[2]) < 0.0000010000000000)
7827 {
7828 {
7829 IkReal j0eval[1];
7830 CheckValue<IkReal> x331 =
7831 IKatan2WithCheck(IkReal(new_r00),
7832 IkReal(((-1.0) * new_r10)),
7833 IKFAST_ATAN2_MAGTHRESH);
7834 if (!x331.valid)
7835 {
7836 continue;
7837 }
7838 IkReal x328 = ((-1.0) * (x331.value));
7839 IkReal x329 = x310;
7840 IkReal x330 = ((-1.0) * x329);
7841 sj1 = 0;
7842 cj1 = 1.0;
7843 j1 = 0;
7844 sj2 = gconst1;
7845 cj2 = gconst2;
7846 j2 = x328;
7847 IkReal gconst0 = x328;
7848 IkReal gconst1 = (new_r00 * x330);
7849 IkReal gconst2 = (new_r10 * x330);
7850 IkReal x332 = new_r10 * new_r10;
7851 IkReal x333 = new_r00 * new_r00;
7852 CheckValue<IkReal> x340 =
7853 IKPowWithIntegerCheck((x333 + x332), -1);
7854 if (!x340.valid)
7855 {
7856 continue;
7857 }
7858 IkReal x334 = x340.value;
7859 IkReal x335 = (x332 * x334);
7860 CheckValue<IkReal> x341 = IKPowWithIntegerCheck(
7861 ((((-1.0) * x332)) + (((-1.0) * x333))), -1);
7862 if (!x341.valid)
7863 {
7864 continue;
7865 }
7866 IkReal x336 = x341.value;
7867 IkReal x337 = ((1.0) * x336);
7868 IkReal x338 = (new_r00 * x337);
7869 IkReal x339 = (new_r10 * x337);
7870 j0eval[0] =
7871 ((IKabs(((((-1.0) * new_r10 * x338)) +
7872 (((-1.0) * x338 *
7873 (new_r10 * new_r10 * new_r10))) +
7874 (((-1.0) * new_r10 * x338 *
7875 (new_r00 * new_r00)))))) +
7876 (IKabs((((x334 * (x333 * x333))) +
7877 (((-1.0) * x335)) + ((x333 * x335))))));
7878 if (IKabs(j0eval[0]) < 0.0000010000000000)
7879 {
7880 continue; // no branches [j0]
7881 }
7882 else
7883 {
7884 {
7885 IkReal j0array[1], cj0array[1], sj0array[1];
7886 bool j0valid[1] = { false };
7887 _nj0 = 1;
7888 CheckValue<IkReal> x342 = IKatan2WithCheck(
7889 IkReal(((((-1.0) * (gconst2 * gconst2))) +
7890 (new_r00 * new_r00))),
7891 IkReal(((((-1.0) * gconst1 * gconst2)) +
7892 (((-1.0) * new_r00 * new_r10)))),
7893 IKFAST_ATAN2_MAGTHRESH);
7894 if (!x342.valid)
7895 {
7896 continue;
7897 }
7898 CheckValue<IkReal> x343 = IKPowWithIntegerCheck(
7899 IKsign((((gconst2 * new_r10)) +
7900 ((gconst1 * new_r00)))),
7901 -1);
7902 if (!x343.valid)
7903 {
7904 continue;
7905 }
7906 j0array[0] = ((-1.5707963267949) + (x342.value) +
7907 (((1.5707963267949) * (x343.value))));
7908 sj0array[0] = IKsin(j0array[0]);
7909 cj0array[0] = IKcos(j0array[0]);
7910 if (j0array[0] > IKPI)
7911 {
7912 j0array[0] -= IK2PI;
7913 }
7914 else if (j0array[0] < -IKPI)
7915 {
7916 j0array[0] += IK2PI;
7917 }
7918 j0valid[0] = true;
7919 for (int ij0 = 0; ij0 < 1; ++ij0)
7920 {
7921 if (!j0valid[ij0])
7922 {
7923 continue;
7924 }
7925 _ij0[0] = ij0;
7926 _ij0[1] = -1;
7927 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
7928 {
7929 if (j0valid[iij0] &&
7930 IKabs(cj0array[ij0] - cj0array[iij0]) <
7931 IKFAST_SOLUTION_THRESH &&
7932 IKabs(sj0array[ij0] - sj0array[iij0]) <
7933 IKFAST_SOLUTION_THRESH)
7934 {
7935 j0valid[iij0] = false;
7936 _ij0[1] = iij0;
7937 break;
7938 }
7939 }
7940 j0 = j0array[ij0];
7941 cj0 = cj0array[ij0];
7942 sj0 = sj0array[ij0];
7943 {
7944 IkReal evalcond[8];
7945 IkReal x344 = IKcos(j0);
7946 IkReal x345 = IKsin(j0);
7947 IkReal x346 = ((1.0) * gconst1);
7948 IkReal x347 = (gconst2 * x344);
7949 IkReal x348 = (gconst1 * x344);
7950 IkReal x349 = (gconst2 * x345);
7951 IkReal x350 = ((1.0) * x345);
7952 IkReal x351 = (x345 * x346);
7953 evalcond[0] = (gconst2 + ((new_r00 * x344)) +
7954 ((new_r10 * x345)));
7955 evalcond[1] = (x348 + x349 + new_r10);
7956 evalcond[2] = (gconst1 + ((new_r10 * x344)) +
7957 (((-1.0) * new_r00 * x350)));
7958 evalcond[3] = (gconst2 + ((new_r11 * x344)) +
7959 (((-1.0) * new_r01 * x350)));
7960 evalcond[4] =
7961 ((((-1.0) * x351)) + x347 + new_r00);
7962 evalcond[5] =
7963 ((((-1.0) * x351)) + x347 + new_r11);
7964 evalcond[6] =
7965 (((new_r11 * x345)) + ((new_r01 * x344)) +
7966 (((-1.0) * x346)));
7967 evalcond[7] = ((((-1.0) * x344 * x346)) +
7968 new_r01 + (((-1.0) * x349)));
7969 if (IKabs(evalcond[0]) >
7970 IKFAST_EVALCOND_THRESH ||
7971 IKabs(evalcond[1]) >
7972 IKFAST_EVALCOND_THRESH ||
7973 IKabs(evalcond[2]) >
7974 IKFAST_EVALCOND_THRESH ||
7975 IKabs(evalcond[3]) >
7976 IKFAST_EVALCOND_THRESH ||
7977 IKabs(evalcond[4]) >
7978 IKFAST_EVALCOND_THRESH ||
7979 IKabs(evalcond[5]) >
7980 IKFAST_EVALCOND_THRESH ||
7981 IKabs(evalcond[6]) >
7982 IKFAST_EVALCOND_THRESH ||
7983 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
7984 {
7985 continue;
7986 }
7987 }
7988
7989 {
7990 std::vector<IkSingleDOFSolutionBase<IkReal> >
7991 vinfos(7);
7992 vinfos[0].jointtype = 1;
7993 vinfos[0].foffset = j0;
7994 vinfos[0].indices[0] = _ij0[0];
7995 vinfos[0].indices[1] = _ij0[1];
7996 vinfos[0].maxsolutions = _nj0;
7997 vinfos[1].jointtype = 1;
7998 vinfos[1].foffset = j1;
7999 vinfos[1].indices[0] = _ij1[0];
8000 vinfos[1].indices[1] = _ij1[1];
8001 vinfos[1].maxsolutions = _nj1;
8002 vinfos[2].jointtype = 1;
8003 vinfos[2].foffset = j2;
8004 vinfos[2].indices[0] = _ij2[0];
8005 vinfos[2].indices[1] = _ij2[1];
8006 vinfos[2].maxsolutions = _nj2;
8007 vinfos[3].jointtype = 1;
8008 vinfos[3].foffset = j3;
8009 vinfos[3].indices[0] = _ij3[0];
8010 vinfos[3].indices[1] = _ij3[1];
8011 vinfos[3].maxsolutions = _nj3;
8012 vinfos[4].jointtype = 1;
8013 vinfos[4].foffset = j4;
8014 vinfos[4].indices[0] = _ij4[0];
8015 vinfos[4].indices[1] = _ij4[1];
8016 vinfos[4].maxsolutions = _nj4;
8017 vinfos[5].jointtype = 1;
8018 vinfos[5].foffset = j5;
8019 vinfos[5].indices[0] = _ij5[0];
8020 vinfos[5].indices[1] = _ij5[1];
8021 vinfos[5].maxsolutions = _nj5;
8022 vinfos[6].jointtype = 1;
8023 vinfos[6].foffset = j6;
8024 vinfos[6].indices[0] = _ij6[0];
8025 vinfos[6].indices[1] = _ij6[1];
8026 vinfos[6].maxsolutions = _nj6;
8027 std::vector<int> vfree(0);
8028 solutions.AddSolution(vinfos, vfree);
8029 }
8030 }
8031 }
8032 }
8033 }
8034 }
8035 else
8036 {
8037 {
8038 IkReal j0array[1], cj0array[1], sj0array[1];
8039 bool j0valid[1] = { false };
8040 _nj0 = 1;
8041 IkReal x352 = ((1.0) * gconst2);
8042 CheckValue<IkReal> x353 = IKatan2WithCheck(
8043 IkReal((((gconst2 * new_r00)) +
8044 (((-1.0) * new_r11 * x352)))),
8045 IkReal(((((-1.0) * new_r01 * x352)) +
8046 (((-1.0) * new_r10 * x352)))),
8047 IKFAST_ATAN2_MAGTHRESH);
8048 if (!x353.valid)
8049 {
8050 continue;
8051 }
8052 CheckValue<IkReal> x354 = IKPowWithIntegerCheck(
8053 IKsign((((new_r10 * new_r11)) +
8054 ((new_r00 * new_r01)))),
8055 -1);
8056 if (!x354.valid)
8057 {
8058 continue;
8059 }
8060 j0array[0] = ((-1.5707963267949) + (x353.value) +
8061 (((1.5707963267949) * (x354.value))));
8062 sj0array[0] = IKsin(j0array[0]);
8063 cj0array[0] = IKcos(j0array[0]);
8064 if (j0array[0] > IKPI)
8065 {
8066 j0array[0] -= IK2PI;
8067 }
8068 else if (j0array[0] < -IKPI)
8069 {
8070 j0array[0] += IK2PI;
8071 }
8072 j0valid[0] = true;
8073 for (int ij0 = 0; ij0 < 1; ++ij0)
8074 {
8075 if (!j0valid[ij0])
8076 {
8077 continue;
8078 }
8079 _ij0[0] = ij0;
8080 _ij0[1] = -1;
8081 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8082 {
8083 if (j0valid[iij0] &&
8084 IKabs(cj0array[ij0] - cj0array[iij0]) <
8085 IKFAST_SOLUTION_THRESH &&
8086 IKabs(sj0array[ij0] - sj0array[iij0]) <
8087 IKFAST_SOLUTION_THRESH)
8088 {
8089 j0valid[iij0] = false;
8090 _ij0[1] = iij0;
8091 break;
8092 }
8093 }
8094 j0 = j0array[ij0];
8095 cj0 = cj0array[ij0];
8096 sj0 = sj0array[ij0];
8097 {
8098 IkReal evalcond[8];
8099 IkReal x355 = IKcos(j0);
8100 IkReal x356 = IKsin(j0);
8101 IkReal x357 = ((1.0) * gconst1);
8102 IkReal x358 = (gconst2 * x355);
8103 IkReal x359 = (gconst1 * x355);
8104 IkReal x360 = (gconst2 * x356);
8105 IkReal x361 = ((1.0) * x356);
8106 IkReal x362 = (x356 * x357);
8107 evalcond[0] = (gconst2 + ((new_r10 * x356)) +
8108 ((new_r00 * x355)));
8109 evalcond[1] = (x359 + x360 + new_r10);
8110 evalcond[2] = ((((-1.0) * new_r00 * x361)) +
8111 gconst1 + ((new_r10 * x355)));
8112 evalcond[3] = (gconst2 + ((new_r11 * x355)) +
8113 (((-1.0) * new_r01 * x361)));
8114 evalcond[4] = ((((-1.0) * x362)) + x358 + new_r00);
8115 evalcond[5] = ((((-1.0) * x362)) + x358 + new_r11);
8116 evalcond[6] =
8117 (((new_r01 * x355)) + (((-1.0) * x357)) +
8118 ((new_r11 * x356)));
8119 evalcond[7] = ((((-1.0) * x360)) + new_r01 +
8120 (((-1.0) * x355 * x357)));
8121 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8122 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8123 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8124 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8125 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8126 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8127 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8128 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8129 {
8130 continue;
8131 }
8132 }
8133
8134 {
8135 std::vector<IkSingleDOFSolutionBase<IkReal> >
8136 vinfos(7);
8137 vinfos[0].jointtype = 1;
8138 vinfos[0].foffset = j0;
8139 vinfos[0].indices[0] = _ij0[0];
8140 vinfos[0].indices[1] = _ij0[1];
8141 vinfos[0].maxsolutions = _nj0;
8142 vinfos[1].jointtype = 1;
8143 vinfos[1].foffset = j1;
8144 vinfos[1].indices[0] = _ij1[0];
8145 vinfos[1].indices[1] = _ij1[1];
8146 vinfos[1].maxsolutions = _nj1;
8147 vinfos[2].jointtype = 1;
8148 vinfos[2].foffset = j2;
8149 vinfos[2].indices[0] = _ij2[0];
8150 vinfos[2].indices[1] = _ij2[1];
8151 vinfos[2].maxsolutions = _nj2;
8152 vinfos[3].jointtype = 1;
8153 vinfos[3].foffset = j3;
8154 vinfos[3].indices[0] = _ij3[0];
8155 vinfos[3].indices[1] = _ij3[1];
8156 vinfos[3].maxsolutions = _nj3;
8157 vinfos[4].jointtype = 1;
8158 vinfos[4].foffset = j4;
8159 vinfos[4].indices[0] = _ij4[0];
8160 vinfos[4].indices[1] = _ij4[1];
8161 vinfos[4].maxsolutions = _nj4;
8162 vinfos[5].jointtype = 1;
8163 vinfos[5].foffset = j5;
8164 vinfos[5].indices[0] = _ij5[0];
8165 vinfos[5].indices[1] = _ij5[1];
8166 vinfos[5].maxsolutions = _nj5;
8167 vinfos[6].jointtype = 1;
8168 vinfos[6].foffset = j6;
8169 vinfos[6].indices[0] = _ij6[0];
8170 vinfos[6].indices[1] = _ij6[1];
8171 vinfos[6].maxsolutions = _nj6;
8172 std::vector<int> vfree(0);
8173 solutions.AddSolution(vinfos, vfree);
8174 }
8175 }
8176 }
8177 }
8178 }
8179 }
8180 else
8181 {
8182 {
8183 IkReal j0array[1], cj0array[1], sj0array[1];
8184 bool j0valid[1] = { false };
8185 _nj0 = 1;
8186 IkReal x363 = ((1.0) * new_r10);
8187 CheckValue<IkReal> x364 = IKPowWithIntegerCheck(
8188 IKsign(((new_r10 * new_r10) + (new_r00 * new_r00))),
8189 -1);
8190 if (!x364.valid)
8191 {
8192 continue;
8193 }
8194 CheckValue<IkReal> x365 = IKatan2WithCheck(
8195 IkReal((((gconst1 * new_r00)) +
8196 (((-1.0) * gconst2 * x363)))),
8197 IkReal(((((-1.0) * gconst1 * x363)) +
8198 (((-1.0) * gconst2 * new_r00)))),
8199 IKFAST_ATAN2_MAGTHRESH);
8200 if (!x365.valid)
8201 {
8202 continue;
8203 }
8204 j0array[0] =
8205 ((-1.5707963267949) +
8206 (((1.5707963267949) * (x364.value))) + (x365.value));
8207 sj0array[0] = IKsin(j0array[0]);
8208 cj0array[0] = IKcos(j0array[0]);
8209 if (j0array[0] > IKPI)
8210 {
8211 j0array[0] -= IK2PI;
8212 }
8213 else if (j0array[0] < -IKPI)
8214 {
8215 j0array[0] += IK2PI;
8216 }
8217 j0valid[0] = true;
8218 for (int ij0 = 0; ij0 < 1; ++ij0)
8219 {
8220 if (!j0valid[ij0])
8221 {
8222 continue;
8223 }
8224 _ij0[0] = ij0;
8225 _ij0[1] = -1;
8226 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8227 {
8228 if (j0valid[iij0] &&
8229 IKabs(cj0array[ij0] - cj0array[iij0]) <
8230 IKFAST_SOLUTION_THRESH &&
8231 IKabs(sj0array[ij0] - sj0array[iij0]) <
8232 IKFAST_SOLUTION_THRESH)
8233 {
8234 j0valid[iij0] = false;
8235 _ij0[1] = iij0;
8236 break;
8237 }
8238 }
8239 j0 = j0array[ij0];
8240 cj0 = cj0array[ij0];
8241 sj0 = sj0array[ij0];
8242 {
8243 IkReal evalcond[8];
8244 IkReal x366 = IKcos(j0);
8245 IkReal x367 = IKsin(j0);
8246 IkReal x368 = ((1.0) * gconst1);
8247 IkReal x369 = (gconst2 * x366);
8248 IkReal x370 = (gconst1 * x366);
8249 IkReal x371 = (gconst2 * x367);
8250 IkReal x372 = ((1.0) * x367);
8251 IkReal x373 = (x367 * x368);
8252 evalcond[0] =
8253 (gconst2 + ((new_r00 * x366)) + ((new_r10 * x367)));
8254 evalcond[1] = (x371 + x370 + new_r10);
8255 evalcond[2] = ((((-1.0) * new_r00 * x372)) + gconst1 +
8256 ((new_r10 * x366)));
8257 evalcond[3] = (gconst2 + (((-1.0) * new_r01 * x372)) +
8258 ((new_r11 * x366)));
8259 evalcond[4] = ((((-1.0) * x373)) + x369 + new_r00);
8260 evalcond[5] = ((((-1.0) * x373)) + x369 + new_r11);
8261 evalcond[6] = ((((-1.0) * x368)) + ((new_r01 * x366)) +
8262 ((new_r11 * x367)));
8263 evalcond[7] = ((((-1.0) * x371)) +
8264 (((-1.0) * x366 * x368)) + new_r01);
8265 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8266 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8267 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8268 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8269 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8270 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8271 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8272 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8273 {
8274 continue;
8275 }
8276 }
8277
8278 {
8279 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
8280 vinfos[0].jointtype = 1;
8281 vinfos[0].foffset = j0;
8282 vinfos[0].indices[0] = _ij0[0];
8283 vinfos[0].indices[1] = _ij0[1];
8284 vinfos[0].maxsolutions = _nj0;
8285 vinfos[1].jointtype = 1;
8286 vinfos[1].foffset = j1;
8287 vinfos[1].indices[0] = _ij1[0];
8288 vinfos[1].indices[1] = _ij1[1];
8289 vinfos[1].maxsolutions = _nj1;
8290 vinfos[2].jointtype = 1;
8291 vinfos[2].foffset = j2;
8292 vinfos[2].indices[0] = _ij2[0];
8293 vinfos[2].indices[1] = _ij2[1];
8294 vinfos[2].maxsolutions = _nj2;
8295 vinfos[3].jointtype = 1;
8296 vinfos[3].foffset = j3;
8297 vinfos[3].indices[0] = _ij3[0];
8298 vinfos[3].indices[1] = _ij3[1];
8299 vinfos[3].maxsolutions = _nj3;
8300 vinfos[4].jointtype = 1;
8301 vinfos[4].foffset = j4;
8302 vinfos[4].indices[0] = _ij4[0];
8303 vinfos[4].indices[1] = _ij4[1];
8304 vinfos[4].maxsolutions = _nj4;
8305 vinfos[5].jointtype = 1;
8306 vinfos[5].foffset = j5;
8307 vinfos[5].indices[0] = _ij5[0];
8308 vinfos[5].indices[1] = _ij5[1];
8309 vinfos[5].maxsolutions = _nj5;
8310 vinfos[6].jointtype = 1;
8311 vinfos[6].foffset = j6;
8312 vinfos[6].indices[0] = _ij6[0];
8313 vinfos[6].indices[1] = _ij6[1];
8314 vinfos[6].maxsolutions = _nj6;
8315 std::vector<int> vfree(0);
8316 solutions.AddSolution(vinfos, vfree);
8317 }
8318 }
8319 }
8320 }
8321 }
8322 }
8323 } while (0);
8324 if (bgotonextstatement)
8325 {
8326 bool bgotonextstatement = true;
8327 do
8328 {
8329 IkReal x376 = ((new_r10 * new_r10) + (new_r00 * new_r00));
8330 if (IKabs(x376) == 0)
8331 {
8332 continue;
8333 }
8334 IkReal x374 = pow(x376, -0.5);
8335 IkReal x375 = ((1.0) * x374);
8336 CheckValue<IkReal> x377 =
8337 IKatan2WithCheck(IkReal(new_r00),
8338 IkReal(((-1.0) * new_r10)),
8339 IKFAST_ATAN2_MAGTHRESH);
8340 if (!x377.valid)
8341 {
8342 continue;
8343 }
8344 IkReal gconst3 = ((3.14159265358979) + (((-1.0) * (x377.value))));
8345 IkReal gconst4 = (new_r00 * x375);
8346 IkReal gconst5 = (new_r10 * x375);
8347 CheckValue<IkReal> x378 =
8348 IKatan2WithCheck(IkReal(new_r00),
8349 IkReal(((-1.0) * new_r10)),
8350 IKFAST_ATAN2_MAGTHRESH);
8351 if (!x378.valid)
8352 {
8353 continue;
8354 }
8355 evalcond[0] =
8356 ((-3.14159265358979) +
8357 (IKfmod(((3.14159265358979) +
8358 (IKabs(((-3.14159265358979) + (x378.value) + j2)))),
8359 6.28318530717959)));
8360 if (IKabs(evalcond[0]) < 0.0000050000000000)
8361 {
8362 bgotonextstatement = false;
8363 {
8364 IkReal j0eval[2];
8365 CheckValue<IkReal> x382 =
8366 IKatan2WithCheck(IkReal(new_r00),
8367 IkReal(((-1.0) * new_r10)),
8368 IKFAST_ATAN2_MAGTHRESH);
8369 if (!x382.valid)
8370 {
8371 continue;
8372 }
8373 IkReal x379 = ((1.0) * (x382.value));
8374 IkReal x380 = x374;
8375 IkReal x381 = ((1.0) * x380);
8376 sj1 = 0;
8377 cj1 = 1.0;
8378 j1 = 0;
8379 sj2 = gconst4;
8380 cj2 = gconst5;
8381 j2 = ((3.14159265) + (((-1.0) * x379)));
8382 IkReal gconst3 = ((3.14159265358979) + (((-1.0) * x379)));
8383 IkReal gconst4 = (new_r00 * x381);
8384 IkReal gconst5 = (new_r10 * x381);
8385 IkReal x383 = ((new_r10 * new_r10) + (new_r00 * new_r00));
8386 j0eval[0] = x383;
8387 j0eval[1] = IKsign(x383);
8388 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
8389 IKabs(j0eval[1]) < 0.0000010000000000)
8390 {
8391 {
8392 IkReal j0eval[3];
8393 CheckValue<IkReal> x387 =
8394 IKatan2WithCheck(IkReal(new_r00),
8395 IkReal(((-1.0) * new_r10)),
8396 IKFAST_ATAN2_MAGTHRESH);
8397 if (!x387.valid)
8398 {
8399 continue;
8400 }
8401 IkReal x384 = ((1.0) * (x387.value));
8402 IkReal x385 = x374;
8403 IkReal x386 = ((1.0) * x385);
8404 sj1 = 0;
8405 cj1 = 1.0;
8406 j1 = 0;
8407 sj2 = gconst4;
8408 cj2 = gconst5;
8409 j2 = ((3.14159265) + (((-1.0) * x384)));
8410 IkReal gconst3 = ((3.14159265358979) + (((-1.0) * x384)));
8411 IkReal gconst4 = (new_r00 * x386);
8412 IkReal gconst5 = (new_r10 * x386);
8413 IkReal x388 = new_r10 * new_r10;
8414 IkReal x389 = (new_r10 * new_r11);
8415 IkReal x390 = (((new_r00 * new_r01)) + x389);
8416 IkReal x391 = x374;
8417 IkReal x392 = ((1.0) * x391);
8418 j0eval[0] = x390;
8419 j0eval[1] =
8420 ((IKabs(((((-1.0) * new_r01 * new_r10 * x392)) +
8421 (((-1.0) * x388 * x392))))) +
8422 (IKabs(((((-1.0) * x389 * x392)) +
8423 ((new_r00 * new_r10 * x391))))));
8424 j0eval[2] = IKsign(x390);
8425 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
8426 IKabs(j0eval[1]) < 0.0000010000000000 ||
8427 IKabs(j0eval[2]) < 0.0000010000000000)
8428 {
8429 {
8430 IkReal j0eval[1];
8431 CheckValue<IkReal> x396 =
8432 IKatan2WithCheck(IkReal(new_r00),
8433 IkReal(((-1.0) * new_r10)),
8434 IKFAST_ATAN2_MAGTHRESH);
8435 if (!x396.valid)
8436 {
8437 continue;
8438 }
8439 IkReal x393 = ((1.0) * (x396.value));
8440 IkReal x394 = x374;
8441 IkReal x395 = ((1.0) * x394);
8442 sj1 = 0;
8443 cj1 = 1.0;
8444 j1 = 0;
8445 sj2 = gconst4;
8446 cj2 = gconst5;
8447 j2 = ((3.14159265) + (((-1.0) * x393)));
8448 IkReal gconst3 =
8449 ((3.14159265358979) + (((-1.0) * x393)));
8450 IkReal gconst4 = (new_r00 * x395);
8451 IkReal gconst5 = (new_r10 * x395);
8452 IkReal x397 = new_r10 * new_r10;
8453 IkReal x398 = new_r00 * new_r00;
8454 CheckValue<IkReal> x405 =
8455 IKPowWithIntegerCheck((x397 + x398), -1);
8456 if (!x405.valid)
8457 {
8458 continue;
8459 }
8460 IkReal x399 = x405.value;
8461 IkReal x400 = (x397 * x399);
8462 CheckValue<IkReal> x406 = IKPowWithIntegerCheck(
8463 ((((-1.0) * x398)) + (((-1.0) * x397))), -1);
8464 if (!x406.valid)
8465 {
8466 continue;
8467 }
8468 IkReal x401 = x406.value;
8469 IkReal x402 = ((1.0) * x401);
8470 IkReal x403 = (new_r00 * x402);
8471 IkReal x404 = (new_r10 * x402);
8472 j0eval[0] =
8473 ((IKabs((((x399 * (x398 * x398))) +
8474 ((x398 * x400)) + (((-1.0) * x400))))) +
8475 (IKabs(((((-1.0) * x403 *
8476 (new_r10 * new_r10 * new_r10))) +
8477 (((-1.0) * new_r10 * x403)) +
8478 (((-1.0) * new_r10 * x403 *
8479 (new_r00 * new_r00)))))));
8480 if (IKabs(j0eval[0]) < 0.0000010000000000)
8481 {
8482 continue; // no branches [j0]
8483 }
8484 else
8485 {
8486 {
8487 IkReal j0array[1], cj0array[1], sj0array[1];
8488 bool j0valid[1] = { false };
8489 _nj0 = 1;
8490 CheckValue<IkReal> x407 = IKPowWithIntegerCheck(
8491 IKsign((((gconst4 * new_r00)) +
8492 ((gconst5 * new_r10)))),
8493 -1);
8494 if (!x407.valid)
8495 {
8496 continue;
8497 }
8498 CheckValue<IkReal> x408 = IKatan2WithCheck(
8499 IkReal(((((-1.0) * (gconst5 * gconst5))) +
8500 (new_r00 * new_r00))),
8501 IkReal(((((-1.0) * gconst4 * gconst5)) +
8502 (((-1.0) * new_r00 * new_r10)))),
8503 IKFAST_ATAN2_MAGTHRESH);
8504 if (!x408.valid)
8505 {
8506 continue;
8507 }
8508 j0array[0] =
8509 ((-1.5707963267949) +
8510 (((1.5707963267949) * (x407.value))) +
8511 (x408.value));
8512 sj0array[0] = IKsin(j0array[0]);
8513 cj0array[0] = IKcos(j0array[0]);
8514 if (j0array[0] > IKPI)
8515 {
8516 j0array[0] -= IK2PI;
8517 }
8518 else if (j0array[0] < -IKPI)
8519 {
8520 j0array[0] += IK2PI;
8521 }
8522 j0valid[0] = true;
8523 for (int ij0 = 0; ij0 < 1; ++ij0)
8524 {
8525 if (!j0valid[ij0])
8526 {
8527 continue;
8528 }
8529 _ij0[0] = ij0;
8530 _ij0[1] = -1;
8531 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8532 {
8533 if (j0valid[iij0] &&
8534 IKabs(cj0array[ij0] - cj0array[iij0]) <
8535 IKFAST_SOLUTION_THRESH &&
8536 IKabs(sj0array[ij0] - sj0array[iij0]) <
8537 IKFAST_SOLUTION_THRESH)
8538 {
8539 j0valid[iij0] = false;
8540 _ij0[1] = iij0;
8541 break;
8542 }
8543 }
8544 j0 = j0array[ij0];
8545 cj0 = cj0array[ij0];
8546 sj0 = sj0array[ij0];
8547 {
8548 IkReal evalcond[8];
8549 IkReal x409 = IKcos(j0);
8550 IkReal x410 = IKsin(j0);
8551 IkReal x411 = ((1.0) * gconst4);
8552 IkReal x412 = (gconst5 * x409);
8553 IkReal x413 = ((1.0) * x410);
8554 IkReal x414 = (x410 * x411);
8555 evalcond[0] = (gconst5 + ((new_r10 * x410)) +
8556 ((new_r00 * x409)));
8557 evalcond[1] = (((gconst5 * x410)) +
8558 ((gconst4 * x409)) + new_r10);
8559 evalcond[2] = (gconst4 + ((new_r10 * x409)) +
8560 (((-1.0) * new_r00 * x413)));
8561 evalcond[3] = (gconst5 + ((new_r11 * x409)) +
8562 (((-1.0) * new_r01 * x413)));
8563 evalcond[4] =
8564 (x412 + new_r00 + (((-1.0) * x414)));
8565 evalcond[5] =
8566 (x412 + new_r11 + (((-1.0) * x414)));
8567 evalcond[6] =
8568 (((new_r11 * x410)) + ((new_r01 * x409)) +
8569 (((-1.0) * x411)));
8570 evalcond[7] =
8571 ((((-1.0) * gconst5 * x413)) + new_r01 +
8572 (((-1.0) * x409 * x411)));
8573 if (IKabs(evalcond[0]) >
8574 IKFAST_EVALCOND_THRESH ||
8575 IKabs(evalcond[1]) >
8576 IKFAST_EVALCOND_THRESH ||
8577 IKabs(evalcond[2]) >
8578 IKFAST_EVALCOND_THRESH ||
8579 IKabs(evalcond[3]) >
8580 IKFAST_EVALCOND_THRESH ||
8581 IKabs(evalcond[4]) >
8582 IKFAST_EVALCOND_THRESH ||
8583 IKabs(evalcond[5]) >
8584 IKFAST_EVALCOND_THRESH ||
8585 IKabs(evalcond[6]) >
8586 IKFAST_EVALCOND_THRESH ||
8587 IKabs(evalcond[7]) >
8588 IKFAST_EVALCOND_THRESH)
8589 {
8590 continue;
8591 }
8592 }
8593
8594 {
8595 std::vector<IkSingleDOFSolutionBase<IkReal> >
8596 vinfos(7);
8597 vinfos[0].jointtype = 1;
8598 vinfos[0].foffset = j0;
8599 vinfos[0].indices[0] = _ij0[0];
8600 vinfos[0].indices[1] = _ij0[1];
8601 vinfos[0].maxsolutions = _nj0;
8602 vinfos[1].jointtype = 1;
8603 vinfos[1].foffset = j1;
8604 vinfos[1].indices[0] = _ij1[0];
8605 vinfos[1].indices[1] = _ij1[1];
8606 vinfos[1].maxsolutions = _nj1;
8607 vinfos[2].jointtype = 1;
8608 vinfos[2].foffset = j2;
8609 vinfos[2].indices[0] = _ij2[0];
8610 vinfos[2].indices[1] = _ij2[1];
8611 vinfos[2].maxsolutions = _nj2;
8612 vinfos[3].jointtype = 1;
8613 vinfos[3].foffset = j3;
8614 vinfos[3].indices[0] = _ij3[0];
8615 vinfos[3].indices[1] = _ij3[1];
8616 vinfos[3].maxsolutions = _nj3;
8617 vinfos[4].jointtype = 1;
8618 vinfos[4].foffset = j4;
8619 vinfos[4].indices[0] = _ij4[0];
8620 vinfos[4].indices[1] = _ij4[1];
8621 vinfos[4].maxsolutions = _nj4;
8622 vinfos[5].jointtype = 1;
8623 vinfos[5].foffset = j5;
8624 vinfos[5].indices[0] = _ij5[0];
8625 vinfos[5].indices[1] = _ij5[1];
8626 vinfos[5].maxsolutions = _nj5;
8627 vinfos[6].jointtype = 1;
8628 vinfos[6].foffset = j6;
8629 vinfos[6].indices[0] = _ij6[0];
8630 vinfos[6].indices[1] = _ij6[1];
8631 vinfos[6].maxsolutions = _nj6;
8632 std::vector<int> vfree(0);
8633 solutions.AddSolution(vinfos, vfree);
8634 }
8635 }
8636 }
8637 }
8638 }
8639 }
8640 else
8641 {
8642 {
8643 IkReal j0array[1], cj0array[1], sj0array[1];
8644 bool j0valid[1] = { false };
8645 _nj0 = 1;
8646 IkReal x415 = ((1.0) * gconst5);
8647 CheckValue<IkReal> x416 = IKatan2WithCheck(
8648 IkReal((((gconst5 * new_r00)) +
8649 (((-1.0) * new_r11 * x415)))),
8650 IkReal(((((-1.0) * new_r10 * x415)) +
8651 (((-1.0) * new_r01 * x415)))),
8652 IKFAST_ATAN2_MAGTHRESH);
8653 if (!x416.valid)
8654 {
8655 continue;
8656 }
8657 CheckValue<IkReal> x417 = IKPowWithIntegerCheck(
8658 IKsign((((new_r10 * new_r11)) +
8659 ((new_r00 * new_r01)))),
8660 -1);
8661 if (!x417.valid)
8662 {
8663 continue;
8664 }
8665 j0array[0] = ((-1.5707963267949) + (x416.value) +
8666 (((1.5707963267949) * (x417.value))));
8667 sj0array[0] = IKsin(j0array[0]);
8668 cj0array[0] = IKcos(j0array[0]);
8669 if (j0array[0] > IKPI)
8670 {
8671 j0array[0] -= IK2PI;
8672 }
8673 else if (j0array[0] < -IKPI)
8674 {
8675 j0array[0] += IK2PI;
8676 }
8677 j0valid[0] = true;
8678 for (int ij0 = 0; ij0 < 1; ++ij0)
8679 {
8680 if (!j0valid[ij0])
8681 {
8682 continue;
8683 }
8684 _ij0[0] = ij0;
8685 _ij0[1] = -1;
8686 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8687 {
8688 if (j0valid[iij0] &&
8689 IKabs(cj0array[ij0] - cj0array[iij0]) <
8690 IKFAST_SOLUTION_THRESH &&
8691 IKabs(sj0array[ij0] - sj0array[iij0]) <
8692 IKFAST_SOLUTION_THRESH)
8693 {
8694 j0valid[iij0] = false;
8695 _ij0[1] = iij0;
8696 break;
8697 }
8698 }
8699 j0 = j0array[ij0];
8700 cj0 = cj0array[ij0];
8701 sj0 = sj0array[ij0];
8702 {
8703 IkReal evalcond[8];
8704 IkReal x418 = IKcos(j0);
8705 IkReal x419 = IKsin(j0);
8706 IkReal x420 = ((1.0) * gconst4);
8707 IkReal x421 = (gconst5 * x418);
8708 IkReal x422 = ((1.0) * x419);
8709 IkReal x423 = (x419 * x420);
8710 evalcond[0] = (gconst5 + ((new_r10 * x419)) +
8711 ((new_r00 * x418)));
8712 evalcond[1] = (((gconst5 * x419)) + new_r10 +
8713 ((gconst4 * x418)));
8714 evalcond[2] = ((((-1.0) * new_r00 * x422)) +
8715 gconst4 + ((new_r10 * x418)));
8716 evalcond[3] = (gconst5 + ((new_r11 * x418)) +
8717 (((-1.0) * new_r01 * x422)));
8718 evalcond[4] =
8719 ((((-1.0) * x423)) + x421 + new_r00);
8720 evalcond[5] =
8721 ((((-1.0) * x423)) + x421 + new_r11);
8722 evalcond[6] =
8723 ((((-1.0) * x420)) + ((new_r11 * x419)) +
8724 ((new_r01 * x418)));
8725 evalcond[7] =
8726 ((((-1.0) * x418 * x420)) + new_r01 +
8727 (((-1.0) * gconst5 * x422)));
8728 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8729 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8730 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8731 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8732 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8733 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8734 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8735 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8736 {
8737 continue;
8738 }
8739 }
8740
8741 {
8742 std::vector<IkSingleDOFSolutionBase<IkReal> >
8743 vinfos(7);
8744 vinfos[0].jointtype = 1;
8745 vinfos[0].foffset = j0;
8746 vinfos[0].indices[0] = _ij0[0];
8747 vinfos[0].indices[1] = _ij0[1];
8748 vinfos[0].maxsolutions = _nj0;
8749 vinfos[1].jointtype = 1;
8750 vinfos[1].foffset = j1;
8751 vinfos[1].indices[0] = _ij1[0];
8752 vinfos[1].indices[1] = _ij1[1];
8753 vinfos[1].maxsolutions = _nj1;
8754 vinfos[2].jointtype = 1;
8755 vinfos[2].foffset = j2;
8756 vinfos[2].indices[0] = _ij2[0];
8757 vinfos[2].indices[1] = _ij2[1];
8758 vinfos[2].maxsolutions = _nj2;
8759 vinfos[3].jointtype = 1;
8760 vinfos[3].foffset = j3;
8761 vinfos[3].indices[0] = _ij3[0];
8762 vinfos[3].indices[1] = _ij3[1];
8763 vinfos[3].maxsolutions = _nj3;
8764 vinfos[4].jointtype = 1;
8765 vinfos[4].foffset = j4;
8766 vinfos[4].indices[0] = _ij4[0];
8767 vinfos[4].indices[1] = _ij4[1];
8768 vinfos[4].maxsolutions = _nj4;
8769 vinfos[5].jointtype = 1;
8770 vinfos[5].foffset = j5;
8771 vinfos[5].indices[0] = _ij5[0];
8772 vinfos[5].indices[1] = _ij5[1];
8773 vinfos[5].maxsolutions = _nj5;
8774 vinfos[6].jointtype = 1;
8775 vinfos[6].foffset = j6;
8776 vinfos[6].indices[0] = _ij6[0];
8777 vinfos[6].indices[1] = _ij6[1];
8778 vinfos[6].maxsolutions = _nj6;
8779 std::vector<int> vfree(0);
8780 solutions.AddSolution(vinfos, vfree);
8781 }
8782 }
8783 }
8784 }
8785 }
8786 }
8787 else
8788 {
8789 {
8790 IkReal j0array[1], cj0array[1], sj0array[1];
8791 bool j0valid[1] = { false };
8792 _nj0 = 1;
8793 IkReal x424 = ((1.0) * new_r10);
8794 CheckValue<IkReal> x425 = IKatan2WithCheck(
8795 IkReal((((gconst4 * new_r00)) +
8796 (((-1.0) * gconst5 * x424)))),
8797 IkReal(((((-1.0) * gconst4 * x424)) +
8798 (((-1.0) * gconst5 * new_r00)))),
8799 IKFAST_ATAN2_MAGTHRESH);
8800 if (!x425.valid)
8801 {
8802 continue;
8803 }
8804 CheckValue<IkReal> x426 = IKPowWithIntegerCheck(
8805 IKsign(((new_r10 * new_r10) + (new_r00 * new_r00))),
8806 -1);
8807 if (!x426.valid)
8808 {
8809 continue;
8810 }
8811 j0array[0] = ((-1.5707963267949) + (x425.value) +
8812 (((1.5707963267949) * (x426.value))));
8813 sj0array[0] = IKsin(j0array[0]);
8814 cj0array[0] = IKcos(j0array[0]);
8815 if (j0array[0] > IKPI)
8816 {
8817 j0array[0] -= IK2PI;
8818 }
8819 else if (j0array[0] < -IKPI)
8820 {
8821 j0array[0] += IK2PI;
8822 }
8823 j0valid[0] = true;
8824 for (int ij0 = 0; ij0 < 1; ++ij0)
8825 {
8826 if (!j0valid[ij0])
8827 {
8828 continue;
8829 }
8830 _ij0[0] = ij0;
8831 _ij0[1] = -1;
8832 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8833 {
8834 if (j0valid[iij0] &&
8835 IKabs(cj0array[ij0] - cj0array[iij0]) <
8836 IKFAST_SOLUTION_THRESH &&
8837 IKabs(sj0array[ij0] - sj0array[iij0]) <
8838 IKFAST_SOLUTION_THRESH)
8839 {
8840 j0valid[iij0] = false;
8841 _ij0[1] = iij0;
8842 break;
8843 }
8844 }
8845 j0 = j0array[ij0];
8846 cj0 = cj0array[ij0];
8847 sj0 = sj0array[ij0];
8848 {
8849 IkReal evalcond[8];
8850 IkReal x427 = IKcos(j0);
8851 IkReal x428 = IKsin(j0);
8852 IkReal x429 = ((1.0) * gconst4);
8853 IkReal x430 = (gconst5 * x427);
8854 IkReal x431 = ((1.0) * x428);
8855 IkReal x432 = (x428 * x429);
8856 evalcond[0] = (gconst5 + ((new_r10 * x428)) +
8857 ((new_r00 * x427)));
8858 evalcond[1] = (((gconst4 * x427)) +
8859 ((gconst5 * x428)) + new_r10);
8860 evalcond[2] = ((((-1.0) * new_r00 * x431)) + gconst4 +
8861 ((new_r10 * x427)));
8862 evalcond[3] = ((((-1.0) * new_r01 * x431)) + gconst5 +
8863 ((new_r11 * x427)));
8864 evalcond[4] = ((((-1.0) * x432)) + x430 + new_r00);
8865 evalcond[5] = ((((-1.0) * x432)) + x430 + new_r11);
8866 evalcond[6] =
8867 ((((-1.0) * x429)) + ((new_r11 * x428)) +
8868 ((new_r01 * x427)));
8869 evalcond[7] = ((((-1.0) * x427 * x429)) +
8870 (((-1.0) * gconst5 * x431)) + new_r01);
8871 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
8872 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
8873 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
8874 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
8875 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
8876 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
8877 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
8878 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
8879 {
8880 continue;
8881 }
8882 }
8883
8884 {
8885 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(
8886 7);
8887 vinfos[0].jointtype = 1;
8888 vinfos[0].foffset = j0;
8889 vinfos[0].indices[0] = _ij0[0];
8890 vinfos[0].indices[1] = _ij0[1];
8891 vinfos[0].maxsolutions = _nj0;
8892 vinfos[1].jointtype = 1;
8893 vinfos[1].foffset = j1;
8894 vinfos[1].indices[0] = _ij1[0];
8895 vinfos[1].indices[1] = _ij1[1];
8896 vinfos[1].maxsolutions = _nj1;
8897 vinfos[2].jointtype = 1;
8898 vinfos[2].foffset = j2;
8899 vinfos[2].indices[0] = _ij2[0];
8900 vinfos[2].indices[1] = _ij2[1];
8901 vinfos[2].maxsolutions = _nj2;
8902 vinfos[3].jointtype = 1;
8903 vinfos[3].foffset = j3;
8904 vinfos[3].indices[0] = _ij3[0];
8905 vinfos[3].indices[1] = _ij3[1];
8906 vinfos[3].maxsolutions = _nj3;
8907 vinfos[4].jointtype = 1;
8908 vinfos[4].foffset = j4;
8909 vinfos[4].indices[0] = _ij4[0];
8910 vinfos[4].indices[1] = _ij4[1];
8911 vinfos[4].maxsolutions = _nj4;
8912 vinfos[5].jointtype = 1;
8913 vinfos[5].foffset = j5;
8914 vinfos[5].indices[0] = _ij5[0];
8915 vinfos[5].indices[1] = _ij5[1];
8916 vinfos[5].maxsolutions = _nj5;
8917 vinfos[6].jointtype = 1;
8918 vinfos[6].foffset = j6;
8919 vinfos[6].indices[0] = _ij6[0];
8920 vinfos[6].indices[1] = _ij6[1];
8921 vinfos[6].maxsolutions = _nj6;
8922 std::vector<int> vfree(0);
8923 solutions.AddSolution(vinfos, vfree);
8924 }
8925 }
8926 }
8927 }
8928 }
8929 }
8930 } while (0);
8931 if (bgotonextstatement)
8932 {
8933 bool bgotonextstatement = true;
8934 do
8935 {
8936 evalcond[0] = ((-3.14159265358979) +
8937 (IKfmod(((3.14159265358979) +
8938 (IKabs(((-1.5707963267949) + j2)))),
8939 6.28318530717959)));
8940 if (IKabs(evalcond[0]) < 0.0000050000000000)
8941 {
8942 bgotonextstatement = false;
8943 {
8944 IkReal j0array[1], cj0array[1], sj0array[1];
8945 bool j0valid[1] = { false };
8946 _nj0 = 1;
8947 if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
8948 IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
8949 IKabs(IKsqr(new_r00) + IKsqr(((-1.0) * new_r10)) - 1) <=
8950 IKFAST_SINCOS_THRESH)
8951 continue;
8952 j0array[0] = IKatan2(new_r00, ((-1.0) * new_r10));
8953 sj0array[0] = IKsin(j0array[0]);
8954 cj0array[0] = IKcos(j0array[0]);
8955 if (j0array[0] > IKPI)
8956 {
8957 j0array[0] -= IK2PI;
8958 }
8959 else if (j0array[0] < -IKPI)
8960 {
8961 j0array[0] += IK2PI;
8962 }
8963 j0valid[0] = true;
8964 for (int ij0 = 0; ij0 < 1; ++ij0)
8965 {
8966 if (!j0valid[ij0])
8967 {
8968 continue;
8969 }
8970 _ij0[0] = ij0;
8971 _ij0[1] = -1;
8972 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
8973 {
8974 if (j0valid[iij0] &&
8975 IKabs(cj0array[ij0] - cj0array[iij0]) <
8976 IKFAST_SOLUTION_THRESH &&
8977 IKabs(sj0array[ij0] - sj0array[iij0]) <
8978 IKFAST_SOLUTION_THRESH)
8979 {
8980 j0valid[iij0] = false;
8981 _ij0[1] = iij0;
8982 break;
8983 }
8984 }
8985 j0 = j0array[ij0];
8986 cj0 = cj0array[ij0];
8987 sj0 = sj0array[ij0];
8988 {
8989 IkReal evalcond[8];
8990 IkReal x433 = IKcos(j0);
8991 IkReal x434 = IKsin(j0);
8992 IkReal x435 = ((1.0) * x434);
8993 evalcond[0] = (x433 + new_r10);
8994 evalcond[1] = ((((-1.0) * x435)) + new_r00);
8995 evalcond[2] = ((((-1.0) * x435)) + new_r11);
8996 evalcond[3] = ((((-1.0) * x433)) + new_r01);
8997 evalcond[4] = (((new_r00 * x433)) + ((new_r10 * x434)));
8998 evalcond[5] =
8999 ((((-1.0) * new_r01 * x435)) + ((new_r11 * x433)));
9000 evalcond[6] =
9001 ((-1.0) + ((new_r01 * x433)) + ((new_r11 * x434)));
9002 evalcond[7] = ((1.0) + (((-1.0) * new_r00 * x435)) +
9003 ((new_r10 * x433)));
9004 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9005 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9006 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9007 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9008 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9009 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9010 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9011 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9012 {
9013 continue;
9014 }
9015 }
9016
9017 {
9018 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9019 vinfos[0].jointtype = 1;
9020 vinfos[0].foffset = j0;
9021 vinfos[0].indices[0] = _ij0[0];
9022 vinfos[0].indices[1] = _ij0[1];
9023 vinfos[0].maxsolutions = _nj0;
9024 vinfos[1].jointtype = 1;
9025 vinfos[1].foffset = j1;
9026 vinfos[1].indices[0] = _ij1[0];
9027 vinfos[1].indices[1] = _ij1[1];
9028 vinfos[1].maxsolutions = _nj1;
9029 vinfos[2].jointtype = 1;
9030 vinfos[2].foffset = j2;
9031 vinfos[2].indices[0] = _ij2[0];
9032 vinfos[2].indices[1] = _ij2[1];
9033 vinfos[2].maxsolutions = _nj2;
9034 vinfos[3].jointtype = 1;
9035 vinfos[3].foffset = j3;
9036 vinfos[3].indices[0] = _ij3[0];
9037 vinfos[3].indices[1] = _ij3[1];
9038 vinfos[3].maxsolutions = _nj3;
9039 vinfos[4].jointtype = 1;
9040 vinfos[4].foffset = j4;
9041 vinfos[4].indices[0] = _ij4[0];
9042 vinfos[4].indices[1] = _ij4[1];
9043 vinfos[4].maxsolutions = _nj4;
9044 vinfos[5].jointtype = 1;
9045 vinfos[5].foffset = j5;
9046 vinfos[5].indices[0] = _ij5[0];
9047 vinfos[5].indices[1] = _ij5[1];
9048 vinfos[5].maxsolutions = _nj5;
9049 vinfos[6].jointtype = 1;
9050 vinfos[6].foffset = j6;
9051 vinfos[6].indices[0] = _ij6[0];
9052 vinfos[6].indices[1] = _ij6[1];
9053 vinfos[6].maxsolutions = _nj6;
9054 std::vector<int> vfree(0);
9055 solutions.AddSolution(vinfos, vfree);
9056 }
9057 }
9058 }
9059 }
9060 } while (0);
9061 if (bgotonextstatement)
9062 {
9063 bool bgotonextstatement = true;
9064 do
9065 {
9066 evalcond[0] = ((-3.14159265358979) +
9067 (IKfmod(((3.14159265358979) +
9068 (IKabs(((1.5707963267949) + j2)))),
9069 6.28318530717959)));
9070 if (IKabs(evalcond[0]) < 0.0000050000000000)
9071 {
9072 bgotonextstatement = false;
9073 {
9074 IkReal j0array[1], cj0array[1], sj0array[1];
9075 bool j0valid[1] = { false };
9076 _nj0 = 1;
9077 if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
9078 IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
9079 IKabs(IKsqr(((-1.0) * new_r00)) +
9080 IKsqr(((-1.0) * new_r01)) - 1) <=
9081 IKFAST_SINCOS_THRESH)
9082 continue;
9083 j0array[0] =
9084 IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
9085 sj0array[0] = IKsin(j0array[0]);
9086 cj0array[0] = IKcos(j0array[0]);
9087 if (j0array[0] > IKPI)
9088 {
9089 j0array[0] -= IK2PI;
9090 }
9091 else if (j0array[0] < -IKPI)
9092 {
9093 j0array[0] += IK2PI;
9094 }
9095 j0valid[0] = true;
9096 for (int ij0 = 0; ij0 < 1; ++ij0)
9097 {
9098 if (!j0valid[ij0])
9099 {
9100 continue;
9101 }
9102 _ij0[0] = ij0;
9103 _ij0[1] = -1;
9104 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9105 {
9106 if (j0valid[iij0] &&
9107 IKabs(cj0array[ij0] - cj0array[iij0]) <
9108 IKFAST_SOLUTION_THRESH &&
9109 IKabs(sj0array[ij0] - sj0array[iij0]) <
9110 IKFAST_SOLUTION_THRESH)
9111 {
9112 j0valid[iij0] = false;
9113 _ij0[1] = iij0;
9114 break;
9115 }
9116 }
9117 j0 = j0array[ij0];
9118 cj0 = cj0array[ij0];
9119 sj0 = sj0array[ij0];
9120 {
9121 IkReal evalcond[8];
9122 IkReal x436 = IKsin(j0);
9123 IkReal x437 = IKcos(j0);
9124 IkReal x438 = ((1.0) * x436);
9125 evalcond[0] = (x436 + new_r00);
9126 evalcond[1] = (x436 + new_r11);
9127 evalcond[2] = (x437 + new_r01);
9128 evalcond[3] = ((((-1.0) * x437)) + new_r10);
9129 evalcond[4] =
9130 (((new_r00 * x437)) + ((new_r10 * x436)));
9131 evalcond[5] = ((((-1.0) * new_r01 * x438)) +
9132 ((new_r11 * x437)));
9133 evalcond[6] =
9134 ((1.0) + ((new_r01 * x437)) + ((new_r11 * x436)));
9135 evalcond[7] = ((-1.0) + (((-1.0) * new_r00 * x438)) +
9136 ((new_r10 * x437)));
9137 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9138 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9139 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9140 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9141 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9142 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9143 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9144 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9145 {
9146 continue;
9147 }
9148 }
9149
9150 {
9151 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(
9152 7);
9153 vinfos[0].jointtype = 1;
9154 vinfos[0].foffset = j0;
9155 vinfos[0].indices[0] = _ij0[0];
9156 vinfos[0].indices[1] = _ij0[1];
9157 vinfos[0].maxsolutions = _nj0;
9158 vinfos[1].jointtype = 1;
9159 vinfos[1].foffset = j1;
9160 vinfos[1].indices[0] = _ij1[0];
9161 vinfos[1].indices[1] = _ij1[1];
9162 vinfos[1].maxsolutions = _nj1;
9163 vinfos[2].jointtype = 1;
9164 vinfos[2].foffset = j2;
9165 vinfos[2].indices[0] = _ij2[0];
9166 vinfos[2].indices[1] = _ij2[1];
9167 vinfos[2].maxsolutions = _nj2;
9168 vinfos[3].jointtype = 1;
9169 vinfos[3].foffset = j3;
9170 vinfos[3].indices[0] = _ij3[0];
9171 vinfos[3].indices[1] = _ij3[1];
9172 vinfos[3].maxsolutions = _nj3;
9173 vinfos[4].jointtype = 1;
9174 vinfos[4].foffset = j4;
9175 vinfos[4].indices[0] = _ij4[0];
9176 vinfos[4].indices[1] = _ij4[1];
9177 vinfos[4].maxsolutions = _nj4;
9178 vinfos[5].jointtype = 1;
9179 vinfos[5].foffset = j5;
9180 vinfos[5].indices[0] = _ij5[0];
9181 vinfos[5].indices[1] = _ij5[1];
9182 vinfos[5].maxsolutions = _nj5;
9183 vinfos[6].jointtype = 1;
9184 vinfos[6].foffset = j6;
9185 vinfos[6].indices[0] = _ij6[0];
9186 vinfos[6].indices[1] = _ij6[1];
9187 vinfos[6].maxsolutions = _nj6;
9188 std::vector<int> vfree(0);
9189 solutions.AddSolution(vinfos, vfree);
9190 }
9191 }
9192 }
9193 }
9194 } while (0);
9195 if (bgotonextstatement)
9196 {
9197 bool bgotonextstatement = true;
9198 do
9199 {
9200 evalcond[0] = ((new_r10 * new_r10) + (new_r00 * new_r00));
9201 if (IKabs(evalcond[0]) < 0.0000050000000000)
9202 {
9203 bgotonextstatement = false;
9204 {
9205 IkReal j0eval[1];
9206 sj1 = 0;
9207 cj1 = 1.0;
9208 j1 = 0;
9209 new_r10 = 0;
9210 new_r00 = 0;
9211 j0eval[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
9212 if (IKabs(j0eval[0]) < 0.0000010000000000)
9213 {
9214 continue; // 3 cases reached
9215 }
9216 else
9217 {
9218 {
9219 IkReal j0array[2], cj0array[2], sj0array[2];
9220 bool j0valid[2] = { false };
9221 _nj0 = 2;
9222 CheckValue<IkReal> x440 =
9223 IKatan2WithCheck(IkReal(new_r01),
9224 IkReal(new_r11),
9225 IKFAST_ATAN2_MAGTHRESH);
9226 if (!x440.valid)
9227 {
9228 continue;
9229 }
9230 IkReal x439 = x440.value;
9231 j0array[0] = ((-1.0) * x439);
9232 sj0array[0] = IKsin(j0array[0]);
9233 cj0array[0] = IKcos(j0array[0]);
9234 j0array[1] =
9235 ((3.14159265358979) + (((-1.0) * x439)));
9236 sj0array[1] = IKsin(j0array[1]);
9237 cj0array[1] = IKcos(j0array[1]);
9238 if (j0array[0] > IKPI)
9239 {
9240 j0array[0] -= IK2PI;
9241 }
9242 else if (j0array[0] < -IKPI)
9243 {
9244 j0array[0] += IK2PI;
9245 }
9246 j0valid[0] = true;
9247 if (j0array[1] > IKPI)
9248 {
9249 j0array[1] -= IK2PI;
9250 }
9251 else if (j0array[1] < -IKPI)
9252 {
9253 j0array[1] += IK2PI;
9254 }
9255 j0valid[1] = true;
9256 for (int ij0 = 0; ij0 < 2; ++ij0)
9257 {
9258 if (!j0valid[ij0])
9259 {
9260 continue;
9261 }
9262 _ij0[0] = ij0;
9263 _ij0[1] = -1;
9264 for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
9265 {
9266 if (j0valid[iij0] &&
9267 IKabs(cj0array[ij0] - cj0array[iij0]) <
9268 IKFAST_SOLUTION_THRESH &&
9269 IKabs(sj0array[ij0] - sj0array[iij0]) <
9270 IKFAST_SOLUTION_THRESH)
9271 {
9272 j0valid[iij0] = false;
9273 _ij0[1] = iij0;
9274 break;
9275 }
9276 }
9277 j0 = j0array[ij0];
9278 cj0 = cj0array[ij0];
9279 sj0 = sj0array[ij0];
9280 {
9281 IkReal evalcond[1];
9282 evalcond[0] =
9283 (((new_r11 * (IKcos(j0)))) +
9284 (((-1.0) * new_r01 * (IKsin(j0)))));
9285 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
9286 {
9287 continue;
9288 }
9289 }
9290
9291 {
9292 std::vector<IkSingleDOFSolutionBase<IkReal> >
9293 vinfos(7);
9294 vinfos[0].jointtype = 1;
9295 vinfos[0].foffset = j0;
9296 vinfos[0].indices[0] = _ij0[0];
9297 vinfos[0].indices[1] = _ij0[1];
9298 vinfos[0].maxsolutions = _nj0;
9299 vinfos[1].jointtype = 1;
9300 vinfos[1].foffset = j1;
9301 vinfos[1].indices[0] = _ij1[0];
9302 vinfos[1].indices[1] = _ij1[1];
9303 vinfos[1].maxsolutions = _nj1;
9304 vinfos[2].jointtype = 1;
9305 vinfos[2].foffset = j2;
9306 vinfos[2].indices[0] = _ij2[0];
9307 vinfos[2].indices[1] = _ij2[1];
9308 vinfos[2].maxsolutions = _nj2;
9309 vinfos[3].jointtype = 1;
9310 vinfos[3].foffset = j3;
9311 vinfos[3].indices[0] = _ij3[0];
9312 vinfos[3].indices[1] = _ij3[1];
9313 vinfos[3].maxsolutions = _nj3;
9314 vinfos[4].jointtype = 1;
9315 vinfos[4].foffset = j4;
9316 vinfos[4].indices[0] = _ij4[0];
9317 vinfos[4].indices[1] = _ij4[1];
9318 vinfos[4].maxsolutions = _nj4;
9319 vinfos[5].jointtype = 1;
9320 vinfos[5].foffset = j5;
9321 vinfos[5].indices[0] = _ij5[0];
9322 vinfos[5].indices[1] = _ij5[1];
9323 vinfos[5].maxsolutions = _nj5;
9324 vinfos[6].jointtype = 1;
9325 vinfos[6].foffset = j6;
9326 vinfos[6].indices[0] = _ij6[0];
9327 vinfos[6].indices[1] = _ij6[1];
9328 vinfos[6].maxsolutions = _nj6;
9329 std::vector<int> vfree(0);
9330 solutions.AddSolution(vinfos, vfree);
9331 }
9332 }
9333 }
9334 }
9335 }
9336 }
9337 } while (0);
9338 if (bgotonextstatement)
9339 {
9340 bool bgotonextstatement = true;
9341 do
9342 {
9343 evalcond[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
9344 if (IKabs(evalcond[0]) < 0.0000050000000000)
9345 {
9346 bgotonextstatement = false;
9347 {
9348 IkReal j0eval[1];
9349 sj1 = 0;
9350 cj1 = 1.0;
9351 j1 = 0;
9352 new_r00 = 0;
9353 new_r10 = 0;
9354 new_r21 = 0;
9355 new_r22 = 0;
9356 j0eval[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
9357 if (IKabs(j0eval[0]) < 0.0000010000000000)
9358 {
9359 continue; // no branches [j0]
9360 }
9361 else
9362 {
9363 {
9364 IkReal j0array[2], cj0array[2], sj0array[2];
9365 bool j0valid[2] = { false };
9366 _nj0 = 2;
9367 CheckValue<IkReal> x442 =
9368 IKatan2WithCheck(IkReal(new_r01),
9369 IkReal(new_r11),
9370 IKFAST_ATAN2_MAGTHRESH);
9371 if (!x442.valid)
9372 {
9373 continue;
9374 }
9375 IkReal x441 = x442.value;
9376 j0array[0] = ((-1.0) * x441);
9377 sj0array[0] = IKsin(j0array[0]);
9378 cj0array[0] = IKcos(j0array[0]);
9379 j0array[1] =
9380 ((3.14159265358979) + (((-1.0) * x441)));
9381 sj0array[1] = IKsin(j0array[1]);
9382 cj0array[1] = IKcos(j0array[1]);
9383 if (j0array[0] > IKPI)
9384 {
9385 j0array[0] -= IK2PI;
9386 }
9387 else if (j0array[0] < -IKPI)
9388 {
9389 j0array[0] += IK2PI;
9390 }
9391 j0valid[0] = true;
9392 if (j0array[1] > IKPI)
9393 {
9394 j0array[1] -= IK2PI;
9395 }
9396 else if (j0array[1] < -IKPI)
9397 {
9398 j0array[1] += IK2PI;
9399 }
9400 j0valid[1] = true;
9401 for (int ij0 = 0; ij0 < 2; ++ij0)
9402 {
9403 if (!j0valid[ij0])
9404 {
9405 continue;
9406 }
9407 _ij0[0] = ij0;
9408 _ij0[1] = -1;
9409 for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
9410 {
9411 if (j0valid[iij0] &&
9412 IKabs(cj0array[ij0] - cj0array[iij0]) <
9413 IKFAST_SOLUTION_THRESH &&
9414 IKabs(sj0array[ij0] - sj0array[iij0]) <
9415 IKFAST_SOLUTION_THRESH)
9416 {
9417 j0valid[iij0] = false;
9418 _ij0[1] = iij0;
9419 break;
9420 }
9421 }
9422 j0 = j0array[ij0];
9423 cj0 = cj0array[ij0];
9424 sj0 = sj0array[ij0];
9425 {
9426 IkReal evalcond[1];
9427 evalcond[0] =
9428 (((new_r11 * (IKcos(j0)))) +
9429 (((-1.0) * new_r01 * (IKsin(j0)))));
9430 if (IKabs(evalcond[0]) >
9431 IKFAST_EVALCOND_THRESH)
9432 {
9433 continue;
9434 }
9435 }
9436
9437 {
9438 std::vector<IkSingleDOFSolutionBase<IkReal> >
9439 vinfos(7);
9440 vinfos[0].jointtype = 1;
9441 vinfos[0].foffset = j0;
9442 vinfos[0].indices[0] = _ij0[0];
9443 vinfos[0].indices[1] = _ij0[1];
9444 vinfos[0].maxsolutions = _nj0;
9445 vinfos[1].jointtype = 1;
9446 vinfos[1].foffset = j1;
9447 vinfos[1].indices[0] = _ij1[0];
9448 vinfos[1].indices[1] = _ij1[1];
9449 vinfos[1].maxsolutions = _nj1;
9450 vinfos[2].jointtype = 1;
9451 vinfos[2].foffset = j2;
9452 vinfos[2].indices[0] = _ij2[0];
9453 vinfos[2].indices[1] = _ij2[1];
9454 vinfos[2].maxsolutions = _nj2;
9455 vinfos[3].jointtype = 1;
9456 vinfos[3].foffset = j3;
9457 vinfos[3].indices[0] = _ij3[0];
9458 vinfos[3].indices[1] = _ij3[1];
9459 vinfos[3].maxsolutions = _nj3;
9460 vinfos[4].jointtype = 1;
9461 vinfos[4].foffset = j4;
9462 vinfos[4].indices[0] = _ij4[0];
9463 vinfos[4].indices[1] = _ij4[1];
9464 vinfos[4].maxsolutions = _nj4;
9465 vinfos[5].jointtype = 1;
9466 vinfos[5].foffset = j5;
9467 vinfos[5].indices[0] = _ij5[0];
9468 vinfos[5].indices[1] = _ij5[1];
9469 vinfos[5].maxsolutions = _nj5;
9470 vinfos[6].jointtype = 1;
9471 vinfos[6].foffset = j6;
9472 vinfos[6].indices[0] = _ij6[0];
9473 vinfos[6].indices[1] = _ij6[1];
9474 vinfos[6].maxsolutions = _nj6;
9475 std::vector<int> vfree(0);
9476 solutions.AddSolution(vinfos, vfree);
9477 }
9478 }
9479 }
9480 }
9481 }
9482 }
9483 } while (0);
9484 if (bgotonextstatement)
9485 {
9486 bool bgotonextstatement = true;
9487 do
9488 {
9489 if (1)
9490 {
9491 bgotonextstatement = false;
9492 continue; // branch miss [j0]
9493 }
9494 } while (0);
9495 if (bgotonextstatement)
9496 {
9497 }
9498 }
9499 }
9500 }
9501 }
9502 }
9503 }
9504 }
9505 }
9506 else
9507 {
9508 {
9509 IkReal j0array[1], cj0array[1], sj0array[1];
9510 bool j0valid[1] = { false };
9511 _nj0 = 1;
9512 IkReal x443 = ((1.0) * new_r10);
9513 CheckValue<IkReal> x444 = IKatan2WithCheck(
9514 IkReal((((cj2 * sj2)) + (((-1.0) * new_r00 * x443)))),
9515 IkReal(((((-1.0) * (cj2 * cj2))) + (new_r10 * new_r10))),
9516 IKFAST_ATAN2_MAGTHRESH);
9517 if (!x444.valid)
9518 {
9519 continue;
9520 }
9521 CheckValue<IkReal> x445 = IKPowWithIntegerCheck(
9522 IKsign(((((-1.0) * sj2 * x443)) + ((cj2 * new_r00)))), -1);
9523 if (!x445.valid)
9524 {
9525 continue;
9526 }
9527 j0array[0] = ((-1.5707963267949) + (x444.value) +
9528 (((1.5707963267949) * (x445.value))));
9529 sj0array[0] = IKsin(j0array[0]);
9530 cj0array[0] = IKcos(j0array[0]);
9531 if (j0array[0] > IKPI)
9532 {
9533 j0array[0] -= IK2PI;
9534 }
9535 else if (j0array[0] < -IKPI)
9536 {
9537 j0array[0] += IK2PI;
9538 }
9539 j0valid[0] = true;
9540 for (int ij0 = 0; ij0 < 1; ++ij0)
9541 {
9542 if (!j0valid[ij0])
9543 {
9544 continue;
9545 }
9546 _ij0[0] = ij0;
9547 _ij0[1] = -1;
9548 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9549 {
9550 if (j0valid[iij0] &&
9551 IKabs(cj0array[ij0] - cj0array[iij0]) <
9552 IKFAST_SOLUTION_THRESH &&
9553 IKabs(sj0array[ij0] - sj0array[iij0]) <
9554 IKFAST_SOLUTION_THRESH)
9555 {
9556 j0valid[iij0] = false;
9557 _ij0[1] = iij0;
9558 break;
9559 }
9560 }
9561 j0 = j0array[ij0];
9562 cj0 = cj0array[ij0];
9563 sj0 = sj0array[ij0];
9564 {
9565 IkReal evalcond[8];
9566 IkReal x446 = IKcos(j0);
9567 IkReal x447 = IKsin(j0);
9568 IkReal x448 = ((1.0) * sj2);
9569 IkReal x449 = (cj2 * x446);
9570 IkReal x450 = ((1.0) * x447);
9571 IkReal x451 = (x447 * x448);
9572 evalcond[0] = (((new_r00 * x446)) + cj2 + ((new_r10 * x447)));
9573 evalcond[1] = (((cj2 * x447)) + ((sj2 * x446)) + new_r10);
9574 evalcond[2] =
9575 (sj2 + (((-1.0) * new_r00 * x450)) + ((new_r10 * x446)));
9576 evalcond[3] =
9577 (cj2 + (((-1.0) * new_r01 * x450)) + ((new_r11 * x446)));
9578 evalcond[4] = ((((-1.0) * x451)) + x449 + new_r00);
9579 evalcond[5] = ((((-1.0) * x451)) + x449 + new_r11);
9580 evalcond[6] =
9581 ((((-1.0) * x448)) + ((new_r01 * x446)) + ((new_r11 * x447)));
9582 evalcond[7] = (new_r01 + (((-1.0) * cj2 * x450)) +
9583 (((-1.0) * x446 * x448)));
9584 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9585 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9586 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9587 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9588 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9589 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9590 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9591 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9592 {
9593 continue;
9594 }
9595 }
9596
9597 {
9598 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9599 vinfos[0].jointtype = 1;
9600 vinfos[0].foffset = j0;
9601 vinfos[0].indices[0] = _ij0[0];
9602 vinfos[0].indices[1] = _ij0[1];
9603 vinfos[0].maxsolutions = _nj0;
9604 vinfos[1].jointtype = 1;
9605 vinfos[1].foffset = j1;
9606 vinfos[1].indices[0] = _ij1[0];
9607 vinfos[1].indices[1] = _ij1[1];
9608 vinfos[1].maxsolutions = _nj1;
9609 vinfos[2].jointtype = 1;
9610 vinfos[2].foffset = j2;
9611 vinfos[2].indices[0] = _ij2[0];
9612 vinfos[2].indices[1] = _ij2[1];
9613 vinfos[2].maxsolutions = _nj2;
9614 vinfos[3].jointtype = 1;
9615 vinfos[3].foffset = j3;
9616 vinfos[3].indices[0] = _ij3[0];
9617 vinfos[3].indices[1] = _ij3[1];
9618 vinfos[3].maxsolutions = _nj3;
9619 vinfos[4].jointtype = 1;
9620 vinfos[4].foffset = j4;
9621 vinfos[4].indices[0] = _ij4[0];
9622 vinfos[4].indices[1] = _ij4[1];
9623 vinfos[4].maxsolutions = _nj4;
9624 vinfos[5].jointtype = 1;
9625 vinfos[5].foffset = j5;
9626 vinfos[5].indices[0] = _ij5[0];
9627 vinfos[5].indices[1] = _ij5[1];
9628 vinfos[5].maxsolutions = _nj5;
9629 vinfos[6].jointtype = 1;
9630 vinfos[6].foffset = j6;
9631 vinfos[6].indices[0] = _ij6[0];
9632 vinfos[6].indices[1] = _ij6[1];
9633 vinfos[6].maxsolutions = _nj6;
9634 std::vector<int> vfree(0);
9635 solutions.AddSolution(vinfos, vfree);
9636 }
9637 }
9638 }
9639 }
9640 }
9641 }
9642 else
9643 {
9644 {
9645 IkReal j0array[1], cj0array[1], sj0array[1];
9646 bool j0valid[1] = { false };
9647 _nj0 = 1;
9648 IkReal x452 = ((1.0) * cj2);
9649 CheckValue<IkReal> x453 = IKatan2WithCheck(
9650 IkReal(((((-1.0) * new_r11 * x452)) + ((cj2 * new_r00)))),
9651 IkReal(((((-1.0) * new_r10 * x452)) + (((-1.0) * new_r01 * x452)))),
9652 IKFAST_ATAN2_MAGTHRESH);
9653 if (!x453.valid)
9654 {
9655 continue;
9656 }
9657 CheckValue<IkReal> x454 = IKPowWithIntegerCheck(
9658 IKsign((((new_r10 * new_r11)) + ((new_r00 * new_r01)))), -1);
9659 if (!x454.valid)
9660 {
9661 continue;
9662 }
9663 j0array[0] = ((-1.5707963267949) + (x453.value) +
9664 (((1.5707963267949) * (x454.value))));
9665 sj0array[0] = IKsin(j0array[0]);
9666 cj0array[0] = IKcos(j0array[0]);
9667 if (j0array[0] > IKPI)
9668 {
9669 j0array[0] -= IK2PI;
9670 }
9671 else if (j0array[0] < -IKPI)
9672 {
9673 j0array[0] += IK2PI;
9674 }
9675 j0valid[0] = true;
9676 for (int ij0 = 0; ij0 < 1; ++ij0)
9677 {
9678 if (!j0valid[ij0])
9679 {
9680 continue;
9681 }
9682 _ij0[0] = ij0;
9683 _ij0[1] = -1;
9684 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9685 {
9686 if (j0valid[iij0] &&
9687 IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
9688 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
9689 {
9690 j0valid[iij0] = false;
9691 _ij0[1] = iij0;
9692 break;
9693 }
9694 }
9695 j0 = j0array[ij0];
9696 cj0 = cj0array[ij0];
9697 sj0 = sj0array[ij0];
9698 {
9699 IkReal evalcond[8];
9700 IkReal x455 = IKcos(j0);
9701 IkReal x456 = IKsin(j0);
9702 IkReal x457 = ((1.0) * sj2);
9703 IkReal x458 = (cj2 * x455);
9704 IkReal x459 = ((1.0) * x456);
9705 IkReal x460 = (x456 * x457);
9706 evalcond[0] = (((new_r10 * x456)) + cj2 + ((new_r00 * x455)));
9707 evalcond[1] = (((sj2 * x455)) + ((cj2 * x456)) + new_r10);
9708 evalcond[2] =
9709 (((new_r10 * x455)) + sj2 + (((-1.0) * new_r00 * x459)));
9710 evalcond[3] =
9711 (((new_r11 * x455)) + cj2 + (((-1.0) * new_r01 * x459)));
9712 evalcond[4] = ((((-1.0) * x460)) + x458 + new_r00);
9713 evalcond[5] = ((((-1.0) * x460)) + x458 + new_r11);
9714 evalcond[6] =
9715 (((new_r11 * x456)) + ((new_r01 * x455)) + (((-1.0) * x457)));
9716 evalcond[7] =
9717 ((((-1.0) * x455 * x457)) + new_r01 + (((-1.0) * cj2 * x459)));
9718 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9719 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9720 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9721 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9722 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9723 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9724 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9725 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9726 {
9727 continue;
9728 }
9729 }
9730
9731 {
9732 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9733 vinfos[0].jointtype = 1;
9734 vinfos[0].foffset = j0;
9735 vinfos[0].indices[0] = _ij0[0];
9736 vinfos[0].indices[1] = _ij0[1];
9737 vinfos[0].maxsolutions = _nj0;
9738 vinfos[1].jointtype = 1;
9739 vinfos[1].foffset = j1;
9740 vinfos[1].indices[0] = _ij1[0];
9741 vinfos[1].indices[1] = _ij1[1];
9742 vinfos[1].maxsolutions = _nj1;
9743 vinfos[2].jointtype = 1;
9744 vinfos[2].foffset = j2;
9745 vinfos[2].indices[0] = _ij2[0];
9746 vinfos[2].indices[1] = _ij2[1];
9747 vinfos[2].maxsolutions = _nj2;
9748 vinfos[3].jointtype = 1;
9749 vinfos[3].foffset = j3;
9750 vinfos[3].indices[0] = _ij3[0];
9751 vinfos[3].indices[1] = _ij3[1];
9752 vinfos[3].maxsolutions = _nj3;
9753 vinfos[4].jointtype = 1;
9754 vinfos[4].foffset = j4;
9755 vinfos[4].indices[0] = _ij4[0];
9756 vinfos[4].indices[1] = _ij4[1];
9757 vinfos[4].maxsolutions = _nj4;
9758 vinfos[5].jointtype = 1;
9759 vinfos[5].foffset = j5;
9760 vinfos[5].indices[0] = _ij5[0];
9761 vinfos[5].indices[1] = _ij5[1];
9762 vinfos[5].maxsolutions = _nj5;
9763 vinfos[6].jointtype = 1;
9764 vinfos[6].foffset = j6;
9765 vinfos[6].indices[0] = _ij6[0];
9766 vinfos[6].indices[1] = _ij6[1];
9767 vinfos[6].maxsolutions = _nj6;
9768 std::vector<int> vfree(0);
9769 solutions.AddSolution(vinfos, vfree);
9770 }
9771 }
9772 }
9773 }
9774 }
9775 }
9776 else
9777 {
9778 {
9779 IkReal j0array[1], cj0array[1], sj0array[1];
9780 bool j0valid[1] = { false };
9781 _nj0 = 1;
9782 IkReal x461 = ((1.0) * cj2);
9783 CheckValue<IkReal> x462 = IKPowWithIntegerCheck(
9784 IKsign(((new_r10 * new_r10) + (new_r00 * new_r00))), -1);
9785 if (!x462.valid)
9786 {
9787 continue;
9788 }
9789 CheckValue<IkReal> x463 = IKatan2WithCheck(
9790 IkReal(((((-1.0) * new_r10 * x461)) + ((new_r00 * sj2)))),
9791 IkReal(((((-1.0) * new_r00 * x461)) + (((-1.0) * new_r10 * sj2)))),
9792 IKFAST_ATAN2_MAGTHRESH);
9793 if (!x463.valid)
9794 {
9795 continue;
9796 }
9797 j0array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x462.value))) +
9798 (x463.value));
9799 sj0array[0] = IKsin(j0array[0]);
9800 cj0array[0] = IKcos(j0array[0]);
9801 if (j0array[0] > IKPI)
9802 {
9803 j0array[0] -= IK2PI;
9804 }
9805 else if (j0array[0] < -IKPI)
9806 {
9807 j0array[0] += IK2PI;
9808 }
9809 j0valid[0] = true;
9810 for (int ij0 = 0; ij0 < 1; ++ij0)
9811 {
9812 if (!j0valid[ij0])
9813 {
9814 continue;
9815 }
9816 _ij0[0] = ij0;
9817 _ij0[1] = -1;
9818 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
9819 {
9820 if (j0valid[iij0] &&
9821 IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
9822 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
9823 {
9824 j0valid[iij0] = false;
9825 _ij0[1] = iij0;
9826 break;
9827 }
9828 }
9829 j0 = j0array[ij0];
9830 cj0 = cj0array[ij0];
9831 sj0 = sj0array[ij0];
9832 {
9833 IkReal evalcond[8];
9834 IkReal x464 = IKcos(j0);
9835 IkReal x465 = IKsin(j0);
9836 IkReal x466 = ((1.0) * sj2);
9837 IkReal x467 = (cj2 * x464);
9838 IkReal x468 = ((1.0) * x465);
9839 IkReal x469 = (x465 * x466);
9840 evalcond[0] = (((new_r10 * x465)) + cj2 + ((new_r00 * x464)));
9841 evalcond[1] = (((cj2 * x465)) + new_r10 + ((sj2 * x464)));
9842 evalcond[2] = (sj2 + ((new_r10 * x464)) + (((-1.0) * new_r00 * x468)));
9843 evalcond[3] = (((new_r11 * x464)) + cj2 + (((-1.0) * new_r01 * x468)));
9844 evalcond[4] = ((((-1.0) * x469)) + x467 + new_r00);
9845 evalcond[5] = ((((-1.0) * x469)) + x467 + new_r11);
9846 evalcond[6] =
9847 (((new_r11 * x465)) + ((new_r01 * x464)) + (((-1.0) * x466)));
9848 evalcond[7] =
9849 ((((-1.0) * x464 * x466)) + new_r01 + (((-1.0) * cj2 * x468)));
9850 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
9851 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
9852 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
9853 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
9854 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
9855 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
9856 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
9857 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
9858 {
9859 continue;
9860 }
9861 }
9862
9863 {
9864 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
9865 vinfos[0].jointtype = 1;
9866 vinfos[0].foffset = j0;
9867 vinfos[0].indices[0] = _ij0[0];
9868 vinfos[0].indices[1] = _ij0[1];
9869 vinfos[0].maxsolutions = _nj0;
9870 vinfos[1].jointtype = 1;
9871 vinfos[1].foffset = j1;
9872 vinfos[1].indices[0] = _ij1[0];
9873 vinfos[1].indices[1] = _ij1[1];
9874 vinfos[1].maxsolutions = _nj1;
9875 vinfos[2].jointtype = 1;
9876 vinfos[2].foffset = j2;
9877 vinfos[2].indices[0] = _ij2[0];
9878 vinfos[2].indices[1] = _ij2[1];
9879 vinfos[2].maxsolutions = _nj2;
9880 vinfos[3].jointtype = 1;
9881 vinfos[3].foffset = j3;
9882 vinfos[3].indices[0] = _ij3[0];
9883 vinfos[3].indices[1] = _ij3[1];
9884 vinfos[3].maxsolutions = _nj3;
9885 vinfos[4].jointtype = 1;
9886 vinfos[4].foffset = j4;
9887 vinfos[4].indices[0] = _ij4[0];
9888 vinfos[4].indices[1] = _ij4[1];
9889 vinfos[4].maxsolutions = _nj4;
9890 vinfos[5].jointtype = 1;
9891 vinfos[5].foffset = j5;
9892 vinfos[5].indices[0] = _ij5[0];
9893 vinfos[5].indices[1] = _ij5[1];
9894 vinfos[5].maxsolutions = _nj5;
9895 vinfos[6].jointtype = 1;
9896 vinfos[6].foffset = j6;
9897 vinfos[6].indices[0] = _ij6[0];
9898 vinfos[6].indices[1] = _ij6[1];
9899 vinfos[6].maxsolutions = _nj6;
9900 std::vector<int> vfree(0);
9901 solutions.AddSolution(vinfos, vfree);
9902 }
9903 }
9904 }
9905 }
9906 }
9907 }
9908 } while (0);
9909 if (bgotonextstatement)
9910 {
9911 bool bgotonextstatement = true;
9912 do
9913 {
9914 evalcond[0] = ((-3.14159265358979) +
9915 (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
9916 6.28318530717959)));
9917 evalcond[1] = new_r21;
9918 evalcond[2] = new_r02;
9919 evalcond[3] = new_r12;
9920 evalcond[4] = new_r20;
9921 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
9922 IKabs(evalcond[1]) < 0.0000050000000000 &&
9923 IKabs(evalcond[2]) < 0.0000050000000000 &&
9924 IKabs(evalcond[3]) < 0.0000050000000000 &&
9925 IKabs(evalcond[4]) < 0.0000050000000000)
9926 {
9927 bgotonextstatement = false;
9928 {
9929 IkReal j0eval[3];
9930 sj1 = 0;
9931 cj1 = -1.0;
9932 j1 = 3.14159265358979;
9933 IkReal x470 = ((1.0) * sj2);
9934 IkReal x471 = (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
9935 j0eval[0] = x471;
9936 j0eval[1] =
9937 ((IKabs(((((-1.0) * new_r00 * x470)) + (((-1.0) * new_r11 * x470))))) +
9938 (IKabs((((new_r01 * sj2)) + (((-1.0) * new_r10 * x470))))));
9939 j0eval[2] = IKsign(x471);
9940 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
9941 IKabs(j0eval[1]) < 0.0000010000000000 ||
9942 IKabs(j0eval[2]) < 0.0000010000000000)
9943 {
9944 {
9945 IkReal j0eval[3];
9946 sj1 = 0;
9947 cj1 = -1.0;
9948 j1 = 3.14159265358979;
9949 IkReal x472 = ((1.0) * new_r11);
9950 IkReal x473 = ((new_r01 * new_r01) + (new_r11 * new_r11));
9951 j0eval[0] = x473;
9952 j0eval[1] =
9953 ((IKabs(((((-1.0) * sj2 * x472)) + ((cj2 * new_r01))))) +
9954 (IKabs(((((-1.0) * new_r01 * sj2)) + (((-1.0) * cj2 * x472))))));
9955 j0eval[2] = IKsign(x473);
9956 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
9957 IKabs(j0eval[1]) < 0.0000010000000000 ||
9958 IKabs(j0eval[2]) < 0.0000010000000000)
9959 {
9960 {
9961 IkReal j0eval[3];
9962 sj1 = 0;
9963 cj1 = -1.0;
9964 j1 = 3.14159265358979;
9965 IkReal x474 = (((new_r11 * sj2)) + ((cj2 * new_r01)));
9966 j0eval[0] = x474;
9967 j0eval[1] = IKsign(x474);
9968 j0eval[2] = ((IKabs(((((-1.0) * cj2 * sj2)) +
9969 (((-1.0) * new_r10 * new_r11))))) +
9970 (IKabs(((-1.0) + (cj2 * cj2) + ((new_r01 * new_r10))))));
9971 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
9972 IKabs(j0eval[1]) < 0.0000010000000000 ||
9973 IKabs(j0eval[2]) < 0.0000010000000000)
9974 {
9975 {
9976 IkReal evalcond[1];
9977 bool bgotonextstatement = true;
9978 do
9979 {
9980 IkReal x476 = ((new_r01 * new_r01) + (new_r11 * new_r11));
9981 if (IKabs(x476) == 0)
9982 {
9983 continue;
9984 }
9985 IkReal x475 = pow(x476, -0.5);
9986 CheckValue<IkReal> x477 = IKatan2WithCheck(
9987 IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
9988 if (!x477.valid)
9989 {
9990 continue;
9991 }
9992 IkReal gconst6 = ((-1.0) * (x477.value));
9993 IkReal gconst7 = ((-1.0) * new_r01 * x475);
9994 IkReal gconst8 = (new_r11 * x475);
9995 CheckValue<IkReal> x478 = IKatan2WithCheck(
9996 IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
9997 if (!x478.valid)
9998 {
9999 continue;
10000 }
10001 evalcond[0] =
10002 ((-3.14159265358979) +
10003 (IKfmod(((3.14159265358979) + (IKabs(((x478.value) + j2)))),
10004 6.28318530717959)));
10005 if (IKabs(evalcond[0]) < 0.0000050000000000)
10006 {
10007 bgotonextstatement = false;
10008 {
10009 IkReal j0eval[3];
10010 CheckValue<IkReal> x481 = IKatan2WithCheck(
10011 IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
10012 if (!x481.valid)
10013 {
10014 continue;
10015 }
10016 IkReal x479 = ((-1.0) * (x481.value));
10017 IkReal x480 = x475;
10018 sj1 = 0;
10019 cj1 = -1.0;
10020 j1 = 3.14159265358979;
10021 sj2 = gconst7;
10022 cj2 = gconst8;
10023 j2 = x479;
10024 IkReal gconst6 = x479;
10025 IkReal gconst7 = ((-1.0) * new_r01 * x480);
10026 IkReal gconst8 = (new_r11 * x480);
10027 IkReal x482 = new_r01 * new_r01;
10028 IkReal x483 = (new_r00 * new_r01);
10029 IkReal x484 = (((new_r10 * new_r11)) + x483);
10030 IkReal x485 = x475;
10031 IkReal x486 = (new_r01 * x485);
10032 j0eval[0] = x484;
10033 j0eval[1] = IKsign(x484);
10034 j0eval[2] = ((IKabs((((new_r10 * x486)) +
10035 (((-1.0) * x482 * x485))))) +
10036 (IKabs((((x483 * x485)) + ((new_r11 * x486))))));
10037 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
10038 IKabs(j0eval[1]) < 0.0000010000000000 ||
10039 IKabs(j0eval[2]) < 0.0000010000000000)
10040 {
10041 {
10042 IkReal j0eval[2];
10043 CheckValue<IkReal> x489 =
10044 IKatan2WithCheck(IkReal(new_r01),
10045 IkReal(new_r11),
10046 IKFAST_ATAN2_MAGTHRESH);
10047 if (!x489.valid)
10048 {
10049 continue;
10050 }
10051 IkReal x487 = ((-1.0) * (x489.value));
10052 IkReal x488 = x475;
10053 sj1 = 0;
10054 cj1 = -1.0;
10055 j1 = 3.14159265358979;
10056 sj2 = gconst7;
10057 cj2 = gconst8;
10058 j2 = x487;
10059 IkReal gconst6 = x487;
10060 IkReal gconst7 = ((-1.0) * new_r01 * x488);
10061 IkReal gconst8 = (new_r11 * x488);
10062 IkReal x490 = ((new_r01 * new_r01) + (new_r11 * new_r11));
10063 j0eval[0] = x490;
10064 j0eval[1] = IKsign(x490);
10065 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
10066 IKabs(j0eval[1]) < 0.0000010000000000)
10067 {
10068 {
10069 IkReal j0eval[1];
10070 CheckValue<IkReal> x493 =
10071 IKatan2WithCheck(IkReal(new_r01),
10072 IkReal(new_r11),
10073 IKFAST_ATAN2_MAGTHRESH);
10074 if (!x493.valid)
10075 {
10076 continue;
10077 }
10078 IkReal x491 = ((-1.0) * (x493.value));
10079 IkReal x492 = x475;
10080 sj1 = 0;
10081 cj1 = -1.0;
10082 j1 = 3.14159265358979;
10083 sj2 = gconst7;
10084 cj2 = gconst8;
10085 j2 = x491;
10086 IkReal gconst6 = x491;
10087 IkReal gconst7 = ((-1.0) * new_r01 * x492);
10088 IkReal gconst8 = (new_r11 * x492);
10089 IkReal x494 = new_r01 * new_r01;
10090 IkReal x495 = new_r11 * new_r11;
10091 IkReal x496 = ((1.0) * x494);
10092 CheckValue<IkReal> x502 =
10093 IKPowWithIntegerCheck((x494 + x495), -1);
10094 if (!x502.valid)
10095 {
10096 continue;
10097 }
10098 IkReal x497 = x502.value;
10099 CheckValue<IkReal> x503 = IKPowWithIntegerCheck(
10100 ((((-1.0) * x496)) + (((-1.0) * x495))), -1);
10101 if (!x503.valid)
10102 {
10103 continue;
10104 }
10105 IkReal x498 = x503.value;
10106 IkReal x499 = ((1.0) * x498);
10107 IkReal x500 = (new_r11 * x499);
10108 IkReal x501 = (new_r01 * x499);
10109 j0eval[0] =
10110 ((IKabs((((x494 * x495 * x497)) +
10111 (((-1.0) * x496 * x497)) +
10112 ((x497 * (x495 * x495)))))) +
10113 (IKabs(((((-1.0) * new_r01 * x500)) +
10114 (((-1.0) * x500 *
10115 (new_r01 * new_r01 * new_r01))) +
10116 (((-1.0) * new_r01 * x500 *
10117 (new_r11 * new_r11)))))));
10118 if (IKabs(j0eval[0]) < 0.0000010000000000)
10119 {
10120 {
10121 IkReal evalcond[3];
10122 bool bgotonextstatement = true;
10123 do
10124 {
10125 evalcond[0] =
10126 ((IKabs(new_r11)) + (IKabs(new_r00)));
10127 if (IKabs(evalcond[0]) < 0.0000050000000000)
10128 {
10129 bgotonextstatement = false;
10130 {
10131 IkReal j0eval[1];
10132 CheckValue<IkReal> x505 = IKatan2WithCheck(
10133 IkReal(new_r01),
10134 IkReal(0),
10135 IKFAST_ATAN2_MAGTHRESH);
10136 if (!x505.valid)
10137 {
10138 continue;
10139 }
10140 IkReal x504 = ((-1.0) * (x505.value));
10141 sj1 = 0;
10142 cj1 = -1.0;
10143 j1 = 3.14159265358979;
10144 sj2 = gconst7;
10145 cj2 = gconst8;
10146 j2 = x504;
10147 new_r11 = 0;
10148 new_r00 = 0;
10149 IkReal gconst6 = x504;
10150 IkReal x506 = new_r01 * new_r01;
10151 if (IKabs(x506) == 0)
10152 {
10153 continue;
10154 }
10155 IkReal gconst7 =
10156 ((-1.0) * new_r01 * (pow(x506, -0.5)));
10157 IkReal gconst8 = 0;
10158 j0eval[0] = new_r10;
10159 if (IKabs(j0eval[0]) < 0.0000010000000000)
10160 {
10161 {
10162 IkReal j0array[2], cj0array[2],
10163 sj0array[2];
10164 bool j0valid[2] = { false };
10165 _nj0 = 2;
10166 CheckValue<IkReal> x507 =
10167 IKPowWithIntegerCheck(gconst7, -1);
10168 if (!x507.valid)
10169 {
10170 continue;
10171 }
10172 cj0array[0] =
10173 ((-1.0) * new_r10 * (x507.value));
10174 if (cj0array[0] >=
10175 -1 - IKFAST_SINCOS_THRESH &&
10176 cj0array[0] <=
10177 1 + IKFAST_SINCOS_THRESH)
10178 {
10179 j0valid[0] = j0valid[1] = true;
10180 j0array[0] = IKacos(cj0array[0]);
10181 sj0array[0] = IKsin(j0array[0]);
10182 cj0array[1] = cj0array[0];
10183 j0array[1] = -j0array[0];
10184 sj0array[1] = -sj0array[0];
10185 }
10186 else if (isnan(cj0array[0]))
10187 {
10188 // probably any value will work
10189 j0valid[0] = true;
10190 cj0array[0] = 1;
10191 sj0array[0] = 0;
10192 j0array[0] = 0;
10193 }
10194 for (int ij0 = 0; ij0 < 2; ++ij0)
10195 {
10196 if (!j0valid[ij0])
10197 {
10198 continue;
10199 }
10200 _ij0[0] = ij0;
10201 _ij0[1] = -1;
10202 for (int iij0 = ij0 + 1; iij0 < 2;
10203 ++iij0)
10204 {
10205 if (j0valid[iij0] &&
10206 IKabs(cj0array[ij0] -
10207 cj0array[iij0]) <
10208 IKFAST_SOLUTION_THRESH &&
10209 IKabs(sj0array[ij0] -
10210 sj0array[iij0]) <
10211 IKFAST_SOLUTION_THRESH)
10212 {
10213 j0valid[iij0] = false;
10214 _ij0[1] = iij0;
10215 break;
10216 }
10217 }
10218 j0 = j0array[ij0];
10219 cj0 = cj0array[ij0];
10220 sj0 = sj0array[ij0];
10221 {
10222 IkReal evalcond[6];
10223 IkReal x508 = IKsin(j0);
10224 IkReal x509 = IKcos(j0);
10225 IkReal x510 = ((-1.0) * x508);
10226 evalcond[0] = (new_r10 * x508);
10227 evalcond[1] = (new_r01 * x510);
10228 evalcond[2] = (gconst7 * x510);
10229 evalcond[3] =
10230 (gconst7 + ((new_r10 * x509)));
10231 evalcond[4] =
10232 (gconst7 + ((new_r01 * x509)));
10233 evalcond[5] =
10234 (((gconst7 * x509)) + new_r01);
10235 if (IKabs(evalcond[0]) >
10236 IKFAST_EVALCOND_THRESH ||
10237 IKabs(evalcond[1]) >
10238 IKFAST_EVALCOND_THRESH ||
10239 IKabs(evalcond[2]) >
10240 IKFAST_EVALCOND_THRESH ||
10241 IKabs(evalcond[3]) >
10242 IKFAST_EVALCOND_THRESH ||
10243 IKabs(evalcond[4]) >
10244 IKFAST_EVALCOND_THRESH ||
10245 IKabs(evalcond[5]) >
10246 IKFAST_EVALCOND_THRESH)
10247 {
10248 continue;
10249 }
10250 }
10251
10252 {
10253 std::vector<IkSingleDOFSolutionBase<
10254 IkReal> >
10255 vinfos(7);
10256 vinfos[0].jointtype = 1;
10257 vinfos[0].foffset = j0;
10258 vinfos[0].indices[0] = _ij0[0];
10259 vinfos[0].indices[1] = _ij0[1];
10260 vinfos[0].maxsolutions = _nj0;
10261 vinfos[1].jointtype = 1;
10262 vinfos[1].foffset = j1;
10263 vinfos[1].indices[0] = _ij1[0];
10264 vinfos[1].indices[1] = _ij1[1];
10265 vinfos[1].maxsolutions = _nj1;
10266 vinfos[2].jointtype = 1;
10267 vinfos[2].foffset = j2;
10268 vinfos[2].indices[0] = _ij2[0];
10269 vinfos[2].indices[1] = _ij2[1];
10270 vinfos[2].maxsolutions = _nj2;
10271 vinfos[3].jointtype = 1;
10272 vinfos[3].foffset = j3;
10273 vinfos[3].indices[0] = _ij3[0];
10274 vinfos[3].indices[1] = _ij3[1];
10275 vinfos[3].maxsolutions = _nj3;
10276 vinfos[4].jointtype = 1;
10277 vinfos[4].foffset = j4;
10278 vinfos[4].indices[0] = _ij4[0];
10279 vinfos[4].indices[1] = _ij4[1];
10280 vinfos[4].maxsolutions = _nj4;
10281 vinfos[5].jointtype = 1;
10282 vinfos[5].foffset = j5;
10283 vinfos[5].indices[0] = _ij5[0];
10284 vinfos[5].indices[1] = _ij5[1];
10285 vinfos[5].maxsolutions = _nj5;
10286 vinfos[6].jointtype = 1;
10287 vinfos[6].foffset = j6;
10288 vinfos[6].indices[0] = _ij6[0];
10289 vinfos[6].indices[1] = _ij6[1];
10290 vinfos[6].maxsolutions = _nj6;
10291 std::vector<int> vfree(0);
10292 solutions.AddSolution(vinfos,
10293 vfree);
10294 }
10295 }
10296 }
10297 }
10298 else
10299 {
10300 {
10301 IkReal j0array[2], cj0array[2],
10302 sj0array[2];
10303 bool j0valid[2] = { false };
10304 _nj0 = 2;
10305 CheckValue<IkReal> x511 =
10306 IKPowWithIntegerCheck(new_r10, -1);
10307 if (!x511.valid)
10308 {
10309 continue;
10310 }
10311 cj0array[0] =
10312 ((-1.0) * gconst7 * (x511.value));
10313 if (cj0array[0] >=
10314 -1 - IKFAST_SINCOS_THRESH &&
10315 cj0array[0] <=
10316 1 + IKFAST_SINCOS_THRESH)
10317 {
10318 j0valid[0] = j0valid[1] = true;
10319 j0array[0] = IKacos(cj0array[0]);
10320 sj0array[0] = IKsin(j0array[0]);
10321 cj0array[1] = cj0array[0];
10322 j0array[1] = -j0array[0];
10323 sj0array[1] = -sj0array[0];
10324 }
10325 else if (isnan(cj0array[0]))
10326 {
10327 // probably any value will work
10328 j0valid[0] = true;
10329 cj0array[0] = 1;
10330 sj0array[0] = 0;
10331 j0array[0] = 0;
10332 }
10333 for (int ij0 = 0; ij0 < 2; ++ij0)
10334 {
10335 if (!j0valid[ij0])
10336 {
10337 continue;
10338 }
10339 _ij0[0] = ij0;
10340 _ij0[1] = -1;
10341 for (int iij0 = ij0 + 1; iij0 < 2;
10342 ++iij0)
10343 {
10344 if (j0valid[iij0] &&
10345 IKabs(cj0array[ij0] -
10346 cj0array[iij0]) <
10347 IKFAST_SOLUTION_THRESH &&
10348 IKabs(sj0array[ij0] -
10349 sj0array[iij0]) <
10350 IKFAST_SOLUTION_THRESH)
10351 {
10352 j0valid[iij0] = false;
10353 _ij0[1] = iij0;
10354 break;
10355 }
10356 }
10357 j0 = j0array[ij0];
10358 cj0 = cj0array[ij0];
10359 sj0 = sj0array[ij0];
10360 {
10361 IkReal evalcond[6];
10362 IkReal x512 = IKsin(j0);
10363 IkReal x513 = IKcos(j0);
10364 IkReal x514 = (gconst7 * x513);
10365 IkReal x515 = ((-1.0) * x512);
10366 evalcond[0] = (new_r10 * x512);
10367 evalcond[1] = (new_r01 * x515);
10368 evalcond[2] = (gconst7 * x515);
10369 evalcond[3] = (x514 + new_r10);
10370 evalcond[4] =
10371 (((new_r01 * x513)) + gconst7);
10372 evalcond[5] = (x514 + new_r01);
10373 if (IKabs(evalcond[0]) >
10374 IKFAST_EVALCOND_THRESH ||
10375 IKabs(evalcond[1]) >
10376 IKFAST_EVALCOND_THRESH ||
10377 IKabs(evalcond[2]) >
10378 IKFAST_EVALCOND_THRESH ||
10379 IKabs(evalcond[3]) >
10380 IKFAST_EVALCOND_THRESH ||
10381 IKabs(evalcond[4]) >
10382 IKFAST_EVALCOND_THRESH ||
10383 IKabs(evalcond[5]) >
10384 IKFAST_EVALCOND_THRESH)
10385 {
10386 continue;
10387 }
10388 }
10389
10390 {
10391 std::vector<IkSingleDOFSolutionBase<
10392 IkReal> >
10393 vinfos(7);
10394 vinfos[0].jointtype = 1;
10395 vinfos[0].foffset = j0;
10396 vinfos[0].indices[0] = _ij0[0];
10397 vinfos[0].indices[1] = _ij0[1];
10398 vinfos[0].maxsolutions = _nj0;
10399 vinfos[1].jointtype = 1;
10400 vinfos[1].foffset = j1;
10401 vinfos[1].indices[0] = _ij1[0];
10402 vinfos[1].indices[1] = _ij1[1];
10403 vinfos[1].maxsolutions = _nj1;
10404 vinfos[2].jointtype = 1;
10405 vinfos[2].foffset = j2;
10406 vinfos[2].indices[0] = _ij2[0];
10407 vinfos[2].indices[1] = _ij2[1];
10408 vinfos[2].maxsolutions = _nj2;
10409 vinfos[3].jointtype = 1;
10410 vinfos[3].foffset = j3;
10411 vinfos[3].indices[0] = _ij3[0];
10412 vinfos[3].indices[1] = _ij3[1];
10413 vinfos[3].maxsolutions = _nj3;
10414 vinfos[4].jointtype = 1;
10415 vinfos[4].foffset = j4;
10416 vinfos[4].indices[0] = _ij4[0];
10417 vinfos[4].indices[1] = _ij4[1];
10418 vinfos[4].maxsolutions = _nj4;
10419 vinfos[5].jointtype = 1;
10420 vinfos[5].foffset = j5;
10421 vinfos[5].indices[0] = _ij5[0];
10422 vinfos[5].indices[1] = _ij5[1];
10423 vinfos[5].maxsolutions = _nj5;
10424 vinfos[6].jointtype = 1;
10425 vinfos[6].foffset = j6;
10426 vinfos[6].indices[0] = _ij6[0];
10427 vinfos[6].indices[1] = _ij6[1];
10428 vinfos[6].maxsolutions = _nj6;
10429 std::vector<int> vfree(0);
10430 solutions.AddSolution(vinfos,
10431 vfree);
10432 }
10433 }
10434 }
10435 }
10436 }
10437 }
10438 } while (0);
10439 if (bgotonextstatement)
10440 {
10441 bool bgotonextstatement = true;
10442 do
10443 {
10444 evalcond[0] =
10445 ((IKabs(new_r10)) + (IKabs(new_r00)));
10446 evalcond[1] = gconst7;
10447 evalcond[2] = gconst8;
10448 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
10449 IKabs(evalcond[1]) < 0.0000050000000000 &&
10450 IKabs(evalcond[2]) < 0.0000050000000000)
10451 {
10452 bgotonextstatement = false;
10453 {
10454 IkReal j0eval[3];
10455 CheckValue<IkReal> x517 =
10456 IKatan2WithCheck(
10457 IkReal(new_r01),
10458 IkReal(new_r11),
10459 IKFAST_ATAN2_MAGTHRESH);
10460 if (!x517.valid)
10461 {
10462 continue;
10463 }
10464 IkReal x516 = ((-1.0) * (x517.value));
10465 sj1 = 0;
10466 cj1 = -1.0;
10467 j1 = 3.14159265358979;
10468 sj2 = gconst7;
10469 cj2 = gconst8;
10470 j2 = x516;
10471 new_r00 = 0;
10472 new_r10 = 0;
10473 new_r21 = 0;
10474 new_r22 = 0;
10475 IkReal gconst6 = x516;
10476 IkReal gconst7 = ((-1.0) * new_r01);
10477 IkReal gconst8 = new_r11;
10478 j0eval[0] = -1.0;
10479 j0eval[1] =
10480 ((IKabs((new_r01 * new_r11))) +
10481 (IKabs(((1.0) +
10482 (((-1.0) *
10483 (new_r01 * new_r01)))))));
10484 j0eval[2] = -1.0;
10485 if (IKabs(j0eval[0]) <
10486 0.0000010000000000 ||
10487 IKabs(j0eval[1]) <
10488 0.0000010000000000 ||
10489 IKabs(j0eval[2]) < 0.0000010000000000)
10490 {
10491 {
10492 IkReal j0eval[3];
10493 CheckValue<IkReal> x519 =
10494 IKatan2WithCheck(
10495 IkReal(new_r01),
10496 IkReal(new_r11),
10497 IKFAST_ATAN2_MAGTHRESH);
10498 if (!x519.valid)
10499 {
10500 continue;
10501 }
10502 IkReal x518 = ((-1.0) * (x519.value));
10503 sj1 = 0;
10504 cj1 = -1.0;
10505 j1 = 3.14159265358979;
10506 sj2 = gconst7;
10507 cj2 = gconst8;
10508 j2 = x518;
10509 new_r00 = 0;
10510 new_r10 = 0;
10511 new_r21 = 0;
10512 new_r22 = 0;
10513 IkReal gconst6 = x518;
10514 IkReal gconst7 = ((-1.0) * new_r01);
10515 IkReal gconst8 = new_r11;
10516 j0eval[0] = -1.0;
10517 j0eval[1] =
10518 ((IKabs((new_r01 * new_r11))) +
10519 (IKabs(
10520 ((1.0) +
10521 (((-1.0) *
10522 (new_r01 * new_r01)))))));
10523 j0eval[2] = -1.0;
10524 if (IKabs(j0eval[0]) <
10525 0.0000010000000000 ||
10526 IKabs(j0eval[1]) <
10527 0.0000010000000000 ||
10528 IKabs(j0eval[2]) <
10529 0.0000010000000000)
10530 {
10531 {
10532 IkReal j0eval[3];
10533 CheckValue<IkReal> x521 =
10534 IKatan2WithCheck(
10535 IkReal(new_r01),
10536 IkReal(new_r11),
10537 IKFAST_ATAN2_MAGTHRESH);
10538 if (!x521.valid)
10539 {
10540 continue;
10541 }
10542 IkReal x520 =
10543 ((-1.0) * (x521.value));
10544 sj1 = 0;
10545 cj1 = -1.0;
10546 j1 = 3.14159265358979;
10547 sj2 = gconst7;
10548 cj2 = gconst8;
10549 j2 = x520;
10550 new_r00 = 0;
10551 new_r10 = 0;
10552 new_r21 = 0;
10553 new_r22 = 0;
10554 IkReal gconst6 = x520;
10555 IkReal gconst7 =
10556 ((-1.0) * new_r01);
10557 IkReal gconst8 = new_r11;
10558 j0eval[0] = 1.0;
10559 j0eval[1] =
10560 ((((0.5) *
10561 (IKabs(
10562 ((-1.0) +
10563 (((2.0) *
10564 (new_r01 *
10565 new_r01)))))))) +
10566 (IKabs(
10567 (new_r01 * new_r11))));
10568 j0eval[2] = 1.0;
10569 if (IKabs(j0eval[0]) <
10570 0.0000010000000000 ||
10571 IKabs(j0eval[1]) <
10572 0.0000010000000000 ||
10573 IKabs(j0eval[2]) <
10574 0.0000010000000000)
10575 {
10576 continue; // 3 cases reached
10577 }
10578 else
10579 {
10580 {
10581 IkReal j0array[1],
10582 cj0array[1], sj0array[1];
10583 bool j0valid[1] = { false };
10584 _nj0 = 1;
10585 IkReal x522 =
10586 ((1.0) * new_r11);
10587 CheckValue<IkReal> x523 =
10588 IKatan2WithCheck(
10589 IkReal((((gconst8 *
10590 new_r01)) +
10591 (((-1.0) *
10592 gconst7 *
10593 x522)))),
10594 IkReal(((((-1.0) *
10595 gconst8 *
10596 x522)) +
10597 (((-1.0) *
10598 gconst7 *
10599 new_r01)))),
10600 IKFAST_ATAN2_MAGTHRESH);
10601 if (!x523.valid)
10602 {
10603 continue;
10604 }
10605 CheckValue<IkReal> x524 =
10606 IKPowWithIntegerCheck(
10607 IKsign(((new_r01 *
10608 new_r01) +
10609 (new_r11 *
10610 new_r11))),
10611 -1);
10612 if (!x524.valid)
10613 {
10614 continue;
10615 }
10616 j0array[0] =
10617 ((-1.5707963267949) +
10618 (x523.value) +
10619 (((1.5707963267949) *
10620 (x524.value))));
10621 sj0array[0] =
10622 IKsin(j0array[0]);
10623 cj0array[0] =
10624 IKcos(j0array[0]);
10625 if (j0array[0] > IKPI)
10626 {
10627 j0array[0] -= IK2PI;
10628 }
10629 else if (j0array[0] < -IKPI)
10630 {
10631 j0array[0] += IK2PI;
10632 }
10633 j0valid[0] = true;
10634 for (int ij0 = 0; ij0 < 1;
10635 ++ij0)
10636 {
10637 if (!j0valid[ij0])
10638 {
10639 continue;
10640 }
10641 _ij0[0] = ij0;
10642 _ij0[1] = -1;
10643 for (int iij0 = ij0 + 1;
10644 iij0 < 1;
10645 ++iij0)
10646 {
10647 if (j0valid[iij0] &&
10648 IKabs(
10649 cj0array[ij0] -
10650 cj0array[iij0]) <
10651 IKFAST_SOLUTION_THRESH &&
10652 IKabs(
10653 sj0array[ij0] -
10654 sj0array[iij0]) <
10655 IKFAST_SOLUTION_THRESH)
10656 {
10657 j0valid[iij0] = false;
10658 _ij0[1] = iij0;
10659 break;
10660 }
10661 }
10662 j0 = j0array[ij0];
10663 cj0 = cj0array[ij0];
10664 sj0 = sj0array[ij0];
10665 {
10666 IkReal evalcond[6];
10667 IkReal x525 = IKsin(j0);
10668 IkReal x526 = IKcos(j0);
10669 IkReal x527 =
10670 (gconst7 * x526);
10671 IkReal x528 =
10672 ((1.0) * x525);
10673 IkReal x529 =
10674 (gconst8 * x526);
10675 IkReal x530 =
10676 (gconst8 * x528);
10677 evalcond[0] =
10678 ((((-1.0) * x530)) +
10679 x527);
10680 evalcond[1] =
10681 (((new_r01 * x526)) +
10682 gconst7 +
10683 ((new_r11 * x525)));
10684 evalcond[2] =
10685 (((gconst7 * x525)) +
10686 x529 + new_r11);
10687 evalcond[3] =
10688 (gconst8 +
10689 ((new_r11 * x526)) +
10690 (((-1.0) * new_r01 *
10691 x528)));
10692 evalcond[4] =
10693 ((((-1.0) * x529)) +
10694 (((-1.0) * gconst7 *
10695 x528)));
10696 evalcond[5] =
10697 ((((-1.0) * x530)) +
10698 x527 + new_r01);
10699 if (IKabs(evalcond[0]) >
10700 IKFAST_EVALCOND_THRESH ||
10701 IKabs(evalcond[1]) >
10702 IKFAST_EVALCOND_THRESH ||
10703 IKabs(evalcond[2]) >
10704 IKFAST_EVALCOND_THRESH ||
10705 IKabs(evalcond[3]) >
10706 IKFAST_EVALCOND_THRESH ||
10707 IKabs(evalcond[4]) >
10708 IKFAST_EVALCOND_THRESH ||
10709 IKabs(evalcond[5]) >
10710 IKFAST_EVALCOND_THRESH)
10711 {
10712 continue;
10713 }
10714 }
10715
10716 {
10717 std::vector<
10718 IkSingleDOFSolutionBase<
10719 IkReal> >
10720 vinfos(7);
10721 vinfos[0].jointtype = 1;
10722 vinfos[0].foffset = j0;
10723 vinfos[0].indices[0] =
10724 _ij0[0];
10725 vinfos[0].indices[1] =
10726 _ij0[1];
10727 vinfos[0].maxsolutions =
10728 _nj0;
10729 vinfos[1].jointtype = 1;
10730 vinfos[1].foffset = j1;
10731 vinfos[1].indices[0] =
10732 _ij1[0];
10733 vinfos[1].indices[1] =
10734 _ij1[1];
10735 vinfos[1].maxsolutions =
10736 _nj1;
10737 vinfos[2].jointtype = 1;
10738 vinfos[2].foffset = j2;
10739 vinfos[2].indices[0] =
10740 _ij2[0];
10741 vinfos[2].indices[1] =
10742 _ij2[1];
10743 vinfos[2].maxsolutions =
10744 _nj2;
10745 vinfos[3].jointtype = 1;
10746 vinfos[3].foffset = j3;
10747 vinfos[3].indices[0] =
10748 _ij3[0];
10749 vinfos[3].indices[1] =
10750 _ij3[1];
10751 vinfos[3].maxsolutions =
10752 _nj3;
10753 vinfos[4].jointtype = 1;
10754 vinfos[4].foffset = j4;
10755 vinfos[4].indices[0] =
10756 _ij4[0];
10757 vinfos[4].indices[1] =
10758 _ij4[1];
10759 vinfos[4].maxsolutions =
10760 _nj4;
10761 vinfos[5].jointtype = 1;
10762 vinfos[5].foffset = j5;
10763 vinfos[5].indices[0] =
10764 _ij5[0];
10765 vinfos[5].indices[1] =
10766 _ij5[1];
10767 vinfos[5].maxsolutions =
10768 _nj5;
10769 vinfos[6].jointtype = 1;
10770 vinfos[6].foffset = j6;
10771 vinfos[6].indices[0] =
10772 _ij6[0];
10773 vinfos[6].indices[1] =
10774 _ij6[1];
10775 vinfos[6].maxsolutions =
10776 _nj6;
10777 std::vector<int> vfree(0);
10778 solutions.AddSolution(
10779 vinfos, vfree);
10780 }
10781 }
10782 }
10783 }
10784 }
10785 }
10786 else
10787 {
10788 {
10789 IkReal j0array[1], cj0array[1],
10790 sj0array[1];
10791 bool j0valid[1] = { false };
10792 _nj0 = 1;
10793 CheckValue<IkReal> x531 =
10794 IKatan2WithCheck(
10795 IkReal(
10796 (gconst7 * new_r11)),
10797 IkReal(
10798 (gconst8 * new_r11)),
10799 IKFAST_ATAN2_MAGTHRESH);
10800 if (!x531.valid)
10801 {
10802 continue;
10803 }
10804 CheckValue<IkReal> x532 =
10805 IKPowWithIntegerCheck(
10806 IKsign(((((-1.0) *
10807 (gconst8 *
10808 gconst8))) +
10809 (((-1.0) *
10810 (gconst7 *
10811 gconst7))))),
10812 -1);
10813 if (!x532.valid)
10814 {
10815 continue;
10816 }
10817 j0array[0] =
10818 ((-1.5707963267949) +
10819 (x531.value) +
10820 (((1.5707963267949) *
10821 (x532.value))));
10822 sj0array[0] = IKsin(j0array[0]);
10823 cj0array[0] = IKcos(j0array[0]);
10824 if (j0array[0] > IKPI)
10825 {
10826 j0array[0] -= IK2PI;
10827 }
10828 else if (j0array[0] < -IKPI)
10829 {
10830 j0array[0] += IK2PI;
10831 }
10832 j0valid[0] = true;
10833 for (int ij0 = 0; ij0 < 1; ++ij0)
10834 {
10835 if (!j0valid[ij0])
10836 {
10837 continue;
10838 }
10839 _ij0[0] = ij0;
10840 _ij0[1] = -1;
10841 for (int iij0 = ij0 + 1;
10842 iij0 < 1;
10843 ++iij0)
10844 {
10845 if (j0valid[iij0] &&
10846 IKabs(cj0array[ij0] -
10847 cj0array[iij0]) <
10848 IKFAST_SOLUTION_THRESH &&
10849 IKabs(sj0array[ij0] -
10850 sj0array[iij0]) <
10851 IKFAST_SOLUTION_THRESH)
10852 {
10853 j0valid[iij0] = false;
10854 _ij0[1] = iij0;
10855 break;
10856 }
10857 }
10858 j0 = j0array[ij0];
10859 cj0 = cj0array[ij0];
10860 sj0 = sj0array[ij0];
10861 {
10862 IkReal evalcond[6];
10863 IkReal x533 = IKsin(j0);
10864 IkReal x534 = IKcos(j0);
10865 IkReal x535 =
10866 (gconst7 * x534);
10867 IkReal x536 = ((1.0) * x533);
10868 IkReal x537 =
10869 (gconst8 * x534);
10870 IkReal x538 =
10871 (gconst8 * x536);
10872 evalcond[0] =
10873 ((((-1.0) * x538)) +
10874 x535);
10875 evalcond[1] =
10876 (((new_r01 * x534)) +
10877 gconst7 +
10878 ((new_r11 * x533)));
10879 evalcond[2] =
10880 (((gconst7 * x533)) +
10881 x537 + new_r11);
10882 evalcond[3] =
10883 (gconst8 +
10884 ((new_r11 * x534)) +
10885 (((-1.0) * new_r01 *
10886 x536)));
10887 evalcond[4] =
10888 ((((-1.0) * x537)) +
10889 (((-1.0) * gconst7 *
10890 x536)));
10891 evalcond[5] =
10892 ((((-1.0) * x538)) +
10893 x535 + new_r01);
10894 if (IKabs(evalcond[0]) >
10895 IKFAST_EVALCOND_THRESH ||
10896 IKabs(evalcond[1]) >
10897 IKFAST_EVALCOND_THRESH ||
10898 IKabs(evalcond[2]) >
10899 IKFAST_EVALCOND_THRESH ||
10900 IKabs(evalcond[3]) >
10901 IKFAST_EVALCOND_THRESH ||
10902 IKabs(evalcond[4]) >
10903 IKFAST_EVALCOND_THRESH ||
10904 IKabs(evalcond[5]) >
10905 IKFAST_EVALCOND_THRESH)
10906 {
10907 continue;
10908 }
10909 }
10910
10911 {
10912 std::vector<
10913 IkSingleDOFSolutionBase<
10914 IkReal> >
10915 vinfos(7);
10916 vinfos[0].jointtype = 1;
10917 vinfos[0].foffset = j0;
10918 vinfos[0].indices[0] =
10919 _ij0[0];
10920 vinfos[0].indices[1] =
10921 _ij0[1];
10922 vinfos[0].maxsolutions = _nj0;
10923 vinfos[1].jointtype = 1;
10924 vinfos[1].foffset = j1;
10925 vinfos[1].indices[0] =
10926 _ij1[0];
10927 vinfos[1].indices[1] =
10928 _ij1[1];
10929 vinfos[1].maxsolutions = _nj1;
10930 vinfos[2].jointtype = 1;
10931 vinfos[2].foffset = j2;
10932 vinfos[2].indices[0] =
10933 _ij2[0];
10934 vinfos[2].indices[1] =
10935 _ij2[1];
10936 vinfos[2].maxsolutions = _nj2;
10937 vinfos[3].jointtype = 1;
10938 vinfos[3].foffset = j3;
10939 vinfos[3].indices[0] =
10940 _ij3[0];
10941 vinfos[3].indices[1] =
10942 _ij3[1];
10943 vinfos[3].maxsolutions = _nj3;
10944 vinfos[4].jointtype = 1;
10945 vinfos[4].foffset = j4;
10946 vinfos[4].indices[0] =
10947 _ij4[0];
10948 vinfos[4].indices[1] =
10949 _ij4[1];
10950 vinfos[4].maxsolutions = _nj4;
10951 vinfos[5].jointtype = 1;
10952 vinfos[5].foffset = j5;
10953 vinfos[5].indices[0] =
10954 _ij5[0];
10955 vinfos[5].indices[1] =
10956 _ij5[1];
10957 vinfos[5].maxsolutions = _nj5;
10958 vinfos[6].jointtype = 1;
10959 vinfos[6].foffset = j6;
10960 vinfos[6].indices[0] =
10961 _ij6[0];
10962 vinfos[6].indices[1] =
10963 _ij6[1];
10964 vinfos[6].maxsolutions = _nj6;
10965 std::vector<int> vfree(0);
10966 solutions.AddSolution(vinfos,
10967 vfree);
10968 }
10969 }
10970 }
10971 }
10972 }
10973 }
10974 else
10975 {
10976 {
10977 IkReal j0array[1], cj0array[1],
10978 sj0array[1];
10979 bool j0valid[1] = { false };
10980 _nj0 = 1;
10981 CheckValue<IkReal> x539 =
10982 IKatan2WithCheck(
10983 IkReal((gconst7 * gconst8)),
10984 IkReal(gconst8 * gconst8),
10985 IKFAST_ATAN2_MAGTHRESH);
10986 if (!x539.valid)
10987 {
10988 continue;
10989 }
10990 CheckValue<IkReal> x540 =
10991 IKPowWithIntegerCheck(
10992 IKsign(
10993 ((((-1.0) * gconst8 *
10994 new_r11)) +
10995 ((gconst7 * new_r01)))),
10996 -1);
10997 if (!x540.valid)
10998 {
10999 continue;
11000 }
11001 j0array[0] = ((-1.5707963267949) +
11002 (x539.value) +
11003 (((1.5707963267949) *
11004 (x540.value))));
11005 sj0array[0] = IKsin(j0array[0]);
11006 cj0array[0] = IKcos(j0array[0]);
11007 if (j0array[0] > IKPI)
11008 {
11009 j0array[0] -= IK2PI;
11010 }
11011 else if (j0array[0] < -IKPI)
11012 {
11013 j0array[0] += IK2PI;
11014 }
11015 j0valid[0] = true;
11016 for (int ij0 = 0; ij0 < 1; ++ij0)
11017 {
11018 if (!j0valid[ij0])
11019 {
11020 continue;
11021 }
11022 _ij0[0] = ij0;
11023 _ij0[1] = -1;
11024 for (int iij0 = ij0 + 1; iij0 < 1;
11025 ++iij0)
11026 {
11027 if (j0valid[iij0] &&
11028 IKabs(cj0array[ij0] -
11029 cj0array[iij0]) <
11030 IKFAST_SOLUTION_THRESH &&
11031 IKabs(sj0array[ij0] -
11032 sj0array[iij0]) <
11033 IKFAST_SOLUTION_THRESH)
11034 {
11035 j0valid[iij0] = false;
11036 _ij0[1] = iij0;
11037 break;
11038 }
11039 }
11040 j0 = j0array[ij0];
11041 cj0 = cj0array[ij0];
11042 sj0 = sj0array[ij0];
11043 {
11044 IkReal evalcond[6];
11045 IkReal x541 = IKsin(j0);
11046 IkReal x542 = IKcos(j0);
11047 IkReal x543 = (gconst7 * x542);
11048 IkReal x544 = ((1.0) * x541);
11049 IkReal x545 = (gconst8 * x542);
11050 IkReal x546 = (gconst8 * x544);
11051 evalcond[0] =
11052 ((((-1.0) * x546)) + x543);
11053 evalcond[1] =
11054 (gconst7 +
11055 ((new_r11 * x541)) +
11056 ((new_r01 * x542)));
11057 evalcond[2] =
11058 (x545 + ((gconst7 * x541)) +
11059 new_r11);
11060 evalcond[3] =
11061 (gconst8 +
11062 ((new_r11 * x542)) +
11063 (((-1.0) * new_r01 * x544)));
11064 evalcond[4] =
11065 ((((-1.0) * x545)) +
11066 (((-1.0) * gconst7 * x544)));
11067 evalcond[5] = ((((-1.0) * x546)) +
11068 x543 + new_r01);
11069 if (IKabs(evalcond[0]) >
11070 IKFAST_EVALCOND_THRESH ||
11071 IKabs(evalcond[1]) >
11072 IKFAST_EVALCOND_THRESH ||
11073 IKabs(evalcond[2]) >
11074 IKFAST_EVALCOND_THRESH ||
11075 IKabs(evalcond[3]) >
11076 IKFAST_EVALCOND_THRESH ||
11077 IKabs(evalcond[4]) >
11078 IKFAST_EVALCOND_THRESH ||
11079 IKabs(evalcond[5]) >
11080 IKFAST_EVALCOND_THRESH)
11081 {
11082 continue;
11083 }
11084 }
11085
11086 {
11087 std::vector<
11088 IkSingleDOFSolutionBase<
11089 IkReal> >
11090 vinfos(7);
11091 vinfos[0].jointtype = 1;
11092 vinfos[0].foffset = j0;
11093 vinfos[0].indices[0] = _ij0[0];
11094 vinfos[0].indices[1] = _ij0[1];
11095 vinfos[0].maxsolutions = _nj0;
11096 vinfos[1].jointtype = 1;
11097 vinfos[1].foffset = j1;
11098 vinfos[1].indices[0] = _ij1[0];
11099 vinfos[1].indices[1] = _ij1[1];
11100 vinfos[1].maxsolutions = _nj1;
11101 vinfos[2].jointtype = 1;
11102 vinfos[2].foffset = j2;
11103 vinfos[2].indices[0] = _ij2[0];
11104 vinfos[2].indices[1] = _ij2[1];
11105 vinfos[2].maxsolutions = _nj2;
11106 vinfos[3].jointtype = 1;
11107 vinfos[3].foffset = j3;
11108 vinfos[3].indices[0] = _ij3[0];
11109 vinfos[3].indices[1] = _ij3[1];
11110 vinfos[3].maxsolutions = _nj3;
11111 vinfos[4].jointtype = 1;
11112 vinfos[4].foffset = j4;
11113 vinfos[4].indices[0] = _ij4[0];
11114 vinfos[4].indices[1] = _ij4[1];
11115 vinfos[4].maxsolutions = _nj4;
11116 vinfos[5].jointtype = 1;
11117 vinfos[5].foffset = j5;
11118 vinfos[5].indices[0] = _ij5[0];
11119 vinfos[5].indices[1] = _ij5[1];
11120 vinfos[5].maxsolutions = _nj5;
11121 vinfos[6].jointtype = 1;
11122 vinfos[6].foffset = j6;
11123 vinfos[6].indices[0] = _ij6[0];
11124 vinfos[6].indices[1] = _ij6[1];
11125 vinfos[6].maxsolutions = _nj6;
11126 std::vector<int> vfree(0);
11127 solutions.AddSolution(vinfos,
11128 vfree);
11129 }
11130 }
11131 }
11132 }
11133 }
11134 }
11135 } while (0);
11136 if (bgotonextstatement)
11137 {
11138 bool bgotonextstatement = true;
11139 do
11140 {
11141 evalcond[0] =
11142 ((IKabs(new_r10)) + (IKabs(new_r01)));
11143 if (IKabs(evalcond[0]) < 0.0000050000000000)
11144 {
11145 bgotonextstatement = false;
11146 {
11147 IkReal j0array[2], cj0array[2],
11148 sj0array[2];
11149 bool j0valid[2] = { false };
11150 _nj0 = 2;
11151 CheckValue<IkReal> x547 =
11152 IKPowWithIntegerCheck(gconst8, -1);
11153 if (!x547.valid)
11154 {
11155 continue;
11156 }
11157 cj0array[0] = (new_r00 * (x547.value));
11158 if (cj0array[0] >=
11159 -1 - IKFAST_SINCOS_THRESH &&
11160 cj0array[0] <=
11161 1 + IKFAST_SINCOS_THRESH)
11162 {
11163 j0valid[0] = j0valid[1] = true;
11164 j0array[0] = IKacos(cj0array[0]);
11165 sj0array[0] = IKsin(j0array[0]);
11166 cj0array[1] = cj0array[0];
11167 j0array[1] = -j0array[0];
11168 sj0array[1] = -sj0array[0];
11169 }
11170 else if (isnan(cj0array[0]))
11171 {
11172 // probably any value will work
11173 j0valid[0] = true;
11174 cj0array[0] = 1;
11175 sj0array[0] = 0;
11176 j0array[0] = 0;
11177 }
11178 for (int ij0 = 0; ij0 < 2; ++ij0)
11179 {
11180 if (!j0valid[ij0])
11181 {
11182 continue;
11183 }
11184 _ij0[0] = ij0;
11185 _ij0[1] = -1;
11186 for (int iij0 = ij0 + 1; iij0 < 2;
11187 ++iij0)
11188 {
11189 if (j0valid[iij0] &&
11190 IKabs(cj0array[ij0] -
11191 cj0array[iij0]) <
11192 IKFAST_SOLUTION_THRESH &&
11193 IKabs(sj0array[ij0] -
11194 sj0array[iij0]) <
11195 IKFAST_SOLUTION_THRESH)
11196 {
11197 j0valid[iij0] = false;
11198 _ij0[1] = iij0;
11199 break;
11200 }
11201 }
11202 j0 = j0array[ij0];
11203 cj0 = cj0array[ij0];
11204 sj0 = sj0array[ij0];
11205 {
11206 IkReal evalcond[6];
11207 IkReal x548 = IKsin(j0);
11208 IkReal x549 = IKcos(j0);
11209 IkReal x550 = ((-1.0) * x548);
11210 evalcond[0] = (new_r11 * x548);
11211 evalcond[1] = (new_r00 * x550);
11212 evalcond[2] = (gconst8 * x550);
11213 evalcond[3] =
11214 (gconst8 + ((new_r11 * x549)));
11215 evalcond[4] =
11216 (((gconst8 * x549)) + new_r11);
11217 evalcond[5] =
11218 (((new_r00 * x549)) +
11219 (((-1.0) * gconst8)));
11220 if (IKabs(evalcond[0]) >
11221 IKFAST_EVALCOND_THRESH ||
11222 IKabs(evalcond[1]) >
11223 IKFAST_EVALCOND_THRESH ||
11224 IKabs(evalcond[2]) >
11225 IKFAST_EVALCOND_THRESH ||
11226 IKabs(evalcond[3]) >
11227 IKFAST_EVALCOND_THRESH ||
11228 IKabs(evalcond[4]) >
11229 IKFAST_EVALCOND_THRESH ||
11230 IKabs(evalcond[5]) >
11231 IKFAST_EVALCOND_THRESH)
11232 {
11233 continue;
11234 }
11235 }
11236
11237 {
11238 std::vector<IkSingleDOFSolutionBase<
11239 IkReal> >
11240 vinfos(7);
11241 vinfos[0].jointtype = 1;
11242 vinfos[0].foffset = j0;
11243 vinfos[0].indices[0] = _ij0[0];
11244 vinfos[0].indices[1] = _ij0[1];
11245 vinfos[0].maxsolutions = _nj0;
11246 vinfos[1].jointtype = 1;
11247 vinfos[1].foffset = j1;
11248 vinfos[1].indices[0] = _ij1[0];
11249 vinfos[1].indices[1] = _ij1[1];
11250 vinfos[1].maxsolutions = _nj1;
11251 vinfos[2].jointtype = 1;
11252 vinfos[2].foffset = j2;
11253 vinfos[2].indices[0] = _ij2[0];
11254 vinfos[2].indices[1] = _ij2[1];
11255 vinfos[2].maxsolutions = _nj2;
11256 vinfos[3].jointtype = 1;
11257 vinfos[3].foffset = j3;
11258 vinfos[3].indices[0] = _ij3[0];
11259 vinfos[3].indices[1] = _ij3[1];
11260 vinfos[3].maxsolutions = _nj3;
11261 vinfos[4].jointtype = 1;
11262 vinfos[4].foffset = j4;
11263 vinfos[4].indices[0] = _ij4[0];
11264 vinfos[4].indices[1] = _ij4[1];
11265 vinfos[4].maxsolutions = _nj4;
11266 vinfos[5].jointtype = 1;
11267 vinfos[5].foffset = j5;
11268 vinfos[5].indices[0] = _ij5[0];
11269 vinfos[5].indices[1] = _ij5[1];
11270 vinfos[5].maxsolutions = _nj5;
11271 vinfos[6].jointtype = 1;
11272 vinfos[6].foffset = j6;
11273 vinfos[6].indices[0] = _ij6[0];
11274 vinfos[6].indices[1] = _ij6[1];
11275 vinfos[6].maxsolutions = _nj6;
11276 std::vector<int> vfree(0);
11277 solutions.AddSolution(vinfos,
11278 vfree);
11279 }
11280 }
11281 }
11282 }
11283 } while (0);
11284 if (bgotonextstatement)
11285 {
11286 bool bgotonextstatement = true;
11287 do
11288 {
11289 evalcond[0] =
11290 ((IKabs(new_r00)) + (IKabs(new_r01)));
11291 if (IKabs(evalcond[0]) <
11292 0.0000050000000000)
11293 {
11294 bgotonextstatement = false;
11295 {
11296 IkReal j0eval[1];
11297 CheckValue<IkReal> x552 =
11298 IKatan2WithCheck(
11299 IkReal(0),
11300 IkReal(new_r11),
11301 IKFAST_ATAN2_MAGTHRESH);
11302 if (!x552.valid)
11303 {
11304 continue;
11305 }
11306 IkReal x551 = ((-1.0) * (x552.value));
11307 sj1 = 0;
11308 cj1 = -1.0;
11309 j1 = 3.14159265358979;
11310 sj2 = gconst7;
11311 cj2 = gconst8;
11312 j2 = x551;
11313 new_r00 = 0;
11314 new_r01 = 0;
11315 new_r12 = 0;
11316 new_r22 = 0;
11317 IkReal gconst6 = x551;
11318 IkReal gconst7 = 0;
11319 IkReal x553 =
11320 ((1.0) + (((-1.0) *
11321 (new_r10 * new_r10))));
11322 if (IKabs(x553) == 0)
11323 {
11324 continue;
11325 }
11326 IkReal gconst8 =
11327 (new_r11 * (pow(x553, -0.5)));
11328 j0eval[0] = ((IKabs(new_r11)) +
11329 (IKabs(new_r10)));
11330 if (IKabs(j0eval[0]) <
11331 0.0000010000000000)
11332 {
11333 {
11334 IkReal j0eval[1];
11335 CheckValue<IkReal> x555 =
11336 IKatan2WithCheck(
11337 IkReal(0),
11338 IkReal(new_r11),
11339 IKFAST_ATAN2_MAGTHRESH);
11340 if (!x555.valid)
11341 {
11342 continue;
11343 }
11344 IkReal x554 =
11345 ((-1.0) * (x555.value));
11346 sj1 = 0;
11347 cj1 = -1.0;
11348 j1 = 3.14159265358979;
11349 sj2 = gconst7;
11350 cj2 = gconst8;
11351 j2 = x554;
11352 new_r00 = 0;
11353 new_r01 = 0;
11354 new_r12 = 0;
11355 new_r22 = 0;
11356 IkReal gconst6 = x554;
11357 IkReal gconst7 = 0;
11358 IkReal x556 =
11359 ((1.0) +
11360 (((-1.0) *
11361 (new_r10 * new_r10))));
11362 if (IKabs(x556) == 0)
11363 {
11364 continue;
11365 }
11366 IkReal gconst8 =
11367 (new_r11 * (pow(x556, -0.5)));
11368 j0eval[0] = new_r11;
11369 if (IKabs(j0eval[0]) <
11370 0.0000010000000000)
11371 {
11372 {
11373 IkReal j0eval[2];
11374 CheckValue<IkReal> x558 =
11375 IKatan2WithCheck(
11376 IkReal(0),
11377 IkReal(new_r11),
11378 IKFAST_ATAN2_MAGTHRESH);
11379 if (!x558.valid)
11380 {
11381 continue;
11382 }
11383 IkReal x557 =
11384 ((-1.0) * (x558.value));
11385 sj1 = 0;
11386 cj1 = -1.0;
11387 j1 = 3.14159265358979;
11388 sj2 = gconst7;
11389 cj2 = gconst8;
11390 j2 = x557;
11391 new_r00 = 0;
11392 new_r01 = 0;
11393 new_r12 = 0;
11394 new_r22 = 0;
11395 IkReal gconst6 = x557;
11396 IkReal gconst7 = 0;
11397 IkReal x559 =
11398 ((1.0) +
11399 (((-1.0) *
11400 (new_r10 * new_r10))));
11401 if (IKabs(x559) == 0)
11402 {
11403 continue;
11404 }
11405 IkReal gconst8 =
11406 (new_r11 *
11407 (pow(x559, -0.5)));
11408 j0eval[0] = new_r10;
11409 j0eval[1] = new_r11;
11410 if (IKabs(j0eval[0]) <
11411 0.0000010000000000 ||
11412 IKabs(j0eval[1]) <
11413 0.0000010000000000)
11414 {
11415 continue; // 3 cases
11416 // reached
11417 }
11418 else
11419 {
11420 {
11421 IkReal j0array[1],
11422 cj0array[1],
11423 sj0array[1];
11424 bool j0valid[1] = {
11425 false
11426 };
11427 _nj0 = 1;
11428 CheckValue<IkReal> x560 =
11429 IKPowWithIntegerCheck(
11430 new_r10, -1);
11431 if (!x560.valid)
11432 {
11433 continue;
11434 }
11435 CheckValue<IkReal> x561 =
11436 IKPowWithIntegerCheck(
11437 new_r11, -1);
11438 if (!x561.valid)
11439 {
11440 continue;
11441 }
11442 if (IKabs(
11443 (gconst8 *
11444 (x560.value))) <
11445 IKFAST_ATAN2_MAGTHRESH &&
11446 IKabs((
11447 (-1.0) * gconst8 *
11448 (x561.value))) <
11449 IKFAST_ATAN2_MAGTHRESH &&
11450 IKabs(
11451 IKsqr((
11452 gconst8 *
11453 (x560.value))) +
11454 IKsqr((
11455 (-1.0) *
11456 gconst8 *
11457 (x561.value))) -
11458 1) <=
11459 IKFAST_SINCOS_THRESH)
11460 continue;
11461 j0array[0] = IKatan2(
11462 (gconst8 *
11463 (x560.value)),
11464 ((-1.0) * gconst8 *
11465 (x561.value)));
11466 sj0array[0] =
11467 IKsin(j0array[0]);
11468 cj0array[0] =
11469 IKcos(j0array[0]);
11470 if (j0array[0] > IKPI)
11471 {
11472 j0array[0] -= IK2PI;
11473 }
11474 else if (j0array[0] <
11475 -IKPI)
11476 {
11477 j0array[0] += IK2PI;
11478 }
11479 j0valid[0] = true;
11480 for (int ij0 = 0; ij0 < 1;
11481 ++ij0)
11482 {
11483 if (!j0valid[ij0])
11484 {
11485 continue;
11486 }
11487 _ij0[0] = ij0;
11488 _ij0[1] = -1;
11489 for (int iij0 = ij0 + 1;
11490 iij0 < 1;
11491 ++iij0)
11492 {
11493 if (j0valid[iij0] &&
11494 IKabs(
11495 cj0array
11496 [ij0] -
11497 cj0array
11498 [iij0]) <
11499 IKFAST_SOLUTION_THRESH &&
11500 IKabs(
11501 sj0array
11502 [ij0] -
11503 sj0array
11504 [iij0]) <
11505 IKFAST_SOLUTION_THRESH)
11506 {
11507 j0valid[iij0] =
11508 false;
11509 _ij0[1] = iij0;
11510 break;
11511 }
11512 }
11513 j0 = j0array[ij0];
11514 cj0 = cj0array[ij0];
11515 sj0 = sj0array[ij0];
11516 {
11517 IkReal evalcond[8];
11518 IkReal x562 =
11519 IKcos(j0);
11520 IkReal x563 =
11521 IKsin(j0);
11522 IkReal x564 =
11523 (gconst8 * x563);
11524 IkReal x565 =
11525 (gconst8 * x562);
11526 evalcond[0] =
11527 (new_r10 * x562);
11528 evalcond[1] =
11529 (new_r11 * x563);
11530 evalcond[2] =
11531 ((-1.0) * x565);
11532 evalcond[3] =
11533 ((-1.0) * x564);
11534 evalcond[4] =
11535 (gconst8 +
11536 ((new_r11 *
11537 x562)));
11538 evalcond[5] =
11539 (x565 + new_r11);
11540 evalcond[6] =
11541 ((((-1.0) *
11542 x564)) +
11543 new_r10);
11544 evalcond[7] =
11545 ((((-1.0) *
11546 gconst8)) +
11547 ((new_r10 *
11548 x563)));
11549 if (IKabs(
11550 evalcond[0]) >
11551 IKFAST_EVALCOND_THRESH ||
11552 IKabs(
11553 evalcond[1]) >
11554 IKFAST_EVALCOND_THRESH ||
11555 IKabs(
11556 evalcond[2]) >
11557 IKFAST_EVALCOND_THRESH ||
11558 IKabs(
11559 evalcond[3]) >
11560 IKFAST_EVALCOND_THRESH ||
11561 IKabs(
11562 evalcond[4]) >
11563 IKFAST_EVALCOND_THRESH ||
11564 IKabs(
11565 evalcond[5]) >
11566 IKFAST_EVALCOND_THRESH ||
11567 IKabs(
11568 evalcond[6]) >
11569 IKFAST_EVALCOND_THRESH ||
11570 IKabs(
11571 evalcond[7]) >
11572 IKFAST_EVALCOND_THRESH)
11573 {
11574 continue;
11575 }
11576 }
11577
11578 {
11579 std::vector<
11580 IkSingleDOFSolutionBase<
11581 IkReal> >
11582 vinfos(7);
11583 vinfos[0].jointtype =
11584 1;
11585 vinfos[0].foffset =
11586 j0;
11587 vinfos[0].indices[0] =
11588 _ij0[0];
11589 vinfos[0].indices[1] =
11590 _ij0[1];
11591 vinfos[0]
11592 .maxsolutions =
11593 _nj0;
11594 vinfos[1].jointtype =
11595 1;
11596 vinfos[1].foffset =
11597 j1;
11598 vinfos[1].indices[0] =
11599 _ij1[0];
11600 vinfos[1].indices[1] =
11601 _ij1[1];
11602 vinfos[1]
11603 .maxsolutions =
11604 _nj1;
11605 vinfos[2].jointtype =
11606 1;
11607 vinfos[2].foffset =
11608 j2;
11609 vinfos[2].indices[0] =
11610 _ij2[0];
11611 vinfos[2].indices[1] =
11612 _ij2[1];
11613 vinfos[2]
11614 .maxsolutions =
11615 _nj2;
11616 vinfos[3].jointtype =
11617 1;
11618 vinfos[3].foffset =
11619 j3;
11620 vinfos[3].indices[0] =
11621 _ij3[0];
11622 vinfos[3].indices[1] =
11623 _ij3[1];
11624 vinfos[3]
11625 .maxsolutions =
11626 _nj3;
11627 vinfos[4].jointtype =
11628 1;
11629 vinfos[4].foffset =
11630 j4;
11631 vinfos[4].indices[0] =
11632 _ij4[0];
11633 vinfos[4].indices[1] =
11634 _ij4[1];
11635 vinfos[4]
11636 .maxsolutions =
11637 _nj4;
11638 vinfos[5].jointtype =
11639 1;
11640 vinfos[5].foffset =
11641 j5;
11642 vinfos[5].indices[0] =
11643 _ij5[0];
11644 vinfos[5].indices[1] =
11645 _ij5[1];
11646 vinfos[5]
11647 .maxsolutions =
11648 _nj5;
11649 vinfos[6].jointtype =
11650 1;
11651 vinfos[6].foffset =
11652 j6;
11653 vinfos[6].indices[0] =
11654 _ij6[0];
11655 vinfos[6].indices[1] =
11656 _ij6[1];
11657 vinfos[6]
11658 .maxsolutions =
11659 _nj6;
11660 std::vector<int>
11661 vfree(0);
11662 solutions.AddSolution(
11663 vinfos, vfree);
11664 }
11665 }
11666 }
11667 }
11668 }
11669 }
11670 else
11671 {
11672 {
11673 IkReal j0array[1],
11674 cj0array[1], sj0array[1];
11675 bool j0valid[1] = { false };
11676 _nj0 = 1;
11677 CheckValue<IkReal> x566 =
11678 IKPowWithIntegerCheck(
11679 gconst8, -1);
11680 if (!x566.valid)
11681 {
11682 continue;
11683 }
11684 CheckValue<IkReal> x567 =
11685 IKPowWithIntegerCheck(
11686 new_r11, -1);
11687 if (!x567.valid)
11688 {
11689 continue;
11690 }
11691 if (IKabs((new_r10 *
11692 (x566.value))) <
11693 IKFAST_ATAN2_MAGTHRESH &&
11694 IKabs(((-1.0) * gconst8 *
11695 (x567.value))) <
11696 IKFAST_ATAN2_MAGTHRESH &&
11697 IKabs(
11698 IKsqr(
11699 (new_r10 *
11700 (x566.value))) +
11701 IKsqr((
11702 (-1.0) * gconst8 *
11703 (x567.value))) -
11704 1) <=
11705 IKFAST_SINCOS_THRESH)
11706 continue;
11707 j0array[0] = IKatan2(
11708 (new_r10 * (x566.value)),
11709 ((-1.0) * gconst8 *
11710 (x567.value)));
11711 sj0array[0] =
11712 IKsin(j0array[0]);
11713 cj0array[0] =
11714 IKcos(j0array[0]);
11715 if (j0array[0] > IKPI)
11716 {
11717 j0array[0] -= IK2PI;
11718 }
11719 else if (j0array[0] < -IKPI)
11720 {
11721 j0array[0] += IK2PI;
11722 }
11723 j0valid[0] = true;
11724 for (int ij0 = 0; ij0 < 1;
11725 ++ij0)
11726 {
11727 if (!j0valid[ij0])
11728 {
11729 continue;
11730 }
11731 _ij0[0] = ij0;
11732 _ij0[1] = -1;
11733 for (int iij0 = ij0 + 1;
11734 iij0 < 1;
11735 ++iij0)
11736 {
11737 if (j0valid[iij0] &&
11738 IKabs(
11739 cj0array[ij0] -
11740 cj0array[iij0]) <
11741 IKFAST_SOLUTION_THRESH &&
11742 IKabs(
11743 sj0array[ij0] -
11744 sj0array[iij0]) <
11745 IKFAST_SOLUTION_THRESH)
11746 {
11747 j0valid[iij0] = false;
11748 _ij0[1] = iij0;
11749 break;
11750 }
11751 }
11752 j0 = j0array[ij0];
11753 cj0 = cj0array[ij0];
11754 sj0 = sj0array[ij0];
11755 {
11756 IkReal evalcond[8];
11757 IkReal x568 = IKcos(j0);
11758 IkReal x569 = IKsin(j0);
11759 IkReal x570 =
11760 (gconst8 * x569);
11761 IkReal x571 =
11762 (gconst8 * x568);
11763 evalcond[0] =
11764 (new_r10 * x568);
11765 evalcond[1] =
11766 (new_r11 * x569);
11767 evalcond[2] =
11768 ((-1.0) * x571);
11769 evalcond[3] =
11770 ((-1.0) * x570);
11771 evalcond[4] =
11772 (gconst8 +
11773 ((new_r11 * x568)));
11774 evalcond[5] =
11775 (x571 + new_r11);
11776 evalcond[6] =
11777 ((((-1.0) * x570)) +
11778 new_r10);
11779 evalcond[7] =
11780 ((((-1.0) *
11781 gconst8)) +
11782 ((new_r10 * x569)));
11783 if (IKabs(evalcond[0]) >
11784 IKFAST_EVALCOND_THRESH ||
11785 IKabs(evalcond[1]) >
11786 IKFAST_EVALCOND_THRESH ||
11787 IKabs(evalcond[2]) >
11788 IKFAST_EVALCOND_THRESH ||
11789 IKabs(evalcond[3]) >
11790 IKFAST_EVALCOND_THRESH ||
11791 IKabs(evalcond[4]) >
11792 IKFAST_EVALCOND_THRESH ||
11793 IKabs(evalcond[5]) >
11794 IKFAST_EVALCOND_THRESH ||
11795 IKabs(evalcond[6]) >
11796 IKFAST_EVALCOND_THRESH ||
11797 IKabs(evalcond[7]) >
11798 IKFAST_EVALCOND_THRESH)
11799 {
11800 continue;
11801 }
11802 }
11803
11804 {
11805 std::vector<
11806 IkSingleDOFSolutionBase<
11807 IkReal> >
11808 vinfos(7);
11809 vinfos[0].jointtype = 1;
11810 vinfos[0].foffset = j0;
11811 vinfos[0].indices[0] =
11812 _ij0[0];
11813 vinfos[0].indices[1] =
11814 _ij0[1];
11815 vinfos[0].maxsolutions =
11816 _nj0;
11817 vinfos[1].jointtype = 1;
11818 vinfos[1].foffset = j1;
11819 vinfos[1].indices[0] =
11820 _ij1[0];
11821 vinfos[1].indices[1] =
11822 _ij1[1];
11823 vinfos[1].maxsolutions =
11824 _nj1;
11825 vinfos[2].jointtype = 1;
11826 vinfos[2].foffset = j2;
11827 vinfos[2].indices[0] =
11828 _ij2[0];
11829 vinfos[2].indices[1] =
11830 _ij2[1];
11831 vinfos[2].maxsolutions =
11832 _nj2;
11833 vinfos[3].jointtype = 1;
11834 vinfos[3].foffset = j3;
11835 vinfos[3].indices[0] =
11836 _ij3[0];
11837 vinfos[3].indices[1] =
11838 _ij3[1];
11839 vinfos[3].maxsolutions =
11840 _nj3;
11841 vinfos[4].jointtype = 1;
11842 vinfos[4].foffset = j4;
11843 vinfos[4].indices[0] =
11844 _ij4[0];
11845 vinfos[4].indices[1] =
11846 _ij4[1];
11847 vinfos[4].maxsolutions =
11848 _nj4;
11849 vinfos[5].jointtype = 1;
11850 vinfos[5].foffset = j5;
11851 vinfos[5].indices[0] =
11852 _ij5[0];
11853 vinfos[5].indices[1] =
11854 _ij5[1];
11855 vinfos[5].maxsolutions =
11856 _nj5;
11857 vinfos[6].jointtype = 1;
11858 vinfos[6].foffset = j6;
11859 vinfos[6].indices[0] =
11860 _ij6[0];
11861 vinfos[6].indices[1] =
11862 _ij6[1];
11863 vinfos[6].maxsolutions =
11864 _nj6;
11865 std::vector<int> vfree(0);
11866 solutions.AddSolution(
11867 vinfos, vfree);
11868 }
11869 }
11870 }
11871 }
11872 }
11873 }
11874 else
11875 {
11876 {
11877 IkReal j0array[1], cj0array[1],
11878 sj0array[1];
11879 bool j0valid[1] = { false };
11880 _nj0 = 1;
11881 CheckValue<IkReal> x572 =
11882 IKatan2WithCheck(
11883 IkReal(new_r10),
11884 IkReal(
11885 ((-1.0) * new_r11)),
11886 IKFAST_ATAN2_MAGTHRESH);
11887 if (!x572.valid)
11888 {
11889 continue;
11890 }
11891 CheckValue<IkReal> x573 =
11892 IKPowWithIntegerCheck(
11893 IKsign(gconst8), -1);
11894 if (!x573.valid)
11895 {
11896 continue;
11897 }
11898 j0array[0] =
11899 ((-1.5707963267949) +
11900 (x572.value) +
11901 (((1.5707963267949) *
11902 (x573.value))));
11903 sj0array[0] = IKsin(j0array[0]);
11904 cj0array[0] = IKcos(j0array[0]);
11905 if (j0array[0] > IKPI)
11906 {
11907 j0array[0] -= IK2PI;
11908 }
11909 else if (j0array[0] < -IKPI)
11910 {
11911 j0array[0] += IK2PI;
11912 }
11913 j0valid[0] = true;
11914 for (int ij0 = 0; ij0 < 1; ++ij0)
11915 {
11916 if (!j0valid[ij0])
11917 {
11918 continue;
11919 }
11920 _ij0[0] = ij0;
11921 _ij0[1] = -1;
11922 for (int iij0 = ij0 + 1;
11923 iij0 < 1;
11924 ++iij0)
11925 {
11926 if (j0valid[iij0] &&
11927 IKabs(cj0array[ij0] -
11928 cj0array[iij0]) <
11929 IKFAST_SOLUTION_THRESH &&
11930 IKabs(sj0array[ij0] -
11931 sj0array[iij0]) <
11932 IKFAST_SOLUTION_THRESH)
11933 {
11934 j0valid[iij0] = false;
11935 _ij0[1] = iij0;
11936 break;
11937 }
11938 }
11939 j0 = j0array[ij0];
11940 cj0 = cj0array[ij0];
11941 sj0 = sj0array[ij0];
11942 {
11943 IkReal evalcond[8];
11944 IkReal x574 = IKcos(j0);
11945 IkReal x575 = IKsin(j0);
11946 IkReal x576 =
11947 (gconst8 * x575);
11948 IkReal x577 =
11949 (gconst8 * x574);
11950 evalcond[0] =
11951 (new_r10 * x574);
11952 evalcond[1] =
11953 (new_r11 * x575);
11954 evalcond[2] = ((-1.0) * x577);
11955 evalcond[3] = ((-1.0) * x576);
11956 evalcond[4] =
11957 (((new_r11 * x574)) +
11958 gconst8);
11959 evalcond[5] =
11960 (x577 + new_r11);
11961 evalcond[6] =
11962 ((((-1.0) * x576)) +
11963 new_r10);
11964 evalcond[7] =
11965 (((new_r10 * x575)) +
11966 (((-1.0) * gconst8)));
11967 if (IKabs(evalcond[0]) >
11968 IKFAST_EVALCOND_THRESH ||
11969 IKabs(evalcond[1]) >
11970 IKFAST_EVALCOND_THRESH ||
11971 IKabs(evalcond[2]) >
11972 IKFAST_EVALCOND_THRESH ||
11973 IKabs(evalcond[3]) >
11974 IKFAST_EVALCOND_THRESH ||
11975 IKabs(evalcond[4]) >
11976 IKFAST_EVALCOND_THRESH ||
11977 IKabs(evalcond[5]) >
11978 IKFAST_EVALCOND_THRESH ||
11979 IKabs(evalcond[6]) >
11980 IKFAST_EVALCOND_THRESH ||
11981 IKabs(evalcond[7]) >
11982 IKFAST_EVALCOND_THRESH)
11983 {
11984 continue;
11985 }
11986 }
11987
11988 {
11989 std::vector<
11990 IkSingleDOFSolutionBase<
11991 IkReal> >
11992 vinfos(7);
11993 vinfos[0].jointtype = 1;
11994 vinfos[0].foffset = j0;
11995 vinfos[0].indices[0] =
11996 _ij0[0];
11997 vinfos[0].indices[1] =
11998 _ij0[1];
11999 vinfos[0].maxsolutions = _nj0;
12000 vinfos[1].jointtype = 1;
12001 vinfos[1].foffset = j1;
12002 vinfos[1].indices[0] =
12003 _ij1[0];
12004 vinfos[1].indices[1] =
12005 _ij1[1];
12006 vinfos[1].maxsolutions = _nj1;
12007 vinfos[2].jointtype = 1;
12008 vinfos[2].foffset = j2;
12009 vinfos[2].indices[0] =
12010 _ij2[0];
12011 vinfos[2].indices[1] =
12012 _ij2[1];
12013 vinfos[2].maxsolutions = _nj2;
12014 vinfos[3].jointtype = 1;
12015 vinfos[3].foffset = j3;
12016 vinfos[3].indices[0] =
12017 _ij3[0];
12018 vinfos[3].indices[1] =
12019 _ij3[1];
12020 vinfos[3].maxsolutions = _nj3;
12021 vinfos[4].jointtype = 1;
12022 vinfos[4].foffset = j4;
12023 vinfos[4].indices[0] =
12024 _ij4[0];
12025 vinfos[4].indices[1] =
12026 _ij4[1];
12027 vinfos[4].maxsolutions = _nj4;
12028 vinfos[5].jointtype = 1;
12029 vinfos[5].foffset = j5;
12030 vinfos[5].indices[0] =
12031 _ij5[0];
12032 vinfos[5].indices[1] =
12033 _ij5[1];
12034 vinfos[5].maxsolutions = _nj5;
12035 vinfos[6].jointtype = 1;
12036 vinfos[6].foffset = j6;
12037 vinfos[6].indices[0] =
12038 _ij6[0];
12039 vinfos[6].indices[1] =
12040 _ij6[1];
12041 vinfos[6].maxsolutions = _nj6;
12042 std::vector<int> vfree(0);
12043 solutions.AddSolution(vinfos,
12044 vfree);
12045 }
12046 }
12047 }
12048 }
12049 }
12050 }
12051 } while (0);
12052 if (bgotonextstatement)
12053 {
12054 bool bgotonextstatement = true;
12055 do
12056 {
12057 evalcond[0] = IKabs(new_r01);
12058 if (IKabs(evalcond[0]) <
12059 0.0000050000000000)
12060 {
12061 bgotonextstatement = false;
12062 {
12063 IkReal j0eval[1];
12064 CheckValue<IkReal> x579 =
12065 IKatan2WithCheck(
12066 IkReal(0),
12067 IkReal(new_r11),
12068 IKFAST_ATAN2_MAGTHRESH);
12069 if (!x579.valid)
12070 {
12071 continue;
12072 }
12073 IkReal x578 =
12074 ((-1.0) * (x579.value));
12075 sj1 = 0;
12076 cj1 = -1.0;
12077 j1 = 3.14159265358979;
12078 sj2 = gconst7;
12079 cj2 = gconst8;
12080 j2 = x578;
12081 new_r01 = 0;
12082 IkReal gconst6 = x578;
12083 IkReal gconst7 = 0;
12084 IkReal x580 = new_r11 * new_r11;
12085 if (IKabs(x580) == 0)
12086 {
12087 continue;
12088 }
12089 IkReal gconst8 =
12090 (new_r11 * (pow(x580, -0.5)));
12091 j0eval[0] = ((IKabs(new_r10)) +
12092 (IKabs(new_r00)));
12093 if (IKabs(j0eval[0]) <
12094 0.0000010000000000)
12095 {
12096 {
12097 IkReal j0eval[1];
12098 CheckValue<IkReal> x582 =
12099 IKatan2WithCheck(
12100 IkReal(0),
12101 IkReal(new_r11),
12102 IKFAST_ATAN2_MAGTHRESH);
12103 if (!x582.valid)
12104 {
12105 continue;
12106 }
12107 IkReal x581 =
12108 ((-1.0) * (x582.value));
12109 sj1 = 0;
12110 cj1 = -1.0;
12111 j1 = 3.14159265358979;
12112 sj2 = gconst7;
12113 cj2 = gconst8;
12114 j2 = x581;
12115 new_r01 = 0;
12116 IkReal gconst6 = x581;
12117 IkReal gconst7 = 0;
12118 IkReal x583 = new_r11 * new_r11;
12119 if (IKabs(x583) == 0)
12120 {
12121 continue;
12122 }
12123 IkReal gconst8 =
12124 (new_r11 *
12125 (pow(x583, -0.5)));
12126 j0eval[0] = ((IKabs(new_r11)) +
12127 (IKabs(new_r10)));
12128 if (IKabs(j0eval[0]) <
12129 0.0000010000000000)
12130 {
12131 {
12132 IkReal j0eval[1];
12133 CheckValue<IkReal> x585 =
12134 IKatan2WithCheck(
12135 IkReal(0),
12136 IkReal(new_r11),
12137 IKFAST_ATAN2_MAGTHRESH);
12138 if (!x585.valid)
12139 {
12140 continue;
12141 }
12142 IkReal x584 =
12143 ((-1.0) * (x585.value));
12144 sj1 = 0;
12145 cj1 = -1.0;
12146 j1 = 3.14159265358979;
12147 sj2 = gconst7;
12148 cj2 = gconst8;
12149 j2 = x584;
12150 new_r01 = 0;
12151 IkReal gconst6 = x584;
12152 IkReal gconst7 = 0;
12153 IkReal x586 =
12154 new_r11 * new_r11;
12155 if (IKabs(x586) == 0)
12156 {
12157 continue;
12158 }
12159 IkReal gconst8 =
12160 (new_r11 *
12161 (pow(x586, -0.5)));
12162 j0eval[0] = new_r11;
12163 if (IKabs(j0eval[0]) <
12164 0.0000010000000000)
12165 {
12166 continue; // 3 cases
12167 // reached
12168 }
12169 else
12170 {
12171 {
12172 IkReal j0array[1],
12173 cj0array[1],
12174 sj0array[1];
12175 bool j0valid[1] = {
12176 false
12177 };
12178 _nj0 = 1;
12179 CheckValue<IkReal> x587 =
12180 IKPowWithIntegerCheck(
12181 gconst8, -1);
12182 if (!x587.valid)
12183 {
12184 continue;
12185 }
12186 CheckValue<IkReal> x588 =
12187 IKPowWithIntegerCheck(
12188 new_r11, -1);
12189 if (!x588.valid)
12190 {
12191 continue;
12192 }
12193 if (IKabs((
12194 new_r10 *
12195 (x587.value))) <
12196 IKFAST_ATAN2_MAGTHRESH &&
12197 IKabs((
12198 (-1.0) *
12199 gconst8 *
12200 (x588.value))) <
12201 IKFAST_ATAN2_MAGTHRESH &&
12202 IKabs(
12203 IKsqr((
12204 new_r10 *
12205 (x587.value))) +
12206 IKsqr((
12207 (-1.0) *
12208 gconst8 *
12209 (x588.value))) -
12210 1) <=
12211 IKFAST_SINCOS_THRESH)
12212 continue;
12213 j0array[0] = IKatan2(
12214 (new_r10 *
12215 (x587.value)),
12216 ((-1.0) * gconst8 *
12217 (x588.value)));
12218 sj0array[0] =
12219 IKsin(j0array[0]);
12220 cj0array[0] =
12221 IKcos(j0array[0]);
12222 if (j0array[0] > IKPI)
12223 {
12224 j0array[0] -= IK2PI;
12225 }
12226 else if (j0array[0] <
12227 -IKPI)
12228 {
12229 j0array[0] += IK2PI;
12230 }
12231 j0valid[0] = true;
12232 for (int ij0 = 0;
12233 ij0 < 1;
12234 ++ij0)
12235 {
12236 if (!j0valid[ij0])
12237 {
12238 continue;
12239 }
12240 _ij0[0] = ij0;
12241 _ij0[1] = -1;
12242 for (int iij0 =
12243 ij0 + 1;
12244 iij0 < 1;
12245 ++iij0)
12246 {
12247 if (j0valid[iij0] &&
12248 IKabs(
12249 cj0array
12250 [ij0] -
12251 cj0array
12252 [iij0]) <
12253 IKFAST_SOLUTION_THRESH &&
12254 IKabs(
12255 sj0array
12256 [ij0] -
12257 sj0array
12258 [iij0]) <
12259 IKFAST_SOLUTION_THRESH)
12260 {
12261 j0valid[iij0] =
12262 false;
12263 _ij0[1] = iij0;
12264 break;
12265 }
12266 }
12267 j0 = j0array[ij0];
12268 cj0 = cj0array[ij0];
12269 sj0 = sj0array[ij0];
12270 {
12271 IkReal evalcond[8];
12272 IkReal x589 =
12273 IKsin(j0);
12274 IkReal x590 =
12275 IKcos(j0);
12276 IkReal x591 =
12277 ((1.0) *
12278 gconst8);
12279 IkReal x592 =
12280 ((1.0) * x589);
12281 evalcond[0] =
12282 (new_r11 *
12283 x589);
12284 evalcond[1] =
12285 ((-1.0) *
12286 gconst8 *
12287 x589);
12288 evalcond[2] =
12289 (((new_r11 *
12290 x590)) +
12291 gconst8);
12292 evalcond[3] =
12293 (((gconst8 *
12294 x590)) +
12295 new_r11);
12296 evalcond[4] =
12297 ((((-1.0) *
12298 x589 *
12299 x591)) +
12300 new_r10);
12301 evalcond[5] =
12302 ((((-1.0) *
12303 x590 *
12304 x591)) +
12305 new_r00);
12306 evalcond[6] =
12307 ((((-1.0) *
12308 new_r00 *
12309 x592)) +
12310 ((new_r10 *
12311 x590)));
12312 evalcond[7] =
12313 (((new_r00 *
12314 x590)) +
12315 (((-1.0) *
12316 x591)) +
12317 ((new_r10 *
12318 x589)));
12319 if (IKabs(evalcond
12320 [0]) >
12321 IKFAST_EVALCOND_THRESH ||
12322 IKabs(evalcond
12323 [1]) >
12324 IKFAST_EVALCOND_THRESH ||
12325 IKabs(evalcond
12326 [2]) >
12327 IKFAST_EVALCOND_THRESH ||
12328 IKabs(evalcond
12329 [3]) >
12330 IKFAST_EVALCOND_THRESH ||
12331 IKabs(evalcond
12332 [4]) >
12333 IKFAST_EVALCOND_THRESH ||
12334 IKabs(evalcond
12335 [5]) >
12336 IKFAST_EVALCOND_THRESH ||
12337 IKabs(evalcond
12338 [6]) >
12339 IKFAST_EVALCOND_THRESH ||
12340 IKabs(evalcond
12341 [7]) >
12342 IKFAST_EVALCOND_THRESH)
12343 {
12344 continue;
12345 }
12346 }
12347
12348 {
12349 std::vector<
12350 IkSingleDOFSolutionBase<
12351 IkReal> >
12352 vinfos(7);
12353 vinfos[0]
12354 .jointtype = 1;
12355 vinfos[0].foffset =
12356 j0;
12357 vinfos[0]
12358 .indices[0] =
12359 _ij0[0];
12360 vinfos[0]
12361 .indices[1] =
12362 _ij0[1];
12363 vinfos[0]
12364 .maxsolutions =
12365 _nj0;
12366 vinfos[1]
12367 .jointtype = 1;
12368 vinfos[1].foffset =
12369 j1;
12370 vinfos[1]
12371 .indices[0] =
12372 _ij1[0];
12373 vinfos[1]
12374 .indices[1] =
12375 _ij1[1];
12376 vinfos[1]
12377 .maxsolutions =
12378 _nj1;
12379 vinfos[2]
12380 .jointtype = 1;
12381 vinfos[2].foffset =
12382 j2;
12383 vinfos[2]
12384 .indices[0] =
12385 _ij2[0];
12386 vinfos[2]
12387 .indices[1] =
12388 _ij2[1];
12389 vinfos[2]
12390 .maxsolutions =
12391 _nj2;
12392 vinfos[3]
12393 .jointtype = 1;
12394 vinfos[3].foffset =
12395 j3;
12396 vinfos[3]
12397 .indices[0] =
12398 _ij3[0];
12399 vinfos[3]
12400 .indices[1] =
12401 _ij3[1];
12402 vinfos[3]
12403 .maxsolutions =
12404 _nj3;
12405 vinfos[4]
12406 .jointtype = 1;
12407 vinfos[4].foffset =
12408 j4;
12409 vinfos[4]
12410 .indices[0] =
12411 _ij4[0];
12412 vinfos[4]
12413 .indices[1] =
12414 _ij4[1];
12415 vinfos[4]
12416 .maxsolutions =
12417 _nj4;
12418 vinfos[5]
12419 .jointtype = 1;
12420 vinfos[5].foffset =
12421 j5;
12422 vinfos[5]
12423 .indices[0] =
12424 _ij5[0];
12425 vinfos[5]
12426 .indices[1] =
12427 _ij5[1];
12428 vinfos[5]
12429 .maxsolutions =
12430 _nj5;
12431 vinfos[6]
12432 .jointtype = 1;
12433 vinfos[6].foffset =
12434 j6;
12435 vinfos[6]
12436 .indices[0] =
12437 _ij6[0];
12438 vinfos[6]
12439 .indices[1] =
12440 _ij6[1];
12441 vinfos[6]
12442 .maxsolutions =
12443 _nj6;
12444 std::vector<int>
12445 vfree(0);
12446 solutions
12447 .AddSolution(
12448 vinfos,
12449 vfree);
12450 }
12451 }
12452 }
12453 }
12454 }
12455 }
12456 else
12457 {
12458 {
12459 IkReal j0array[1],
12460 cj0array[1],
12461 sj0array[1];
12462 bool j0valid[1] = { false };
12463 _nj0 = 1;
12464 CheckValue<IkReal> x593 =
12465 IKatan2WithCheck(
12466 IkReal(new_r10),
12467 IkReal(((-1.0) *
12468 new_r11)),
12469 IKFAST_ATAN2_MAGTHRESH);
12470 if (!x593.valid)
12471 {
12472 continue;
12473 }
12474 CheckValue<IkReal> x594 =
12475 IKPowWithIntegerCheck(
12476 IKsign(gconst8),
12477 -1);
12478 if (!x594.valid)
12479 {
12480 continue;
12481 }
12482 j0array[0] =
12483 ((-1.5707963267949) +
12484 (x593.value) +
12485 (((1.5707963267949) *
12486 (x594.value))));
12487 sj0array[0] =
12488 IKsin(j0array[0]);
12489 cj0array[0] =
12490 IKcos(j0array[0]);
12491 if (j0array[0] > IKPI)
12492 {
12493 j0array[0] -= IK2PI;
12494 }
12495 else if (j0array[0] < -IKPI)
12496 {
12497 j0array[0] += IK2PI;
12498 }
12499 j0valid[0] = true;
12500 for (int ij0 = 0; ij0 < 1;
12501 ++ij0)
12502 {
12503 if (!j0valid[ij0])
12504 {
12505 continue;
12506 }
12507 _ij0[0] = ij0;
12508 _ij0[1] = -1;
12509 for (int iij0 = ij0 + 1;
12510 iij0 < 1;
12511 ++iij0)
12512 {
12513 if (j0valid[iij0] &&
12514 IKabs(
12515 cj0array[ij0] -
12516 cj0array
12517 [iij0]) <
12518 IKFAST_SOLUTION_THRESH &&
12519 IKabs(
12520 sj0array[ij0] -
12521 sj0array
12522 [iij0]) <
12523 IKFAST_SOLUTION_THRESH)
12524 {
12525 j0valid[iij0] = false;
12526 _ij0[1] = iij0;
12527 break;
12528 }
12529 }
12530 j0 = j0array[ij0];
12531 cj0 = cj0array[ij0];
12532 sj0 = sj0array[ij0];
12533 {
12534 IkReal evalcond[8];
12535 IkReal x595 = IKsin(j0);
12536 IkReal x596 = IKcos(j0);
12537 IkReal x597 =
12538 ((1.0) * gconst8);
12539 IkReal x598 =
12540 ((1.0) * x595);
12541 evalcond[0] =
12542 (new_r11 * x595);
12543 evalcond[1] =
12544 ((-1.0) * gconst8 *
12545 x595);
12546 evalcond[2] =
12547 (((new_r11 *
12548 x596)) +
12549 gconst8);
12550 evalcond[3] =
12551 (((gconst8 *
12552 x596)) +
12553 new_r11);
12554 evalcond[4] =
12555 ((((-1.0) * x595 *
12556 x597)) +
12557 new_r10);
12558 evalcond[5] =
12559 ((((-1.0) * x596 *
12560 x597)) +
12561 new_r00);
12562 evalcond[6] =
12563 ((((-1.0) *
12564 new_r00 *
12565 x598)) +
12566 ((new_r10 *
12567 x596)));
12568 evalcond[7] =
12569 (((new_r10 *
12570 x595)) +
12571 ((new_r00 *
12572 x596)) +
12573 (((-1.0) * x597)));
12574 if (IKabs(evalcond[0]) >
12575 IKFAST_EVALCOND_THRESH ||
12576 IKabs(evalcond[1]) >
12577 IKFAST_EVALCOND_THRESH ||
12578 IKabs(evalcond[2]) >
12579 IKFAST_EVALCOND_THRESH ||
12580 IKabs(evalcond[3]) >
12581 IKFAST_EVALCOND_THRESH ||
12582 IKabs(evalcond[4]) >
12583 IKFAST_EVALCOND_THRESH ||
12584 IKabs(evalcond[5]) >
12585 IKFAST_EVALCOND_THRESH ||
12586 IKabs(evalcond[6]) >
12587 IKFAST_EVALCOND_THRESH ||
12588 IKabs(evalcond[7]) >
12589 IKFAST_EVALCOND_THRESH)
12590 {
12591 continue;
12592 }
12593 }
12594
12595 {
12596 std::vector<
12597 IkSingleDOFSolutionBase<
12598 IkReal> >
12599 vinfos(7);
12600 vinfos[0].jointtype = 1;
12601 vinfos[0].foffset = j0;
12602 vinfos[0].indices[0] =
12603 _ij0[0];
12604 vinfos[0].indices[1] =
12605 _ij0[1];
12606 vinfos[0].maxsolutions =
12607 _nj0;
12608 vinfos[1].jointtype = 1;
12609 vinfos[1].foffset = j1;
12610 vinfos[1].indices[0] =
12611 _ij1[0];
12612 vinfos[1].indices[1] =
12613 _ij1[1];
12614 vinfos[1].maxsolutions =
12615 _nj1;
12616 vinfos[2].jointtype = 1;
12617 vinfos[2].foffset = j2;
12618 vinfos[2].indices[0] =
12619 _ij2[0];
12620 vinfos[2].indices[1] =
12621 _ij2[1];
12622 vinfos[2].maxsolutions =
12623 _nj2;
12624 vinfos[3].jointtype = 1;
12625 vinfos[3].foffset = j3;
12626 vinfos[3].indices[0] =
12627 _ij3[0];
12628 vinfos[3].indices[1] =
12629 _ij3[1];
12630 vinfos[3].maxsolutions =
12631 _nj3;
12632 vinfos[4].jointtype = 1;
12633 vinfos[4].foffset = j4;
12634 vinfos[4].indices[0] =
12635 _ij4[0];
12636 vinfos[4].indices[1] =
12637 _ij4[1];
12638 vinfos[4].maxsolutions =
12639 _nj4;
12640 vinfos[5].jointtype = 1;
12641 vinfos[5].foffset = j5;
12642 vinfos[5].indices[0] =
12643 _ij5[0];
12644 vinfos[5].indices[1] =
12645 _ij5[1];
12646 vinfos[5].maxsolutions =
12647 _nj5;
12648 vinfos[6].jointtype = 1;
12649 vinfos[6].foffset = j6;
12650 vinfos[6].indices[0] =
12651 _ij6[0];
12652 vinfos[6].indices[1] =
12653 _ij6[1];
12654 vinfos[6].maxsolutions =
12655 _nj6;
12656 std::vector<int> vfree(
12657 0);
12658 solutions.AddSolution(
12659 vinfos, vfree);
12660 }
12661 }
12662 }
12663 }
12664 }
12665 }
12666 else
12667 {
12668 {
12669 IkReal j0array[1], cj0array[1],
12670 sj0array[1];
12671 bool j0valid[1] = { false };
12672 _nj0 = 1;
12673 CheckValue<IkReal> x599 =
12674 IKPowWithIntegerCheck(
12675 IKsign(gconst8), -1);
12676 if (!x599.valid)
12677 {
12678 continue;
12679 }
12680 CheckValue<IkReal> x600 =
12681 IKatan2WithCheck(
12682 IkReal(new_r10),
12683 IkReal(new_r00),
12684 IKFAST_ATAN2_MAGTHRESH);
12685 if (!x600.valid)
12686 {
12687 continue;
12688 }
12689 j0array[0] =
12690 ((-1.5707963267949) +
12691 (((1.5707963267949) *
12692 (x599.value))) +
12693 (x600.value));
12694 sj0array[0] = IKsin(j0array[0]);
12695 cj0array[0] = IKcos(j0array[0]);
12696 if (j0array[0] > IKPI)
12697 {
12698 j0array[0] -= IK2PI;
12699 }
12700 else if (j0array[0] < -IKPI)
12701 {
12702 j0array[0] += IK2PI;
12703 }
12704 j0valid[0] = true;
12705 for (int ij0 = 0; ij0 < 1;
12706 ++ij0)
12707 {
12708 if (!j0valid[ij0])
12709 {
12710 continue;
12711 }
12712 _ij0[0] = ij0;
12713 _ij0[1] = -1;
12714 for (int iij0 = ij0 + 1;
12715 iij0 < 1;
12716 ++iij0)
12717 {
12718 if (j0valid[iij0] &&
12719 IKabs(cj0array[ij0] -
12720 cj0array[iij0]) <
12721 IKFAST_SOLUTION_THRESH &&
12722 IKabs(sj0array[ij0] -
12723 sj0array[iij0]) <
12724 IKFAST_SOLUTION_THRESH)
12725 {
12726 j0valid[iij0] = false;
12727 _ij0[1] = iij0;
12728 break;
12729 }
12730 }
12731 j0 = j0array[ij0];
12732 cj0 = cj0array[ij0];
12733 sj0 = sj0array[ij0];
12734 {
12735 IkReal evalcond[8];
12736 IkReal x601 = IKsin(j0);
12737 IkReal x602 = IKcos(j0);
12738 IkReal x603 =
12739 ((1.0) * gconst8);
12740 IkReal x604 =
12741 ((1.0) * x601);
12742 evalcond[0] =
12743 (new_r11 * x601);
12744 evalcond[1] =
12745 ((-1.0) * gconst8 *
12746 x601);
12747 evalcond[2] =
12748 (gconst8 +
12749 ((new_r11 * x602)));
12750 evalcond[3] =
12751 (((gconst8 * x602)) +
12752 new_r11);
12753 evalcond[4] =
12754 (new_r10 +
12755 (((-1.0) * x601 *
12756 x603)));
12757 evalcond[5] =
12758 ((((-1.0) * x602 *
12759 x603)) +
12760 new_r00);
12761 evalcond[6] =
12762 (((new_r10 * x602)) +
12763 (((-1.0) * new_r00 *
12764 x604)));
12765 evalcond[7] =
12766 (((new_r10 * x601)) +
12767 ((new_r00 * x602)) +
12768 (((-1.0) * x603)));
12769 if (IKabs(evalcond[0]) >
12770 IKFAST_EVALCOND_THRESH ||
12771 IKabs(evalcond[1]) >
12772 IKFAST_EVALCOND_THRESH ||
12773 IKabs(evalcond[2]) >
12774 IKFAST_EVALCOND_THRESH ||
12775 IKabs(evalcond[3]) >
12776 IKFAST_EVALCOND_THRESH ||
12777 IKabs(evalcond[4]) >
12778 IKFAST_EVALCOND_THRESH ||
12779 IKabs(evalcond[5]) >
12780 IKFAST_EVALCOND_THRESH ||
12781 IKabs(evalcond[6]) >
12782 IKFAST_EVALCOND_THRESH ||
12783 IKabs(evalcond[7]) >
12784 IKFAST_EVALCOND_THRESH)
12785 {
12786 continue;
12787 }
12788 }
12789
12790 {
12791 std::vector<
12792 IkSingleDOFSolutionBase<
12793 IkReal> >
12794 vinfos(7);
12795 vinfos[0].jointtype = 1;
12796 vinfos[0].foffset = j0;
12797 vinfos[0].indices[0] =
12798 _ij0[0];
12799 vinfos[0].indices[1] =
12800 _ij0[1];
12801 vinfos[0].maxsolutions =
12802 _nj0;
12803 vinfos[1].jointtype = 1;
12804 vinfos[1].foffset = j1;
12805 vinfos[1].indices[0] =
12806 _ij1[0];
12807 vinfos[1].indices[1] =
12808 _ij1[1];
12809 vinfos[1].maxsolutions =
12810 _nj1;
12811 vinfos[2].jointtype = 1;
12812 vinfos[2].foffset = j2;
12813 vinfos[2].indices[0] =
12814 _ij2[0];
12815 vinfos[2].indices[1] =
12816 _ij2[1];
12817 vinfos[2].maxsolutions =
12818 _nj2;
12819 vinfos[3].jointtype = 1;
12820 vinfos[3].foffset = j3;
12821 vinfos[3].indices[0] =
12822 _ij3[0];
12823 vinfos[3].indices[1] =
12824 _ij3[1];
12825 vinfos[3].maxsolutions =
12826 _nj3;
12827 vinfos[4].jointtype = 1;
12828 vinfos[4].foffset = j4;
12829 vinfos[4].indices[0] =
12830 _ij4[0];
12831 vinfos[4].indices[1] =
12832 _ij4[1];
12833 vinfos[4].maxsolutions =
12834 _nj4;
12835 vinfos[5].jointtype = 1;
12836 vinfos[5].foffset = j5;
12837 vinfos[5].indices[0] =
12838 _ij5[0];
12839 vinfos[5].indices[1] =
12840 _ij5[1];
12841 vinfos[5].maxsolutions =
12842 _nj5;
12843 vinfos[6].jointtype = 1;
12844 vinfos[6].foffset = j6;
12845 vinfos[6].indices[0] =
12846 _ij6[0];
12847 vinfos[6].indices[1] =
12848 _ij6[1];
12849 vinfos[6].maxsolutions =
12850 _nj6;
12851 std::vector<int> vfree(0);
12852 solutions.AddSolution(
12853 vinfos, vfree);
12854 }
12855 }
12856 }
12857 }
12858 }
12859 }
12860 } while (0);
12861 if (bgotonextstatement)
12862 {
12863 bool bgotonextstatement = true;
12864 do
12865 {
12866 if (1)
12867 {
12868 bgotonextstatement = false;
12869 continue; // branch miss [j0]
12870 }
12871 } while (0);
12872 if (bgotonextstatement)
12873 {
12874 }
12875 }
12876 }
12877 }
12878 }
12879 }
12880 }
12881 }
12882 else
12883 {
12884 {
12885 IkReal j0array[1], cj0array[1], sj0array[1];
12886 bool j0valid[1] = { false };
12887 _nj0 = 1;
12888 IkReal x605 = ((1.0) * new_r11);
12889 CheckValue<IkReal> x606 = IKatan2WithCheck(
12890 IkReal(((((-1.0) * new_r01 * x605)) +
12891 ((gconst7 * gconst8)))),
12892 IkReal(((((-1.0) * (gconst7 * gconst7))) +
12893 (new_r11 * new_r11))),
12894 IKFAST_ATAN2_MAGTHRESH);
12895 if (!x606.valid)
12896 {
12897 continue;
12898 }
12899 CheckValue<IkReal> x607 = IKPowWithIntegerCheck(
12900 IKsign(((((-1.0) * gconst8 * x605)) +
12901 ((gconst7 * new_r01)))),
12902 -1);
12903 if (!x607.valid)
12904 {
12905 continue;
12906 }
12907 j0array[0] =
12908 ((-1.5707963267949) + (x606.value) +
12909 (((1.5707963267949) * (x607.value))));
12910 sj0array[0] = IKsin(j0array[0]);
12911 cj0array[0] = IKcos(j0array[0]);
12912 if (j0array[0] > IKPI)
12913 {
12914 j0array[0] -= IK2PI;
12915 }
12916 else if (j0array[0] < -IKPI)
12917 {
12918 j0array[0] += IK2PI;
12919 }
12920 j0valid[0] = true;
12921 for (int ij0 = 0; ij0 < 1; ++ij0)
12922 {
12923 if (!j0valid[ij0])
12924 {
12925 continue;
12926 }
12927 _ij0[0] = ij0;
12928 _ij0[1] = -1;
12929 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
12930 {
12931 if (j0valid[iij0] &&
12932 IKabs(cj0array[ij0] - cj0array[iij0]) <
12933 IKFAST_SOLUTION_THRESH &&
12934 IKabs(sj0array[ij0] - sj0array[iij0]) <
12935 IKFAST_SOLUTION_THRESH)
12936 {
12937 j0valid[iij0] = false;
12938 _ij0[1] = iij0;
12939 break;
12940 }
12941 }
12942 j0 = j0array[ij0];
12943 cj0 = cj0array[ij0];
12944 sj0 = sj0array[ij0];
12945 {
12946 IkReal evalcond[8];
12947 IkReal x608 = IKcos(j0);
12948 IkReal x609 = IKsin(j0);
12949 IkReal x610 = (gconst7 * x608);
12950 IkReal x611 = ((1.0) * x609);
12951 IkReal x612 = (gconst8 * x608);
12952 IkReal x613 = (gconst8 * x611);
12953 evalcond[0] = (gconst7 + ((new_r11 * x609)) +
12954 ((new_r01 * x608)));
12955 evalcond[1] =
12956 (((gconst7 * x609)) + x612 + new_r11);
12957 evalcond[2] = (gconst7 + ((new_r10 * x608)) +
12958 (((-1.0) * new_r00 * x611)));
12959 evalcond[3] = (gconst8 + ((new_r11 * x608)) +
12960 (((-1.0) * new_r01 * x611)));
12961 evalcond[4] =
12962 ((((-1.0) * x613)) + x610 + new_r10);
12963 evalcond[5] =
12964 ((((-1.0) * x613)) + x610 + new_r01);
12965 evalcond[6] =
12966 ((((-1.0) * gconst8)) +
12967 ((new_r10 * x609)) + ((new_r00 * x608)));
12968 evalcond[7] = ((((-1.0) * x612)) + new_r00 +
12969 (((-1.0) * gconst7 * x611)));
12970 if (IKabs(evalcond[0]) >
12971 IKFAST_EVALCOND_THRESH ||
12972 IKabs(evalcond[1]) >
12973 IKFAST_EVALCOND_THRESH ||
12974 IKabs(evalcond[2]) >
12975 IKFAST_EVALCOND_THRESH ||
12976 IKabs(evalcond[3]) >
12977 IKFAST_EVALCOND_THRESH ||
12978 IKabs(evalcond[4]) >
12979 IKFAST_EVALCOND_THRESH ||
12980 IKabs(evalcond[5]) >
12981 IKFAST_EVALCOND_THRESH ||
12982 IKabs(evalcond[6]) >
12983 IKFAST_EVALCOND_THRESH ||
12984 IKabs(evalcond[7]) >
12985 IKFAST_EVALCOND_THRESH)
12986 {
12987 continue;
12988 }
12989 }
12990
12991 {
12992 std::vector<IkSingleDOFSolutionBase<IkReal> >
12993 vinfos(7);
12994 vinfos[0].jointtype = 1;
12995 vinfos[0].foffset = j0;
12996 vinfos[0].indices[0] = _ij0[0];
12997 vinfos[0].indices[1] = _ij0[1];
12998 vinfos[0].maxsolutions = _nj0;
12999 vinfos[1].jointtype = 1;
13000 vinfos[1].foffset = j1;
13001 vinfos[1].indices[0] = _ij1[0];
13002 vinfos[1].indices[1] = _ij1[1];
13003 vinfos[1].maxsolutions = _nj1;
13004 vinfos[2].jointtype = 1;
13005 vinfos[2].foffset = j2;
13006 vinfos[2].indices[0] = _ij2[0];
13007 vinfos[2].indices[1] = _ij2[1];
13008 vinfos[2].maxsolutions = _nj2;
13009 vinfos[3].jointtype = 1;
13010 vinfos[3].foffset = j3;
13011 vinfos[3].indices[0] = _ij3[0];
13012 vinfos[3].indices[1] = _ij3[1];
13013 vinfos[3].maxsolutions = _nj3;
13014 vinfos[4].jointtype = 1;
13015 vinfos[4].foffset = j4;
13016 vinfos[4].indices[0] = _ij4[0];
13017 vinfos[4].indices[1] = _ij4[1];
13018 vinfos[4].maxsolutions = _nj4;
13019 vinfos[5].jointtype = 1;
13020 vinfos[5].foffset = j5;
13021 vinfos[5].indices[0] = _ij5[0];
13022 vinfos[5].indices[1] = _ij5[1];
13023 vinfos[5].maxsolutions = _nj5;
13024 vinfos[6].jointtype = 1;
13025 vinfos[6].foffset = j6;
13026 vinfos[6].indices[0] = _ij6[0];
13027 vinfos[6].indices[1] = _ij6[1];
13028 vinfos[6].maxsolutions = _nj6;
13029 std::vector<int> vfree(0);
13030 solutions.AddSolution(vinfos, vfree);
13031 }
13032 }
13033 }
13034 }
13035 }
13036 }
13037 else
13038 {
13039 {
13040 IkReal j0array[1], cj0array[1], sj0array[1];
13041 bool j0valid[1] = { false };
13042 _nj0 = 1;
13043 IkReal x614 = ((1.0) * new_r11);
13044 CheckValue<IkReal> x615 = IKPowWithIntegerCheck(
13045 IKsign(
13046 ((new_r01 * new_r01) + (new_r11 * new_r11))),
13047 -1);
13048 if (!x615.valid)
13049 {
13050 continue;
13051 }
13052 CheckValue<IkReal> x616 = IKatan2WithCheck(
13053 IkReal((((gconst8 * new_r01)) +
13054 (((-1.0) * gconst7 * x614)))),
13055 IkReal(((((-1.0) * gconst7 * new_r01)) +
13056 (((-1.0) * gconst8 * x614)))),
13057 IKFAST_ATAN2_MAGTHRESH);
13058 if (!x616.valid)
13059 {
13060 continue;
13061 }
13062 j0array[0] = ((-1.5707963267949) +
13063 (((1.5707963267949) * (x615.value))) +
13064 (x616.value));
13065 sj0array[0] = IKsin(j0array[0]);
13066 cj0array[0] = IKcos(j0array[0]);
13067 if (j0array[0] > IKPI)
13068 {
13069 j0array[0] -= IK2PI;
13070 }
13071 else if (j0array[0] < -IKPI)
13072 {
13073 j0array[0] += IK2PI;
13074 }
13075 j0valid[0] = true;
13076 for (int ij0 = 0; ij0 < 1; ++ij0)
13077 {
13078 if (!j0valid[ij0])
13079 {
13080 continue;
13081 }
13082 _ij0[0] = ij0;
13083 _ij0[1] = -1;
13084 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
13085 {
13086 if (j0valid[iij0] &&
13087 IKabs(cj0array[ij0] - cj0array[iij0]) <
13088 IKFAST_SOLUTION_THRESH &&
13089 IKabs(sj0array[ij0] - sj0array[iij0]) <
13090 IKFAST_SOLUTION_THRESH)
13091 {
13092 j0valid[iij0] = false;
13093 _ij0[1] = iij0;
13094 break;
13095 }
13096 }
13097 j0 = j0array[ij0];
13098 cj0 = cj0array[ij0];
13099 sj0 = sj0array[ij0];
13100 {
13101 IkReal evalcond[8];
13102 IkReal x617 = IKcos(j0);
13103 IkReal x618 = IKsin(j0);
13104 IkReal x619 = (gconst7 * x617);
13105 IkReal x620 = ((1.0) * x618);
13106 IkReal x621 = (gconst8 * x617);
13107 IkReal x622 = (gconst8 * x620);
13108 evalcond[0] = (gconst7 + ((new_r11 * x618)) +
13109 ((new_r01 * x617)));
13110 evalcond[1] =
13111 (((gconst7 * x618)) + x621 + new_r11);
13112 evalcond[2] = ((((-1.0) * new_r00 * x620)) +
13113 gconst7 + ((new_r10 * x617)));
13114 evalcond[3] = ((((-1.0) * new_r01 * x620)) +
13115 gconst8 + ((new_r11 * x617)));
13116 evalcond[4] =
13117 ((((-1.0) * x622)) + x619 + new_r10);
13118 evalcond[5] =
13119 ((((-1.0) * x622)) + x619 + new_r01);
13120 evalcond[6] =
13121 ((((-1.0) * gconst8)) + ((new_r10 * x618)) +
13122 ((new_r00 * x617)));
13123 evalcond[7] = ((((-1.0) * gconst7 * x620)) +
13124 (((-1.0) * x621)) + new_r00);
13125 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
13126 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
13127 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
13128 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
13129 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
13130 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
13131 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
13132 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
13133 {
13134 continue;
13135 }
13136 }
13137
13138 {
13139 std::vector<IkSingleDOFSolutionBase<IkReal> >
13140 vinfos(7);
13141 vinfos[0].jointtype = 1;
13142 vinfos[0].foffset = j0;
13143 vinfos[0].indices[0] = _ij0[0];
13144 vinfos[0].indices[1] = _ij0[1];
13145 vinfos[0].maxsolutions = _nj0;
13146 vinfos[1].jointtype = 1;
13147 vinfos[1].foffset = j1;
13148 vinfos[1].indices[0] = _ij1[0];
13149 vinfos[1].indices[1] = _ij1[1];
13150 vinfos[1].maxsolutions = _nj1;
13151 vinfos[2].jointtype = 1;
13152 vinfos[2].foffset = j2;
13153 vinfos[2].indices[0] = _ij2[0];
13154 vinfos[2].indices[1] = _ij2[1];
13155 vinfos[2].maxsolutions = _nj2;
13156 vinfos[3].jointtype = 1;
13157 vinfos[3].foffset = j3;
13158 vinfos[3].indices[0] = _ij3[0];
13159 vinfos[3].indices[1] = _ij3[1];
13160 vinfos[3].maxsolutions = _nj3;
13161 vinfos[4].jointtype = 1;
13162 vinfos[4].foffset = j4;
13163 vinfos[4].indices[0] = _ij4[0];
13164 vinfos[4].indices[1] = _ij4[1];
13165 vinfos[4].maxsolutions = _nj4;
13166 vinfos[5].jointtype = 1;
13167 vinfos[5].foffset = j5;
13168 vinfos[5].indices[0] = _ij5[0];
13169 vinfos[5].indices[1] = _ij5[1];
13170 vinfos[5].maxsolutions = _nj5;
13171 vinfos[6].jointtype = 1;
13172 vinfos[6].foffset = j6;
13173 vinfos[6].indices[0] = _ij6[0];
13174 vinfos[6].indices[1] = _ij6[1];
13175 vinfos[6].maxsolutions = _nj6;
13176 std::vector<int> vfree(0);
13177 solutions.AddSolution(vinfos, vfree);
13178 }
13179 }
13180 }
13181 }
13182 }
13183 }
13184 else
13185 {
13186 {
13187 IkReal j0array[1], cj0array[1], sj0array[1];
13188 bool j0valid[1] = { false };
13189 _nj0 = 1;
13190 IkReal x623 = ((1.0) * gconst7);
13191 CheckValue<IkReal> x624 = IKatan2WithCheck(
13192 IkReal(((((-1.0) * new_r10 * x623)) +
13193 ((gconst7 * new_r01)))),
13194 IkReal(((((-1.0) * new_r00 * x623)) +
13195 (((-1.0) * new_r11 * x623)))),
13196 IKFAST_ATAN2_MAGTHRESH);
13197 if (!x624.valid)
13198 {
13199 continue;
13200 }
13201 CheckValue<IkReal> x625 =
13202 IKPowWithIntegerCheck(IKsign((((new_r10 * new_r11)) +
13203 ((new_r00 * new_r01)))),
13204 -1);
13205 if (!x625.valid)
13206 {
13207 continue;
13208 }
13209 j0array[0] = ((-1.5707963267949) + (x624.value) +
13210 (((1.5707963267949) * (x625.value))));
13211 sj0array[0] = IKsin(j0array[0]);
13212 cj0array[0] = IKcos(j0array[0]);
13213 if (j0array[0] > IKPI)
13214 {
13215 j0array[0] -= IK2PI;
13216 }
13217 else if (j0array[0] < -IKPI)
13218 {
13219 j0array[0] += IK2PI;
13220 }
13221 j0valid[0] = true;
13222 for (int ij0 = 0; ij0 < 1; ++ij0)
13223 {
13224 if (!j0valid[ij0])
13225 {
13226 continue;
13227 }
13228 _ij0[0] = ij0;
13229 _ij0[1] = -1;
13230 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
13231 {
13232 if (j0valid[iij0] &&
13233 IKabs(cj0array[ij0] - cj0array[iij0]) <
13234 IKFAST_SOLUTION_THRESH &&
13235 IKabs(sj0array[ij0] - sj0array[iij0]) <
13236 IKFAST_SOLUTION_THRESH)
13237 {
13238 j0valid[iij0] = false;
13239 _ij0[1] = iij0;
13240 break;
13241 }
13242 }
13243 j0 = j0array[ij0];
13244 cj0 = cj0array[ij0];
13245 sj0 = sj0array[ij0];
13246 {
13247 IkReal evalcond[8];
13248 IkReal x626 = IKcos(j0);
13249 IkReal x627 = IKsin(j0);
13250 IkReal x628 = (gconst7 * x626);
13251 IkReal x629 = ((1.0) * x627);
13252 IkReal x630 = (gconst8 * x626);
13253 IkReal x631 = (gconst8 * x629);
13254 evalcond[0] = (gconst7 + ((new_r01 * x626)) +
13255 ((new_r11 * x627)));
13256 evalcond[1] = (x630 + new_r11 + ((gconst7 * x627)));
13257 evalcond[2] = ((((-1.0) * new_r00 * x629)) + gconst7 +
13258 ((new_r10 * x626)));
13259 evalcond[3] = ((((-1.0) * new_r01 * x629)) + gconst8 +
13260 ((new_r11 * x626)));
13261 evalcond[4] = ((((-1.0) * x631)) + x628 + new_r10);
13262 evalcond[5] = ((((-1.0) * x631)) + x628 + new_r01);
13263 evalcond[6] =
13264 ((((-1.0) * gconst8)) + ((new_r00 * x626)) +
13265 ((new_r10 * x627)));
13266 evalcond[7] = ((((-1.0) * gconst7 * x629)) +
13267 (((-1.0) * x630)) + new_r00);
13268 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
13269 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
13270 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
13271 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
13272 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
13273 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
13274 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
13275 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
13276 {
13277 continue;
13278 }
13279 }
13280
13281 {
13282 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(
13283 7);
13284 vinfos[0].jointtype = 1;
13285 vinfos[0].foffset = j0;
13286 vinfos[0].indices[0] = _ij0[0];
13287 vinfos[0].indices[1] = _ij0[1];
13288 vinfos[0].maxsolutions = _nj0;
13289 vinfos[1].jointtype = 1;
13290 vinfos[1].foffset = j1;
13291 vinfos[1].indices[0] = _ij1[0];
13292 vinfos[1].indices[1] = _ij1[1];
13293 vinfos[1].maxsolutions = _nj1;
13294 vinfos[2].jointtype = 1;
13295 vinfos[2].foffset = j2;
13296 vinfos[2].indices[0] = _ij2[0];
13297 vinfos[2].indices[1] = _ij2[1];
13298 vinfos[2].maxsolutions = _nj2;
13299 vinfos[3].jointtype = 1;
13300 vinfos[3].foffset = j3;
13301 vinfos[3].indices[0] = _ij3[0];
13302 vinfos[3].indices[1] = _ij3[1];
13303 vinfos[3].maxsolutions = _nj3;
13304 vinfos[4].jointtype = 1;
13305 vinfos[4].foffset = j4;
13306 vinfos[4].indices[0] = _ij4[0];
13307 vinfos[4].indices[1] = _ij4[1];
13308 vinfos[4].maxsolutions = _nj4;
13309 vinfos[5].jointtype = 1;
13310 vinfos[5].foffset = j5;
13311 vinfos[5].indices[0] = _ij5[0];
13312 vinfos[5].indices[1] = _ij5[1];
13313 vinfos[5].maxsolutions = _nj5;
13314 vinfos[6].jointtype = 1;
13315 vinfos[6].foffset = j6;
13316 vinfos[6].indices[0] = _ij6[0];
13317 vinfos[6].indices[1] = _ij6[1];
13318 vinfos[6].maxsolutions = _nj6;
13319 std::vector<int> vfree(0);
13320 solutions.AddSolution(vinfos, vfree);
13321 }
13322 }
13323 }
13324 }
13325 }
13326 }
13327 } while (0);
13328 if (bgotonextstatement)
13329 {
13330 bool bgotonextstatement = true;
13331 do
13332 {
13333 IkReal x633 = ((new_r01 * new_r01) + (new_r11 * new_r11));
13334 if (IKabs(x633) == 0)
13335 {
13336 continue;
13337 }
13338 IkReal x632 = pow(x633, -0.5);
13339 CheckValue<IkReal> x634 = IKatan2WithCheck(
13340 IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
13341 if (!x634.valid)
13342 {
13343 continue;
13344 }
13345 IkReal gconst9 =
13346 ((3.14159265358979) + (((-1.0) * (x634.value))));
13347 IkReal gconst10 = ((1.0) * new_r01 * x632);
13348 IkReal gconst11 = ((-1.0) * new_r11 * x632);
13349 CheckValue<IkReal> x635 = IKatan2WithCheck(
13350 IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
13351 if (!x635.valid)
13352 {
13353 continue;
13354 }
13355 evalcond[0] =
13356 ((-3.14159265358979) +
13357 (IKfmod(
13358 ((3.14159265358979) +
13359 (IKabs(((-3.14159265358979) + (x635.value) + j2)))),
13360 6.28318530717959)));
13361 if (IKabs(evalcond[0]) < 0.0000050000000000)
13362 {
13363 bgotonextstatement = false;
13364 {
13365 IkReal j0eval[3];
13366 CheckValue<IkReal> x638 =
13367 IKatan2WithCheck(IkReal(new_r01),
13368 IkReal(new_r11),
13369 IKFAST_ATAN2_MAGTHRESH);
13370 if (!x638.valid)
13371 {
13372 continue;
13373 }
13374 IkReal x636 = ((1.0) * (x638.value));
13375 IkReal x637 = x632;
13376 sj1 = 0;
13377 cj1 = -1.0;
13378 j1 = 3.14159265358979;
13379 sj2 = gconst10;
13380 cj2 = gconst11;
13381 j2 = ((3.14159265) + (((-1.0) * x636)));
13382 IkReal gconst9 = ((3.14159265358979) + (((-1.0) * x636)));
13383 IkReal gconst10 = ((1.0) * new_r01 * x637);
13384 IkReal gconst11 = ((-1.0) * new_r11 * x637);
13385 IkReal x639 = new_r01 * new_r01;
13386 IkReal x640 =
13387 (((new_r10 * new_r11)) + ((new_r00 * new_r01)));
13388 IkReal x641 = x632;
13389 IkReal x642 = ((1.0) * new_r01 * x641);
13390 j0eval[0] = x640;
13391 j0eval[1] = ((IKabs((((x639 * x641)) +
13392 (((-1.0) * new_r10 * x642))))) +
13393 (IKabs(((((-1.0) * new_r11 * x642)) +
13394 (((-1.0) * new_r00 * x642))))));
13395 j0eval[2] = IKsign(x640);
13396 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
13397 IKabs(j0eval[1]) < 0.0000010000000000 ||
13398 IKabs(j0eval[2]) < 0.0000010000000000)
13399 {
13400 {
13401 IkReal j0eval[2];
13402 CheckValue<IkReal> x645 =
13403 IKatan2WithCheck(IkReal(new_r01),
13404 IkReal(new_r11),
13405 IKFAST_ATAN2_MAGTHRESH);
13406 if (!x645.valid)
13407 {
13408 continue;
13409 }
13410 IkReal x643 = ((1.0) * (x645.value));
13411 IkReal x644 = x632;
13412 sj1 = 0;
13413 cj1 = -1.0;
13414 j1 = 3.14159265358979;
13415 sj2 = gconst10;
13416 cj2 = gconst11;
13417 j2 = ((3.14159265) + (((-1.0) * x643)));
13418 IkReal gconst9 =
13419 ((3.14159265358979) + (((-1.0) * x643)));
13420 IkReal gconst10 = ((1.0) * new_r01 * x644);
13421 IkReal gconst11 = ((-1.0) * new_r11 * x644);
13422 IkReal x646 =
13423 ((new_r01 * new_r01) + (new_r11 * new_r11));
13424 j0eval[0] = x646;
13425 j0eval[1] = IKsign(x646);
13426 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
13427 IKabs(j0eval[1]) < 0.0000010000000000)
13428 {
13429 {
13430 IkReal j0eval[1];
13431 CheckValue<IkReal> x649 =
13432 IKatan2WithCheck(IkReal(new_r01),
13433 IkReal(new_r11),
13434 IKFAST_ATAN2_MAGTHRESH);
13435 if (!x649.valid)
13436 {
13437 continue;
13438 }
13439 IkReal x647 = ((1.0) * (x649.value));
13440 IkReal x648 = x632;
13441 sj1 = 0;
13442 cj1 = -1.0;
13443 j1 = 3.14159265358979;
13444 sj2 = gconst10;
13445 cj2 = gconst11;
13446 j2 = ((3.14159265) + (((-1.0) * x647)));
13447 IkReal gconst9 =
13448 ((3.14159265358979) + (((-1.0) * x647)));
13449 IkReal gconst10 = ((1.0) * new_r01 * x648);
13450 IkReal gconst11 = ((-1.0) * new_r11 * x648);
13451 IkReal x650 = new_r01 * new_r01;
13452 IkReal x651 = new_r11 * new_r11;
13453 IkReal x652 = ((1.0) * x650);
13454 CheckValue<IkReal> x658 =
13455 IKPowWithIntegerCheck((x650 + x651), -1);
13456 if (!x658.valid)
13457 {
13458 continue;
13459 }
13460 IkReal x653 = x658.value;
13461 CheckValue<IkReal> x659 = IKPowWithIntegerCheck(
13462 ((((-1.0) * x651)) + (((-1.0) * x652))), -1);
13463 if (!x659.valid)
13464 {
13465 continue;
13466 }
13467 IkReal x654 = x659.value;
13468 IkReal x655 = ((1.0) * x654);
13469 IkReal x656 = (new_r11 * x655);
13470 IkReal x657 = (new_r01 * x655);
13471 j0eval[0] =
13472 ((IKabs(((((-1.0) * new_r01 * x656)) +
13473 (((-1.0) * x656 *
13474 (new_r01 * new_r01 * new_r01))) +
13475 (((-1.0) * new_r01 * x656 *
13476 (new_r11 * new_r11)))))) +
13477 (IKabs((((x650 * x651 * x653)) +
13478 ((x653 * (x651 * x651))) +
13479 (((-1.0) * x652 * x653))))));
13480 if (IKabs(j0eval[0]) < 0.0000010000000000)
13481 {
13482 {
13483 IkReal evalcond[3];
13484 bool bgotonextstatement = true;
13485 do
13486 {
13487 evalcond[0] =
13488 ((IKabs(new_r11)) + (IKabs(new_r00)));
13489 if (IKabs(evalcond[0]) < 0.0000050000000000)
13490 {
13491 bgotonextstatement = false;
13492 {
13493 IkReal j0eval[1];
13494 CheckValue<IkReal> x661 =
13495 IKatan2WithCheck(
13496 IkReal(new_r01),
13497 IkReal(0),
13498 IKFAST_ATAN2_MAGTHRESH);
13499 if (!x661.valid)
13500 {
13501 continue;
13502 }
13503 IkReal x660 = ((1.0) * (x661.value));
13504 sj1 = 0;
13505 cj1 = -1.0;
13506 j1 = 3.14159265358979;
13507 sj2 = gconst10;
13508 cj2 = gconst11;
13509 j2 = ((3.14159265) + (((-1.0) * x660)));
13510 new_r11 = 0;
13511 new_r00 = 0;
13512 IkReal gconst9 = ((3.14159265358979) +
13513 (((-1.0) * x660)));
13514 IkReal x662 = new_r01 * new_r01;
13515 if (IKabs(x662) == 0)
13516 {
13517 continue;
13518 }
13519 IkReal gconst10 =
13520 ((1.0) * new_r01 * (pow(x662, -0.5)));
13521 IkReal gconst11 = 0;
13522 j0eval[0] = new_r10;
13523 if (IKabs(j0eval[0]) < 0.0000010000000000)
13524 {
13525 {
13526 IkReal j0array[2], cj0array[2],
13527 sj0array[2];
13528 bool j0valid[2] = { false };
13529 _nj0 = 2;
13530 CheckValue<IkReal> x663 =
13531 IKPowWithIntegerCheck(gconst10,
13532 -1);
13533 if (!x663.valid)
13534 {
13535 continue;
13536 }
13537 cj0array[0] =
13538 ((-1.0) * new_r10 * (x663.value));
13539 if (cj0array[0] >=
13540 -1 - IKFAST_SINCOS_THRESH &&
13541 cj0array[0] <=
13542 1 + IKFAST_SINCOS_THRESH)
13543 {
13544 j0valid[0] = j0valid[1] = true;
13545 j0array[0] = IKacos(cj0array[0]);
13546 sj0array[0] = IKsin(j0array[0]);
13547 cj0array[1] = cj0array[0];
13548 j0array[1] = -j0array[0];
13549 sj0array[1] = -sj0array[0];
13550 }
13551 else if (isnan(cj0array[0]))
13552 {
13553 // probably any value will work
13554 j0valid[0] = true;
13555 cj0array[0] = 1;
13556 sj0array[0] = 0;
13557 j0array[0] = 0;
13558 }
13559 for (int ij0 = 0; ij0 < 2; ++ij0)
13560 {
13561 if (!j0valid[ij0])
13562 {
13563 continue;
13564 }
13565 _ij0[0] = ij0;
13566 _ij0[1] = -1;
13567 for (int iij0 = ij0 + 1; iij0 < 2;
13568 ++iij0)
13569 {
13570 if (j0valid[iij0] &&
13571 IKabs(cj0array[ij0] -
13572 cj0array[iij0]) <
13573 IKFAST_SOLUTION_THRESH &&
13574 IKabs(sj0array[ij0] -
13575 sj0array[iij0]) <
13576 IKFAST_SOLUTION_THRESH)
13577 {
13578 j0valid[iij0] = false;
13579 _ij0[1] = iij0;
13580 break;
13581 }
13582 }
13583 j0 = j0array[ij0];
13584 cj0 = cj0array[ij0];
13585 sj0 = sj0array[ij0];
13586 {
13587 IkReal evalcond[6];
13588 IkReal x664 = IKsin(j0);
13589 IkReal x665 = IKcos(j0);
13590 IkReal x666 = ((-1.0) * x664);
13591 evalcond[0] = (new_r10 * x664);
13592 evalcond[1] = (new_r01 * x666);
13593 evalcond[2] = (gconst10 * x666);
13594 evalcond[3] =
13595 (gconst10 +
13596 ((new_r10 * x665)));
13597 evalcond[4] =
13598 (gconst10 +
13599 ((new_r01 * x665)));
13600 evalcond[5] =
13601 (new_r01 +
13602 ((gconst10 * x665)));
13603 if (IKabs(evalcond[0]) >
13604 IKFAST_EVALCOND_THRESH ||
13605 IKabs(evalcond[1]) >
13606 IKFAST_EVALCOND_THRESH ||
13607 IKabs(evalcond[2]) >
13608 IKFAST_EVALCOND_THRESH ||
13609 IKabs(evalcond[3]) >
13610 IKFAST_EVALCOND_THRESH ||
13611 IKabs(evalcond[4]) >
13612 IKFAST_EVALCOND_THRESH ||
13613 IKabs(evalcond[5]) >
13614 IKFAST_EVALCOND_THRESH)
13615 {
13616 continue;
13617 }
13618 }
13619
13620 {
13621 std::vector<
13622 IkSingleDOFSolutionBase<
13623 IkReal> >
13624 vinfos(7);
13625 vinfos[0].jointtype = 1;
13626 vinfos[0].foffset = j0;
13627 vinfos[0].indices[0] = _ij0[0];
13628 vinfos[0].indices[1] = _ij0[1];
13629 vinfos[0].maxsolutions = _nj0;
13630 vinfos[1].jointtype = 1;
13631 vinfos[1].foffset = j1;
13632 vinfos[1].indices[0] = _ij1[0];
13633 vinfos[1].indices[1] = _ij1[1];
13634 vinfos[1].maxsolutions = _nj1;
13635 vinfos[2].jointtype = 1;
13636 vinfos[2].foffset = j2;
13637 vinfos[2].indices[0] = _ij2[0];
13638 vinfos[2].indices[1] = _ij2[1];
13639 vinfos[2].maxsolutions = _nj2;
13640 vinfos[3].jointtype = 1;
13641 vinfos[3].foffset = j3;
13642 vinfos[3].indices[0] = _ij3[0];
13643 vinfos[3].indices[1] = _ij3[1];
13644 vinfos[3].maxsolutions = _nj3;
13645 vinfos[4].jointtype = 1;
13646 vinfos[4].foffset = j4;
13647 vinfos[4].indices[0] = _ij4[0];
13648 vinfos[4].indices[1] = _ij4[1];
13649 vinfos[4].maxsolutions = _nj4;
13650 vinfos[5].jointtype = 1;
13651 vinfos[5].foffset = j5;
13652 vinfos[5].indices[0] = _ij5[0];
13653 vinfos[5].indices[1] = _ij5[1];
13654 vinfos[5].maxsolutions = _nj5;
13655 vinfos[6].jointtype = 1;
13656 vinfos[6].foffset = j6;
13657 vinfos[6].indices[0] = _ij6[0];
13658 vinfos[6].indices[1] = _ij6[1];
13659 vinfos[6].maxsolutions = _nj6;
13660 std::vector<int> vfree(0);
13661 solutions.AddSolution(vinfos,
13662 vfree);
13663 }
13664 }
13665 }
13666 }
13667 else
13668 {
13669 {
13670 IkReal j0array[2], cj0array[2],
13671 sj0array[2];
13672 bool j0valid[2] = { false };
13673 _nj0 = 2;
13674 CheckValue<IkReal> x667 =
13675 IKPowWithIntegerCheck(new_r10,
13676 -1);
13677 if (!x667.valid)
13678 {
13679 continue;
13680 }
13681 cj0array[0] = ((-1.0) * gconst10 *
13682 (x667.value));
13683 if (cj0array[0] >=
13684 -1 - IKFAST_SINCOS_THRESH &&
13685 cj0array[0] <=
13686 1 + IKFAST_SINCOS_THRESH)
13687 {
13688 j0valid[0] = j0valid[1] = true;
13689 j0array[0] = IKacos(cj0array[0]);
13690 sj0array[0] = IKsin(j0array[0]);
13691 cj0array[1] = cj0array[0];
13692 j0array[1] = -j0array[0];
13693 sj0array[1] = -sj0array[0];
13694 }
13695 else if (isnan(cj0array[0]))
13696 {
13697 // probably any value will work
13698 j0valid[0] = true;
13699 cj0array[0] = 1;
13700 sj0array[0] = 0;
13701 j0array[0] = 0;
13702 }
13703 for (int ij0 = 0; ij0 < 2; ++ij0)
13704 {
13705 if (!j0valid[ij0])
13706 {
13707 continue;
13708 }
13709 _ij0[0] = ij0;
13710 _ij0[1] = -1;
13711 for (int iij0 = ij0 + 1; iij0 < 2;
13712 ++iij0)
13713 {
13714 if (j0valid[iij0] &&
13715 IKabs(cj0array[ij0] -
13716 cj0array[iij0]) <
13717 IKFAST_SOLUTION_THRESH &&
13718 IKabs(sj0array[ij0] -
13719 sj0array[iij0]) <
13720 IKFAST_SOLUTION_THRESH)
13721 {
13722 j0valid[iij0] = false;
13723 _ij0[1] = iij0;
13724 break;
13725 }
13726 }
13727 j0 = j0array[ij0];
13728 cj0 = cj0array[ij0];
13729 sj0 = sj0array[ij0];
13730 {
13731 IkReal evalcond[6];
13732 IkReal x668 = IKsin(j0);
13733 IkReal x669 = IKcos(j0);
13734 IkReal x670 = (gconst10 * x669);
13735 IkReal x671 = ((-1.0) * x668);
13736 evalcond[0] = (new_r10 * x668);
13737 evalcond[1] = (new_r01 * x671);
13738 evalcond[2] = (gconst10 * x671);
13739 evalcond[3] = (x670 + new_r10);
13740 evalcond[4] =
13741 (gconst10 +
13742 ((new_r01 * x669)));
13743 evalcond[5] = (x670 + new_r01);
13744 if (IKabs(evalcond[0]) >
13745 IKFAST_EVALCOND_THRESH ||
13746 IKabs(evalcond[1]) >
13747 IKFAST_EVALCOND_THRESH ||
13748 IKabs(evalcond[2]) >
13749 IKFAST_EVALCOND_THRESH ||
13750 IKabs(evalcond[3]) >
13751 IKFAST_EVALCOND_THRESH ||
13752 IKabs(evalcond[4]) >
13753 IKFAST_EVALCOND_THRESH ||
13754 IKabs(evalcond[5]) >
13755 IKFAST_EVALCOND_THRESH)
13756 {
13757 continue;
13758 }
13759 }
13760
13761 {
13762 std::vector<
13763 IkSingleDOFSolutionBase<
13764 IkReal> >
13765 vinfos(7);
13766 vinfos[0].jointtype = 1;
13767 vinfos[0].foffset = j0;
13768 vinfos[0].indices[0] = _ij0[0];
13769 vinfos[0].indices[1] = _ij0[1];
13770 vinfos[0].maxsolutions = _nj0;
13771 vinfos[1].jointtype = 1;
13772 vinfos[1].foffset = j1;
13773 vinfos[1].indices[0] = _ij1[0];
13774 vinfos[1].indices[1] = _ij1[1];
13775 vinfos[1].maxsolutions = _nj1;
13776 vinfos[2].jointtype = 1;
13777 vinfos[2].foffset = j2;
13778 vinfos[2].indices[0] = _ij2[0];
13779 vinfos[2].indices[1] = _ij2[1];
13780 vinfos[2].maxsolutions = _nj2;
13781 vinfos[3].jointtype = 1;
13782 vinfos[3].foffset = j3;
13783 vinfos[3].indices[0] = _ij3[0];
13784 vinfos[3].indices[1] = _ij3[1];
13785 vinfos[3].maxsolutions = _nj3;
13786 vinfos[4].jointtype = 1;
13787 vinfos[4].foffset = j4;
13788 vinfos[4].indices[0] = _ij4[0];
13789 vinfos[4].indices[1] = _ij4[1];
13790 vinfos[4].maxsolutions = _nj4;
13791 vinfos[5].jointtype = 1;
13792 vinfos[5].foffset = j5;
13793 vinfos[5].indices[0] = _ij5[0];
13794 vinfos[5].indices[1] = _ij5[1];
13795 vinfos[5].maxsolutions = _nj5;
13796 vinfos[6].jointtype = 1;
13797 vinfos[6].foffset = j6;
13798 vinfos[6].indices[0] = _ij6[0];
13799 vinfos[6].indices[1] = _ij6[1];
13800 vinfos[6].maxsolutions = _nj6;
13801 std::vector<int> vfree(0);
13802 solutions.AddSolution(vinfos,
13803 vfree);
13804 }
13805 }
13806 }
13807 }
13808 }
13809 }
13810 } while (0);
13811 if (bgotonextstatement)
13812 {
13813 bool bgotonextstatement = true;
13814 do
13815 {
13816 evalcond[0] =
13817 ((IKabs(new_r10)) + (IKabs(new_r00)));
13818 evalcond[1] = gconst10;
13819 evalcond[2] = gconst11;
13820 if (IKabs(evalcond[0]) <
13821 0.0000050000000000 &&
13822 IKabs(evalcond[1]) <
13823 0.0000050000000000 &&
13824 IKabs(evalcond[2]) < 0.0000050000000000)
13825 {
13826 bgotonextstatement = false;
13827 {
13828 IkReal j0eval[3];
13829 CheckValue<IkReal> x673 =
13830 IKatan2WithCheck(
13831 IkReal(new_r01),
13832 IkReal(new_r11),
13833 IKFAST_ATAN2_MAGTHRESH);
13834 if (!x673.valid)
13835 {
13836 continue;
13837 }
13838 IkReal x672 = ((1.0) * (x673.value));
13839 sj1 = 0;
13840 cj1 = -1.0;
13841 j1 = 3.14159265358979;
13842 sj2 = gconst10;
13843 cj2 = gconst11;
13844 j2 = ((3.14159265) + (((-1.0) * x672)));
13845 new_r00 = 0;
13846 new_r10 = 0;
13847 new_r21 = 0;
13848 new_r22 = 0;
13849 IkReal gconst9 = ((3.14159265358979) +
13850 (((-1.0) * x672)));
13851 IkReal gconst10 = ((1.0) * new_r01);
13852 IkReal gconst11 = ((-1.0) * new_r11);
13853 j0eval[0] = 1.0;
13854 j0eval[1] = 1.0;
13855 j0eval[2] =
13856 ((IKabs(((1.0) +
13857 (((-1.0) * (new_r01 *
13858 new_r01)))))) +
13859 (IKabs(
13860 ((1.0) * new_r01 * new_r11))));
13861 if (IKabs(j0eval[0]) <
13862 0.0000010000000000 ||
13863 IKabs(j0eval[1]) <
13864 0.0000010000000000 ||
13865 IKabs(j0eval[2]) <
13866 0.0000010000000000)
13867 {
13868 {
13869 IkReal j0eval[3];
13870 CheckValue<IkReal> x675 =
13871 IKatan2WithCheck(
13872 IkReal(new_r01),
13873 IkReal(new_r11),
13874 IKFAST_ATAN2_MAGTHRESH);
13875 if (!x675.valid)
13876 {
13877 continue;
13878 }
13879 IkReal x674 =
13880 ((1.0) * (x675.value));
13881 sj1 = 0;
13882 cj1 = -1.0;
13883 j1 = 3.14159265358979;
13884 sj2 = gconst10;
13885 cj2 = gconst11;
13886 j2 = ((3.14159265) +
13887 (((-1.0) * x674)));
13888 new_r00 = 0;
13889 new_r10 = 0;
13890 new_r21 = 0;
13891 new_r22 = 0;
13892 IkReal gconst9 =
13893 ((3.14159265358979) +
13894 (((-1.0) * x674)));
13895 IkReal gconst10 = ((1.0) * new_r01);
13896 IkReal gconst11 =
13897 ((-1.0) * new_r11);
13898 j0eval[0] = -1.0;
13899 j0eval[1] = -1.0;
13900 j0eval[2] =
13901 ((IKabs(((-1.0) + (new_r01 *
13902 new_r01)))) +
13903 (IKabs(((1.0) * new_r01 *
13904 new_r11))));
13905 if (IKabs(j0eval[0]) <
13906 0.0000010000000000 ||
13907 IKabs(j0eval[1]) <
13908 0.0000010000000000 ||
13909 IKabs(j0eval[2]) <
13910 0.0000010000000000)
13911 {
13912 {
13913 IkReal j0eval[3];
13914 CheckValue<IkReal> x677 =
13915 IKatan2WithCheck(
13916 IkReal(new_r01),
13917 IkReal(new_r11),
13918 IKFAST_ATAN2_MAGTHRESH);
13919 if (!x677.valid)
13920 {
13921 continue;
13922 }
13923 IkReal x676 =
13924 ((1.0) * (x677.value));
13925 sj1 = 0;
13926 cj1 = -1.0;
13927 j1 = 3.14159265358979;
13928 sj2 = gconst10;
13929 cj2 = gconst11;
13930 j2 = ((3.14159265) +
13931 (((-1.0) * x676)));
13932 new_r00 = 0;
13933 new_r10 = 0;
13934 new_r21 = 0;
13935 new_r22 = 0;
13936 IkReal gconst9 =
13937 ((3.14159265358979) +
13938 (((-1.0) * x676)));
13939 IkReal gconst10 =
13940 ((1.0) * new_r01);
13941 IkReal gconst11 =
13942 ((-1.0) * new_r11);
13943 j0eval[0] = 1.0;
13944 j0eval[1] = 1.0;
13945 j0eval[2] =
13946 ((IKabs(((2.0) * new_r01 *
13947 new_r11))) +
13948 (IKabs(((1.0) +
13949 (((-2.0) *
13950 (new_r01 *
13951 new_r01)))))));
13952 if (IKabs(j0eval[0]) <
13953 0.0000010000000000 ||
13954 IKabs(j0eval[1]) <
13955 0.0000010000000000 ||
13956 IKabs(j0eval[2]) <
13957 0.0000010000000000)
13958 {
13959 continue; // 3 cases reached
13960 }
13961 else
13962 {
13963 {
13964 IkReal j0array[1],
13965 cj0array[1],
13966 sj0array[1];
13967 bool j0valid[1] = { false };
13968 _nj0 = 1;
13969 IkReal x678 =
13970 ((1.0) * new_r11);
13971 CheckValue<IkReal> x679 =
13972 IKatan2WithCheck(
13973 IkReal(
13974 (((gconst11 *
13975 new_r01)) +
13976 (((-1.0) *
13977 gconst10 *
13978 x678)))),
13979 IkReal(
13980 ((((-1.0) *
13981 gconst11 *
13982 x678)) +
13983 (((-1.0) *
13984 gconst10 *
13985 new_r01)))),
13986 IKFAST_ATAN2_MAGTHRESH);
13987 if (!x679.valid)
13988 {
13989 continue;
13990 }
13991 CheckValue<IkReal> x680 =
13992 IKPowWithIntegerCheck(
13993 IKsign(((new_r01 *
13994 new_r01) +
13995 (new_r11 *
13996 new_r11))),
13997 -1);
13998 if (!x680.valid)
13999 {
14000 continue;
14001 }
14002 j0array[0] =
14003 ((-1.5707963267949) +
14004 (x679.value) +
14005 (((1.5707963267949) *
14006 (x680.value))));
14007 sj0array[0] =
14008 IKsin(j0array[0]);
14009 cj0array[0] =
14010 IKcos(j0array[0]);
14011 if (j0array[0] > IKPI)
14012 {
14013 j0array[0] -= IK2PI;
14014 }
14015 else if (j0array[0] < -IKPI)
14016 {
14017 j0array[0] += IK2PI;
14018 }
14019 j0valid[0] = true;
14020 for (int ij0 = 0; ij0 < 1;
14021 ++ij0)
14022 {
14023 if (!j0valid[ij0])
14024 {
14025 continue;
14026 }
14027 _ij0[0] = ij0;
14028 _ij0[1] = -1;
14029 for (int iij0 = ij0 + 1;
14030 iij0 < 1;
14031 ++iij0)
14032 {
14033 if (j0valid[iij0] &&
14034 IKabs(
14035 cj0array[ij0] -
14036 cj0array
14037 [iij0]) <
14038 IKFAST_SOLUTION_THRESH &&
14039 IKabs(
14040 sj0array[ij0] -
14041 sj0array
14042 [iij0]) <
14043 IKFAST_SOLUTION_THRESH)
14044 {
14045 j0valid[iij0] = false;
14046 _ij0[1] = iij0;
14047 break;
14048 }
14049 }
14050 j0 = j0array[ij0];
14051 cj0 = cj0array[ij0];
14052 sj0 = sj0array[ij0];
14053 {
14054 IkReal evalcond[6];
14055 IkReal x681 = IKsin(j0);
14056 IkReal x682 = IKcos(j0);
14057 IkReal x683 =
14058 (gconst10 * x682);
14059 IkReal x684 =
14060 (gconst11 * x682);
14061 IkReal x685 =
14062 (gconst10 * x681);
14063 IkReal x686 =
14064 ((1.0) * x681);
14065 IkReal x687 =
14066 (gconst11 * x686);
14067 evalcond[0] =
14068 ((((-1.0) * x687)) +
14069 x683);
14070 evalcond[1] =
14071 (gconst10 +
14072 ((new_r01 *
14073 x682)) +
14074 ((new_r11 *
14075 x681)));
14076 evalcond[2] =
14077 (x685 + x684 +
14078 new_r11);
14079 evalcond[3] =
14080 ((((-1.0) *
14081 new_r01 *
14082 x686)) +
14083 gconst11 +
14084 ((new_r11 *
14085 x682)));
14086 evalcond[4] =
14087 ((((-1.0) * x685)) +
14088 (((-1.0) * x684)));
14089 evalcond[5] =
14090 ((((-1.0) * x687)) +
14091 x683 + new_r01);
14092 if (IKabs(evalcond[0]) >
14093 IKFAST_EVALCOND_THRESH ||
14094 IKabs(evalcond[1]) >
14095 IKFAST_EVALCOND_THRESH ||
14096 IKabs(evalcond[2]) >
14097 IKFAST_EVALCOND_THRESH ||
14098 IKabs(evalcond[3]) >
14099 IKFAST_EVALCOND_THRESH ||
14100 IKabs(evalcond[4]) >
14101 IKFAST_EVALCOND_THRESH ||
14102 IKabs(evalcond[5]) >
14103 IKFAST_EVALCOND_THRESH)
14104 {
14105 continue;
14106 }
14107 }
14108
14109 {
14110 std::vector<
14111 IkSingleDOFSolutionBase<
14112 IkReal> >
14113 vinfos(7);
14114 vinfos[0].jointtype = 1;
14115 vinfos[0].foffset = j0;
14116 vinfos[0].indices[0] =
14117 _ij0[0];
14118 vinfos[0].indices[1] =
14119 _ij0[1];
14120 vinfos[0].maxsolutions =
14121 _nj0;
14122 vinfos[1].jointtype = 1;
14123 vinfos[1].foffset = j1;
14124 vinfos[1].indices[0] =
14125 _ij1[0];
14126 vinfos[1].indices[1] =
14127 _ij1[1];
14128 vinfos[1].maxsolutions =
14129 _nj1;
14130 vinfos[2].jointtype = 1;
14131 vinfos[2].foffset = j2;
14132 vinfos[2].indices[0] =
14133 _ij2[0];
14134 vinfos[2].indices[1] =
14135 _ij2[1];
14136 vinfos[2].maxsolutions =
14137 _nj2;
14138 vinfos[3].jointtype = 1;
14139 vinfos[3].foffset = j3;
14140 vinfos[3].indices[0] =
14141 _ij3[0];
14142 vinfos[3].indices[1] =
14143 _ij3[1];
14144 vinfos[3].maxsolutions =
14145 _nj3;
14146 vinfos[4].jointtype = 1;
14147 vinfos[4].foffset = j4;
14148 vinfos[4].indices[0] =
14149 _ij4[0];
14150 vinfos[4].indices[1] =
14151 _ij4[1];
14152 vinfos[4].maxsolutions =
14153 _nj4;
14154 vinfos[5].jointtype = 1;
14155 vinfos[5].foffset = j5;
14156 vinfos[5].indices[0] =
14157 _ij5[0];
14158 vinfos[5].indices[1] =
14159 _ij5[1];
14160 vinfos[5].maxsolutions =
14161 _nj5;
14162 vinfos[6].jointtype = 1;
14163 vinfos[6].foffset = j6;
14164 vinfos[6].indices[0] =
14165 _ij6[0];
14166 vinfos[6].indices[1] =
14167 _ij6[1];
14168 vinfos[6].maxsolutions =
14169 _nj6;
14170 std::vector<int> vfree(
14171 0);
14172 solutions.AddSolution(
14173 vinfos, vfree);
14174 }
14175 }
14176 }
14177 }
14178 }
14179 }
14180 else
14181 {
14182 {
14183 IkReal j0array[1], cj0array[1],
14184 sj0array[1];
14185 bool j0valid[1] = { false };
14186 _nj0 = 1;
14187 CheckValue<IkReal> x688 =
14188 IKPowWithIntegerCheck(
14189 IKsign(
14190 ((((-1.0) *
14191 (gconst11 *
14192 gconst11))) +
14193 (((-1.0) *
14194 (gconst10 *
14195 gconst10))))),
14196 -1);
14197 if (!x688.valid)
14198 {
14199 continue;
14200 }
14201 CheckValue<IkReal> x689 =
14202 IKatan2WithCheck(
14203 IkReal((gconst10 *
14204 new_r11)),
14205 IkReal((gconst11 *
14206 new_r11)),
14207 IKFAST_ATAN2_MAGTHRESH);
14208 if (!x689.valid)
14209 {
14210 continue;
14211 }
14212 j0array[0] =
14213 ((-1.5707963267949) +
14214 (((1.5707963267949) *
14215 (x688.value))) +
14216 (x689.value));
14217 sj0array[0] = IKsin(j0array[0]);
14218 cj0array[0] = IKcos(j0array[0]);
14219 if (j0array[0] > IKPI)
14220 {
14221 j0array[0] -= IK2PI;
14222 }
14223 else if (j0array[0] < -IKPI)
14224 {
14225 j0array[0] += IK2PI;
14226 }
14227 j0valid[0] = true;
14228 for (int ij0 = 0; ij0 < 1;
14229 ++ij0)
14230 {
14231 if (!j0valid[ij0])
14232 {
14233 continue;
14234 }
14235 _ij0[0] = ij0;
14236 _ij0[1] = -1;
14237 for (int iij0 = ij0 + 1;
14238 iij0 < 1;
14239 ++iij0)
14240 {
14241 if (j0valid[iij0] &&
14242 IKabs(cj0array[ij0] -
14243 cj0array[iij0]) <
14244 IKFAST_SOLUTION_THRESH &&
14245 IKabs(sj0array[ij0] -
14246 sj0array[iij0]) <
14247 IKFAST_SOLUTION_THRESH)
14248 {
14249 j0valid[iij0] = false;
14250 _ij0[1] = iij0;
14251 break;
14252 }
14253 }
14254 j0 = j0array[ij0];
14255 cj0 = cj0array[ij0];
14256 sj0 = sj0array[ij0];
14257 {
14258 IkReal evalcond[6];
14259 IkReal x690 = IKsin(j0);
14260 IkReal x691 = IKcos(j0);
14261 IkReal x692 =
14262 (gconst10 * x691);
14263 IkReal x693 =
14264 (gconst11 * x691);
14265 IkReal x694 =
14266 (gconst10 * x690);
14267 IkReal x695 =
14268 ((1.0) * x690);
14269 IkReal x696 =
14270 (gconst11 * x695);
14271 evalcond[0] =
14272 (x692 +
14273 (((-1.0) * x696)));
14274 evalcond[1] =
14275 (gconst10 +
14276 ((new_r11 * x690)) +
14277 ((new_r01 * x691)));
14278 evalcond[2] =
14279 (x694 + x693 + new_r11);
14280 evalcond[3] =
14281 ((((-1.0) * new_r01 *
14282 x695)) +
14283 gconst11 +
14284 ((new_r11 * x691)));
14285 evalcond[4] =
14286 ((((-1.0) * x693)) +
14287 (((-1.0) * x694)));
14288 evalcond[5] =
14289 (x692 +
14290 (((-1.0) * x696)) +
14291 new_r01);
14292 if (IKabs(evalcond[0]) >
14293 IKFAST_EVALCOND_THRESH ||
14294 IKabs(evalcond[1]) >
14295 IKFAST_EVALCOND_THRESH ||
14296 IKabs(evalcond[2]) >
14297 IKFAST_EVALCOND_THRESH ||
14298 IKabs(evalcond[3]) >
14299 IKFAST_EVALCOND_THRESH ||
14300 IKabs(evalcond[4]) >
14301 IKFAST_EVALCOND_THRESH ||
14302 IKabs(evalcond[5]) >
14303 IKFAST_EVALCOND_THRESH)
14304 {
14305 continue;
14306 }
14307 }
14308
14309 {
14310 std::vector<
14311 IkSingleDOFSolutionBase<
14312 IkReal> >
14313 vinfos(7);
14314 vinfos[0].jointtype = 1;
14315 vinfos[0].foffset = j0;
14316 vinfos[0].indices[0] =
14317 _ij0[0];
14318 vinfos[0].indices[1] =
14319 _ij0[1];
14320 vinfos[0].maxsolutions =
14321 _nj0;
14322 vinfos[1].jointtype = 1;
14323 vinfos[1].foffset = j1;
14324 vinfos[1].indices[0] =
14325 _ij1[0];
14326 vinfos[1].indices[1] =
14327 _ij1[1];
14328 vinfos[1].maxsolutions =
14329 _nj1;
14330 vinfos[2].jointtype = 1;
14331 vinfos[2].foffset = j2;
14332 vinfos[2].indices[0] =
14333 _ij2[0];
14334 vinfos[2].indices[1] =
14335 _ij2[1];
14336 vinfos[2].maxsolutions =
14337 _nj2;
14338 vinfos[3].jointtype = 1;
14339 vinfos[3].foffset = j3;
14340 vinfos[3].indices[0] =
14341 _ij3[0];
14342 vinfos[3].indices[1] =
14343 _ij3[1];
14344 vinfos[3].maxsolutions =
14345 _nj3;
14346 vinfos[4].jointtype = 1;
14347 vinfos[4].foffset = j4;
14348 vinfos[4].indices[0] =
14349 _ij4[0];
14350 vinfos[4].indices[1] =
14351 _ij4[1];
14352 vinfos[4].maxsolutions =
14353 _nj4;
14354 vinfos[5].jointtype = 1;
14355 vinfos[5].foffset = j5;
14356 vinfos[5].indices[0] =
14357 _ij5[0];
14358 vinfos[5].indices[1] =
14359 _ij5[1];
14360 vinfos[5].maxsolutions =
14361 _nj5;
14362 vinfos[6].jointtype = 1;
14363 vinfos[6].foffset = j6;
14364 vinfos[6].indices[0] =
14365 _ij6[0];
14366 vinfos[6].indices[1] =
14367 _ij6[1];
14368 vinfos[6].maxsolutions =
14369 _nj6;
14370 std::vector<int> vfree(0);
14371 solutions.AddSolution(
14372 vinfos, vfree);
14373 }
14374 }
14375 }
14376 }
14377 }
14378 }
14379 else
14380 {
14381 {
14382 IkReal j0array[1], cj0array[1],
14383 sj0array[1];
14384 bool j0valid[1] = { false };
14385 _nj0 = 1;
14386 CheckValue<IkReal> x697 =
14387 IKPowWithIntegerCheck(
14388 IKsign((
14389 ((gconst10 * new_r01)) +
14390 (((-1.0) * gconst11 *
14391 new_r11)))),
14392 -1);
14393 if (!x697.valid)
14394 {
14395 continue;
14396 }
14397 CheckValue<IkReal> x698 =
14398 IKatan2WithCheck(
14399 IkReal(
14400 (gconst10 * gconst11)),
14401 IkReal(gconst11 * gconst11),
14402 IKFAST_ATAN2_MAGTHRESH);
14403 if (!x698.valid)
14404 {
14405 continue;
14406 }
14407 j0array[0] = ((-1.5707963267949) +
14408 (((1.5707963267949) *
14409 (x697.value))) +
14410 (x698.value));
14411 sj0array[0] = IKsin(j0array[0]);
14412 cj0array[0] = IKcos(j0array[0]);
14413 if (j0array[0] > IKPI)
14414 {
14415 j0array[0] -= IK2PI;
14416 }
14417 else if (j0array[0] < -IKPI)
14418 {
14419 j0array[0] += IK2PI;
14420 }
14421 j0valid[0] = true;
14422 for (int ij0 = 0; ij0 < 1; ++ij0)
14423 {
14424 if (!j0valid[ij0])
14425 {
14426 continue;
14427 }
14428 _ij0[0] = ij0;
14429 _ij0[1] = -1;
14430 for (int iij0 = ij0 + 1; iij0 < 1;
14431 ++iij0)
14432 {
14433 if (j0valid[iij0] &&
14434 IKabs(cj0array[ij0] -
14435 cj0array[iij0]) <
14436 IKFAST_SOLUTION_THRESH &&
14437 IKabs(sj0array[ij0] -
14438 sj0array[iij0]) <
14439 IKFAST_SOLUTION_THRESH)
14440 {
14441 j0valid[iij0] = false;
14442 _ij0[1] = iij0;
14443 break;
14444 }
14445 }
14446 j0 = j0array[ij0];
14447 cj0 = cj0array[ij0];
14448 sj0 = sj0array[ij0];
14449 {
14450 IkReal evalcond[6];
14451 IkReal x699 = IKsin(j0);
14452 IkReal x700 = IKcos(j0);
14453 IkReal x701 = (gconst10 * x700);
14454 IkReal x702 = (gconst11 * x700);
14455 IkReal x703 = (gconst10 * x699);
14456 IkReal x704 = ((1.0) * x699);
14457 IkReal x705 = (gconst11 * x704);
14458 evalcond[0] =
14459 ((((-1.0) * x705)) + x701);
14460 evalcond[1] =
14461 (((new_r01 * x700)) +
14462 gconst10 +
14463 ((new_r11 * x699)));
14464 evalcond[2] =
14465 (x702 + x703 + new_r11);
14466 evalcond[3] =
14467 (gconst11 +
14468 ((new_r11 * x700)) +
14469 (((-1.0) * new_r01 *
14470 x704)));
14471 evalcond[4] =
14472 ((((-1.0) * x702)) +
14473 (((-1.0) * x703)));
14474 evalcond[5] =
14475 ((((-1.0) * x705)) + x701 +
14476 new_r01);
14477 if (IKabs(evalcond[0]) >
14478 IKFAST_EVALCOND_THRESH ||
14479 IKabs(evalcond[1]) >
14480 IKFAST_EVALCOND_THRESH ||
14481 IKabs(evalcond[2]) >
14482 IKFAST_EVALCOND_THRESH ||
14483 IKabs(evalcond[3]) >
14484 IKFAST_EVALCOND_THRESH ||
14485 IKabs(evalcond[4]) >
14486 IKFAST_EVALCOND_THRESH ||
14487 IKabs(evalcond[5]) >
14488 IKFAST_EVALCOND_THRESH)
14489 {
14490 continue;
14491 }
14492 }
14493
14494 {
14495 std::vector<
14496 IkSingleDOFSolutionBase<
14497 IkReal> >
14498 vinfos(7);
14499 vinfos[0].jointtype = 1;
14500 vinfos[0].foffset = j0;
14501 vinfos[0].indices[0] = _ij0[0];
14502 vinfos[0].indices[1] = _ij0[1];
14503 vinfos[0].maxsolutions = _nj0;
14504 vinfos[1].jointtype = 1;
14505 vinfos[1].foffset = j1;
14506 vinfos[1].indices[0] = _ij1[0];
14507 vinfos[1].indices[1] = _ij1[1];
14508 vinfos[1].maxsolutions = _nj1;
14509 vinfos[2].jointtype = 1;
14510 vinfos[2].foffset = j2;
14511 vinfos[2].indices[0] = _ij2[0];
14512 vinfos[2].indices[1] = _ij2[1];
14513 vinfos[2].maxsolutions = _nj2;
14514 vinfos[3].jointtype = 1;
14515 vinfos[3].foffset = j3;
14516 vinfos[3].indices[0] = _ij3[0];
14517 vinfos[3].indices[1] = _ij3[1];
14518 vinfos[3].maxsolutions = _nj3;
14519 vinfos[4].jointtype = 1;
14520 vinfos[4].foffset = j4;
14521 vinfos[4].indices[0] = _ij4[0];
14522 vinfos[4].indices[1] = _ij4[1];
14523 vinfos[4].maxsolutions = _nj4;
14524 vinfos[5].jointtype = 1;
14525 vinfos[5].foffset = j5;
14526 vinfos[5].indices[0] = _ij5[0];
14527 vinfos[5].indices[1] = _ij5[1];
14528 vinfos[5].maxsolutions = _nj5;
14529 vinfos[6].jointtype = 1;
14530 vinfos[6].foffset = j6;
14531 vinfos[6].indices[0] = _ij6[0];
14532 vinfos[6].indices[1] = _ij6[1];
14533 vinfos[6].maxsolutions = _nj6;
14534 std::vector<int> vfree(0);
14535 solutions.AddSolution(vinfos,
14536 vfree);
14537 }
14538 }
14539 }
14540 }
14541 }
14542 }
14543 } while (0);
14544 if (bgotonextstatement)
14545 {
14546 bool bgotonextstatement = true;
14547 do
14548 {
14549 evalcond[0] =
14550 ((IKabs(new_r10)) + (IKabs(new_r01)));
14551 if (IKabs(evalcond[0]) <
14552 0.0000050000000000)
14553 {
14554 bgotonextstatement = false;
14555 {
14556 IkReal j0array[2], cj0array[2],
14557 sj0array[2];
14558 bool j0valid[2] = { false };
14559 _nj0 = 2;
14560 CheckValue<IkReal> x706 =
14561 IKPowWithIntegerCheck(gconst11,
14562 -1);
14563 if (!x706.valid)
14564 {
14565 continue;
14566 }
14567 cj0array[0] =
14568 (new_r00 * (x706.value));
14569 if (cj0array[0] >=
14570 -1 - IKFAST_SINCOS_THRESH &&
14571 cj0array[0] <=
14572 1 + IKFAST_SINCOS_THRESH)
14573 {
14574 j0valid[0] = j0valid[1] = true;
14575 j0array[0] = IKacos(cj0array[0]);
14576 sj0array[0] = IKsin(j0array[0]);
14577 cj0array[1] = cj0array[0];
14578 j0array[1] = -j0array[0];
14579 sj0array[1] = -sj0array[0];
14580 }
14581 else if (isnan(cj0array[0]))
14582 {
14583 // probably any value will work
14584 j0valid[0] = true;
14585 cj0array[0] = 1;
14586 sj0array[0] = 0;
14587 j0array[0] = 0;
14588 }
14589 for (int ij0 = 0; ij0 < 2; ++ij0)
14590 {
14591 if (!j0valid[ij0])
14592 {
14593 continue;
14594 }
14595 _ij0[0] = ij0;
14596 _ij0[1] = -1;
14597 for (int iij0 = ij0 + 1; iij0 < 2;
14598 ++iij0)
14599 {
14600 if (j0valid[iij0] &&
14601 IKabs(cj0array[ij0] -
14602 cj0array[iij0]) <
14603 IKFAST_SOLUTION_THRESH &&
14604 IKabs(sj0array[ij0] -
14605 sj0array[iij0]) <
14606 IKFAST_SOLUTION_THRESH)
14607 {
14608 j0valid[iij0] = false;
14609 _ij0[1] = iij0;
14610 break;
14611 }
14612 }
14613 j0 = j0array[ij0];
14614 cj0 = cj0array[ij0];
14615 sj0 = sj0array[ij0];
14616 {
14617 IkReal evalcond[6];
14618 IkReal x707 = IKsin(j0);
14619 IkReal x708 = IKcos(j0);
14620 IkReal x709 = ((-1.0) * x707);
14621 evalcond[0] = (new_r11 * x707);
14622 evalcond[1] = (new_r00 * x709);
14623 evalcond[2] = (gconst11 * x709);
14624 evalcond[3] =
14625 (gconst11 +
14626 ((new_r11 * x708)));
14627 evalcond[4] =
14628 (new_r11 +
14629 ((gconst11 * x708)));
14630 evalcond[5] =
14631 (((new_r00 * x708)) +
14632 (((-1.0) * gconst11)));
14633 if (IKabs(evalcond[0]) >
14634 IKFAST_EVALCOND_THRESH ||
14635 IKabs(evalcond[1]) >
14636 IKFAST_EVALCOND_THRESH ||
14637 IKabs(evalcond[2]) >
14638 IKFAST_EVALCOND_THRESH ||
14639 IKabs(evalcond[3]) >
14640 IKFAST_EVALCOND_THRESH ||
14641 IKabs(evalcond[4]) >
14642 IKFAST_EVALCOND_THRESH ||
14643 IKabs(evalcond[5]) >
14644 IKFAST_EVALCOND_THRESH)
14645 {
14646 continue;
14647 }
14648 }
14649
14650 {
14651 std::vector<
14652 IkSingleDOFSolutionBase<
14653 IkReal> >
14654 vinfos(7);
14655 vinfos[0].jointtype = 1;
14656 vinfos[0].foffset = j0;
14657 vinfos[0].indices[0] = _ij0[0];
14658 vinfos[0].indices[1] = _ij0[1];
14659 vinfos[0].maxsolutions = _nj0;
14660 vinfos[1].jointtype = 1;
14661 vinfos[1].foffset = j1;
14662 vinfos[1].indices[0] = _ij1[0];
14663 vinfos[1].indices[1] = _ij1[1];
14664 vinfos[1].maxsolutions = _nj1;
14665 vinfos[2].jointtype = 1;
14666 vinfos[2].foffset = j2;
14667 vinfos[2].indices[0] = _ij2[0];
14668 vinfos[2].indices[1] = _ij2[1];
14669 vinfos[2].maxsolutions = _nj2;
14670 vinfos[3].jointtype = 1;
14671 vinfos[3].foffset = j3;
14672 vinfos[3].indices[0] = _ij3[0];
14673 vinfos[3].indices[1] = _ij3[1];
14674 vinfos[3].maxsolutions = _nj3;
14675 vinfos[4].jointtype = 1;
14676 vinfos[4].foffset = j4;
14677 vinfos[4].indices[0] = _ij4[0];
14678 vinfos[4].indices[1] = _ij4[1];
14679 vinfos[4].maxsolutions = _nj4;
14680 vinfos[5].jointtype = 1;
14681 vinfos[5].foffset = j5;
14682 vinfos[5].indices[0] = _ij5[0];
14683 vinfos[5].indices[1] = _ij5[1];
14684 vinfos[5].maxsolutions = _nj5;
14685 vinfos[6].jointtype = 1;
14686 vinfos[6].foffset = j6;
14687 vinfos[6].indices[0] = _ij6[0];
14688 vinfos[6].indices[1] = _ij6[1];
14689 vinfos[6].maxsolutions = _nj6;
14690 std::vector<int> vfree(0);
14691 solutions.AddSolution(vinfos,
14692 vfree);
14693 }
14694 }
14695 }
14696 }
14697 } while (0);
14698 if (bgotonextstatement)
14699 {
14700 bool bgotonextstatement = true;
14701 do
14702 {
14703 evalcond[0] = ((IKabs(new_r00)) +
14704 (IKabs(new_r01)));
14705 if (IKabs(evalcond[0]) <
14706 0.0000050000000000)
14707 {
14708 bgotonextstatement = false;
14709 {
14710 IkReal j0eval[1];
14711 CheckValue<IkReal> x711 =
14712 IKatan2WithCheck(
14713 IkReal(0),
14714 IkReal(new_r11),
14715 IKFAST_ATAN2_MAGTHRESH);
14716 if (!x711.valid)
14717 {
14718 continue;
14719 }
14720 IkReal x710 =
14721 ((1.0) * (x711.value));
14722 sj1 = 0;
14723 cj1 = -1.0;
14724 j1 = 3.14159265358979;
14725 sj2 = gconst10;
14726 cj2 = gconst11;
14727 j2 = ((3.14159265) +
14728 (((-1.0) * x710)));
14729 new_r00 = 0;
14730 new_r01 = 0;
14731 new_r12 = 0;
14732 new_r22 = 0;
14733 IkReal gconst9 =
14734 ((3.14159265358979) +
14735 (((-1.0) * x710)));
14736 IkReal gconst10 = 0;
14737 IkReal x712 =
14738 ((1.0) +
14739 (((-1.0) *
14740 (new_r10 * new_r10))));
14741 if (IKabs(x712) == 0)
14742 {
14743 continue;
14744 }
14745 IkReal gconst11 =
14746 ((-1.0) * new_r11 *
14747 (pow(x712, -0.5)));
14748 j0eval[0] = ((IKabs(new_r11)) +
14749 (IKabs(new_r10)));
14750 if (IKabs(j0eval[0]) <
14751 0.0000010000000000)
14752 {
14753 {
14754 IkReal j0eval[1];
14755 CheckValue<IkReal> x714 =
14756 IKatan2WithCheck(
14757 IkReal(0),
14758 IkReal(new_r11),
14759 IKFAST_ATAN2_MAGTHRESH);
14760 if (!x714.valid)
14761 {
14762 continue;
14763 }
14764 IkReal x713 =
14765 ((1.0) * (x714.value));
14766 sj1 = 0;
14767 cj1 = -1.0;
14768 j1 = 3.14159265358979;
14769 sj2 = gconst10;
14770 cj2 = gconst11;
14771 j2 = ((3.14159265) +
14772 (((-1.0) * x713)));
14773 new_r00 = 0;
14774 new_r01 = 0;
14775 new_r12 = 0;
14776 new_r22 = 0;
14777 IkReal gconst9 =
14778 ((3.14159265358979) +
14779 (((-1.0) * x713)));
14780 IkReal gconst10 = 0;
14781 IkReal x715 =
14782 ((1.0) +
14783 (((-1.0) *
14784 (new_r10 * new_r10))));
14785 if (IKabs(x715) == 0)
14786 {
14787 continue;
14788 }
14789 IkReal gconst11 =
14790 ((-1.0) * new_r11 *
14791 (pow(x715, -0.5)));
14792 j0eval[0] = new_r11;
14793 if (IKabs(j0eval[0]) <
14794 0.0000010000000000)
14795 {
14796 {
14797 IkReal j0eval[2];
14798 CheckValue<IkReal> x717 =
14799 IKatan2WithCheck(
14800 IkReal(0),
14801 IkReal(new_r11),
14802 IKFAST_ATAN2_MAGTHRESH);
14803 if (!x717.valid)
14804 {
14805 continue;
14806 }
14807 IkReal x716 =
14808 ((1.0) * (x717.value));
14809 sj1 = 0;
14810 cj1 = -1.0;
14811 j1 = 3.14159265358979;
14812 sj2 = gconst10;
14813 cj2 = gconst11;
14814 j2 = ((3.14159265) +
14815 (((-1.0) * x716)));
14816 new_r00 = 0;
14817 new_r01 = 0;
14818 new_r12 = 0;
14819 new_r22 = 0;
14820 IkReal gconst9 =
14821 ((3.14159265358979) +
14822 (((-1.0) * x716)));
14823 IkReal gconst10 = 0;
14824 IkReal x718 =
14825 ((1.0) + (((-1.0) *
14826 (new_r10 *
14827 new_r10))));
14828 if (IKabs(x718) == 0)
14829 {
14830 continue;
14831 }
14832 IkReal gconst11 =
14833 ((-1.0) * new_r11 *
14834 (pow(x718, -0.5)));
14835 j0eval[0] = new_r10;
14836 j0eval[1] = new_r11;
14837 if (IKabs(j0eval[0]) <
14838 0.0000010000000000 ||
14839 IKabs(j0eval[1]) <
14840 0.0000010000000000)
14841 {
14842 continue; // 3 cases
14843 // reached
14844 }
14845 else
14846 {
14847 {
14848 IkReal j0array[1],
14849 cj0array[1],
14850 sj0array[1];
14851 bool j0valid[1] = {
14852 false
14853 };
14854 _nj0 = 1;
14855 CheckValue<IkReal> x719 =
14856 IKPowWithIntegerCheck(
14857 new_r10, -1);
14858 if (!x719.valid)
14859 {
14860 continue;
14861 }
14862 CheckValue<IkReal> x720 =
14863 IKPowWithIntegerCheck(
14864 new_r11, -1);
14865 if (!x720.valid)
14866 {
14867 continue;
14868 }
14869 if (IKabs((
14870 gconst11 *
14871 (x719.value))) <
14872 IKFAST_ATAN2_MAGTHRESH &&
14873 IKabs((
14874 (-1.0) *
14875 gconst11 *
14876 (x720.value))) <
14877 IKFAST_ATAN2_MAGTHRESH &&
14878 IKabs(
14879 IKsqr((
14880 gconst11 *
14881 (x719.value))) +
14882 IKsqr((
14883 (-1.0) *
14884 gconst11 *
14885 (x720.value))) -
14886 1) <=
14887 IKFAST_SINCOS_THRESH)
14888 continue;
14889 j0array[0] = IKatan2(
14890 (gconst11 *
14891 (x719.value)),
14892 ((-1.0) * gconst11 *
14893 (x720.value)));
14894 sj0array[0] =
14895 IKsin(j0array[0]);
14896 cj0array[0] =
14897 IKcos(j0array[0]);
14898 if (j0array[0] > IKPI)
14899 {
14900 j0array[0] -= IK2PI;
14901 }
14902 else if (j0array[0] <
14903 -IKPI)
14904 {
14905 j0array[0] += IK2PI;
14906 }
14907 j0valid[0] = true;
14908 for (int ij0 = 0;
14909 ij0 < 1;
14910 ++ij0)
14911 {
14912 if (!j0valid[ij0])
14913 {
14914 continue;
14915 }
14916 _ij0[0] = ij0;
14917 _ij0[1] = -1;
14918 for (int iij0 =
14919 ij0 + 1;
14920 iij0 < 1;
14921 ++iij0)
14922 {
14923 if (j0valid[iij0] &&
14924 IKabs(
14925 cj0array
14926 [ij0] -
14927 cj0array
14928 [iij0]) <
14929 IKFAST_SOLUTION_THRESH &&
14930 IKabs(
14931 sj0array
14932 [ij0] -
14933 sj0array
14934 [iij0]) <
14935 IKFAST_SOLUTION_THRESH)
14936 {
14937 j0valid[iij0] =
14938 false;
14939 _ij0[1] = iij0;
14940 break;
14941 }
14942 }
14943 j0 = j0array[ij0];
14944 cj0 = cj0array[ij0];
14945 sj0 = sj0array[ij0];
14946 {
14947 IkReal evalcond[8];
14948 IkReal x721 =
14949 IKcos(j0);
14950 IkReal x722 =
14951 IKsin(j0);
14952 IkReal x723 =
14953 ((1.0) *
14954 gconst11);
14955 IkReal x724 =
14956 ((-1.0) *
14957 gconst11);
14958 evalcond[0] =
14959 (new_r10 *
14960 x721);
14961 evalcond[1] =
14962 (new_r11 *
14963 x722);
14964 evalcond[2] =
14965 (x721 * x724);
14966 evalcond[3] =
14967 (x722 * x724);
14968 evalcond[4] =
14969 (gconst11 +
14970 ((new_r11 *
14971 x721)));
14972 evalcond[5] =
14973 (((gconst11 *
14974 x721)) +
14975 new_r11);
14976 evalcond[6] =
14977 ((((-1.0) *
14978 x722 *
14979 x723)) +
14980 new_r10);
14981 evalcond[7] =
14982 ((((-1.0) *
14983 x723)) +
14984 ((new_r10 *
14985 x722)));
14986 if (IKabs(evalcond
14987 [0]) >
14988 IKFAST_EVALCOND_THRESH ||
14989 IKabs(evalcond
14990 [1]) >
14991 IKFAST_EVALCOND_THRESH ||
14992 IKabs(evalcond
14993 [2]) >
14994 IKFAST_EVALCOND_THRESH ||
14995 IKabs(evalcond
14996 [3]) >
14997 IKFAST_EVALCOND_THRESH ||
14998 IKabs(evalcond
14999 [4]) >
15000 IKFAST_EVALCOND_THRESH ||
15001 IKabs(evalcond
15002 [5]) >
15003 IKFAST_EVALCOND_THRESH ||
15004 IKabs(evalcond
15005 [6]) >
15006 IKFAST_EVALCOND_THRESH ||
15007 IKabs(evalcond
15008 [7]) >
15009 IKFAST_EVALCOND_THRESH)
15010 {
15011 continue;
15012 }
15013 }
15014
15015 {
15016 std::vector<
15017 IkSingleDOFSolutionBase<
15018 IkReal> >
15019 vinfos(7);
15020 vinfos[0]
15021 .jointtype = 1;
15022 vinfos[0].foffset =
15023 j0;
15024 vinfos[0]
15025 .indices[0] =
15026 _ij0[0];
15027 vinfos[0]
15028 .indices[1] =
15029 _ij0[1];
15030 vinfos[0]
15031 .maxsolutions =
15032 _nj0;
15033 vinfos[1]
15034 .jointtype = 1;
15035 vinfos[1].foffset =
15036 j1;
15037 vinfos[1]
15038 .indices[0] =
15039 _ij1[0];
15040 vinfos[1]
15041 .indices[1] =
15042 _ij1[1];
15043 vinfos[1]
15044 .maxsolutions =
15045 _nj1;
15046 vinfos[2]
15047 .jointtype = 1;
15048 vinfos[2].foffset =
15049 j2;
15050 vinfos[2]
15051 .indices[0] =
15052 _ij2[0];
15053 vinfos[2]
15054 .indices[1] =
15055 _ij2[1];
15056 vinfos[2]
15057 .maxsolutions =
15058 _nj2;
15059 vinfos[3]
15060 .jointtype = 1;
15061 vinfos[3].foffset =
15062 j3;
15063 vinfos[3]
15064 .indices[0] =
15065 _ij3[0];
15066 vinfos[3]
15067 .indices[1] =
15068 _ij3[1];
15069 vinfos[3]
15070 .maxsolutions =
15071 _nj3;
15072 vinfos[4]
15073 .jointtype = 1;
15074 vinfos[4].foffset =
15075 j4;
15076 vinfos[4]
15077 .indices[0] =
15078 _ij4[0];
15079 vinfos[4]
15080 .indices[1] =
15081 _ij4[1];
15082 vinfos[4]
15083 .maxsolutions =
15084 _nj4;
15085 vinfos[5]
15086 .jointtype = 1;
15087 vinfos[5].foffset =
15088 j5;
15089 vinfos[5]
15090 .indices[0] =
15091 _ij5[0];
15092 vinfos[5]
15093 .indices[1] =
15094 _ij5[1];
15095 vinfos[5]
15096 .maxsolutions =
15097 _nj5;
15098 vinfos[6]
15099 .jointtype = 1;
15100 vinfos[6].foffset =
15101 j6;
15102 vinfos[6]
15103 .indices[0] =
15104 _ij6[0];
15105 vinfos[6]
15106 .indices[1] =
15107 _ij6[1];
15108 vinfos[6]
15109 .maxsolutions =
15110 _nj6;
15111 std::vector<int>
15112 vfree(0);
15113 solutions
15114 .AddSolution(
15115 vinfos,
15116 vfree);
15117 }
15118 }
15119 }
15120 }
15121 }
15122 }
15123 else
15124 {
15125 {
15126 IkReal j0array[1],
15127 cj0array[1],
15128 sj0array[1];
15129 bool j0valid[1] = { false };
15130 _nj0 = 1;
15131 CheckValue<IkReal> x725 =
15132 IKPowWithIntegerCheck(
15133 gconst11, -1);
15134 if (!x725.valid)
15135 {
15136 continue;
15137 }
15138 CheckValue<IkReal> x726 =
15139 IKPowWithIntegerCheck(
15140 new_r11, -1);
15141 if (!x726.valid)
15142 {
15143 continue;
15144 }
15145 if (IKabs((new_r10 *
15146 (x725.value))) <
15147 IKFAST_ATAN2_MAGTHRESH &&
15148 IKabs(
15149 ((-1.0) * gconst11 *
15150 (x726.value))) <
15151 IKFAST_ATAN2_MAGTHRESH &&
15152 IKabs(
15153 IKsqr((
15154 new_r10 *
15155 (x725.value))) +
15156 IKsqr((
15157 (-1.0) *
15158 gconst11 *
15159 (x726.value))) -
15160 1) <=
15161 IKFAST_SINCOS_THRESH)
15162 continue;
15163 j0array[0] = IKatan2(
15164 (new_r10 *
15165 (x725.value)),
15166 ((-1.0) * gconst11 *
15167 (x726.value)));
15168 sj0array[0] =
15169 IKsin(j0array[0]);
15170 cj0array[0] =
15171 IKcos(j0array[0]);
15172 if (j0array[0] > IKPI)
15173 {
15174 j0array[0] -= IK2PI;
15175 }
15176 else if (j0array[0] < -IKPI)
15177 {
15178 j0array[0] += IK2PI;
15179 }
15180 j0valid[0] = true;
15181 for (int ij0 = 0; ij0 < 1;
15182 ++ij0)
15183 {
15184 if (!j0valid[ij0])
15185 {
15186 continue;
15187 }
15188 _ij0[0] = ij0;
15189 _ij0[1] = -1;
15190 for (int iij0 = ij0 + 1;
15191 iij0 < 1;
15192 ++iij0)
15193 {
15194 if (j0valid[iij0] &&
15195 IKabs(
15196 cj0array[ij0] -
15197 cj0array
15198 [iij0]) <
15199 IKFAST_SOLUTION_THRESH &&
15200 IKabs(
15201 sj0array[ij0] -
15202 sj0array
15203 [iij0]) <
15204 IKFAST_SOLUTION_THRESH)
15205 {
15206 j0valid[iij0] = false;
15207 _ij0[1] = iij0;
15208 break;
15209 }
15210 }
15211 j0 = j0array[ij0];
15212 cj0 = cj0array[ij0];
15213 sj0 = sj0array[ij0];
15214 {
15215 IkReal evalcond[8];
15216 IkReal x727 = IKcos(j0);
15217 IkReal x728 = IKsin(j0);
15218 IkReal x729 =
15219 ((1.0) * gconst11);
15220 IkReal x730 =
15221 ((-1.0) * gconst11);
15222 evalcond[0] =
15223 (new_r10 * x727);
15224 evalcond[1] =
15225 (new_r11 * x728);
15226 evalcond[2] =
15227 (x727 * x730);
15228 evalcond[3] =
15229 (x728 * x730);
15230 evalcond[4] =
15231 (gconst11 +
15232 ((new_r11 *
15233 x727)));
15234 evalcond[5] =
15235 (((gconst11 *
15236 x727)) +
15237 new_r11);
15238 evalcond[6] =
15239 (new_r10 +
15240 (((-1.0) * x728 *
15241 x729)));
15242 evalcond[7] =
15243 ((((-1.0) * x729)) +
15244 ((new_r10 *
15245 x728)));
15246 if (IKabs(evalcond[0]) >
15247 IKFAST_EVALCOND_THRESH ||
15248 IKabs(evalcond[1]) >
15249 IKFAST_EVALCOND_THRESH ||
15250 IKabs(evalcond[2]) >
15251 IKFAST_EVALCOND_THRESH ||
15252 IKabs(evalcond[3]) >
15253 IKFAST_EVALCOND_THRESH ||
15254 IKabs(evalcond[4]) >
15255 IKFAST_EVALCOND_THRESH ||
15256 IKabs(evalcond[5]) >
15257 IKFAST_EVALCOND_THRESH ||
15258 IKabs(evalcond[6]) >
15259 IKFAST_EVALCOND_THRESH ||
15260 IKabs(evalcond[7]) >
15261 IKFAST_EVALCOND_THRESH)
15262 {
15263 continue;
15264 }
15265 }
15266
15267 {
15268 std::vector<
15269 IkSingleDOFSolutionBase<
15270 IkReal> >
15271 vinfos(7);
15272 vinfos[0].jointtype = 1;
15273 vinfos[0].foffset = j0;
15274 vinfos[0].indices[0] =
15275 _ij0[0];
15276 vinfos[0].indices[1] =
15277 _ij0[1];
15278 vinfos[0].maxsolutions =
15279 _nj0;
15280 vinfos[1].jointtype = 1;
15281 vinfos[1].foffset = j1;
15282 vinfos[1].indices[0] =
15283 _ij1[0];
15284 vinfos[1].indices[1] =
15285 _ij1[1];
15286 vinfos[1].maxsolutions =
15287 _nj1;
15288 vinfos[2].jointtype = 1;
15289 vinfos[2].foffset = j2;
15290 vinfos[2].indices[0] =
15291 _ij2[0];
15292 vinfos[2].indices[1] =
15293 _ij2[1];
15294 vinfos[2].maxsolutions =
15295 _nj2;
15296 vinfos[3].jointtype = 1;
15297 vinfos[3].foffset = j3;
15298 vinfos[3].indices[0] =
15299 _ij3[0];
15300 vinfos[3].indices[1] =
15301 _ij3[1];
15302 vinfos[3].maxsolutions =
15303 _nj3;
15304 vinfos[4].jointtype = 1;
15305 vinfos[4].foffset = j4;
15306 vinfos[4].indices[0] =
15307 _ij4[0];
15308 vinfos[4].indices[1] =
15309 _ij4[1];
15310 vinfos[4].maxsolutions =
15311 _nj4;
15312 vinfos[5].jointtype = 1;
15313 vinfos[5].foffset = j5;
15314 vinfos[5].indices[0] =
15315 _ij5[0];
15316 vinfos[5].indices[1] =
15317 _ij5[1];
15318 vinfos[5].maxsolutions =
15319 _nj5;
15320 vinfos[6].jointtype = 1;
15321 vinfos[6].foffset = j6;
15322 vinfos[6].indices[0] =
15323 _ij6[0];
15324 vinfos[6].indices[1] =
15325 _ij6[1];
15326 vinfos[6].maxsolutions =
15327 _nj6;
15328 std::vector<int> vfree(
15329 0);
15330 solutions.AddSolution(
15331 vinfos, vfree);
15332 }
15333 }
15334 }
15335 }
15336 }
15337 }
15338 else
15339 {
15340 {
15341 IkReal j0array[1], cj0array[1],
15342 sj0array[1];
15343 bool j0valid[1] = { false };
15344 _nj0 = 1;
15345 CheckValue<IkReal> x731 =
15346 IKPowWithIntegerCheck(
15347 IKsign(gconst11), -1);
15348 if (!x731.valid)
15349 {
15350 continue;
15351 }
15352 CheckValue<IkReal> x732 =
15353 IKatan2WithCheck(
15354 IkReal(new_r10),
15355 IkReal(
15356 ((-1.0) * new_r11)),
15357 IKFAST_ATAN2_MAGTHRESH);
15358 if (!x732.valid)
15359 {
15360 continue;
15361 }
15362 j0array[0] =
15363 ((-1.5707963267949) +
15364 (((1.5707963267949) *
15365 (x731.value))) +
15366 (x732.value));
15367 sj0array[0] = IKsin(j0array[0]);
15368 cj0array[0] = IKcos(j0array[0]);
15369 if (j0array[0] > IKPI)
15370 {
15371 j0array[0] -= IK2PI;
15372 }
15373 else if (j0array[0] < -IKPI)
15374 {
15375 j0array[0] += IK2PI;
15376 }
15377 j0valid[0] = true;
15378 for (int ij0 = 0; ij0 < 1;
15379 ++ij0)
15380 {
15381 if (!j0valid[ij0])
15382 {
15383 continue;
15384 }
15385 _ij0[0] = ij0;
15386 _ij0[1] = -1;
15387 for (int iij0 = ij0 + 1;
15388 iij0 < 1;
15389 ++iij0)
15390 {
15391 if (j0valid[iij0] &&
15392 IKabs(cj0array[ij0] -
15393 cj0array[iij0]) <
15394 IKFAST_SOLUTION_THRESH &&
15395 IKabs(sj0array[ij0] -
15396 sj0array[iij0]) <
15397 IKFAST_SOLUTION_THRESH)
15398 {
15399 j0valid[iij0] = false;
15400 _ij0[1] = iij0;
15401 break;
15402 }
15403 }
15404 j0 = j0array[ij0];
15405 cj0 = cj0array[ij0];
15406 sj0 = sj0array[ij0];
15407 {
15408 IkReal evalcond[8];
15409 IkReal x733 = IKcos(j0);
15410 IkReal x734 = IKsin(j0);
15411 IkReal x735 =
15412 ((1.0) * gconst11);
15413 IkReal x736 =
15414 ((-1.0) * gconst11);
15415 evalcond[0] =
15416 (new_r10 * x733);
15417 evalcond[1] =
15418 (new_r11 * x734);
15419 evalcond[2] = (x733 * x736);
15420 evalcond[3] = (x734 * x736);
15421 evalcond[4] =
15422 (gconst11 +
15423 ((new_r11 * x733)));
15424 evalcond[5] =
15425 (new_r11 +
15426 ((gconst11 * x733)));
15427 evalcond[6] =
15428 ((((-1.0) * x734 *
15429 x735)) +
15430 new_r10);
15431 evalcond[7] =
15432 ((((-1.0) * x735)) +
15433 ((new_r10 * x734)));
15434 if (IKabs(evalcond[0]) >
15435 IKFAST_EVALCOND_THRESH ||
15436 IKabs(evalcond[1]) >
15437 IKFAST_EVALCOND_THRESH ||
15438 IKabs(evalcond[2]) >
15439 IKFAST_EVALCOND_THRESH ||
15440 IKabs(evalcond[3]) >
15441 IKFAST_EVALCOND_THRESH ||
15442 IKabs(evalcond[4]) >
15443 IKFAST_EVALCOND_THRESH ||
15444 IKabs(evalcond[5]) >
15445 IKFAST_EVALCOND_THRESH ||
15446 IKabs(evalcond[6]) >
15447 IKFAST_EVALCOND_THRESH ||
15448 IKabs(evalcond[7]) >
15449 IKFAST_EVALCOND_THRESH)
15450 {
15451 continue;
15452 }
15453 }
15454
15455 {
15456 std::vector<
15457 IkSingleDOFSolutionBase<
15458 IkReal> >
15459 vinfos(7);
15460 vinfos[0].jointtype = 1;
15461 vinfos[0].foffset = j0;
15462 vinfos[0].indices[0] =
15463 _ij0[0];
15464 vinfos[0].indices[1] =
15465 _ij0[1];
15466 vinfos[0].maxsolutions =
15467 _nj0;
15468 vinfos[1].jointtype = 1;
15469 vinfos[1].foffset = j1;
15470 vinfos[1].indices[0] =
15471 _ij1[0];
15472 vinfos[1].indices[1] =
15473 _ij1[1];
15474 vinfos[1].maxsolutions =
15475 _nj1;
15476 vinfos[2].jointtype = 1;
15477 vinfos[2].foffset = j2;
15478 vinfos[2].indices[0] =
15479 _ij2[0];
15480 vinfos[2].indices[1] =
15481 _ij2[1];
15482 vinfos[2].maxsolutions =
15483 _nj2;
15484 vinfos[3].jointtype = 1;
15485 vinfos[3].foffset = j3;
15486 vinfos[3].indices[0] =
15487 _ij3[0];
15488 vinfos[3].indices[1] =
15489 _ij3[1];
15490 vinfos[3].maxsolutions =
15491 _nj3;
15492 vinfos[4].jointtype = 1;
15493 vinfos[4].foffset = j4;
15494 vinfos[4].indices[0] =
15495 _ij4[0];
15496 vinfos[4].indices[1] =
15497 _ij4[1];
15498 vinfos[4].maxsolutions =
15499 _nj4;
15500 vinfos[5].jointtype = 1;
15501 vinfos[5].foffset = j5;
15502 vinfos[5].indices[0] =
15503 _ij5[0];
15504 vinfos[5].indices[1] =
15505 _ij5[1];
15506 vinfos[5].maxsolutions =
15507 _nj5;
15508 vinfos[6].jointtype = 1;
15509 vinfos[6].foffset = j6;
15510 vinfos[6].indices[0] =
15511 _ij6[0];
15512 vinfos[6].indices[1] =
15513 _ij6[1];
15514 vinfos[6].maxsolutions =
15515 _nj6;
15516 std::vector<int> vfree(0);
15517 solutions.AddSolution(
15518 vinfos, vfree);
15519 }
15520 }
15521 }
15522 }
15523 }
15524 }
15525 } while (0);
15526 if (bgotonextstatement)
15527 {
15528 bool bgotonextstatement = true;
15529 do
15530 {
15531 evalcond[0] = IKabs(new_r01);
15532 if (IKabs(evalcond[0]) <
15533 0.0000050000000000)
15534 {
15535 bgotonextstatement = false;
15536 {
15537 IkReal j0eval[1];
15538 CheckValue<IkReal> x738 =
15539 IKatan2WithCheck(
15540 IkReal(0),
15541 IkReal(new_r11),
15542 IKFAST_ATAN2_MAGTHRESH);
15543 if (!x738.valid)
15544 {
15545 continue;
15546 }
15547 IkReal x737 =
15548 ((1.0) * (x738.value));
15549 sj1 = 0;
15550 cj1 = -1.0;
15551 j1 = 3.14159265358979;
15552 sj2 = gconst10;
15553 cj2 = gconst11;
15554 j2 = ((3.14159265) +
15555 (((-1.0) * x737)));
15556 new_r01 = 0;
15557 IkReal gconst9 =
15558 ((3.14159265358979) +
15559 (((-1.0) * x737)));
15560 IkReal gconst10 = 0;
15561 IkReal x739 = new_r11 * new_r11;
15562 if (IKabs(x739) == 0)
15563 {
15564 continue;
15565 }
15566 IkReal gconst11 =
15567 ((-1.0) * new_r11 *
15568 (pow(x739, -0.5)));
15569 j0eval[0] = ((IKabs(new_r10)) +
15570 (IKabs(new_r00)));
15571 if (IKabs(j0eval[0]) <
15572 0.0000010000000000)
15573 {
15574 {
15575 IkReal j0eval[1];
15576 CheckValue<IkReal> x741 =
15577 IKatan2WithCheck(
15578 IkReal(0),
15579 IkReal(new_r11),
15580 IKFAST_ATAN2_MAGTHRESH);
15581 if (!x741.valid)
15582 {
15583 continue;
15584 }
15585 IkReal x740 =
15586 ((1.0) * (x741.value));
15587 sj1 = 0;
15588 cj1 = -1.0;
15589 j1 = 3.14159265358979;
15590 sj2 = gconst10;
15591 cj2 = gconst11;
15592 j2 = ((3.14159265) +
15593 (((-1.0) * x740)));
15594 new_r01 = 0;
15595 IkReal gconst9 =
15596 ((3.14159265358979) +
15597 (((-1.0) * x740)));
15598 IkReal gconst10 = 0;
15599 IkReal x742 =
15600 new_r11 * new_r11;
15601 if (IKabs(x742) == 0)
15602 {
15603 continue;
15604 }
15605 IkReal gconst11 =
15606 ((-1.0) * new_r11 *
15607 (pow(x742, -0.5)));
15608 j0eval[0] =
15609 ((IKabs(new_r11)) +
15610 (IKabs(new_r10)));
15611 if (IKabs(j0eval[0]) <
15612 0.0000010000000000)
15613 {
15614 {
15615 IkReal j0eval[1];
15616 CheckValue<IkReal> x744 =
15617 IKatan2WithCheck(
15618 IkReal(0),
15619 IkReal(new_r11),
15620 IKFAST_ATAN2_MAGTHRESH);
15621 if (!x744.valid)
15622 {
15623 continue;
15624 }
15625 IkReal x743 =
15626 ((1.0) *
15627 (x744.value));
15628 sj1 = 0;
15629 cj1 = -1.0;
15630 j1 = 3.14159265358979;
15631 sj2 = gconst10;
15632 cj2 = gconst11;
15633 j2 = ((3.14159265) +
15634 (((-1.0) * x743)));
15635 new_r01 = 0;
15636 IkReal gconst9 =
15637 ((3.14159265358979) +
15638 (((-1.0) * x743)));
15639 IkReal gconst10 = 0;
15640 IkReal x745 =
15641 new_r11 * new_r11;
15642 if (IKabs(x745) == 0)
15643 {
15644 continue;
15645 }
15646 IkReal gconst11 =
15647 ((-1.0) * new_r11 *
15648 (pow(x745, -0.5)));
15649 j0eval[0] = new_r11;
15650 if (IKabs(j0eval[0]) <
15651 0.0000010000000000)
15652 {
15653 continue; // 3 cases
15654 // reached
15655 }
15656 else
15657 {
15658 {
15659 IkReal j0array[1],
15660 cj0array[1],
15661 sj0array[1];
15662 bool j0valid[1] = {
15663 false
15664 };
15665 _nj0 = 1;
15666 CheckValue<IkReal> x746 =
15667 IKPowWithIntegerCheck(
15668 gconst11, -1);
15669 if (!x746.valid)
15670 {
15671 continue;
15672 }
15673 CheckValue<IkReal> x747 =
15674 IKPowWithIntegerCheck(
15675 new_r11, -1);
15676 if (!x747.valid)
15677 {
15678 continue;
15679 }
15680 if (IKabs((
15681 new_r10 *
15682 (x746.value))) <
15683 IKFAST_ATAN2_MAGTHRESH &&
15684 IKabs((
15685 (-1.0) *
15686 gconst11 *
15687 (x747.value))) <
15688 IKFAST_ATAN2_MAGTHRESH &&
15689 IKabs(
15690 IKsqr((
15691 new_r10 *
15692 (x746.value))) +
15693 IKsqr((
15694 (-1.0) *
15695 gconst11 *
15696 (x747.value))) -
15697 1) <=
15698 IKFAST_SINCOS_THRESH)
15699 continue;
15700 j0array[0] = IKatan2(
15701 (new_r10 *
15702 (x746.value)),
15703 ((-1.0) *
15704 gconst11 *
15705 (x747.value)));
15706 sj0array[0] =
15707 IKsin(j0array[0]);
15708 cj0array[0] =
15709 IKcos(j0array[0]);
15710 if (j0array[0] > IKPI)
15711 {
15712 j0array[0] -= IK2PI;
15713 }
15714 else if (j0array[0] <
15715 -IKPI)
15716 {
15717 j0array[0] += IK2PI;
15718 }
15719 j0valid[0] = true;
15720 for (int ij0 = 0;
15721 ij0 < 1;
15722 ++ij0)
15723 {
15724 if (!j0valid[ij0])
15725 {
15726 continue;
15727 }
15728 _ij0[0] = ij0;
15729 _ij0[1] = -1;
15730 for (int iij0 =
15731 ij0 + 1;
15732 iij0 < 1;
15733 ++iij0)
15734 {
15735 if (j0valid
15736 [iij0] &&
15737 IKabs(
15738 cj0array
15739 [ij0] -
15740 cj0array
15741 [iij0]) <
15742 IKFAST_SOLUTION_THRESH &&
15743 IKabs(
15744 sj0array
15745 [ij0] -
15746 sj0array
15747 [iij0]) <
15748 IKFAST_SOLUTION_THRESH)
15749 {
15750 j0valid[iij0] =
15751 false;
15752 _ij0[1] = iij0;
15753 break;
15754 }
15755 }
15756 j0 = j0array[ij0];
15757 cj0 = cj0array[ij0];
15758 sj0 = sj0array[ij0];
15759 {
15760 IkReal
15761 evalcond[8];
15762 IkReal x748 =
15763 IKsin(j0);
15764 IkReal x749 =
15765 IKcos(j0);
15766 IkReal x750 =
15767 (gconst11 *
15768 x748);
15769 IkReal x751 =
15770 (gconst11 *
15771 x749);
15772 evalcond[0] =
15773 (new_r11 *
15774 x748);
15775 evalcond[1] =
15776 ((-1.0) *
15777 x750);
15778 evalcond[2] =
15779 (gconst11 +
15780 ((new_r11 *
15781 x749)));
15782 evalcond[3] =
15783 (x751 +
15784 new_r11);
15785 evalcond[4] =
15786 ((((-1.0) *
15787 x750)) +
15788 new_r10);
15789 evalcond[5] =
15790 ((((-1.0) *
15791 x751)) +
15792 new_r00);
15793 evalcond[6] =
15794 ((((-1.0) *
15795 new_r00 *
15796 x748)) +
15797 ((new_r10 *
15798 x749)));
15799 evalcond[7] =
15800 (((new_r10 *
15801 x748)) +
15802 ((new_r00 *
15803 x749)) +
15804 (((-1.0) *
15805 gconst11)));
15806 if (IKabs(
15807 evalcond
15808 [0]) >
15809 IKFAST_EVALCOND_THRESH ||
15810 IKabs(
15811 evalcond
15812 [1]) >
15813 IKFAST_EVALCOND_THRESH ||
15814 IKabs(
15815 evalcond
15816 [2]) >
15817 IKFAST_EVALCOND_THRESH ||
15818 IKabs(
15819 evalcond
15820 [3]) >
15821 IKFAST_EVALCOND_THRESH ||
15822 IKabs(
15823 evalcond
15824 [4]) >
15825 IKFAST_EVALCOND_THRESH ||
15826 IKabs(
15827 evalcond
15828 [5]) >
15829 IKFAST_EVALCOND_THRESH ||
15830 IKabs(
15831 evalcond
15832 [6]) >
15833 IKFAST_EVALCOND_THRESH ||
15834 IKabs(
15835 evalcond
15836 [7]) >
15837 IKFAST_EVALCOND_THRESH)
15838 {
15839 continue;
15840 }
15841 }
15842
15843 {
15844 std::vector<
15845 IkSingleDOFSolutionBase<
15846 IkReal> >
15847 vinfos(7);
15848 vinfos[0]
15849 .jointtype =
15850 1;
15851 vinfos[0]
15852 .foffset = j0;
15853 vinfos[0]
15854 .indices[0] =
15855 _ij0[0];
15856 vinfos[0]
15857 .indices[1] =
15858 _ij0[1];
15859 vinfos[0]
15860 .maxsolutions =
15861 _nj0;
15862 vinfos[1]
15863 .jointtype =
15864 1;
15865 vinfos[1]
15866 .foffset = j1;
15867 vinfos[1]
15868 .indices[0] =
15869 _ij1[0];
15870 vinfos[1]
15871 .indices[1] =
15872 _ij1[1];
15873 vinfos[1]
15874 .maxsolutions =
15875 _nj1;
15876 vinfos[2]
15877 .jointtype =
15878 1;
15879 vinfos[2]
15880 .foffset = j2;
15881 vinfos[2]
15882 .indices[0] =
15883 _ij2[0];
15884 vinfos[2]
15885 .indices[1] =
15886 _ij2[1];
15887 vinfos[2]
15888 .maxsolutions =
15889 _nj2;
15890 vinfos[3]
15891 .jointtype =
15892 1;
15893 vinfos[3]
15894 .foffset = j3;
15895 vinfos[3]
15896 .indices[0] =
15897 _ij3[0];
15898 vinfos[3]
15899 .indices[1] =
15900 _ij3[1];
15901 vinfos[3]
15902 .maxsolutions =
15903 _nj3;
15904 vinfos[4]
15905 .jointtype =
15906 1;
15907 vinfos[4]
15908 .foffset = j4;
15909 vinfos[4]
15910 .indices[0] =
15911 _ij4[0];
15912 vinfos[4]
15913 .indices[1] =
15914 _ij4[1];
15915 vinfos[4]
15916 .maxsolutions =
15917 _nj4;
15918 vinfos[5]
15919 .jointtype =
15920 1;
15921 vinfos[5]
15922 .foffset = j5;
15923 vinfos[5]
15924 .indices[0] =
15925 _ij5[0];
15926 vinfos[5]
15927 .indices[1] =
15928 _ij5[1];
15929 vinfos[5]
15930 .maxsolutions =
15931 _nj5;
15932 vinfos[6]
15933 .jointtype =
15934 1;
15935 vinfos[6]
15936 .foffset = j6;
15937 vinfos[6]
15938 .indices[0] =
15939 _ij6[0];
15940 vinfos[6]
15941 .indices[1] =
15942 _ij6[1];
15943 vinfos[6]
15944 .maxsolutions =
15945 _nj6;
15946 std::vector<int>
15947 vfree(0);
15948 solutions
15949 .AddSolution(
15950 vinfos,
15951 vfree);
15952 }
15953 }
15954 }
15955 }
15956 }
15957 }
15958 else
15959 {
15960 {
15961 IkReal j0array[1],
15962 cj0array[1],
15963 sj0array[1];
15964 bool j0valid[1] = {
15965 false
15966 };
15967 _nj0 = 1;
15968 CheckValue<IkReal> x752 =
15969 IKPowWithIntegerCheck(
15970 IKsign(gconst11),
15971 -1);
15972 if (!x752.valid)
15973 {
15974 continue;
15975 }
15976 CheckValue<IkReal> x753 =
15977 IKatan2WithCheck(
15978 IkReal(new_r10),
15979 IkReal(((-1.0) *
15980 new_r11)),
15981 IKFAST_ATAN2_MAGTHRESH);
15982 if (!x753.valid)
15983 {
15984 continue;
15985 }
15986 j0array[0] =
15987 ((-1.5707963267949) +
15988 (((1.5707963267949) *
15989 (x752.value))) +
15990 (x753.value));
15991 sj0array[0] =
15992 IKsin(j0array[0]);
15993 cj0array[0] =
15994 IKcos(j0array[0]);
15995 if (j0array[0] > IKPI)
15996 {
15997 j0array[0] -= IK2PI;
15998 }
15999 else if (j0array[0] <
16000 -IKPI)
16001 {
16002 j0array[0] += IK2PI;
16003 }
16004 j0valid[0] = true;
16005 for (int ij0 = 0; ij0 < 1;
16006 ++ij0)
16007 {
16008 if (!j0valid[ij0])
16009 {
16010 continue;
16011 }
16012 _ij0[0] = ij0;
16013 _ij0[1] = -1;
16014 for (int iij0 = ij0 + 1;
16015 iij0 < 1;
16016 ++iij0)
16017 {
16018 if (j0valid[iij0] &&
16019 IKabs(
16020 cj0array
16021 [ij0] -
16022 cj0array
16023 [iij0]) <
16024 IKFAST_SOLUTION_THRESH &&
16025 IKabs(
16026 sj0array
16027 [ij0] -
16028 sj0array
16029 [iij0]) <
16030 IKFAST_SOLUTION_THRESH)
16031 {
16032 j0valid[iij0] =
16033 false;
16034 _ij0[1] = iij0;
16035 break;
16036 }
16037 }
16038 j0 = j0array[ij0];
16039 cj0 = cj0array[ij0];
16040 sj0 = sj0array[ij0];
16041 {
16042 IkReal evalcond[8];
16043 IkReal x754 =
16044 IKsin(j0);
16045 IkReal x755 =
16046 IKcos(j0);
16047 IkReal x756 =
16048 (gconst11 * x754);
16049 IkReal x757 =
16050 (gconst11 * x755);
16051 evalcond[0] =
16052 (new_r11 * x754);
16053 evalcond[1] =
16054 ((-1.0) * x756);
16055 evalcond[2] =
16056 (gconst11 +
16057 ((new_r11 *
16058 x755)));
16059 evalcond[3] =
16060 (x757 + new_r11);
16061 evalcond[4] =
16062 ((((-1.0) *
16063 x756)) +
16064 new_r10);
16065 evalcond[5] =
16066 ((((-1.0) *
16067 x757)) +
16068 new_r00);
16069 evalcond[6] =
16070 ((((-1.0) *
16071 new_r00 *
16072 x754)) +
16073 ((new_r10 *
16074 x755)));
16075 evalcond[7] =
16076 (((new_r00 *
16077 x755)) +
16078 ((new_r10 *
16079 x754)) +
16080 (((-1.0) *
16081 gconst11)));
16082 if (IKabs(
16083 evalcond[0]) >
16084 IKFAST_EVALCOND_THRESH ||
16085 IKabs(
16086 evalcond[1]) >
16087 IKFAST_EVALCOND_THRESH ||
16088 IKabs(
16089 evalcond[2]) >
16090 IKFAST_EVALCOND_THRESH ||
16091 IKabs(
16092 evalcond[3]) >
16093 IKFAST_EVALCOND_THRESH ||
16094 IKabs(
16095 evalcond[4]) >
16096 IKFAST_EVALCOND_THRESH ||
16097 IKabs(
16098 evalcond[5]) >
16099 IKFAST_EVALCOND_THRESH ||
16100 IKabs(
16101 evalcond[6]) >
16102 IKFAST_EVALCOND_THRESH ||
16103 IKabs(
16104 evalcond[7]) >
16105 IKFAST_EVALCOND_THRESH)
16106 {
16107 continue;
16108 }
16109 }
16110
16111 {
16112 std::vector<
16113 IkSingleDOFSolutionBase<
16114 IkReal> >
16115 vinfos(7);
16116 vinfos[0].jointtype =
16117 1;
16118 vinfos[0].foffset =
16119 j0;
16120 vinfos[0].indices[0] =
16121 _ij0[0];
16122 vinfos[0].indices[1] =
16123 _ij0[1];
16124 vinfos[0]
16125 .maxsolutions =
16126 _nj0;
16127 vinfos[1].jointtype =
16128 1;
16129 vinfos[1].foffset =
16130 j1;
16131 vinfos[1].indices[0] =
16132 _ij1[0];
16133 vinfos[1].indices[1] =
16134 _ij1[1];
16135 vinfos[1]
16136 .maxsolutions =
16137 _nj1;
16138 vinfos[2].jointtype =
16139 1;
16140 vinfos[2].foffset =
16141 j2;
16142 vinfos[2].indices[0] =
16143 _ij2[0];
16144 vinfos[2].indices[1] =
16145 _ij2[1];
16146 vinfos[2]
16147 .maxsolutions =
16148 _nj2;
16149 vinfos[3].jointtype =
16150 1;
16151 vinfos[3].foffset =
16152 j3;
16153 vinfos[3].indices[0] =
16154 _ij3[0];
16155 vinfos[3].indices[1] =
16156 _ij3[1];
16157 vinfos[3]
16158 .maxsolutions =
16159 _nj3;
16160 vinfos[4].jointtype =
16161 1;
16162 vinfos[4].foffset =
16163 j4;
16164 vinfos[4].indices[0] =
16165 _ij4[0];
16166 vinfos[4].indices[1] =
16167 _ij4[1];
16168 vinfos[4]
16169 .maxsolutions =
16170 _nj4;
16171 vinfos[5].jointtype =
16172 1;
16173 vinfos[5].foffset =
16174 j5;
16175 vinfos[5].indices[0] =
16176 _ij5[0];
16177 vinfos[5].indices[1] =
16178 _ij5[1];
16179 vinfos[5]
16180 .maxsolutions =
16181 _nj5;
16182 vinfos[6].jointtype =
16183 1;
16184 vinfos[6].foffset =
16185 j6;
16186 vinfos[6].indices[0] =
16187 _ij6[0];
16188 vinfos[6].indices[1] =
16189 _ij6[1];
16190 vinfos[6]
16191 .maxsolutions =
16192 _nj6;
16193 std::vector<int>
16194 vfree(0);
16195 solutions.AddSolution(
16196 vinfos, vfree);
16197 }
16198 }
16199 }
16200 }
16201 }
16202 }
16203 else
16204 {
16205 {
16206 IkReal j0array[1],
16207 cj0array[1], sj0array[1];
16208 bool j0valid[1] = { false };
16209 _nj0 = 1;
16210 CheckValue<IkReal> x758 =
16211 IKPowWithIntegerCheck(
16212 IKsign(gconst11), -1);
16213 if (!x758.valid)
16214 {
16215 continue;
16216 }
16217 CheckValue<IkReal> x759 =
16218 IKatan2WithCheck(
16219 IkReal(new_r10),
16220 IkReal(new_r00),
16221 IKFAST_ATAN2_MAGTHRESH);
16222 if (!x759.valid)
16223 {
16224 continue;
16225 }
16226 j0array[0] =
16227 ((-1.5707963267949) +
16228 (((1.5707963267949) *
16229 (x758.value))) +
16230 (x759.value));
16231 sj0array[0] =
16232 IKsin(j0array[0]);
16233 cj0array[0] =
16234 IKcos(j0array[0]);
16235 if (j0array[0] > IKPI)
16236 {
16237 j0array[0] -= IK2PI;
16238 }
16239 else if (j0array[0] < -IKPI)
16240 {
16241 j0array[0] += IK2PI;
16242 }
16243 j0valid[0] = true;
16244 for (int ij0 = 0; ij0 < 1;
16245 ++ij0)
16246 {
16247 if (!j0valid[ij0])
16248 {
16249 continue;
16250 }
16251 _ij0[0] = ij0;
16252 _ij0[1] = -1;
16253 for (int iij0 = ij0 + 1;
16254 iij0 < 1;
16255 ++iij0)
16256 {
16257 if (j0valid[iij0] &&
16258 IKabs(
16259 cj0array[ij0] -
16260 cj0array[iij0]) <
16261 IKFAST_SOLUTION_THRESH &&
16262 IKabs(
16263 sj0array[ij0] -
16264 sj0array[iij0]) <
16265 IKFAST_SOLUTION_THRESH)
16266 {
16267 j0valid[iij0] = false;
16268 _ij0[1] = iij0;
16269 break;
16270 }
16271 }
16272 j0 = j0array[ij0];
16273 cj0 = cj0array[ij0];
16274 sj0 = sj0array[ij0];
16275 {
16276 IkReal evalcond[8];
16277 IkReal x760 = IKsin(j0);
16278 IkReal x761 = IKcos(j0);
16279 IkReal x762 =
16280 (gconst11 * x760);
16281 IkReal x763 =
16282 (gconst11 * x761);
16283 evalcond[0] =
16284 (new_r11 * x760);
16285 evalcond[1] =
16286 ((-1.0) * x762);
16287 evalcond[2] =
16288 (((new_r11 * x761)) +
16289 gconst11);
16290 evalcond[3] =
16291 (x763 + new_r11);
16292 evalcond[4] =
16293 ((((-1.0) * x762)) +
16294 new_r10);
16295 evalcond[5] =
16296 ((((-1.0) * x763)) +
16297 new_r00);
16298 evalcond[6] =
16299 (((new_r10 * x761)) +
16300 (((-1.0) * new_r00 *
16301 x760)));
16302 evalcond[7] =
16303 (((new_r10 * x760)) +
16304 ((new_r00 * x761)) +
16305 (((-1.0) *
16306 gconst11)));
16307 if (IKabs(evalcond[0]) >
16308 IKFAST_EVALCOND_THRESH ||
16309 IKabs(evalcond[1]) >
16310 IKFAST_EVALCOND_THRESH ||
16311 IKabs(evalcond[2]) >
16312 IKFAST_EVALCOND_THRESH ||
16313 IKabs(evalcond[3]) >
16314 IKFAST_EVALCOND_THRESH ||
16315 IKabs(evalcond[4]) >
16316 IKFAST_EVALCOND_THRESH ||
16317 IKabs(evalcond[5]) >
16318 IKFAST_EVALCOND_THRESH ||
16319 IKabs(evalcond[6]) >
16320 IKFAST_EVALCOND_THRESH ||
16321 IKabs(evalcond[7]) >
16322 IKFAST_EVALCOND_THRESH)
16323 {
16324 continue;
16325 }
16326 }
16327
16328 {
16329 std::vector<
16330 IkSingleDOFSolutionBase<
16331 IkReal> >
16332 vinfos(7);
16333 vinfos[0].jointtype = 1;
16334 vinfos[0].foffset = j0;
16335 vinfos[0].indices[0] =
16336 _ij0[0];
16337 vinfos[0].indices[1] =
16338 _ij0[1];
16339 vinfos[0].maxsolutions =
16340 _nj0;
16341 vinfos[1].jointtype = 1;
16342 vinfos[1].foffset = j1;
16343 vinfos[1].indices[0] =
16344 _ij1[0];
16345 vinfos[1].indices[1] =
16346 _ij1[1];
16347 vinfos[1].maxsolutions =
16348 _nj1;
16349 vinfos[2].jointtype = 1;
16350 vinfos[2].foffset = j2;
16351 vinfos[2].indices[0] =
16352 _ij2[0];
16353 vinfos[2].indices[1] =
16354 _ij2[1];
16355 vinfos[2].maxsolutions =
16356 _nj2;
16357 vinfos[3].jointtype = 1;
16358 vinfos[3].foffset = j3;
16359 vinfos[3].indices[0] =
16360 _ij3[0];
16361 vinfos[3].indices[1] =
16362 _ij3[1];
16363 vinfos[3].maxsolutions =
16364 _nj3;
16365 vinfos[4].jointtype = 1;
16366 vinfos[4].foffset = j4;
16367 vinfos[4].indices[0] =
16368 _ij4[0];
16369 vinfos[4].indices[1] =
16370 _ij4[1];
16371 vinfos[4].maxsolutions =
16372 _nj4;
16373 vinfos[5].jointtype = 1;
16374 vinfos[5].foffset = j5;
16375 vinfos[5].indices[0] =
16376 _ij5[0];
16377 vinfos[5].indices[1] =
16378 _ij5[1];
16379 vinfos[5].maxsolutions =
16380 _nj5;
16381 vinfos[6].jointtype = 1;
16382 vinfos[6].foffset = j6;
16383 vinfos[6].indices[0] =
16384 _ij6[0];
16385 vinfos[6].indices[1] =
16386 _ij6[1];
16387 vinfos[6].maxsolutions =
16388 _nj6;
16389 std::vector<int> vfree(0);
16390 solutions.AddSolution(
16391 vinfos, vfree);
16392 }
16393 }
16394 }
16395 }
16396 }
16397 }
16398 } while (0);
16399 if (bgotonextstatement)
16400 {
16401 bool bgotonextstatement = true;
16402 do
16403 {
16404 if (1)
16405 {
16406 bgotonextstatement = false;
16407 continue; // branch miss [j0]
16408 }
16409 } while (0);
16410 if (bgotonextstatement)
16411 {
16412 }
16413 }
16414 }
16415 }
16416 }
16417 }
16418 }
16419 }
16420 else
16421 {
16422 {
16423 IkReal j0array[1], cj0array[1], sj0array[1];
16424 bool j0valid[1] = { false };
16425 _nj0 = 1;
16426 IkReal x764 = ((1.0) * new_r11);
16427 CheckValue<IkReal> x765 = IKPowWithIntegerCheck(
16428 IKsign(((((-1.0) * gconst11 * x764)) +
16429 ((gconst10 * new_r01)))),
16430 -1);
16431 if (!x765.valid)
16432 {
16433 continue;
16434 }
16435 CheckValue<IkReal> x766 = IKatan2WithCheck(
16436 IkReal((((gconst10 * gconst11)) +
16437 (((-1.0) * new_r01 * x764)))),
16438 IkReal(
16439 ((new_r11 * new_r11) +
16440 (((-1.0) * (gconst10 * gconst10))))),
16441 IKFAST_ATAN2_MAGTHRESH);
16442 if (!x766.valid)
16443 {
16444 continue;
16445 }
16446 j0array[0] =
16447 ((-1.5707963267949) +
16448 (((1.5707963267949) * (x765.value))) +
16449 (x766.value));
16450 sj0array[0] = IKsin(j0array[0]);
16451 cj0array[0] = IKcos(j0array[0]);
16452 if (j0array[0] > IKPI)
16453 {
16454 j0array[0] -= IK2PI;
16455 }
16456 else if (j0array[0] < -IKPI)
16457 {
16458 j0array[0] += IK2PI;
16459 }
16460 j0valid[0] = true;
16461 for (int ij0 = 0; ij0 < 1; ++ij0)
16462 {
16463 if (!j0valid[ij0])
16464 {
16465 continue;
16466 }
16467 _ij0[0] = ij0;
16468 _ij0[1] = -1;
16469 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
16470 {
16471 if (j0valid[iij0] &&
16472 IKabs(cj0array[ij0] - cj0array[iij0]) <
16473 IKFAST_SOLUTION_THRESH &&
16474 IKabs(sj0array[ij0] - sj0array[iij0]) <
16475 IKFAST_SOLUTION_THRESH)
16476 {
16477 j0valid[iij0] = false;
16478 _ij0[1] = iij0;
16479 break;
16480 }
16481 }
16482 j0 = j0array[ij0];
16483 cj0 = cj0array[ij0];
16484 sj0 = sj0array[ij0];
16485 {
16486 IkReal evalcond[8];
16487 IkReal x767 = IKcos(j0);
16488 IkReal x768 = IKsin(j0);
16489 IkReal x769 = (gconst10 * x767);
16490 IkReal x770 = (gconst11 * x767);
16491 IkReal x771 = ((1.0) * x768);
16492 IkReal x772 = (gconst11 * x771);
16493 evalcond[0] =
16494 (((new_r11 * x768)) + gconst10 +
16495 ((new_r01 * x767)));
16496 evalcond[1] =
16497 (((gconst10 * x768)) + x770 + new_r11);
16498 evalcond[2] = (((new_r10 * x767)) +
16499 (((-1.0) * new_r00 * x771)) +
16500 gconst10);
16501 evalcond[3] = (((new_r11 * x767)) +
16502 (((-1.0) * new_r01 * x771)) +
16503 gconst11);
16504 evalcond[4] =
16505 (x769 + (((-1.0) * x772)) + new_r10);
16506 evalcond[5] =
16507 (x769 + (((-1.0) * x772)) + new_r01);
16508 evalcond[6] = (((new_r10 * x768)) +
16509 ((new_r00 * x767)) +
16510 (((-1.0) * gconst11)));
16511 evalcond[7] =
16512 ((((-1.0) * gconst10 * x771)) +
16513 new_r00 + (((-1.0) * x770)));
16514 if (IKabs(evalcond[0]) >
16515 IKFAST_EVALCOND_THRESH ||
16516 IKabs(evalcond[1]) >
16517 IKFAST_EVALCOND_THRESH ||
16518 IKabs(evalcond[2]) >
16519 IKFAST_EVALCOND_THRESH ||
16520 IKabs(evalcond[3]) >
16521 IKFAST_EVALCOND_THRESH ||
16522 IKabs(evalcond[4]) >
16523 IKFAST_EVALCOND_THRESH ||
16524 IKabs(evalcond[5]) >
16525 IKFAST_EVALCOND_THRESH ||
16526 IKabs(evalcond[6]) >
16527 IKFAST_EVALCOND_THRESH ||
16528 IKabs(evalcond[7]) >
16529 IKFAST_EVALCOND_THRESH)
16530 {
16531 continue;
16532 }
16533 }
16534
16535 {
16536 std::vector<
16537 IkSingleDOFSolutionBase<IkReal> >
16538 vinfos(7);
16539 vinfos[0].jointtype = 1;
16540 vinfos[0].foffset = j0;
16541 vinfos[0].indices[0] = _ij0[0];
16542 vinfos[0].indices[1] = _ij0[1];
16543 vinfos[0].maxsolutions = _nj0;
16544 vinfos[1].jointtype = 1;
16545 vinfos[1].foffset = j1;
16546 vinfos[1].indices[0] = _ij1[0];
16547 vinfos[1].indices[1] = _ij1[1];
16548 vinfos[1].maxsolutions = _nj1;
16549 vinfos[2].jointtype = 1;
16550 vinfos[2].foffset = j2;
16551 vinfos[2].indices[0] = _ij2[0];
16552 vinfos[2].indices[1] = _ij2[1];
16553 vinfos[2].maxsolutions = _nj2;
16554 vinfos[3].jointtype = 1;
16555 vinfos[3].foffset = j3;
16556 vinfos[3].indices[0] = _ij3[0];
16557 vinfos[3].indices[1] = _ij3[1];
16558 vinfos[3].maxsolutions = _nj3;
16559 vinfos[4].jointtype = 1;
16560 vinfos[4].foffset = j4;
16561 vinfos[4].indices[0] = _ij4[0];
16562 vinfos[4].indices[1] = _ij4[1];
16563 vinfos[4].maxsolutions = _nj4;
16564 vinfos[5].jointtype = 1;
16565 vinfos[5].foffset = j5;
16566 vinfos[5].indices[0] = _ij5[0];
16567 vinfos[5].indices[1] = _ij5[1];
16568 vinfos[5].maxsolutions = _nj5;
16569 vinfos[6].jointtype = 1;
16570 vinfos[6].foffset = j6;
16571 vinfos[6].indices[0] = _ij6[0];
16572 vinfos[6].indices[1] = _ij6[1];
16573 vinfos[6].maxsolutions = _nj6;
16574 std::vector<int> vfree(0);
16575 solutions.AddSolution(vinfos, vfree);
16576 }
16577 }
16578 }
16579 }
16580 }
16581 }
16582 else
16583 {
16584 {
16585 IkReal j0array[1], cj0array[1], sj0array[1];
16586 bool j0valid[1] = { false };
16587 _nj0 = 1;
16588 IkReal x773 = ((1.0) * new_r11);
16589 CheckValue<IkReal> x774 = IKPowWithIntegerCheck(
16590 IKsign(((new_r01 * new_r01) +
16591 (new_r11 * new_r11))),
16592 -1);
16593 if (!x774.valid)
16594 {
16595 continue;
16596 }
16597 CheckValue<IkReal> x775 = IKatan2WithCheck(
16598 IkReal((((gconst11 * new_r01)) +
16599 (((-1.0) * gconst10 * x773)))),
16600 IkReal(((((-1.0) * gconst10 * new_r01)) +
16601 (((-1.0) * gconst11 * x773)))),
16602 IKFAST_ATAN2_MAGTHRESH);
16603 if (!x775.valid)
16604 {
16605 continue;
16606 }
16607 j0array[0] = ((-1.5707963267949) +
16608 (((1.5707963267949) * (x774.value))) +
16609 (x775.value));
16610 sj0array[0] = IKsin(j0array[0]);
16611 cj0array[0] = IKcos(j0array[0]);
16612 if (j0array[0] > IKPI)
16613 {
16614 j0array[0] -= IK2PI;
16615 }
16616 else if (j0array[0] < -IKPI)
16617 {
16618 j0array[0] += IK2PI;
16619 }
16620 j0valid[0] = true;
16621 for (int ij0 = 0; ij0 < 1; ++ij0)
16622 {
16623 if (!j0valid[ij0])
16624 {
16625 continue;
16626 }
16627 _ij0[0] = ij0;
16628 _ij0[1] = -1;
16629 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
16630 {
16631 if (j0valid[iij0] &&
16632 IKabs(cj0array[ij0] - cj0array[iij0]) <
16633 IKFAST_SOLUTION_THRESH &&
16634 IKabs(sj0array[ij0] - sj0array[iij0]) <
16635 IKFAST_SOLUTION_THRESH)
16636 {
16637 j0valid[iij0] = false;
16638 _ij0[1] = iij0;
16639 break;
16640 }
16641 }
16642 j0 = j0array[ij0];
16643 cj0 = cj0array[ij0];
16644 sj0 = sj0array[ij0];
16645 {
16646 IkReal evalcond[8];
16647 IkReal x776 = IKcos(j0);
16648 IkReal x777 = IKsin(j0);
16649 IkReal x778 = (gconst10 * x776);
16650 IkReal x779 = (gconst11 * x776);
16651 IkReal x780 = ((1.0) * x777);
16652 IkReal x781 = (gconst11 * x780);
16653 evalcond[0] = (gconst10 + ((new_r11 * x777)) +
16654 ((new_r01 * x776)));
16655 evalcond[1] =
16656 (x779 + new_r11 + ((gconst10 * x777)));
16657 evalcond[2] = (gconst10 + ((new_r10 * x776)) +
16658 (((-1.0) * new_r00 * x780)));
16659 evalcond[3] = ((((-1.0) * new_r01 * x780)) +
16660 gconst11 + ((new_r11 * x776)));
16661 evalcond[4] =
16662 ((((-1.0) * x781)) + x778 + new_r10);
16663 evalcond[5] =
16664 ((((-1.0) * x781)) + x778 + new_r01);
16665 evalcond[6] =
16666 (((new_r00 * x776)) + ((new_r10 * x777)) +
16667 (((-1.0) * gconst11)));
16668 evalcond[7] = (new_r00 + (((-1.0) * x779)) +
16669 (((-1.0) * gconst10 * x780)));
16670 if (IKabs(evalcond[0]) >
16671 IKFAST_EVALCOND_THRESH ||
16672 IKabs(evalcond[1]) >
16673 IKFAST_EVALCOND_THRESH ||
16674 IKabs(evalcond[2]) >
16675 IKFAST_EVALCOND_THRESH ||
16676 IKabs(evalcond[3]) >
16677 IKFAST_EVALCOND_THRESH ||
16678 IKabs(evalcond[4]) >
16679 IKFAST_EVALCOND_THRESH ||
16680 IKabs(evalcond[5]) >
16681 IKFAST_EVALCOND_THRESH ||
16682 IKabs(evalcond[6]) >
16683 IKFAST_EVALCOND_THRESH ||
16684 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
16685 {
16686 continue;
16687 }
16688 }
16689
16690 {
16691 std::vector<IkSingleDOFSolutionBase<IkReal> >
16692 vinfos(7);
16693 vinfos[0].jointtype = 1;
16694 vinfos[0].foffset = j0;
16695 vinfos[0].indices[0] = _ij0[0];
16696 vinfos[0].indices[1] = _ij0[1];
16697 vinfos[0].maxsolutions = _nj0;
16698 vinfos[1].jointtype = 1;
16699 vinfos[1].foffset = j1;
16700 vinfos[1].indices[0] = _ij1[0];
16701 vinfos[1].indices[1] = _ij1[1];
16702 vinfos[1].maxsolutions = _nj1;
16703 vinfos[2].jointtype = 1;
16704 vinfos[2].foffset = j2;
16705 vinfos[2].indices[0] = _ij2[0];
16706 vinfos[2].indices[1] = _ij2[1];
16707 vinfos[2].maxsolutions = _nj2;
16708 vinfos[3].jointtype = 1;
16709 vinfos[3].foffset = j3;
16710 vinfos[3].indices[0] = _ij3[0];
16711 vinfos[3].indices[1] = _ij3[1];
16712 vinfos[3].maxsolutions = _nj3;
16713 vinfos[4].jointtype = 1;
16714 vinfos[4].foffset = j4;
16715 vinfos[4].indices[0] = _ij4[0];
16716 vinfos[4].indices[1] = _ij4[1];
16717 vinfos[4].maxsolutions = _nj4;
16718 vinfos[5].jointtype = 1;
16719 vinfos[5].foffset = j5;
16720 vinfos[5].indices[0] = _ij5[0];
16721 vinfos[5].indices[1] = _ij5[1];
16722 vinfos[5].maxsolutions = _nj5;
16723 vinfos[6].jointtype = 1;
16724 vinfos[6].foffset = j6;
16725 vinfos[6].indices[0] = _ij6[0];
16726 vinfos[6].indices[1] = _ij6[1];
16727 vinfos[6].maxsolutions = _nj6;
16728 std::vector<int> vfree(0);
16729 solutions.AddSolution(vinfos, vfree);
16730 }
16731 }
16732 }
16733 }
16734 }
16735 }
16736 else
16737 {
16738 {
16739 IkReal j0array[1], cj0array[1], sj0array[1];
16740 bool j0valid[1] = { false };
16741 _nj0 = 1;
16742 IkReal x782 = ((1.0) * gconst10);
16743 CheckValue<IkReal> x783 = IKatan2WithCheck(
16744 IkReal(((((-1.0) * new_r10 * x782)) +
16745 ((gconst10 * new_r01)))),
16746 IkReal(((((-1.0) * new_r11 * x782)) +
16747 (((-1.0) * new_r00 * x782)))),
16748 IKFAST_ATAN2_MAGTHRESH);
16749 if (!x783.valid)
16750 {
16751 continue;
16752 }
16753 CheckValue<IkReal> x784 = IKPowWithIntegerCheck(
16754 IKsign((((new_r10 * new_r11)) +
16755 ((new_r00 * new_r01)))),
16756 -1);
16757 if (!x784.valid)
16758 {
16759 continue;
16760 }
16761 j0array[0] = ((-1.5707963267949) + (x783.value) +
16762 (((1.5707963267949) * (x784.value))));
16763 sj0array[0] = IKsin(j0array[0]);
16764 cj0array[0] = IKcos(j0array[0]);
16765 if (j0array[0] > IKPI)
16766 {
16767 j0array[0] -= IK2PI;
16768 }
16769 else if (j0array[0] < -IKPI)
16770 {
16771 j0array[0] += IK2PI;
16772 }
16773 j0valid[0] = true;
16774 for (int ij0 = 0; ij0 < 1; ++ij0)
16775 {
16776 if (!j0valid[ij0])
16777 {
16778 continue;
16779 }
16780 _ij0[0] = ij0;
16781 _ij0[1] = -1;
16782 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
16783 {
16784 if (j0valid[iij0] &&
16785 IKabs(cj0array[ij0] - cj0array[iij0]) <
16786 IKFAST_SOLUTION_THRESH &&
16787 IKabs(sj0array[ij0] - sj0array[iij0]) <
16788 IKFAST_SOLUTION_THRESH)
16789 {
16790 j0valid[iij0] = false;
16791 _ij0[1] = iij0;
16792 break;
16793 }
16794 }
16795 j0 = j0array[ij0];
16796 cj0 = cj0array[ij0];
16797 sj0 = sj0array[ij0];
16798 {
16799 IkReal evalcond[8];
16800 IkReal x785 = IKcos(j0);
16801 IkReal x786 = IKsin(j0);
16802 IkReal x787 = (gconst10 * x785);
16803 IkReal x788 = (gconst11 * x785);
16804 IkReal x789 = ((1.0) * x786);
16805 IkReal x790 = (gconst11 * x789);
16806 evalcond[0] = (((new_r11 * x786)) + gconst10 +
16807 ((new_r01 * x785)));
16808 evalcond[1] =
16809 (((gconst10 * x786)) + x788 + new_r11);
16810 evalcond[2] = (((new_r10 * x785)) + gconst10 +
16811 (((-1.0) * new_r00 * x789)));
16812 evalcond[3] = ((((-1.0) * new_r01 * x789)) +
16813 ((new_r11 * x785)) + gconst11);
16814 evalcond[4] = ((((-1.0) * x790)) + x787 + new_r10);
16815 evalcond[5] = ((((-1.0) * x790)) + x787 + new_r01);
16816 evalcond[6] =
16817 (((new_r10 * x786)) + ((new_r00 * x785)) +
16818 (((-1.0) * gconst11)));
16819 evalcond[7] = ((((-1.0) * x788)) + new_r00 +
16820 (((-1.0) * gconst10 * x789)));
16821 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
16822 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
16823 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
16824 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
16825 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
16826 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
16827 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
16828 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
16829 {
16830 continue;
16831 }
16832 }
16833
16834 {
16835 std::vector<IkSingleDOFSolutionBase<IkReal> >
16836 vinfos(7);
16837 vinfos[0].jointtype = 1;
16838 vinfos[0].foffset = j0;
16839 vinfos[0].indices[0] = _ij0[0];
16840 vinfos[0].indices[1] = _ij0[1];
16841 vinfos[0].maxsolutions = _nj0;
16842 vinfos[1].jointtype = 1;
16843 vinfos[1].foffset = j1;
16844 vinfos[1].indices[0] = _ij1[0];
16845 vinfos[1].indices[1] = _ij1[1];
16846 vinfos[1].maxsolutions = _nj1;
16847 vinfos[2].jointtype = 1;
16848 vinfos[2].foffset = j2;
16849 vinfos[2].indices[0] = _ij2[0];
16850 vinfos[2].indices[1] = _ij2[1];
16851 vinfos[2].maxsolutions = _nj2;
16852 vinfos[3].jointtype = 1;
16853 vinfos[3].foffset = j3;
16854 vinfos[3].indices[0] = _ij3[0];
16855 vinfos[3].indices[1] = _ij3[1];
16856 vinfos[3].maxsolutions = _nj3;
16857 vinfos[4].jointtype = 1;
16858 vinfos[4].foffset = j4;
16859 vinfos[4].indices[0] = _ij4[0];
16860 vinfos[4].indices[1] = _ij4[1];
16861 vinfos[4].maxsolutions = _nj4;
16862 vinfos[5].jointtype = 1;
16863 vinfos[5].foffset = j5;
16864 vinfos[5].indices[0] = _ij5[0];
16865 vinfos[5].indices[1] = _ij5[1];
16866 vinfos[5].maxsolutions = _nj5;
16867 vinfos[6].jointtype = 1;
16868 vinfos[6].foffset = j6;
16869 vinfos[6].indices[0] = _ij6[0];
16870 vinfos[6].indices[1] = _ij6[1];
16871 vinfos[6].maxsolutions = _nj6;
16872 std::vector<int> vfree(0);
16873 solutions.AddSolution(vinfos, vfree);
16874 }
16875 }
16876 }
16877 }
16878 }
16879 }
16880 } while (0);
16881 if (bgotonextstatement)
16882 {
16883 bool bgotonextstatement = true;
16884 do
16885 {
16886 evalcond[0] = ((new_r01 * new_r01) + (new_r11 * new_r11));
16887 if (IKabs(evalcond[0]) < 0.0000050000000000)
16888 {
16889 bgotonextstatement = false;
16890 {
16891 IkReal j0eval[1];
16892 sj1 = 0;
16893 cj1 = -1.0;
16894 j1 = 3.14159265358979;
16895 new_r01 = 0;
16896 new_r11 = 0;
16897 j0eval[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
16898 if (IKabs(j0eval[0]) < 0.0000010000000000)
16899 {
16900 continue; // 3 cases reached
16901 }
16902 else
16903 {
16904 {
16905 IkReal j0array[2], cj0array[2], sj0array[2];
16906 bool j0valid[2] = { false };
16907 _nj0 = 2;
16908 CheckValue<IkReal> x792 =
16909 IKatan2WithCheck(IkReal(new_r00),
16910 IkReal(new_r10),
16911 IKFAST_ATAN2_MAGTHRESH);
16912 if (!x792.valid)
16913 {
16914 continue;
16915 }
16916 IkReal x791 = x792.value;
16917 j0array[0] = ((-1.0) * x791);
16918 sj0array[0] = IKsin(j0array[0]);
16919 cj0array[0] = IKcos(j0array[0]);
16920 j0array[1] = ((3.14159265358979) + (((-1.0) * x791)));
16921 sj0array[1] = IKsin(j0array[1]);
16922 cj0array[1] = IKcos(j0array[1]);
16923 if (j0array[0] > IKPI)
16924 {
16925 j0array[0] -= IK2PI;
16926 }
16927 else if (j0array[0] < -IKPI)
16928 {
16929 j0array[0] += IK2PI;
16930 }
16931 j0valid[0] = true;
16932 if (j0array[1] > IKPI)
16933 {
16934 j0array[1] -= IK2PI;
16935 }
16936 else if (j0array[1] < -IKPI)
16937 {
16938 j0array[1] += IK2PI;
16939 }
16940 j0valid[1] = true;
16941 for (int ij0 = 0; ij0 < 2; ++ij0)
16942 {
16943 if (!j0valid[ij0])
16944 {
16945 continue;
16946 }
16947 _ij0[0] = ij0;
16948 _ij0[1] = -1;
16949 for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
16950 {
16951 if (j0valid[iij0] &&
16952 IKabs(cj0array[ij0] - cj0array[iij0]) <
16953 IKFAST_SOLUTION_THRESH &&
16954 IKabs(sj0array[ij0] - sj0array[iij0]) <
16955 IKFAST_SOLUTION_THRESH)
16956 {
16957 j0valid[iij0] = false;
16958 _ij0[1] = iij0;
16959 break;
16960 }
16961 }
16962 j0 = j0array[ij0];
16963 cj0 = cj0array[ij0];
16964 sj0 = sj0array[ij0];
16965 {
16966 IkReal evalcond[1];
16967 evalcond[0] =
16968 ((((-1.0) * new_r00 * (IKsin(j0)))) +
16969 ((new_r10 * (IKcos(j0)))));
16970 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
16971 {
16972 continue;
16973 }
16974 }
16975
16976 {
16977 std::vector<IkSingleDOFSolutionBase<IkReal> >
16978 vinfos(7);
16979 vinfos[0].jointtype = 1;
16980 vinfos[0].foffset = j0;
16981 vinfos[0].indices[0] = _ij0[0];
16982 vinfos[0].indices[1] = _ij0[1];
16983 vinfos[0].maxsolutions = _nj0;
16984 vinfos[1].jointtype = 1;
16985 vinfos[1].foffset = j1;
16986 vinfos[1].indices[0] = _ij1[0];
16987 vinfos[1].indices[1] = _ij1[1];
16988 vinfos[1].maxsolutions = _nj1;
16989 vinfos[2].jointtype = 1;
16990 vinfos[2].foffset = j2;
16991 vinfos[2].indices[0] = _ij2[0];
16992 vinfos[2].indices[1] = _ij2[1];
16993 vinfos[2].maxsolutions = _nj2;
16994 vinfos[3].jointtype = 1;
16995 vinfos[3].foffset = j3;
16996 vinfos[3].indices[0] = _ij3[0];
16997 vinfos[3].indices[1] = _ij3[1];
16998 vinfos[3].maxsolutions = _nj3;
16999 vinfos[4].jointtype = 1;
17000 vinfos[4].foffset = j4;
17001 vinfos[4].indices[0] = _ij4[0];
17002 vinfos[4].indices[1] = _ij4[1];
17003 vinfos[4].maxsolutions = _nj4;
17004 vinfos[5].jointtype = 1;
17005 vinfos[5].foffset = j5;
17006 vinfos[5].indices[0] = _ij5[0];
17007 vinfos[5].indices[1] = _ij5[1];
17008 vinfos[5].maxsolutions = _nj5;
17009 vinfos[6].jointtype = 1;
17010 vinfos[6].foffset = j6;
17011 vinfos[6].indices[0] = _ij6[0];
17012 vinfos[6].indices[1] = _ij6[1];
17013 vinfos[6].maxsolutions = _nj6;
17014 std::vector<int> vfree(0);
17015 solutions.AddSolution(vinfos, vfree);
17016 }
17017 }
17018 }
17019 }
17020 }
17021 }
17022 } while (0);
17023 if (bgotonextstatement)
17024 {
17025 bool bgotonextstatement = true;
17026 do
17027 {
17028 evalcond[0] = ((-3.14159265358979) +
17029 (IKfmod(((3.14159265358979) + (IKabs(j2))),
17030 6.28318530717959)));
17031 if (IKabs(evalcond[0]) < 0.0000050000000000)
17032 {
17033 bgotonextstatement = false;
17034 {
17035 IkReal j0array[1], cj0array[1], sj0array[1];
17036 bool j0valid[1] = { false };
17037 _nj0 = 1;
17038 if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
17039 IKabs(((-1.0) * new_r11)) <
17040 IKFAST_ATAN2_MAGTHRESH &&
17041 IKabs(IKsqr(new_r10) + IKsqr(((-1.0) * new_r11)) -
17042 1) <= IKFAST_SINCOS_THRESH)
17043 continue;
17044 j0array[0] = IKatan2(new_r10, ((-1.0) * new_r11));
17045 sj0array[0] = IKsin(j0array[0]);
17046 cj0array[0] = IKcos(j0array[0]);
17047 if (j0array[0] > IKPI)
17048 {
17049 j0array[0] -= IK2PI;
17050 }
17051 else if (j0array[0] < -IKPI)
17052 {
17053 j0array[0] += IK2PI;
17054 }
17055 j0valid[0] = true;
17056 for (int ij0 = 0; ij0 < 1; ++ij0)
17057 {
17058 if (!j0valid[ij0])
17059 {
17060 continue;
17061 }
17062 _ij0[0] = ij0;
17063 _ij0[1] = -1;
17064 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17065 {
17066 if (j0valid[iij0] &&
17067 IKabs(cj0array[ij0] - cj0array[iij0]) <
17068 IKFAST_SOLUTION_THRESH &&
17069 IKabs(sj0array[ij0] - sj0array[iij0]) <
17070 IKFAST_SOLUTION_THRESH)
17071 {
17072 j0valid[iij0] = false;
17073 _ij0[1] = iij0;
17074 break;
17075 }
17076 }
17077 j0 = j0array[ij0];
17078 cj0 = cj0array[ij0];
17079 sj0 = sj0array[ij0];
17080 {
17081 IkReal evalcond[8];
17082 IkReal x793 = IKcos(j0);
17083 IkReal x794 = IKsin(j0);
17084 IkReal x795 = ((1.0) * x794);
17085 evalcond[0] = (x793 + new_r11);
17086 evalcond[1] = ((((-1.0) * x795)) + new_r10);
17087 evalcond[2] = ((((-1.0) * x793)) + new_r00);
17088 evalcond[3] = ((((-1.0) * x795)) + new_r01);
17089 evalcond[4] =
17090 (((new_r11 * x794)) + ((new_r01 * x793)));
17091 evalcond[5] = (((new_r10 * x793)) +
17092 (((-1.0) * new_r00 * x795)));
17093 evalcond[6] = ((-1.0) + ((new_r10 * x794)) +
17094 ((new_r00 * x793)));
17095 evalcond[7] = ((1.0) + (((-1.0) * new_r01 * x795)) +
17096 ((new_r11 * x793)));
17097 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
17098 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
17099 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
17100 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
17101 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
17102 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
17103 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
17104 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
17105 {
17106 continue;
17107 }
17108 }
17109
17110 {
17111 std::vector<IkSingleDOFSolutionBase<IkReal> >
17112 vinfos(7);
17113 vinfos[0].jointtype = 1;
17114 vinfos[0].foffset = j0;
17115 vinfos[0].indices[0] = _ij0[0];
17116 vinfos[0].indices[1] = _ij0[1];
17117 vinfos[0].maxsolutions = _nj0;
17118 vinfos[1].jointtype = 1;
17119 vinfos[1].foffset = j1;
17120 vinfos[1].indices[0] = _ij1[0];
17121 vinfos[1].indices[1] = _ij1[1];
17122 vinfos[1].maxsolutions = _nj1;
17123 vinfos[2].jointtype = 1;
17124 vinfos[2].foffset = j2;
17125 vinfos[2].indices[0] = _ij2[0];
17126 vinfos[2].indices[1] = _ij2[1];
17127 vinfos[2].maxsolutions = _nj2;
17128 vinfos[3].jointtype = 1;
17129 vinfos[3].foffset = j3;
17130 vinfos[3].indices[0] = _ij3[0];
17131 vinfos[3].indices[1] = _ij3[1];
17132 vinfos[3].maxsolutions = _nj3;
17133 vinfos[4].jointtype = 1;
17134 vinfos[4].foffset = j4;
17135 vinfos[4].indices[0] = _ij4[0];
17136 vinfos[4].indices[1] = _ij4[1];
17137 vinfos[4].maxsolutions = _nj4;
17138 vinfos[5].jointtype = 1;
17139 vinfos[5].foffset = j5;
17140 vinfos[5].indices[0] = _ij5[0];
17141 vinfos[5].indices[1] = _ij5[1];
17142 vinfos[5].maxsolutions = _nj5;
17143 vinfos[6].jointtype = 1;
17144 vinfos[6].foffset = j6;
17145 vinfos[6].indices[0] = _ij6[0];
17146 vinfos[6].indices[1] = _ij6[1];
17147 vinfos[6].maxsolutions = _nj6;
17148 std::vector<int> vfree(0);
17149 solutions.AddSolution(vinfos, vfree);
17150 }
17151 }
17152 }
17153 }
17154 } while (0);
17155 if (bgotonextstatement)
17156 {
17157 bool bgotonextstatement = true;
17158 do
17159 {
17160 evalcond[0] =
17161 ((-3.14159265358979) +
17162 (IKfmod(((3.14159265358979) +
17163 (IKabs(((-3.14159265358979) + j2)))),
17164 6.28318530717959)));
17165 if (IKabs(evalcond[0]) < 0.0000050000000000)
17166 {
17167 bgotonextstatement = false;
17168 {
17169 IkReal j0array[1], cj0array[1], sj0array[1];
17170 bool j0valid[1] = { false };
17171 _nj0 = 1;
17172 if (IKabs(((-1.0) * new_r10)) <
17173 IKFAST_ATAN2_MAGTHRESH &&
17174 IKabs(((-1.0) * new_r00)) <
17175 IKFAST_ATAN2_MAGTHRESH &&
17176 IKabs(IKsqr(((-1.0) * new_r10)) +
17177 IKsqr(((-1.0) * new_r00)) - 1) <=
17178 IKFAST_SINCOS_THRESH)
17179 continue;
17180 j0array[0] =
17181 IKatan2(((-1.0) * new_r10), ((-1.0) * new_r00));
17182 sj0array[0] = IKsin(j0array[0]);
17183 cj0array[0] = IKcos(j0array[0]);
17184 if (j0array[0] > IKPI)
17185 {
17186 j0array[0] -= IK2PI;
17187 }
17188 else if (j0array[0] < -IKPI)
17189 {
17190 j0array[0] += IK2PI;
17191 }
17192 j0valid[0] = true;
17193 for (int ij0 = 0; ij0 < 1; ++ij0)
17194 {
17195 if (!j0valid[ij0])
17196 {
17197 continue;
17198 }
17199 _ij0[0] = ij0;
17200 _ij0[1] = -1;
17201 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17202 {
17203 if (j0valid[iij0] &&
17204 IKabs(cj0array[ij0] - cj0array[iij0]) <
17205 IKFAST_SOLUTION_THRESH &&
17206 IKabs(sj0array[ij0] - sj0array[iij0]) <
17207 IKFAST_SOLUTION_THRESH)
17208 {
17209 j0valid[iij0] = false;
17210 _ij0[1] = iij0;
17211 break;
17212 }
17213 }
17214 j0 = j0array[ij0];
17215 cj0 = cj0array[ij0];
17216 sj0 = sj0array[ij0];
17217 {
17218 IkReal evalcond[8];
17219 IkReal x796 = IKsin(j0);
17220 IkReal x797 = IKcos(j0);
17221 IkReal x798 = ((1.0) * x796);
17222 evalcond[0] = (x796 + new_r10);
17223 evalcond[1] = (x797 + new_r00);
17224 evalcond[2] = (x796 + new_r01);
17225 evalcond[3] = ((((-1.0) * x797)) + new_r11);
17226 evalcond[4] =
17227 (((new_r11 * x796)) + ((new_r01 * x797)));
17228 evalcond[5] = (((new_r10 * x797)) +
17229 (((-1.0) * new_r00 * x798)));
17230 evalcond[6] = ((1.0) + ((new_r10 * x796)) +
17231 ((new_r00 * x797)));
17232 evalcond[7] =
17233 ((-1.0) + (((-1.0) * new_r01 * x798)) +
17234 ((new_r11 * x797)));
17235 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
17236 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
17237 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
17238 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
17239 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
17240 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
17241 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
17242 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
17243 {
17244 continue;
17245 }
17246 }
17247
17248 {
17249 std::vector<IkSingleDOFSolutionBase<IkReal> >
17250 vinfos(7);
17251 vinfos[0].jointtype = 1;
17252 vinfos[0].foffset = j0;
17253 vinfos[0].indices[0] = _ij0[0];
17254 vinfos[0].indices[1] = _ij0[1];
17255 vinfos[0].maxsolutions = _nj0;
17256 vinfos[1].jointtype = 1;
17257 vinfos[1].foffset = j1;
17258 vinfos[1].indices[0] = _ij1[0];
17259 vinfos[1].indices[1] = _ij1[1];
17260 vinfos[1].maxsolutions = _nj1;
17261 vinfos[2].jointtype = 1;
17262 vinfos[2].foffset = j2;
17263 vinfos[2].indices[0] = _ij2[0];
17264 vinfos[2].indices[1] = _ij2[1];
17265 vinfos[2].maxsolutions = _nj2;
17266 vinfos[3].jointtype = 1;
17267 vinfos[3].foffset = j3;
17268 vinfos[3].indices[0] = _ij3[0];
17269 vinfos[3].indices[1] = _ij3[1];
17270 vinfos[3].maxsolutions = _nj3;
17271 vinfos[4].jointtype = 1;
17272 vinfos[4].foffset = j4;
17273 vinfos[4].indices[0] = _ij4[0];
17274 vinfos[4].indices[1] = _ij4[1];
17275 vinfos[4].maxsolutions = _nj4;
17276 vinfos[5].jointtype = 1;
17277 vinfos[5].foffset = j5;
17278 vinfos[5].indices[0] = _ij5[0];
17279 vinfos[5].indices[1] = _ij5[1];
17280 vinfos[5].maxsolutions = _nj5;
17281 vinfos[6].jointtype = 1;
17282 vinfos[6].foffset = j6;
17283 vinfos[6].indices[0] = _ij6[0];
17284 vinfos[6].indices[1] = _ij6[1];
17285 vinfos[6].maxsolutions = _nj6;
17286 std::vector<int> vfree(0);
17287 solutions.AddSolution(vinfos, vfree);
17288 }
17289 }
17290 }
17291 }
17292 } while (0);
17293 if (bgotonextstatement)
17294 {
17295 bool bgotonextstatement = true;
17296 do
17297 {
17298 evalcond[0] = ((IKabs(new_r11)) + (IKabs(new_r00)));
17299 if (IKabs(evalcond[0]) < 0.0000050000000000)
17300 {
17301 bgotonextstatement = false;
17302 {
17303 IkReal j0eval[3];
17304 sj1 = 0;
17305 cj1 = -1.0;
17306 j1 = 3.14159265358979;
17307 new_r11 = 0;
17308 new_r00 = 0;
17309 j0eval[0] = new_r01;
17310 j0eval[1] = IKsign(new_r01);
17311 j0eval[2] = ((IKabs(cj2)) + (IKabs(sj2)));
17312 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
17313 IKabs(j0eval[1]) < 0.0000010000000000 ||
17314 IKabs(j0eval[2]) < 0.0000010000000000)
17315 {
17316 {
17317 IkReal j0eval[3];
17318 sj1 = 0;
17319 cj1 = -1.0;
17320 j1 = 3.14159265358979;
17321 new_r11 = 0;
17322 new_r00 = 0;
17323 j0eval[0] = new_r10;
17324 j0eval[1] = ((IKabs(cj2)) + (IKabs(sj2)));
17325 j0eval[2] = IKsign(new_r10);
17326 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
17327 IKabs(j0eval[1]) < 0.0000010000000000 ||
17328 IKabs(j0eval[2]) < 0.0000010000000000)
17329 {
17330 {
17331 IkReal j0eval[2];
17332 sj1 = 0;
17333 cj1 = -1.0;
17334 j1 = 3.14159265358979;
17335 new_r11 = 0;
17336 new_r00 = 0;
17337 j0eval[0] = new_r01;
17338 j0eval[1] = new_r10;
17339 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
17340 IKabs(j0eval[1]) < 0.0000010000000000)
17341 {
17342 continue; // no branches [j0]
17343 }
17344 else
17345 {
17346 {
17347 IkReal j0array[1], cj0array[1],
17348 sj0array[1];
17349 bool j0valid[1] = { false };
17350 _nj0 = 1;
17351 CheckValue<IkReal> x799 =
17352 IKPowWithIntegerCheck(new_r01, -1);
17353 if (!x799.valid)
17354 {
17355 continue;
17356 }
17357 CheckValue<IkReal> x800 =
17358 IKPowWithIntegerCheck(new_r10, -1);
17359 if (!x800.valid)
17360 {
17361 continue;
17362 }
17363 if (IKabs((cj2 * (x799.value))) <
17364 IKFAST_ATAN2_MAGTHRESH &&
17365 IKabs(
17366 ((-1.0) * sj2 * (x800.value))) <
17367 IKFAST_ATAN2_MAGTHRESH &&
17368 IKabs(IKsqr((cj2 * (x799.value))) +
17369 IKsqr(((-1.0) * sj2 *
17370 (x800.value))) -
17371 1) <= IKFAST_SINCOS_THRESH)
17372 continue;
17373 j0array[0] = IKatan2(
17374 (cj2 * (x799.value)),
17375 ((-1.0) * sj2 * (x800.value)));
17376 sj0array[0] = IKsin(j0array[0]);
17377 cj0array[0] = IKcos(j0array[0]);
17378 if (j0array[0] > IKPI)
17379 {
17380 j0array[0] -= IK2PI;
17381 }
17382 else if (j0array[0] < -IKPI)
17383 {
17384 j0array[0] += IK2PI;
17385 }
17386 j0valid[0] = true;
17387 for (int ij0 = 0; ij0 < 1; ++ij0)
17388 {
17389 if (!j0valid[ij0])
17390 {
17391 continue;
17392 }
17393 _ij0[0] = ij0;
17394 _ij0[1] = -1;
17395 for (int iij0 = ij0 + 1; iij0 < 1;
17396 ++iij0)
17397 {
17398 if (j0valid[iij0] &&
17399 IKabs(cj0array[ij0] -
17400 cj0array[iij0]) <
17401 IKFAST_SOLUTION_THRESH &&
17402 IKabs(sj0array[ij0] -
17403 sj0array[iij0]) <
17404 IKFAST_SOLUTION_THRESH)
17405 {
17406 j0valid[iij0] = false;
17407 _ij0[1] = iij0;
17408 break;
17409 }
17410 }
17411 j0 = j0array[ij0];
17412 cj0 = cj0array[ij0];
17413 sj0 = sj0array[ij0];
17414 {
17415 IkReal evalcond[7];
17416 IkReal x801 = IKcos(j0);
17417 IkReal x802 = IKsin(j0);
17418 IkReal x803 = ((1.0) * cj2);
17419 IkReal x804 = (sj2 * x801);
17420 IkReal x805 = ((1.0) * x802);
17421 IkReal x806 = (x802 * x803);
17422 evalcond[0] =
17423 (sj2 + ((new_r10 * x801)));
17424 evalcond[1] =
17425 (sj2 + ((new_r01 * x801)));
17426 evalcond[2] =
17427 ((((-1.0) * new_r01 * x805)) +
17428 cj2);
17429 evalcond[3] = (((new_r10 * x802)) +
17430 (((-1.0) * x803)));
17431 evalcond[4] = (new_r10 + x804 +
17432 (((-1.0) * x806)));
17433 evalcond[5] =
17434 ((((-1.0) * sj2 * x805)) +
17435 (((-1.0) * x801 * x803)));
17436 evalcond[6] = (new_r01 + x804 +
17437 (((-1.0) * x806)));
17438 if (IKabs(evalcond[0]) >
17439 IKFAST_EVALCOND_THRESH ||
17440 IKabs(evalcond[1]) >
17441 IKFAST_EVALCOND_THRESH ||
17442 IKabs(evalcond[2]) >
17443 IKFAST_EVALCOND_THRESH ||
17444 IKabs(evalcond[3]) >
17445 IKFAST_EVALCOND_THRESH ||
17446 IKabs(evalcond[4]) >
17447 IKFAST_EVALCOND_THRESH ||
17448 IKabs(evalcond[5]) >
17449 IKFAST_EVALCOND_THRESH ||
17450 IKabs(evalcond[6]) >
17451 IKFAST_EVALCOND_THRESH)
17452 {
17453 continue;
17454 }
17455 }
17456
17457 {
17458 std::vector<IkSingleDOFSolutionBase<
17459 IkReal> >
17460 vinfos(7);
17461 vinfos[0].jointtype = 1;
17462 vinfos[0].foffset = j0;
17463 vinfos[0].indices[0] = _ij0[0];
17464 vinfos[0].indices[1] = _ij0[1];
17465 vinfos[0].maxsolutions = _nj0;
17466 vinfos[1].jointtype = 1;
17467 vinfos[1].foffset = j1;
17468 vinfos[1].indices[0] = _ij1[0];
17469 vinfos[1].indices[1] = _ij1[1];
17470 vinfos[1].maxsolutions = _nj1;
17471 vinfos[2].jointtype = 1;
17472 vinfos[2].foffset = j2;
17473 vinfos[2].indices[0] = _ij2[0];
17474 vinfos[2].indices[1] = _ij2[1];
17475 vinfos[2].maxsolutions = _nj2;
17476 vinfos[3].jointtype = 1;
17477 vinfos[3].foffset = j3;
17478 vinfos[3].indices[0] = _ij3[0];
17479 vinfos[3].indices[1] = _ij3[1];
17480 vinfos[3].maxsolutions = _nj3;
17481 vinfos[4].jointtype = 1;
17482 vinfos[4].foffset = j4;
17483 vinfos[4].indices[0] = _ij4[0];
17484 vinfos[4].indices[1] = _ij4[1];
17485 vinfos[4].maxsolutions = _nj4;
17486 vinfos[5].jointtype = 1;
17487 vinfos[5].foffset = j5;
17488 vinfos[5].indices[0] = _ij5[0];
17489 vinfos[5].indices[1] = _ij5[1];
17490 vinfos[5].maxsolutions = _nj5;
17491 vinfos[6].jointtype = 1;
17492 vinfos[6].foffset = j6;
17493 vinfos[6].indices[0] = _ij6[0];
17494 vinfos[6].indices[1] = _ij6[1];
17495 vinfos[6].maxsolutions = _nj6;
17496 std::vector<int> vfree(0);
17497 solutions.AddSolution(vinfos,
17498 vfree);
17499 }
17500 }
17501 }
17502 }
17503 }
17504 }
17505 else
17506 {
17507 {
17508 IkReal j0array[1], cj0array[1], sj0array[1];
17509 bool j0valid[1] = { false };
17510 _nj0 = 1;
17511 CheckValue<IkReal> x807 =
17512 IKPowWithIntegerCheck(IKsign(new_r10),
17513 -1);
17514 if (!x807.valid)
17515 {
17516 continue;
17517 }
17518 CheckValue<IkReal> x808 = IKatan2WithCheck(
17519 IkReal(cj2),
17520 IkReal(((-1.0) * sj2)),
17521 IKFAST_ATAN2_MAGTHRESH);
17522 if (!x808.valid)
17523 {
17524 continue;
17525 }
17526 j0array[0] =
17527 ((-1.5707963267949) +
17528 (((1.5707963267949) * (x807.value))) +
17529 (x808.value));
17530 sj0array[0] = IKsin(j0array[0]);
17531 cj0array[0] = IKcos(j0array[0]);
17532 if (j0array[0] > IKPI)
17533 {
17534 j0array[0] -= IK2PI;
17535 }
17536 else if (j0array[0] < -IKPI)
17537 {
17538 j0array[0] += IK2PI;
17539 }
17540 j0valid[0] = true;
17541 for (int ij0 = 0; ij0 < 1; ++ij0)
17542 {
17543 if (!j0valid[ij0])
17544 {
17545 continue;
17546 }
17547 _ij0[0] = ij0;
17548 _ij0[1] = -1;
17549 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17550 {
17551 if (j0valid[iij0] &&
17552 IKabs(cj0array[ij0] -
17553 cj0array[iij0]) <
17554 IKFAST_SOLUTION_THRESH &&
17555 IKabs(sj0array[ij0] -
17556 sj0array[iij0]) <
17557 IKFAST_SOLUTION_THRESH)
17558 {
17559 j0valid[iij0] = false;
17560 _ij0[1] = iij0;
17561 break;
17562 }
17563 }
17564 j0 = j0array[ij0];
17565 cj0 = cj0array[ij0];
17566 sj0 = sj0array[ij0];
17567 {
17568 IkReal evalcond[7];
17569 IkReal x809 = IKcos(j0);
17570 IkReal x810 = IKsin(j0);
17571 IkReal x811 = ((1.0) * cj2);
17572 IkReal x812 = (sj2 * x809);
17573 IkReal x813 = ((1.0) * x810);
17574 IkReal x814 = (x810 * x811);
17575 evalcond[0] =
17576 (sj2 + ((new_r10 * x809)));
17577 evalcond[1] =
17578 (sj2 + ((new_r01 * x809)));
17579 evalcond[2] =
17580 (cj2 + (((-1.0) * new_r01 * x813)));
17581 evalcond[3] = (((new_r10 * x810)) +
17582 (((-1.0) * x811)));
17583 evalcond[4] = ((((-1.0) * x814)) +
17584 new_r10 + x812);
17585 evalcond[5] =
17586 ((((-1.0) * x809 * x811)) +
17587 (((-1.0) * sj2 * x813)));
17588 evalcond[6] = ((((-1.0) * x814)) +
17589 new_r01 + x812);
17590 if (IKabs(evalcond[0]) >
17591 IKFAST_EVALCOND_THRESH ||
17592 IKabs(evalcond[1]) >
17593 IKFAST_EVALCOND_THRESH ||
17594 IKabs(evalcond[2]) >
17595 IKFAST_EVALCOND_THRESH ||
17596 IKabs(evalcond[3]) >
17597 IKFAST_EVALCOND_THRESH ||
17598 IKabs(evalcond[4]) >
17599 IKFAST_EVALCOND_THRESH ||
17600 IKabs(evalcond[5]) >
17601 IKFAST_EVALCOND_THRESH ||
17602 IKabs(evalcond[6]) >
17603 IKFAST_EVALCOND_THRESH)
17604 {
17605 continue;
17606 }
17607 }
17608
17609 {
17610 std::vector<
17611 IkSingleDOFSolutionBase<IkReal> >
17612 vinfos(7);
17613 vinfos[0].jointtype = 1;
17614 vinfos[0].foffset = j0;
17615 vinfos[0].indices[0] = _ij0[0];
17616 vinfos[0].indices[1] = _ij0[1];
17617 vinfos[0].maxsolutions = _nj0;
17618 vinfos[1].jointtype = 1;
17619 vinfos[1].foffset = j1;
17620 vinfos[1].indices[0] = _ij1[0];
17621 vinfos[1].indices[1] = _ij1[1];
17622 vinfos[1].maxsolutions = _nj1;
17623 vinfos[2].jointtype = 1;
17624 vinfos[2].foffset = j2;
17625 vinfos[2].indices[0] = _ij2[0];
17626 vinfos[2].indices[1] = _ij2[1];
17627 vinfos[2].maxsolutions = _nj2;
17628 vinfos[3].jointtype = 1;
17629 vinfos[3].foffset = j3;
17630 vinfos[3].indices[0] = _ij3[0];
17631 vinfos[3].indices[1] = _ij3[1];
17632 vinfos[3].maxsolutions = _nj3;
17633 vinfos[4].jointtype = 1;
17634 vinfos[4].foffset = j4;
17635 vinfos[4].indices[0] = _ij4[0];
17636 vinfos[4].indices[1] = _ij4[1];
17637 vinfos[4].maxsolutions = _nj4;
17638 vinfos[5].jointtype = 1;
17639 vinfos[5].foffset = j5;
17640 vinfos[5].indices[0] = _ij5[0];
17641 vinfos[5].indices[1] = _ij5[1];
17642 vinfos[5].maxsolutions = _nj5;
17643 vinfos[6].jointtype = 1;
17644 vinfos[6].foffset = j6;
17645 vinfos[6].indices[0] = _ij6[0];
17646 vinfos[6].indices[1] = _ij6[1];
17647 vinfos[6].maxsolutions = _nj6;
17648 std::vector<int> vfree(0);
17649 solutions.AddSolution(vinfos, vfree);
17650 }
17651 }
17652 }
17653 }
17654 }
17655 }
17656 else
17657 {
17658 {
17659 IkReal j0array[1], cj0array[1], sj0array[1];
17660 bool j0valid[1] = { false };
17661 _nj0 = 1;
17662 CheckValue<IkReal> x815 =
17663 IKPowWithIntegerCheck(IKsign(new_r01), -1);
17664 if (!x815.valid)
17665 {
17666 continue;
17667 }
17668 CheckValue<IkReal> x816 =
17669 IKatan2WithCheck(IkReal(cj2),
17670 IkReal(((-1.0) * sj2)),
17671 IKFAST_ATAN2_MAGTHRESH);
17672 if (!x816.valid)
17673 {
17674 continue;
17675 }
17676 j0array[0] =
17677 ((-1.5707963267949) +
17678 (((1.5707963267949) * (x815.value))) +
17679 (x816.value));
17680 sj0array[0] = IKsin(j0array[0]);
17681 cj0array[0] = IKcos(j0array[0]);
17682 if (j0array[0] > IKPI)
17683 {
17684 j0array[0] -= IK2PI;
17685 }
17686 else if (j0array[0] < -IKPI)
17687 {
17688 j0array[0] += IK2PI;
17689 }
17690 j0valid[0] = true;
17691 for (int ij0 = 0; ij0 < 1; ++ij0)
17692 {
17693 if (!j0valid[ij0])
17694 {
17695 continue;
17696 }
17697 _ij0[0] = ij0;
17698 _ij0[1] = -1;
17699 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
17700 {
17701 if (j0valid[iij0] &&
17702 IKabs(cj0array[ij0] - cj0array[iij0]) <
17703 IKFAST_SOLUTION_THRESH &&
17704 IKabs(sj0array[ij0] - sj0array[iij0]) <
17705 IKFAST_SOLUTION_THRESH)
17706 {
17707 j0valid[iij0] = false;
17708 _ij0[1] = iij0;
17709 break;
17710 }
17711 }
17712 j0 = j0array[ij0];
17713 cj0 = cj0array[ij0];
17714 sj0 = sj0array[ij0];
17715 {
17716 IkReal evalcond[7];
17717 IkReal x817 = IKcos(j0);
17718 IkReal x818 = IKsin(j0);
17719 IkReal x819 = ((1.0) * cj2);
17720 IkReal x820 = (sj2 * x817);
17721 IkReal x821 = ((1.0) * x818);
17722 IkReal x822 = (x818 * x819);
17723 evalcond[0] = (sj2 + ((new_r10 * x817)));
17724 evalcond[1] = (sj2 + ((new_r01 * x817)));
17725 evalcond[2] =
17726 (cj2 + (((-1.0) * new_r01 * x821)));
17727 evalcond[3] = (((new_r10 * x818)) +
17728 (((-1.0) * x819)));
17729 evalcond[4] =
17730 ((((-1.0) * x822)) + new_r10 + x820);
17731 evalcond[5] = ((((-1.0) * x817 * x819)) +
17732 (((-1.0) * sj2 * x821)));
17733 evalcond[6] =
17734 ((((-1.0) * x822)) + new_r01 + x820);
17735 if (IKabs(evalcond[0]) >
17736 IKFAST_EVALCOND_THRESH ||
17737 IKabs(evalcond[1]) >
17738 IKFAST_EVALCOND_THRESH ||
17739 IKabs(evalcond[2]) >
17740 IKFAST_EVALCOND_THRESH ||
17741 IKabs(evalcond[3]) >
17742 IKFAST_EVALCOND_THRESH ||
17743 IKabs(evalcond[4]) >
17744 IKFAST_EVALCOND_THRESH ||
17745 IKabs(evalcond[5]) >
17746 IKFAST_EVALCOND_THRESH ||
17747 IKabs(evalcond[6]) >
17748 IKFAST_EVALCOND_THRESH)
17749 {
17750 continue;
17751 }
17752 }
17753
17754 {
17755 std::vector<
17756 IkSingleDOFSolutionBase<IkReal> >
17757 vinfos(7);
17758 vinfos[0].jointtype = 1;
17759 vinfos[0].foffset = j0;
17760 vinfos[0].indices[0] = _ij0[0];
17761 vinfos[0].indices[1] = _ij0[1];
17762 vinfos[0].maxsolutions = _nj0;
17763 vinfos[1].jointtype = 1;
17764 vinfos[1].foffset = j1;
17765 vinfos[1].indices[0] = _ij1[0];
17766 vinfos[1].indices[1] = _ij1[1];
17767 vinfos[1].maxsolutions = _nj1;
17768 vinfos[2].jointtype = 1;
17769 vinfos[2].foffset = j2;
17770 vinfos[2].indices[0] = _ij2[0];
17771 vinfos[2].indices[1] = _ij2[1];
17772 vinfos[2].maxsolutions = _nj2;
17773 vinfos[3].jointtype = 1;
17774 vinfos[3].foffset = j3;
17775 vinfos[3].indices[0] = _ij3[0];
17776 vinfos[3].indices[1] = _ij3[1];
17777 vinfos[3].maxsolutions = _nj3;
17778 vinfos[4].jointtype = 1;
17779 vinfos[4].foffset = j4;
17780 vinfos[4].indices[0] = _ij4[0];
17781 vinfos[4].indices[1] = _ij4[1];
17782 vinfos[4].maxsolutions = _nj4;
17783 vinfos[5].jointtype = 1;
17784 vinfos[5].foffset = j5;
17785 vinfos[5].indices[0] = _ij5[0];
17786 vinfos[5].indices[1] = _ij5[1];
17787 vinfos[5].maxsolutions = _nj5;
17788 vinfos[6].jointtype = 1;
17789 vinfos[6].foffset = j6;
17790 vinfos[6].indices[0] = _ij6[0];
17791 vinfos[6].indices[1] = _ij6[1];
17792 vinfos[6].maxsolutions = _nj6;
17793 std::vector<int> vfree(0);
17794 solutions.AddSolution(vinfos, vfree);
17795 }
17796 }
17797 }
17798 }
17799 }
17800 }
17801 } while (0);
17802 if (bgotonextstatement)
17803 {
17804 bool bgotonextstatement = true;
17805 do
17806 {
17807 evalcond[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
17808 if (IKabs(evalcond[0]) < 0.0000050000000000)
17809 {
17810 bgotonextstatement = false;
17811 {
17812 IkReal j0eval[1];
17813 sj1 = 0;
17814 cj1 = -1.0;
17815 j1 = 3.14159265358979;
17816 new_r11 = 0;
17817 new_r01 = 0;
17818 new_r22 = 0;
17819 new_r20 = 0;
17820 j0eval[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
17821 if (IKabs(j0eval[0]) < 0.0000010000000000)
17822 {
17823 continue; // no branches [j0]
17824 }
17825 else
17826 {
17827 {
17828 IkReal j0array[2], cj0array[2], sj0array[2];
17829 bool j0valid[2] = { false };
17830 _nj0 = 2;
17831 CheckValue<IkReal> x824 =
17832 IKatan2WithCheck(IkReal(new_r00),
17833 IkReal(new_r10),
17834 IKFAST_ATAN2_MAGTHRESH);
17835 if (!x824.valid)
17836 {
17837 continue;
17838 }
17839 IkReal x823 = x824.value;
17840 j0array[0] = ((-1.0) * x823);
17841 sj0array[0] = IKsin(j0array[0]);
17842 cj0array[0] = IKcos(j0array[0]);
17843 j0array[1] =
17844 ((3.14159265358979) + (((-1.0) * x823)));
17845 sj0array[1] = IKsin(j0array[1]);
17846 cj0array[1] = IKcos(j0array[1]);
17847 if (j0array[0] > IKPI)
17848 {
17849 j0array[0] -= IK2PI;
17850 }
17851 else if (j0array[0] < -IKPI)
17852 {
17853 j0array[0] += IK2PI;
17854 }
17855 j0valid[0] = true;
17856 if (j0array[1] > IKPI)
17857 {
17858 j0array[1] -= IK2PI;
17859 }
17860 else if (j0array[1] < -IKPI)
17861 {
17862 j0array[1] += IK2PI;
17863 }
17864 j0valid[1] = true;
17865 for (int ij0 = 0; ij0 < 2; ++ij0)
17866 {
17867 if (!j0valid[ij0])
17868 {
17869 continue;
17870 }
17871 _ij0[0] = ij0;
17872 _ij0[1] = -1;
17873 for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
17874 {
17875 if (j0valid[iij0] &&
17876 IKabs(cj0array[ij0] -
17877 cj0array[iij0]) <
17878 IKFAST_SOLUTION_THRESH &&
17879 IKabs(sj0array[ij0] -
17880 sj0array[iij0]) <
17881 IKFAST_SOLUTION_THRESH)
17882 {
17883 j0valid[iij0] = false;
17884 _ij0[1] = iij0;
17885 break;
17886 }
17887 }
17888 j0 = j0array[ij0];
17889 cj0 = cj0array[ij0];
17890 sj0 = sj0array[ij0];
17891 {
17892 IkReal evalcond[1];
17893 evalcond[0] =
17894 ((((-1.0) * new_r00 * (IKsin(j0)))) +
17895 ((new_r10 * (IKcos(j0)))));
17896 if (IKabs(evalcond[0]) >
17897 IKFAST_EVALCOND_THRESH)
17898 {
17899 continue;
17900 }
17901 }
17902
17903 {
17904 std::vector<
17905 IkSingleDOFSolutionBase<IkReal> >
17906 vinfos(7);
17907 vinfos[0].jointtype = 1;
17908 vinfos[0].foffset = j0;
17909 vinfos[0].indices[0] = _ij0[0];
17910 vinfos[0].indices[1] = _ij0[1];
17911 vinfos[0].maxsolutions = _nj0;
17912 vinfos[1].jointtype = 1;
17913 vinfos[1].foffset = j1;
17914 vinfos[1].indices[0] = _ij1[0];
17915 vinfos[1].indices[1] = _ij1[1];
17916 vinfos[1].maxsolutions = _nj1;
17917 vinfos[2].jointtype = 1;
17918 vinfos[2].foffset = j2;
17919 vinfos[2].indices[0] = _ij2[0];
17920 vinfos[2].indices[1] = _ij2[1];
17921 vinfos[2].maxsolutions = _nj2;
17922 vinfos[3].jointtype = 1;
17923 vinfos[3].foffset = j3;
17924 vinfos[3].indices[0] = _ij3[0];
17925 vinfos[3].indices[1] = _ij3[1];
17926 vinfos[3].maxsolutions = _nj3;
17927 vinfos[4].jointtype = 1;
17928 vinfos[4].foffset = j4;
17929 vinfos[4].indices[0] = _ij4[0];
17930 vinfos[4].indices[1] = _ij4[1];
17931 vinfos[4].maxsolutions = _nj4;
17932 vinfos[5].jointtype = 1;
17933 vinfos[5].foffset = j5;
17934 vinfos[5].indices[0] = _ij5[0];
17935 vinfos[5].indices[1] = _ij5[1];
17936 vinfos[5].maxsolutions = _nj5;
17937 vinfos[6].jointtype = 1;
17938 vinfos[6].foffset = j6;
17939 vinfos[6].indices[0] = _ij6[0];
17940 vinfos[6].indices[1] = _ij6[1];
17941 vinfos[6].maxsolutions = _nj6;
17942 std::vector<int> vfree(0);
17943 solutions.AddSolution(vinfos, vfree);
17944 }
17945 }
17946 }
17947 }
17948 }
17949 }
17950 } while (0);
17951 if (bgotonextstatement)
17952 {
17953 bool bgotonextstatement = true;
17954 do
17955 {
17956 evalcond[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
17957 if (IKabs(evalcond[0]) < 0.0000050000000000)
17958 {
17959 bgotonextstatement = false;
17960 {
17961 IkReal j0eval[1];
17962 sj1 = 0;
17963 cj1 = -1.0;
17964 j1 = 3.14159265358979;
17965 new_r00 = 0;
17966 new_r10 = 0;
17967 new_r21 = 0;
17968 new_r22 = 0;
17969 j0eval[0] =
17970 ((IKabs(new_r11)) + (IKabs(new_r01)));
17971 if (IKabs(j0eval[0]) < 0.0000010000000000)
17972 {
17973 continue; // no branches [j0]
17974 }
17975 else
17976 {
17977 {
17978 IkReal j0array[2], cj0array[2], sj0array[2];
17979 bool j0valid[2] = { false };
17980 _nj0 = 2;
17981 CheckValue<IkReal> x826 = IKatan2WithCheck(
17982 IkReal(new_r01),
17983 IkReal(new_r11),
17984 IKFAST_ATAN2_MAGTHRESH);
17985 if (!x826.valid)
17986 {
17987 continue;
17988 }
17989 IkReal x825 = x826.value;
17990 j0array[0] = ((-1.0) * x825);
17991 sj0array[0] = IKsin(j0array[0]);
17992 cj0array[0] = IKcos(j0array[0]);
17993 j0array[1] = ((3.14159265358979) +
17994 (((-1.0) * x825)));
17995 sj0array[1] = IKsin(j0array[1]);
17996 cj0array[1] = IKcos(j0array[1]);
17997 if (j0array[0] > IKPI)
17998 {
17999 j0array[0] -= IK2PI;
18000 }
18001 else if (j0array[0] < -IKPI)
18002 {
18003 j0array[0] += IK2PI;
18004 }
18005 j0valid[0] = true;
18006 if (j0array[1] > IKPI)
18007 {
18008 j0array[1] -= IK2PI;
18009 }
18010 else if (j0array[1] < -IKPI)
18011 {
18012 j0array[1] += IK2PI;
18013 }
18014 j0valid[1] = true;
18015 for (int ij0 = 0; ij0 < 2; ++ij0)
18016 {
18017 if (!j0valid[ij0])
18018 {
18019 continue;
18020 }
18021 _ij0[0] = ij0;
18022 _ij0[1] = -1;
18023 for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
18024 {
18025 if (j0valid[iij0] &&
18026 IKabs(cj0array[ij0] -
18027 cj0array[iij0]) <
18028 IKFAST_SOLUTION_THRESH &&
18029 IKabs(sj0array[ij0] -
18030 sj0array[iij0]) <
18031 IKFAST_SOLUTION_THRESH)
18032 {
18033 j0valid[iij0] = false;
18034 _ij0[1] = iij0;
18035 break;
18036 }
18037 }
18038 j0 = j0array[ij0];
18039 cj0 = cj0array[ij0];
18040 sj0 = sj0array[ij0];
18041 {
18042 IkReal evalcond[1];
18043 evalcond[0] =
18044 (((new_r11 * (IKcos(j0)))) +
18045 (((-1.0) * new_r01 *
18046 (IKsin(j0)))));
18047 if (IKabs(evalcond[0]) >
18048 IKFAST_EVALCOND_THRESH)
18049 {
18050 continue;
18051 }
18052 }
18053
18054 {
18055 std::vector<
18056 IkSingleDOFSolutionBase<IkReal> >
18057 vinfos(7);
18058 vinfos[0].jointtype = 1;
18059 vinfos[0].foffset = j0;
18060 vinfos[0].indices[0] = _ij0[0];
18061 vinfos[0].indices[1] = _ij0[1];
18062 vinfos[0].maxsolutions = _nj0;
18063 vinfos[1].jointtype = 1;
18064 vinfos[1].foffset = j1;
18065 vinfos[1].indices[0] = _ij1[0];
18066 vinfos[1].indices[1] = _ij1[1];
18067 vinfos[1].maxsolutions = _nj1;
18068 vinfos[2].jointtype = 1;
18069 vinfos[2].foffset = j2;
18070 vinfos[2].indices[0] = _ij2[0];
18071 vinfos[2].indices[1] = _ij2[1];
18072 vinfos[2].maxsolutions = _nj2;
18073 vinfos[3].jointtype = 1;
18074 vinfos[3].foffset = j3;
18075 vinfos[3].indices[0] = _ij3[0];
18076 vinfos[3].indices[1] = _ij3[1];
18077 vinfos[3].maxsolutions = _nj3;
18078 vinfos[4].jointtype = 1;
18079 vinfos[4].foffset = j4;
18080 vinfos[4].indices[0] = _ij4[0];
18081 vinfos[4].indices[1] = _ij4[1];
18082 vinfos[4].maxsolutions = _nj4;
18083 vinfos[5].jointtype = 1;
18084 vinfos[5].foffset = j5;
18085 vinfos[5].indices[0] = _ij5[0];
18086 vinfos[5].indices[1] = _ij5[1];
18087 vinfos[5].maxsolutions = _nj5;
18088 vinfos[6].jointtype = 1;
18089 vinfos[6].foffset = j6;
18090 vinfos[6].indices[0] = _ij6[0];
18091 vinfos[6].indices[1] = _ij6[1];
18092 vinfos[6].maxsolutions = _nj6;
18093 std::vector<int> vfree(0);
18094 solutions.AddSolution(vinfos, vfree);
18095 }
18096 }
18097 }
18098 }
18099 }
18100 }
18101 } while (0);
18102 if (bgotonextstatement)
18103 {
18104 bool bgotonextstatement = true;
18105 do
18106 {
18107 evalcond[0] =
18108 ((IKabs(new_r10)) + (IKabs(new_r01)));
18109 if (IKabs(evalcond[0]) < 0.0000050000000000)
18110 {
18111 bgotonextstatement = false;
18112 {
18113 IkReal j0eval[3];
18114 sj1 = 0;
18115 cj1 = -1.0;
18116 j1 = 3.14159265358979;
18117 new_r01 = 0;
18118 new_r10 = 0;
18119 j0eval[0] = new_r11;
18120 j0eval[1] = IKsign(new_r11);
18121 j0eval[2] = ((IKabs(cj2)) + (IKabs(sj2)));
18122 if (IKabs(j0eval[0]) < 0.0000010000000000 ||
18123 IKabs(j0eval[1]) < 0.0000010000000000 ||
18124 IKabs(j0eval[2]) < 0.0000010000000000)
18125 {
18126 {
18127 IkReal j0eval[2];
18128 sj1 = 0;
18129 cj1 = -1.0;
18130 j1 = 3.14159265358979;
18131 new_r01 = 0;
18132 new_r10 = 0;
18133 j0eval[0] = new_r00;
18134 j0eval[1] = new_r11;
18135 if (IKabs(j0eval[0]) <
18136 0.0000010000000000 ||
18137 IKabs(j0eval[1]) < 0.0000010000000000)
18138 {
18139 continue; // no branches [j0]
18140 }
18141 else
18142 {
18143 {
18144 IkReal j0array[1], cj0array[1],
18145 sj0array[1];
18146 bool j0valid[1] = { false };
18147 _nj0 = 1;
18148 CheckValue<IkReal> x827 =
18149 IKPowWithIntegerCheck(new_r00,
18150 -1);
18151 if (!x827.valid)
18152 {
18153 continue;
18154 }
18155 CheckValue<IkReal> x828 =
18156 IKPowWithIntegerCheck(new_r11,
18157 -1);
18158 if (!x828.valid)
18159 {
18160 continue;
18161 }
18162 if (IKabs((sj2 * (x827.value))) <
18163 IKFAST_ATAN2_MAGTHRESH &&
18164 IKabs(((-1.0) * cj2 *
18165 (x828.value))) <
18166 IKFAST_ATAN2_MAGTHRESH &&
18167 IKabs(
18168 IKsqr((sj2 * (x827.value))) +
18169 IKsqr(((-1.0) * cj2 *
18170 (x828.value))) -
18171 1) <= IKFAST_SINCOS_THRESH)
18172 continue;
18173 j0array[0] = IKatan2(
18174 (sj2 * (x827.value)),
18175 ((-1.0) * cj2 * (x828.value)));
18176 sj0array[0] = IKsin(j0array[0]);
18177 cj0array[0] = IKcos(j0array[0]);
18178 if (j0array[0] > IKPI)
18179 {
18180 j0array[0] -= IK2PI;
18181 }
18182 else if (j0array[0] < -IKPI)
18183 {
18184 j0array[0] += IK2PI;
18185 }
18186 j0valid[0] = true;
18187 for (int ij0 = 0; ij0 < 1; ++ij0)
18188 {
18189 if (!j0valid[ij0])
18190 {
18191 continue;
18192 }
18193 _ij0[0] = ij0;
18194 _ij0[1] = -1;
18195 for (int iij0 = ij0 + 1; iij0 < 1;
18196 ++iij0)
18197 {
18198 if (j0valid[iij0] &&
18199 IKabs(cj0array[ij0] -
18200 cj0array[iij0]) <
18201 IKFAST_SOLUTION_THRESH &&
18202 IKabs(sj0array[ij0] -
18203 sj0array[iij0]) <
18204 IKFAST_SOLUTION_THRESH)
18205 {
18206 j0valid[iij0] = false;
18207 _ij0[1] = iij0;
18208 break;
18209 }
18210 }
18211 j0 = j0array[ij0];
18212 cj0 = cj0array[ij0];
18213 sj0 = sj0array[ij0];
18214 {
18215 IkReal evalcond[7];
18216 IkReal x829 = IKsin(j0);
18217 IkReal x830 = IKcos(j0);
18218 IkReal x831 = ((1.0) * cj2);
18219 IkReal x832 = (sj2 * x829);
18220 evalcond[0] =
18221 (((new_r11 * x830)) + cj2);
18222 evalcond[1] =
18223 (sj2 + ((new_r11 * x829)));
18224 evalcond[2] =
18225 (sj2 +
18226 (((-1.0) * new_r00 * x829)));
18227 evalcond[3] =
18228 (((new_r00 * x830)) +
18229 (((-1.0) * x831)));
18230 evalcond[4] =
18231 (((sj2 * x830)) +
18232 (((-1.0) * x829 * x831)));
18233 evalcond[5] = (new_r11 + x832 +
18234 ((cj2 * x830)));
18235 evalcond[6] =
18236 ((((-1.0) * x830 * x831)) +
18237 (((-1.0) * x832)) + new_r00);
18238 if (IKabs(evalcond[0]) >
18239 IKFAST_EVALCOND_THRESH ||
18240 IKabs(evalcond[1]) >
18241 IKFAST_EVALCOND_THRESH ||
18242 IKabs(evalcond[2]) >
18243 IKFAST_EVALCOND_THRESH ||
18244 IKabs(evalcond[3]) >
18245 IKFAST_EVALCOND_THRESH ||
18246 IKabs(evalcond[4]) >
18247 IKFAST_EVALCOND_THRESH ||
18248 IKabs(evalcond[5]) >
18249 IKFAST_EVALCOND_THRESH ||
18250 IKabs(evalcond[6]) >
18251 IKFAST_EVALCOND_THRESH)
18252 {
18253 continue;
18254 }
18255 }
18256
18257 {
18258 std::vector<
18259 IkSingleDOFSolutionBase<
18260 IkReal> >
18261 vinfos(7);
18262 vinfos[0].jointtype = 1;
18263 vinfos[0].foffset = j0;
18264 vinfos[0].indices[0] = _ij0[0];
18265 vinfos[0].indices[1] = _ij0[1];
18266 vinfos[0].maxsolutions = _nj0;
18267 vinfos[1].jointtype = 1;
18268 vinfos[1].foffset = j1;
18269 vinfos[1].indices[0] = _ij1[0];
18270 vinfos[1].indices[1] = _ij1[1];
18271 vinfos[1].maxsolutions = _nj1;
18272 vinfos[2].jointtype = 1;
18273 vinfos[2].foffset = j2;
18274 vinfos[2].indices[0] = _ij2[0];
18275 vinfos[2].indices[1] = _ij2[1];
18276 vinfos[2].maxsolutions = _nj2;
18277 vinfos[3].jointtype = 1;
18278 vinfos[3].foffset = j3;
18279 vinfos[3].indices[0] = _ij3[0];
18280 vinfos[3].indices[1] = _ij3[1];
18281 vinfos[3].maxsolutions = _nj3;
18282 vinfos[4].jointtype = 1;
18283 vinfos[4].foffset = j4;
18284 vinfos[4].indices[0] = _ij4[0];
18285 vinfos[4].indices[1] = _ij4[1];
18286 vinfos[4].maxsolutions = _nj4;
18287 vinfos[5].jointtype = 1;
18288 vinfos[5].foffset = j5;
18289 vinfos[5].indices[0] = _ij5[0];
18290 vinfos[5].indices[1] = _ij5[1];
18291 vinfos[5].maxsolutions = _nj5;
18292 vinfos[6].jointtype = 1;
18293 vinfos[6].foffset = j6;
18294 vinfos[6].indices[0] = _ij6[0];
18295 vinfos[6].indices[1] = _ij6[1];
18296 vinfos[6].maxsolutions = _nj6;
18297 std::vector<int> vfree(0);
18298 solutions.AddSolution(vinfos,
18299 vfree);
18300 }
18301 }
18302 }
18303 }
18304 }
18305 }
18306 else
18307 {
18308 {
18309 IkReal j0array[1], cj0array[1],
18310 sj0array[1];
18311 bool j0valid[1] = { false };
18312 _nj0 = 1;
18313 CheckValue<IkReal> x833 =
18314 IKPowWithIntegerCheck(IKsign(new_r11),
18315 -1);
18316 if (!x833.valid)
18317 {
18318 continue;
18319 }
18320 CheckValue<IkReal> x834 =
18321 IKatan2WithCheck(
18322 IkReal(((-1.0) * sj2)),
18323 IkReal(((-1.0) * cj2)),
18324 IKFAST_ATAN2_MAGTHRESH);
18325 if (!x834.valid)
18326 {
18327 continue;
18328 }
18329 j0array[0] = ((-1.5707963267949) +
18330 (((1.5707963267949) *
18331 (x833.value))) +
18332 (x834.value));
18333 sj0array[0] = IKsin(j0array[0]);
18334 cj0array[0] = IKcos(j0array[0]);
18335 if (j0array[0] > IKPI)
18336 {
18337 j0array[0] -= IK2PI;
18338 }
18339 else if (j0array[0] < -IKPI)
18340 {
18341 j0array[0] += IK2PI;
18342 }
18343 j0valid[0] = true;
18344 for (int ij0 = 0; ij0 < 1; ++ij0)
18345 {
18346 if (!j0valid[ij0])
18347 {
18348 continue;
18349 }
18350 _ij0[0] = ij0;
18351 _ij0[1] = -1;
18352 for (int iij0 = ij0 + 1; iij0 < 1;
18353 ++iij0)
18354 {
18355 if (j0valid[iij0] &&
18356 IKabs(cj0array[ij0] -
18357 cj0array[iij0]) <
18358 IKFAST_SOLUTION_THRESH &&
18359 IKabs(sj0array[ij0] -
18360 sj0array[iij0]) <
18361 IKFAST_SOLUTION_THRESH)
18362 {
18363 j0valid[iij0] = false;
18364 _ij0[1] = iij0;
18365 break;
18366 }
18367 }
18368 j0 = j0array[ij0];
18369 cj0 = cj0array[ij0];
18370 sj0 = sj0array[ij0];
18371 {
18372 IkReal evalcond[7];
18373 IkReal x835 = IKsin(j0);
18374 IkReal x836 = IKcos(j0);
18375 IkReal x837 = ((1.0) * cj2);
18376 IkReal x838 = (sj2 * x835);
18377 evalcond[0] =
18378 (((new_r11 * x836)) + cj2);
18379 evalcond[1] =
18380 (((new_r11 * x835)) + sj2);
18381 evalcond[2] =
18382 (sj2 +
18383 (((-1.0) * new_r00 * x835)));
18384 evalcond[3] = (((new_r00 * x836)) +
18385 (((-1.0) * x837)));
18386 evalcond[4] =
18387 ((((-1.0) * x835 * x837)) +
18388 ((sj2 * x836)));
18389 evalcond[5] =
18390 (new_r11 + x838 + ((cj2 * x836)));
18391 evalcond[6] =
18392 ((((-1.0) * x836 * x837)) +
18393 (((-1.0) * x838)) + new_r00);
18394 if (IKabs(evalcond[0]) >
18395 IKFAST_EVALCOND_THRESH ||
18396 IKabs(evalcond[1]) >
18397 IKFAST_EVALCOND_THRESH ||
18398 IKabs(evalcond[2]) >
18399 IKFAST_EVALCOND_THRESH ||
18400 IKabs(evalcond[3]) >
18401 IKFAST_EVALCOND_THRESH ||
18402 IKabs(evalcond[4]) >
18403 IKFAST_EVALCOND_THRESH ||
18404 IKabs(evalcond[5]) >
18405 IKFAST_EVALCOND_THRESH ||
18406 IKabs(evalcond[6]) >
18407 IKFAST_EVALCOND_THRESH)
18408 {
18409 continue;
18410 }
18411 }
18412
18413 {
18414 std::vector<
18415 IkSingleDOFSolutionBase<IkReal> >
18416 vinfos(7);
18417 vinfos[0].jointtype = 1;
18418 vinfos[0].foffset = j0;
18419 vinfos[0].indices[0] = _ij0[0];
18420 vinfos[0].indices[1] = _ij0[1];
18421 vinfos[0].maxsolutions = _nj0;
18422 vinfos[1].jointtype = 1;
18423 vinfos[1].foffset = j1;
18424 vinfos[1].indices[0] = _ij1[0];
18425 vinfos[1].indices[1] = _ij1[1];
18426 vinfos[1].maxsolutions = _nj1;
18427 vinfos[2].jointtype = 1;
18428 vinfos[2].foffset = j2;
18429 vinfos[2].indices[0] = _ij2[0];
18430 vinfos[2].indices[1] = _ij2[1];
18431 vinfos[2].maxsolutions = _nj2;
18432 vinfos[3].jointtype = 1;
18433 vinfos[3].foffset = j3;
18434 vinfos[3].indices[0] = _ij3[0];
18435 vinfos[3].indices[1] = _ij3[1];
18436 vinfos[3].maxsolutions = _nj3;
18437 vinfos[4].jointtype = 1;
18438 vinfos[4].foffset = j4;
18439 vinfos[4].indices[0] = _ij4[0];
18440 vinfos[4].indices[1] = _ij4[1];
18441 vinfos[4].maxsolutions = _nj4;
18442 vinfos[5].jointtype = 1;
18443 vinfos[5].foffset = j5;
18444 vinfos[5].indices[0] = _ij5[0];
18445 vinfos[5].indices[1] = _ij5[1];
18446 vinfos[5].maxsolutions = _nj5;
18447 vinfos[6].jointtype = 1;
18448 vinfos[6].foffset = j6;
18449 vinfos[6].indices[0] = _ij6[0];
18450 vinfos[6].indices[1] = _ij6[1];
18451 vinfos[6].maxsolutions = _nj6;
18452 std::vector<int> vfree(0);
18453 solutions.AddSolution(vinfos, vfree);
18454 }
18455 }
18456 }
18457 }
18458 }
18459 }
18460 } while (0);
18461 if (bgotonextstatement)
18462 {
18463 bool bgotonextstatement = true;
18464 do
18465 {
18466 if (1)
18467 {
18468 bgotonextstatement = false;
18469 continue; // branch miss [j0]
18470 }
18471 } while (0);
18472 if (bgotonextstatement)
18473 {
18474 }
18475 }
18476 }
18477 }
18478 }
18479 }
18480 }
18481 }
18482 }
18483 }
18484 }
18485 }
18486 else
18487 {
18488 {
18489 IkReal j0array[1], cj0array[1], sj0array[1];
18490 bool j0valid[1] = { false };
18491 _nj0 = 1;
18492 CheckValue<IkReal> x839 = IKPowWithIntegerCheck(
18493 IKsign((((new_r11 * sj2)) + ((cj2 * new_r01)))), -1);
18494 if (!x839.valid)
18495 {
18496 continue;
18497 }
18498 CheckValue<IkReal> x840 = IKatan2WithCheck(
18499 IkReal(((-1.0) + (cj2 * cj2) + ((new_r01 * new_r10)))),
18500 IkReal(
18501 ((((-1.0) * cj2 * sj2)) + (((-1.0) * new_r10 * new_r11)))),
18502 IKFAST_ATAN2_MAGTHRESH);
18503 if (!x840.valid)
18504 {
18505 continue;
18506 }
18507 j0array[0] = ((-1.5707963267949) +
18508 (((1.5707963267949) * (x839.value))) + (x840.value));
18509 sj0array[0] = IKsin(j0array[0]);
18510 cj0array[0] = IKcos(j0array[0]);
18511 if (j0array[0] > IKPI)
18512 {
18513 j0array[0] -= IK2PI;
18514 }
18515 else if (j0array[0] < -IKPI)
18516 {
18517 j0array[0] += IK2PI;
18518 }
18519 j0valid[0] = true;
18520 for (int ij0 = 0; ij0 < 1; ++ij0)
18521 {
18522 if (!j0valid[ij0])
18523 {
18524 continue;
18525 }
18526 _ij0[0] = ij0;
18527 _ij0[1] = -1;
18528 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
18529 {
18530 if (j0valid[iij0] &&
18531 IKabs(cj0array[ij0] - cj0array[iij0]) <
18532 IKFAST_SOLUTION_THRESH &&
18533 IKabs(sj0array[ij0] - sj0array[iij0]) <
18534 IKFAST_SOLUTION_THRESH)
18535 {
18536 j0valid[iij0] = false;
18537 _ij0[1] = iij0;
18538 break;
18539 }
18540 }
18541 j0 = j0array[ij0];
18542 cj0 = cj0array[ij0];
18543 sj0 = sj0array[ij0];
18544 {
18545 IkReal evalcond[8];
18546 IkReal x841 = IKcos(j0);
18547 IkReal x842 = IKsin(j0);
18548 IkReal x843 = ((1.0) * cj2);
18549 IkReal x844 = (sj2 * x841);
18550 IkReal x845 = (sj2 * x842);
18551 IkReal x846 = ((1.0) * x842);
18552 IkReal x847 = (x842 * x843);
18553 evalcond[0] = (sj2 + ((new_r01 * x841)) + ((new_r11 * x842)));
18554 evalcond[1] = (((cj2 * x841)) + new_r11 + x845);
18555 evalcond[2] =
18556 (((new_r10 * x841)) + sj2 + (((-1.0) * new_r00 * x846)));
18557 evalcond[3] =
18558 (cj2 + (((-1.0) * new_r01 * x846)) + ((new_r11 * x841)));
18559 evalcond[4] = ((((-1.0) * x847)) + new_r10 + x844);
18560 evalcond[5] = ((((-1.0) * x847)) + new_r01 + x844);
18561 evalcond[6] = (((new_r10 * x842)) + ((new_r00 * x841)) +
18562 (((-1.0) * x843)));
18563 evalcond[7] =
18564 ((((-1.0) * x841 * x843)) + (((-1.0) * x845)) + new_r00);
18565 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
18566 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
18567 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
18568 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
18569 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
18570 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
18571 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
18572 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
18573 {
18574 continue;
18575 }
18576 }
18577
18578 {
18579 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18580 vinfos[0].jointtype = 1;
18581 vinfos[0].foffset = j0;
18582 vinfos[0].indices[0] = _ij0[0];
18583 vinfos[0].indices[1] = _ij0[1];
18584 vinfos[0].maxsolutions = _nj0;
18585 vinfos[1].jointtype = 1;
18586 vinfos[1].foffset = j1;
18587 vinfos[1].indices[0] = _ij1[0];
18588 vinfos[1].indices[1] = _ij1[1];
18589 vinfos[1].maxsolutions = _nj1;
18590 vinfos[2].jointtype = 1;
18591 vinfos[2].foffset = j2;
18592 vinfos[2].indices[0] = _ij2[0];
18593 vinfos[2].indices[1] = _ij2[1];
18594 vinfos[2].maxsolutions = _nj2;
18595 vinfos[3].jointtype = 1;
18596 vinfos[3].foffset = j3;
18597 vinfos[3].indices[0] = _ij3[0];
18598 vinfos[3].indices[1] = _ij3[1];
18599 vinfos[3].maxsolutions = _nj3;
18600 vinfos[4].jointtype = 1;
18601 vinfos[4].foffset = j4;
18602 vinfos[4].indices[0] = _ij4[0];
18603 vinfos[4].indices[1] = _ij4[1];
18604 vinfos[4].maxsolutions = _nj4;
18605 vinfos[5].jointtype = 1;
18606 vinfos[5].foffset = j5;
18607 vinfos[5].indices[0] = _ij5[0];
18608 vinfos[5].indices[1] = _ij5[1];
18609 vinfos[5].maxsolutions = _nj5;
18610 vinfos[6].jointtype = 1;
18611 vinfos[6].foffset = j6;
18612 vinfos[6].indices[0] = _ij6[0];
18613 vinfos[6].indices[1] = _ij6[1];
18614 vinfos[6].maxsolutions = _nj6;
18615 std::vector<int> vfree(0);
18616 solutions.AddSolution(vinfos, vfree);
18617 }
18618 }
18619 }
18620 }
18621 }
18622 }
18623 else
18624 {
18625 {
18626 IkReal j0array[1], cj0array[1], sj0array[1];
18627 bool j0valid[1] = { false };
18628 _nj0 = 1;
18629 IkReal x848 = ((1.0) * new_r11);
18630 CheckValue<IkReal> x849 = IKPowWithIntegerCheck(
18631 IKsign(((new_r01 * new_r01) + (new_r11 * new_r11))), -1);
18632 if (!x849.valid)
18633 {
18634 continue;
18635 }
18636 CheckValue<IkReal> x850 = IKatan2WithCheck(
18637 IkReal(((((-1.0) * sj2 * x848)) + ((cj2 * new_r01)))),
18638 IkReal(((((-1.0) * new_r01 * sj2)) + (((-1.0) * cj2 * x848)))),
18639 IKFAST_ATAN2_MAGTHRESH);
18640 if (!x850.valid)
18641 {
18642 continue;
18643 }
18644 j0array[0] = ((-1.5707963267949) +
18645 (((1.5707963267949) * (x849.value))) + (x850.value));
18646 sj0array[0] = IKsin(j0array[0]);
18647 cj0array[0] = IKcos(j0array[0]);
18648 if (j0array[0] > IKPI)
18649 {
18650 j0array[0] -= IK2PI;
18651 }
18652 else if (j0array[0] < -IKPI)
18653 {
18654 j0array[0] += IK2PI;
18655 }
18656 j0valid[0] = true;
18657 for (int ij0 = 0; ij0 < 1; ++ij0)
18658 {
18659 if (!j0valid[ij0])
18660 {
18661 continue;
18662 }
18663 _ij0[0] = ij0;
18664 _ij0[1] = -1;
18665 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
18666 {
18667 if (j0valid[iij0] &&
18668 IKabs(cj0array[ij0] - cj0array[iij0]) <
18669 IKFAST_SOLUTION_THRESH &&
18670 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
18671 {
18672 j0valid[iij0] = false;
18673 _ij0[1] = iij0;
18674 break;
18675 }
18676 }
18677 j0 = j0array[ij0];
18678 cj0 = cj0array[ij0];
18679 sj0 = sj0array[ij0];
18680 {
18681 IkReal evalcond[8];
18682 IkReal x851 = IKcos(j0);
18683 IkReal x852 = IKsin(j0);
18684 IkReal x853 = ((1.0) * cj2);
18685 IkReal x854 = (sj2 * x851);
18686 IkReal x855 = (sj2 * x852);
18687 IkReal x856 = ((1.0) * x852);
18688 IkReal x857 = (x852 * x853);
18689 evalcond[0] = (sj2 + ((new_r11 * x852)) + ((new_r01 * x851)));
18690 evalcond[1] = (((cj2 * x851)) + new_r11 + x855);
18691 evalcond[2] =
18692 (sj2 + (((-1.0) * new_r00 * x856)) + ((new_r10 * x851)));
18693 evalcond[3] =
18694 (cj2 + (((-1.0) * new_r01 * x856)) + ((new_r11 * x851)));
18695 evalcond[4] = ((((-1.0) * x857)) + new_r10 + x854);
18696 evalcond[5] = ((((-1.0) * x857)) + new_r01 + x854);
18697 evalcond[6] =
18698 ((((-1.0) * x853)) + ((new_r10 * x852)) + ((new_r00 * x851)));
18699 evalcond[7] =
18700 ((((-1.0) * x855)) + new_r00 + (((-1.0) * x851 * x853)));
18701 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
18702 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
18703 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
18704 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
18705 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
18706 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
18707 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
18708 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
18709 {
18710 continue;
18711 }
18712 }
18713
18714 {
18715 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18716 vinfos[0].jointtype = 1;
18717 vinfos[0].foffset = j0;
18718 vinfos[0].indices[0] = _ij0[0];
18719 vinfos[0].indices[1] = _ij0[1];
18720 vinfos[0].maxsolutions = _nj0;
18721 vinfos[1].jointtype = 1;
18722 vinfos[1].foffset = j1;
18723 vinfos[1].indices[0] = _ij1[0];
18724 vinfos[1].indices[1] = _ij1[1];
18725 vinfos[1].maxsolutions = _nj1;
18726 vinfos[2].jointtype = 1;
18727 vinfos[2].foffset = j2;
18728 vinfos[2].indices[0] = _ij2[0];
18729 vinfos[2].indices[1] = _ij2[1];
18730 vinfos[2].maxsolutions = _nj2;
18731 vinfos[3].jointtype = 1;
18732 vinfos[3].foffset = j3;
18733 vinfos[3].indices[0] = _ij3[0];
18734 vinfos[3].indices[1] = _ij3[1];
18735 vinfos[3].maxsolutions = _nj3;
18736 vinfos[4].jointtype = 1;
18737 vinfos[4].foffset = j4;
18738 vinfos[4].indices[0] = _ij4[0];
18739 vinfos[4].indices[1] = _ij4[1];
18740 vinfos[4].maxsolutions = _nj4;
18741 vinfos[5].jointtype = 1;
18742 vinfos[5].foffset = j5;
18743 vinfos[5].indices[0] = _ij5[0];
18744 vinfos[5].indices[1] = _ij5[1];
18745 vinfos[5].maxsolutions = _nj5;
18746 vinfos[6].jointtype = 1;
18747 vinfos[6].foffset = j6;
18748 vinfos[6].indices[0] = _ij6[0];
18749 vinfos[6].indices[1] = _ij6[1];
18750 vinfos[6].maxsolutions = _nj6;
18751 std::vector<int> vfree(0);
18752 solutions.AddSolution(vinfos, vfree);
18753 }
18754 }
18755 }
18756 }
18757 }
18758 }
18759 else
18760 {
18761 {
18762 IkReal j0array[1], cj0array[1], sj0array[1];
18763 bool j0valid[1] = { false };
18764 _nj0 = 1;
18765 IkReal x858 = ((1.0) * sj2);
18766 CheckValue<IkReal> x859 = IKatan2WithCheck(
18767 IkReal((((new_r01 * sj2)) + (((-1.0) * new_r10 * x858)))),
18768 IkReal(((((-1.0) * new_r00 * x858)) + (((-1.0) * new_r11 * x858)))),
18769 IKFAST_ATAN2_MAGTHRESH);
18770 if (!x859.valid)
18771 {
18772 continue;
18773 }
18774 CheckValue<IkReal> x860 = IKPowWithIntegerCheck(
18775 IKsign((((new_r10 * new_r11)) + ((new_r00 * new_r01)))), -1);
18776 if (!x860.valid)
18777 {
18778 continue;
18779 }
18780 j0array[0] = ((-1.5707963267949) + (x859.value) +
18781 (((1.5707963267949) * (x860.value))));
18782 sj0array[0] = IKsin(j0array[0]);
18783 cj0array[0] = IKcos(j0array[0]);
18784 if (j0array[0] > IKPI)
18785 {
18786 j0array[0] -= IK2PI;
18787 }
18788 else if (j0array[0] < -IKPI)
18789 {
18790 j0array[0] += IK2PI;
18791 }
18792 j0valid[0] = true;
18793 for (int ij0 = 0; ij0 < 1; ++ij0)
18794 {
18795 if (!j0valid[ij0])
18796 {
18797 continue;
18798 }
18799 _ij0[0] = ij0;
18800 _ij0[1] = -1;
18801 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
18802 {
18803 if (j0valid[iij0] &&
18804 IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
18805 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
18806 {
18807 j0valid[iij0] = false;
18808 _ij0[1] = iij0;
18809 break;
18810 }
18811 }
18812 j0 = j0array[ij0];
18813 cj0 = cj0array[ij0];
18814 sj0 = sj0array[ij0];
18815 {
18816 IkReal evalcond[8];
18817 IkReal x861 = IKcos(j0);
18818 IkReal x862 = IKsin(j0);
18819 IkReal x863 = ((1.0) * cj2);
18820 IkReal x864 = (sj2 * x861);
18821 IkReal x865 = (sj2 * x862);
18822 IkReal x866 = ((1.0) * x862);
18823 IkReal x867 = (x862 * x863);
18824 evalcond[0] = (((new_r01 * x861)) + sj2 + ((new_r11 * x862)));
18825 evalcond[1] = (((cj2 * x861)) + new_r11 + x865);
18826 evalcond[2] = (sj2 + (((-1.0) * new_r00 * x866)) + ((new_r10 * x861)));
18827 evalcond[3] = (cj2 + (((-1.0) * new_r01 * x866)) + ((new_r11 * x861)));
18828 evalcond[4] = ((((-1.0) * x867)) + new_r10 + x864);
18829 evalcond[5] = ((((-1.0) * x867)) + new_r01 + x864);
18830 evalcond[6] =
18831 ((((-1.0) * x863)) + ((new_r00 * x861)) + ((new_r10 * x862)));
18832 evalcond[7] = ((((-1.0) * x865)) + (((-1.0) * x861 * x863)) + new_r00);
18833 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
18834 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
18835 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
18836 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
18837 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
18838 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
18839 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
18840 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
18841 {
18842 continue;
18843 }
18844 }
18845
18846 {
18847 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
18848 vinfos[0].jointtype = 1;
18849 vinfos[0].foffset = j0;
18850 vinfos[0].indices[0] = _ij0[0];
18851 vinfos[0].indices[1] = _ij0[1];
18852 vinfos[0].maxsolutions = _nj0;
18853 vinfos[1].jointtype = 1;
18854 vinfos[1].foffset = j1;
18855 vinfos[1].indices[0] = _ij1[0];
18856 vinfos[1].indices[1] = _ij1[1];
18857 vinfos[1].maxsolutions = _nj1;
18858 vinfos[2].jointtype = 1;
18859 vinfos[2].foffset = j2;
18860 vinfos[2].indices[0] = _ij2[0];
18861 vinfos[2].indices[1] = _ij2[1];
18862 vinfos[2].maxsolutions = _nj2;
18863 vinfos[3].jointtype = 1;
18864 vinfos[3].foffset = j3;
18865 vinfos[3].indices[0] = _ij3[0];
18866 vinfos[3].indices[1] = _ij3[1];
18867 vinfos[3].maxsolutions = _nj3;
18868 vinfos[4].jointtype = 1;
18869 vinfos[4].foffset = j4;
18870 vinfos[4].indices[0] = _ij4[0];
18871 vinfos[4].indices[1] = _ij4[1];
18872 vinfos[4].maxsolutions = _nj4;
18873 vinfos[5].jointtype = 1;
18874 vinfos[5].foffset = j5;
18875 vinfos[5].indices[0] = _ij5[0];
18876 vinfos[5].indices[1] = _ij5[1];
18877 vinfos[5].maxsolutions = _nj5;
18878 vinfos[6].jointtype = 1;
18879 vinfos[6].foffset = j6;
18880 vinfos[6].indices[0] = _ij6[0];
18881 vinfos[6].indices[1] = _ij6[1];
18882 vinfos[6].maxsolutions = _nj6;
18883 std::vector<int> vfree(0);
18884 solutions.AddSolution(vinfos, vfree);
18885 }
18886 }
18887 }
18888 }
18889 }
18890 }
18891 } while (0);
18892 if (bgotonextstatement)
18893 {
18894 bool bgotonextstatement = true;
18895 do
18896 {
18897 evalcond[0] = ((IKabs(new_r12)) + (IKabs(new_r02)));
18898 if (IKabs(evalcond[0]) < 0.0000050000000000)
18899 {
18900 bgotonextstatement = false;
18901 {
18902 IkReal j0eval[1];
18903 new_r02 = 0;
18904 new_r12 = 0;
18905 new_r20 = 0;
18906 new_r21 = 0;
18907 j0eval[0] = ((IKabs(new_r10)) + (IKabs(new_r00)));
18908 if (IKabs(j0eval[0]) < 0.0000010000000000)
18909 {
18910 {
18911 IkReal j0eval[1];
18912 new_r02 = 0;
18913 new_r12 = 0;
18914 new_r20 = 0;
18915 new_r21 = 0;
18916 j0eval[0] = ((IKabs(new_r11)) + (IKabs(new_r01)));
18917 if (IKabs(j0eval[0]) < 0.0000010000000000)
18918 {
18919 {
18920 IkReal j0eval[1];
18921 new_r02 = 0;
18922 new_r12 = 0;
18923 new_r20 = 0;
18924 new_r21 = 0;
18925 j0eval[0] =
18926 ((IKabs((new_r10 * new_r22))) + (IKabs((new_r00 * new_r22))));
18927 if (IKabs(j0eval[0]) < 0.0000010000000000)
18928 {
18929 continue; // no branches [j0]
18930 }
18931 else
18932 {
18933 {
18934 IkReal j0array[2], cj0array[2], sj0array[2];
18935 bool j0valid[2] = { false };
18936 _nj0 = 2;
18937 CheckValue<IkReal> x869 =
18938 IKatan2WithCheck(IkReal((new_r00 * new_r22)),
18939 IkReal((new_r10 * new_r22)),
18940 IKFAST_ATAN2_MAGTHRESH);
18941 if (!x869.valid)
18942 {
18943 continue;
18944 }
18945 IkReal x868 = x869.value;
18946 j0array[0] = ((-1.0) * x868);
18947 sj0array[0] = IKsin(j0array[0]);
18948 cj0array[0] = IKcos(j0array[0]);
18949 j0array[1] = ((3.14159265358979) + (((-1.0) * x868)));
18950 sj0array[1] = IKsin(j0array[1]);
18951 cj0array[1] = IKcos(j0array[1]);
18952 if (j0array[0] > IKPI)
18953 {
18954 j0array[0] -= IK2PI;
18955 }
18956 else if (j0array[0] < -IKPI)
18957 {
18958 j0array[0] += IK2PI;
18959 }
18960 j0valid[0] = true;
18961 if (j0array[1] > IKPI)
18962 {
18963 j0array[1] -= IK2PI;
18964 }
18965 else if (j0array[1] < -IKPI)
18966 {
18967 j0array[1] += IK2PI;
18968 }
18969 j0valid[1] = true;
18970 for (int ij0 = 0; ij0 < 2; ++ij0)
18971 {
18972 if (!j0valid[ij0])
18973 {
18974 continue;
18975 }
18976 _ij0[0] = ij0;
18977 _ij0[1] = -1;
18978 for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
18979 {
18980 if (j0valid[iij0] &&
18981 IKabs(cj0array[ij0] - cj0array[iij0]) <
18982 IKFAST_SOLUTION_THRESH &&
18983 IKabs(sj0array[ij0] - sj0array[iij0]) <
18984 IKFAST_SOLUTION_THRESH)
18985 {
18986 j0valid[iij0] = false;
18987 _ij0[1] = iij0;
18988 break;
18989 }
18990 }
18991 j0 = j0array[ij0];
18992 cj0 = cj0array[ij0];
18993 sj0 = sj0array[ij0];
18994 {
18995 IkReal evalcond[5];
18996 IkReal x870 = IKsin(j0);
18997 IkReal x871 = IKcos(j0);
18998 IkReal x872 = ((1.0) * x870);
18999 IkReal x873 = (new_r11 * x870);
19000 IkReal x874 = (new_r01 * x871);
19001 evalcond[0] = (((new_r00 * x871)) + ((new_r10 * x870)));
19002 evalcond[1] = (x873 + x874);
19003 evalcond[2] =
19004 ((((-1.0) * new_r00 * x872)) + ((new_r10 * x871)));
19005 evalcond[3] =
19006 ((((-1.0) * new_r01 * x872)) + ((new_r11 * x871)));
19007 evalcond[4] = (((new_r22 * x874)) + ((new_r22 * x873)));
19008 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19009 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19010 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19011 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19012 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH)
19013 {
19014 continue;
19015 }
19016 }
19017
19018 {
19019 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19020 vinfos[0].jointtype = 1;
19021 vinfos[0].foffset = j0;
19022 vinfos[0].indices[0] = _ij0[0];
19023 vinfos[0].indices[1] = _ij0[1];
19024 vinfos[0].maxsolutions = _nj0;
19025 vinfos[1].jointtype = 1;
19026 vinfos[1].foffset = j1;
19027 vinfos[1].indices[0] = _ij1[0];
19028 vinfos[1].indices[1] = _ij1[1];
19029 vinfos[1].maxsolutions = _nj1;
19030 vinfos[2].jointtype = 1;
19031 vinfos[2].foffset = j2;
19032 vinfos[2].indices[0] = _ij2[0];
19033 vinfos[2].indices[1] = _ij2[1];
19034 vinfos[2].maxsolutions = _nj2;
19035 vinfos[3].jointtype = 1;
19036 vinfos[3].foffset = j3;
19037 vinfos[3].indices[0] = _ij3[0];
19038 vinfos[3].indices[1] = _ij3[1];
19039 vinfos[3].maxsolutions = _nj3;
19040 vinfos[4].jointtype = 1;
19041 vinfos[4].foffset = j4;
19042 vinfos[4].indices[0] = _ij4[0];
19043 vinfos[4].indices[1] = _ij4[1];
19044 vinfos[4].maxsolutions = _nj4;
19045 vinfos[5].jointtype = 1;
19046 vinfos[5].foffset = j5;
19047 vinfos[5].indices[0] = _ij5[0];
19048 vinfos[5].indices[1] = _ij5[1];
19049 vinfos[5].maxsolutions = _nj5;
19050 vinfos[6].jointtype = 1;
19051 vinfos[6].foffset = j6;
19052 vinfos[6].indices[0] = _ij6[0];
19053 vinfos[6].indices[1] = _ij6[1];
19054 vinfos[6].maxsolutions = _nj6;
19055 std::vector<int> vfree(0);
19056 solutions.AddSolution(vinfos, vfree);
19057 }
19058 }
19059 }
19060 }
19061 }
19062 }
19063 else
19064 {
19065 {
19066 IkReal j0array[2], cj0array[2], sj0array[2];
19067 bool j0valid[2] = { false };
19068 _nj0 = 2;
19069 CheckValue<IkReal> x876 = IKatan2WithCheck(
19070 IkReal(new_r01), IkReal(new_r11), IKFAST_ATAN2_MAGTHRESH);
19071 if (!x876.valid)
19072 {
19073 continue;
19074 }
19075 IkReal x875 = x876.value;
19076 j0array[0] = ((-1.0) * x875);
19077 sj0array[0] = IKsin(j0array[0]);
19078 cj0array[0] = IKcos(j0array[0]);
19079 j0array[1] = ((3.14159265358979) + (((-1.0) * x875)));
19080 sj0array[1] = IKsin(j0array[1]);
19081 cj0array[1] = IKcos(j0array[1]);
19082 if (j0array[0] > IKPI)
19083 {
19084 j0array[0] -= IK2PI;
19085 }
19086 else if (j0array[0] < -IKPI)
19087 {
19088 j0array[0] += IK2PI;
19089 }
19090 j0valid[0] = true;
19091 if (j0array[1] > IKPI)
19092 {
19093 j0array[1] -= IK2PI;
19094 }
19095 else if (j0array[1] < -IKPI)
19096 {
19097 j0array[1] += IK2PI;
19098 }
19099 j0valid[1] = true;
19100 for (int ij0 = 0; ij0 < 2; ++ij0)
19101 {
19102 if (!j0valid[ij0])
19103 {
19104 continue;
19105 }
19106 _ij0[0] = ij0;
19107 _ij0[1] = -1;
19108 for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
19109 {
19110 if (j0valid[iij0] &&
19111 IKabs(cj0array[ij0] - cj0array[iij0]) <
19112 IKFAST_SOLUTION_THRESH &&
19113 IKabs(sj0array[ij0] - sj0array[iij0]) <
19114 IKFAST_SOLUTION_THRESH)
19115 {
19116 j0valid[iij0] = false;
19117 _ij0[1] = iij0;
19118 break;
19119 }
19120 }
19121 j0 = j0array[ij0];
19122 cj0 = cj0array[ij0];
19123 sj0 = sj0array[ij0];
19124 {
19125 IkReal evalcond[5];
19126 IkReal x877 = IKcos(j0);
19127 IkReal x878 = IKsin(j0);
19128 IkReal x879 = (new_r10 * x878);
19129 IkReal x880 = ((1.0) * x878);
19130 IkReal x881 = (new_r00 * x877);
19131 evalcond[0] = (x879 + x881);
19132 evalcond[1] = ((((-1.0) * new_r00 * x880)) + ((new_r10 * x877)));
19133 evalcond[2] = ((((-1.0) * new_r01 * x880)) + ((new_r11 * x877)));
19134 evalcond[3] = (((new_r22 * x881)) + ((new_r22 * x879)));
19135 evalcond[4] =
19136 (((new_r11 * new_r22 * x878)) + ((new_r01 * new_r22 * x877)));
19137 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19138 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19139 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19140 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19141 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH)
19142 {
19143 continue;
19144 }
19145 }
19146
19147 {
19148 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19149 vinfos[0].jointtype = 1;
19150 vinfos[0].foffset = j0;
19151 vinfos[0].indices[0] = _ij0[0];
19152 vinfos[0].indices[1] = _ij0[1];
19153 vinfos[0].maxsolutions = _nj0;
19154 vinfos[1].jointtype = 1;
19155 vinfos[1].foffset = j1;
19156 vinfos[1].indices[0] = _ij1[0];
19157 vinfos[1].indices[1] = _ij1[1];
19158 vinfos[1].maxsolutions = _nj1;
19159 vinfos[2].jointtype = 1;
19160 vinfos[2].foffset = j2;
19161 vinfos[2].indices[0] = _ij2[0];
19162 vinfos[2].indices[1] = _ij2[1];
19163 vinfos[2].maxsolutions = _nj2;
19164 vinfos[3].jointtype = 1;
19165 vinfos[3].foffset = j3;
19166 vinfos[3].indices[0] = _ij3[0];
19167 vinfos[3].indices[1] = _ij3[1];
19168 vinfos[3].maxsolutions = _nj3;
19169 vinfos[4].jointtype = 1;
19170 vinfos[4].foffset = j4;
19171 vinfos[4].indices[0] = _ij4[0];
19172 vinfos[4].indices[1] = _ij4[1];
19173 vinfos[4].maxsolutions = _nj4;
19174 vinfos[5].jointtype = 1;
19175 vinfos[5].foffset = j5;
19176 vinfos[5].indices[0] = _ij5[0];
19177 vinfos[5].indices[1] = _ij5[1];
19178 vinfos[5].maxsolutions = _nj5;
19179 vinfos[6].jointtype = 1;
19180 vinfos[6].foffset = j6;
19181 vinfos[6].indices[0] = _ij6[0];
19182 vinfos[6].indices[1] = _ij6[1];
19183 vinfos[6].maxsolutions = _nj6;
19184 std::vector<int> vfree(0);
19185 solutions.AddSolution(vinfos, vfree);
19186 }
19187 }
19188 }
19189 }
19190 }
19191 }
19192 else
19193 {
19194 {
19195 IkReal j0array[2], cj0array[2], sj0array[2];
19196 bool j0valid[2] = { false };
19197 _nj0 = 2;
19198 CheckValue<IkReal> x883 = IKatan2WithCheck(
19199 IkReal(new_r00), IkReal(new_r10), IKFAST_ATAN2_MAGTHRESH);
19200 if (!x883.valid)
19201 {
19202 continue;
19203 }
19204 IkReal x882 = x883.value;
19205 j0array[0] = ((-1.0) * x882);
19206 sj0array[0] = IKsin(j0array[0]);
19207 cj0array[0] = IKcos(j0array[0]);
19208 j0array[1] = ((3.14159265358979) + (((-1.0) * x882)));
19209 sj0array[1] = IKsin(j0array[1]);
19210 cj0array[1] = IKcos(j0array[1]);
19211 if (j0array[0] > IKPI)
19212 {
19213 j0array[0] -= IK2PI;
19214 }
19215 else if (j0array[0] < -IKPI)
19216 {
19217 j0array[0] += IK2PI;
19218 }
19219 j0valid[0] = true;
19220 if (j0array[1] > IKPI)
19221 {
19222 j0array[1] -= IK2PI;
19223 }
19224 else if (j0array[1] < -IKPI)
19225 {
19226 j0array[1] += IK2PI;
19227 }
19228 j0valid[1] = true;
19229 for (int ij0 = 0; ij0 < 2; ++ij0)
19230 {
19231 if (!j0valid[ij0])
19232 {
19233 continue;
19234 }
19235 _ij0[0] = ij0;
19236 _ij0[1] = -1;
19237 for (int iij0 = ij0 + 1; iij0 < 2; ++iij0)
19238 {
19239 if (j0valid[iij0] &&
19240 IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19241 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19242 {
19243 j0valid[iij0] = false;
19244 _ij0[1] = iij0;
19245 break;
19246 }
19247 }
19248 j0 = j0array[ij0];
19249 cj0 = cj0array[ij0];
19250 sj0 = sj0array[ij0];
19251 {
19252 IkReal evalcond[5];
19253 IkReal x884 = IKcos(j0);
19254 IkReal x885 = IKsin(j0);
19255 IkReal x886 = ((1.0) * x885);
19256 IkReal x887 = (new_r11 * x885);
19257 IkReal x888 = (new_r22 * x884);
19258 evalcond[0] = (((new_r01 * x884)) + x887);
19259 evalcond[1] = (((new_r10 * x884)) + (((-1.0) * new_r00 * x886)));
19260 evalcond[2] = (((new_r11 * x884)) + (((-1.0) * new_r01 * x886)));
19261 evalcond[3] = (((new_r00 * x888)) + ((new_r10 * new_r22 * x885)));
19262 evalcond[4] = (((new_r01 * x888)) + ((new_r22 * x887)));
19263 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19264 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19265 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19266 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19267 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH)
19268 {
19269 continue;
19270 }
19271 }
19272
19273 {
19274 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19275 vinfos[0].jointtype = 1;
19276 vinfos[0].foffset = j0;
19277 vinfos[0].indices[0] = _ij0[0];
19278 vinfos[0].indices[1] = _ij0[1];
19279 vinfos[0].maxsolutions = _nj0;
19280 vinfos[1].jointtype = 1;
19281 vinfos[1].foffset = j1;
19282 vinfos[1].indices[0] = _ij1[0];
19283 vinfos[1].indices[1] = _ij1[1];
19284 vinfos[1].maxsolutions = _nj1;
19285 vinfos[2].jointtype = 1;
19286 vinfos[2].foffset = j2;
19287 vinfos[2].indices[0] = _ij2[0];
19288 vinfos[2].indices[1] = _ij2[1];
19289 vinfos[2].maxsolutions = _nj2;
19290 vinfos[3].jointtype = 1;
19291 vinfos[3].foffset = j3;
19292 vinfos[3].indices[0] = _ij3[0];
19293 vinfos[3].indices[1] = _ij3[1];
19294 vinfos[3].maxsolutions = _nj3;
19295 vinfos[4].jointtype = 1;
19296 vinfos[4].foffset = j4;
19297 vinfos[4].indices[0] = _ij4[0];
19298 vinfos[4].indices[1] = _ij4[1];
19299 vinfos[4].maxsolutions = _nj4;
19300 vinfos[5].jointtype = 1;
19301 vinfos[5].foffset = j5;
19302 vinfos[5].indices[0] = _ij5[0];
19303 vinfos[5].indices[1] = _ij5[1];
19304 vinfos[5].maxsolutions = _nj5;
19305 vinfos[6].jointtype = 1;
19306 vinfos[6].foffset = j6;
19307 vinfos[6].indices[0] = _ij6[0];
19308 vinfos[6].indices[1] = _ij6[1];
19309 vinfos[6].maxsolutions = _nj6;
19310 std::vector<int> vfree(0);
19311 solutions.AddSolution(vinfos, vfree);
19312 }
19313 }
19314 }
19315 }
19316 }
19317 }
19318 } while (0);
19319 if (bgotonextstatement)
19320 {
19321 bool bgotonextstatement = true;
19322 do
19323 {
19324 if (1)
19325 {
19326 bgotonextstatement = false;
19327 continue; // branch miss [j0]
19328 }
19329 } while (0);
19330 if (bgotonextstatement)
19331 {
19332 }
19333 }
19334 }
19335 }
19336 }
19337 }
19338 else
19339 {
19340 {
19341 IkReal j0array[1], cj0array[1], sj0array[1];
19342 bool j0valid[1] = { false };
19343 _nj0 = 1;
19344 CheckValue<IkReal> x890 = IKPowWithIntegerCheck(sj1, -1);
19345 if (!x890.valid)
19346 {
19347 continue;
19348 }
19349 IkReal x889 = x890.value;
19350 CheckValue<IkReal> x891 = IKPowWithIntegerCheck(new_r00, -1);
19351 if (!x891.valid)
19352 {
19353 continue;
19354 }
19355 if (IKabs((x889 * (x891.value) * ((((sj1 * sj2)) + ((new_r02 * new_r10)))))) <
19356 IKFAST_ATAN2_MAGTHRESH &&
19357 IKabs((new_r02 * x889)) < IKFAST_ATAN2_MAGTHRESH &&
19358 IKabs(IKsqr((x889 * (x891.value) * ((((sj1 * sj2)) + ((new_r02 * new_r10)))))) +
19359 IKsqr((new_r02 * x889)) - 1) <= IKFAST_SINCOS_THRESH)
19360 continue;
19361 j0array[0] = IKatan2((x889 * (x891.value) * ((((sj1 * sj2)) + ((new_r02 * new_r10))))),
19362 (new_r02 * x889));
19363 sj0array[0] = IKsin(j0array[0]);
19364 cj0array[0] = IKcos(j0array[0]);
19365 if (j0array[0] > IKPI)
19366 {
19367 j0array[0] -= IK2PI;
19368 }
19369 else if (j0array[0] < -IKPI)
19370 {
19371 j0array[0] += IK2PI;
19372 }
19373 j0valid[0] = true;
19374 for (int ij0 = 0; ij0 < 1; ++ij0)
19375 {
19376 if (!j0valid[ij0])
19377 {
19378 continue;
19379 }
19380 _ij0[0] = ij0;
19381 _ij0[1] = -1;
19382 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
19383 {
19384 if (j0valid[iij0] &&
19385 IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19386 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19387 {
19388 j0valid[iij0] = false;
19389 _ij0[1] = iij0;
19390 break;
19391 }
19392 }
19393 j0 = j0array[ij0];
19394 cj0 = cj0array[ij0];
19395 sj0 = sj0array[ij0];
19396 {
19397 IkReal evalcond[18];
19398 IkReal x892 = IKcos(j0);
19399 IkReal x893 = IKsin(j0);
19400 IkReal x894 = ((1.0) * sj1);
19401 IkReal x895 = ((1.0) * sj2);
19402 IkReal x896 = ((1.0) * cj1);
19403 IkReal x897 = (new_r10 * x893);
19404 IkReal x898 = (cj1 * x892);
19405 IkReal x899 = (cj1 * x893);
19406 IkReal x900 = (new_r00 * x892);
19407 IkReal x901 = ((1.0) * x893);
19408 IkReal x902 = (new_r11 * x893);
19409 IkReal x903 = (new_r12 * x893);
19410 IkReal x904 = (new_r02 * x892);
19411 IkReal x905 = (new_r01 * x892);
19412 evalcond[0] = ((((-1.0) * x892 * x894)) + new_r02);
19413 evalcond[1] = ((((-1.0) * x893 * x894)) + new_r12);
19414 evalcond[2] = ((((-1.0) * new_r02 * x901)) + ((new_r12 * x892)));
19415 evalcond[3] = (sj2 + ((new_r10 * x892)) + (((-1.0) * new_r00 * x901)));
19416 evalcond[4] = (((new_r11 * x892)) + cj2 + (((-1.0) * new_r01 * x901)));
19417 evalcond[5] = (((cj2 * x899)) + ((sj2 * x892)) + new_r10);
19418 evalcond[6] = ((((-1.0) * x894)) + x904 + x903);
19419 evalcond[7] = (((cj1 * cj2)) + x897 + x900);
19420 evalcond[8] = (((cj2 * x898)) + (((-1.0) * x893 * x895)) + new_r00);
19421 evalcond[9] = (((cj2 * x892)) + (((-1.0) * x895 * x899)) + new_r11);
19422 evalcond[10] = ((((-1.0) * cj1 * x895)) + x905 + x902);
19423 evalcond[11] = ((((-1.0) * x895 * x898)) + new_r01 + (((-1.0) * cj2 * x901)));
19424 evalcond[12] =
19425 (((new_r12 * x899)) + ((new_r02 * x898)) + (((-1.0) * new_r22 * x894)));
19426 evalcond[13] =
19427 (cj2 + ((new_r00 * x898)) + (((-1.0) * new_r20 * x894)) + ((cj1 * x897)));
19428 evalcond[14] = ((((-1.0) * x894 * x900)) + (((-1.0) * new_r20 * x896)) +
19429 (((-1.0) * x894 * x897)));
19430 evalcond[15] = ((((-1.0) * x894 * x902)) + (((-1.0) * x894 * x905)) +
19431 (((-1.0) * new_r21 * x896)));
19432 evalcond[16] = ((1.0) + (((-1.0) * x894 * x903)) + (((-1.0) * x894 * x904)) +
19433 (((-1.0) * new_r22 * x896)));
19434 evalcond[17] = (((new_r11 * x899)) + ((new_r01 * x898)) +
19435 (((-1.0) * new_r21 * x894)) + (((-1.0) * x895)));
19436 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19437 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19438 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19439 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19440 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19441 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19442 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19443 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
19444 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
19445 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
19446 IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
19447 IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ||
19448 IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH ||
19449 IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH ||
19450 IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH ||
19451 IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH ||
19452 IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH ||
19453 IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH)
19454 {
19455 continue;
19456 }
19457 }
19458
19459 {
19460 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19461 vinfos[0].jointtype = 1;
19462 vinfos[0].foffset = j0;
19463 vinfos[0].indices[0] = _ij0[0];
19464 vinfos[0].indices[1] = _ij0[1];
19465 vinfos[0].maxsolutions = _nj0;
19466 vinfos[1].jointtype = 1;
19467 vinfos[1].foffset = j1;
19468 vinfos[1].indices[0] = _ij1[0];
19469 vinfos[1].indices[1] = _ij1[1];
19470 vinfos[1].maxsolutions = _nj1;
19471 vinfos[2].jointtype = 1;
19472 vinfos[2].foffset = j2;
19473 vinfos[2].indices[0] = _ij2[0];
19474 vinfos[2].indices[1] = _ij2[1];
19475 vinfos[2].maxsolutions = _nj2;
19476 vinfos[3].jointtype = 1;
19477 vinfos[3].foffset = j3;
19478 vinfos[3].indices[0] = _ij3[0];
19479 vinfos[3].indices[1] = _ij3[1];
19480 vinfos[3].maxsolutions = _nj3;
19481 vinfos[4].jointtype = 1;
19482 vinfos[4].foffset = j4;
19483 vinfos[4].indices[0] = _ij4[0];
19484 vinfos[4].indices[1] = _ij4[1];
19485 vinfos[4].maxsolutions = _nj4;
19486 vinfos[5].jointtype = 1;
19487 vinfos[5].foffset = j5;
19488 vinfos[5].indices[0] = _ij5[0];
19489 vinfos[5].indices[1] = _ij5[1];
19490 vinfos[5].maxsolutions = _nj5;
19491 vinfos[6].jointtype = 1;
19492 vinfos[6].foffset = j6;
19493 vinfos[6].indices[0] = _ij6[0];
19494 vinfos[6].indices[1] = _ij6[1];
19495 vinfos[6].maxsolutions = _nj6;
19496 std::vector<int> vfree(0);
19497 solutions.AddSolution(vinfos, vfree);
19498 }
19499 }
19500 }
19501 }
19502 }
19503 }
19504 else
19505 {
19506 {
19507 IkReal j0array[1], cj0array[1], sj0array[1];
19508 bool j0valid[1] = { false };
19509 _nj0 = 1;
19510 CheckValue<IkReal> x906 = IKPowWithIntegerCheck(IKsign(sj1), -1);
19511 if (!x906.valid)
19512 {
19513 continue;
19514 }
19515 CheckValue<IkReal> x907 =
19516 IKatan2WithCheck(IkReal(new_r12), IkReal(new_r02), IKFAST_ATAN2_MAGTHRESH);
19517 if (!x907.valid)
19518 {
19519 continue;
19520 }
19521 j0array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x906.value))) + (x907.value));
19522 sj0array[0] = IKsin(j0array[0]);
19523 cj0array[0] = IKcos(j0array[0]);
19524 if (j0array[0] > IKPI)
19525 {
19526 j0array[0] -= IK2PI;
19527 }
19528 else if (j0array[0] < -IKPI)
19529 {
19530 j0array[0] += IK2PI;
19531 }
19532 j0valid[0] = true;
19533 for (int ij0 = 0; ij0 < 1; ++ij0)
19534 {
19535 if (!j0valid[ij0])
19536 {
19537 continue;
19538 }
19539 _ij0[0] = ij0;
19540 _ij0[1] = -1;
19541 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
19542 {
19543 if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19544 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19545 {
19546 j0valid[iij0] = false;
19547 _ij0[1] = iij0;
19548 break;
19549 }
19550 }
19551 j0 = j0array[ij0];
19552 cj0 = cj0array[ij0];
19553 sj0 = sj0array[ij0];
19554 {
19555 IkReal evalcond[18];
19556 IkReal x908 = IKcos(j0);
19557 IkReal x909 = IKsin(j0);
19558 IkReal x910 = ((1.0) * sj1);
19559 IkReal x911 = ((1.0) * sj2);
19560 IkReal x912 = ((1.0) * cj1);
19561 IkReal x913 = (new_r10 * x909);
19562 IkReal x914 = (cj1 * x908);
19563 IkReal x915 = (cj1 * x909);
19564 IkReal x916 = (new_r00 * x908);
19565 IkReal x917 = ((1.0) * x909);
19566 IkReal x918 = (new_r11 * x909);
19567 IkReal x919 = (new_r12 * x909);
19568 IkReal x920 = (new_r02 * x908);
19569 IkReal x921 = (new_r01 * x908);
19570 evalcond[0] = ((((-1.0) * x908 * x910)) + new_r02);
19571 evalcond[1] = (new_r12 + (((-1.0) * x909 * x910)));
19572 evalcond[2] = ((((-1.0) * new_r02 * x917)) + ((new_r12 * x908)));
19573 evalcond[3] = (sj2 + ((new_r10 * x908)) + (((-1.0) * new_r00 * x917)));
19574 evalcond[4] = (cj2 + ((new_r11 * x908)) + (((-1.0) * new_r01 * x917)));
19575 evalcond[5] = (((sj2 * x908)) + ((cj2 * x915)) + new_r10);
19576 evalcond[6] = ((((-1.0) * x910)) + x920 + x919);
19577 evalcond[7] = (((cj1 * cj2)) + x913 + x916);
19578 evalcond[8] = (((cj2 * x914)) + new_r00 + (((-1.0) * x909 * x911)));
19579 evalcond[9] = (((cj2 * x908)) + new_r11 + (((-1.0) * x911 * x915)));
19580 evalcond[10] = ((((-1.0) * cj1 * x911)) + x921 + x918);
19581 evalcond[11] = (new_r01 + (((-1.0) * x911 * x914)) + (((-1.0) * cj2 * x917)));
19582 evalcond[12] = ((((-1.0) * new_r22 * x910)) + ((new_r12 * x915)) + ((new_r02 * x914)));
19583 evalcond[13] =
19584 (cj2 + (((-1.0) * new_r20 * x910)) + ((new_r00 * x914)) + ((cj1 * x913)));
19585 evalcond[14] =
19586 ((((-1.0) * new_r20 * x912)) + (((-1.0) * x910 * x913)) + (((-1.0) * x910 * x916)));
19587 evalcond[15] =
19588 ((((-1.0) * new_r21 * x912)) + (((-1.0) * x910 * x918)) + (((-1.0) * x910 * x921)));
19589 evalcond[16] = ((1.0) + (((-1.0) * new_r22 * x912)) + (((-1.0) * x910 * x919)) +
19590 (((-1.0) * x910 * x920)));
19591 evalcond[17] = ((((-1.0) * new_r21 * x910)) + ((new_r11 * x915)) + (((-1.0) * x911)) +
19592 ((new_r01 * x914)));
19593 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19594 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19595 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19596 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19597 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19598 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19599 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19600 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
19601 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
19602 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
19603 IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
19604 IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH ||
19605 IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH ||
19606 IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH ||
19607 IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH ||
19608 IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH ||
19609 IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH ||
19610 IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH)
19611 {
19612 continue;
19613 }
19614 }
19615
19616 {
19617 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19618 vinfos[0].jointtype = 1;
19619 vinfos[0].foffset = j0;
19620 vinfos[0].indices[0] = _ij0[0];
19621 vinfos[0].indices[1] = _ij0[1];
19622 vinfos[0].maxsolutions = _nj0;
19623 vinfos[1].jointtype = 1;
19624 vinfos[1].foffset = j1;
19625 vinfos[1].indices[0] = _ij1[0];
19626 vinfos[1].indices[1] = _ij1[1];
19627 vinfos[1].maxsolutions = _nj1;
19628 vinfos[2].jointtype = 1;
19629 vinfos[2].foffset = j2;
19630 vinfos[2].indices[0] = _ij2[0];
19631 vinfos[2].indices[1] = _ij2[1];
19632 vinfos[2].maxsolutions = _nj2;
19633 vinfos[3].jointtype = 1;
19634 vinfos[3].foffset = j3;
19635 vinfos[3].indices[0] = _ij3[0];
19636 vinfos[3].indices[1] = _ij3[1];
19637 vinfos[3].maxsolutions = _nj3;
19638 vinfos[4].jointtype = 1;
19639 vinfos[4].foffset = j4;
19640 vinfos[4].indices[0] = _ij4[0];
19641 vinfos[4].indices[1] = _ij4[1];
19642 vinfos[4].maxsolutions = _nj4;
19643 vinfos[5].jointtype = 1;
19644 vinfos[5].foffset = j5;
19645 vinfos[5].indices[0] = _ij5[0];
19646 vinfos[5].indices[1] = _ij5[1];
19647 vinfos[5].maxsolutions = _nj5;
19648 vinfos[6].jointtype = 1;
19649 vinfos[6].foffset = j6;
19650 vinfos[6].indices[0] = _ij6[0];
19651 vinfos[6].indices[1] = _ij6[1];
19652 vinfos[6].maxsolutions = _nj6;
19653 std::vector<int> vfree(0);
19654 solutions.AddSolution(vinfos, vfree);
19655 }
19656 }
19657 }
19658 }
19659 }
19660 }
19661 }
19662 }
19663 }
19664 }
19665 else
19666 {
19667 {
19668 IkReal j0array[1], cj0array[1], sj0array[1];
19669 bool j0valid[1] = { false };
19670 _nj0 = 1;
19671 CheckValue<IkReal> x922 = IKPowWithIntegerCheck(IKsign(sj1), -1);
19672 if (!x922.valid)
19673 {
19674 continue;
19675 }
19676 CheckValue<IkReal> x923 = IKatan2WithCheck(IkReal(new_r12), IkReal(new_r02), IKFAST_ATAN2_MAGTHRESH);
19677 if (!x923.valid)
19678 {
19679 continue;
19680 }
19681 j0array[0] = ((-1.5707963267949) + (((1.5707963267949) * (x922.value))) + (x923.value));
19682 sj0array[0] = IKsin(j0array[0]);
19683 cj0array[0] = IKcos(j0array[0]);
19684 if (j0array[0] > IKPI)
19685 {
19686 j0array[0] -= IK2PI;
19687 }
19688 else if (j0array[0] < -IKPI)
19689 {
19690 j0array[0] += IK2PI;
19691 }
19692 j0valid[0] = true;
19693 for (int ij0 = 0; ij0 < 1; ++ij0)
19694 {
19695 if (!j0valid[ij0])
19696 {
19697 continue;
19698 }
19699 _ij0[0] = ij0;
19700 _ij0[1] = -1;
19701 for (int iij0 = ij0 + 1; iij0 < 1; ++iij0)
19702 {
19703 if (j0valid[iij0] && IKabs(cj0array[ij0] - cj0array[iij0]) < IKFAST_SOLUTION_THRESH &&
19704 IKabs(sj0array[ij0] - sj0array[iij0]) < IKFAST_SOLUTION_THRESH)
19705 {
19706 j0valid[iij0] = false;
19707 _ij0[1] = iij0;
19708 break;
19709 }
19710 }
19711 j0 = j0array[ij0];
19712 cj0 = cj0array[ij0];
19713 sj0 = sj0array[ij0];
19714 {
19715 IkReal evalcond[8];
19716 IkReal x924 = IKcos(j0);
19717 IkReal x925 = IKsin(j0);
19718 IkReal x926 = ((1.0) * cj1);
19719 IkReal x927 = ((1.0) * sj1);
19720 IkReal x928 = (new_r12 * x925);
19721 IkReal x929 = (new_r02 * x924);
19722 evalcond[0] = ((((-1.0) * x924 * x927)) + new_r02);
19723 evalcond[1] = ((((-1.0) * x925 * x927)) + new_r12);
19724 evalcond[2] = ((((-1.0) * new_r02 * x925)) + ((new_r12 * x924)));
19725 evalcond[3] = ((((-1.0) * x927)) + x928 + x929);
19726 evalcond[4] = ((((-1.0) * new_r22 * x927)) + ((cj1 * x929)) + ((cj1 * x928)));
19727 evalcond[5] = ((((-1.0) * new_r10 * x925 * x927)) + (((-1.0) * new_r00 * x924 * x927)) +
19728 (((-1.0) * new_r20 * x926)));
19729 evalcond[6] = ((((-1.0) * new_r21 * x926)) + (((-1.0) * new_r01 * x924 * x927)) +
19730 (((-1.0) * new_r11 * x925 * x927)));
19731 evalcond[7] =
19732 ((1.0) + (((-1.0) * x927 * x928)) + (((-1.0) * x927 * x929)) + (((-1.0) * new_r22 * x926)));
19733 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19734 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19735 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19736 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
19737 {
19738 continue;
19739 }
19740 }
19741
19742 {
19743 IkReal j2eval[3];
19744 j2eval[0] = sj1;
19745 j2eval[1] = IKsign(sj1);
19746 j2eval[2] = ((IKabs(new_r20)) + (IKabs(new_r21)));
19747 if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
19748 IKabs(j2eval[2]) < 0.0000010000000000)
19749 {
19750 {
19751 IkReal j2eval[2];
19752 j2eval[0] = sj1;
19753 j2eval[1] = cj0;
19754 if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000)
19755 {
19756 {
19757 IkReal j2eval[3];
19758 j2eval[0] = sj1;
19759 j2eval[1] = cj1;
19760 j2eval[2] = sj0;
19761 if (IKabs(j2eval[0]) < 0.0000010000000000 || IKabs(j2eval[1]) < 0.0000010000000000 ||
19762 IKabs(j2eval[2]) < 0.0000010000000000)
19763 {
19764 {
19765 IkReal evalcond[5];
19766 bool bgotonextstatement = true;
19767 do
19768 {
19769 evalcond[0] = ((-3.14159265358979) +
19770 (IKfmod(((3.14159265358979) + (IKabs(j1))), 6.28318530717959)));
19771 evalcond[1] = new_r21;
19772 evalcond[2] = new_r02;
19773 evalcond[3] = new_r12;
19774 evalcond[4] = new_r20;
19775 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
19776 IKabs(evalcond[1]) < 0.0000050000000000 &&
19777 IKabs(evalcond[2]) < 0.0000050000000000 &&
19778 IKabs(evalcond[3]) < 0.0000050000000000 &&
19779 IKabs(evalcond[4]) < 0.0000050000000000)
19780 {
19781 bgotonextstatement = false;
19782 {
19783 IkReal j2array[1], cj2array[1], sj2array[1];
19784 bool j2valid[1] = { false };
19785 _nj2 = 1;
19786 IkReal x930 = ((1.0) * cj0);
19787 if (IKabs((((new_r00 * sj0)) + (((-1.0) * new_r10 * x930)))) <
19788 IKFAST_ATAN2_MAGTHRESH &&
19789 IKabs(((((-1.0) * new_r00 * x930)) + (((-1.0) * new_r10 * sj0)))) <
19790 IKFAST_ATAN2_MAGTHRESH &&
19791 IKabs(IKsqr((((new_r00 * sj0)) + (((-1.0) * new_r10 * x930)))) +
19792 IKsqr(((((-1.0) * new_r00 * x930)) + (((-1.0) * new_r10 * sj0)))) -
19793 1) <= IKFAST_SINCOS_THRESH)
19794 continue;
19795 j2array[0] = IKatan2((((new_r00 * sj0)) + (((-1.0) * new_r10 * x930))),
19796 ((((-1.0) * new_r00 * x930)) + (((-1.0) * new_r10 * sj0))));
19797 sj2array[0] = IKsin(j2array[0]);
19798 cj2array[0] = IKcos(j2array[0]);
19799 if (j2array[0] > IKPI)
19800 {
19801 j2array[0] -= IK2PI;
19802 }
19803 else if (j2array[0] < -IKPI)
19804 {
19805 j2array[0] += IK2PI;
19806 }
19807 j2valid[0] = true;
19808 for (int ij2 = 0; ij2 < 1; ++ij2)
19809 {
19810 if (!j2valid[ij2])
19811 {
19812 continue;
19813 }
19814 _ij2[0] = ij2;
19815 _ij2[1] = -1;
19816 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
19817 {
19818 if (j2valid[iij2] &&
19819 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
19820 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
19821 {
19822 j2valid[iij2] = false;
19823 _ij2[1] = iij2;
19824 break;
19825 }
19826 }
19827 j2 = j2array[ij2];
19828 cj2 = cj2array[ij2];
19829 sj2 = sj2array[ij2];
19830 {
19831 IkReal evalcond[8];
19832 IkReal x931 = IKcos(j2);
19833 IkReal x932 = IKsin(j2);
19834 IkReal x933 = ((1.0) * sj0);
19835 IkReal x934 = (cj0 * x931);
19836 IkReal x935 = (cj0 * x932);
19837 IkReal x936 = (x932 * x933);
19838 evalcond[0] = (((new_r10 * sj0)) + ((cj0 * new_r00)) + x931);
19839 evalcond[1] = ((((-1.0) * new_r00 * x933)) + ((cj0 * new_r10)) + x932);
19840 evalcond[2] = ((((-1.0) * new_r01 * x933)) + ((cj0 * new_r11)) + x931);
19841 evalcond[3] = (((sj0 * x931)) + new_r10 + x935);
19842 evalcond[4] = (((new_r11 * sj0)) + (((-1.0) * x932)) + ((cj0 * new_r01)));
19843 evalcond[5] = (new_r00 + x934 + (((-1.0) * x936)));
19844 evalcond[6] = (new_r11 + x934 + (((-1.0) * x936)));
19845 evalcond[7] = ((((-1.0) * x931 * x933)) + (((-1.0) * x935)) + new_r01);
19846 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19847 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19848 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19849 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19850 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19851 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19852 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19853 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
19854 {
19855 continue;
19856 }
19857 }
19858
19859 {
19860 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
19861 vinfos[0].jointtype = 1;
19862 vinfos[0].foffset = j0;
19863 vinfos[0].indices[0] = _ij0[0];
19864 vinfos[0].indices[1] = _ij0[1];
19865 vinfos[0].maxsolutions = _nj0;
19866 vinfos[1].jointtype = 1;
19867 vinfos[1].foffset = j1;
19868 vinfos[1].indices[0] = _ij1[0];
19869 vinfos[1].indices[1] = _ij1[1];
19870 vinfos[1].maxsolutions = _nj1;
19871 vinfos[2].jointtype = 1;
19872 vinfos[2].foffset = j2;
19873 vinfos[2].indices[0] = _ij2[0];
19874 vinfos[2].indices[1] = _ij2[1];
19875 vinfos[2].maxsolutions = _nj2;
19876 vinfos[3].jointtype = 1;
19877 vinfos[3].foffset = j3;
19878 vinfos[3].indices[0] = _ij3[0];
19879 vinfos[3].indices[1] = _ij3[1];
19880 vinfos[3].maxsolutions = _nj3;
19881 vinfos[4].jointtype = 1;
19882 vinfos[4].foffset = j4;
19883 vinfos[4].indices[0] = _ij4[0];
19884 vinfos[4].indices[1] = _ij4[1];
19885 vinfos[4].maxsolutions = _nj4;
19886 vinfos[5].jointtype = 1;
19887 vinfos[5].foffset = j5;
19888 vinfos[5].indices[0] = _ij5[0];
19889 vinfos[5].indices[1] = _ij5[1];
19890 vinfos[5].maxsolutions = _nj5;
19891 vinfos[6].jointtype = 1;
19892 vinfos[6].foffset = j6;
19893 vinfos[6].indices[0] = _ij6[0];
19894 vinfos[6].indices[1] = _ij6[1];
19895 vinfos[6].maxsolutions = _nj6;
19896 std::vector<int> vfree(0);
19897 solutions.AddSolution(vinfos, vfree);
19898 }
19899 }
19900 }
19901 }
19902 } while (0);
19903 if (bgotonextstatement)
19904 {
19905 bool bgotonextstatement = true;
19906 do
19907 {
19908 evalcond[0] = ((-3.14159265358979) +
19909 (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j1)))),
19910 6.28318530717959)));
19911 evalcond[1] = new_r21;
19912 evalcond[2] = new_r02;
19913 evalcond[3] = new_r12;
19914 evalcond[4] = new_r20;
19915 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
19916 IKabs(evalcond[1]) < 0.0000050000000000 &&
19917 IKabs(evalcond[2]) < 0.0000050000000000 &&
19918 IKabs(evalcond[3]) < 0.0000050000000000 &&
19919 IKabs(evalcond[4]) < 0.0000050000000000)
19920 {
19921 bgotonextstatement = false;
19922 {
19923 IkReal j2array[1], cj2array[1], sj2array[1];
19924 bool j2valid[1] = { false };
19925 _nj2 = 1;
19926 IkReal x937 = ((1.0) * cj0);
19927 if (IKabs(((((-1.0) * new_r10 * x937)) + (((-1.0) * new_r11 * sj0)))) <
19928 IKFAST_ATAN2_MAGTHRESH &&
19929 IKabs((((new_r10 * sj0)) + (((-1.0) * new_r11 * x937)))) <
19930 IKFAST_ATAN2_MAGTHRESH &&
19931 IKabs(IKsqr(((((-1.0) * new_r10 * x937)) + (((-1.0) * new_r11 * sj0)))) +
19932 IKsqr((((new_r10 * sj0)) + (((-1.0) * new_r11 * x937)))) - 1) <=
19933 IKFAST_SINCOS_THRESH)
19934 continue;
19935 j2array[0] = IKatan2(((((-1.0) * new_r10 * x937)) + (((-1.0) * new_r11 * sj0))),
19936 (((new_r10 * sj0)) + (((-1.0) * new_r11 * x937))));
19937 sj2array[0] = IKsin(j2array[0]);
19938 cj2array[0] = IKcos(j2array[0]);
19939 if (j2array[0] > IKPI)
19940 {
19941 j2array[0] -= IK2PI;
19942 }
19943 else if (j2array[0] < -IKPI)
19944 {
19945 j2array[0] += IK2PI;
19946 }
19947 j2valid[0] = true;
19948 for (int ij2 = 0; ij2 < 1; ++ij2)
19949 {
19950 if (!j2valid[ij2])
19951 {
19952 continue;
19953 }
19954 _ij2[0] = ij2;
19955 _ij2[1] = -1;
19956 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
19957 {
19958 if (j2valid[iij2] &&
19959 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
19960 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
19961 {
19962 j2valid[iij2] = false;
19963 _ij2[1] = iij2;
19964 break;
19965 }
19966 }
19967 j2 = j2array[ij2];
19968 cj2 = cj2array[ij2];
19969 sj2 = sj2array[ij2];
19970 {
19971 IkReal evalcond[8];
19972 IkReal x938 = IKsin(j2);
19973 IkReal x939 = IKcos(j2);
19974 IkReal x940 = ((1.0) * sj0);
19975 IkReal x941 = (cj0 * x938);
19976 IkReal x942 = ((1.0) * x939);
19977 IkReal x943 = (x939 * x940);
19978 evalcond[0] = (((new_r11 * sj0)) + ((cj0 * new_r01)) + x938);
19979 evalcond[1] = (((cj0 * new_r10)) + (((-1.0) * new_r00 * x940)) + x938);
19980 evalcond[2] = ((((-1.0) * new_r01 * x940)) + ((cj0 * new_r11)) + x939);
19981 evalcond[3] = ((((-1.0) * x942)) + ((new_r10 * sj0)) + ((cj0 * new_r00)));
19982 evalcond[4] = (((sj0 * x938)) + ((cj0 * x939)) + new_r11);
19983 evalcond[5] = ((((-1.0) * x943)) + new_r10 + x941);
19984 evalcond[6] = ((((-1.0) * x943)) + new_r01 + x941);
19985 evalcond[7] =
19986 ((((-1.0) * x938 * x940)) + (((-1.0) * cj0 * x942)) + new_r00);
19987 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
19988 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
19989 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
19990 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
19991 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
19992 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
19993 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
19994 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
19995 {
19996 continue;
19997 }
19998 }
19999
20000 {
20001 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20002 vinfos[0].jointtype = 1;
20003 vinfos[0].foffset = j0;
20004 vinfos[0].indices[0] = _ij0[0];
20005 vinfos[0].indices[1] = _ij0[1];
20006 vinfos[0].maxsolutions = _nj0;
20007 vinfos[1].jointtype = 1;
20008 vinfos[1].foffset = j1;
20009 vinfos[1].indices[0] = _ij1[0];
20010 vinfos[1].indices[1] = _ij1[1];
20011 vinfos[1].maxsolutions = _nj1;
20012 vinfos[2].jointtype = 1;
20013 vinfos[2].foffset = j2;
20014 vinfos[2].indices[0] = _ij2[0];
20015 vinfos[2].indices[1] = _ij2[1];
20016 vinfos[2].maxsolutions = _nj2;
20017 vinfos[3].jointtype = 1;
20018 vinfos[3].foffset = j3;
20019 vinfos[3].indices[0] = _ij3[0];
20020 vinfos[3].indices[1] = _ij3[1];
20021 vinfos[3].maxsolutions = _nj3;
20022 vinfos[4].jointtype = 1;
20023 vinfos[4].foffset = j4;
20024 vinfos[4].indices[0] = _ij4[0];
20025 vinfos[4].indices[1] = _ij4[1];
20026 vinfos[4].maxsolutions = _nj4;
20027 vinfos[5].jointtype = 1;
20028 vinfos[5].foffset = j5;
20029 vinfos[5].indices[0] = _ij5[0];
20030 vinfos[5].indices[1] = _ij5[1];
20031 vinfos[5].maxsolutions = _nj5;
20032 vinfos[6].jointtype = 1;
20033 vinfos[6].foffset = j6;
20034 vinfos[6].indices[0] = _ij6[0];
20035 vinfos[6].indices[1] = _ij6[1];
20036 vinfos[6].maxsolutions = _nj6;
20037 std::vector<int> vfree(0);
20038 solutions.AddSolution(vinfos, vfree);
20039 }
20040 }
20041 }
20042 }
20043 } while (0);
20044 if (bgotonextstatement)
20045 {
20046 bool bgotonextstatement = true;
20047 do
20048 {
20049 evalcond[0] = ((-3.14159265358979) +
20050 (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j1)))),
20051 6.28318530717959)));
20052 evalcond[1] = new_r22;
20053 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20054 IKabs(evalcond[1]) < 0.0000050000000000)
20055 {
20056 bgotonextstatement = false;
20057 {
20058 IkReal j2array[1], cj2array[1], sj2array[1];
20059 bool j2valid[1] = { false };
20060 _nj2 = 1;
20061 if (IKabs(((-1.0) * new_r21)) < IKFAST_ATAN2_MAGTHRESH &&
20062 IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH &&
20063 IKabs(IKsqr(((-1.0) * new_r21)) + IKsqr(new_r20) - 1) <=
20064 IKFAST_SINCOS_THRESH)
20065 continue;
20066 j2array[0] = IKatan2(((-1.0) * new_r21), new_r20);
20067 sj2array[0] = IKsin(j2array[0]);
20068 cj2array[0] = IKcos(j2array[0]);
20069 if (j2array[0] > IKPI)
20070 {
20071 j2array[0] -= IK2PI;
20072 }
20073 else if (j2array[0] < -IKPI)
20074 {
20075 j2array[0] += IK2PI;
20076 }
20077 j2valid[0] = true;
20078 for (int ij2 = 0; ij2 < 1; ++ij2)
20079 {
20080 if (!j2valid[ij2])
20081 {
20082 continue;
20083 }
20084 _ij2[0] = ij2;
20085 _ij2[1] = -1;
20086 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20087 {
20088 if (j2valid[iij2] &&
20089 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
20090 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20091 {
20092 j2valid[iij2] = false;
20093 _ij2[1] = iij2;
20094 break;
20095 }
20096 }
20097 j2 = j2array[ij2];
20098 cj2 = cj2array[ij2];
20099 sj2 = sj2array[ij2];
20100 {
20101 IkReal evalcond[8];
20102 IkReal x944 = IKsin(j2);
20103 IkReal x945 = IKcos(j2);
20104 IkReal x946 = ((1.0) * sj0);
20105 evalcond[0] = (new_r21 + x944);
20106 evalcond[1] = ((((-1.0) * x945)) + new_r20);
20107 evalcond[2] = (((new_r02 * x944)) + new_r10);
20108 evalcond[3] = (((cj0 * x945)) + new_r11);
20109 evalcond[4] = ((((-1.0) * x944 * x946)) + new_r00);
20110 evalcond[5] = ((((-1.0) * x945 * x946)) + new_r01);
20111 evalcond[6] = (((cj0 * new_r10)) + (((-1.0) * new_r00 * x946)) + x944);
20112 evalcond[7] = ((((-1.0) * new_r01 * x946)) + ((cj0 * new_r11)) + x945);
20113 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20114 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20115 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20116 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20117 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20118 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20119 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20120 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20121 {
20122 continue;
20123 }
20124 }
20125
20126 {
20127 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20128 vinfos[0].jointtype = 1;
20129 vinfos[0].foffset = j0;
20130 vinfos[0].indices[0] = _ij0[0];
20131 vinfos[0].indices[1] = _ij0[1];
20132 vinfos[0].maxsolutions = _nj0;
20133 vinfos[1].jointtype = 1;
20134 vinfos[1].foffset = j1;
20135 vinfos[1].indices[0] = _ij1[0];
20136 vinfos[1].indices[1] = _ij1[1];
20137 vinfos[1].maxsolutions = _nj1;
20138 vinfos[2].jointtype = 1;
20139 vinfos[2].foffset = j2;
20140 vinfos[2].indices[0] = _ij2[0];
20141 vinfos[2].indices[1] = _ij2[1];
20142 vinfos[2].maxsolutions = _nj2;
20143 vinfos[3].jointtype = 1;
20144 vinfos[3].foffset = j3;
20145 vinfos[3].indices[0] = _ij3[0];
20146 vinfos[3].indices[1] = _ij3[1];
20147 vinfos[3].maxsolutions = _nj3;
20148 vinfos[4].jointtype = 1;
20149 vinfos[4].foffset = j4;
20150 vinfos[4].indices[0] = _ij4[0];
20151 vinfos[4].indices[1] = _ij4[1];
20152 vinfos[4].maxsolutions = _nj4;
20153 vinfos[5].jointtype = 1;
20154 vinfos[5].foffset = j5;
20155 vinfos[5].indices[0] = _ij5[0];
20156 vinfos[5].indices[1] = _ij5[1];
20157 vinfos[5].maxsolutions = _nj5;
20158 vinfos[6].jointtype = 1;
20159 vinfos[6].foffset = j6;
20160 vinfos[6].indices[0] = _ij6[0];
20161 vinfos[6].indices[1] = _ij6[1];
20162 vinfos[6].maxsolutions = _nj6;
20163 std::vector<int> vfree(0);
20164 solutions.AddSolution(vinfos, vfree);
20165 }
20166 }
20167 }
20168 }
20169 } while (0);
20170 if (bgotonextstatement)
20171 {
20172 bool bgotonextstatement = true;
20173 do
20174 {
20175 evalcond[0] = ((-3.14159265358979) +
20176 (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j1)))),
20177 6.28318530717959)));
20178 evalcond[1] = new_r22;
20179 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20180 IKabs(evalcond[1]) < 0.0000050000000000)
20181 {
20182 bgotonextstatement = false;
20183 {
20184 IkReal j2array[1], cj2array[1], sj2array[1];
20185 bool j2valid[1] = { false };
20186 _nj2 = 1;
20187 if (IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH &&
20188 IKabs(((-1.0) * new_r20)) < IKFAST_ATAN2_MAGTHRESH &&
20189 IKabs(IKsqr(new_r21) + IKsqr(((-1.0) * new_r20)) - 1) <=
20190 IKFAST_SINCOS_THRESH)
20191 continue;
20192 j2array[0] = IKatan2(new_r21, ((-1.0) * new_r20));
20193 sj2array[0] = IKsin(j2array[0]);
20194 cj2array[0] = IKcos(j2array[0]);
20195 if (j2array[0] > IKPI)
20196 {
20197 j2array[0] -= IK2PI;
20198 }
20199 else if (j2array[0] < -IKPI)
20200 {
20201 j2array[0] += IK2PI;
20202 }
20203 j2valid[0] = true;
20204 for (int ij2 = 0; ij2 < 1; ++ij2)
20205 {
20206 if (!j2valid[ij2])
20207 {
20208 continue;
20209 }
20210 _ij2[0] = ij2;
20211 _ij2[1] = -1;
20212 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20213 {
20214 if (j2valid[iij2] &&
20215 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
20216 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20217 {
20218 j2valid[iij2] = false;
20219 _ij2[1] = iij2;
20220 break;
20221 }
20222 }
20223 j2 = j2array[ij2];
20224 cj2 = cj2array[ij2];
20225 sj2 = sj2array[ij2];
20226 {
20227 IkReal evalcond[8];
20228 IkReal x947 = IKcos(j2);
20229 IkReal x948 = IKsin(j2);
20230 IkReal x949 = ((1.0) * sj0);
20231 IkReal x950 = ((1.0) * x948);
20232 evalcond[0] = (new_r20 + x947);
20233 evalcond[1] = (new_r21 + (((-1.0) * x950)));
20234 evalcond[2] = (((cj0 * x947)) + new_r11);
20235 evalcond[3] = (new_r10 + (((-1.0) * new_r02 * x950)));
20236 evalcond[4] = ((((-1.0) * x948 * x949)) + new_r00);
20237 evalcond[5] = ((((-1.0) * x947 * x949)) + new_r01);
20238 evalcond[6] = (((cj0 * new_r10)) + (((-1.0) * new_r00 * x949)) + x948);
20239 evalcond[7] = ((((-1.0) * new_r01 * x949)) + ((cj0 * new_r11)) + x947);
20240 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20241 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20242 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20243 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20244 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20245 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20246 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20247 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20248 {
20249 continue;
20250 }
20251 }
20252
20253 {
20254 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20255 vinfos[0].jointtype = 1;
20256 vinfos[0].foffset = j0;
20257 vinfos[0].indices[0] = _ij0[0];
20258 vinfos[0].indices[1] = _ij0[1];
20259 vinfos[0].maxsolutions = _nj0;
20260 vinfos[1].jointtype = 1;
20261 vinfos[1].foffset = j1;
20262 vinfos[1].indices[0] = _ij1[0];
20263 vinfos[1].indices[1] = _ij1[1];
20264 vinfos[1].maxsolutions = _nj1;
20265 vinfos[2].jointtype = 1;
20266 vinfos[2].foffset = j2;
20267 vinfos[2].indices[0] = _ij2[0];
20268 vinfos[2].indices[1] = _ij2[1];
20269 vinfos[2].maxsolutions = _nj2;
20270 vinfos[3].jointtype = 1;
20271 vinfos[3].foffset = j3;
20272 vinfos[3].indices[0] = _ij3[0];
20273 vinfos[3].indices[1] = _ij3[1];
20274 vinfos[3].maxsolutions = _nj3;
20275 vinfos[4].jointtype = 1;
20276 vinfos[4].foffset = j4;
20277 vinfos[4].indices[0] = _ij4[0];
20278 vinfos[4].indices[1] = _ij4[1];
20279 vinfos[4].maxsolutions = _nj4;
20280 vinfos[5].jointtype = 1;
20281 vinfos[5].foffset = j5;
20282 vinfos[5].indices[0] = _ij5[0];
20283 vinfos[5].indices[1] = _ij5[1];
20284 vinfos[5].maxsolutions = _nj5;
20285 vinfos[6].jointtype = 1;
20286 vinfos[6].foffset = j6;
20287 vinfos[6].indices[0] = _ij6[0];
20288 vinfos[6].indices[1] = _ij6[1];
20289 vinfos[6].maxsolutions = _nj6;
20290 std::vector<int> vfree(0);
20291 solutions.AddSolution(vinfos, vfree);
20292 }
20293 }
20294 }
20295 }
20296 } while (0);
20297 if (bgotonextstatement)
20298 {
20299 bool bgotonextstatement = true;
20300 do
20301 {
20302 evalcond[0] =
20303 ((-3.14159265358979) +
20304 (IKfmod(((3.14159265358979) + (IKabs(j0))), 6.28318530717959)));
20305 evalcond[1] = new_r12;
20306 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20307 IKabs(evalcond[1]) < 0.0000050000000000)
20308 {
20309 bgotonextstatement = false;
20310 {
20311 IkReal j2array[1], cj2array[1], sj2array[1];
20312 bool j2valid[1] = { false };
20313 _nj2 = 1;
20314 if (IKabs(((-1.0) * new_r10)) < IKFAST_ATAN2_MAGTHRESH &&
20315 IKabs(((-1.0) * new_r11)) < IKFAST_ATAN2_MAGTHRESH &&
20316 IKabs(IKsqr(((-1.0) * new_r10)) + IKsqr(((-1.0) * new_r11)) - 1) <=
20317 IKFAST_SINCOS_THRESH)
20318 continue;
20319 j2array[0] = IKatan2(((-1.0) * new_r10), ((-1.0) * new_r11));
20320 sj2array[0] = IKsin(j2array[0]);
20321 cj2array[0] = IKcos(j2array[0]);
20322 if (j2array[0] > IKPI)
20323 {
20324 j2array[0] -= IK2PI;
20325 }
20326 else if (j2array[0] < -IKPI)
20327 {
20328 j2array[0] += IK2PI;
20329 }
20330 j2valid[0] = true;
20331 for (int ij2 = 0; ij2 < 1; ++ij2)
20332 {
20333 if (!j2valid[ij2])
20334 {
20335 continue;
20336 }
20337 _ij2[0] = ij2;
20338 _ij2[1] = -1;
20339 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20340 {
20341 if (j2valid[iij2] &&
20342 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
20343 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20344 {
20345 j2valid[iij2] = false;
20346 _ij2[1] = iij2;
20347 break;
20348 }
20349 }
20350 j2 = j2array[ij2];
20351 cj2 = cj2array[ij2];
20352 sj2 = sj2array[ij2];
20353 {
20354 IkReal evalcond[8];
20355 IkReal x951 = IKsin(j2);
20356 IkReal x952 = IKcos(j2);
20357 IkReal x953 = ((1.0) * sj1);
20358 IkReal x954 = ((1.0) * x951);
20359 evalcond[0] = (new_r10 + x951);
20360 evalcond[1] = (new_r11 + x952);
20361 evalcond[2] = (((sj1 * x951)) + new_r21);
20362 evalcond[3] = (((cj1 * x952)) + new_r00);
20363 evalcond[4] = (new_r20 + (((-1.0) * x952 * x953)));
20364 evalcond[5] = ((((-1.0) * cj1 * x954)) + new_r01);
20365 evalcond[6] =
20366 (((cj1 * new_r00)) + x952 + (((-1.0) * new_r20 * x953)));
20367 evalcond[7] = ((((-1.0) * new_r21 * x953)) + ((cj1 * new_r01)) +
20368 (((-1.0) * x954)));
20369 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20370 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20371 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20372 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20373 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20374 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20375 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20376 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20377 {
20378 continue;
20379 }
20380 }
20381
20382 {
20383 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20384 vinfos[0].jointtype = 1;
20385 vinfos[0].foffset = j0;
20386 vinfos[0].indices[0] = _ij0[0];
20387 vinfos[0].indices[1] = _ij0[1];
20388 vinfos[0].maxsolutions = _nj0;
20389 vinfos[1].jointtype = 1;
20390 vinfos[1].foffset = j1;
20391 vinfos[1].indices[0] = _ij1[0];
20392 vinfos[1].indices[1] = _ij1[1];
20393 vinfos[1].maxsolutions = _nj1;
20394 vinfos[2].jointtype = 1;
20395 vinfos[2].foffset = j2;
20396 vinfos[2].indices[0] = _ij2[0];
20397 vinfos[2].indices[1] = _ij2[1];
20398 vinfos[2].maxsolutions = _nj2;
20399 vinfos[3].jointtype = 1;
20400 vinfos[3].foffset = j3;
20401 vinfos[3].indices[0] = _ij3[0];
20402 vinfos[3].indices[1] = _ij3[1];
20403 vinfos[3].maxsolutions = _nj3;
20404 vinfos[4].jointtype = 1;
20405 vinfos[4].foffset = j4;
20406 vinfos[4].indices[0] = _ij4[0];
20407 vinfos[4].indices[1] = _ij4[1];
20408 vinfos[4].maxsolutions = _nj4;
20409 vinfos[5].jointtype = 1;
20410 vinfos[5].foffset = j5;
20411 vinfos[5].indices[0] = _ij5[0];
20412 vinfos[5].indices[1] = _ij5[1];
20413 vinfos[5].maxsolutions = _nj5;
20414 vinfos[6].jointtype = 1;
20415 vinfos[6].foffset = j6;
20416 vinfos[6].indices[0] = _ij6[0];
20417 vinfos[6].indices[1] = _ij6[1];
20418 vinfos[6].maxsolutions = _nj6;
20419 std::vector<int> vfree(0);
20420 solutions.AddSolution(vinfos, vfree);
20421 }
20422 }
20423 }
20424 }
20425 } while (0);
20426 if (bgotonextstatement)
20427 {
20428 bool bgotonextstatement = true;
20429 do
20430 {
20431 evalcond[0] =
20432 ((-3.14159265358979) +
20433 (IKfmod(((3.14159265358979) + (IKabs(((-3.14159265358979) + j0)))),
20434 6.28318530717959)));
20435 evalcond[1] = new_r12;
20436 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20437 IKabs(evalcond[1]) < 0.0000050000000000)
20438 {
20439 bgotonextstatement = false;
20440 {
20441 IkReal j2array[1], cj2array[1], sj2array[1];
20442 bool j2valid[1] = { false };
20443 _nj2 = 1;
20444 if (IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH &&
20445 IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH &&
20446 IKabs(IKsqr(new_r10) + IKsqr(new_r11) - 1) <= IKFAST_SINCOS_THRESH)
20447 continue;
20448 j2array[0] = IKatan2(new_r10, new_r11);
20449 sj2array[0] = IKsin(j2array[0]);
20450 cj2array[0] = IKcos(j2array[0]);
20451 if (j2array[0] > IKPI)
20452 {
20453 j2array[0] -= IK2PI;
20454 }
20455 else if (j2array[0] < -IKPI)
20456 {
20457 j2array[0] += IK2PI;
20458 }
20459 j2valid[0] = true;
20460 for (int ij2 = 0; ij2 < 1; ++ij2)
20461 {
20462 if (!j2valid[ij2])
20463 {
20464 continue;
20465 }
20466 _ij2[0] = ij2;
20467 _ij2[1] = -1;
20468 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20469 {
20470 if (j2valid[iij2] &&
20471 IKabs(cj2array[ij2] - cj2array[iij2]) <
20472 IKFAST_SOLUTION_THRESH &&
20473 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
20474 {
20475 j2valid[iij2] = false;
20476 _ij2[1] = iij2;
20477 break;
20478 }
20479 }
20480 j2 = j2array[ij2];
20481 cj2 = cj2array[ij2];
20482 sj2 = sj2array[ij2];
20483 {
20484 IkReal evalcond[8];
20485 IkReal x955 = IKsin(j2);
20486 IkReal x956 = IKcos(j2);
20487 IkReal x957 = ((1.0) * sj1);
20488 IkReal x958 = ((1.0) * new_r00);
20489 IkReal x959 = ((1.0) * new_r01);
20490 IkReal x960 = ((1.0) * x955);
20491 evalcond[0] = (((sj1 * x955)) + new_r21);
20492 evalcond[1] = ((((-1.0) * new_r10)) + x955);
20493 evalcond[2] = ((((-1.0) * new_r11)) + x956);
20494 evalcond[3] = ((((-1.0) * x956 * x957)) + new_r20);
20495 evalcond[4] = (((cj1 * x956)) + (((-1.0) * x958)));
20496 evalcond[5] = ((((-1.0) * cj1 * x960)) + (((-1.0) * x959)));
20497 evalcond[6] =
20498 ((((-1.0) * cj1 * x958)) + x956 + (((-1.0) * new_r20 * x957)));
20499 evalcond[7] = ((((-1.0) * new_r21 * x957)) +
20500 (((-1.0) * cj1 * x959)) + (((-1.0) * x960)));
20501 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20502 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20503 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20504 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20505 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20506 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20507 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20508 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20509 {
20510 continue;
20511 }
20512 }
20513
20514 {
20515 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20516 vinfos[0].jointtype = 1;
20517 vinfos[0].foffset = j0;
20518 vinfos[0].indices[0] = _ij0[0];
20519 vinfos[0].indices[1] = _ij0[1];
20520 vinfos[0].maxsolutions = _nj0;
20521 vinfos[1].jointtype = 1;
20522 vinfos[1].foffset = j1;
20523 vinfos[1].indices[0] = _ij1[0];
20524 vinfos[1].indices[1] = _ij1[1];
20525 vinfos[1].maxsolutions = _nj1;
20526 vinfos[2].jointtype = 1;
20527 vinfos[2].foffset = j2;
20528 vinfos[2].indices[0] = _ij2[0];
20529 vinfos[2].indices[1] = _ij2[1];
20530 vinfos[2].maxsolutions = _nj2;
20531 vinfos[3].jointtype = 1;
20532 vinfos[3].foffset = j3;
20533 vinfos[3].indices[0] = _ij3[0];
20534 vinfos[3].indices[1] = _ij3[1];
20535 vinfos[3].maxsolutions = _nj3;
20536 vinfos[4].jointtype = 1;
20537 vinfos[4].foffset = j4;
20538 vinfos[4].indices[0] = _ij4[0];
20539 vinfos[4].indices[1] = _ij4[1];
20540 vinfos[4].maxsolutions = _nj4;
20541 vinfos[5].jointtype = 1;
20542 vinfos[5].foffset = j5;
20543 vinfos[5].indices[0] = _ij5[0];
20544 vinfos[5].indices[1] = _ij5[1];
20545 vinfos[5].maxsolutions = _nj5;
20546 vinfos[6].jointtype = 1;
20547 vinfos[6].foffset = j6;
20548 vinfos[6].indices[0] = _ij6[0];
20549 vinfos[6].indices[1] = _ij6[1];
20550 vinfos[6].maxsolutions = _nj6;
20551 std::vector<int> vfree(0);
20552 solutions.AddSolution(vinfos, vfree);
20553 }
20554 }
20555 }
20556 }
20557 } while (0);
20558 if (bgotonextstatement)
20559 {
20560 bool bgotonextstatement = true;
20561 do
20562 {
20563 evalcond[0] =
20564 ((-3.14159265358979) +
20565 (IKfmod(((3.14159265358979) + (IKabs(((-1.5707963267949) + j0)))),
20566 6.28318530717959)));
20567 evalcond[1] = new_r02;
20568 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20569 IKabs(evalcond[1]) < 0.0000050000000000)
20570 {
20571 bgotonextstatement = false;
20572 {
20573 IkReal j2array[1], cj2array[1], sj2array[1];
20574 bool j2valid[1] = { false };
20575 _nj2 = 1;
20576 if (IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH &&
20577 IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH &&
20578 IKabs(IKsqr(new_r00) + IKsqr(new_r01) - 1) <=
20579 IKFAST_SINCOS_THRESH)
20580 continue;
20581 j2array[0] = IKatan2(new_r00, new_r01);
20582 sj2array[0] = IKsin(j2array[0]);
20583 cj2array[0] = IKcos(j2array[0]);
20584 if (j2array[0] > IKPI)
20585 {
20586 j2array[0] -= IK2PI;
20587 }
20588 else if (j2array[0] < -IKPI)
20589 {
20590 j2array[0] += IK2PI;
20591 }
20592 j2valid[0] = true;
20593 for (int ij2 = 0; ij2 < 1; ++ij2)
20594 {
20595 if (!j2valid[ij2])
20596 {
20597 continue;
20598 }
20599 _ij2[0] = ij2;
20600 _ij2[1] = -1;
20601 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20602 {
20603 if (j2valid[iij2] &&
20604 IKabs(cj2array[ij2] - cj2array[iij2]) <
20605 IKFAST_SOLUTION_THRESH &&
20606 IKabs(sj2array[ij2] - sj2array[iij2]) <
20607 IKFAST_SOLUTION_THRESH)
20608 {
20609 j2valid[iij2] = false;
20610 _ij2[1] = iij2;
20611 break;
20612 }
20613 }
20614 j2 = j2array[ij2];
20615 cj2 = cj2array[ij2];
20616 sj2 = sj2array[ij2];
20617 {
20618 IkReal evalcond[8];
20619 IkReal x961 = IKsin(j2);
20620 IkReal x962 = IKcos(j2);
20621 IkReal x963 = ((1.0) * sj1);
20622 IkReal x964 = ((1.0) * x961);
20623 evalcond[0] = (((sj1 * x961)) + new_r21);
20624 evalcond[1] = ((((-1.0) * new_r00)) + x961);
20625 evalcond[2] = ((((-1.0) * new_r01)) + x962);
20626 evalcond[3] = (((cj1 * x962)) + new_r10);
20627 evalcond[4] = ((((-1.0) * x962 * x963)) + new_r20);
20628 evalcond[5] = ((((-1.0) * cj1 * x964)) + new_r11);
20629 evalcond[6] =
20630 (((cj1 * new_r10)) + x962 + (((-1.0) * new_r20 * x963)));
20631 evalcond[7] = ((((-1.0) * new_r21 * x963)) + ((cj1 * new_r11)) +
20632 (((-1.0) * x964)));
20633 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20634 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20635 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20636 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20637 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20638 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20639 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20640 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20641 {
20642 continue;
20643 }
20644 }
20645
20646 {
20647 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20648 vinfos[0].jointtype = 1;
20649 vinfos[0].foffset = j0;
20650 vinfos[0].indices[0] = _ij0[0];
20651 vinfos[0].indices[1] = _ij0[1];
20652 vinfos[0].maxsolutions = _nj0;
20653 vinfos[1].jointtype = 1;
20654 vinfos[1].foffset = j1;
20655 vinfos[1].indices[0] = _ij1[0];
20656 vinfos[1].indices[1] = _ij1[1];
20657 vinfos[1].maxsolutions = _nj1;
20658 vinfos[2].jointtype = 1;
20659 vinfos[2].foffset = j2;
20660 vinfos[2].indices[0] = _ij2[0];
20661 vinfos[2].indices[1] = _ij2[1];
20662 vinfos[2].maxsolutions = _nj2;
20663 vinfos[3].jointtype = 1;
20664 vinfos[3].foffset = j3;
20665 vinfos[3].indices[0] = _ij3[0];
20666 vinfos[3].indices[1] = _ij3[1];
20667 vinfos[3].maxsolutions = _nj3;
20668 vinfos[4].jointtype = 1;
20669 vinfos[4].foffset = j4;
20670 vinfos[4].indices[0] = _ij4[0];
20671 vinfos[4].indices[1] = _ij4[1];
20672 vinfos[4].maxsolutions = _nj4;
20673 vinfos[5].jointtype = 1;
20674 vinfos[5].foffset = j5;
20675 vinfos[5].indices[0] = _ij5[0];
20676 vinfos[5].indices[1] = _ij5[1];
20677 vinfos[5].maxsolutions = _nj5;
20678 vinfos[6].jointtype = 1;
20679 vinfos[6].foffset = j6;
20680 vinfos[6].indices[0] = _ij6[0];
20681 vinfos[6].indices[1] = _ij6[1];
20682 vinfos[6].maxsolutions = _nj6;
20683 std::vector<int> vfree(0);
20684 solutions.AddSolution(vinfos, vfree);
20685 }
20686 }
20687 }
20688 }
20689 } while (0);
20690 if (bgotonextstatement)
20691 {
20692 bool bgotonextstatement = true;
20693 do
20694 {
20695 evalcond[0] =
20696 ((-3.14159265358979) +
20697 (IKfmod(((3.14159265358979) + (IKabs(((1.5707963267949) + j0)))),
20698 6.28318530717959)));
20699 evalcond[1] = new_r02;
20700 if (IKabs(evalcond[0]) < 0.0000050000000000 &&
20701 IKabs(evalcond[1]) < 0.0000050000000000)
20702 {
20703 bgotonextstatement = false;
20704 {
20705 IkReal j2array[1], cj2array[1], sj2array[1];
20706 bool j2valid[1] = { false };
20707 _nj2 = 1;
20708 if (IKabs(((-1.0) * new_r00)) < IKFAST_ATAN2_MAGTHRESH &&
20709 IKabs(((-1.0) * new_r01)) < IKFAST_ATAN2_MAGTHRESH &&
20710 IKabs(IKsqr(((-1.0) * new_r00)) + IKsqr(((-1.0) * new_r01)) -
20711 1) <= IKFAST_SINCOS_THRESH)
20712 continue;
20713 j2array[0] = IKatan2(((-1.0) * new_r00), ((-1.0) * new_r01));
20714 sj2array[0] = IKsin(j2array[0]);
20715 cj2array[0] = IKcos(j2array[0]);
20716 if (j2array[0] > IKPI)
20717 {
20718 j2array[0] -= IK2PI;
20719 }
20720 else if (j2array[0] < -IKPI)
20721 {
20722 j2array[0] += IK2PI;
20723 }
20724 j2valid[0] = true;
20725 for (int ij2 = 0; ij2 < 1; ++ij2)
20726 {
20727 if (!j2valid[ij2])
20728 {
20729 continue;
20730 }
20731 _ij2[0] = ij2;
20732 _ij2[1] = -1;
20733 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
20734 {
20735 if (j2valid[iij2] &&
20736 IKabs(cj2array[ij2] - cj2array[iij2]) <
20737 IKFAST_SOLUTION_THRESH &&
20738 IKabs(sj2array[ij2] - sj2array[iij2]) <
20739 IKFAST_SOLUTION_THRESH)
20740 {
20741 j2valid[iij2] = false;
20742 _ij2[1] = iij2;
20743 break;
20744 }
20745 }
20746 j2 = j2array[ij2];
20747 cj2 = cj2array[ij2];
20748 sj2 = sj2array[ij2];
20749 {
20750 IkReal evalcond[8];
20751 IkReal x965 = IKsin(j2);
20752 IkReal x966 = IKcos(j2);
20753 IkReal x967 = ((1.0) * new_r11);
20754 IkReal x968 = ((1.0) * sj1);
20755 IkReal x969 = ((1.0) * new_r10);
20756 IkReal x970 = ((1.0) * x965);
20757 evalcond[0] = (new_r00 + x965);
20758 evalcond[1] = (new_r01 + x966);
20759 evalcond[2] = (((sj1 * x965)) + new_r21);
20760 evalcond[3] = ((((-1.0) * x966 * x968)) + new_r20);
20761 evalcond[4] = (((cj1 * x966)) + (((-1.0) * x969)));
20762 evalcond[5] = ((((-1.0) * cj1 * x970)) + (((-1.0) * x967)));
20763 evalcond[6] = ((((-1.0) * cj1 * x969)) + x966 +
20764 (((-1.0) * new_r20 * x968)));
20765 evalcond[7] = ((((-1.0) * cj1 * x967)) +
20766 (((-1.0) * new_r21 * x968)) + (((-1.0) * x970)));
20767 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
20768 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
20769 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
20770 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
20771 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
20772 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
20773 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
20774 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
20775 {
20776 continue;
20777 }
20778 }
20779
20780 {
20781 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20782 vinfos[0].jointtype = 1;
20783 vinfos[0].foffset = j0;
20784 vinfos[0].indices[0] = _ij0[0];
20785 vinfos[0].indices[1] = _ij0[1];
20786 vinfos[0].maxsolutions = _nj0;
20787 vinfos[1].jointtype = 1;
20788 vinfos[1].foffset = j1;
20789 vinfos[1].indices[0] = _ij1[0];
20790 vinfos[1].indices[1] = _ij1[1];
20791 vinfos[1].maxsolutions = _nj1;
20792 vinfos[2].jointtype = 1;
20793 vinfos[2].foffset = j2;
20794 vinfos[2].indices[0] = _ij2[0];
20795 vinfos[2].indices[1] = _ij2[1];
20796 vinfos[2].maxsolutions = _nj2;
20797 vinfos[3].jointtype = 1;
20798 vinfos[3].foffset = j3;
20799 vinfos[3].indices[0] = _ij3[0];
20800 vinfos[3].indices[1] = _ij3[1];
20801 vinfos[3].maxsolutions = _nj3;
20802 vinfos[4].jointtype = 1;
20803 vinfos[4].foffset = j4;
20804 vinfos[4].indices[0] = _ij4[0];
20805 vinfos[4].indices[1] = _ij4[1];
20806 vinfos[4].maxsolutions = _nj4;
20807 vinfos[5].jointtype = 1;
20808 vinfos[5].foffset = j5;
20809 vinfos[5].indices[0] = _ij5[0];
20810 vinfos[5].indices[1] = _ij5[1];
20811 vinfos[5].maxsolutions = _nj5;
20812 vinfos[6].jointtype = 1;
20813 vinfos[6].foffset = j6;
20814 vinfos[6].indices[0] = _ij6[0];
20815 vinfos[6].indices[1] = _ij6[1];
20816 vinfos[6].maxsolutions = _nj6;
20817 std::vector<int> vfree(0);
20818 solutions.AddSolution(vinfos, vfree);
20819 }
20820 }
20821 }
20822 }
20823 } while (0);
20824 if (bgotonextstatement)
20825 {
20826 bool bgotonextstatement = true;
20827 do
20828 {
20829 evalcond[0] = ((IKabs(new_r20)) + (IKabs(new_r21)));
20830 if (IKabs(evalcond[0]) < 0.0000050000000000)
20831 {
20832 bgotonextstatement = false;
20833 {
20834 IkReal j2eval[1];
20835 new_r21 = 0;
20836 new_r20 = 0;
20837 new_r02 = 0;
20838 new_r12 = 0;
20839 j2eval[0] = IKabs(new_r22);
20840 if (IKabs(j2eval[0]) < 0.0000000100000000)
20841 {
20842 continue; // no branches [j2]
20843 }
20844 else
20845 {
20846 IkReal op[2 + 1], zeror[2];
20847 int numroots;
20848 op[0] = ((-1.0) * new_r22);
20849 op[1] = 0;
20850 op[2] = new_r22;
20851 polyroots2(op, zeror, numroots);
20852 IkReal j2array[2], cj2array[2], sj2array[2], tempj2array[1];
20853 int numsolutions = 0;
20854 for (int ij2 = 0; ij2 < numroots; ++ij2)
20855 {
20856 IkReal htj2 = zeror[ij2];
20857 tempj2array[0] = ((2.0) * (atan(htj2)));
20858 for (int kj2 = 0; kj2 < 1; ++kj2)
20859 {
20860 j2array[numsolutions] = tempj2array[kj2];
20861 if (j2array[numsolutions] > IKPI)
20862 {
20863 j2array[numsolutions] -= IK2PI;
20864 }
20865 else if (j2array[numsolutions] < -IKPI)
20866 {
20867 j2array[numsolutions] += IK2PI;
20868 }
20869 sj2array[numsolutions] = IKsin(j2array[numsolutions]);
20870 cj2array[numsolutions] = IKcos(j2array[numsolutions]);
20871 numsolutions++;
20872 }
20873 }
20874 bool j2valid[2] = { true, true };
20875 _nj2 = 2;
20876 for (int ij2 = 0; ij2 < numsolutions; ++ij2)
20877 {
20878 if (!j2valid[ij2])
20879 {
20880 continue;
20881 }
20882 j2 = j2array[ij2];
20883 cj2 = cj2array[ij2];
20884 sj2 = sj2array[ij2];
20885 htj2 = IKtan(j2 / 2);
20886
20887 _ij2[0] = ij2;
20888 _ij2[1] = -1;
20889 for (int iij2 = ij2 + 1; iij2 < numsolutions; ++iij2)
20890 {
20891 if (j2valid[iij2] &&
20892 IKabs(cj2array[ij2] - cj2array[iij2]) <
20893 IKFAST_SOLUTION_THRESH &&
20894 IKabs(sj2array[ij2] - sj2array[iij2]) <
20895 IKFAST_SOLUTION_THRESH)
20896 {
20897 j2valid[iij2] = false;
20898 _ij2[1] = iij2;
20899 break;
20900 }
20901 }
20902 {
20903 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
20904 vinfos[0].jointtype = 1;
20905 vinfos[0].foffset = j0;
20906 vinfos[0].indices[0] = _ij0[0];
20907 vinfos[0].indices[1] = _ij0[1];
20908 vinfos[0].maxsolutions = _nj0;
20909 vinfos[1].jointtype = 1;
20910 vinfos[1].foffset = j1;
20911 vinfos[1].indices[0] = _ij1[0];
20912 vinfos[1].indices[1] = _ij1[1];
20913 vinfos[1].maxsolutions = _nj1;
20914 vinfos[2].jointtype = 1;
20915 vinfos[2].foffset = j2;
20916 vinfos[2].indices[0] = _ij2[0];
20917 vinfos[2].indices[1] = _ij2[1];
20918 vinfos[2].maxsolutions = _nj2;
20919 vinfos[3].jointtype = 1;
20920 vinfos[3].foffset = j3;
20921 vinfos[3].indices[0] = _ij3[0];
20922 vinfos[3].indices[1] = _ij3[1];
20923 vinfos[3].maxsolutions = _nj3;
20924 vinfos[4].jointtype = 1;
20925 vinfos[4].foffset = j4;
20926 vinfos[4].indices[0] = _ij4[0];
20927 vinfos[4].indices[1] = _ij4[1];
20928 vinfos[4].maxsolutions = _nj4;
20929 vinfos[5].jointtype = 1;
20930 vinfos[5].foffset = j5;
20931 vinfos[5].indices[0] = _ij5[0];
20932 vinfos[5].indices[1] = _ij5[1];
20933 vinfos[5].maxsolutions = _nj5;
20934 vinfos[6].jointtype = 1;
20935 vinfos[6].foffset = j6;
20936 vinfos[6].indices[0] = _ij6[0];
20937 vinfos[6].indices[1] = _ij6[1];
20938 vinfos[6].maxsolutions = _nj6;
20939 std::vector<int> vfree(0);
20940 solutions.AddSolution(vinfos, vfree);
20941 }
20942 }
20943 }
20944 }
20945 }
20946 } while (0);
20947 if (bgotonextstatement)
20948 {
20949 bool bgotonextstatement = true;
20950 do
20951 {
20952 if (1)
20953 {
20954 bgotonextstatement = false;
20955 continue; // branch miss [j2]
20956 }
20957 } while (0);
20958 if (bgotonextstatement)
20959 {
20960 }
20961 }
20962 }
20963 }
20964 }
20965 }
20966 }
20967 }
20968 }
20969 }
20970 }
20971 }
20972 else
20973 {
20974 {
20975 IkReal j2array[1], cj2array[1], sj2array[1];
20976 bool j2valid[1] = { false };
20977 _nj2 = 1;
20978 CheckValue<IkReal> x972 = IKPowWithIntegerCheck(sj1, -1);
20979 if (!x972.valid)
20980 {
20981 continue;
20982 }
20983 IkReal x971 = x972.value;
20984 CheckValue<IkReal> x973 = IKPowWithIntegerCheck(cj1, -1);
20985 if (!x973.valid)
20986 {
20987 continue;
20988 }
20989 CheckValue<IkReal> x974 = IKPowWithIntegerCheck(sj0, -1);
20990 if (!x974.valid)
20991 {
20992 continue;
20993 }
20994 if (IKabs(((-1.0) * new_r21 * x971)) < IKFAST_ATAN2_MAGTHRESH &&
20995 IKabs((x971 * (x973.value) * (x974.value) *
20996 ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) <
20997 IKFAST_ATAN2_MAGTHRESH &&
20998 IKabs(IKsqr(((-1.0) * new_r21 * x971)) +
20999 IKsqr((x971 * (x973.value) * (x974.value) *
21000 ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1)))))) -
21001 1) <= IKFAST_SINCOS_THRESH)
21002 continue;
21003 j2array[0] = IKatan2(((-1.0) * new_r21 * x971),
21004 (x971 * (x973.value) * (x974.value) *
21005 ((((cj0 * new_r21)) + (((-1.0) * new_r10 * sj1))))));
21006 sj2array[0] = IKsin(j2array[0]);
21007 cj2array[0] = IKcos(j2array[0]);
21008 if (j2array[0] > IKPI)
21009 {
21010 j2array[0] -= IK2PI;
21011 }
21012 else if (j2array[0] < -IKPI)
21013 {
21014 j2array[0] += IK2PI;
21015 }
21016 j2valid[0] = true;
21017 for (int ij2 = 0; ij2 < 1; ++ij2)
21018 {
21019 if (!j2valid[ij2])
21020 {
21021 continue;
21022 }
21023 _ij2[0] = ij2;
21024 _ij2[1] = -1;
21025 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
21026 {
21027 if (j2valid[iij2] &&
21028 IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
21029 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
21030 {
21031 j2valid[iij2] = false;
21032 _ij2[1] = iij2;
21033 break;
21034 }
21035 }
21036 j2 = j2array[ij2];
21037 cj2 = cj2array[ij2];
21038 sj2 = sj2array[ij2];
21039 {
21040 IkReal evalcond[12];
21041 IkReal x975 = IKsin(j2);
21042 IkReal x976 = IKcos(j2);
21043 IkReal x977 = ((1.0) * sj0);
21044 IkReal x978 = ((1.0) * sj1);
21045 IkReal x979 = (cj0 * new_r00);
21046 IkReal x980 = (cj1 * sj0);
21047 IkReal x981 = (cj0 * new_r01);
21048 IkReal x982 = (cj1 * x976);
21049 IkReal x983 = (cj1 * x975);
21050 evalcond[0] = (((sj1 * x975)) + new_r21);
21051 evalcond[1] = ((((-1.0) * x976 * x978)) + new_r20);
21052 evalcond[2] = ((((-1.0) * new_r00 * x977)) + ((cj0 * new_r10)) + x975);
21053 evalcond[3] = ((((-1.0) * new_r01 * x977)) + ((cj0 * new_r11)) + x976);
21054 evalcond[4] = (((new_r10 * sj0)) + x982 + x979);
21055 evalcond[5] = (((x976 * x980)) + new_r10 + ((cj0 * x975)));
21056 evalcond[6] = ((((-1.0) * x983)) + ((new_r11 * sj0)) + x981);
21057 evalcond[7] = (new_r00 + ((cj0 * x982)) + (((-1.0) * x975 * x977)));
21058 evalcond[8] = ((((-1.0) * x977 * x983)) + new_r11 + ((cj0 * x976)));
21059 evalcond[9] = ((((-1.0) * x976 * x977)) + new_r01 + (((-1.0) * cj0 * x983)));
21060 evalcond[10] =
21061 ((((-1.0) * new_r20 * x978)) + ((cj1 * x979)) + ((new_r10 * x980)) + x976);
21062 evalcond[11] = (((cj1 * x981)) + (((-1.0) * x975)) + (((-1.0) * new_r21 * x978)) +
21063 ((new_r11 * x980)));
21064 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
21065 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
21066 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
21067 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
21068 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
21069 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
21070 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
21071 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
21072 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
21073 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
21074 IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
21075 IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
21076 {
21077 continue;
21078 }
21079 }
21080
21081 {
21082 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
21083 vinfos[0].jointtype = 1;
21084 vinfos[0].foffset = j0;
21085 vinfos[0].indices[0] = _ij0[0];
21086 vinfos[0].indices[1] = _ij0[1];
21087 vinfos[0].maxsolutions = _nj0;
21088 vinfos[1].jointtype = 1;
21089 vinfos[1].foffset = j1;
21090 vinfos[1].indices[0] = _ij1[0];
21091 vinfos[1].indices[1] = _ij1[1];
21092 vinfos[1].maxsolutions = _nj1;
21093 vinfos[2].jointtype = 1;
21094 vinfos[2].foffset = j2;
21095 vinfos[2].indices[0] = _ij2[0];
21096 vinfos[2].indices[1] = _ij2[1];
21097 vinfos[2].maxsolutions = _nj2;
21098 vinfos[3].jointtype = 1;
21099 vinfos[3].foffset = j3;
21100 vinfos[3].indices[0] = _ij3[0];
21101 vinfos[3].indices[1] = _ij3[1];
21102 vinfos[3].maxsolutions = _nj3;
21103 vinfos[4].jointtype = 1;
21104 vinfos[4].foffset = j4;
21105 vinfos[4].indices[0] = _ij4[0];
21106 vinfos[4].indices[1] = _ij4[1];
21107 vinfos[4].maxsolutions = _nj4;
21108 vinfos[5].jointtype = 1;
21109 vinfos[5].foffset = j5;
21110 vinfos[5].indices[0] = _ij5[0];
21111 vinfos[5].indices[1] = _ij5[1];
21112 vinfos[5].maxsolutions = _nj5;
21113 vinfos[6].jointtype = 1;
21114 vinfos[6].foffset = j6;
21115 vinfos[6].indices[0] = _ij6[0];
21116 vinfos[6].indices[1] = _ij6[1];
21117 vinfos[6].maxsolutions = _nj6;
21118 std::vector<int> vfree(0);
21119 solutions.AddSolution(vinfos, vfree);
21120 }
21121 }
21122 }
21123 }
21124 }
21125 }
21126 else
21127 {
21128 {
21129 IkReal j2array[1], cj2array[1], sj2array[1];
21130 bool j2valid[1] = { false };
21131 _nj2 = 1;
21132 CheckValue<IkReal> x985 = IKPowWithIntegerCheck(sj1, -1);
21133 if (!x985.valid)
21134 {
21135 continue;
21136 }
21137 IkReal x984 = x985.value;
21138 CheckValue<IkReal> x986 = IKPowWithIntegerCheck(cj0, -1);
21139 if (!x986.valid)
21140 {
21141 continue;
21142 }
21143 if (IKabs(((-1.0) * new_r21 * x984)) < IKFAST_ATAN2_MAGTHRESH &&
21144 IKabs((x984 * (x986.value) *
21145 (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) <
21146 IKFAST_ATAN2_MAGTHRESH &&
21147 IKabs(IKsqr(((-1.0) * new_r21 * x984)) +
21148 IKsqr((x984 * (x986.value) *
21149 (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1)))))) -
21150 1) <= IKFAST_SINCOS_THRESH)
21151 continue;
21152 j2array[0] = IKatan2(((-1.0) * new_r21 * x984),
21153 (x984 * (x986.value) *
21154 (((((-1.0) * cj1 * new_r21 * sj0)) + (((-1.0) * new_r11 * sj1))))));
21155 sj2array[0] = IKsin(j2array[0]);
21156 cj2array[0] = IKcos(j2array[0]);
21157 if (j2array[0] > IKPI)
21158 {
21159 j2array[0] -= IK2PI;
21160 }
21161 else if (j2array[0] < -IKPI)
21162 {
21163 j2array[0] += IK2PI;
21164 }
21165 j2valid[0] = true;
21166 for (int ij2 = 0; ij2 < 1; ++ij2)
21167 {
21168 if (!j2valid[ij2])
21169 {
21170 continue;
21171 }
21172 _ij2[0] = ij2;
21173 _ij2[1] = -1;
21174 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
21175 {
21176 if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
21177 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
21178 {
21179 j2valid[iij2] = false;
21180 _ij2[1] = iij2;
21181 break;
21182 }
21183 }
21184 j2 = j2array[ij2];
21185 cj2 = cj2array[ij2];
21186 sj2 = sj2array[ij2];
21187 {
21188 IkReal evalcond[12];
21189 IkReal x987 = IKsin(j2);
21190 IkReal x988 = IKcos(j2);
21191 IkReal x989 = ((1.0) * sj0);
21192 IkReal x990 = ((1.0) * sj1);
21193 IkReal x991 = (cj0 * new_r00);
21194 IkReal x992 = (cj1 * sj0);
21195 IkReal x993 = (cj0 * new_r01);
21196 IkReal x994 = (cj1 * x988);
21197 IkReal x995 = (cj1 * x987);
21198 evalcond[0] = (((sj1 * x987)) + new_r21);
21199 evalcond[1] = (new_r20 + (((-1.0) * x988 * x990)));
21200 evalcond[2] = ((((-1.0) * new_r00 * x989)) + ((cj0 * new_r10)) + x987);
21201 evalcond[3] = ((((-1.0) * new_r01 * x989)) + ((cj0 * new_r11)) + x988);
21202 evalcond[4] = (((new_r10 * sj0)) + x991 + x994);
21203 evalcond[5] = (((x988 * x992)) + new_r10 + ((cj0 * x987)));
21204 evalcond[6] = ((((-1.0) * x995)) + ((new_r11 * sj0)) + x993);
21205 evalcond[7] = (((cj0 * x994)) + (((-1.0) * x987 * x989)) + new_r00);
21206 evalcond[8] = (new_r11 + (((-1.0) * x989 * x995)) + ((cj0 * x988)));
21207 evalcond[9] = ((((-1.0) * x988 * x989)) + (((-1.0) * cj0 * x995)) + new_r01);
21208 evalcond[10] =
21209 (((cj1 * x991)) + ((new_r10 * x992)) + (((-1.0) * new_r20 * x990)) + x988);
21210 evalcond[11] = (((cj1 * x993)) + (((-1.0) * x987)) + ((new_r11 * x992)) +
21211 (((-1.0) * new_r21 * x990)));
21212 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
21213 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
21214 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
21215 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
21216 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
21217 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
21218 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
21219 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
21220 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
21221 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
21222 IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
21223 IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
21224 {
21225 continue;
21226 }
21227 }
21228
21229 {
21230 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
21231 vinfos[0].jointtype = 1;
21232 vinfos[0].foffset = j0;
21233 vinfos[0].indices[0] = _ij0[0];
21234 vinfos[0].indices[1] = _ij0[1];
21235 vinfos[0].maxsolutions = _nj0;
21236 vinfos[1].jointtype = 1;
21237 vinfos[1].foffset = j1;
21238 vinfos[1].indices[0] = _ij1[0];
21239 vinfos[1].indices[1] = _ij1[1];
21240 vinfos[1].maxsolutions = _nj1;
21241 vinfos[2].jointtype = 1;
21242 vinfos[2].foffset = j2;
21243 vinfos[2].indices[0] = _ij2[0];
21244 vinfos[2].indices[1] = _ij2[1];
21245 vinfos[2].maxsolutions = _nj2;
21246 vinfos[3].jointtype = 1;
21247 vinfos[3].foffset = j3;
21248 vinfos[3].indices[0] = _ij3[0];
21249 vinfos[3].indices[1] = _ij3[1];
21250 vinfos[3].maxsolutions = _nj3;
21251 vinfos[4].jointtype = 1;
21252 vinfos[4].foffset = j4;
21253 vinfos[4].indices[0] = _ij4[0];
21254 vinfos[4].indices[1] = _ij4[1];
21255 vinfos[4].maxsolutions = _nj4;
21256 vinfos[5].jointtype = 1;
21257 vinfos[5].foffset = j5;
21258 vinfos[5].indices[0] = _ij5[0];
21259 vinfos[5].indices[1] = _ij5[1];
21260 vinfos[5].maxsolutions = _nj5;
21261 vinfos[6].jointtype = 1;
21262 vinfos[6].foffset = j6;
21263 vinfos[6].indices[0] = _ij6[0];
21264 vinfos[6].indices[1] = _ij6[1];
21265 vinfos[6].maxsolutions = _nj6;
21266 std::vector<int> vfree(0);
21267 solutions.AddSolution(vinfos, vfree);
21268 }
21269 }
21270 }
21271 }
21272 }
21273 }
21274 else
21275 {
21276 {
21277 IkReal j2array[1], cj2array[1], sj2array[1];
21278 bool j2valid[1] = { false };
21279 _nj2 = 1;
21280 CheckValue<IkReal> x996 =
21281 IKatan2WithCheck(IkReal(((-1.0) * new_r21)), IkReal(new_r20), IKFAST_ATAN2_MAGTHRESH);
21282 if (!x996.valid)
21283 {
21284 continue;
21285 }
21286 CheckValue<IkReal> x997 = IKPowWithIntegerCheck(IKsign(sj1), -1);
21287 if (!x997.valid)
21288 {
21289 continue;
21290 }
21291 j2array[0] = ((-1.5707963267949) + (x996.value) + (((1.5707963267949) * (x997.value))));
21292 sj2array[0] = IKsin(j2array[0]);
21293 cj2array[0] = IKcos(j2array[0]);
21294 if (j2array[0] > IKPI)
21295 {
21296 j2array[0] -= IK2PI;
21297 }
21298 else if (j2array[0] < -IKPI)
21299 {
21300 j2array[0] += IK2PI;
21301 }
21302 j2valid[0] = true;
21303 for (int ij2 = 0; ij2 < 1; ++ij2)
21304 {
21305 if (!j2valid[ij2])
21306 {
21307 continue;
21308 }
21309 _ij2[0] = ij2;
21310 _ij2[1] = -1;
21311 for (int iij2 = ij2 + 1; iij2 < 1; ++iij2)
21312 {
21313 if (j2valid[iij2] && IKabs(cj2array[ij2] - cj2array[iij2]) < IKFAST_SOLUTION_THRESH &&
21314 IKabs(sj2array[ij2] - sj2array[iij2]) < IKFAST_SOLUTION_THRESH)
21315 {
21316 j2valid[iij2] = false;
21317 _ij2[1] = iij2;
21318 break;
21319 }
21320 }
21321 j2 = j2array[ij2];
21322 cj2 = cj2array[ij2];
21323 sj2 = sj2array[ij2];
21324 {
21325 IkReal evalcond[12];
21326 IkReal x998 = IKsin(j2);
21327 IkReal x999 = IKcos(j2);
21328 IkReal x1000 = ((1.0) * sj0);
21329 IkReal x1001 = ((1.0) * sj1);
21330 IkReal x1002 = (cj0 * new_r00);
21331 IkReal x1003 = (cj1 * sj0);
21332 IkReal x1004 = (cj0 * new_r01);
21333 IkReal x1005 = (cj1 * x999);
21334 IkReal x1006 = (cj1 * x998);
21335 evalcond[0] = (new_r21 + ((sj1 * x998)));
21336 evalcond[1] = ((((-1.0) * x1001 * x999)) + new_r20);
21337 evalcond[2] = ((((-1.0) * new_r00 * x1000)) + ((cj0 * new_r10)) + x998);
21338 evalcond[3] = ((((-1.0) * new_r01 * x1000)) + ((cj0 * new_r11)) + x999);
21339 evalcond[4] = (x1005 + x1002 + ((new_r10 * sj0)));
21340 evalcond[5] = (((x1003 * x999)) + ((cj0 * x998)) + new_r10);
21341 evalcond[6] = ((((-1.0) * x1006)) + x1004 + ((new_r11 * sj0)));
21342 evalcond[7] = (((cj0 * x1005)) + (((-1.0) * x1000 * x998)) + new_r00);
21343 evalcond[8] = (((cj0 * x999)) + new_r11 + (((-1.0) * x1000 * x1006)));
21344 evalcond[9] = ((((-1.0) * cj0 * x1006)) + (((-1.0) * x1000 * x999)) + new_r01);
21345 evalcond[10] =
21346 (((new_r10 * x1003)) + (((-1.0) * new_r20 * x1001)) + ((cj1 * x1002)) + x999);
21347 evalcond[11] = ((((-1.0) * x998)) + ((new_r11 * x1003)) + (((-1.0) * new_r21 * x1001)) +
21348 ((cj1 * x1004)));
21349 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH ||
21350 IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH ||
21351 IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH ||
21352 IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH ||
21353 IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH ||
21354 IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH ||
21355 IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH ||
21356 IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH ||
21357 IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH ||
21358 IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH ||
21359 IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH ||
21360 IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
21361 {
21362 continue;
21363 }
21364 }
21365
21366 {
21367 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(7);
21368 vinfos[0].jointtype = 1;
21369 vinfos[0].foffset = j0;
21370 vinfos[0].indices[0] = _ij0[0];
21371 vinfos[0].indices[1] = _ij0[1];
21372 vinfos[0].maxsolutions = _nj0;
21373 vinfos[1].jointtype = 1;
21374 vinfos[1].foffset = j1;
21375 vinfos[1].indices[0] = _ij1[0];
21376 vinfos[1].indices[1] = _ij1[1];
21377 vinfos[1].maxsolutions = _nj1;
21378 vinfos[2].jointtype = 1;
21379 vinfos[2].foffset = j2;
21380 vinfos[2].indices[0] = _ij2[0];
21381 vinfos[2].indices[1] = _ij2[1];
21382 vinfos[2].maxsolutions = _nj2;
21383 vinfos[3].jointtype = 1;
21384 vinfos[3].foffset = j3;
21385 vinfos[3].indices[0] = _ij3[0];
21386 vinfos[3].indices[1] = _ij3[1];
21387 vinfos[3].maxsolutions = _nj3;
21388 vinfos[4].jointtype = 1;
21389 vinfos[4].foffset = j4;
21390 vinfos[4].indices[0] = _ij4[0];
21391 vinfos[4].indices[1] = _ij4[1];
21392 vinfos[4].maxsolutions = _nj4;
21393 vinfos[5].jointtype = 1;
21394 vinfos[5].foffset = j5;
21395 vinfos[5].indices[0] = _ij5[0];
21396 vinfos[5].indices[1] = _ij5[1];
21397 vinfos[5].maxsolutions = _nj5;
21398 vinfos[6].jointtype = 1;
21399 vinfos[6].foffset = j6;
21400 vinfos[6].indices[0] = _ij6[0];
21401 vinfos[6].indices[1] = _ij6[1];
21402 vinfos[6].maxsolutions = _nj6;
21403 std::vector<int> vfree(0);
21404 solutions.AddSolution(vinfos, vfree);
21405 }
21406 }
21407 }
21408 }
21409 }
21410 }
21411 }
21412 }
21413 }
21414 }
21415 }
21416 }
21417 }
21418 static inline void polyroots3(IkReal rawcoeffs[3 + 1], IkReal rawroots[3], int& numroots)
21419 {
21420 using std::complex;
21421 if (rawcoeffs[0] == 0)
21422 {
21423 // solve with one reduced degree
21424 polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
21425 return;
21426 }
21427 IKFAST_ASSERT(rawcoeffs[0] != 0);
21428 const IkReal tol = 128.0 * std::numeric_limits<IkReal>::epsilon();
21429 const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
21430 complex<IkReal> coeffs[3];
21431 const int maxsteps = 110;
21432 for (int i = 0; i < 3; ++i)
21433 {
21434 coeffs[i] = complex<IkReal>(rawcoeffs[i + 1] / rawcoeffs[0]);
21435 }
21436 complex<IkReal> roots[3];
21437 IkReal err[3];
21438 roots[0] = complex<IkReal>(1, 0);
21439 roots[1] = complex<IkReal>(0.4, 0.9); // any complex number not a root of unity works
21440 err[0] = 1.0;
21441 err[1] = 1.0;
21442 for (int i = 2; i < 3; ++i)
21443 {
21444 roots[i] = roots[i - 1] * roots[1];
21445 err[i] = 1.0;
21446 }
21447 for (int step = 0; step < maxsteps; ++step)
21448 {
21449 bool changed = false;
21450 for (int i = 0; i < 3; ++i)
21451 {
21452 if (err[i] >= tol)
21453 {
21454 changed = true;
21455 // evaluate
21456 complex<IkReal> x = roots[i] + coeffs[0];
21457 for (int j = 1; j < 3; ++j)
21458 {
21459 x = roots[i] * x + coeffs[j];
21460 }
21461 for (int j = 0; j < 3; ++j)
21462 {
21463 if (i != j)
21464 {
21465 if (roots[i] != roots[j])
21466 {
21467 x /= (roots[i] - roots[j]);
21468 }
21469 }
21470 }
21471 roots[i] -= x;
21472 err[i] = abs(x);
21473 }
21474 }
21475 if (!changed)
21476 {
21477 break;
21478 }
21479 }
21480
21481 numroots = 0;
21482 bool visited[3] = { false };
21483 for (int i = 0; i < 3; ++i)
21484 {
21485 if (!visited[i])
21486 {
21487 // might be a multiple root, in which case it will have more error than the other roots
21488 // find any neighboring roots, and take the average
21489 complex<IkReal> newroot = roots[i];
21490 int n = 1;
21491 for (int j = i + 1; j < 3; ++j)
21492 {
21493 // care about error in real much more than imaginary
21494 if (abs(real(roots[i]) - real(roots[j])) < tolsqrt && abs(imag(roots[i]) - imag(roots[j])) < 0.002)
21495 {
21496 newroot += roots[j];
21497 n += 1;
21498 visited[j] = true;
21499 }
21500 }
21501 if (n > 1)
21502 {
21503 newroot /= n;
21504 }
21505 // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is
21506 // used, need to use the sqrt
21507 if (IKabs(imag(newroot)) < tolsqrt)
21508 {
21509 rawroots[numroots++] = real(newroot);
21510 }
21511 }
21512 }
21513 }
21514 static inline void polyroots2(IkReal rawcoeffs[2 + 1], IkReal rawroots[2], int& numroots)
21515 {
21516 IkReal det = rawcoeffs[1] * rawcoeffs[1] - 4 * rawcoeffs[0] * rawcoeffs[2];
21517 if (det < 0)
21518 {
21519 numroots = 0;
21520 }
21521 else if (det == 0)
21522 {
21523 rawroots[0] = -0.5 * rawcoeffs[1] / rawcoeffs[0];
21524 numroots = 1;
21525 }
21526 else
21527 {
21528 det = IKsqrt(det);
21529 rawroots[0] = (-rawcoeffs[1] + det) / (2 * rawcoeffs[0]);
21530 rawroots[1] = (-rawcoeffs[1] - det) / (2 * rawcoeffs[0]); // rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
21531 numroots = 2;
21532 }
21533 }
21534 static inline void polyroots4(IkReal rawcoeffs[4 + 1], IkReal rawroots[4], int& numroots)
21535 {
21536 using std::complex;
21537 if (rawcoeffs[0] == 0)
21538 {
21539 // solve with one reduced degree
21540 polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
21541 return;
21542 }
21543 IKFAST_ASSERT(rawcoeffs[0] != 0);
21544 const IkReal tol = 128.0 * std::numeric_limits<IkReal>::epsilon();
21545 const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
21546 complex<IkReal> coeffs[4];
21547 const int maxsteps = 110;
21548 for (int i = 0; i < 4; ++i)
21549 {
21550 coeffs[i] = complex<IkReal>(rawcoeffs[i + 1] / rawcoeffs[0]);
21551 }
21552 complex<IkReal> roots[4];
21553 IkReal err[4];
21554 roots[0] = complex<IkReal>(1, 0);
21555 roots[1] = complex<IkReal>(0.4, 0.9); // any complex number not a root of unity works
21556 err[0] = 1.0;
21557 err[1] = 1.0;
21558 for (int i = 2; i < 4; ++i)
21559 {
21560 roots[i] = roots[i - 1] * roots[1];
21561 err[i] = 1.0;
21562 }
21563 for (int step = 0; step < maxsteps; ++step)
21564 {
21565 bool changed = false;
21566 for (int i = 0; i < 4; ++i)
21567 {
21568 if (err[i] >= tol)
21569 {
21570 changed = true;
21571 // evaluate
21572 complex<IkReal> x = roots[i] + coeffs[0];
21573 for (int j = 1; j < 4; ++j)
21574 {
21575 x = roots[i] * x + coeffs[j];
21576 }
21577 for (int j = 0; j < 4; ++j)
21578 {
21579 if (i != j)
21580 {
21581 if (roots[i] != roots[j])
21582 {
21583 x /= (roots[i] - roots[j]);
21584 }
21585 }
21586 }
21587 roots[i] -= x;
21588 err[i] = abs(x);
21589 }
21590 }
21591 if (!changed)
21592 {
21593 break;
21594 }
21595 }
21596
21597 numroots = 0;
21598 bool visited[4] = { false };
21599 for (int i = 0; i < 4; ++i)
21600 {
21601 if (!visited[i])
21602 {
21603 // might be a multiple root, in which case it will have more error than the other roots
21604 // find any neighboring roots, and take the average
21605 complex<IkReal> newroot = roots[i];
21606 int n = 1;
21607 for (int j = i + 1; j < 4; ++j)
21608 {
21609 // care about error in real much more than imaginary
21610 if (abs(real(roots[i]) - real(roots[j])) < tolsqrt && abs(imag(roots[i]) - imag(roots[j])) < 0.002)
21611 {
21612 newroot += roots[j];
21613 n += 1;
21614 visited[j] = true;
21615 }
21616 }
21617 if (n > 1)
21618 {
21619 newroot /= n;
21620 }
21621 // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is
21622 // used, need to use the sqrt
21623 if (IKabs(imag(newroot)) < tolsqrt)
21624 {
21625 rawroots[numroots++] = real(newroot);
21626 }
21627 }
21628 }
21629 }
21630};
21631
21634IKFAST_API bool
21635ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions)
21636{
21637 IKSolver solver;
21638 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
21639}
21640
21641IKFAST_API bool ComputeIk2(const IkReal* eetrans,
21642 const IkReal* eerot,
21643 const IkReal* pfree,
21644 IkSolutionListBase<IkReal>& solutions,
21645 void* pOpenRAVEManip)
21646{
21647 IKSolver solver;
21648 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
21649}
21650
21651IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - iiwa7 (ad1a4bdb54d4d6b54bc346ee8087a8e6)>"; }
21652
21653IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; }
21654
21655#ifdef IKFAST_NAMESPACE
21656} // end namespace
21657#endif
21658
21659#ifndef IKFAST_NO_MAIN
21660#include <stdio.h>
21661#include <stdlib.h>
21662#ifdef IKFAST_NAMESPACE
21663using namespace IKFAST_NAMESPACE;
21664#endif
21665int main(int argc, char** argv)
21666{
21667 if (argc != 12 + GetNumFreeParameters() + 1)
21668 {
21669 printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
21670 "Returns the ik solutions given the transformation of the end effector specified by\n"
21671 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
21672 "There are %d free parameters that have to be specified.\n\n",
21673 GetNumFreeParameters());
21674 return 1;
21675 }
21676
21677 IkSolutionList<IkReal> solutions;
21678 std::vector<IkReal> vfree(GetNumFreeParameters());
21679 IkReal eerot[9], eetrans[3];
21680 eerot[0] = atof(argv[1]);
21681 eerot[1] = atof(argv[2]);
21682 eerot[2] = atof(argv[3]);
21683 eetrans[0] = atof(argv[4]);
21684 eerot[3] = atof(argv[5]);
21685 eerot[4] = atof(argv[6]);
21686 eerot[5] = atof(argv[7]);
21687 eetrans[1] = atof(argv[8]);
21688 eerot[6] = atof(argv[9]);
21689 eerot[7] = atof(argv[10]);
21690 eerot[8] = atof(argv[11]);
21691 eetrans[2] = atof(argv[12]);
21692 for (std::size_t i = 0; i < vfree.size(); ++i)
21693 vfree[i] = atof(argv[13 + i]);
21694 bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
21695
21696 if (!bSuccess)
21697 {
21698 fprintf(stderr, "Failed to get ik solution\n");
21699 return -1;
21700 }
21701
21702 printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
21703 std::vector<IkReal> solvalues(GetNumJoints());
21704 for (std::size_t i = 0; i < solutions.GetNumSolutions(); ++i)
21705 {
21706 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
21707 printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
21708 std::vector<IkReal> vsolfree(sol.GetFree().size());
21709 sol.GetSolution(&solvalues[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
21710 for (std::size_t j = 0; j < solvalues.size(); ++j)
21711 printf("%.15f, ", solvalues[j]);
21712 printf("\n");
21713 }
21714 return 0;
21715}
21716
21717#endif
21718TESSERACT_COMMON_IGNORE_WARNINGS_POP
21719// clang-format on
IKPI
#define IKPI
Definition: abb_irb2400_ikfast_solver.hpp:79
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: abb_irb2400_ikfast_solver.hpp:142
IKasin
float IKasin(float f)
Definition: abb_irb2400_ikfast_solver.hpp:150
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: abb_irb2400_ikfast_solver.hpp:380
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: abb_irb2400_ikfast_solver.hpp:381
IKPI_2
#define IKPI_2
Definition: abb_irb2400_ikfast_solver.hpp:80
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: abb_irb2400_ikfast_solver.hpp:3221
IKatan2
float IKatan2(float fy, float fx)
Definition: abb_irb2400_ikfast_solver.hpp:229
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: abb_irb2400_ikfast_solver.hpp:137
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: abb_irb2400_ikfast_solver.hpp:383
IKacos
float IKacos(float f)
Definition: abb_irb2400_ikfast_solver.hpp:191
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: abb_irb2400_ikfast_solver.hpp:3216
IKsqrt
float IKsqrt(float f)
Definition: abb_irb2400_ikfast_solver.hpp:217
IKsin
float IKsin(float f)
Definition: abb_irb2400_ikfast_solver.hpp:211
IKsign
float IKsign(float f)
Definition: abb_irb2400_ikfast_solver.hpp:256
IKfmod
float IKfmod(float x, float y)
Definition: abb_irb2400_ikfast_solver.hpp:172
IKcos
float IKcos(float f)
Definition: abb_irb2400_ikfast_solver.hpp:213
IKsqr
float IKsqr(float f)
Definition: abb_irb2400_ikfast_solver.hpp:129
GetIkType
IKFAST_API int GetIkType()
Definition: abb_irb2400_ikfast_solver.hpp:385
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: abb_irb2400_ikfast_solver.hpp:284
IKtan
float IKtan(float f)
Definition: abb_irb2400_ikfast_solver.hpp:215
IK2PI
#define IK2PI
Definition: abb_irb2400_ikfast_solver.hpp:78
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: abb_irb2400_ikfast_solver.hpp:3210
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: abb_irb2400_ikfast_solver.hpp:147
IKlog
float IKlog(float f)
Definition: abb_irb2400_ikfast_solver.hpp:132
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: abb_irb2400_ikfast_solver.hpp:379
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: abb_irb2400_ikfast_solver.hpp:59
IKabs
float IKabs(float f)
Definition: abb_irb2400_ikfast_solver.hpp:126
IKSolver
Definition: abb_irb2400_ikfast_solver.hpp:388
IKSolver::rotationfunction0
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
Definition: iiwa7_ikfast_solver.hpp:3219
IKSolver::polyroots2
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
Definition: iiwa7_ikfast_solver.hpp:21514
IKSolver::polyroots3
static void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int &numroots)
Definition: iiwa7_ikfast_solver.hpp:21418
IKSolver::polyroots4
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
Definition: iiwa7_ikfast_solver.hpp:21534
IKSolver::j0mul
IkReal j0mul
Definition: iiwa7_ikfast_solver.hpp:490
IKSolver::cj6
IkReal cj6
Definition: iiwa7_ikfast_solver.hpp:491
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: iiwa7_ikfast_solver.hpp:498
IKSolver::cj100
IkReal cj100
Definition: iiwa7_ikfast_solver.hpp:496
ikfast::IkSingleDOFSolutionBase
holds the solution for a single dof
Definition: ikfast.h:52
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:72
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...
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:103
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
ikfast::IkSolutionListBase::AddSolution
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:259
ikfast::IkSolutionList::GetSolution
const IkSolutionBase< T > & GetSolution(size_t index) const override
returns the solution pointer
Definition: ikfast.h:268
ikfast::IkSolutionList::GetNumSolutions
size_t GetNumSolutions() const override
returns the number of solutions stored
Definition: ikfast.h:279
tesseract_geometry::PolygonMesh
Definition: polygon_mesh.h:46
main
int main(int argc, char **argv)
Definition: create_convex_hull.cpp:43
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *pOpenRAVEManip)
Definition: iiwa7_ikfast_solver.hpp:21641
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: iiwa7_ikfast_solver.hpp:33
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)
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition: iiwa7_ikfast_solver.hpp:240
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: iiwa7_ikfast_solver.hpp:481
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition: iiwa7_ikfast_solver.hpp:158
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T epsilon)
Definition: iiwa7_ikfast_solver.hpp:277
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: iiwa7_ikfast_solver.hpp:21635
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition: iiwa7_ikfast_solver.hpp:320
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)
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: iiwa7_ikfast_solver.hpp:475
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:45
m
tesseract_kinematics::Manipulability m
Definition: kinematics_core_unit.cpp:179
macros.h
Common Tesseract Macros.
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
TESSERACT_COMMON_IGNORE_WARNINGS_POP
Definition: create_convex_hull.cpp:36
ikfast
Definition: ikfast.h:48
CheckValue
Definition: iiwa7_ikfast_solver.hpp:271
CheckValue::valid
bool valid
Definition: iiwa7_ikfast_solver.hpp:273
CheckValue::value
T value
Definition: iiwa7_ikfast_solver.hpp:272
T
tesseract_geometry::PolygonMesh T
Definition: tesseract_geometry_unit.cpp:140
j
JointDynamics j
Definition: tesseract_scene_graph_joint_unit.cpp:15
info
KinematicsInformation info
Definition: tesseract_srdf_unit.cpp:1765
Generated on Thu Mar 30 2023 23:19:52 for Tesseract by doxygen 1.9.5