Debian Codegraph @

unittests/integration/SharedLibraryWamIkFast.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include "SharedLibraryWamIkFast.hpp"
34
35 #include "dart/external/ikfast/ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
36 using namespace ikfast;
37
38 // check if the included ikfast version matches what this file was compiled with
39 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
40 IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 71);
41
42 #include <algorithm>
43 #include <cmath>
44 #include <complex>
45 #include <limits>
46 #include <vector>
47
48 #define IKFAST_STRINGIZE2(s) #s
49 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
50
51 #ifndef IKFAST_ASSERT
52 #include <iostream>
53 #include <sstream>
54 #include <stdexcept>
55
56 #ifdef _MSC_VER
57 #ifndef __PRETTY_FUNCTION__
58 #define __PRETTY_FUNCTION__ __FUNCDNAME__
59 #endif
60 #endif
61
62 #ifndef __PRETTY_FUNCTION__
63 #define __PRETTY_FUNCTION__ __func__
64 #endif
65
66 #define IKFAST_ASSERT(b) \
67 { \
68 if (!(b)) \
69 { \
70 std::stringstream ss; \
71 ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " \
72 << __PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; \
73 throw std::runtime_error(ss.str()); \
74 } \
75 }
76
77 #endif
78
79 #if defined(_MSC_VER)
80 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
81 #else
82 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
83 #endif
84
85 #define IK2PI ((IkReal)6.28318530717959)
86 #define IKPI ((IkReal)3.14159265358979)
87 #define IKPI_2 ((IkReal)1.57079632679490)
88
89 // lapack routines
90 extern "C" {
91 void dgetrf_(
92 const int* m,
93 const int* n,
94 double* a,
95 const int* lda,
96 int* ipiv,
97 int* info);
98 void zgetrf_(
99 const int* m,
100 const int* n,
101 std::complex<double>* a,
102 const int* lda,
103 int* ipiv,
104 int* info);
105 void dgetri_(
106 const int* n,
107 const double* a,
108 const int* lda,
109 int* ipiv,
110 double* work,
111 const int* lwork,
112 int* info);
113 void dgesv_(
114 const int* n,
115 const int* nrhs,
116 double* a,
117 const int* lda,
118 int* ipiv,
119 double* b,
120 const int* ldb,
121 int* info);
122 void dgetrs_(
123 const char* trans,
124 const int* n,
125 const int* nrhs,
126 double* a,
127 const int* lda,
128 int* ipiv,
129 double* b,
130 const int* ldb,
131 int* info);
132 void dgeev_(
133 const char* jobvl,
134 const char* jobvr,
135 const int* n,
136 double* a,
137 const int* lda,
138 double* wr,
139 double* wi,
140 double* vl,
141 const int* ldvl,
142 double* vr,
143 const int* ldvr,
144 double* work,
145 const int* lwork,
146 int* info);
147 }
148
149 using namespace std; // necessary to get std math routines
150
151 #ifdef IKFAST_NAMESPACE
152 namespace IKFAST_NAMESPACE {
153 #endif
154
155 inline float IKabs(float f)
156 {
157 return fabsf(f);
158 }
159 inline double IKabs(double f)
160 {
161 return fabs(f);
162 }
163
164 inline float IKsqr(float f)
165 {
166 return f * f;
167 }
168 inline double IKsqr(double f)
169 {
170 return f * f;
171 }
172
173 inline float IKlog(float f)
174 {
175 return logf(f);
176 }
177 inline double IKlog(double f)
178 {
179 return log(f);
180 }
181
182 // allows asin and acos to exceed 1
183 #ifndef IKFAST_SINCOS_THRESH
184 #define IKFAST_SINCOS_THRESH ((IkReal)2e-6)
185 #endif
186
187 // used to check input to atan2 for degenerate cases
188 #ifndef IKFAST_ATAN2_MAGTHRESH
189 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
190 #endif
191
192 // minimum distance of separate solutions
193 #ifndef IKFAST_SOLUTION_THRESH
194 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
195 #endif
196
197 // there are checkpoints in ikfast that are evaluated to make sure they are 0.
198 // This threshold speicfies by how much they can deviate
199 #ifndef IKFAST_EVALCOND_THRESH
200 #define IKFAST_EVALCOND_THRESH ((IkReal)0.000005)
201 #endif
202
203 inline float IKasin(float f)
204 {
205 IKFAST_ASSERT(
206 f > -1 - IKFAST_SINCOS_THRESH
207 && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
208 // wrong with the solver
209 if (f <= -1)
210 return float(-IKPI_2);
211 else if (f >= 1)
212 return float(IKPI_2);
213 return asinf(f);
214 }
215 inline double IKasin(double f)
216 {
217 IKFAST_ASSERT(
218 f > -1 - IKFAST_SINCOS_THRESH
219 && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
220 // wrong with the solver
221 if (f <= -1)
222 return -IKPI_2;
223 else if (f >= 1)
224 return IKPI_2;
225 return asin(f);
226 }
227
228 // return positive value in [0,y)
229 inline float IKfmod(float x, float y)
230 {
231 while (x < 0)
232 {
233 x += y;
234 }
235 return fmodf(x, y);
236 }
237
238 // return positive value in [0,y)
239 inline double IKfmod(double x, double y)
240 {
241 while (x < 0)
242 {
243 x += y;
244 }
245 return fmod(x, y);
246 }
247
248 inline float IKacos(float f)
249 {
250 IKFAST_ASSERT(
251 f > -1 - IKFAST_SINCOS_THRESH
252 && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
253 // wrong with the solver
254 if (f <= -1)
255 return float(IKPI);
256 else if (f >= 1)
257 return float(0);
258 return acosf(f);
259 }
260 inline double IKacos(double f)
261 {
262 IKFAST_ASSERT(
263 f > -1 - IKFAST_SINCOS_THRESH
264 && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
265 // wrong with the solver
266 if (f <= -1)
267 return IKPI;
268 else if (f >= 1)
269 return 0;
270 return acos(f);
271 }
272 inline float IKsin(float f)
273 {
274 return sinf(f);
275 }
276 inline double IKsin(double f)
277 {
278 return sin(f);
279 }
280 inline float IKcos(float f)
281 {
282 return cosf(f);
283 }
284 inline double IKcos(double f)
285 {
286 return cos(f);
287 }
288 inline float IKtan(float f)
289 {
290 return tanf(f);
291 }
292 inline double IKtan(double f)
293 {
294 return tan(f);
295 }
296 inline float IKsqrt(float f)
297 {
298 if (f <= 0.0f)
299 return 0.0f;
300 return sqrtf(f);
301 }
302 inline double IKsqrt(double f)
303 {
304 if (f <= 0.0)
305 return 0.0;
306 return sqrt(f);
307 }
308 inline float IKatan2Simple(float fy, float fx)
309 {
310 return atan2f(fy, fx);
311 }
312 inline float IKatan2(float fy, float fx)
313 {
314 if (isnan(fy))
315 {
316 IKFAST_ASSERT(
317 !isnan(fx)); // if both are nan, probably wrong value will be returned
318 return float(IKPI_2);
319 }
320 else if (isnan(fx))
321 {
322 return 0;
323 }
324 return atan2f(fy, fx);
325 }
326 inline double IKatan2Simple(double fy, double fx)
327 {
328 return atan2(fy, fx);
329 }
330 inline double IKatan2(double fy, double fx)
331 {
332 if (std::isnan(fy))
333 {
334 IKFAST_ASSERT(!std::isnan(
335 fx)); // if both are nan, probably wrong value will be returned
336 return IKPI_2;
337 }
338 else if (std::isnan(fx))
339 {
340 return 0;
341 }
342 return atan2(fy, fx);
343 }
344
345 template <typename T>
346 struct CheckValue
347 {
348 T value;
349 bool valid;
350 };
351
352 template <typename T>
353 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
354 {
355 CheckValue<T> ret;
356 ret.valid = false;
357 ret.value = 0;
358 if (!std::isnan(fy) && !std::isnan(fx))
359 {
360 if (IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH
361 || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH)
362 {
363 ret.value = IKatan2Simple(fy, fx);
364 ret.valid = true;
365 }
366 }
367 return ret;
368 }
369
370 inline float IKsign(float f)
371 {
372 if (f > 0)
373 {
374 return float(1);
375 }
376 else if (f < 0)
377 {
378 return float(-1);
379 }
380 return 0;
381 }
382
383 inline double IKsign(double f)
384 {
385 if (f > 0)
386 {
387 return 1.0;
388 }
389 else if (f < 0)
390 {
391 return -1.0;
392 }
393 return 0;
394 }
395
396 template <typename T>
397 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
398 {
399 CheckValue<T> ret;
400 ret.valid = true;
401 if (n == 0)
402 {
403 ret.value = 1.0;
404 return ret;
405 }
406 else if (n == 1)
407 {
408 ret.value = f;
409 return ret;
410 }
411 else if (n < 0)
412 {
413 if (f == 0)
414 {
415 ret.valid = false;
416 ret.value = (T)1.0e30;
417 return ret;
418 }
419 if (n == -1)
420 {
421 ret.value = T(1.0) / f;
422 return ret;
423 }
424 }
425
426 int num = n > 0 ? n : -n;
427 if (num == 2)
428 {
429 ret.value = f * f;
430 }
431 else if (num == 3)
432 {
433 ret.value = f * f * f;
434 }
435 else
436 {
437 ret.value = 1.0;
438 while (num > 0)
439 {
440 if (num & 1)
441 {
442 ret.value *= f;
443 }
444 num >>= 1;
445 f *= f;
446 }
447 }
448
449 if (n < 0)
450 {
451 ret.value = T(1.0) / ret.value;
452 }
453 return ret;
454 }
455
456 /// solves the forward kinematics equations.
457 /// \param pfree is an array specifying the free joints of the chain.
458 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
459 {
460 IkReal x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13, x14, x15,
461 x16, x17, x18, x19, x20, x21, x22, x23, x24, x25, x26, x27, x28, x29, x30,
462 x31, x32, x33, x34, x35, x36, x37, x38, x39, x40, x41, x42, x43, x44, x45,
463 x46, x47, x48, x49, x50, x51, x52, x53, x54, x55, x56, x57, x58, x59, x60,
464 x61, x62, x63;
465 x0 = IKsin(j[6]);
466 x1 = IKcos(j[4]);
467 x2 = IKsin(j[0]);
468 x3 = IKcos(j[2]);
469 x4 = (x2 * x3);
470 x5 = ((1.0) * x4);
471 x6 = IKcos(j[1]);
472 x7 = IKsin(j[2]);
473 x8 = IKcos(j[0]);
474 x9 = (x7 * x8);
475 x10 = ((1.0) * x9);
476 x11 = ((((-1.0) * (1.0) * x5)) + (((-1.0) * (1.0) * x10 * x6)));
477 x12 = IKsin(j[4]);
478 x13 = IKsin(j[1]);
479 x14 = IKsin(j[3]);
480 x15 = (x13 * x14 * x8);
481 x16 = ((1.0) * x15);
482 x17 = IKcos(j[3]);
483 x18 = ((1.0) * x17);
484 x19 = (x2 * x7);
485 x20 = ((1.0) * x19);
486 x21 = (x3 * x8);
487 x22 = (x21 * x6);
488 x23 = (x22 + (((-1.0) * (1.0) * x20)));
489 x24 = (((x12 * (((((-1.0) * (1.0) * x18 * x23)) + x16)))) + ((x1 * x11)));
490 x25 = IKcos(j[6]);
491 x26 = IKsin(j[5]);
492 x27 = (x13 * x17 * x8);
493 x28 = ((1.0) * x27);
494 x29 = (x14 * ((x20 + (((-1.0) * (1.0) * x22)))));
495 x30 = (x26 * ((x29 + (((-1.0) * (1.0) * x28)))));
496 x31 = IKcos(j[5]);
497 x32 = (((x11 * x12)) + ((x1 * (((((-1.0) * (1.0) * x16)) + ((x17 * x23)))))));
498 x33 = (x31 * x32);
499 x34 = ((0.045) * x19);
500 x35 = ((0.55) * x13);
501 x36 = ((0.045) * x22);
502 x37 = ((((-1.0) * (1.0) * x20 * x6)) + x21);
503 x38 = (x13 * x14 * x2);
504 x39 = ((1.0) * x38);
505 x40 = (x4 * x6);
506 x41 = (x9 + x40);
507 x42 = (((x12 * ((x39 + (((-1.0) * (1.0) * x18 * x41)))))) + ((x1 * x37)));
508 x43 = (x13 * x17 * x2);
509 x44 = ((1.0) * x43);
510 x45 = (x14 * (((((-1.0) * (1.0) * x10)) + (((-1.0) * (1.0) * x5 * x6)))));
511 x46 = (x26 * (((((-1.0) * (1.0) * x44)) + x45)));
512 x47 = (((x1 * (((((-1.0) * (1.0) * x39)) + ((x17 * x41)))))) + ((x12 * x37)));
513 x48 = (x31 * x47);
514 x49 = ((0.045) * x9);
515 x50 = ((0.045) * x40);
516 x51 = (x13 * x7);
517 x52 = (x14 * x6);
518 x53 = ((1.0) * x52);
519 x54 = (x13 * x3);
520 x55 = (x18 * x54);
521 x56 = (((x1 * x51)) + ((x12 * ((x55 + x53)))));
522 x57 = (x17 * x6);
523 x58 = ((1.0) * x57);
524 x59 = (x14 * x54);
525 x60 = (x26 * (((((-1.0) * (1.0) * x58)) + x59)));
526 x61
527 = (((x1 * (((((-1.0) * (1.0) * x55)) + (((-1.0) * (1.0) * x53))))))
528 + ((x12 * x51)));
529 x62 = (x31 * x61);
530 x63 = ((0.045) * x54);
531 eerot[0] = (((x0 * x24)) + ((x25 * ((x30 + x33)))));
532 eerot[1]
533 = (((x0 * (((((-1.0) * (1.0) * x30)) + (((-1.0) * (1.0) * x33))))))
534 + ((x24 * x25)));
535 eerot[2] = (((x26 * x32)) + ((x31 * ((x28 + (((-1.0) * (1.0) * x29)))))));
536 eetrans[0]
537 = ((0.22) + ((x17 * ((x34 + (((-1.0) * (1.0) * x36)))))) + x36
538 + (((0.045) * x15))
539 + ((x14 * (((((0.3) * x22)) + (((-1.0) * (0.3) * x19))))))
540 + (((0.3) * x27)) + (((-1.0) * (1.0) * x34)) + ((x35 * x8)));
541 eerot[3] = (((x25 * ((x48 + x46)))) + ((x0 * x42)));
542 eerot[4]
543 = (((x0 * (((((-1.0) * (1.0) * x48)) + (((-1.0) * (1.0) * x46))))))
544 + ((x25 * x42)));
545 eerot[5] = (((x26 * x47)) + ((x31 * (((((-1.0) * (1.0) * x45)) + x44)))));
546 eetrans[1]
547 = ((0.14) + ((x2 * x35)) + (((0.3) * x43)) + (((0.045) * x38))
548 + ((x17 * (((((-1.0) * (1.0) * x49)) + (((-1.0) * (1.0) * x50))))))
549 + x49 + x50 + ((x14 * (((((0.3) * x40)) + (((0.3) * x9)))))));
550 eerot[6] = (((x0 * x56)) + ((x25 * ((x60 + x62)))));
551 eerot[7]
552 = (((x0 * (((((-1.0) * (1.0) * x60)) + (((-1.0) * (1.0) * x62))))))
553 + ((x25 * x56)));
554 eerot[8] = (((x26 * x61)) + ((x31 * (((((-1.0) * (1.0) * x59)) + x58)))));
555 eetrans[2]
556 = ((0.346) + (((0.045) * x52)) + (((-1.0) * (0.3) * x59))
557 + (((0.55) * x6)) + (((0.3) * x57)) + (((-1.0) * (1.0) * x63))
558 + ((x17 * x63)));
559 }
560
561 IKFAST_API int GetNumFreeParameters()
562 {
563 return 1;
564 }
565 IKFAST_API int* GetFreeParameters()
566 {
567 static int freeparams[] = {2};
568 return freeparams;
569 }
570 IKFAST_API int GetNumJoints()
571 {
572 return 7;
573 }
574
575 IKFAST_API int GetIkRealSize()
576 {
577 return sizeof(IkReal);
578 }
579
580 IKFAST_API int GetIkType()
581 {
582 return 0x67000001;
583 }
584
585 class IKSolver
586 {
587 public:
588 IkReal j4, cj4, sj4, htj4, j4mul, j6, cj6, sj6, htj6, j6mul, j9, cj9, sj9,
589 htj9, j9mul, j10, cj10, sj10, htj10, j10mul, j11, cj11, sj11, htj11,
590 j11mul, j12, cj12, sj12, htj12, j12mul, j8, cj8, sj8, htj8, new_r00, r00,
591 rxp0_0, new_r01, r01, rxp0_1, new_r02, r02, rxp0_2, new_r10, r10, rxp1_0,
592 new_r11, r11, rxp1_1, new_r12, r12, rxp1_2, new_r20, r20, rxp2_0, new_r21,
593 r21, rxp2_1, new_r22, r22, rxp2_2, new_px, px, npx, new_py, py, npy,
594 new_pz, pz, npz, pp;
595 unsigned char _ij4[2], _nj4, _ij6[2], _nj6, _ij9[2], _nj9, _ij10[2], _nj10,
596 _ij11[2], _nj11, _ij12[2], _nj12, _ij8[2], _nj8;
597
598 IkReal j100, cj100, sj100;
599 unsigned char _ij100[2], _nj100;
600 bool ComputeIk(
601 const IkReal* eetrans,
602 const IkReal* eerot,
603 const IkReal* pfree,
604 IkSolutionListBase<IkReal>& solutions)
605 {
606 j4 = numeric_limits<IkReal>::quiet_NaN();
607 _ij4[0] = -1;
608 _ij4[1] = -1;
609 _nj4 = -1;
610 j6 = numeric_limits<IkReal>::quiet_NaN();
611 _ij6[0] = -1;
612 _ij6[1] = -1;
613 _nj6 = -1;
614 j9 = numeric_limits<IkReal>::quiet_NaN();
615 _ij9[0] = -1;
616 _ij9[1] = -1;
617 _nj9 = -1;
618 j10 = numeric_limits<IkReal>::quiet_NaN();
619 _ij10[0] = -1;
620 _ij10[1] = -1;
621 _nj10 = -1;
622 j11 = numeric_limits<IkReal>::quiet_NaN();
623 _ij11[0] = -1;
624 _ij11[1] = -1;
625 _nj11 = -1;
626 j12 = numeric_limits<IkReal>::quiet_NaN();
627 _ij12[0] = -1;
628 _ij12[1] = -1;
629 _nj12 = -1;
630 _ij8[0] = -1;
631 _ij8[1] = -1;
632 _nj8 = 0;
633 for (int dummyiter = 0; dummyiter < 1; ++dummyiter)
634 {
635 solutions.Clear();
636 j8 = pfree[0];
637 cj8 = cos(pfree[0]);
638 sj8 = sin(pfree[0]), htj8 = tan(pfree[0] * 0.5);
639 r00 = eerot[0 * 3 + 0];
640 r01 = eerot[0 * 3 + 1];
641 r02 = eerot[0 * 3 + 2];
642 r10 = eerot[1 * 3 + 0];
643 r11 = eerot[1 * 3 + 1];
644 r12 = eerot[1 * 3 + 2];
645 r20 = eerot[2 * 3 + 0];
646 r21 = eerot[2 * 3 + 1];
647 r22 = eerot[2 * 3 + 2];
648 px = eetrans[0];
649 py = eetrans[1];
650 pz = eetrans[2];
651
652 new_r00 = r00;
653 new_r01 = r01;
654 new_r02 = r02;
655 new_px = ((-0.22) + px);
656 new_r10 = r10;
657 new_r11 = r11;
658 new_r12 = r12;
659 new_py = ((-0.14) + py);
660 new_r20 = r20;
661 new_r21 = r21;
662 new_r22 = r22;
663 new_pz = ((-0.346) + pz);
664 r00 = new_r00;
665 r01 = new_r01;
666 r02 = new_r02;
667 r10 = new_r10;
668 r11 = new_r11;
669 r12 = new_r12;
670 r20 = new_r20;
671 r21 = new_r21;
672 r22 = new_r22;
673 px = new_px;
674 py = new_py;
675 pz = new_pz;
676 IkReal x64 = ((1.0) * py);
677 IkReal x65 = ((1.0) * pz);
678 IkReal x66 = ((1.0) * px);
679 pp = ((pz * pz) + (py * py) + (px * px));
680 npx = (((pz * r20)) + ((py * r10)) + ((px * r00)));
681 npy = (((pz * r21)) + ((py * r11)) + ((px * r01)));
682 npz = (((px * r02)) + ((pz * r22)) + ((py * r12)));
683 rxp0_0 = (((pz * r10)) + (((-1.0) * r20 * x64)));
684 rxp0_1 = (((px * r20)) + (((-1.0) * r00 * x65)));
685 rxp0_2 = (((py * r00)) + (((-1.0) * r10 * x66)));
686 rxp1_0 = ((((-1.0) * r21 * x64)) + ((pz * r11)));
687 rxp1_1 = (((px * r21)) + (((-1.0) * r01 * x65)));
688 rxp1_2 = ((((-1.0) * r11 * x66)) + ((py * r01)));
689 rxp2_0 = ((((-1.0) * r22 * x64)) + ((pz * r12)));
690 rxp2_1 = ((((-1.0) * r02 * x65)) + ((px * r22)));
691 rxp2_2 = (((py * r02)) + (((-1.0) * r12 * x66)));
692 {
693 IkReal j9array[2], cj9array[2], sj9array[2];
694 bool j9valid[2] = {false};
695 _nj9 = 2;
696 if ((((-1.18441410190393) + (((2.9867963734811) * pp))))
697 < -1 - IKFAST_SINCOS_THRESH
698 || (((-1.18441410190393) + (((2.9867963734811) * pp))))
699 > 1 + IKFAST_SINCOS_THRESH)
700 continue;
701 IkReal x67 = IKasin(((-1.18441410190393) + (((2.9867963734811) * pp))));
702 j9array[0] = ((-1.34027003705633) + (((1.0) * x67)));
703 sj9array[0] = IKsin(j9array[0]);
704 cj9array[0] = IKcos(j9array[0]);
705 j9array[1] = ((1.80132261653346) + (((-1.0) * x67)));
706 sj9array[1] = IKsin(j9array[1]);
707 cj9array[1] = IKcos(j9array[1]);
708 if (j9array[0] > IKPI)
709 {
710 j9array[0] -= IK2PI;
711 }
712 else if (j9array[0] < -IKPI)
713 {
714 j9array[0] += IK2PI;
715 }
716 j9valid[0] = true;
717 if (j9array[1] > IKPI)
718 {
719 j9array[1] -= IK2PI;
720 }
721 else if (j9array[1] < -IKPI)
722 {
723 j9array[1] += IK2PI;
724 }
725 j9valid[1] = true;
726 for (int ij9 = 0; ij9 < 2; ++ij9)
727 {
728 if (!j9valid[ij9])
729 {
730 continue;
731 }
732 _ij9[0] = ij9;
733 _ij9[1] = -1;
734 for (int iij9 = ij9 + 1; iij9 < 2; ++iij9)
735 {
736 if (j9valid[iij9]
737 && IKabs(cj9array[ij9] - cj9array[iij9])
738 < IKFAST_SOLUTION_THRESH
739 && IKabs(sj9array[ij9] - sj9array[iij9])
740 < IKFAST_SOLUTION_THRESH)
741 {
742 j9valid[iij9] = false;
743 _ij9[1] = iij9;
744 break;
745 }
746 }
747 j9 = j9array[ij9];
748 cj9 = cj9array[ij9];
749 sj9 = sj9array[ij9];
750
751 {
752 IkReal j4eval[2];
753 j4eval[0] = ((IKabs(py)) + (IKabs(px)));
754 j4eval[1] = ((py * py) + (px * px));
755 if (IKabs(j4eval[0]) < 0.0000010000000000
756 || IKabs(j4eval[1]) < 0.0000010000000000)
757 {
758 {
759 IkReal j6eval[2];
760 IkReal x68 = cj8 * cj8;
761 IkReal x69 = sj9 * sj9;
762 IkReal x70 = ((13.3333333333333) * sj9);
763 IkReal x71 = (cj9 * x70);
764 IkReal x72 = cj9 * cj9;
765 IkReal x73 = ((3.0) * cj8);
766 j6eval[0]
767 = ((149.382716049383) + (((-2.0) * cj9 * x68))
768 + ((x68 * x72)) + (((44.4444444444444) * x68 * x69))
769 + (((44.4444444444444) * x72)) + x68 + x69
770 + (((24.4444444444444) * sj9))
771 + (((162.962962962963) * cj9)) + x71
772 + (((-1.0) * x68 * x71)) + ((x68 * x70)));
773 j6eval[1]
774 = ((((66.6666666666667)
775 * (IKabs(
776 ((-0.55) + (((-1.0) * (0.3) * cj9))
777 + (((-1.0) * (0.045) * sj9)))))))
778 + (IKabs(
779 ((((-1.0) * cj9 * x73)) + x73
780 + (((20.0) * cj8 * sj9))))));
781 if (IKabs(j6eval[0]) < 0.0000010000000000
782 || IKabs(j6eval[1]) < 0.0000010000000000)
783 {
784 {
785 IkReal j4eval[2];
786 IkReal x74 = cj8 * cj8 * cj8 * cj8;
787 IkReal x75 = py * py * py * py;
788 IkReal x76 = sj8 * sj8 * sj8 * sj8;
789 IkReal x77 = px * px;
790 IkReal x78 = py * py;
791 IkReal x79 = (x77 * x78);
792 IkReal x80 = sj8 * sj8;
793 IkReal x81 = ((2.0) * x80);
794 IkReal x82 = cj8 * cj8;
795 IkReal x83 = (px * py);
796 j4eval[0]
797 = (((x75 * x76)) + ((x74 * x75)) + ((x76 * x79))
798 + ((x74 * x79)) + ((x75 * x81 * x82))
799 + ((x77 * x78 * x81 * x82)));
800 j4eval[1]
801 = ((IKabs((((x80 * x83)) + ((x82 * x83)))))
802 + (IKabs((((x78 * x82)) + ((x78 * x80))))));
803 if (IKabs(j4eval[0]) < 0.0000010000000000
804 || IKabs(j4eval[1]) < 0.0000010000000000)
805 {
806 continue; // no branches [j4, j6]
807 }
808 else
809 {
810 {
811 IkReal j4array[2], cj4array[2], sj4array[2];
812 bool j4valid[2] = {false};
813 _nj4 = 2;
814 IkReal x84 = cj8 * cj8;
815 IkReal x85 = py * py;
816 IkReal x86 = sj8 * sj8;
817 IkReal x87 = (((x85 * x86)) + ((x84 * x85)));
818 IkReal x88 = ((1.0) * px * py);
819 IkReal x89
820 = ((((-1.0) * x86 * x88)) + (((-1.0) * x84 * x88)));
821 CheckValue<IkReal> x93 = IKatan2WithCheck(
822 IkReal(x87), x89, IKFAST_ATAN2_MAGTHRESH);
823 if (!x93.valid)
824 {
825 continue;
826 }
827 IkReal x90 = ((-1.0) * (x93.value));
828 IkReal x91 = ((0.045) * py * sj8);
829 if ((((x87 * x87) + (x89 * x89))) < -0.00001)
830 continue;
831 CheckValue<IkReal> x94 = IKPowWithIntegerCheck(
832 IKabs(IKsqrt(((x87 * x87) + (x89 * x89)))), -1);
833 if (!x94.valid)
834 {
835 continue;
836 }
837 if ((((x94.value)
838 * ((((cj9 * x91)) + (((-1.0) * x91))
839 + (((-1.0) * (0.3) * py * sj8 * sj9))))))
840 < -1 - IKFAST_SINCOS_THRESH
841 || (((x94.value)
842 * ((((cj9 * x91)) + (((-1.0) * x91))
843 + (((-1.0) * (0.3) * py * sj8 * sj9))))))
844 > 1 + IKFAST_SINCOS_THRESH)
845 continue;
846 IkReal x92 = IKasin(
847 ((x94.value)
848 * ((((cj9 * x91)) + (((-1.0) * x91))
849 + (((-1.0) * (0.3) * py * sj8 * sj9))))));
850 j4array[0] = (x90 + (((-1.0) * x92)));
851 sj4array[0] = IKsin(j4array[0]);
852 cj4array[0] = IKcos(j4array[0]);
853 j4array[1] = ((3.14159265358979) + x92 + x90);
854 sj4array[1] = IKsin(j4array[1]);
855 cj4array[1] = IKcos(j4array[1]);
856 if (j4array[0] > IKPI)
857 {
858 j4array[0] -= IK2PI;
859 }
860 else if (j4array[0] < -IKPI)
861 {
862 j4array[0] += IK2PI;
863 }
864 j4valid[0] = true;
865 if (j4array[1] > IKPI)
866 {
867 j4array[1] -= IK2PI;
868 }
869 else if (j4array[1] < -IKPI)
870 {
871 j4array[1] += IK2PI;
872 }
873 j4valid[1] = true;
874 for (int ij4 = 0; ij4 < 2; ++ij4)
875 {
876 if (!j4valid[ij4])
877 {
878 continue;
879 }
880 _ij4[0] = ij4;
881 _ij4[1] = -1;
882 for (int iij4 = ij4 + 1; iij4 < 2; ++iij4)
883 {
884 if (j4valid[iij4]
885 && IKabs(cj4array[ij4] - cj4array[iij4])
886 < IKFAST_SOLUTION_THRESH
887 && IKabs(sj4array[ij4] - sj4array[iij4])
888 < IKFAST_SOLUTION_THRESH)
889 {
890 j4valid[iij4] = false;
891 _ij4[1] = iij4;
892 break;
893 }
894 }
895 j4 = j4array[ij4];
896 cj4 = cj4array[ij4];
897 sj4 = sj4array[ij4];
898 {
899 IkReal evalcond[2];
900 IkReal x95 = ((0.045) * sj8);
901 IkReal x96 = (cj9 * x95);
902 IkReal x97 = ((0.3) * sj8 * sj9);
903 IkReal x98 = IKcos(j4);
904 IkReal x99 = cj8 * cj8;
905 IkReal x100 = (px * py);
906 IkReal x101 = sj8 * sj8;
907 IkReal x102 = IKsin(j4);
908 IkReal x103 = ((1.0) * (px * px));
909 evalcond[0]
910 = ((((-1.0) * px * x95)) + ((px * x96))
911 + ((x98
912 * ((((x100 * x101)) + ((x100 * x99))))))
913 + (((-1.0) * px * x97))
914 + ((x102
915 * (((((-1.0) * x101 * x103))
916 + (((-1.0) * x103 * x99)))))));
917 evalcond[1]
918 = (x95 + x97 + ((px * x102)) + (((-1.0) * x96))
919 + (((-1.0) * py * x98)));
920 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
921 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
922 {
923 continue;
924 }
925 }
926
927 {
928 IkReal j6eval[2];
929 IkReal x104 = (cj4 * px);
930 IkReal x105 = (cj8 * pz);
931 IkReal x106 = (py * sj4);
932 IkReal x107 = (cj4 * cj9 * px);
933 IkReal x108 = (cj4 * px * sj9);
934 IkReal x109 = (cj8 * pz * sj9);
935 IkReal x110 = (cj9 * py * sj4);
936 IkReal x111 = (py * sj4 * sj9);
937 IkReal x112 = ((0.045) * x105);
938 j6eval[0]
939 = ((((-1.0) * x111)) + (((-1.0) * x108))
940 + (((-12.2222222222222) * x106))
941 + (((-6.66666666666667) * x109))
942 + ((cj9 * x105))
943 + (((-12.2222222222222) * x104))
944 + (((-1.0) * x105))
945 + (((-6.66666666666667) * x110))
946 + (((-6.66666666666667) * x107)));
947 j6eval[1] = IKsign(
948 ((((-1.0) * x112)) + (((-0.045) * x108))
949 + (((-0.55) * x106)) + (((-0.55) * x104))
950 + ((cj9 * x112)) + (((-0.3) * x107))
951 + (((-0.045) * x111)) + (((-0.3) * x110))
952 + (((-0.3) * x109))));
953 if (IKabs(j6eval[0]) < 0.0000010000000000
954 || IKabs(j6eval[1]) < 0.0000010000000000)
955 {
956 {
957 IkReal j6eval[2];
958 IkReal x113 = (sj8 * (py * py));
959 IkReal x114 = cj4 * cj4;
960 IkReal x115
961 = (((sj8 * x114 * (px * px)))
962 + ((sj8 * (pz * pz)))
963 + (((-1.0) * x113 * x114))
964 + (((2.0) * cj4 * px * py * sj4 * sj8))
965 + x113);
966 j6eval[0] = x115;
967 j6eval[1] = IKsign(x115);
968 if (IKabs(j6eval[0]) < 0.0000010000000000
969 || IKabs(j6eval[1]) < 0.0000010000000000)
970 {
971 {
972 IkReal j6eval[2];
973 IkReal x116 = (pz * sj8);
974 IkReal x117 = (cj9 * pz * sj8);
975 IkReal x118 = (pz * sj8 * sj9);
976 IkReal x119 = (cj8 * sj8);
977 IkReal x120 = (cj4 * px * x119);
978 IkReal x121 = (py * sj4 * x119);
979 IkReal x122 = ((1.0) * cj9);
980 IkReal x123 = (cj4 * cj8 * px * sj8 * sj9);
981 IkReal x124 = (cj8 * py * sj4 * sj8 * sj9);
982 IkReal x125 = ((0.045) * x120);
983 IkReal x126 = ((0.045) * x121);
984 j6eval[0]
985 = ((((6.66666666666667) * x123))
986 + (((-1.0) * x121 * x122)) + x120
987 + x121
988 + (((-12.2222222222222) * x116))
989 + (((-1.0) * x120 * x122))
990 + (((6.66666666666667) * x124))
991 + (((-6.66666666666667) * x117))
992 + (((-1.0) * x118)));
993 j6eval[1] = IKsign((
994 (((-1.0) * cj9 * x126)) + x125 + x126
995 + (((-0.55) * x116)) + (((0.3) * x123))
996 + (((0.3) * x124)) + (((-0.045) * x118))
997 + (((-0.3) * x117))
998 + (((-1.0) * cj9 * x125))));
999 if (IKabs(j6eval[0]) < 0.0000010000000000
1000 || IKabs(j6eval[1])
1001 < 0.0000010000000000)
1002 {
1003 {
1004 IkReal evalcond[4];
1005 bool bgotonextstatement = true;
1006 do
1007 {
1008 IkReal x127
1009 = (((px * sj4))
1010 + (((-1.0) * (1.0) * cj4
1011 * py)));
1012 evalcond[0]
1013 = ((-3.14159265358979)
1014 + (IKfmod(
1015 ((3.14159265358979)
1016 + (IKabs(j8))),
1017 6.28318530717959)));
1018 evalcond[1]
1019 = ((0.39655) + (((0.0765) * sj9))
1020 + (((0.32595) * cj9))
1021 + (((-1.0) * (1.0) * pp)));
1022 evalcond[2] = x127;
1023 evalcond[3] = x127;
1024 if (IKabs(evalcond[0])
1025 < 0.0000010000000000
1026 && IKabs(evalcond[1])
1027 < 0.0000010000000000
1028 && IKabs(evalcond[2])
1029 < 0.0000010000000000
1030 && IKabs(evalcond[3])
1031 < 0.0000010000000000)
1032 {
1033 bgotonextstatement = false;
1034 {
1035 IkReal j6eval[2];
1036 sj8 = 0;
1037 cj8 = 1.0;
1038 j8 = 0;
1039 IkReal x128 = (cj9 * pz);
1040 IkReal x129 = (cj4 * px);
1041 IkReal x130 = (pp * pz);
1042 IkReal x131 = (py * sj4);
1043 IkReal x132 = (cj4 * cj9 * px);
1044 IkReal x133 = (cj4 * pp * px);
1045 IkReal x134 = (cj9 * py * sj4);
1046 IkReal x135 = (pp * py * sj4);
1047 j6eval[0]
1048 = (x128
1049 + (((5.4333061668025)
1050 * x130))
1051 + (((13.9482024812098)
1052 * x129))
1053 + (((12.2222222222222)
1054 * x134))
1055 + (((2.92556370551481)
1056 * pz))
1057 + (((12.2222222222222)
1058 * x132))
1059 + (((-36.2220411120167)
1060 * x135))
1061 + (((13.9482024812098)
1062 * x131))
1063 + (((-36.2220411120167)
1064 * x133)));
1065 j6eval[1] = IKsign((
1066 (((1.32323529411765) * x132))
1067 + (((0.316735294117647) * pz))
1068 + (((0.108264705882353)
1069 * x128))
1070 + (((-3.92156862745098)
1071 * x135))
1072 + (((1.32323529411765)
1073 * x134))
1074 + (((1.51009803921569)
1075 * x131))
1076 + (((1.51009803921569)
1077 * x129))
1078 + (((0.588235294117647)
1079 * x130))
1080 + (((-3.92156862745098)
1081 * x133))));
1082 if (IKabs(j6eval[0])
1083 < 0.0000010000000000
1084 || IKabs(j6eval[1])
1085 < 0.0000010000000000)
1086 {
1087 {
1088 IkReal j6eval[2];
1089 sj8 = 0;
1090 cj8 = 1.0;
1091 j8 = 0;
1092 IkReal x136 = (cj4 * px);
1093 IkReal x137 = (py * sj4);
1094 IkReal x138 = (cj9 * pz);
1095 IkReal x139 = (pz * sj9);
1096 IkReal x140 = ((1.0) * cj9);
1097 IkReal x141
1098 = (cj4 * px * sj9);
1099 IkReal x142
1100 = (py * sj4 * sj9);
1101 IkReal x143
1102 = ((0.045) * x136);
1103 IkReal x144
1104 = ((0.045) * x137);
1105 j6eval[0]
1106 = (x137 + x136
1107 + (((-1.0)
1108 * (12.2222222222222)
1109 * pz))
1110 + (((6.66666666666667)
1111 * x141))
1112 + (((-1.0) * x136
1113 * x140))
1114 + (((6.66666666666667)
1115 * x142))
1116 + (((-6.66666666666667)
1117 * x138))
1118 + (((-1.0) * x139))
1119 + (((-1.0) * x137
1120 * x140)));
1121 j6eval[1] = IKsign((
1122 (((-0.3) * x138))
1123 + (((-1.0) * (0.55) * pz))
1124 + (((0.3) * x142)) + x143
1125 + x144 + (((0.3) * x141))
1126 + (((-0.045) * x139))
1127 + (((-1.0) * cj9 * x143))
1128 + (((-1.0) * cj9
1129 * x144))));
1130 if (IKabs(j6eval[0])
1131 < 0.0000010000000000
1132 || IKabs(j6eval[1])
1133 < 0.0000010000000000)
1134 {
1135 {
1136 IkReal j6eval[2];
1137 sj8 = 0;
1138 cj8 = 1.0;
1139 j8 = 0;
1140 IkReal x145 = (cj4 * px);
1141 IkReal x146 = (cj9 * pz);
1142 IkReal x147 = (pp * pz);
1143 IkReal x148 = (py * sj4);
1144 IkReal x149
1145 = (cj4 * cj9 * px);
1146 IkReal x150
1147 = (cj4 * pp * px);
1148 IkReal x151
1149 = (cj9 * py * sj4);
1150 IkReal x152
1151 = (pp * py * sj4);
1152 j6eval[0]
1153 = ((((-1.0) * x151))
1154 + (((-5.4333061668025)
1155 * x150))
1156 + (((-1.0) * x149))
1157 + (((-5.4333061668025)
1158 * x152))
1159 + (((12.2222222222222)
1160 * x146))
1161 + (((13.9482024812098)
1162 * pz))
1163 + (((-2.92556370551481)
1164 * x148))
1165 + (((-2.92556370551481)
1166 * x145))
1167 + (((-36.2220411120167)
1168 * x147)));
1169 j6eval[1] = IKsign((
1170 (((-0.108264705882353)
1171 * x151))
1172 + (((1.51009803921569)
1173 * pz))
1174 + (((-0.588235294117647)
1175 * x152))
1176 + (((-0.316735294117647)
1177 * x148))
1178 + (((-0.588235294117647)
1179 * x150))
1180 + (((-0.108264705882353)
1181 * x149))
1182 + (((1.32323529411765)
1183 * x146))
1184 + (((-3.92156862745098)
1185 * x147))
1186 + (((-0.316735294117647)
1187 * x145))));
1188 if (IKabs(j6eval[0])
1189 < 0.0000010000000000
1190 || IKabs(j6eval[1])
1191 < 0.0000010000000000)
1192 {
1193 {
1194 IkReal evalcond[1];
1195 bool
1196 bgotonextstatement
1197 = true;
1198 do
1199 {
1200 evalcond[0]
1201 = ((IKabs((
1202 (-3.14159265358979)
1203 + (IKfmod(
1204 ((3.14159265358979)
1205 + j9),
1206 6.28318530717959)))))
1207 + (IKabs(
1208 pz)));
1209 if (IKabs(
1210 evalcond[0])
1211 < 0.0000010000000000)
1212 {
1213 bgotonextstatement
1214 = false;
1215 {
1216 IkReal
1217 j6eval[1];
1218 IkReal x153
1219 = ((1.0)
1220 * py);
1221 sj8 = 0;
1222 cj8 = 1.0;
1223 j8 = 0;
1224 pz = 0;
1225 j9 = 0;
1226 sj9 = 0;
1227 cj9 = 1.0;
1228 pp
1229 = ((py * py)
1230 + (px
1231 * px));
1232 npx
1233 = (((py
1234 * r10))
1235 + ((px
1236 * r00)));
1237 npy
1238 = (((py
1239 * r11))
1240 + ((px
1241 * r01)));
1242 npz
1243 = (((px
1244 * r02))
1245 + ((py
1246 * r12)));
1247 rxp0_0
1248 = ((-1.0)
1249 * r20
1250 * x153);
1251 rxp0_1
1252 = (px
1253 * r20);
1254 rxp1_0
1255 = ((-1.0)
1256 * r21
1257 * x153);
1258 rxp1_1
1259 = (px
1260 * r21);
1261 rxp2_0
1262 = ((-1.0)
1263 * r22
1264 * x153);
1265 rxp2_1
1266 = (px
1267 * r22);
1268 j6eval[0]
1269 = (((py
1270 * sj4))
1271 + ((cj4
1272 * px)));
1273 if (IKabs(
1274 j6eval
1275 [0])
1276 < 0.0000010000000000)
1277 {
1278 {
1279 IkReal
1280 j6eval
1281 [1];
1282 IkReal x154
1283 = ((1.0)
1284 * py);
1285 sj8 = 0;
1286 cj8 = 1.0;
1287 j8 = 0;
1288 pz = 0;
1289 j9 = 0;
1290 sj9 = 0;
1291 cj9 = 1.0;
1292 pp
1293 = ((py
1294 * py)
1295 + (px
1296 * px));
1297 npx
1298 = (((py
1299 * r10))
1300 + ((px
1301 * r00)));
1302 npy
1303 = (((py
1304 * r11))
1305 + ((px
1306 * r01)));
1307 npz
1308 = (((px
1309 * r02))
1310 + ((py
1311 * r12)));
1312 rxp0_0
1313 = ((-1.0)
1314 * r20
1315 * x154);
1316 rxp0_1
1317 = (px
1318 * r20);
1319 rxp1_0
1320 = ((-1.0)
1321 * r21
1322 * x154);
1323 rxp1_1
1324 = (px
1325 * r21);
1326 rxp2_0
1327 = ((-1.0)
1328 * r22
1329 * x154);
1330 rxp2_1
1331 = (px
1332 * r22);
1333 j6eval[0]
1334 = ((-1.0)
1335 + (((-1.0)
1336 * (1.3840830449827)
1337 * (px
1338 * px)))
1339 + (((-1.0)
1340 * (1.3840830449827)
1341 * (py
1342 * py))));
1343 if (IKabs(
1344 j6eval
1345 [0])
1346 < 0.0000010000000000)
1347 {
1348 {
1349 IkReal j6eval
1350 [2];
1351 IkReal
1352 x155
1353 = ((1.0)
1354 * py);
1355 sj8 = 0;
1356 cj8 = 1.0;
1357 j8 = 0;
1358 pz = 0;
1359 j9 = 0;
1360 sj9 = 0;
1361 cj9 = 1.0;
1362 pp
1363 = ((py
1364 * py)
1365 + (px
1366 * px));
1367 npx
1368 = (((py
1369 * r10))
1370 + ((px
1371 * r00)));
1372 npy
1373 = (((py
1374 * r11))
1375 + ((px
1376 * r01)));
1377 npz
1378 = (((px
1379 * r02))
1380 + ((py
1381 * r12)));
1382 rxp0_0
1383 = ((-1.0)
1384 * r20
1385 * x155);
1386 rxp0_1
1387 = (px
1388 * r20);
1389 rxp1_0
1390 = ((-1.0)
1391 * r21
1392 * x155);
1393 rxp1_1
1394 = (px
1395 * r21);
1396 rxp2_0
1397 = ((-1.0)
1398 * r22
1399 * x155);
1400 rxp2_1
1401 = (px
1402 * r22);
1403 IkReal
1404 x156
1405 = (cj4
1406 * px);
1407 IkReal
1408 x157
1409 = (py
1410 * sj4);
1411 j6eval
1412 [0]
1413 = (x156
1414 + x157);
1415 j6eval
1416 [1]
1417 = ((((-1.0)
1418 * (1.3840830449827)
1419 * cj4
1420 * (px
1421 * px
1422 * px)))
1423 + (((-1.0)
1424 * x157))
1425 + (((-1.3840830449827)
1426 * x157
1427 * (px
1428 * px)))
1429 + (((-1.3840830449827)
1430 * x156
1431 * (py
1432 * py)))
1433 + (((-1.0)
1434 * x156))
1435 + (((-1.0)
1436 * (1.3840830449827)
1437 * sj4
1438 * (py
1439 * py
1440 * py))));
1441 if (IKabs(
1442 j6eval
1443 [0])
1444 < 0.0000010000000000
1445 || IKabs(
1446 j6eval
1447 [1])
1448 < 0.0000010000000000)
1449 {
1450 {
1451 IkReal evalcond
1452 [4];
1453 bool
1454 bgotonextstatement
1455 = true;
1456 do
1457 {
1458 evalcond
1459 [0]
1460 = ((IKabs(
1461 py))
1462 + (IKabs(
1463 px)));
1464 evalcond
1465 [1]
1466 = -0.85;
1467 evalcond
1468 [2]
1469 = 0;
1470 evalcond
1471 [3]
1472 = -0.2125;
1473 if (IKabs(
1474 evalcond
1475 [0])
1476 < 0.0000010000000000
1477 && IKabs(
1478 evalcond
1479 [1])
1480 < 0.0000010000000000
1481 && IKabs(
1482 evalcond
1483 [2])
1484 < 0.0000010000000000
1485 && IKabs(
1486 evalcond
1487 [3])
1488 < 0.0000010000000000)
1489 {
1490 bgotonextstatement
1491 = false;
1492 {
1493 IkReal j6array
1494 [2],
1495 cj6array
1496 [2],
1497 sj6array
1498 [2];
1499 bool j6valid
1500 [2]
1501 = {false};
1502 _nj6
1503 = 2;
1504 j6array
1505 [0]
1506 = 2.9927027059803;
1507 sj6array
1508 [0]
1509 = IKsin(
1510 j6array
1511 [0]);
1512 cj6array
1513 [0]
1514 = IKcos(
1515 j6array
1516 [0]);
1517 j6array
1518 [1]
1519 = 6.13429535957009;
1520 sj6array
1521 [1]
1522 = IKsin(
1523 j6array
1524 [1]);
1525 cj6array
1526 [1]
1527 = IKcos(
1528 j6array
1529 [1]);
1530 if (j6array
1531 [0]
1532 > IKPI)
1533 {
1534 j6array
1535 [0]
1536 -= IK2PI;
1537 }
1538 else if (
1539 j6array
1540 [0]
1541 < -IKPI)
1542 {
1543 j6array
1544 [0]
1545 += IK2PI;
1546 }
1547 j6valid
1548 [0]
1549 = true;
1550 if (j6array
1551 [1]
1552 > IKPI)
1553 {
1554 j6array
1555 [1]
1556 -= IK2PI;
1557 }
1558 else if (
1559 j6array
1560 [1]
1561 < -IKPI)
1562 {
1563 j6array
1564 [1]
1565 += IK2PI;
1566 }
1567 j6valid
1568 [1]
1569 = true;
1570 for (
1571 int ij6
1572 = 0;
1573 ij6
1574 < 2;
1575 ++ij6)
1576 {
1577 if (!j6valid
1578 [ij6])
1579 {
1580 continue;
1581 }
1582 _ij6[0]
1583 = ij6;
1584 _ij6[1]
1585 = -1;
1586 for (
1587 int iij6
1588 = ij6
1589 + 1;
1590 iij6
1591 < 2;
1592 ++iij6)
1593 {
1594 if (j6valid
1595 [iij6]
1596 && IKabs(
1597 cj6array
1598 [ij6]
1599 - cj6array
1600 [iij6])
1601 < IKFAST_SOLUTION_THRESH
1602 && IKabs(
1603 sj6array
1604 [ij6]
1605 - sj6array
1606 [iij6])
1607 < IKFAST_SOLUTION_THRESH)
1608 {
1609 j6valid
1610 [iij6]
1611 = false;
1612 _ij6[1]
1613 = iij6;
1614 break;
1615 }
1616 }
1617 j6 = j6array
1618 [ij6];
1619 cj6 = cj6array
1620 [ij6];
1621 sj6 = sj6array
1622 [ij6];
1623 {
1624 IkReal evalcond
1625 [1];
1626 evalcond
1627 [0]
1628 = ((0.85)
1629 * (IKsin(
1630 j6)));
1631 if (IKabs(
1632 evalcond
1633 [0])
1634 > IKFAST_EVALCOND_THRESH)
1635 {
1636 continue;
1637 }
1638 }
1639
1640 rotationfunction0(
1641 solutions);
1642 }
1643 }
1644 }
1645 } while (
1646 0);
1647 if (bgotonextstatement)
1648 {
1649 bool
1650 bgotonextstatement
1651 = true;
1652 do
1653 {
1654 evalcond
1655 [0]
1656 = ((IKabs((
1657 (-3.14159265358979)
1658 + (IKfmod(
1659 ((3.14159265358979)
1660 + j4),
1661 6.28318530717959)))))
1662 + (IKabs(
1663 px)));
1664 evalcond
1665 [1]
1666 = -0.85;
1667 evalcond
1668 [2]
1669 = 0;
1670 evalcond
1671 [3]
1672 = ((-0.2125)
1673 + (((-1.0)
1674 * (1.0)
1675 * (py
1676 * py))));
1677 if (IKabs(
1678 evalcond
1679 [0])
1680 < 0.0000010000000000
1681 && IKabs(
1682 evalcond
1683 [1])
1684 < 0.0000010000000000
1685 && IKabs(
1686 evalcond
1687 [2])
1688 < 0.0000010000000000
1689 && IKabs(
1690 evalcond
1691 [3])
1692 < 0.0000010000000000)
1693 {
1694 bgotonextstatement
1695 = false;
1696 {
1697 IkReal j6eval
1698 [1];
1699 IkReal
1700 x606
1701 = ((1.0)
1702 * py);
1703 sj8 = 0;
1704 cj8 = 1.0;
1705 j8 = 0;
1706 pz = 0;
1707 j9 = 0;
1708 sj9 = 0;
1709 cj9 = 1.0;
1710 pp = py
1711 * py;
1712 npx
1713 = (py
1714 * r10);
1715 npy
1716 = (py
1717 * r11);
1718 npz
1719 = (py
1720 * r12);
1721 rxp0_0
1722 = ((-1.0)
1723 * r20
1724 * x606);
1725 rxp0_1
1726 = 0;
1727 rxp1_0
1728 = ((-1.0)
1729 * r21
1730 * x606);
1731 rxp1_1
1732 = 0;
1733 rxp2_0
1734 = ((-1.0)
1735 * r22
1736 * x606);
1737 rxp2_1
1738 = 0;
1739 px = 0;
1740 j4 = 0;
1741 sj4 = 0;
1742 cj4 = 1.0;
1743 rxp0_2
1744 = (py
1745 * r00);
1746 rxp1_2
1747 = (py
1748 * r01);
1749 rxp2_2
1750 = (py
1751 * r02);
1752 j6eval
1753 [0]
1754 = ((1.0)
1755 + (((1.91568587540858)
1756 * (py
1757 * py
1758 * py
1759 * py)))
1760 + (((-1.0)
1761 * (2.64633970947792)
1762 * (py
1763 * py))));
1764 if (IKabs(
1765 j6eval
1766 [0])
1767 < 0.0000010000000000)
1768 {
1769 continue; // no branches [j6]
1770 }
1771 else
1772 {
1773 {
1774 IkReal j6array
1775 [2],
1776 cj6array
1777 [2],
1778 sj6array
1779 [2];
1780 bool j6valid
1781 [2]
1782 = {false};
1783 _nj6
1784 = 2;
1785 IkReal
1786 x607
1787 = py
1788 * py;
1789 CheckValue<IkReal> x609 = IKatan2WithCheck(
1790 IkReal((
1791 (-0.425)
1792 + (((-0.588235294117647)
1793 * x607)))),
1794 ((-2.83333333333333)
1795 + (((3.92156862745098)
1796 * x607))),
1797 IKFAST_ATAN2_MAGTHRESH);
1798 if (!x609.valid)
1799 {
1800 continue;
1801 }
1802 IkReal
1803 x608
1804 = ((-1.0)
1805 * (x609.value));
1806 j6array
1807 [0]
1808 = x608;
1809 sj6array
1810 [0]
1811 = IKsin(
1812 j6array
1813 [0]);
1814 cj6array
1815 [0]
1816 = IKcos(
1817 j6array
1818 [0]);
1819 j6array
1820 [1]
1821 = ((3.14159265358979)
1822 + x608);
1823 sj6array
1824 [1]
1825 = IKsin(
1826 j6array
1827 [1]);
1828 cj6array
1829 [1]
1830 = IKcos(
1831 j6array
1832 [1]);
1833 if (j6array
1834 [0]
1835 > IKPI)
1836 {
1837 j6array
1838 [0]
1839 -= IK2PI;
1840 }
1841 else if (
1842 j6array
1843 [0]
1844 < -IKPI)
1845 {
1846 j6array
1847 [0]
1848 += IK2PI;
1849 }
1850 j6valid
1851 [0]
1852 = true;
1853 if (j6array
1854 [1]
1855 > IKPI)
1856 {
1857 j6array
1858 [1]
1859 -= IK2PI;
1860 }
1861 else if (
1862 j6array
1863 [1]
1864 < -IKPI)
1865 {
1866 j6array
1867 [1]
1868 += IK2PI;
1869 }
1870 j6valid
1871 [1]
1872 = true;
1873 for (
1874 int ij6
1875 = 0;
1876 ij6
1877 < 2;
1878 ++ij6)
1879 {
1880 if (!j6valid
1881 [ij6])
1882 {
1883 continue;
1884 }
1885 _ij6[0]
1886 = ij6;
1887 _ij6[1]
1888 = -1;
1889 for (
1890 int iij6
1891 = ij6
1892 + 1;
1893 iij6
1894 < 2;
1895 ++iij6)
1896 {
1897 if (j6valid
1898 [iij6]
1899 && IKabs(
1900 cj6array
1901 [ij6]
1902 - cj6array
1903 [iij6])
1904 < IKFAST_SOLUTION_THRESH
1905 && IKabs(
1906 sj6array
1907 [ij6]
1908 - sj6array
1909 [iij6])
1910 < IKFAST_SOLUTION_THRESH)
1911 {
1912 j6valid
1913 [iij6]
1914 = false;
1915 _ij6[1]
1916 = iij6;
1917 break;
1918 }
1919 }
1920 j6 = j6array
1921 [ij6];
1922 cj6 = cj6array
1923 [ij6];
1924 sj6 = sj6array
1925 [ij6];
1926 {
1927 IkReal evalcond
1928 [1];
1929 evalcond
1930 [0]
1931 = ((0.85)
1932 * (IKsin(
1933 j6)));
1934 if (IKabs(
1935 evalcond
1936 [0])
1937 > IKFAST_EVALCOND_THRESH)
1938 {
1939 continue;
1940 }
1941 }
1942
1943 rotationfunction0(
1944 solutions);
1945 }
1946 }
1947 }
1948 }
1949 }
1950 } while (
1951 0);
1952 if (bgotonextstatement)
1953 {
1954 bool
1955 bgotonextstatement
1956 = true;
1957 do
1958 {
1959 evalcond
1960 [0]
1961 = ((IKabs((
1962 (-3.14159265358979)
1963 + (IKfmod(
1964 j4,
1965 6.28318530717959)))))
1966 + (IKabs(
1967 px)));
1968 evalcond
1969 [1]
1970 = -0.85;
1971 evalcond
1972 [2]
1973 = 0;
1974 evalcond
1975 [3]
1976 = ((-0.2125)
1977 + (((-1.0)
1978 * (1.0)
1979 * (py
1980 * py))));
1981 if (IKabs(
1982 evalcond
1983 [0])
1984 < 0.0000010000000000
1985 && IKabs(
1986 evalcond
1987 [1])
1988 < 0.0000010000000000
1989 && IKabs(
1990 evalcond
1991 [2])
1992 < 0.0000010000000000
1993 && IKabs(
1994 evalcond
1995 [3])
1996 < 0.0000010000000000)
1997 {
1998 bgotonextstatement
1999 = false;
2000 {
2001 IkReal j6eval
2002 [1];
2003 IkReal
2004 x610
2005 = ((1.0)
2006 * py);
2007 sj8 = 0;
2008 cj8 = 1.0;
2009 j8 = 0;
2010 pz = 0;
2011 j9 = 0;
2012 sj9 = 0;
2013 cj9 = 1.0;
2014 pp = py
2015 * py;
2016 npx
2017 = (py
2018 * r10);
2019 npy
2020 = (py
2021 * r11);
2022 npz
2023 = (py
2024 * r12);
2025 rxp0_0
2026 = ((-1.0)
2027 * r20
2028 * x610);
2029 rxp0_1
2030 = 0;
2031 rxp1_0
2032 = ((-1.0)
2033 * r21
2034 * x610);
2035 rxp1_1
2036 = 0;
2037 rxp2_0
2038 = ((-1.0)
2039 * r22
2040 * x610);
2041 rxp2_1
2042 = 0;
2043 px = 0;
2044 j4 = 3.14159265358979;
2045 sj4 = 0;
2046 cj4 = -1.0;
2047 rxp0_2
2048 = (py
2049 * r00);
2050 rxp1_2
2051 = (py
2052 * r01);
2053 rxp2_2
2054 = (py
2055 * r02);
2056 j6eval
2057 [0]
2058 = ((1.0)
2059 + (((1.91568587540858)
2060 * (py
2061 * py
2062 * py
2063 * py)))
2064 + (((-1.0)
2065 * (2.64633970947792)
2066 * (py
2067 * py))));
2068 if (IKabs(
2069 j6eval
2070 [0])
2071 < 0.0000010000000000)
2072 {
2073 continue; // no branches [j6]
2074 }
2075 else
2076 {
2077 {
2078 IkReal j6array
2079 [2],
2080 cj6array
2081 [2],
2082 sj6array
2083 [2];
2084 bool j6valid
2085 [2]
2086 = {false};
2087 _nj6
2088 = 2;
2089 IkReal
2090 x611
2091 = py
2092 * py;
2093 CheckValue<IkReal> x613 = IKatan2WithCheck(
2094 IkReal((
2095 (-0.425)
2096 + (((-0.588235294117647)
2097 * x611)))),
2098 ((-2.83333333333333)
2099 + (((3.92156862745098)
2100 * x611))),
2101 IKFAST_ATAN2_MAGTHRESH);
2102 if (!x613.valid)
2103 {
2104 continue;
2105 }
2106 IkReal
2107 x612
2108 = ((-1.0)
2109 * (x613.value));
2110 j6array
2111 [0]
2112 = x612;
2113 sj6array
2114 [0]
2115 = IKsin(
2116 j6array
2117 [0]);
2118 cj6array
2119 [0]
2120 = IKcos(
2121 j6array
2122 [0]);
2123 j6array
2124 [1]
2125 = ((3.14159265358979)
2126 + x612);
2127 sj6array
2128 [1]
2129 = IKsin(
2130 j6array
2131 [1]);
2132 cj6array
2133 [1]
2134 = IKcos(
2135 j6array
2136 [1]);
2137 if (j6array
2138 [0]
2139 > IKPI)
2140 {
2141 j6array
2142 [0]
2143 -= IK2PI;
2144 }
2145 else if (
2146 j6array
2147 [0]
2148 < -IKPI)
2149 {
2150 j6array
2151 [0]
2152 += IK2PI;
2153 }
2154 j6valid
2155 [0]
2156 = true;
2157 if (j6array
2158 [1]
2159 > IKPI)
2160 {
2161 j6array
2162 [1]
2163 -= IK2PI;
2164 }
2165 else if (
2166 j6array
2167 [1]
2168 < -IKPI)
2169 {
2170 j6array
2171 [1]
2172 += IK2PI;
2173 }
2174 j6valid
2175 [1]
2176 = true;
2177 for (
2178 int ij6
2179 = 0;
2180 ij6
2181 < 2;
2182 ++ij6)
2183 {
2184 if (!j6valid
2185 [ij6])
2186 {
2187 continue;
2188 }
2189 _ij6[0]
2190 = ij6;
2191 _ij6[1]
2192 = -1;
2193 for (
2194 int iij6
2195 = ij6
2196 + 1;
2197 iij6
2198 < 2;
2199 ++iij6)
2200 {
2201 if (j6valid
2202 [iij6]
2203 && IKabs(
2204 cj6array
2205 [ij6]
2206 - cj6array
2207 [iij6])
2208 < IKFAST_SOLUTION_THRESH
2209 && IKabs(
2210 sj6array
2211 [ij6]
2212 - sj6array
2213 [iij6])
2214 < IKFAST_SOLUTION_THRESH)
2215 {
2216 j6valid
2217 [iij6]
2218 = false;
2219 _ij6[1]
2220 = iij6;
2221 break;
2222 }
2223 }
2224 j6 = j6array
2225 [ij6];
2226 cj6 = cj6array
2227 [ij6];
2228 sj6 = sj6array
2229 [ij6];
2230 {
2231 IkReal evalcond
2232 [1];
2233 evalcond
2234 [0]
2235 = ((0.85)
2236 * (IKsin(
2237 j6)));
2238 if (IKabs(
2239 evalcond
2240 [0])
2241 > IKFAST_EVALCOND_THRESH)
2242 {
2243 continue;
2244 }
2245 }
2246
2247 rotationfunction0(
2248 solutions);
2249 }
2250 }
2251 }
2252 }
2253 }
2254 } while (
2255 0);
2256 if (bgotonextstatement)
2257 {
2258 bool
2259 bgotonextstatement
2260 = true;
2261 do
2262 {
2263 evalcond
2264 [0]
2265 = ((IKabs((
2266 (-3.14159265358979)
2267 + (IKfmod(
2268 ((1.5707963267949)
2269 + j4),
2270 6.28318530717959)))))
2271 + (IKabs(
2272 py)));
2273 evalcond
2274 [1]
2275 = -0.85;
2276 evalcond
2277 [2]
2278 = 0;
2279 evalcond
2280 [3]
2281 = ((-0.2125)
2282 + (((-1.0)
2283 * (1.0)
2284 * (px
2285 * px))));
2286 if (IKabs(
2287 evalcond
2288 [0])
2289 < 0.0000010000000000
2290 && IKabs(
2291 evalcond
2292 [1])
2293 < 0.0000010000000000
2294 && IKabs(
2295 evalcond
2296 [2])
2297 < 0.0000010000000000
2298 && IKabs(
2299 evalcond
2300 [3])
2301 < 0.0000010000000000)
2302 {
2303 bgotonextstatement
2304 = false;
2305 {
2306 IkReal j6eval
2307 [1];
2308 IkReal
2309 x614
2310 = ((1.0)
2311 * px);
2312 sj8 = 0;
2313 cj8 = 1.0;
2314 j8 = 0;
2315 pz = 0;
2316 j9 = 0;
2317 sj9 = 0;
2318 cj9 = 1.0;
2319 pp = px
2320 * px;
2321 npx
2322 = (px
2323 * r00);
2324 npy
2325 = (px
2326 * r01);
2327 npz
2328 = (px
2329 * r02);
2330 rxp0_0
2331 = 0;
2332 rxp0_1
2333 = (px
2334 * r20);
2335 rxp1_0
2336 = 0;
2337 rxp1_1
2338 = (px
2339 * r21);
2340 rxp2_0
2341 = 0;
2342 rxp2_1
2343 = (px
2344 * r22);
2345 py = 0;
2346 j4 = 1.5707963267949;
2347 sj4 = 1.0;
2348 cj4 = 0;
2349 rxp0_2
2350 = ((-1.0)
2351 * r10
2352 * x614);
2353 rxp1_2
2354 = ((-1.0)
2355 * r11
2356 * x614);
2357 rxp2_2
2358 = ((-1.0)
2359 * r12
2360 * x614);
2361 j6eval
2362 [0]
2363 = ((1.0)
2364 + (((1.91568587540858)
2365 * (px
2366 * px
2367 * px
2368 * px)))
2369 + (((-1.0)
2370 * (2.64633970947792)
2371 * (px
2372 * px))));
2373 if (IKabs(
2374 j6eval
2375 [0])
2376 < 0.0000010000000000)
2377 {
2378 continue; // no branches [j6]
2379 }
2380 else
2381 {
2382 {
2383 IkReal j6array
2384 [2],
2385 cj6array
2386 [2],
2387 sj6array
2388 [2];
2389 bool j6valid
2390 [2]
2391 = {false};
2392 _nj6
2393 = 2;
2394 IkReal
2395 x615
2396 = px
2397 * px;
2398 CheckValue<IkReal> x617 = IKatan2WithCheck(
2399 IkReal((
2400 (-0.425)
2401 + (((-0.588235294117647)
2402 * x615)))),
2403 ((-2.83333333333333)
2404 + (((3.92156862745098)
2405 * x615))),
2406 IKFAST_ATAN2_MAGTHRESH);
2407 if (!x617.valid)
2408 {
2409 continue;
2410 }
2411 IkReal
2412 x616
2413 = ((-1.0)
2414 * (x617.value));
2415 j6array
2416 [0]
2417 = x616;
2418 sj6array
2419 [0]
2420 = IKsin(
2421 j6array
2422 [0]);
2423 cj6array
2424 [0]
2425 = IKcos(
2426 j6array
2427 [0]);
2428 j6array
2429 [1]
2430 = ((3.14159265358979)
2431 + x616);
2432 sj6array
2433 [1]
2434 = IKsin(
2435 j6array
2436 [1]);
2437 cj6array
2438 [1]
2439 = IKcos(
2440 j6array
2441 [1]);
2442 if (j6array
2443 [0]
2444 > IKPI)
2445 {
2446 j6array
2447 [0]
2448 -= IK2PI;
2449 }
2450 else if (
2451 j6array
2452 [0]
2453 < -IKPI)
2454 {
2455 j6array
2456 [0]
2457 += IK2PI;
2458 }
2459 j6valid
2460 [0]
2461 = true;
2462 if (j6array
2463 [1]
2464 > IKPI)
2465 {
2466 j6array
2467 [1]
2468 -= IK2PI;
2469 }
2470 else if (
2471 j6array
2472 [1]
2473 < -IKPI)
2474 {
2475 j6array
2476 [1]
2477 += IK2PI;
2478 }
2479 j6valid
2480 [1]
2481 = true;
2482 for (
2483 int ij6
2484 = 0;
2485 ij6
2486 < 2;
2487 ++ij6)
2488 {
2489 if (!j6valid
2490 [ij6])
2491 {
2492 continue;
2493 }
2494 _ij6[0]
2495 = ij6;
2496 _ij6[1]
2497 = -1;
2498 for (
2499 int iij6
2500 = ij6
2501 + 1;
2502 iij6
2503 < 2;
2504 ++iij6)
2505 {
2506 if (j6valid
2507 [iij6]
2508 && IKabs(
2509 cj6array
2510 [ij6]
2511 - cj6array
2512 [iij6])
2513 < IKFAST_SOLUTION_THRESH
2514 && IKabs(
2515 sj6array
2516 [ij6]
2517 - sj6array
2518 [iij6])
2519 < IKFAST_SOLUTION_THRESH)
2520 {
2521 j6valid
2522 [iij6]
2523 = false;
2524 _ij6[1]
2525 = iij6;
2526 break;
2527 }
2528 }
2529 j6 = j6array
2530 [ij6];
2531 cj6 = cj6array
2532 [ij6];
2533 sj6 = sj6array
2534 [ij6];
2535 {
2536 IkReal evalcond
2537 [1];
2538 evalcond
2539 [0]
2540 = ((0.85)
2541 * (IKsin(
2542 j6)));
2543 if (IKabs(
2544 evalcond
2545 [0])
2546 > IKFAST_EVALCOND_THRESH)
2547 {
2548 continue;
2549 }
2550 }
2551
2552 rotationfunction0(
2553 solutions);
2554 }
2555 }
2556 }
2557 }
2558 }
2559 } while (
2560 0);
2561 if (bgotonextstatement)
2562 {
2563 bool
2564 bgotonextstatement
2565 = true;
2566 do
2567 {
2568 evalcond
2569 [0]
2570 = ((IKabs(
2571 py))
2572 + (IKabs((
2573 (-3.14159265358979)
2574 + (IKfmod(
2575 ((4.71238898038469)
2576 + j4),
2577 6.28318530717959))))));
2578 evalcond
2579 [1]
2580 = -0.85;
2581 evalcond
2582 [2]
2583 = 0;
2584 evalcond
2585 [3]
2586 = ((-0.2125)
2587 + (((-1.0)
2588 * (1.0)
2589 * (px
2590 * px))));
2591 if (IKabs(
2592 evalcond
2593 [0])
2594 < 0.0000010000000000
2595 && IKabs(
2596 evalcond
2597 [1])
2598 < 0.0000010000000000
2599 && IKabs(
2600 evalcond
2601 [2])
2602 < 0.0000010000000000
2603 && IKabs(
2604 evalcond
2605 [3])
2606 < 0.0000010000000000)
2607 {
2608 bgotonextstatement
2609 = false;
2610 {
2611 IkReal j6eval
2612 [1];
2613 IkReal
2614 x618
2615 = ((1.0)
2616 * px);
2617 sj8 = 0;
2618 cj8 = 1.0;
2619 j8 = 0;
2620 pz = 0;
2621 j9 = 0;
2622 sj9 = 0;
2623 cj9 = 1.0;
2624 pp = px
2625 * px;
2626 npx
2627 = (px
2628 * r00);
2629 npy
2630 = (px
2631 * r01);
2632 npz
2633 = (px
2634 * r02);
2635 rxp0_0
2636 = 0;
2637 rxp0_1
2638 = (px
2639 * r20);
2640 rxp1_0
2641 = 0;
2642 rxp1_1
2643 = (px
2644 * r21);
2645 rxp2_0
2646 = 0;
2647 rxp2_1
2648 = (px
2649 * r22);
2650 py = 0;
2651 j4 = -1.5707963267949;
2652 sj4 = -1.0;
2653 cj4 = 0;
2654 rxp0_2
2655 = ((-1.0)
2656 * r10
2657 * x618);
2658 rxp1_2
2659 = ((-1.0)
2660 * r11
2661 * x618);
2662 rxp2_2
2663 = ((-1.0)
2664 * r12
2665 * x618);
2666 j6eval
2667 [0]
2668 = ((1.0)
2669 + (((1.91568587540858)
2670 * (px
2671 * px
2672 * px
2673 * px)))
2674 + (((-1.0)
2675 * (2.64633970947792)
2676 * (px
2677 * px))));
2678 if (IKabs(
2679 j6eval
2680 [0])
2681 < 0.0000010000000000)
2682 {
2683 continue; // no branches [j6]
2684 }
2685 else
2686 {
2687 {
2688 IkReal j6array
2689 [2],
2690 cj6array
2691 [2],
2692 sj6array
2693 [2];
2694 bool j6valid
2695 [2]
2696 = {false};
2697 _nj6
2698 = 2;
2699 IkReal
2700 x619
2701 = px
2702 * px;
2703 CheckValue<IkReal> x621 = IKatan2WithCheck(
2704 IkReal((
2705 (-0.425)
2706 + (((-0.588235294117647)
2707 * x619)))),
2708 ((-2.83333333333333)
2709 + (((3.92156862745098)
2710 * x619))),
2711 IKFAST_ATAN2_MAGTHRESH);
2712 if (!x621.valid)
2713 {
2714 continue;
2715 }
2716 IkReal
2717 x620
2718 = ((-1.0)
2719 * (x621.value));
2720 j6array
2721 [0]
2722 = x620;
2723 sj6array
2724 [0]
2725 = IKsin(
2726 j6array
2727 [0]);
2728 cj6array
2729 [0]
2730 = IKcos(
2731 j6array
2732 [0]);
2733 j6array
2734 [1]
2735 = ((3.14159265358979)
2736 + x620);
2737 sj6array
2738 [1]
2739 = IKsin(
2740 j6array
2741 [1]);
2742 cj6array
2743 [1]
2744 = IKcos(
2745 j6array
2746 [1]);
2747 if (j6array
2748 [0]
2749 > IKPI)
2750 {
2751 j6array
2752 [0]
2753 -= IK2PI;
2754 }
2755 else if (
2756 j6array
2757 [0]
2758 < -IKPI)
2759 {
2760 j6array
2761 [0]
2762 += IK2PI;
2763 }
2764 j6valid
2765 [0]
2766 = true;
2767 if (j6array
2768 [1]
2769 > IKPI)
2770 {
2771 j6array
2772 [1]
2773 -= IK2PI;
2774 }
2775 else if (
2776 j6array
2777 [1]
2778 < -IKPI)
2779 {
2780 j6array
2781 [1]
2782 += IK2PI;
2783 }
2784 j6valid
2785 [1]
2786 = true;
2787 for (
2788 int ij6
2789 = 0;
2790 ij6
2791 < 2;
2792 ++ij6)
2793 {
2794 if (!j6valid
2795 [ij6])
2796 {
2797 continue;
2798 }
2799 _ij6[0]
2800 = ij6;
2801 _ij6[1]
2802 = -1;
2803 for (
2804 int iij6
2805 = ij6
2806 + 1;
2807 iij6
2808 < 2;
2809 ++iij6)
2810 {
2811 if (j6valid
2812 [iij6]
2813 && IKabs(
2814 cj6array
2815 [ij6]
2816 - cj6array
2817 [iij6])
2818 < IKFAST_SOLUTION_THRESH
2819 && IKabs(
2820 sj6array
2821 [ij6]
2822 - sj6array
2823 [iij6])
2824 < IKFAST_SOLUTION_THRESH)
2825 {
2826 j6valid
2827 [iij6]
2828 = false;
2829 _ij6[1]
2830 = iij6;
2831 break;
2832 }
2833 }
2834 j6 = j6array
2835 [ij6];
2836 cj6 = cj6array
2837 [ij6];
2838 sj6 = sj6array
2839 [ij6];
2840 {
2841 IkReal evalcond
2842 [1];
2843 evalcond
2844 [0]
2845 = ((0.85)
2846 * (IKsin(
2847 j6)));
2848 if (IKabs(
2849 evalcond
2850 [0])
2851 > IKFAST_EVALCOND_THRESH)
2852 {
2853 continue;
2854 }
2855 }
2856
2857 rotationfunction0(
2858 solutions);
2859 }
2860 }
2861 }
2862 }
2863 }
2864 } while (
2865 0);
2866 if (bgotonextstatement)
2867 {
2868 bool
2869 bgotonextstatement
2870 = true;
2871 do
2872 {
2873 if (1)
2874 {
2875 bgotonextstatement
2876 = false;
2877 continue; // branch miss [j6]
2878 }
2879 } while (
2880 0);
2881 if (bgotonextstatement)
2882 {
2883 }
2884 }
2885 }
2886 }
2887 }
2888 }
2889 }
2890 }
2891 else
2892 {
2893 {
2894 IkReal j6array
2895 [1],
2896 cj6array
2897 [1],
2898 sj6array
2899 [1];
2900 bool j6valid
2901 [1]
2902 = {false};
2903 _nj6
2904 = 1;
2905 IkReal
2906 x622
2907 = (cj4
2908 * px);
2909 IkReal
2910 x623
2911 = (py
2912 * sj4);
2913 IkReal
2914 x624
2915 = px
2916 * px;
2917 IkReal
2918 x625
2919 = py
2920 * py;
2921 CheckValue<IkReal> x626 = IKPowWithIntegerCheck(
2922 ((((20.0)
2923 * x622))
2924 + (((20.0)
2925 * x623))),
2926 -1);
2927 if (!x626.valid)
2928 {
2929 continue;
2930 }
2931 CheckValue<IkReal> x627 = IKPowWithIntegerCheck(
2932 ((((-1.0)
2933 * (11.7647058823529)
2934 * cj4
2935 * (px
2936 * px
2937 * px)))
2938 + (((-8.5)
2939 * x623))
2940 + (((-11.7647058823529)
2941 * x623
2942 * x624))
2943 + (((-11.7647058823529)
2944 * x622
2945 * x625))
2946 + (((-8.5)
2947 * x622))
2948 + (((-1.0)
2949 * (11.7647058823529)
2950 * sj4
2951 * (py
2952 * py
2953 * py)))),
2954 -1);
2955 if (!x627.valid)
2956 {
2957 continue;
2958 }
2959 if (IKabs((
2960 (17.0)
2961 * (x626.value)))
2962 < IKFAST_ATAN2_MAGTHRESH
2963 && IKabs((
2964 (x627.value)
2965 * (((48.1666666666667)
2966 + (((-66.6666666666667)
2967 * x625))
2968 + (((-66.6666666666667)
2969 * x624))))))
2970 < IKFAST_ATAN2_MAGTHRESH
2971 && IKabs(
2972 IKsqr((
2973 (17.0)
2974 * (x626.value)))
2975 + IKsqr((
2976 (x627.value)
2977 * (((48.1666666666667)
2978 + (((-66.6666666666667)
2979 * x625))
2980 + (((-66.6666666666667)
2981 * x624))))))
2982 - 1)
2983 <= IKFAST_SINCOS_THRESH)
2984 continue;
2985 j6array[0] = IKatan2(
2986 ((17.0)
2987 * (x626.value)),
2988 ((x627.value)
2989 * (((48.1666666666667)
2990 + (((-66.6666666666667)
2991 * x625))
2992 + (((-66.6666666666667)
2993 * x624))))));
2994 sj6array
2995 [0]
2996 = IKsin(
2997 j6array
2998 [0]);
2999 cj6array
3000 [0]
3001 = IKcos(
3002 j6array
3003 [0]);
3004 if (j6array
3005 [0]
3006 > IKPI)
3007 {
3008 j6array
3009 [0]
3010 -= IK2PI;
3011 }
3012 else if (
3013 j6array
3014 [0]
3015 < -IKPI)
3016 {
3017 j6array
3018 [0]
3019 += IK2PI;
3020 }
3021 j6valid
3022 [0]
3023 = true;
3024 for (
3025 int ij6
3026 = 0;
3027 ij6
3028 < 1;
3029 ++ij6)
3030 {
3031 if (!j6valid
3032 [ij6])
3033 {
3034 continue;
3035 }
3036 _ij6[0]
3037 = ij6;
3038 _ij6[1]
3039 = -1;
3040 for (
3041 int iij6
3042 = ij6
3043 + 1;
3044 iij6
3045 < 1;
3046 ++iij6)
3047 {
3048 if (j6valid
3049 [iij6]
3050 && IKabs(
3051 cj6array
3052 [ij6]
3053 - cj6array
3054 [iij6])
3055 < IKFAST_SOLUTION_THRESH
3056 && IKabs(
3057 sj6array
3058 [ij6]
3059 - sj6array
3060 [iij6])
3061 < IKFAST_SOLUTION_THRESH)
3062 {
3063 j6valid
3064 [iij6]
3065 = false;
3066 _ij6[1]
3067 = iij6;
3068 break;
3069 }
3070 }
3071 j6 = j6array
3072 [ij6];
3073 cj6 = cj6array
3074 [ij6];
3075 sj6 = sj6array
3076 [ij6];
3077 {
3078 IkReal evalcond
3079 [5];
3080 IkReal
3081 x628
3082 = IKsin(
3083 j6);
3084 IkReal
3085 x629
3086 = (cj4
3087 * px);
3088 IkReal
3089 x630
3090 = (x628
3091 * x629);
3092 IkReal
3093 x631
3094 = (py
3095 * sj4);
3096 IkReal
3097 x632
3098 = (x628
3099 * x631);
3100 IkReal
3101 x633
3102 = ((1.0)
3103 * x629);
3104 IkReal
3105 x634
3106 = ((1.0)
3107 * x631);
3108 IkReal
3109 x635
3110 = IKcos(
3111 j6);
3112 IkReal
3113 x636
3114 = px
3115 * px;
3116 IkReal
3117 x637
3118 = ((3.92156862745098)
3119 * x628);
3120 IkReal
3121 x638
3122 = ((0.588235294117647)
3123 * x635);
3124 IkReal
3125 x639
3126 = py
3127 * py;
3128 IkReal
3129 x640
3130 = ((0.09)
3131 * x635);
3132 evalcond
3133 [0]
3134 = ((-0.85)
3135 + x630
3136 + x632);
3137 evalcond
3138 [1]
3139 = ((((-1.0)
3140 * x634))
3141 + (((0.85)
3142 * x628))
3143 + (((-1.0)
3144 * x633)));
3145 evalcond
3146 [2]
3147 = ((((-1.0)
3148 * x634
3149 * x635))
3150 + (((-1.0)
3151 * x633
3152 * x635)));
3153 evalcond
3154 [3]
3155 = (((x637
3156 * x639))
3157 + ((x636
3158 * x637))
3159 + (((-1.0)
3160 * x638
3161 * x639))
3162 + (((-0.425)
3163 * x635))
3164 + (((-2.83333333333333)
3165 * x628))
3166 + (((-1.0)
3167 * x636
3168 * x638)));
3169 evalcond
3170 [4]
3171 = ((-0.2125)
3172 + (((-1.0)
3173 * x639))
3174 + ((x631
3175 * x640))
3176 + (((-1.0)
3177 * x636))
3178 + ((x629
3179 * x640))
3180 + (((1.1)
3181 * x632))
3182 + (((1.1)
3183 * x630)));
3184 if (IKabs(
3185 evalcond
3186 [0])
3187 > IKFAST_EVALCOND_THRESH
3188 || IKabs(
3189 evalcond
3190 [1])
3191 > IKFAST_EVALCOND_THRESH
3192 || IKabs(
3193 evalcond
3194 [2])
3195 > IKFAST_EVALCOND_THRESH
3196 || IKabs(
3197 evalcond
3198 [3])
3199 > IKFAST_EVALCOND_THRESH
3200 || IKabs(
3201 evalcond
3202 [4])
3203 > IKFAST_EVALCOND_THRESH)
3204 {
3205 continue;
3206 }
3207 }
3208
3209 rotationfunction0(
3210 solutions);
3211 }
3212 }
3213 }
3214 }
3215 }
3216 else
3217 {
3218 {
3219 IkReal j6array
3220 [1],
3221 cj6array
3222 [1],
3223 sj6array
3224 [1];
3225 bool j6valid
3226 [1]
3227 = {false};
3228 _nj6
3229 = 1;
3230 IkReal
3231 x641
3232 = (cj4
3233 * px);
3234 IkReal
3235 x642
3236 = (py
3237 * sj4);
3238 IkReal
3239 x643
3240 = px
3241 * px;
3242 IkReal
3243 x644
3244 = py
3245 * py;
3246 CheckValue<IkReal> x645 = IKPowWithIntegerCheck(
3247 ((-7.225)
3248 + (((-10.0)
3249 * x643))
3250 + (((-10.0)
3251 * x644))),
3252 -1);
3253 if (!x645.valid)
3254 {
3255 continue;
3256 }
3257 if (IKabs((
3258 (((1.17647058823529)
3259 * x642))
3260 + (((1.17647058823529)
3261 * x641))))
3262 < IKFAST_ATAN2_MAGTHRESH
3263 && IKabs((
3264 (x645.value)
3265 * (((((-1.0)
3266 * (78.4313725490196)
3267 * sj4
3268 * (py
3269 * py
3270 * py)))
3271 + (((-78.4313725490196)
3272 * x641
3273 * x644))
3274 + (((-78.4313725490196)
3275 * x642
3276 * x643))
3277 + (((-1.0)
3278 * (78.4313725490196)
3279 * cj4
3280 * (px
3281 * px
3282 * px)))
3283 + (((56.6666666666667)
3284 * x642))
3285 + (((56.6666666666667)
3286 * x641))))))
3287 < IKFAST_ATAN2_MAGTHRESH
3288 && IKabs(
3289 IKsqr((
3290 (((1.17647058823529)
3291 * x642))
3292 + (((1.17647058823529)
3293 * x641))))
3294 + IKsqr((
3295 (x645.value)
3296 * (((((-1.0)
3297 * (78.4313725490196)
3298 * sj4
3299 * (py
3300 * py
3301 * py)))
3302 + (((-78.4313725490196)
3303 * x641
3304 * x644))
3305 + (((-78.4313725490196) * x642 * x643)) + (((-1.0) * (78.4313725490196) * cj4 * (px * px * px))) + (((56.6666666666667) * x642)) + (((56.6666666666667) * x641))))))
3306 - 1)
3307 <= IKFAST_SINCOS_THRESH)
3308 continue;
3309 j6array[0] = IKatan2(
3310 ((((1.17647058823529)
3311 * x642))
3312 + (((1.17647058823529)
3313 * x641))),
3314 ((x645.value)
3315 * (((((-1.0)
3316 * (78.4313725490196)
3317 * sj4
3318 * (py
3319 * py
3320 * py)))
3321 + (((-78.4313725490196)
3322 * x641
3323 * x644))
3324 + (((-78.4313725490196)
3325 * x642
3326 * x643))
3327 + (((-1.0)
3328 * (78.4313725490196)
3329 * cj4
3330 * (px
3331 * px
3332 * px)))
3333 + (((56.6666666666667)
3334 * x642))
3335 + (((56.6666666666667)
3336 * x641))))));
3337 sj6array
3338 [0]
3339 = IKsin(
3340 j6array
3341 [0]);
3342 cj6array
3343 [0]
3344 = IKcos(
3345 j6array
3346 [0]);
3347 if (j6array
3348 [0]
3349 > IKPI)
3350 {
3351 j6array
3352 [0]
3353 -= IK2PI;
3354 }
3355 else if (
3356 j6array
3357 [0]
3358 < -IKPI)
3359 {
3360 j6array
3361 [0]
3362 += IK2PI;
3363 }
3364 j6valid
3365 [0]
3366 = true;
3367 for (
3368 int ij6
3369 = 0;
3370 ij6
3371 < 1;
3372 ++ij6)
3373 {
3374 if (!j6valid
3375 [ij6])
3376 {
3377 continue;
3378 }
3379 _ij6[0]
3380 = ij6;
3381 _ij6[1]
3382 = -1;
3383 for (
3384 int iij6
3385 = ij6
3386 + 1;
3387 iij6
3388 < 1;
3389 ++iij6)
3390 {
3391 if (j6valid
3392 [iij6]
3393 && IKabs(
3394 cj6array
3395 [ij6]
3396 - cj6array
3397 [iij6])
3398 < IKFAST_SOLUTION_THRESH
3399 && IKabs(
3400 sj6array
3401 [ij6]
3402 - sj6array
3403 [iij6])
3404 < IKFAST_SOLUTION_THRESH)
3405 {
3406 j6valid
3407 [iij6]
3408 = false;
3409 _ij6[1]
3410 = iij6;
3411 break;
3412 }
3413 }
3414 j6 = j6array
3415 [ij6];
3416 cj6 = cj6array
3417 [ij6];
3418 sj6 = sj6array
3419 [ij6];
3420 {
3421 IkReal evalcond
3422 [5];
3423 IkReal
3424 x646
3425 = IKsin(
3426 j6);
3427 IkReal
3428 x647
3429 = (cj4
3430 * px);
3431 IkReal
3432 x648
3433 = (x646
3434 * x647);
3435 IkReal
3436 x649
3437 = (py
3438 * sj4);
3439 IkReal
3440 x650
3441 = (x646
3442 * x649);
3443 IkReal
3444 x651
3445 = ((1.0)
3446 * x647);
3447 IkReal
3448 x652
3449 = ((1.0)
3450 * x649);
3451 IkReal
3452 x653
3453 = IKcos(
3454 j6);
3455 IkReal
3456 x654
3457 = px
3458 * px;
3459 IkReal
3460 x655
3461 = ((3.92156862745098)
3462 * x646);
3463 IkReal
3464 x656
3465 = ((0.588235294117647)
3466 * x653);
3467 IkReal
3468 x657
3469 = py
3470 * py;
3471 IkReal
3472 x658
3473 = ((0.09)
3474 * x653);
3475 evalcond
3476 [0]
3477 = ((-0.85)
3478 + x650
3479 + x648);
3480 evalcond
3481 [1]
3482 = ((((-1.0)
3483 * x652))
3484 + (((-1.0)
3485 * x651))
3486 + (((0.85)
3487 * x646)));
3488 evalcond
3489 [2]
3490 = ((((-1.0)
3491 * x652
3492 * x653))
3493 + (((-1.0)
3494 * x651
3495 * x653)));
3496 evalcond
3497 [3]
3498 = (((x654
3499 * x655))
3500 + ((x655
3501 * x657))
3502 + (((-1.0)
3503 * x654
3504 * x656))
3505 + (((-0.425)
3506 * x653))
3507 + (((-1.0)
3508 * x656
3509 * x657))
3510 + (((-2.83333333333333)
3511 * x646)));
3512 evalcond
3513 [4]
3514 = ((-0.2125)
3515 + (((1.1)
3516 * x648))
3517 + (((1.1)
3518 * x650))
3519 + ((x649
3520 * x658))
3521 + (((-1.0)
3522 * x657))
3523 + ((x647
3524 * x658))
3525 + (((-1.0)
3526 * x654)));
3527 if (IKabs(
3528 evalcond
3529 [0])
3530 > IKFAST_EVALCOND_THRESH
3531 || IKabs(
3532 evalcond
3533 [1])
3534 > IKFAST_EVALCOND_THRESH
3535 || IKabs(
3536 evalcond
3537 [2])
3538 > IKFAST_EVALCOND_THRESH
3539 || IKabs(
3540 evalcond
3541 [3])
3542 > IKFAST_EVALCOND_THRESH
3543 || IKabs(
3544 evalcond
3545 [4])
3546 > IKFAST_EVALCOND_THRESH)
3547 {
3548 continue;
3549 }
3550 }
3551
3552 rotationfunction0(
3553 solutions);
3554 }
3555 }
3556 }
3557 }
3558 }
3559 else
3560 {
3561 {
3562 IkReal
3563 j6array
3564 [1],
3565 cj6array
3566 [1],
3567 sj6array
3568 [1];
3569 bool j6valid
3570 [1]
3571 = {false};
3572 _nj6 = 1;
3573 IkReal x659
3574 = (cj4
3575 * px);
3576 IkReal x660
3577 = (py
3578 * sj4);
3579 IkReal x661
3580 = px
3581 * px;
3582 IkReal x662
3583 = py
3584 * py;
3585 IkReal x663
3586 = ((1.29411764705882)
3587 * (cj4
3588 * cj4));
3589 CheckValue<
3590 IkReal>
3591 x664
3592 = IKPowWithIntegerCheck(
3593 ((((0.09)
3594 * x659))
3595 + (((0.09)
3596 * x660))),
3597 -1);
3598 if (!x664.valid)
3599 {
3600 continue;
3601 }
3602 if (IKabs((
3603 (((1.17647058823529)
3604 * x660))
3605 + (((1.17647058823529)
3606 * x659))))
3607 < IKFAST_ATAN2_MAGTHRESH
3608 && IKabs((
3609 (x664.value)
3610 * (((0.2125)
3611 + (((-2.58823529411765)
3612 * cj4
3613 * px
3614 * x660))
3615 + (((-0.294117647058824)
3616 * x662))
3617 + (((-1.0)
3618 * x661
3619 * x663))
3620 + ((x662
3621 * x663))
3622 + x661))))
3623 < IKFAST_ATAN2_MAGTHRESH
3624 && IKabs(
3625 IKsqr((
3626 (((1.17647058823529)
3627 * x660))
3628 + (((1.17647058823529)
3629 * x659))))
3630 + IKsqr((
3631 (x664.value)
3632 * (((0.2125)
3633 + (((-2.58823529411765)
3634 * cj4
3635 * px
3636 * x660))
3637 + (((-0.294117647058824)
3638 * x662))
3639 + (((-1.0)
3640 * x661
3641 * x663))
3642 + ((x662
3643 * x663))
3644 + x661))))
3645 - 1)
3646 <= IKFAST_SINCOS_THRESH)
3647 continue;
3648 j6array[0] = IKatan2(
3649 ((((1.17647058823529)
3650 * x660))
3651 + (((1.17647058823529)
3652 * x659))),
3653 ((x664.value)
3654 * (((0.2125)
3655 + (((-2.58823529411765)
3656 * cj4
3657 * px
3658 * x660))
3659 + (((-0.294117647058824)
3660 * x662))
3661 + (((-1.0)
3662 * x661
3663 * x663))
3664 + ((x662
3665 * x663))
3666 + x661))));
3667 sj6array[0] = IKsin(
3668 j6array
3669 [0]);
3670 cj6array[0] = IKcos(
3671 j6array
3672 [0]);
3673 if (j6array
3674 [0]
3675 > IKPI)
3676 {
3677 j6array[0]
3678 -= IK2PI;
3679 }
3680 else if (
3681 j6array
3682 [0]
3683 < -IKPI)
3684 {
3685 j6array[0]
3686 += IK2PI;
3687 }
3688 j6valid[0]
3689 = true;
3690 for (int ij6
3691 = 0;
3692 ij6
3693 < 1;
3694 ++ij6)
3695 {
3696 if (!j6valid
3697 [ij6])
3698 {
3699 continue;
3700 }
3701 _ij6[0]
3702 = ij6;
3703 _ij6[1]
3704 = -1;
3705 for (
3706 int iij6
3707 = ij6
3708 + 1;
3709 iij6
3710 < 1;
3711 ++iij6)
3712 {
3713 if (j6valid
3714 [iij6]
3715 && IKabs(
3716 cj6array
3717 [ij6]
3718 - cj6array
3719 [iij6])
3720 < IKFAST_SOLUTION_THRESH
3721 && IKabs(
3722 sj6array
3723 [ij6]
3724 - sj6array
3725 [iij6])
3726 < IKFAST_SOLUTION_THRESH)
3727 {
3728 j6valid
3729 [iij6]
3730 = false;
3731 _ij6[1]
3732 = iij6;
3733 break;
3734 }
3735 }
3736 j6 = j6array
3737 [ij6];
3738 cj6 = cj6array
3739 [ij6];
3740 sj6 = sj6array
3741 [ij6];
3742 {
3743 IkReal evalcond
3744 [5];
3745 IkReal
3746 x665
3747 = IKsin(
3748 j6);
3749 IkReal
3750 x666
3751 = (cj4
3752 * px);
3753 IkReal
3754 x667
3755 = (x665
3756 * x666);
3757 IkReal
3758 x668
3759 = (py
3760 * sj4);
3761 IkReal
3762 x669
3763 = (x665
3764 * x668);
3765 IkReal
3766 x670
3767 = ((1.0)
3768 * x666);
3769 IkReal
3770 x671
3771 = ((1.0)
3772 * x668);
3773 IkReal
3774 x672
3775 = IKcos(
3776 j6);
3777 IkReal
3778 x673
3779 = px
3780 * px;
3781 IkReal
3782 x674
3783 = ((3.92156862745098)
3784 * x665);
3785 IkReal
3786 x675
3787 = ((0.588235294117647)
3788 * x672);
3789 IkReal
3790 x676
3791 = py
3792 * py;
3793 IkReal
3794 x677
3795 = ((0.09)
3796 * x672);
3797 evalcond
3798 [0]
3799 = ((-0.85)
3800 + x667
3801 + x669);
3802 evalcond
3803 [1]
3804 = ((((0.85)
3805 * x665))
3806 + (((-1.0)
3807 * x670))
3808 + (((-1.0)
3809 * x671)));
3810 evalcond
3811 [2]
3812 = ((((-1.0)
3813 * x671
3814 * x672))
3815 + (((-1.0)
3816 * x670
3817 * x672)));
3818 evalcond
3819 [3]
3820 = ((((-2.83333333333333)
3821 * x665))
3822 + (((-0.425)
3823 * x672))
3824 + ((x673
3825 * x674))
3826 + (((-1.0)
3827 * x675
3828 * x676))
3829 + ((x674
3830 * x676))
3831 + (((-1.0)
3832 * x673
3833 * x675)));
3834 evalcond
3835 [4]
3836 = ((-0.2125)
3837 + ((x668
3838 * x677))
3839 + (((-1.0)
3840 * x676))
3841 + ((x666
3842 * x677))
3843 + (((1.1)
3844 * x667))
3845 + (((-1.0)
3846 * x673))
3847 + (((1.1)
3848 * x669)));
3849 if (IKabs(
3850 evalcond
3851 [0])
3852 > IKFAST_EVALCOND_THRESH
3853 || IKabs(
3854 evalcond
3855 [1])
3856 > IKFAST_EVALCOND_THRESH
3857 || IKabs(
3858 evalcond
3859 [2])
3860 > IKFAST_EVALCOND_THRESH
3861 || IKabs(
3862 evalcond
3863 [3])
3864 > IKFAST_EVALCOND_THRESH
3865 || IKabs(
3866 evalcond
3867 [4])
3868 > IKFAST_EVALCOND_THRESH)
3869 {
3870 continue;
3871 }
3872 }
3873
3874 rotationfunction0(
3875 solutions);
3876 }
3877 }
3878 }
3879 }
3880 }
3881 } while (0);
3882 if (bgotonextstatement)
3883 {
3884 bool
3885 bgotonextstatement
3886 = true;
3887 do
3888 {
3889 if (1)
3890 {
3891 bgotonextstatement
3892 = false;
3893 continue; // branch
3894 // miss
3895 // [j6]
3896 }
3897 } while (0);
3898 if (bgotonextstatement)
3899 {
3900 }
3901 }
3902 }
3903 }
3904 else
3905 {
3906 {
3907 IkReal j6array[1],
3908 cj6array[1],
3909 sj6array[1];
3910 bool j6valid[1]
3911 = {false};
3912 _nj6 = 1;
3913 IkReal x678
3914 = (cj4 * px);
3915 IkReal x679
3916 = (py * sj4);
3917 IkReal x680
3918 = ((0.108264705882353)
3919 * cj9);
3920 IkReal x681
3921 = ((0.588235294117647)
3922 * pp);
3923 IkReal x682
3924 = (cj9 * pp);
3925 IkReal x683
3926 = (cj9 * sj9);
3927 IkReal x684
3928 = (pp * sj9);
3929 IkReal x685
3930 = cj9 * cj9;
3931 IkReal x686
3932 = ((1.0) * pz);
3933 CheckValue<IkReal> x687 = IKPowWithIntegerCheck(
3934 IKsign((
3935 (((-1.0)
3936 * x678
3937 * x681))
3938 + (((-1.0)
3939 * x679
3940 * x680))
3941 + (((1.51009803921569)
3942 * pz))
3943 + (((1.32323529411765)
3944 * cj9
3945 * pz))
3946 + (((-1.0)
3947 * (3.92156862745098)
3948 * pp
3949 * pz))
3950 + (((-1.0)
3951 * x678
3952 * x680))
3953 + (((-0.316735294117647)
3954 * x679))
3955 + (((-0.316735294117647)
3956 * x678))
3957 + (((-1.0)
3958 * x679
3959 * x681)))),
3960 -1);
3961 if (!x687.valid)
3962 {
3963 continue;
3964 }
3965 CheckValue<IkReal> x688 = IKatan2WithCheck(
3966 IkReal((
3967 (-0.174204411764706)
3968 + (((-0.0264705882352941)
3969 * x684))
3970 + (((-0.00487191176470588)
3971 * x683))
3972 + (pz * pz)
3973 + (((-0.0324794117647059)
3974 * x685))
3975 + (((-1.0)
3976 * (0.154566176470588)
3977 * cj9))
3978 + (((-0.176470588235294)
3979 * x682))
3980 + (((-1.0)
3981 * (0.323529411764706)
3982 * pp))
3983 + (((-1.0)
3984 * (0.0142530882352941)
3985 * sj9)))),
3986 ((0.830553921568627)
3987 + (((-1.17647058823529)
3988 * x682))
3989 + (((0.396970588235294)
3990 * x685))
3991 + (((1.18080882352941)
3992 * cj9))
3993 + (((0.0595455882352941)
3994 * x683))
3995 + (((-1.0)
3996 * (2.15686274509804)
3997 * pp))
3998 + (((-1.0) * x678
3999 * x686))
4000 + (((-1.0) * x679
4001 * x686))
4002 + (((-0.176470588235294)
4003 * x684))
4004 + (((0.0679544117647059)
4005 * sj9))),
4006 IKFAST_ATAN2_MAGTHRESH);
4007 if (!x688.valid)
4008 {
4009 continue;
4010 }
4011 j6array[0]
4012 = ((-1.5707963267949)
4013 + (((1.5707963267949)
4014 * (x687.value)))
4015 + (x688.value));
4016 sj6array[0] = IKsin(
4017 j6array[0]);
4018 cj6array[0] = IKcos(
4019 j6array[0]);
4020 if (j6array[0] > IKPI)
4021 {
4022 j6array[0] -= IK2PI;
4023 }
4024 else if (
4025 j6array[0]
4026 < -IKPI)
4027 {
4028 j6array[0] += IK2PI;
4029 }
4030 j6valid[0] = true;
4031 for (int ij6 = 0;
4032 ij6 < 1;
4033 ++ij6)
4034 {
4035 if (!j6valid[ij6])
4036 {
4037 continue;
4038 }
4039 _ij6[0] = ij6;
4040 _ij6[1] = -1;
4041 for (int iij6
4042 = ij6 + 1;
4043 iij6 < 1;
4044 ++iij6)
4045 {
4046 if (j6valid[iij6]
4047 && IKabs(
4048 cj6array
4049 [ij6]
4050 - cj6array
4051 [iij6])
4052 < IKFAST_SOLUTION_THRESH
4053 && IKabs(
4054 sj6array
4055 [ij6]
4056 - sj6array
4057 [iij6])
4058 < IKFAST_SOLUTION_THRESH)
4059 {
4060 j6valid[iij6]
4061 = false;
4062 _ij6[1] = iij6;
4063 break;
4064 }
4065 }
4066 j6 = j6array[ij6];
4067 cj6 = cj6array[ij6];
4068 sj6 = sj6array[ij6];
4069 {
4070 IkReal
4071 evalcond[5];
4072 IkReal x689
4073 = ((0.3)
4074 * cj9);
4075 IkReal x690
4076 = ((0.045)
4077 * sj9);
4078 IkReal x691
4079 = IKcos(j6);
4080 IkReal x692
4081 = (pz * x691);
4082 IkReal x693
4083 = IKsin(j6);
4084 IkReal x694
4085 = (cj4 * px);
4086 IkReal x695
4087 = (x693
4088 * x694);
4089 IkReal x696
4090 = (py * sj4);
4091 IkReal x697
4092 = (x693
4093 * x696);
4094 IkReal x698
4095 = ((0.045)
4096 * cj9);
4097 IkReal x699
4098 = ((0.3)
4099 * sj9);
4100 IkReal x700
4101 = (pz * x693);
4102 IkReal x701
4103 = ((1.0)
4104 * x694);
4105 IkReal x702
4106 = ((1.0)
4107 * x696);
4108 IkReal x703
4109 = ((0.09)
4110 * x691);
4111 evalcond[0]
4112 = ((-0.55)
4113 + (((-1.0)
4114 * x689))
4115 + (((-1.0)
4116 * x690))
4117 + x692
4118 + x695
4119 + x697);
4120 evalcond[1]
4121 = ((0.045)
4122 + x700
4123 + (((-1.0)
4124 * x691
4125 * x701))
4126 + (((-1.0)
4127 * x698))
4128 + x699
4129 + (((-1.0)
4130 * x691
4131 * x702)));
4132 evalcond[2]
4133 = ((((-1.51009803921569)
4134 * x693))
4135 + (((-0.588235294117647)
4136 * pp
4137 * x691))
4138 + (((3.92156862745098)
4139 * pp
4140 * x693))
4141 + pz
4142 + (((-1.32323529411765)
4143 * cj9
4144 * x693))
4145 + (((-0.316735294117647)
4146 * x691))
4147 + (((-0.108264705882353)
4148 * cj9
4149 * x691)));
4150 evalcond[3]
4151 = ((((-1.0)
4152 * x691
4153 * x698))
4154 + (((-1.0)
4155 * x702))
4156 + ((x690
4157 * x693))
4158 + ((x691
4159 * x699))
4160 + (((0.55)
4161 * x693))
4162 + (((0.045)
4163 * x691))
4164 + ((x689
4165 * x693))
4166 + (((-1.0)
4167 * x701)));
4168 evalcond[4]
4169 = ((-0.2125)
4170 + ((x696
4171 * x703))
4172 + (((1.1)
4173 * x695))
4174 + (((-0.09)
4175 * x700))
4176 + (((1.1)
4177 * x692))
4178 + (((1.1)
4179 * x697))
4180 + ((x694
4181 * x703))
4182 + (((-1.0)
4183 * (1.0)
4184 * pp)));
4185 if (IKabs(evalcond
4186 [0])
4187 > IKFAST_EVALCOND_THRESH
4188 || IKabs(
4189 evalcond
4190 [1])
4191 > IKFAST_EVALCOND_THRESH
4192 || IKabs(
4193 evalcond
4194 [2])
4195 > IKFAST_EVALCOND_THRESH
4196 || IKabs(
4197 evalcond
4198 [3])
4199 > IKFAST_EVALCOND_THRESH
4200 || IKabs(
4201 evalcond
4202 [4])
4203 > IKFAST_EVALCOND_THRESH)
4204 {
4205 continue;
4206 }
4207 }
4208
4209 rotationfunction0(
4210 solutions);
4211 }
4212 }
4213 }
4214 }
4215 }
4216 else
4217 {
4218 {
4219 IkReal j6array[1],
4220 cj6array[1],
4221 sj6array[1];
4222 bool j6valid[1] = {false};
4223 _nj6 = 1;
4224 IkReal x704
4225 = ((0.045) * cj4
4226 * px);
4227 IkReal x705
4228 = ((0.045) * py
4229 * sj4);
4230 IkReal x706
4231 = ((0.3) * sj9);
4232 IkReal x707 = (cj4 * px);
4233 IkReal x708 = (py * sj4);
4234 IkReal x709 = (cj9 * sj9);
4235 IkReal x710 = cj9 * cj9;
4236 IkReal x711
4237 = ((1.0) * pz);
4238 IkReal x712 = py * py;
4239 IkReal x713 = cj4 * cj4;
4240 CheckValue<IkReal> x714
4241 = IKatan2WithCheck(
4242 IkReal((
4243 (0.03825)
4244 + (((0.087975)
4245 * x709))
4246 + (((-0.027)
4247 * x710))
4248 + (((-1.0)
4249 * x707
4250 * x711))
4251 + (((0.167025)
4252 * sj9))
4253 + (((-1.0)
4254 * (0.01125)
4255 * cj9))
4256 + (((-1.0)
4257 * x708
4258 * x711)))),
4259 ((-0.304525)
4260 + (((-1.0)
4261 * (0.0495)
4262 * sj9))
4263 + (((-0.087975)
4264 * x710))
4265 + (((2.0) * cj4
4266 * px * x708))
4267 + x712
4268 + (((-1.0) * x712
4269 * x713))
4270 + (((-1.0)
4271 * (0.33)
4272 * cj9))
4273 + ((x713
4274 * (px * px)))
4275 + (((-0.027)
4276 * x709))),
4277 IKFAST_ATAN2_MAGTHRESH);
4278 if (!x714.valid)
4279 {
4280 continue;
4281 }
4282 CheckValue<IkReal> x715
4283 = IKPowWithIntegerCheck(
4284 IKsign((
4285 (((-1.0) * cj9
4286 * x704))
4287 + (((-1.0)
4288 * (0.55)
4289 * pz))
4290 + ((x706
4291 * x708))
4292 + ((x706
4293 * x707))
4294 + x705 + x704
4295 + (((-1.0)
4296 * cj9
4297 * x705))
4298 + (((-1.0)
4299 * (0.045)
4300 * pz
4301 * sj9))
4302 + (((-1.0)
4303 * (0.3)
4304 * cj9
4305 * pz)))),
4306 -1);
4307 if (!x715.valid)
4308 {
4309 continue;
4310 }
4311 j6array[0]
4312 = ((-1.5707963267949)
4313 + (x714.value)
4314 + (((1.5707963267949)
4315 * (x715.value))));
4316 sj6array[0]
4317 = IKsin(j6array[0]);
4318 cj6array[0]
4319 = IKcos(j6array[0]);
4320 if (j6array[0] > IKPI)
4321 {
4322 j6array[0] -= IK2PI;
4323 }
4324 else if (
4325 j6array[0] < -IKPI)
4326 {
4327 j6array[0] += IK2PI;
4328 }
4329 j6valid[0] = true;
4330 for (int ij6 = 0; ij6 < 1;
4331 ++ij6)
4332 {
4333 if (!j6valid[ij6])
4334 {
4335 continue;
4336 }
4337 _ij6[0] = ij6;
4338 _ij6[1] = -1;
4339 for (int iij6 = ij6 + 1;
4340 iij6 < 1;
4341 ++iij6)
4342 {
4343 if (j6valid[iij6]
4344 && IKabs(
4345 cj6array
4346 [ij6]
4347 - cj6array
4348 [iij6])
4349 < IKFAST_SOLUTION_THRESH
4350 && IKabs(
4351 sj6array
4352 [ij6]
4353 - sj6array
4354 [iij6])
4355 < IKFAST_SOLUTION_THRESH)
4356 {
4357 j6valid[iij6]
4358 = false;
4359 _ij6[1] = iij6;
4360 break;
4361 }
4362 }
4363 j6 = j6array[ij6];
4364 cj6 = cj6array[ij6];
4365 sj6 = sj6array[ij6];
4366 {
4367 IkReal evalcond[5];
4368 IkReal x716
4369 = ((0.3) * cj9);
4370 IkReal x717
4371 = ((0.045) * sj9);
4372 IkReal x718
4373 = IKcos(j6);
4374 IkReal x719
4375 = (pz * x718);
4376 IkReal x720
4377 = IKsin(j6);
4378 IkReal x721
4379 = (cj4 * px);
4380 IkReal x722
4381 = (x720 * x721);
4382 IkReal x723
4383 = (py * sj4);
4384 IkReal x724
4385 = (x720 * x723);
4386 IkReal x725
4387 = ((0.045) * cj9);
4388 IkReal x726
4389 = ((0.3) * sj9);
4390 IkReal x727
4391 = (pz * x720);
4392 IkReal x728
4393 = ((1.0) * x721);
4394 IkReal x729
4395 = ((1.0) * x723);
4396 IkReal x730
4397 = ((0.09) * x718);
4398 evalcond[0]
4399 = ((-0.55) + x719
4400 + (((-1.0)
4401 * x716))
4402 + (((-1.0)
4403 * x717))
4404 + x724 + x722);
4405 evalcond[1]
4406 = ((0.045)
4407 + (((-1.0)
4408 * x718
4409 * x729))
4410 + (((-1.0)
4411 * x725))
4412 + (((-1.0)
4413 * x718
4414 * x728))
4415 + x727 + x726);
4416 evalcond[2]
4417 = ((((3.92156862745098)
4418 * pp * x720))
4419 + (((-1.32323529411765)
4420 * cj9
4421 * x720))
4422 + (((-0.108264705882353)
4423 * cj9
4424 * x718))
4425 + (((-1.51009803921569)
4426 * x720))
4427 + pz
4428 + (((-0.588235294117647)
4429 * pp
4430 * x718))
4431 + (((-0.316735294117647)
4432 * x718)));
4433 evalcond[3]
4434 = (((x717 * x720))
4435 + (((-1.0)
4436 * x718
4437 * x725))
4438 + (((0.045)
4439 * x718))
4440 + ((x716
4441 * x720))
4442 + (((-1.0)
4443 * x728))
4444 + (((0.55)
4445 * x720))
4446 + (((-1.0)
4447 * x729))
4448 + ((x718
4449 * x726)));
4450 evalcond[4]
4451 = ((-0.2125)
4452 + (((1.1)
4453 * x722))
4454 + (((-0.09)
4455 * x727))
4456 + (((1.1)
4457 * x719))
4458 + ((x723
4459 * x730))
4460 + (((1.1)
4461 * x724))
4462 + (((-1.0)
4463 * (1.0)
4464 * pp))
4465 + ((x721
4466 * x730)));
4467 if (IKabs(evalcond[0])
4468 > IKFAST_EVALCOND_THRESH
4469 || IKabs(evalcond
4470 [1])
4471 > IKFAST_EVALCOND_THRESH
4472 || IKabs(evalcond
4473 [2])
4474 > IKFAST_EVALCOND_THRESH
4475 || IKabs(evalcond
4476 [3])
4477 > IKFAST_EVALCOND_THRESH
4478 || IKabs(evalcond
4479 [4])
4480 > IKFAST_EVALCOND_THRESH)
4481 {
4482 continue;
4483 }
4484 }
4485
4486 rotationfunction0(
4487 solutions);
4488 }
4489 }
4490 }
4491 }
4492 }
4493 else
4494 {
4495 {
4496 IkReal j6array[1],
4497 cj6array[1], sj6array[1];
4498 bool j6valid[1] = {false};
4499 _nj6 = 1;
4500 IkReal x731 = (cj4 * px);
4501 IkReal x732 = (py * sj4);
4502 IkReal x733
4503 = ((1.32323529411765)
4504 * cj9);
4505 IkReal x734
4506 = ((3.92156862745098)
4507 * pp);
4508 IkReal x735
4509 = ((0.0264705882352941)
4510 * pp);
4511 IkReal x736 = (cj9 * sj9);
4512 IkReal x737
4513 = ((0.176470588235294)
4514 * pp);
4515 IkReal x738 = cj9 * cj9;
4516 CheckValue<IkReal> x739 = IKatan2WithCheck(
4517 IkReal((
4518 (-0.0142530882352941)
4519 + (((-0.0324794117647059)
4520 * x736))
4521 + (((0.00938117647058823)
4522 * cj9))
4523 + ((pz * x732))
4524 + ((pz * x731))
4525 + (((-1.0) * x735))
4526 + (((0.00487191176470588)
4527 * x738))
4528 + (((-1.0)
4529 * (0.0950205882352941)
4530 * sj9))
4531 + ((cj9 * x735))
4532 + (((-1.0) * sj9
4533 * x737)))),
4534 ((0.0679544117647059)
4535 + (((-1.0) * x737))
4536 + ((cj9 * x737))
4537 + (pz * pz)
4538 + (((-1.0)
4539 * (0.00840882352941177)
4540 * cj9))
4541 + (((-0.0595455882352941)
4542 * x738))
4543 + (((0.453029411764706)
4544 * sj9))
4545 + (((0.396970588235294)
4546 * x736))
4547 + (((-1.0)
4548 * (1.17647058823529)
4549 * pp * sj9))),
4550 IKFAST_ATAN2_MAGTHRESH);
4551 if (!x739.valid)
4552 {
4553 continue;
4554 }
4555 CheckValue<IkReal> x740 = IKPowWithIntegerCheck(
4556 IKsign((
4557 (((0.316735294117647)
4558 * pz))
4559 + (((0.108264705882353)
4560 * cj9 * pz))
4561 + (((0.588235294117647)
4562 * pp * pz))
4563 + ((x732 * x733))
4564 + (((-1.0) * x732
4565 * x734))
4566 + (((1.51009803921569)
4567 * x731))
4568 + (((1.51009803921569)
4569 * x732))
4570 + ((x731 * x733))
4571 + (((-1.0) * x731
4572 * x734)))),
4573 -1);
4574 if (!x740.valid)
4575 {
4576 continue;
4577 }
4578 j6array[0]
4579 = ((-1.5707963267949)
4580 + (x739.value)
4581 + (((1.5707963267949)
4582 * (x740.value))));
4583 sj6array[0]
4584 = IKsin(j6array[0]);
4585 cj6array[0]
4586 = IKcos(j6array[0]);
4587 if (j6array[0] > IKPI)
4588 {
4589 j6array[0] -= IK2PI;
4590 }
4591 else if (j6array[0] < -IKPI)
4592 {
4593 j6array[0] += IK2PI;
4594 }
4595 j6valid[0] = true;
4596 for (int ij6 = 0; ij6 < 1;
4597 ++ij6)
4598 {
4599 if (!j6valid[ij6])
4600 {
4601 continue;
4602 }
4603 _ij6[0] = ij6;
4604 _ij6[1] = -1;
4605 for (int iij6 = ij6 + 1;
4606 iij6 < 1;
4607 ++iij6)
4608 {
4609 if (j6valid[iij6]
4610 && IKabs(
4611 cj6array[ij6]
4612 - cj6array
4613 [iij6])
4614 < IKFAST_SOLUTION_THRESH
4615 && IKabs(
4616 sj6array[ij6]
4617 - sj6array
4618 [iij6])
4619 < IKFAST_SOLUTION_THRESH)
4620 {
4621 j6valid[iij6] = false;
4622 _ij6[1] = iij6;
4623 break;
4624 }
4625 }
4626 j6 = j6array[ij6];
4627 cj6 = cj6array[ij6];
4628 sj6 = sj6array[ij6];
4629 {
4630 IkReal evalcond[5];
4631 IkReal x741
4632 = ((0.3) * cj9);
4633 IkReal x742
4634 = ((0.045) * sj9);
4635 IkReal x743 = IKcos(j6);
4636 IkReal x744 = (pz * x743);
4637 IkReal x745 = IKsin(j6);
4638 IkReal x746 = (cj4 * px);
4639 IkReal x747
4640 = (x745 * x746);
4641 IkReal x748 = (py * sj4);
4642 IkReal x749
4643 = (x745 * x748);
4644 IkReal x750
4645 = ((0.045) * cj9);
4646 IkReal x751
4647 = ((0.3) * sj9);
4648 IkReal x752 = (pz * x745);
4649 IkReal x753
4650 = ((1.0) * x746);
4651 IkReal x754
4652 = ((1.0) * x748);
4653 IkReal x755
4654 = ((0.09) * x743);
4655 evalcond[0]
4656 = ((-0.55)
4657 + (((-1.0) * x741))
4658 + x749 + x744
4659 + x747
4660 + (((-1.0)
4661 * x742)));
4662 evalcond[1]
4663 = ((0.045)
4664 + (((-1.0) * x743
4665 * x753))
4666 + (((-1.0) * x743
4667 * x754))
4668 + x752 + x751
4669 + (((-1.0)
4670 * x750)));
4671 evalcond[2]
4672 = ((((-0.316735294117647)
4673 * x743))
4674 + (((-1.32323529411765)
4675 * cj9 * x745))
4676 + pz
4677 + (((-0.108264705882353)
4678 * cj9 * x743))
4679 + (((-1.51009803921569)
4680 * x745))
4681 + (((-0.588235294117647)
4682 * pp * x743))
4683 + (((3.92156862745098)
4684 * pp * x745)));
4685 evalcond[3]
4686 = ((((-1.0) * x753))
4687 + (((0.55) * x745))
4688 + (((0.045)
4689 * x743))
4690 + ((x741 * x745))
4691 + ((x743 * x751))
4692 + (((-1.0) * x743
4693 * x750))
4694 + (((-1.0) * x754))
4695 + ((x742 * x745)));
4696 evalcond[4]
4697 = ((-0.2125)
4698 + (((1.1) * x749))
4699 + (((-0.09)
4700 * x752))
4701 + ((x748 * x755))
4702 + (((1.1) * x744))
4703 + (((-1.0) * (1.0)
4704 * pp))
4705 + ((x746 * x755))
4706 + (((1.1)
4707 * x747)));
4708 if (IKabs(evalcond[0])
4709 > IKFAST_EVALCOND_THRESH
4710 || IKabs(evalcond[1])
4711 > IKFAST_EVALCOND_THRESH
4712 || IKabs(evalcond[2])
4713 > IKFAST_EVALCOND_THRESH
4714 || IKabs(evalcond[3])
4715 > IKFAST_EVALCOND_THRESH
4716 || IKabs(evalcond[4])
4717 > IKFAST_EVALCOND_THRESH)
4718 {
4719 continue;
4720 }
4721 }
4722
4723 rotationfunction0(
4724 solutions);
4725 }
4726 }
4727 }
4728 }
4729 }
4730 } while (0);
4731 if (bgotonextstatement)
4732 {
4733 bool bgotonextstatement = true;
4734 do
4735 {
4736 IkReal x756 = (px * sj4);
4737 IkReal x757 = (cj4 * py);
4738 evalcond[0]
4739 = ((-3.14159265358979)
4740 + (IKfmod(
4741 ((3.14159265358979)
4742 + (IKabs(
4743 ((-3.14159265358979)
4744 + j8)))),
4745 6.28318530717959)));
4746 evalcond[1]
4747 = ((0.39655)
4748 + (((0.0765) * sj9))
4749 + (((0.32595) * cj9))
4750 + (((-1.0) * (1.0) * pp)));
4751 evalcond[2]
4752 = ((((-1.0) * x757)) + x756);
4753 evalcond[3]
4754 = ((((-1.0) * x756)) + x757);
4755 if (IKabs(evalcond[0])
4756 < 0.0000010000000000
4757 && IKabs(evalcond[1])
4758 < 0.0000010000000000
4759 && IKabs(evalcond[2])
4760 < 0.0000010000000000
4761 && IKabs(evalcond[3])
4762 < 0.0000010000000000)
4763 {
4764 bgotonextstatement = false;
4765 {
4766 IkReal j6eval[2];
4767 sj8 = 0;
4768 cj8 = -1.0;
4769 j8 = 3.14159265358979;
4770 IkReal x758 = py * py;
4771 IkReal x759 = cj4 * cj4;
4772 IkReal x760
4773 = ((pz * pz)
4774 + ((x759 * (px * px)))
4775 + (((2.0) * cj4 * px * py
4776 * sj4))
4777 + x758
4778 + (((-1.0) * x758
4779 * x759)));
4780 j6eval[0] = x760;
4781 j6eval[1] = IKsign(x760);
4782 if (IKabs(j6eval[0])
4783 < 0.0000010000000000
4784 || IKabs(j6eval[1])
4785 < 0.0000010000000000)
4786 {
4787 {
4788 IkReal j6eval[2];
4789 sj8 = 0;
4790 cj8 = -1.0;
4791 j8 = 3.14159265358979;
4792 IkReal x761 = (cj4 * px);
4793 IkReal x762 = (cj9 * pz);
4794 IkReal x763 = (py * sj4);
4795 IkReal x764 = (pz * sj9);
4796 IkReal x765
4797 = (cj4 * px * sj9);
4798 IkReal x766
4799 = (py * sj4 * sj9);
4800 IkReal x767
4801 = ((0.045) * x761);
4802 IkReal x768
4803 = ((0.045) * x763);
4804 j6eval[0]
4805 = (((cj9 * x761))
4806 + (((-1.0)
4807 * (12.2222222222222)
4808 * pz))
4809 + (((-1.0) * x764))
4810 + (((-6.66666666666667)
4811 * x765))
4812 + (((-1.0) * x763))
4813 + (((-6.66666666666667)
4814 * x766))
4815 + ((cj9 * x763))
4816 + (((-6.66666666666667)
4817 * x762))
4818 + (((-1.0) * x761)));
4819 j6eval[1] = IKsign(
4820 ((((-0.3) * x766))
4821 + (((-1.0) * x767))
4822 + (((-1.0) * (0.55)
4823 * pz))
4824 + ((cj9 * x768))
4825 + (((-0.3) * x765))
4826 + (((-1.0) * x768))
4827 + (((-0.3) * x762))
4828 + (((-0.045) * x764))
4829 + ((cj9 * x767))));
4830 if (IKabs(j6eval[0])
4831 < 0.0000010000000000
4832 || IKabs(j6eval[1])
4833 < 0.0000010000000000)
4834 {
4835 {
4836 IkReal j6eval[2];
4837 sj8 = 0;
4838 cj8 = -1.0;
4839 j8 = 3.14159265358979;
4840 IkReal x769
4841 = (cj4 * px);
4842 IkReal x770
4843 = (cj9 * pz);
4844 IkReal x771 = (pp * pz);
4845 IkReal x772
4846 = (py * sj4);
4847 IkReal x773
4848 = (cj4 * cj9 * px);
4849 IkReal x774
4850 = (cj4 * pp * px);
4851 IkReal x775
4852 = (cj9 * py * sj4);
4853 IkReal x776
4854 = (pp * py * sj4);
4855 j6eval[0]
4856 = ((((-1.0) * x775))
4857 + (((-1.0)
4858 * (13.9482024812098)
4859 * pz))
4860 + (((-2.92556370551481)
4861 * x772))
4862 + (((-12.2222222222222)
4863 * x770))
4864 + (((-5.4333061668025)
4865 * x776))
4866 + (((-1.0)
4867 * x773))
4868 + (((-5.4333061668025)
4869 * x774))
4870 + (((-2.92556370551481)
4871 * x769))
4872 + (((36.2220411120167)
4873 * x771)));
4874 j6eval[1] = IKsign((
4875 (((-0.588235294117647)
4876 * x776))
4877 + (((-0.108264705882353)
4878 * x773))
4879 + (((3.92156862745098)
4880 * x771))
4881 + (((-0.316735294117647)
4882 * x772))
4883 + (((-1.0)
4884 * (1.51009803921569)
4885 * pz))
4886 + (((-0.316735294117647)
4887 * x769))
4888 + (((-1.32323529411765)
4889 * x770))
4890 + (((-0.588235294117647)
4891 * x774))
4892 + (((-0.108264705882353)
4893 * x775))));
4894 if (IKabs(j6eval[0])
4895 < 0.0000010000000000
4896 || IKabs(j6eval[1])
4897 < 0.0000010000000000)
4898 {
4899 {
4900 IkReal evalcond[1];
4901 bool
4902 bgotonextstatement
4903 = true;
4904 do
4905 {
4906 evalcond[0]
4907 = ((IKabs((
4908 (-3.14159265358979)
4909 + (IKfmod(
4910 ((3.14159265358979)
4911 + j9),
4912 6.28318530717959)))))
4913 + (IKabs(
4914 pz)));
4915 if (IKabs(evalcond
4916 [0])
4917 < 0.0000010000000000)
4918 {
4919 bgotonextstatement
4920 = false;
4921 {
4922 IkReal
4923 j6eval[1];
4924 IkReal x777
4925 = ((1.0)
4926 * py);
4927 sj8 = 0;
4928 cj8 = -1.0;
4929 j8 = 3.14159265358979;
4930 pz = 0;
4931 j9 = 0;
4932 sj9 = 0;
4933 cj9 = 1.0;
4934 pp
4935 = ((py
4936 * py)
4937 + (px
4938 * px));
4939 npx
4940 = (((py
4941 * r10))
4942 + ((px
4943 * r00)));
4944 npy
4945 = (((py
4946 * r11))
4947 + ((px
4948 * r01)));
4949 npz
4950 = (((px
4951 * r02))
4952 + ((py
4953 * r12)));
4954 rxp0_0
4955 = ((-1.0)
4956 * r20
4957 * x777);
4958 rxp0_1
4959 = (px
4960 * r20);
4961 rxp1_0
4962 = ((-1.0)
4963 * r21
4964 * x777);
4965 rxp1_1
4966 = (px
4967 * r21);
4968 rxp2_0
4969 = ((-1.0)
4970 * r22
4971 * x777);
4972 rxp2_1
4973 = (px
4974 * r22);
4975 j6eval[0]
4976 = ((((-1.0)
4977 * (1.0)
4978 * cj4
4979 * px))
4980 + (((-1.0)
4981 * (1.0)
4982 * py
4983 * sj4)));
4984 if (IKabs(
4985 j6eval
4986 [0])
4987 < 0.0000010000000000)
4988 {
4989 {
4990 IkReal j6eval
4991 [1];
4992 IkReal
4993 x778
4994 = ((1.0)
4995 * py);
4996 sj8 = 0;
4997 cj8 = -1.0;
4998 j8 = 3.14159265358979;
4999 pz = 0;
5000 j9 = 0;
5001 sj9 = 0;
5002 cj9 = 1.0;
5003 pp
5004 = ((py
5005 * py)
5006 + (px
5007 * px));
5008 npx
5009 = (((py
5010 * r10))
5011 + ((px
5012 * r00)));
5013 npy
5014 = (((py
5015 * r11))
5016 + ((px
5017 * r01)));
5018 npz
5019 = (((px
5020 * r02))
5021 + ((py
5022 * r12)));
5023 rxp0_0
5024 = ((-1.0)
5025 * r20
5026 * x778);
5027 rxp0_1
5028 = (px
5029 * r20);
5030 rxp1_0
5031 = ((-1.0)
5032 * r21
5033 * x778);
5034 rxp1_1
5035 = (px
5036 * r21);
5037 rxp2_0
5038 = ((-1.0)
5039 * r22
5040 * x778);
5041 rxp2_1
5042 = (px
5043 * r22);
5044 j6eval[0]
5045 = ((-1.0)
5046 + (((-1.0)
5047 * (1.3840830449827)
5048 * (px
5049 * px)))
5050 + (((-1.0)
5051 * (1.3840830449827)
5052 * (py
5053 * py))));
5054 if (IKabs(
5055 j6eval
5056 [0])
5057 < 0.0000010000000000)
5058 {
5059 {
5060 IkReal j6eval
5061 [2];
5062 IkReal
5063 x779
5064 = ((1.0)
5065 * py);
5066 sj8 = 0;
5067 cj8 = -1.0;
5068 j8 = 3.14159265358979;
5069 pz = 0;
5070 j9 = 0;
5071 sj9 = 0;
5072 cj9 = 1.0;
5073 pp
5074 = ((py
5075 * py)
5076 + (px
5077 * px));
5078 npx
5079 = (((py
5080 * r10))
5081 + ((px
5082 * r00)));
5083 npy
5084 = (((py
5085 * r11))
5086 + ((px
5087 * r01)));
5088 npz
5089 = (((px
5090 * r02))
5091 + ((py
5092 * r12)));
5093 rxp0_0
5094 = ((-1.0)
5095 * r20
5096 * x779);
5097 rxp0_1
5098 = (px
5099 * r20);
5100 rxp1_0
5101 = ((-1.0)
5102 * r21
5103 * x779);
5104 rxp1_1
5105 = (px
5106 * r21);
5107 rxp2_0
5108 = ((-1.0)
5109 * r22
5110 * x779);
5111 rxp2_1
5112 = (px
5113 * r22);
5114 IkReal
5115 x780
5116 = (cj4
5117 * px);
5118 IkReal
5119 x781
5120 = (py
5121 * sj4);
5122 j6eval
5123 [0]
5124 = (x780
5125 + x781);
5126 j6eval
5127 [1]
5128 = ((((-1.0)
5129 * (1.3840830449827)
5130 * cj4
5131 * (px
5132 * px
5133 * px)))
5134 + (((-1.0)
5135 * x780))
5136 + (((-1.3840830449827)
5137 * x780
5138 * (py
5139 * py)))
5140 + (((-1.0)
5141 * x781))
5142 + (((-1.0)
5143 * (1.3840830449827)
5144 * sj4
5145 * (py
5146 * py
5147 * py)))
5148 + (((-1.3840830449827)
5149 * x781
5150 * (px
5151 * px))));
5152 if (IKabs(
5153 j6eval
5154 [0])
5155 < 0.0000010000000000
5156 || IKabs(
5157 j6eval
5158 [1])
5159 < 0.0000010000000000)
5160 {
5161 {
5162 IkReal evalcond
5163 [4];
5164 bool
5165 bgotonextstatement
5166 = true;
5167 do
5168 {
5169 evalcond
5170 [0]
5171 = ((IKabs(
5172 py))
5173 + (IKabs(
5174 px)));
5175 evalcond
5176 [1]
5177 = -0.85;
5178 evalcond
5179 [2]
5180 = 0;
5181 evalcond
5182 [3]
5183 = -0.2125;
5184 if (IKabs(
5185 evalcond
5186 [0])
5187 < 0.0000010000000000
5188 && IKabs(
5189 evalcond
5190 [1])
5191 < 0.0000010000000000
5192 && IKabs(
5193 evalcond
5194 [2])
5195 < 0.0000010000000000
5196 && IKabs(
5197 evalcond
5198 [3])
5199 < 0.0000010000000000)
5200 {
5201 bgotonextstatement
5202 = false;
5203 {
5204 IkReal j6array
5205 [2],
5206 cj6array
5207 [2],
5208 sj6array
5209 [2];
5210 bool j6valid
5211 [2]
5212 = {false};
5213 _nj6
5214 = 2;
5215 j6array
5216 [0]
5217 = 0.148889947609497;
5218 sj6array
5219 [0]
5220 = IKsin(
5221 j6array
5222 [0]);
5223 cj6array
5224 [0]
5225 = IKcos(
5226 j6array
5227 [0]);
5228 j6array
5229 [1]
5230 = 3.29048260119929;
5231 sj6array
5232 [1]
5233 = IKsin(
5234 j6array
5235 [1]);
5236 cj6array
5237 [1]
5238 = IKcos(
5239 j6array
5240 [1]);
5241 if (j6array
5242 [0]
5243 > IKPI)
5244 {
5245 j6array
5246 [0]
5247 -= IK2PI;
5248 }
5249 else if (
5250 j6array
5251 [0]
5252 < -IKPI)
5253 {
5254 j6array
5255 [0]
5256 += IK2PI;
5257 }
5258 j6valid
5259 [0]
5260 = true;
5261 if (j6array
5262 [1]
5263 > IKPI)
5264 {
5265 j6array
5266 [1]
5267 -= IK2PI;
5268 }
5269 else if (
5270 j6array
5271 [1]
5272 < -IKPI)
5273 {
5274 j6array
5275 [1]
5276 += IK2PI;
5277 }
5278 j6valid
5279 [1]
5280 = true;
5281 for (
5282 int ij6
5283 = 0;
5284 ij6
5285 < 2;
5286 ++ij6)
5287 {
5288 if (!j6valid
5289 [ij6])
5290 {
5291 continue;
5292 }
5293 _ij6[0]
5294 = ij6;
5295 _ij6[1]
5296 = -1;
5297 for (
5298 int iij6
5299 = ij6
5300 + 1;
5301 iij6
5302 < 2;
5303 ++iij6)
5304 {
5305 if (j6valid
5306 [iij6]
5307 && IKabs(
5308 cj6array
5309 [ij6]
5310 - cj6array
5311 [iij6])
5312 < IKFAST_SOLUTION_THRESH
5313 && IKabs(
5314 sj6array
5315 [ij6]
5316 - sj6array
5317 [iij6])
5318 < IKFAST_SOLUTION_THRESH)
5319 {
5320 j6valid
5321 [iij6]
5322 = false;
5323 _ij6[1]
5324 = iij6;
5325 break;
5326 }
5327 }
5328 j6 = j6array
5329 [ij6];
5330 cj6 = cj6array
5331 [ij6];
5332 sj6 = sj6array
5333 [ij6];
5334 {
5335 IkReal evalcond
5336 [1];
5337 evalcond
5338 [0]
5339 = ((0.85)
5340 * (IKsin(
5341 j6)));
5342 if (IKabs(
5343 evalcond
5344 [0])
5345 > IKFAST_EVALCOND_THRESH)
5346 {
5347 continue;
5348 }
5349 }
5350
5351 rotationfunction0(
5352 solutions);
5353 }
5354 }
5355 }
5356 } while (
5357 0);
5358 if (bgotonextstatement)
5359 {
5360 bool
5361 bgotonextstatement
5362 = true;
5363 do
5364 {
5365 evalcond
5366 [0]
5367 = ((IKabs((
5368 (-3.14159265358979)
5369 + (IKfmod(
5370 ((3.14159265358979)
5371 + j4),
5372 6.28318530717959)))))
5373 + (IKabs(
5374 px)));
5375 evalcond
5376 [1]
5377 = -0.85;
5378 evalcond
5379 [2]
5380 = 0;
5381 evalcond
5382 [3]
5383 = ((-0.2125)
5384 + (((-1.0)
5385 * (1.0)
5386 * (py
5387 * py))));
5388 if (IKabs(
5389 evalcond
5390 [0])
5391 < 0.0000010000000000
5392 && IKabs(
5393 evalcond
5394 [1])
5395 < 0.0000010000000000
5396 && IKabs(
5397 evalcond
5398 [2])
5399 < 0.0000010000000000
5400 && IKabs(
5401 evalcond
5402 [3])
5403 < 0.0000010000000000)
5404 {
5405 bgotonextstatement
5406 = false;
5407 {
5408 IkReal j6eval
5409 [1];
5410 IkReal
5411 x782
5412 = ((1.0)
5413 * py);
5414 sj8 = 0;
5415 cj8 = -1.0;
5416 j8 = 3.14159265358979;
5417 pz = 0;
5418 j9 = 0;
5419 sj9 = 0;
5420 cj9 = 1.0;
5421 pp = py
5422 * py;
5423 npx
5424 = (py
5425 * r10);
5426 npy
5427 = (py
5428 * r11);
5429 npz
5430 = (py
5431 * r12);
5432 rxp0_0
5433 = ((-1.0)
5434 * r20
5435 * x782);
5436 rxp0_1
5437 = 0;
5438 rxp1_0
5439 = ((-1.0)
5440 * r21
5441 * x782);
5442 rxp1_1
5443 = 0;
5444 rxp2_0
5445 = ((-1.0)
5446 * r22
5447 * x782);
5448 rxp2_1
5449 = 0;
5450 px = 0;
5451 j4 = 0;
5452 sj4 = 0;
5453 cj4 = 1.0;
5454 rxp0_2
5455 = (py
5456 * r00);
5457 rxp1_2
5458 = (py
5459 * r01);
5460 rxp2_2
5461 = (py
5462 * r02);
5463 j6eval
5464 [0]
5465 = ((1.0)
5466 + (((1.91568587540858)
5467 * (py
5468 * py
5469 * py
5470 * py)))
5471 + (((-1.0)
5472 * (2.64633970947792)
5473 * (py
5474 * py))));
5475 if (IKabs(
5476 j6eval
5477 [0])
5478 < 0.0000010000000000)
5479 {
5480 continue; // no branches [j6]
5481 }
5482 else
5483 {
5484 {
5485 IkReal j6array
5486 [2],
5487 cj6array
5488 [2],
5489 sj6array
5490 [2];
5491 bool j6valid
5492 [2]
5493 = {false};
5494 _nj6
5495 = 2;
5496 IkReal
5497 x783
5498 = py
5499 * py;
5500 CheckValue<IkReal> x785 = IKatan2WithCheck(
5501 IkReal((
5502 (-0.425)
5503 + (((-0.588235294117647)
5504 * x783)))),
5505 ((2.83333333333333)
5506 + (((-3.92156862745098)
5507 * x783))),
5508 IKFAST_ATAN2_MAGTHRESH);
5509 if (!x785.valid)
5510 {
5511 continue;
5512 }
5513 IkReal
5514 x784
5515 = ((-1.0)
5516 * (x785.value));
5517 j6array
5518 [0]
5519 = x784;
5520 sj6array
5521 [0]
5522 = IKsin(
5523 j6array
5524 [0]);
5525 cj6array
5526 [0]
5527 = IKcos(
5528 j6array
5529 [0]);
5530 j6array
5531 [1]
5532 = ((3.14159265358979)
5533 + x784);
5534 sj6array
5535 [1]
5536 = IKsin(
5537 j6array
5538 [1]);
5539 cj6array
5540 [1]
5541 = IKcos(
5542 j6array
5543 [1]);
5544 if (j6array
5545 [0]
5546 > IKPI)
5547 {
5548 j6array
5549 [0]
5550 -= IK2PI;
5551 }
5552 else if (
5553 j6array
5554 [0]
5555 < -IKPI)
5556 {
5557 j6array
5558 [0]
5559 += IK2PI;
5560 }
5561 j6valid
5562 [0]
5563 = true;
5564 if (j6array
5565 [1]
5566 > IKPI)
5567 {
5568 j6array
5569 [1]
5570 -= IK2PI;
5571 }
5572 else if (
5573 j6array
5574 [1]
5575 < -IKPI)
5576 {
5577 j6array
5578 [1]
5579 += IK2PI;
5580 }
5581 j6valid
5582 [1]
5583 = true;
5584 for (
5585 int ij6
5586 = 0;
5587 ij6
5588 < 2;
5589 ++ij6)
5590 {
5591 if (!j6valid
5592 [ij6])
5593 {
5594 continue;
5595 }
5596 _ij6[0]
5597 = ij6;
5598 _ij6[1]
5599 = -1;
5600 for (
5601 int iij6
5602 = ij6
5603 + 1;
5604 iij6
5605 < 2;
5606 ++iij6)
5607 {
5608 if (j6valid
5609 [iij6]
5610 && IKabs(
5611 cj6array
5612 [ij6]
5613 - cj6array
5614 [iij6])
5615 < IKFAST_SOLUTION_THRESH
5616 && IKabs(
5617 sj6array
5618 [ij6]
5619 - sj6array
5620 [iij6])
5621 < IKFAST_SOLUTION_THRESH)
5622 {
5623 j6valid
5624 [iij6]
5625 = false;
5626 _ij6[1]
5627 = iij6;
5628 break;
5629 }
5630 }
5631 j6 = j6array
5632 [ij6];
5633 cj6 = cj6array
5634 [ij6];
5635 sj6 = sj6array
5636 [ij6];
5637 {
5638 IkReal evalcond
5639 [1];
5640 evalcond
5641 [0]
5642 = ((0.85)
5643 * (IKsin(
5644 j6)));
5645 if (IKabs(
5646 evalcond
5647 [0])
5648 > IKFAST_EVALCOND_THRESH)
5649 {
5650 continue;
5651 }
5652 }
5653
5654 rotationfunction0(
5655 solutions);
5656 }
5657 }
5658 }
5659 }
5660 }
5661 } while (
5662 0);
5663 if (bgotonextstatement)
5664 {
5665 bool
5666 bgotonextstatement
5667 = true;
5668 do
5669 {
5670 evalcond
5671 [0]
5672 = ((IKabs((
5673 (-3.14159265358979)
5674 + (IKfmod(
5675 j4,
5676 6.28318530717959)))))
5677 + (IKabs(
5678 px)));
5679 evalcond
5680 [1]
5681 = -0.85;
5682 evalcond
5683 [2]
5684 = 0;
5685 evalcond
5686 [3]
5687 = ((-0.2125)
5688 + (((-1.0)
5689 * (1.0)
5690 * (py
5691 * py))));
5692 if (IKabs(
5693 evalcond
5694 [0])
5695 < 0.0000010000000000
5696 && IKabs(
5697 evalcond
5698 [1])
5699 < 0.0000010000000000
5700 && IKabs(
5701 evalcond
5702 [2])
5703 < 0.0000010000000000
5704 && IKabs(
5705 evalcond
5706 [3])
5707 < 0.0000010000000000)
5708 {
5709 bgotonextstatement
5710 = false;
5711 {
5712 IkReal j6eval
5713 [1];
5714 IkReal
5715 x786
5716 = ((1.0)
5717 * py);
5718 sj8 = 0;
5719 cj8 = -1.0;
5720 j8 = 3.14159265358979;
5721 pz = 0;
5722 j9 = 0;
5723 sj9 = 0;
5724 cj9 = 1.0;
5725 pp = py
5726 * py;
5727 npx
5728 = (py
5729 * r10);
5730 npy
5731 = (py
5732 * r11);
5733 npz
5734 = (py
5735 * r12);
5736 rxp0_0
5737 = ((-1.0)
5738 * r20
5739 * x786);
5740 rxp0_1
5741 = 0;
5742 rxp1_0
5743 = ((-1.0)
5744 * r21
5745 * x786);
5746 rxp1_1
5747 = 0;
5748 rxp2_0
5749 = ((-1.0)
5750 * r22
5751 * x786);
5752 rxp2_1
5753 = 0;
5754 px = 0;
5755 j4 = 3.14159265358979;
5756 sj4 = 0;
5757 cj4 = -1.0;
5758 rxp0_2
5759 = (py
5760 * r00);
5761 rxp1_2
5762 = (py
5763 * r01);
5764 rxp2_2
5765 = (py
5766 * r02);
5767 j6eval
5768 [0]
5769 = ((1.0)
5770 + (((1.91568587540858)
5771 * (py
5772 * py
5773 * py
5774 * py)))
5775 + (((-1.0)
5776 * (2.64633970947792)
5777 * (py
5778 * py))));
5779 if (IKabs(
5780 j6eval
5781 [0])
5782 < 0.0000010000000000)
5783 {
5784 continue; // no branches [j6]
5785 }
5786 else
5787 {
5788 {
5789 IkReal j6array
5790 [2],
5791 cj6array
5792 [2],
5793 sj6array
5794 [2];
5795 bool j6valid
5796 [2]
5797 = {false};
5798 _nj6
5799 = 2;
5800 IkReal
5801 x787
5802 = py
5803 * py;
5804 CheckValue<IkReal> x789 = IKatan2WithCheck(
5805 IkReal((
5806 (-0.425)
5807 + (((-0.588235294117647)
5808 * x787)))),
5809 ((2.83333333333333)
5810 + (((-3.92156862745098)
5811 * x787))),
5812 IKFAST_ATAN2_MAGTHRESH);
5813 if (!x789.valid)
5814 {
5815 continue;
5816 }
5817 IkReal
5818 x788
5819 = ((-1.0)
5820 * (x789.value));
5821 j6array
5822 [0]
5823 = x788;
5824 sj6array
5825 [0]
5826 = IKsin(
5827 j6array
5828 [0]);
5829 cj6array
5830 [0]
5831 = IKcos(
5832 j6array
5833 [0]);
5834 j6array
5835 [1]
5836 = ((3.14159265358979)
5837 + x788);
5838 sj6array
5839 [1]
5840 = IKsin(
5841 j6array
5842 [1]);
5843 cj6array
5844 [1]
5845 = IKcos(
5846 j6array
5847 [1]);
5848 if (j6array
5849 [0]
5850 > IKPI)
5851 {
5852 j6array
5853 [0]
5854 -= IK2PI;
5855 }
5856 else if (
5857 j6array
5858 [0]
5859 < -IKPI)
5860 {
5861 j6array
5862 [0]
5863 += IK2PI;
5864 }
5865 j6valid
5866 [0]
5867 = true;
5868 if (j6array
5869 [1]
5870 > IKPI)
5871 {
5872 j6array
5873 [1]
5874 -= IK2PI;
5875 }
5876 else if (
5877 j6array
5878 [1]
5879 < -IKPI)
5880 {
5881 j6array
5882 [1]
5883 += IK2PI;
5884 }
5885 j6valid
5886 [1]
5887 = true;
5888 for (
5889 int ij6
5890 = 0;
5891 ij6
5892 < 2;
5893 ++ij6)
5894 {
5895 if (!j6valid
5896 [ij6])
5897 {
5898 continue;
5899 }
5900 _ij6[0]
5901 = ij6;
5902 _ij6[1]
5903 = -1;
5904 for (
5905 int iij6
5906 = ij6
5907 + 1;
5908 iij6
5909 < 2;
5910 ++iij6)
5911 {
5912 if (j6valid
5913 [iij6]
5914 && IKabs(
5915 cj6array
5916 [ij6]
5917 - cj6array
5918 [iij6])
5919 < IKFAST_SOLUTION_THRESH
5920 && IKabs(
5921 sj6array
5922 [ij6]
5923 - sj6array
5924 [iij6])
5925 < IKFAST_SOLUTION_THRESH)
5926 {
5927 j6valid
5928 [iij6]
5929 = false;
5930 _ij6[1]
5931 = iij6;
5932 break;
5933 }
5934 }
5935 j6 = j6array
5936 [ij6];
5937 cj6 = cj6array
5938 [ij6];
5939 sj6 = sj6array
5940 [ij6];
5941 {
5942 IkReal evalcond
5943 [1];
5944 evalcond
5945 [0]
5946 = ((0.85)
5947 * (IKsin(
5948 j6)));
5949 if (IKabs(
5950 evalcond
5951 [0])
5952 > IKFAST_EVALCOND_THRESH)
5953 {
5954 continue;
5955 }
5956 }
5957
5958 rotationfunction0(
5959 solutions);
5960 }
5961 }
5962 }
5963 }
5964 }
5965 } while (
5966 0);
5967 if (bgotonextstatement)
5968 {
5969 bool
5970 bgotonextstatement
5971 = true;
5972 do
5973 {
5974 evalcond
5975 [0]
5976 = ((IKabs((
5977 (-3.14159265358979)
5978 + (IKfmod(
5979 ((1.5707963267949)
5980 + j4),
5981 6.28318530717959)))))
5982 + (IKabs(
5983 py)));
5984 evalcond
5985 [1]
5986 = -0.85;
5987 evalcond
5988 [2]
5989 = 0;
5990 evalcond
5991 [3]
5992 = ((-0.2125)
5993 + (((-1.0)
5994 * (1.0)
5995 * (px
5996 * px))));
5997 if (IKabs(
5998 evalcond
5999 [0])
6000 < 0.0000010000000000
6001 && IKabs(
6002 evalcond
6003 [1])
6004 < 0.0000010000000000
6005 && IKabs(
6006 evalcond
6007 [2])
6008 < 0.0000010000000000
6009 && IKabs(
6010 evalcond
6011 [3])
6012 < 0.0000010000000000)
6013 {
6014 bgotonextstatement
6015 = false;
6016 {
6017 IkReal j6eval
6018 [1];
6019 IkReal
6020 x790
6021 = ((1.0)
6022 * px);
6023 sj8 = 0;
6024 cj8 = -1.0;
6025 j8 = 3.14159265358979;
6026 pz = 0;
6027 j9 = 0;
6028 sj9 = 0;
6029 cj9 = 1.0;
6030 pp = px
6031 * px;
6032 npx
6033 = (px
6034 * r00);
6035 npy
6036 = (px
6037 * r01);
6038 npz
6039 = (px
6040 * r02);
6041 rxp0_0
6042 = 0;
6043 rxp0_1
6044 = (px
6045 * r20);
6046 rxp1_0
6047 = 0;
6048 rxp1_1
6049 = (px
6050 * r21);
6051 rxp2_0
6052 = 0;
6053 rxp2_1
6054 = (px
6055 * r22);
6056 py = 0;
6057 j4 = 1.5707963267949;
6058 sj4 = 1.0;
6059 cj4 = 0;
6060 rxp0_2
6061 = ((-1.0)
6062 * r10
6063 * x790);
6064 rxp1_2
6065 = ((-1.0)
6066 * r11
6067 * x790);
6068 rxp2_2
6069 = ((-1.0)
6070 * r12
6071 * x790);
6072 j6eval
6073 [0]
6074 = ((1.0)
6075 + (((1.91568587540858)
6076 * (px
6077 * px
6078 * px
6079 * px)))
6080 + (((-1.0)
6081 * (2.64633970947792)
6082 * (px
6083 * px))));
6084 if (IKabs(
6085 j6eval
6086 [0])
6087 < 0.0000010000000000)
6088 {
6089 continue; // no branches [j6]
6090 }
6091 else
6092 {
6093 {
6094 IkReal j6array
6095 [2],
6096 cj6array
6097 [2],
6098 sj6array
6099 [2];
6100 bool j6valid
6101 [2]
6102 = {false};
6103 _nj6
6104 = 2;
6105 IkReal
6106 x791
6107 = px
6108 * px;
6109 CheckValue<IkReal> x793 = IKatan2WithCheck(
6110 IkReal((
6111 (-0.425)
6112 + (((-0.588235294117647)
6113 * x791)))),
6114 ((2.83333333333333)
6115 + (((-3.92156862745098)
6116 * x791))),
6117 IKFAST_ATAN2_MAGTHRESH);
6118 if (!x793.valid)
6119 {
6120 continue;
6121 }
6122 IkReal
6123 x792
6124 = ((-1.0)
6125 * (x793.value));
6126 j6array
6127 [0]
6128 = x792;
6129 sj6array
6130 [0]
6131 = IKsin(
6132 j6array
6133 [0]);
6134 cj6array
6135 [0]
6136 = IKcos(
6137 j6array
6138 [0]);
6139 j6array
6140 [1]
6141 = ((3.14159265358979)
6142 + x792);
6143 sj6array
6144 [1]
6145 = IKsin(
6146 j6array
6147 [1]);
6148 cj6array
6149 [1]
6150 = IKcos(
6151 j6array
6152 [1]);
6153 if (j6array
6154 [0]
6155 > IKPI)
6156 {
6157 j6array
6158 [0]
6159 -= IK2PI;
6160 }
6161 else if (
6162 j6array
6163 [0]
6164 < -IKPI)
6165 {
6166 j6array
6167 [0]
6168 += IK2PI;
6169 }
6170 j6valid
6171 [0]
6172 = true;
6173 if (j6array
6174 [1]
6175 > IKPI)
6176 {
6177 j6array
6178 [1]
6179 -= IK2PI;
6180 }
6181 else if (
6182 j6array
6183 [1]
6184 < -IKPI)
6185 {
6186 j6array
6187 [1]
6188 += IK2PI;
6189 }
6190 j6valid
6191 [1]
6192 = true;
6193 for (
6194 int ij6
6195 = 0;
6196 ij6
6197 < 2;
6198 ++ij6)
6199 {
6200 if (!j6valid
6201 [ij6])
6202 {
6203 continue;
6204 }
6205 _ij6[0]
6206 = ij6;
6207 _ij6[1]
6208 = -1;
6209 for (
6210 int iij6
6211 = ij6
6212 + 1;
6213 iij6
6214 < 2;
6215 ++iij6)
6216 {
6217 if (j6valid
6218 [iij6]
6219 && IKabs(
6220 cj6array
6221 [ij6]
6222 - cj6array
6223 [iij6])
6224 < IKFAST_SOLUTION_THRESH
6225 && IKabs(
6226 sj6array
6227 [ij6]
6228 - sj6array
6229 [iij6])
6230 < IKFAST_SOLUTION_THRESH)
6231 {
6232 j6valid
6233 [iij6]
6234 = false;
6235 _ij6[1]
6236 = iij6;
6237 break;
6238 }
6239 }
6240 j6 = j6array
6241 [ij6];
6242 cj6 = cj6array
6243 [ij6];
6244 sj6 = sj6array
6245 [ij6];
6246 {
6247 IkReal evalcond
6248 [1];
6249 evalcond
6250 [0]
6251 = ((0.85)
6252 * (IKsin(
6253 j6)));
6254 if (IKabs(
6255 evalcond
6256 [0])
6257 > IKFAST_EVALCOND_THRESH)
6258 {
6259 continue;
6260 }
6261 }
6262
6263 rotationfunction0(
6264 solutions);
6265 }
6266 }
6267 }
6268 }
6269 }
6270 } while (
6271 0);
6272 if (bgotonextstatement)
6273 {
6274 bool
6275 bgotonextstatement
6276 = true;
6277 do
6278 {
6279 evalcond
6280 [0]
6281 = ((IKabs(
6282 py))
6283 + (IKabs((
6284 (-3.14159265358979)
6285 + (IKfmod(
6286 ((4.71238898038469)
6287 + j4),
6288 6.28318530717959))))));
6289 evalcond
6290 [1]
6291 = -0.85;
6292 evalcond
6293 [2]
6294 = 0;
6295 evalcond
6296 [3]
6297 = ((-0.2125)
6298 + (((-1.0)
6299 * (1.0)
6300 * (px
6301 * px))));
6302 if (IKabs(
6303 evalcond
6304 [0])
6305 < 0.0000010000000000
6306 && IKabs(
6307 evalcond
6308 [1])
6309 < 0.0000010000000000
6310 && IKabs(
6311 evalcond
6312 [2])
6313 < 0.0000010000000000
6314 && IKabs(
6315 evalcond
6316 [3])
6317 < 0.0000010000000000)
6318 {
6319 bgotonextstatement
6320 = false;
6321 {
6322 IkReal j6eval
6323 [1];
6324 IkReal
6325 x794
6326 = ((1.0)
6327 * px);
6328 sj8 = 0;
6329 cj8 = -1.0;
6330 j8 = 3.14159265358979;
6331 pz = 0;
6332 j9 = 0;
6333 sj9 = 0;
6334 cj9 = 1.0;
6335 pp = px
6336 * px;
6337 npx
6338 = (px
6339 * r00);
6340 npy
6341 = (px
6342 * r01);
6343 npz
6344 = (px
6345 * r02);
6346 rxp0_0
6347 = 0;
6348 rxp0_1
6349 = (px
6350 * r20);
6351 rxp1_0
6352 = 0;
6353 rxp1_1
6354 = (px
6355 * r21);
6356 rxp2_0
6357 = 0;
6358 rxp2_1
6359 = (px
6360 * r22);
6361 py = 0;
6362 j4 = -1.5707963267949;
6363 sj4 = -1.0;
6364 cj4 = 0;
6365 rxp0_2
6366 = ((-1.0)
6367 * r10
6368 * x794);
6369 rxp1_2
6370 = ((-1.0)
6371 * r11
6372 * x794);
6373 rxp2_2
6374 = ((-1.0)
6375 * r12
6376 * x794);
6377 j6eval
6378 [0]
6379 = ((1.0)
6380 + (((1.91568587540858)
6381 * (px
6382 * px
6383 * px
6384 * px)))
6385 + (((-1.0)
6386 * (2.64633970947792)
6387 * (px
6388 * px))));
6389 if (IKabs(
6390 j6eval
6391 [0])
6392 < 0.0000010000000000)
6393 {
6394 continue; // no branches [j6]
6395 }
6396 else
6397 {
6398 {
6399 IkReal j6array
6400 [2],
6401 cj6array
6402 [2],
6403 sj6array
6404 [2];
6405 bool j6valid
6406 [2]
6407 = {false};
6408 _nj6
6409 = 2;
6410 IkReal
6411 x795
6412 = px
6413 * px;
6414 CheckValue<IkReal> x797 = IKatan2WithCheck(
6415 IkReal((
6416 (-0.425)
6417 + (((-0.588235294117647)
6418 * x795)))),
6419 ((2.83333333333333)
6420 + (((-3.92156862745098)
6421 * x795))),
6422 IKFAST_ATAN2_MAGTHRESH);
6423 if (!x797.valid)
6424 {
6425 continue;
6426 }
6427 IkReal
6428 x796
6429 = ((-1.0)
6430 * (x797.value));
6431 j6array
6432 [0]
6433 = x796;
6434 sj6array
6435 [0]
6436 = IKsin(
6437 j6array
6438 [0]);
6439 cj6array
6440 [0]
6441 = IKcos(
6442 j6array
6443 [0]);
6444 j6array
6445 [1]
6446 = ((3.14159265358979)
6447 + x796);
6448 sj6array
6449 [1]
6450 = IKsin(
6451 j6array
6452 [1]);
6453 cj6array
6454 [1]
6455 = IKcos(
6456 j6array
6457 [1]);
6458 if (j6array
6459 [0]
6460 > IKPI)
6461 {
6462 j6array
6463 [0]
6464 -= IK2PI;
6465 }
6466 else if (
6467 j6array
6468 [0]
6469 < -IKPI)
6470 {
6471 j6array
6472 [0]
6473 += IK2PI;
6474 }
6475 j6valid
6476 [0]
6477 = true;
6478 if (j6array
6479 [1]
6480 > IKPI)
6481 {
6482 j6array
6483 [1]
6484 -= IK2PI;
6485 }
6486 else if (
6487 j6array
6488 [1]
6489 < -IKPI)
6490 {
6491 j6array
6492 [1]
6493 += IK2PI;
6494 }
6495 j6valid
6496 [1]
6497 = true;
6498 for (
6499 int ij6
6500 = 0;
6501 ij6
6502 < 2;
6503 ++ij6)
6504 {
6505 if (!j6valid
6506 [ij6])
6507 {
6508 continue;
6509 }
6510 _ij6[0]
6511 = ij6;
6512 _ij6[1]
6513 = -1;
6514 for (
6515 int iij6
6516 = ij6
6517 + 1;
6518 iij6
6519 < 2;
6520 ++iij6)
6521 {
6522 if (j6valid
6523 [iij6]
6524 && IKabs(
6525 cj6array
6526 [ij6]
6527 - cj6array
6528 [iij6])
6529 < IKFAST_SOLUTION_THRESH
6530 && IKabs(
6531 sj6array
6532 [ij6]
6533 - sj6array
6534 [iij6])
6535 < IKFAST_SOLUTION_THRESH)
6536 {
6537 j6valid
6538 [iij6]
6539 = false;
6540 _ij6[1]
6541 = iij6;
6542 break;
6543 }
6544 }
6545 j6 = j6array
6546 [ij6];
6547 cj6 = cj6array
6548 [ij6];
6549 sj6 = sj6array
6550 [ij6];
6551 {
6552 IkReal evalcond
6553 [1];
6554 evalcond
6555 [0]
6556 = ((0.85)
6557 * (IKsin(
6558 j6)));
6559 if (IKabs(
6560 evalcond
6561 [0])
6562 > IKFAST_EVALCOND_THRESH)
6563 {
6564 continue;
6565 }
6566 }
6567
6568 rotationfunction0(
6569 solutions);
6570 }
6571 }
6572 }
6573 }
6574 }
6575 } while (
6576 0);
6577 if (bgotonextstatement)
6578 {
6579 bool
6580 bgotonextstatement
6581 = true;
6582 do
6583 {
6584 if (1)
6585 {
6586 bgotonextstatement
6587 = false;
6588 continue; // branch miss [j6]
6589 }
6590 } while (
6591 0);
6592 if (bgotonextstatement)
6593 {
6594 }
6595 }
6596 }
6597 }
6598 }
6599 }
6600 }
6601 }
6602 else
6603 {
6604 {
6605 IkReal j6array
6606 [1],
6607 cj6array
6608 [1],
6609 sj6array
6610 [1];
6611 bool j6valid
6612 [1]
6613 = {false};
6614 _nj6
6615 = 1;
6616 IkReal
6617 x798
6618 = (cj4
6619 * px);
6620 IkReal
6621 x799
6622 = (py
6623 * sj4);
6624 IkReal
6625 x800
6626 = px
6627 * px;
6628 IkReal
6629 x801
6630 = py
6631 * py;
6632 CheckValue<IkReal> x802 = IKPowWithIntegerCheck(
6633 ((((20.0)
6634 * x799))
6635 + (((20.0)
6636 * x798))),
6637 -1);
6638 if (!x802.valid)
6639 {
6640 continue;
6641 }
6642 CheckValue<IkReal> x803 = IKPowWithIntegerCheck(
6643 ((((-8.5)
6644 * x798))
6645 + (((-1.0)
6646 * (11.7647058823529)
6647 * cj4
6648 * (px
6649 * px
6650 * px)))
6651 + (((-11.7647058823529)
6652 * x798
6653 * x801))
6654 + (((-11.7647058823529)
6655 * x799
6656 * x800))
6657 + (((-8.5)
6658 * x799))
6659 + (((-1.0)
6660 * (11.7647058823529)
6661 * sj4
6662 * (py
6663 * py
6664 * py)))),
6665 -1);
6666 if (!x803.valid)
6667 {
6668 continue;
6669 }
6670 if (IKabs((
6671 (17.0)
6672 * (x802.value)))
6673 < IKFAST_ATAN2_MAGTHRESH
6674 && IKabs((
6675 (x803.value)
6676 * (((-48.1666666666667)
6677 + (((66.6666666666667)
6678 * x800))
6679 + (((66.6666666666667)
6680 * x801))))))
6681 < IKFAST_ATAN2_MAGTHRESH
6682 && IKabs(
6683 IKsqr((
6684 (17.0)
6685 * (x802.value)))
6686 + IKsqr((
6687 (x803.value)
6688 * (((-48.1666666666667)
6689 + (((66.6666666666667)
6690 * x800))
6691 + (((66.6666666666667)
6692 * x801))))))
6693 - 1)
6694 <= IKFAST_SINCOS_THRESH)
6695 continue;
6696 j6array[0] = IKatan2(
6697 ((17.0)
6698 * (x802.value)),
6699 ((x803.value)
6700 * (((-48.1666666666667)
6701 + (((66.6666666666667)
6702 * x800))
6703 + (((66.6666666666667)
6704 * x801))))));
6705 sj6array
6706 [0]
6707 = IKsin(
6708 j6array
6709 [0]);
6710 cj6array
6711 [0]
6712 = IKcos(
6713 j6array
6714 [0]);
6715 if (j6array
6716 [0]
6717 > IKPI)
6718 {
6719 j6array
6720 [0]
6721 -= IK2PI;
6722 }
6723 else if (
6724 j6array
6725 [0]
6726 < -IKPI)
6727 {
6728 j6array
6729 [0]
6730 += IK2PI;
6731 }
6732 j6valid
6733 [0]
6734 = true;
6735 for (
6736 int ij6
6737 = 0;
6738 ij6
6739 < 1;
6740 ++ij6)
6741 {
6742 if (!j6valid
6743 [ij6])
6744 {
6745 continue;
6746 }
6747 _ij6[0]
6748 = ij6;
6749 _ij6[1]
6750 = -1;
6751 for (
6752 int iij6
6753 = ij6
6754 + 1;
6755 iij6
6756 < 1;
6757 ++iij6)
6758 {
6759 if (j6valid
6760 [iij6]
6761 && IKabs(
6762 cj6array
6763 [ij6]
6764 - cj6array
6765 [iij6])
6766 < IKFAST_SOLUTION_THRESH
6767 && IKabs(
6768 sj6array
6769 [ij6]
6770 - sj6array
6771 [iij6])
6772 < IKFAST_SOLUTION_THRESH)
6773 {
6774 j6valid
6775 [iij6]
6776 = false;
6777 _ij6[1]
6778 = iij6;
6779 break;
6780 }
6781 }
6782 j6 = j6array
6783 [ij6];
6784 cj6 = cj6array
6785 [ij6];
6786 sj6 = sj6array
6787 [ij6];
6788 {
6789 IkReal evalcond
6790 [5];
6791 IkReal
6792 x804
6793 = IKcos(
6794 j6);
6795 IkReal
6796 x805
6797 = (cj4
6798 * px);
6799 IkReal
6800 x806
6801 = (x804
6802 * x805);
6803 IkReal
6804 x807
6805 = (py
6806 * sj4);
6807 IkReal
6808 x808
6809 = (x804
6810 * x807);
6811 IkReal
6812 x809
6813 = IKsin(
6814 j6);
6815 IkReal
6816 x810
6817 = (x805
6818 * x809);
6819 IkReal
6820 x811
6821 = (x807
6822 * x809);
6823 IkReal
6824 x812
6825 = px
6826 * px;
6827 IkReal
6828 x813
6829 = ((3.92156862745098)
6830 * x809);
6831 IkReal
6832 x814
6833 = ((0.588235294117647)
6834 * x804);
6835 IkReal
6836 x815
6837 = py
6838 * py;
6839 evalcond
6840 [0]
6841 = (x806
6842 + x808);
6843 evalcond
6844 [1]
6845 = ((-0.85)
6846 + x811
6847 + x810);
6848 evalcond
6849 [2]
6850 = ((((-1.0)
6851 * x807))
6852 + (((0.85)
6853 * x809))
6854 + (((-1.0)
6855 * x805)));
6856 evalcond
6857 [3]
6858 = ((((-1.0)
6859 * x812
6860 * x814))
6861 + (((-1.0)
6862 * x814
6863 * x815))
6864 + (((-1.0)
6865 * x812
6866 * x813))
6867 + (((2.83333333333333)
6868 * x809))
6869 + (((-0.425)
6870 * x804))
6871 + (((-1.0)
6872 * x813
6873 * x815)));
6874 evalcond
6875 [4]
6876 = ((-0.2125)
6877 + (((1.1)
6878 * x810))
6879 + (((-0.09)
6880 * x808))
6881 + (((-1.0)
6882 * x815))
6883 + (((1.1)
6884 * x811))
6885 + (((-1.0)
6886 * x812))
6887 + (((-0.09)
6888 * x806)));
6889 if (IKabs(
6890 evalcond
6891 [0])
6892 > IKFAST_EVALCOND_THRESH
6893 || IKabs(
6894 evalcond
6895 [1])
6896 > IKFAST_EVALCOND_THRESH
6897 || IKabs(
6898 evalcond
6899 [2])
6900 > IKFAST_EVALCOND_THRESH
6901 || IKabs(
6902 evalcond
6903 [3])
6904 > IKFAST_EVALCOND_THRESH
6905 || IKabs(
6906 evalcond
6907 [4])
6908 > IKFAST_EVALCOND_THRESH)
6909 {
6910 continue;
6911 }
6912 }
6913
6914 rotationfunction0(
6915 solutions);
6916 }
6917 }
6918 }
6919 }
6920 }
6921 else
6922 {
6923 {
6924 IkReal j6array
6925 [1],
6926 cj6array
6927 [1],
6928 sj6array
6929 [1];
6930 bool j6valid
6931 [1]
6932 = {false};
6933 _nj6
6934 = 1;
6935 IkReal
6936 x816
6937 = (cj4
6938 * px);
6939 IkReal
6940 x817
6941 = (py
6942 * sj4);
6943 IkReal
6944 x818
6945 = px
6946 * px;
6947 IkReal
6948 x819
6949 = py
6950 * py;
6951 CheckValue<IkReal> x820 = IKPowWithIntegerCheck(
6952 ((-7.225)
6953 + (((-10.0)
6954 * x818))
6955 + (((-10.0)
6956 * x819))),
6957 -1);
6958 if (!x820.valid)
6959 {
6960 continue;
6961 }
6962 if (IKabs((
6963 (((1.17647058823529)
6964 * x816))
6965 + (((1.17647058823529)
6966 * x817))))
6967 < IKFAST_ATAN2_MAGTHRESH
6968 && IKabs((
6969 (x820.value)
6970 * (((((78.4313725490196)
6971 * x817
6972 * x818))
6973 + (((78.4313725490196)
6974 * sj4
6975 * (py
6976 * py
6977 * py)))
6978 + (((78.4313725490196)
6979 * cj4
6980 * (px
6981 * px
6982 * px)))
6983 + (((-56.6666666666667)
6984 * x816))
6985 + (((-56.6666666666667)
6986 * x817))
6987 + (((78.4313725490196)
6988 * x816
6989 * x819))))))
6990 < IKFAST_ATAN2_MAGTHRESH
6991 && IKabs(
6992 IKsqr((
6993 (((1.17647058823529)
6994 * x816))
6995 + (((1.17647058823529)
6996 * x817))))
6997 + IKsqr((
6998 (x820.value) * (((((78.4313725490196) * x817 * x818)) + (((78.4313725490196) * sj4 * (py * py * py))) + (((78.4313725490196) * cj4 * (px * px * px))) + (((-56.6666666666667) * x816)) + (((-56.6666666666667) * x817)) + (((78.4313725490196) * x816 * x819))))))
6999 - 1)
7000 <= IKFAST_SINCOS_THRESH)
7001 continue;
7002 j6array[0] = IKatan2(
7003 ((((1.17647058823529)
7004 * x816))
7005 + (((1.17647058823529)
7006 * x817))),
7007 ((x820.value)
7008 * (((((78.4313725490196)
7009 * x817
7010 * x818))
7011 + (((78.4313725490196)
7012 * sj4
7013 * (py
7014 * py
7015 * py)))
7016 + (((78.4313725490196)
7017 * cj4
7018 * (px
7019 * px
7020 * px)))
7021 + (((-56.6666666666667)
7022 * x816))
7023 + (((-56.6666666666667)
7024 * x817))
7025 + (((78.4313725490196)
7026 * x816
7027 * x819))))));
7028 sj6array
7029 [0]
7030 = IKsin(
7031 j6array
7032 [0]);
7033 cj6array
7034 [0]
7035 = IKcos(
7036 j6array
7037 [0]);
7038 if (j6array
7039 [0]
7040 > IKPI)
7041 {
7042 j6array
7043 [0]
7044 -= IK2PI;
7045 }
7046 else if (
7047 j6array
7048 [0]
7049 < -IKPI)
7050 {
7051 j6array
7052 [0]
7053 += IK2PI;
7054 }
7055 j6valid
7056 [0]
7057 = true;
7058 for (
7059 int ij6
7060 = 0;
7061 ij6
7062 < 1;
7063 ++ij6)
7064 {
7065 if (!j6valid
7066 [ij6])
7067 {
7068 continue;
7069 }
7070 _ij6[0]
7071 = ij6;
7072 _ij6[1]
7073 = -1;
7074 for (
7075 int iij6
7076 = ij6
7077 + 1;
7078 iij6
7079 < 1;
7080 ++iij6)
7081 {
7082 if (j6valid
7083 [iij6]
7084 && IKabs(
7085 cj6array
7086 [ij6]
7087 - cj6array
7088 [iij6])
7089 < IKFAST_SOLUTION_THRESH
7090 && IKabs(
7091 sj6array
7092 [ij6]
7093 - sj6array
7094 [iij6])
7095 < IKFAST_SOLUTION_THRESH)
7096 {
7097 j6valid
7098 [iij6]
7099 = false;
7100 _ij6[1]
7101 = iij6;
7102 break;
7103 }
7104 }
7105 j6 = j6array
7106 [ij6];
7107 cj6 = cj6array
7108 [ij6];
7109 sj6 = sj6array
7110 [ij6];
7111 {
7112 IkReal evalcond
7113 [5];
7114 IkReal
7115 x821
7116 = IKcos(
7117 j6);
7118 IkReal
7119 x822
7120 = (cj4
7121 * px);
7122 IkReal
7123 x823
7124 = (x821
7125 * x822);
7126 IkReal
7127 x824
7128 = (py
7129 * sj4);
7130 IkReal
7131 x825
7132 = (x821
7133 * x824);
7134 IkReal
7135 x826
7136 = IKsin(
7137 j6);
7138 IkReal
7139 x827
7140 = (x822
7141 * x826);
7142 IkReal
7143 x828
7144 = (x824
7145 * x826);
7146 IkReal
7147 x829
7148 = px
7149 * px;
7150 IkReal
7151 x830
7152 = ((3.92156862745098)
7153 * x826);
7154 IkReal
7155 x831
7156 = ((0.588235294117647)
7157 * x821);
7158 IkReal
7159 x832
7160 = py
7161 * py;
7162 evalcond
7163 [0]
7164 = (x825
7165 + x823);
7166 evalcond
7167 [1]
7168 = ((-0.85)
7169 + x827
7170 + x828);
7171 evalcond
7172 [2]
7173 = ((((0.85)
7174 * x826))
7175 + (((-1.0)
7176 * x822))
7177 + (((-1.0)
7178 * x824)));
7179 evalcond
7180 [3]
7181 = ((((-1.0)
7182 * x830
7183 * x832))
7184 + (((2.83333333333333)
7185 * x826))
7186 + (((-1.0)
7187 * x829
7188 * x831))
7189 + (((-1.0)
7190 * x831
7191 * x832))
7192 + (((-1.0)
7193 * x829
7194 * x830))
7195 + (((-0.425)
7196 * x821)));
7197 evalcond
7198 [4]
7199 = ((-0.2125)
7200 + (((1.1)
7201 * x828))
7202 + (((-0.09)
7203 * x825))
7204 + (((1.1)
7205 * x827))
7206 + (((-0.09)
7207 * x823))
7208 + (((-1.0)
7209 * x832))
7210 + (((-1.0)
7211 * x829)));
7212 if (IKabs(
7213 evalcond
7214 [0])
7215 > IKFAST_EVALCOND_THRESH
7216 || IKabs(
7217 evalcond
7218 [1])
7219 > IKFAST_EVALCOND_THRESH
7220 || IKabs(
7221 evalcond
7222 [2])
7223 > IKFAST_EVALCOND_THRESH
7224 || IKabs(
7225 evalcond
7226 [3])
7227 > IKFAST_EVALCOND_THRESH
7228 || IKabs(
7229 evalcond
7230 [4])
7231 > IKFAST_EVALCOND_THRESH)
7232 {
7233 continue;
7234 }
7235 }
7236
7237 rotationfunction0(
7238 solutions);
7239 }
7240 }
7241 }
7242 }
7243 }
7244 else
7245 {
7246 {
7247 IkReal j6array
7248 [1],
7249 cj6array
7250 [1],
7251 sj6array
7252 [1];
7253 bool j6valid
7254 [1]
7255 = {false};
7256 _nj6 = 1;
7257 IkReal
7258 x833
7259 = (cj4
7260 * px);
7261 IkReal
7262 x834
7263 = (py
7264 * sj4);
7265 IkReal
7266 x835
7267 = px
7268 * px;
7269 IkReal
7270 x836
7271 = py
7272 * py;
7273 IkReal
7274 x837
7275 = ((1.29411764705882)
7276 * (cj4
7277 * cj4));
7278 CheckValue<
7279 IkReal>
7280 x838
7281 = IKPowWithIntegerCheck(
7282 ((((-0.09)
7283 * x834))
7284 + (((-0.09)
7285 * x833))),
7286 -1);
7287 if (!x838.valid)
7288 {
7289 continue;
7290 }
7291 if (IKabs((
7292 (((1.17647058823529)
7293 * x833))
7294 + (((1.17647058823529)
7295 * x834))))
7296 < IKFAST_ATAN2_MAGTHRESH
7297 && IKabs((
7298 (x838.value)
7299 * (((0.2125)
7300 + (((-0.294117647058824)
7301 * x836))
7302 + x835
7303 + (((-2.58823529411765)
7304 * cj4
7305 * px
7306 * x834))
7307 + ((x836
7308 * x837))
7309 + (((-1.0)
7310 * x835
7311 * x837))))))
7312 < IKFAST_ATAN2_MAGTHRESH
7313 && IKabs(
7314 IKsqr((
7315 (((1.17647058823529)
7316 * x833))
7317 + (((1.17647058823529)
7318 * x834))))
7319 + IKsqr((
7320 (x838.value)
7321 * (((0.2125)
7322 + (((-0.294117647058824)
7323 * x836))
7324 + x835
7325 + (((-2.58823529411765)
7326 * cj4
7327 * px
7328 * x834))
7329 + ((x836
7330 * x837))
7331 + (((-1.0)
7332 * x835
7333 * x837))))))
7334 - 1)
7335 <= IKFAST_SINCOS_THRESH)
7336 continue;
7337 j6array[0] = IKatan2(
7338 ((((1.17647058823529)
7339 * x833))
7340 + (((1.17647058823529)
7341 * x834))),
7342 ((x838.value)
7343 * (((0.2125)
7344 + (((-0.294117647058824)
7345 * x836))
7346 + x835
7347 + (((-2.58823529411765)
7348 * cj4
7349 * px
7350 * x834))
7351 + ((x836
7352 * x837))
7353 + (((-1.0)
7354 * x835
7355 * x837))))));
7356 sj6array[0] = IKsin(
7357 j6array
7358 [0]);
7359 cj6array[0] = IKcos(
7360 j6array
7361 [0]);
7362 if (j6array
7363 [0]
7364 > IKPI)
7365 {
7366 j6array
7367 [0]
7368 -= IK2PI;
7369 }
7370 else if (
7371 j6array
7372 [0]
7373 < -IKPI)
7374 {
7375 j6array
7376 [0]
7377 += IK2PI;
7378 }
7379 j6valid[0]
7380 = true;
7381 for (
7382 int ij6
7383 = 0;
7384 ij6
7385 < 1;
7386 ++ij6)
7387 {
7388 if (!j6valid
7389 [ij6])
7390 {
7391 continue;
7392 }
7393 _ij6[0]
7394 = ij6;
7395 _ij6[1]
7396 = -1;
7397 for (
7398 int iij6
7399 = ij6
7400 + 1;
7401 iij6
7402 < 1;
7403 ++iij6)
7404 {
7405 if (j6valid
7406 [iij6]
7407 && IKabs(
7408 cj6array
7409 [ij6]
7410 - cj6array
7411 [iij6])
7412 < IKFAST_SOLUTION_THRESH
7413 && IKabs(
7414 sj6array
7415 [ij6]
7416 - sj6array
7417 [iij6])
7418 < IKFAST_SOLUTION_THRESH)
7419 {
7420 j6valid
7421 [iij6]
7422 = false;
7423 _ij6[1]
7424 = iij6;
7425 break;
7426 }
7427 }
7428 j6 = j6array
7429 [ij6];
7430 cj6 = cj6array
7431 [ij6];
7432 sj6 = sj6array
7433 [ij6];
7434 {
7435 IkReal evalcond
7436 [5];
7437 IkReal
7438 x839
7439 = IKcos(
7440 j6);
7441 IkReal
7442 x840
7443 = (cj4
7444 * px);
7445 IkReal
7446 x841
7447 = (x839
7448 * x840);
7449 IkReal
7450 x842
7451 = (py
7452 * sj4);
7453 IkReal
7454 x843
7455 = (x839
7456 * x842);
7457 IkReal
7458 x844
7459 = IKsin(
7460 j6);
7461 IkReal
7462 x845
7463 = (x840
7464 * x844);
7465 IkReal
7466 x846
7467 = (x842
7468 * x844);
7469 IkReal
7470 x847
7471 = px
7472 * px;
7473 IkReal
7474 x848
7475 = ((3.92156862745098)
7476 * x844);
7477 IkReal
7478 x849
7479 = ((0.588235294117647)
7480 * x839);
7481 IkReal
7482 x850
7483 = py
7484 * py;
7485 evalcond
7486 [0]
7487 = (x843
7488 + x841);
7489 evalcond
7490 [1]
7491 = ((-0.85)
7492 + x846
7493 + x845);
7494 evalcond
7495 [2]
7496 = ((((0.85)
7497 * x844))
7498 + (((-1.0)
7499 * x842))
7500 + (((-1.0)
7501 * x840)));
7502 evalcond
7503 [3]
7504 = ((((-1.0)
7505 * x847
7506 * x849))
7507 + (((-1.0)
7508 * x847
7509 * x848))
7510 + (((-0.425)
7511 * x839))
7512 + (((-1.0)
7513 * x848
7514 * x850))
7515 + (((-1.0)
7516 * x849
7517 * x850))
7518 + (((2.83333333333333)
7519 * x844)));
7520 evalcond
7521 [4]
7522 = ((-0.2125)
7523 + (((-1.0)
7524 * x850))
7525 + (((-0.09)
7526 * x843))
7527 + (((1.1)
7528 * x846))
7529 + (((1.1)
7530 * x845))
7531 + (((-1.0)
7532 * x847))
7533 + (((-0.09)
7534 * x841)));
7535 if (IKabs(
7536 evalcond
7537 [0])
7538 > IKFAST_EVALCOND_THRESH
7539 || IKabs(
7540 evalcond
7541 [1])
7542 > IKFAST_EVALCOND_THRESH
7543 || IKabs(
7544 evalcond
7545 [2])
7546 > IKFAST_EVALCOND_THRESH
7547 || IKabs(
7548 evalcond
7549 [3])
7550 > IKFAST_EVALCOND_THRESH
7551 || IKabs(
7552 evalcond
7553 [4])
7554 > IKFAST_EVALCOND_THRESH)
7555 {
7556 continue;
7557 }
7558 }
7559
7560 rotationfunction0(
7561 solutions);
7562 }
7563 }
7564 }
7565 }
7566 }
7567 } while (0);
7568 if (bgotonextstatement)
7569 {
7570 bool
7571 bgotonextstatement
7572 = true;
7573 do
7574 {
7575 if (1)
7576 {
7577 bgotonextstatement
7578 = false;
7579 continue; // branch miss [j6]
7580 }
7581 } while (0);
7582 if (bgotonextstatement)
7583 {
7584 }
7585 }
7586 }
7587 }
7588 else
7589 {
7590 {
7591 IkReal j6array[1],
7592 cj6array[1],
7593 sj6array[1];
7594 bool j6valid[1]
7595 = {false};
7596 _nj6 = 1;
7597 IkReal x851
7598 = (cj4 * px);
7599 IkReal x852
7600 = (py * sj4);
7601 IkReal x853
7602 = ((0.108264705882353)
7603 * cj9);
7604 IkReal x854
7605 = ((0.588235294117647)
7606 * pp);
7607 IkReal x855
7608 = (cj9 * pp);
7609 IkReal x856
7610 = (cj9 * sj9);
7611 IkReal x857
7612 = (pp * sj9);
7613 IkReal x858
7614 = cj9 * cj9;
7615 IkReal x859
7616 = ((1.0) * pz);
7617 CheckValue<IkReal> x860 = IKatan2WithCheck(
7618 IkReal((
7619 (-0.174204411764706)
7620 + (pz * pz)
7621 + (((-0.176470588235294)
7622 * x855))
7623 + (((-1.0)
7624 * (0.154566176470588)
7625 * cj9))
7626 + (((-1.0)
7627 * (0.323529411764706)
7628 * pp))
7629 + (((-0.0264705882352941)
7630 * x857))
7631 + (((-0.00487191176470588)
7632 * x856))
7633 + (((-1.0)
7634 * (0.0142530882352941)
7635 * sj9))
7636 + (((-0.0324794117647059)
7637 * x858)))),
7638 ((-0.830553921568627)
7639 + (((-0.396970588235294)
7640 * x858))
7641 + (((-1.0)
7642 * (0.0679544117647059)
7643 * sj9))
7644 + (((-1.0)
7645 * (1.18080882352941)
7646 * cj9))
7647 + (((0.176470588235294)
7648 * x857))
7649 + (((-1.0)
7650 * x852
7651 * x859))
7652 + (((2.15686274509804)
7653 * pp))
7654 + (((1.17647058823529)
7655 * x855))
7656 + (((-1.0)
7657 * x851
7658 * x859))
7659 + (((-0.0595455882352941)
7660 * x856))),
7661 IKFAST_ATAN2_MAGTHRESH);
7662 if (!x860.valid)
7663 {
7664 continue;
7665 }
7666 CheckValue<IkReal> x861 = IKPowWithIntegerCheck(
7667 IKsign((
7668 (((-1.0)
7669 * x851
7670 * x854))
7671 + (((-1.0)
7672 * (1.32323529411765)
7673 * cj9
7674 * pz))
7675 + (((3.92156862745098)
7676 * pp
7677 * pz))
7678 + (((-1.0)
7679 * (1.51009803921569)
7680 * pz))
7681 + (((-0.316735294117647)
7682 * x852))
7683 + (((-1.0)
7684 * x852
7685 * x853))
7686 + (((-0.316735294117647)
7687 * x851))
7688 + (((-1.0)
7689 * x851
7690 * x853))
7691 + (((-1.0)
7692 * x852
7693 * x854)))),
7694 -1);
7695 if (!x861.valid)
7696 {
7697 continue;
7698 }
7699 j6array[0]
7700 = ((-1.5707963267949)
7701 + (x860.value)
7702 + (((1.5707963267949)
7703 * (x861.value))));
7704 sj6array[0] = IKsin(
7705 j6array[0]);
7706 cj6array[0] = IKcos(
7707 j6array[0]);
7708 if (j6array[0]
7709 > IKPI)
7710 {
7711 j6array[0]
7712 -= IK2PI;
7713 }
7714 else if (
7715 j6array[0]
7716 < -IKPI)
7717 {
7718 j6array[0]
7719 += IK2PI;
7720 }
7721 j6valid[0] = true;
7722 for (int ij6 = 0;
7723 ij6 < 1;
7724 ++ij6)
7725 {
7726 if (!j6valid[ij6])
7727 {
7728 continue;
7729 }
7730 _ij6[0] = ij6;
7731 _ij6[1] = -1;
7732 for (int iij6
7733 = ij6 + 1;
7734 iij6 < 1;
7735 ++iij6)
7736 {
7737 if (j6valid
7738 [iij6]
7739 && IKabs(
7740 cj6array
7741 [ij6]
7742 - cj6array
7743 [iij6])
7744 < IKFAST_SOLUTION_THRESH
7745 && IKabs(
7746 sj6array
7747 [ij6]
7748 - sj6array
7749 [iij6])
7750 < IKFAST_SOLUTION_THRESH)
7751 {
7752 j6valid[iij6]
7753 = false;
7754 _ij6[1]
7755 = iij6;
7756 break;
7757 }
7758 }
7759 j6 = j6array[ij6];
7760 cj6 = cj6array
7761 [ij6];
7762 sj6 = sj6array
7763 [ij6];
7764 {
7765 IkReal
7766 evalcond[5];
7767 IkReal x862
7768 = ((0.3)
7769 * cj9);
7770 IkReal x863
7771 = ((0.045)
7772 * sj9);
7773 IkReal x864
7774 = IKcos(j6);
7775 IkReal x865
7776 = (pz
7777 * x864);
7778 IkReal x866
7779 = IKsin(j6);
7780 IkReal x867
7781 = (cj4
7782 * px);
7783 IkReal x868
7784 = (x866
7785 * x867);
7786 IkReal x869
7787 = (py
7788 * sj4);
7789 IkReal x870
7790 = (x866
7791 * x869);
7792 IkReal x871
7793 = ((0.045)
7794 * cj9);
7795 IkReal x872
7796 = ((0.3)
7797 * sj9);
7798 IkReal x873
7799 = (pz
7800 * x866);
7801 IkReal x874
7802 = (x864
7803 * x867);
7804 IkReal x875
7805 = (x864
7806 * x869);
7807 evalcond[0]
7808 = ((-0.55)
7809 + (((-1.0)
7810 * x862))
7811 + x868
7812 + x865
7813 + x870
7814 + (((-1.0)
7815 * x863)));
7816 evalcond[1]
7817 = ((0.045)
7818 + (((-1.0)
7819 * x873))
7820 + x872
7821 + x874
7822 + x875
7823 + (((-1.0)
7824 * x871)));
7825 evalcond[2]
7826 = ((((1.51009803921569)
7827 * x866))
7828 + (((-0.316735294117647)
7829 * x864))
7830 + pz
7831 + (((-3.92156862745098)
7832 * pp
7833 * x866))
7834 + (((-0.108264705882353)
7835 * cj9
7836 * x864))
7837 + (((-0.588235294117647)
7838 * pp
7839 * x864))
7840 + (((1.32323529411765)
7841 * cj9
7842 * x866)));
7843 evalcond[3]
7844 = ((((0.55)
7845 * x866))
7846 + (((-1.0)
7847 * x869))
7848 + (((-0.045)
7849 * x864))
7850 + (((-1.0)
7851 * x867))
7852 + (((-1.0)
7853 * x864
7854 * x872))
7855 + ((x862
7856 * x866))
7857 + ((x864
7858 * x871))
7859 + ((x863
7860 * x866)));
7861 evalcond[4]
7862 = ((-0.2125)
7863 + (((-0.09)
7864 * x875))
7865 + (((1.1)
7866 * x865))
7867 + (((0.09)
7868 * x873))
7869 + (((-0.09)
7870 * x874))
7871 + (((1.1)
7872 * x870))
7873 + (((1.1)
7874 * x868))
7875 + (((-1.0)
7876 * (1.0)
7877 * pp)));
7878 if (IKabs(
7879 evalcond
7880 [0])
7881 > IKFAST_EVALCOND_THRESH
7882 || IKabs(
7883 evalcond
7884 [1])
7885 > IKFAST_EVALCOND_THRESH
7886 || IKabs(
7887 evalcond
7888 [2])
7889 > IKFAST_EVALCOND_THRESH
7890 || IKabs(
7891 evalcond
7892 [3])
7893 > IKFAST_EVALCOND_THRESH
7894 || IKabs(
7895 evalcond
7896 [4])
7897 > IKFAST_EVALCOND_THRESH)
7898 {
7899 continue;
7900 }
7901 }
7902
7903 rotationfunction0(
7904 solutions);
7905 }
7906 }
7907 }
7908 }
7909 }
7910 else
7911 {
7912 {
7913 IkReal j6array[1],
7914 cj6array[1],
7915 sj6array[1];
7916 bool j6valid[1]
7917 = {false};
7918 _nj6 = 1;
7919 IkReal x876
7920 = ((0.045) * cj4
7921 * px);
7922 IkReal x877
7923 = ((0.045) * py
7924 * sj4);
7925 IkReal x878
7926 = ((0.3) * sj9);
7927 IkReal x879
7928 = (cj4 * px);
7929 IkReal x880
7930 = (py * sj4);
7931 IkReal x881
7932 = (cj9 * sj9);
7933 IkReal x882 = cj9 * cj9;
7934 IkReal x883
7935 = ((1.0) * pz);
7936 IkReal x884 = py * py;
7937 IkReal x885 = cj4 * cj4;
7938 CheckValue<IkReal> x886
7939 = IKatan2WithCheck(
7940 IkReal((
7941 (-0.03825)
7942 + (((0.027)
7943 * x882))
7944 + (((-0.087975)
7945 * x881))
7946 + (((-1.0)
7947 * (0.167025)
7948 * sj9))
7949 + (((-1.0)
7950 * x879
7951 * x883))
7952 + (((-1.0)
7953 * x880
7954 * x883))
7955 + (((0.01125)
7956 * cj9)))),
7957 ((-0.304525)
7958 + (((-1.0)
7959 * x884
7960 * x885))
7961 + (((2.0) * cj4
7962 * px
7963 * x880))
7964 + (((-1.0)
7965 * (0.0495)
7966 * sj9))
7967 + (((-0.087975)
7968 * x882))
7969 + ((x885
7970 * (px
7971 * px)))
7972 + x884
7973 + (((-1.0)
7974 * (0.33)
7975 * cj9))
7976 + (((-0.027)
7977 * x881))),
7978 IKFAST_ATAN2_MAGTHRESH);
7979 if (!x886.valid)
7980 {
7981 continue;
7982 }
7983 CheckValue<IkReal> x887
7984 = IKPowWithIntegerCheck(
7985 IKsign((
7986 (((-1.0)
7987 * x878
7988 * x879))
7989 + (((-1.0)
7990 * (0.55)
7991 * pz))
7992 + (((-1.0)
7993 * x877))
7994 + (((-1.0)
7995 * x876))
7996 + (((-1.0)
7997 * x878
7998 * x880))
7999 + ((cj9
8000 * x877))
8001 + ((cj9
8002 * x876))
8003 + (((-1.0)
8004 * (0.045)
8005 * pz
8006 * sj9))
8007 + (((-1.0)
8008 * (0.3)
8009 * cj9
8010 * pz)))),
8011 -1);
8012 if (!x887.valid)
8013 {
8014 continue;
8015 }
8016 j6array[0]
8017 = ((-1.5707963267949)
8018 + (x886.value)
8019 + (((1.5707963267949)
8020 * (x887.value))));
8021 sj6array[0]
8022 = IKsin(j6array[0]);
8023 cj6array[0]
8024 = IKcos(j6array[0]);
8025 if (j6array[0] > IKPI)
8026 {
8027 j6array[0] -= IK2PI;
8028 }
8029 else if (
8030 j6array[0] < -IKPI)
8031 {
8032 j6array[0] += IK2PI;
8033 }
8034 j6valid[0] = true;
8035 for (int ij6 = 0;
8036 ij6 < 1;
8037 ++ij6)
8038 {
8039 if (!j6valid[ij6])
8040 {
8041 continue;
8042 }
8043 _ij6[0] = ij6;
8044 _ij6[1] = -1;
8045 for (int iij6
8046 = ij6 + 1;
8047 iij6 < 1;
8048 ++iij6)
8049 {
8050 if (j6valid[iij6]
8051 && IKabs(
8052 cj6array
8053 [ij6]
8054 - cj6array
8055 [iij6])
8056 < IKFAST_SOLUTION_THRESH
8057 && IKabs(
8058 sj6array
8059 [ij6]
8060 - sj6array
8061 [iij6])
8062 < IKFAST_SOLUTION_THRESH)
8063 {
8064 j6valid[iij6]
8065 = false;
8066 _ij6[1] = iij6;
8067 break;
8068 }
8069 }
8070 j6 = j6array[ij6];
8071 cj6 = cj6array[ij6];
8072 sj6 = sj6array[ij6];
8073 {
8074 IkReal evalcond[5];
8075 IkReal x888
8076 = ((0.3) * cj9);
8077 IkReal x889
8078 = ((0.045)
8079 * sj9);
8080 IkReal x890
8081 = IKcos(j6);
8082 IkReal x891
8083 = (pz * x890);
8084 IkReal x892
8085 = IKsin(j6);
8086 IkReal x893
8087 = (cj4 * px);
8088 IkReal x894
8089 = (x892 * x893);
8090 IkReal x895
8091 = (py * sj4);
8092 IkReal x896
8093 = (x892 * x895);
8094 IkReal x897
8095 = ((0.045)
8096 * cj9);
8097 IkReal x898
8098 = ((0.3) * sj9);
8099 IkReal x899
8100 = (pz * x892);
8101 IkReal x900
8102 = (x890 * x893);
8103 IkReal x901
8104 = (x890 * x895);
8105 evalcond[0]
8106 = ((-0.55)
8107 + (((-1.0)
8108 * x888))
8109 + x894 + x896
8110 + x891
8111 + (((-1.0)
8112 * x889)));
8113 evalcond[1]
8114 = ((0.045)
8115 + (((-1.0)
8116 * x897))
8117 + x900 + x901
8118 + x898
8119 + (((-1.0)
8120 * x899)));
8121 evalcond[2]
8122 = ((((-0.588235294117647)
8123 * pp
8124 * x890))
8125 + (((-0.316735294117647)
8126 * x890))
8127 + (((1.32323529411765)
8128 * cj9
8129 * x892))
8130 + pz
8131 + (((-0.108264705882353)
8132 * cj9
8133 * x890))
8134 + (((-3.92156862745098)
8135 * pp
8136 * x892))
8137 + (((1.51009803921569)
8138 * x892)));
8139 evalcond[3]
8140 = ((((-1.0)
8141 * x893))
8142 + ((x890
8143 * x897))
8144 + (((0.55)
8145 * x892))
8146 + ((x888
8147 * x892))
8148 + (((-1.0)
8149 * x890
8150 * x898))
8151 + ((x889
8152 * x892))
8153 + (((-1.0)
8154 * x895))
8155 + (((-0.045)
8156 * x890)));
8157 evalcond[4]
8158 = ((-0.2125)
8159 + (((-0.09)
8160 * x900))
8161 + (((1.1)
8162 * x891))
8163 + (((0.09)
8164 * x899))
8165 + (((-0.09)
8166 * x901))
8167 + (((1.1)
8168 * x896))
8169 + (((-1.0)
8170 * (1.0)
8171 * pp))
8172 + (((1.1)
8173 * x894)));
8174 if (IKabs(
8175 evalcond[0])
8176 > IKFAST_EVALCOND_THRESH
8177 || IKabs(
8178 evalcond
8179 [1])
8180 > IKFAST_EVALCOND_THRESH
8181 || IKabs(
8182 evalcond
8183 [2])
8184 > IKFAST_EVALCOND_THRESH
8185 || IKabs(
8186 evalcond
8187 [3])
8188 > IKFAST_EVALCOND_THRESH
8189 || IKabs(
8190 evalcond
8191 [4])
8192 > IKFAST_EVALCOND_THRESH)
8193 {
8194 continue;
8195 }
8196 }
8197
8198 rotationfunction0(
8199 solutions);
8200 }
8201 }
8202 }
8203 }
8204 }
8205 else
8206 {
8207 {
8208 IkReal j6array[1],
8209 cj6array[1],
8210 sj6array[1];
8211 bool j6valid[1] = {false};
8212 _nj6 = 1;
8213 IkReal x902 = py * py;
8214 IkReal x903 = (py * sj4);
8215 IkReal x904 = cj4 * cj4;
8216 IkReal x905
8217 = ((0.045) * pz);
8218 IkReal x906 = (cj4 * px);
8219 IkReal x907 = ((0.3) * pz);
8220 IkReal x908
8221 = ((0.3) * cj4 * px);
8222 IkReal x909
8223 = ((0.045) * x906);
8224 IkReal x910
8225 = ((0.3) * py * sj4);
8226 IkReal x911
8227 = ((0.045) * x903);
8228 CheckValue<IkReal> x912
8229 = IKatan2WithCheck(
8230 IkReal((
8231 ((sj9 * x911))
8232 + ((sj9 * x909))
8233 + ((cj9 * x910))
8234 + ((sj9 * x907))
8235 + x905
8236 + (((0.55)
8237 * x906))
8238 + (((0.55)
8239 * x903))
8240 + ((cj9 * x908))
8241 + (((-1.0) * cj9
8242 * x905)))),
8243 ((((-1.0) * x911))
8244 + ((cj9 * x907))
8245 + ((cj9 * x911))
8246 + ((cj9 * x909))
8247 + (((-1.0) * sj9
8248 * x910))
8249 + (((0.55) * pz))
8250 + (((-1.0) * sj9
8251 * x908))
8252 + (((-1.0) * x909))
8253 + ((sj9 * x905))),
8254 IKFAST_ATAN2_MAGTHRESH);
8255 if (!x912.valid)
8256 {
8257 continue;
8258 }
8259 CheckValue<IkReal> x913
8260 = IKPowWithIntegerCheck(
8261 IKsign(
8262 ((((2.0) * cj4
8263 * px * x903))
8264 + (pz * pz)
8265 + x902
8266 + ((x904
8267 * (px
8268 * px)))
8269 + (((-1.0)
8270 * x902
8271 * x904)))),
8272 -1);
8273 if (!x913.valid)
8274 {
8275 continue;
8276 }
8277 j6array[0]
8278 = ((-1.5707963267949)
8279 + (x912.value)
8280 + (((1.5707963267949)
8281 * (x913.value))));
8282 sj6array[0]
8283 = IKsin(j6array[0]);
8284 cj6array[0]
8285 = IKcos(j6array[0]);
8286 if (j6array[0] > IKPI)
8287 {
8288 j6array[0] -= IK2PI;
8289 }
8290 else if (j6array[0] < -IKPI)
8291 {
8292 j6array[0] += IK2PI;
8293 }
8294 j6valid[0] = true;
8295 for (int ij6 = 0; ij6 < 1;
8296 ++ij6)
8297 {
8298 if (!j6valid[ij6])
8299 {
8300 continue;
8301 }
8302 _ij6[0] = ij6;
8303 _ij6[1] = -1;
8304 for (int iij6 = ij6 + 1;
8305 iij6 < 1;
8306 ++iij6)
8307 {
8308 if (j6valid[iij6]
8309 && IKabs(
8310 cj6array[ij6]
8311 - cj6array
8312 [iij6])
8313 < IKFAST_SOLUTION_THRESH
8314 && IKabs(
8315 sj6array[ij6]
8316 - sj6array
8317 [iij6])
8318 < IKFAST_SOLUTION_THRESH)
8319 {
8320 j6valid[iij6] = false;
8321 _ij6[1] = iij6;
8322 break;
8323 }
8324 }
8325 j6 = j6array[ij6];
8326 cj6 = cj6array[ij6];
8327 sj6 = sj6array[ij6];
8328 {
8329 IkReal evalcond[5];
8330 IkReal x914
8331 = ((0.3) * cj9);
8332 IkReal x915
8333 = ((0.045) * sj9);
8334 IkReal x916 = IKcos(j6);
8335 IkReal x917
8336 = (pz * x916);
8337 IkReal x918 = IKsin(j6);
8338 IkReal x919
8339 = (cj4 * px);
8340 IkReal x920
8341 = (x918 * x919);
8342 IkReal x921
8343 = (py * sj4);
8344 IkReal x922
8345 = (x918 * x921);
8346 IkReal x923
8347 = ((0.045) * cj9);
8348 IkReal x924
8349 = ((0.3) * sj9);
8350 IkReal x925
8351 = (pz * x918);
8352 IkReal x926
8353 = (x916 * x919);
8354 IkReal x927
8355 = (x916 * x921);
8356 evalcond[0]
8357 = ((-0.55)
8358 + (((-1.0)
8359 * x914))
8360 + x917
8361 + (((-1.0)
8362 * x915))
8363 + x922 + x920);
8364 evalcond[1]
8365 = ((0.045)
8366 + (((-1.0)
8367 * x925))
8368 + (((-1.0)
8369 * x923))
8370 + x927 + x924
8371 + x926);
8372 evalcond[2]
8373 = ((((-0.316735294117647)
8374 * x916))
8375 + (((-3.92156862745098)
8376 * pp * x918))
8377 + (((-0.588235294117647)
8378 * pp * x916))
8379 + pz
8380 + (((-0.108264705882353)
8381 * cj9
8382 * x916))
8383 + (((1.32323529411765)
8384 * cj9
8385 * x918))
8386 + (((1.51009803921569)
8387 * x918)));
8388 evalcond[3]
8389 = ((((0.55) * x918))
8390 + (((-0.045)
8391 * x916))
8392 + ((x914 * x918))
8393 + ((x915 * x918))
8394 + (((-1.0)
8395 * x921))
8396 + ((x916 * x923))
8397 + (((-1.0)
8398 * x919))
8399 + (((-1.0) * x916
8400 * x924)));
8401 evalcond[4]
8402 = ((-0.2125)
8403 + (((-0.09)
8404 * x927))
8405 + (((1.1)
8406 * x920))
8407 + (((0.09)
8408 * x925))
8409 + (((-0.09)
8410 * x926))
8411 + (((1.1)
8412 * x917))
8413 + (((1.1)
8414 * x922))
8415 + (((-1.0)
8416 * (1.0)
8417 * pp)));
8418 if (IKabs(evalcond[0])
8419 > IKFAST_EVALCOND_THRESH
8420 || IKabs(
8421 evalcond[1])
8422 > IKFAST_EVALCOND_THRESH
8423 || IKabs(
8424 evalcond[2])
8425 > IKFAST_EVALCOND_THRESH
8426 || IKabs(
8427 evalcond[3])
8428 > IKFAST_EVALCOND_THRESH
8429 || IKabs(
8430 evalcond[4])
8431 > IKFAST_EVALCOND_THRESH)
8432 {
8433 continue;
8434 }
8435 }
8436
8437 rotationfunction0(
8438 solutions);
8439 }
8440 }
8441 }
8442 }
8443 }
8444 } while (0);
8445 if (bgotonextstatement)
8446 {
8447 bool bgotonextstatement = true;
8448 do
8449 {
8450 if (1)
8451 {
8452 bgotonextstatement = false;
8453 continue; // branch miss [j6]
8454 }
8455 } while (0);
8456 if (bgotonextstatement)
8457 {
8458 }
8459 }
8460 }
8461 }
8462 }
8463 else
8464 {
8465 {
8466 IkReal j6array[1], cj6array[1],
8467 sj6array[1];
8468 bool j6valid[1] = {false};
8469 _nj6 = 1;
8470 IkReal x928 = (pz * sj8);
8471 IkReal x929 = ((0.3) * cj9);
8472 IkReal x930 = ((0.045) * sj9);
8473 IkReal x931 = (cj4 * px);
8474 IkReal x932 = ((0.045) * cj8 * sj8);
8475 IkReal x933 = (x931 * x932);
8476 IkReal x934 = (py * sj4);
8477 IkReal x935 = (x932 * x934);
8478 IkReal x936 = ((0.3) * cj8 * sj8 * sj9);
8479 IkReal x937 = ((0.55) * cj8);
8480 IkReal x938 = (cj4 * py);
8481 IkReal x939 = (px * sj4);
8482 IkReal x940 = (cj4 * cj8 * py);
8483 IkReal x941 = ((1.0) * pz * sj8);
8484 IkReal x942 = (cj8 * px * sj4);
8485 IkReal x943 = cj8 * cj8;
8486 IkReal x944 = ((0.045) * x943);
8487 IkReal x945 = (x938 * x944);
8488 IkReal x946 = (x939 * x944);
8489 IkReal x947 = ((0.3) * sj9 * x943);
8490 CheckValue<IkReal> x948
8491 = IKPowWithIntegerCheck(
8492 IKsign(
8493 ((((-0.55) * x928))
8494 + (((-1.0) * cj9 * x933))
8495 + (((-1.0) * cj9 * x935))
8496 + (((-1.0) * x928 * x929))
8497 + ((x931 * x936))
8498 + (((-1.0) * x928 * x930))
8499 + x933 + x935
8500 + ((x934 * x936)))),
8501 -1);
8502 if (!x948.valid)
8503 {
8504 continue;
8505 }
8506 CheckValue<IkReal> x949
8507 = IKatan2WithCheck(
8508 IkReal(
8509 ((((-1.0) * x937 * x939))
8510 + ((x937 * x938))
8511 + ((x930 * x940))
8512 + (((-1.0) * x930 * x942))
8513 + (((-1.0) * x931 * x941))
8514 + (((-1.0) * x934 * x941))
8515 + (((-1.0) * x929 * x942))
8516 + ((x929 * x940)))),
8517 (((cj9 * x946))
8518 + (((-1.0) * (1.0) * sj8
8519 * (pz * pz)))
8520 + ((x938 * x947))
8521 + (((-1.0) * x939 * x947))
8522 + (((-1.0) * cj9 * x945))
8523 + (((-1.0) * x946)) + x945),
8524 IKFAST_ATAN2_MAGTHRESH);
8525 if (!x949.valid)
8526 {
8527 continue;
8528 }
8529 j6array[0]
8530 = ((-1.5707963267949)
8531 + (((1.5707963267949)
8532 * (x948.value)))
8533 + (x949.value));
8534 sj6array[0] = IKsin(j6array[0]);
8535 cj6array[0] = IKcos(j6array[0]);
8536 if (j6array[0] > IKPI)
8537 {
8538 j6array[0] -= IK2PI;
8539 }
8540 else if (j6array[0] < -IKPI)
8541 {
8542 j6array[0] += IK2PI;
8543 }
8544 j6valid[0] = true;
8545 for (int ij6 = 0; ij6 < 1; ++ij6)
8546 {
8547 if (!j6valid[ij6])
8548 {
8549 continue;
8550 }
8551 _ij6[0] = ij6;
8552 _ij6[1] = -1;
8553 for (int iij6 = ij6 + 1; iij6 < 1;
8554 ++iij6)
8555 {
8556 if (j6valid[iij6]
8557 && IKabs(
8558 cj6array[ij6]
8559 - cj6array[iij6])
8560 < IKFAST_SOLUTION_THRESH
8561 && IKabs(
8562 sj6array[ij6]
8563 - sj6array[iij6])
8564 < IKFAST_SOLUTION_THRESH)
8565 {
8566 j6valid[iij6] = false;
8567 _ij6[1] = iij6;
8568 break;
8569 }
8570 }
8571 j6 = j6array[ij6];
8572 cj6 = cj6array[ij6];
8573 sj6 = sj6array[ij6];
8574 {
8575 IkReal evalcond[6];
8576 IkReal x950 = ((0.3) * cj9);
8577 IkReal x951 = ((0.045) * sj9);
8578 IkReal x952 = IKcos(j6);
8579 IkReal x953 = (pz * x952);
8580 IkReal x954 = IKsin(j6);
8581 IkReal x955 = (cj4 * px * x954);
8582 IkReal x956 = (py * sj4);
8583 IkReal x957 = (x954 * x956);
8584 IkReal x958 = (px * sj4);
8585 IkReal x959 = ((1.0) * cj4 * py);
8586 IkReal x960 = (cj4 * sj8);
8587 IkReal x961 = ((0.045) * cj8);
8588 IkReal x962 = ((0.045) * cj9);
8589 IkReal x963 = (cj8 * x954);
8590 IkReal x964 = ((0.3) * sj9);
8591 IkReal x965 = (sj8 * x958);
8592 IkReal x966 = (pz * x963);
8593 IkReal x967
8594 = (px * (((1.0) * cj4)));
8595 IkReal x968 = (cj8 * x952);
8596 IkReal x969 = ((1.0) * x956);
8597 IkReal x970 = ((0.09) * cj8 * x952);
8598 evalcond[0]
8599 = ((-0.55) + x953 + x955 + x957
8600 + (((-1.0) * x951))
8601 + (((-1.0) * x950)));
8602 evalcond[1]
8603 = ((((-1.0) * cj8 * x959))
8604 + ((cj8 * x958))
8605 + (((-1.0) * pz * sj8
8606 * x954))
8607 + ((sj8 * x952 * x956))
8608 + ((px * x952 * x960)));
8609 evalcond[2]
8610 = ((((-0.55) * x952)) + pz
8611 + (((-1.0) * x962 * x963))
8612 + (((-1.0) * x951 * x952))
8613 + ((x954 * x961))
8614 + ((x963 * x964))
8615 + (((-1.0) * x950 * x952)));
8616 evalcond[3]
8617 = ((0.045)
8618 + (((-1.0) * x968 * x969))
8619 + (((-1.0) * sj8 * x959))
8620 + x966 + x964 + x965
8621 + (((-1.0) * x962))
8622 + (((-1.0) * x967 * x968)));
8623 evalcond[4]
8624 = (((x952 * x961))
8625 + ((x964 * x968))
8626 + (((0.55) * x954))
8627 + (((-1.0) * x962 * x968))
8628 + (((-1.0) * x969))
8629 + ((x951 * x954))
8630 + ((x950 * x954))
8631 + (((-1.0) * x967)));
8632 evalcond[5]
8633 = ((-0.2125) + (((1.1) * x957))
8634 + (((1.1) * x953))
8635 + (((-0.09) * x965))
8636 + (((1.1) * x955))
8637 + (((0.09) * py * x960))
8638 + ((cj4 * px * x970))
8639 + ((x956 * x970))
8640 + (((-0.09) * x966))
8641 + (((-1.0) * (1.0) * pp)));
8642 if (IKabs(evalcond[0])
8643 > IKFAST_EVALCOND_THRESH
8644 || IKabs(evalcond[1])
8645 > IKFAST_EVALCOND_THRESH
8646 || IKabs(evalcond[2])
8647 > IKFAST_EVALCOND_THRESH
8648 || IKabs(evalcond[3])
8649 > IKFAST_EVALCOND_THRESH
8650 || IKabs(evalcond[4])
8651 > IKFAST_EVALCOND_THRESH
8652 || IKabs(evalcond[5])
8653 > IKFAST_EVALCOND_THRESH)
8654 {
8655 continue;
8656 }
8657 }
8658
8659 rotationfunction0(solutions);
8660 }
8661 }
8662 }
8663 }
8664 }
8665 else
8666 {
8667 {
8668 IkReal j6array[1], cj6array[1], sj6array[1];
8669 bool j6valid[1] = {false};
8670 _nj6 = 1;
8671 IkReal x971 = py * py;
8672 IkReal x972 = (sj8 * x971);
8673 IkReal x973 = (cj4 * px * sj8);
8674 IkReal x974 = cj4 * cj4;
8675 IkReal x975 = px * px;
8676 IkReal x976 = ((0.55) * sj8);
8677 IkReal x977 = (cj8 * px);
8678 IkReal x978 = ((0.3) * cj9);
8679 IkReal x979 = ((0.045) * sj9);
8680 IkReal x980 = (py * sj4 * sj8);
8681 IkReal x981 = (pz * sj8);
8682 IkReal x982 = (cj4 * cj8 * sj4);
8683 CheckValue<IkReal> x983 = IKatan2WithCheck(
8684 IkReal(
8685 (((x973 * x978)) + ((x973 * x979))
8686 + ((pz * sj4 * x977))
8687 + (((-1.0) * cj4 * cj8 * py * pz))
8688 + ((cj4 * px * x976))
8689 + ((x979 * x980)) + ((x978 * x980))
8690 + ((py * sj4 * x976)))),
8691 (((pz * x976))
8692 + (((2.0) * py * x974 * x977))
8693 + ((x979 * x981)) + ((x971 * x982))
8694 + (((-1.0) * py * x977))
8695 + ((x978 * x981))
8696 + (((-1.0) * x975 * x982))),
8697 IKFAST_ATAN2_MAGTHRESH);
8698 if (!x983.valid)
8699 {
8700 continue;
8701 }
8702 CheckValue<IkReal> x984
8703 = IKPowWithIntegerCheck(
8704 IKsign(
8705 ((((2.0) * py * sj4 * x973))
8706 + ((sj8 * (pz * pz))) + x972
8707 + (((-1.0) * x972 * x974))
8708 + ((sj8 * x974 * x975)))),
8709 -1);
8710 if (!x984.valid)
8711 {
8712 continue;
8713 }
8714 j6array[0]
8715 = ((-1.5707963267949) + (x983.value)
8716 + (((1.5707963267949)
8717 * (x984.value))));
8718 sj6array[0] = IKsin(j6array[0]);
8719 cj6array[0] = IKcos(j6array[0]);
8720 if (j6array[0] > IKPI)
8721 {
8722 j6array[0] -= IK2PI;
8723 }
8724 else if (j6array[0] < -IKPI)
8725 {
8726 j6array[0] += IK2PI;
8727 }
8728 j6valid[0] = true;
8729 for (int ij6 = 0; ij6 < 1; ++ij6)
8730 {
8731 if (!j6valid[ij6])
8732 {
8733 continue;
8734 }
8735 _ij6[0] = ij6;
8736 _ij6[1] = -1;
8737 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
8738 {
8739 if (j6valid[iij6]
8740 && IKabs(
8741 cj6array[ij6]
8742 - cj6array[iij6])
8743 < IKFAST_SOLUTION_THRESH
8744 && IKabs(
8745 sj6array[ij6]
8746 - sj6array[iij6])
8747 < IKFAST_SOLUTION_THRESH)
8748 {
8749 j6valid[iij6] = false;
8750 _ij6[1] = iij6;
8751 break;
8752 }
8753 }
8754 j6 = j6array[ij6];
8755 cj6 = cj6array[ij6];
8756 sj6 = sj6array[ij6];
8757 {
8758 IkReal evalcond[6];
8759 IkReal x985 = ((0.3) * cj9);
8760 IkReal x986 = ((0.045) * sj9);
8761 IkReal x987 = IKcos(j6);
8762 IkReal x988 = (pz * x987);
8763 IkReal x989 = IKsin(j6);
8764 IkReal x990 = (cj4 * px * x989);
8765 IkReal x991 = (py * sj4);
8766 IkReal x992 = (x989 * x991);
8767 IkReal x993 = (px * sj4);
8768 IkReal x994 = ((1.0) * cj4 * py);
8769 IkReal x995 = (cj4 * sj8);
8770 IkReal x996 = ((0.045) * cj8);
8771 IkReal x997 = ((0.045) * cj9);
8772 IkReal x998 = (cj8 * x989);
8773 IkReal x999 = ((0.3) * sj9);
8774 IkReal x1000 = (sj8 * x993);
8775 IkReal x1001 = (pz * x998);
8776 IkReal x1002 = (px * (((1.0) * cj4)));
8777 IkReal x1003 = (cj8 * x987);
8778 IkReal x1004 = ((1.0) * x991);
8779 IkReal x1005 = ((0.09) * cj8 * x987);
8780 evalcond[0]
8781 = ((-0.55) + (((-1.0) * x985))
8782 + (((-1.0) * x986)) + x988 + x990
8783 + x992);
8784 evalcond[1]
8785 = ((((-1.0) * cj8 * x994))
8786 + (((-1.0) * pz * sj8 * x989))
8787 + ((px * x987 * x995))
8788 + ((sj8 * x987 * x991))
8789 + ((cj8 * x993)));
8790 evalcond[2]
8791 = ((((-1.0) * x986 * x987)) + pz
8792 + (((-0.55) * x987))
8793 + (((-1.0) * x997 * x998))
8794 + ((x998 * x999))
8795 + (((-1.0) * x985 * x987))
8796 + ((x989 * x996)));
8797 evalcond[3]
8798 = ((0.045) + (((-1.0) * sj8 * x994))
8799 + (((-1.0) * x997)) + x1000
8800 + x1001
8801 + (((-1.0) * x1002 * x1003))
8802 + (((-1.0) * x1003 * x1004))
8803 + x999);
8804 evalcond[4]
8805 = ((((-1.0) * x1003 * x997))
8806 + ((x985 * x989))
8807 + ((x1003 * x999))
8808 + (((0.55) * x989))
8809 + (((-1.0) * x1004))
8810 + (((-1.0) * x1002))
8811 + ((x986 * x989))
8812 + ((x987 * x996)));
8813 evalcond[5]
8814 = ((-0.2125) + (((1.1) * x990))
8815 + ((cj4 * px * x1005))
8816 + ((x1005 * x991))
8817 + (((-0.09) * x1001))
8818 + (((0.09) * py * x995))
8819 + (((-0.09) * x1000))
8820 + (((-1.0) * (1.0) * pp))
8821 + (((1.1) * x988))
8822 + (((1.1) * x992)));
8823 if (IKabs(evalcond[0])
8824 > IKFAST_EVALCOND_THRESH
8825 || IKabs(evalcond[1])
8826 > IKFAST_EVALCOND_THRESH
8827 || IKabs(evalcond[2])
8828 > IKFAST_EVALCOND_THRESH
8829 || IKabs(evalcond[3])
8830 > IKFAST_EVALCOND_THRESH
8831 || IKabs(evalcond[4])
8832 > IKFAST_EVALCOND_THRESH
8833 || IKabs(evalcond[5])
8834 > IKFAST_EVALCOND_THRESH)
8835 {
8836 continue;
8837 }
8838 }
8839
8840 rotationfunction0(solutions);
8841 }
8842 }
8843 }
8844 }
8845 }
8846 else
8847 {
8848 {
8849 IkReal j6array[1], cj6array[1], sj6array[1];
8850 bool j6valid[1] = {false};
8851 _nj6 = 1;
8852 IkReal x1006 = (cj4 * px);
8853 IkReal x1007 = ((0.045) * pz);
8854 IkReal x1008 = (py * sj4);
8855 IkReal x1009 = ((0.3) * cj9);
8856 IkReal x1010 = ((0.045) * sj9);
8857 IkReal x1011 = (cj8 * cj9);
8858 IkReal x1012 = (cj8 * sj9);
8859 IkReal x1013 = cj9 * cj9;
8860 IkReal x1014 = ((1.0) * pz);
8861 CheckValue<IkReal> x1015 = IKatan2WithCheck(
8862 IkReal(
8863 ((-0.304525)
8864 + (((-1.0) * (0.027) * cj9 * sj9))
8865 + (((-1.0) * (0.0495) * sj9))
8866 + (pz * pz) + (((-0.087975) * x1013))
8867 + (((-1.0) * (0.33) * cj9)))),
8868 ((((-0.087975) * sj9 * x1011))
8869 + (((-1.0) * x1008 * x1014))
8870 + (((-1.0) * x1006 * x1014))
8871 + (((0.027) * cj8 * x1013))
8872 + (((0.01125) * x1011))
8873 + (((-0.167025) * x1012))
8874 + (((-1.0) * (0.03825) * cj8))),
8875 IKFAST_ATAN2_MAGTHRESH);
8876 if (!x1015.valid)
8877 {
8878 continue;
8879 }
8880 CheckValue<IkReal> x1016
8881 = IKPowWithIntegerCheck(
8882 IKsign(
8883 ((((-1.0) * cj8 * x1007))
8884 + ((x1007 * x1011))
8885 + (((-0.3) * pz * x1012))
8886 + (((-0.55) * x1008))
8887 + (((-1.0) * x1008 * x1010))
8888 + (((-0.55) * x1006))
8889 + (((-1.0) * x1008 * x1009))
8890 + (((-1.0) * x1006 * x1009))
8891 + (((-1.0) * x1006 * x1010)))),
8892 -1);
8893 if (!x1016.valid)
8894 {
8895 continue;
8896 }
8897 j6array[0]
8898 = ((-1.5707963267949) + (x1015.value)
8899 + (((1.5707963267949) * (x1016.value))));
8900 sj6array[0] = IKsin(j6array[0]);
8901 cj6array[0] = IKcos(j6array[0]);
8902 if (j6array[0] > IKPI)
8903 {
8904 j6array[0] -= IK2PI;
8905 }
8906 else if (j6array[0] < -IKPI)
8907 {
8908 j6array[0] += IK2PI;
8909 }
8910 j6valid[0] = true;
8911 for (int ij6 = 0; ij6 < 1; ++ij6)
8912 {
8913 if (!j6valid[ij6])
8914 {
8915 continue;
8916 }
8917 _ij6[0] = ij6;
8918 _ij6[1] = -1;
8919 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
8920 {
8921 if (j6valid[iij6]
8922 && IKabs(cj6array[ij6] - cj6array[iij6])
8923 < IKFAST_SOLUTION_THRESH
8924 && IKabs(sj6array[ij6] - sj6array[iij6])
8925 < IKFAST_SOLUTION_THRESH)
8926 {
8927 j6valid[iij6] = false;
8928 _ij6[1] = iij6;
8929 break;
8930 }
8931 }
8932 j6 = j6array[ij6];
8933 cj6 = cj6array[ij6];
8934 sj6 = sj6array[ij6];
8935 {
8936 IkReal evalcond[6];
8937 IkReal x1017 = ((0.3) * cj9);
8938 IkReal x1018 = ((0.045) * sj9);
8939 IkReal x1019 = IKcos(j6);
8940 IkReal x1020 = (pz * x1019);
8941 IkReal x1021 = IKsin(j6);
8942 IkReal x1022 = (cj4 * px * x1021);
8943 IkReal x1023 = (py * sj4);
8944 IkReal x1024 = (x1021 * x1023);
8945 IkReal x1025 = (px * sj4);
8946 IkReal x1026 = ((1.0) * cj4 * py);
8947 IkReal x1027 = (cj4 * sj8);
8948 IkReal x1028 = ((0.045) * cj8);
8949 IkReal x1029 = ((0.045) * cj9);
8950 IkReal x1030 = (cj8 * x1021);
8951 IkReal x1031 = ((0.3) * sj9);
8952 IkReal x1032 = (sj8 * x1025);
8953 IkReal x1033 = (pz * x1030);
8954 IkReal x1034 = (px * (((1.0) * cj4)));
8955 IkReal x1035 = (cj8 * x1019);
8956 IkReal x1036 = ((1.0) * x1023);
8957 IkReal x1037 = ((0.09) * cj8 * x1019);
8958 evalcond[0]
8959 = ((-0.55) + x1020 + x1022 + x1024
8960 + (((-1.0) * x1018))
8961 + (((-1.0) * x1017)));
8962 evalcond[1]
8963 = ((((-1.0) * cj8 * x1026))
8964 + (((-1.0) * pz * sj8 * x1021))
8965 + ((px * x1019 * x1027))
8966 + ((cj8 * x1025))
8967 + ((sj8 * x1019 * x1023)));
8968 evalcond[2]
8969 = ((((-1.0) * x1018 * x1019))
8970 + (((-1.0) * x1029 * x1030))
8971 + ((x1030 * x1031))
8972 + ((x1021 * x1028))
8973 + (((-0.55) * x1019)) + pz
8974 + (((-1.0) * x1017 * x1019)));
8975 evalcond[3]
8976 = ((0.045) + (((-1.0) * x1029))
8977 + (((-1.0) * x1035 * x1036))
8978 + (((-1.0) * x1034 * x1035)) + x1031
8979 + x1033 + x1032
8980 + (((-1.0) * sj8 * x1026)));
8981 evalcond[4]
8982 = (((x1031 * x1035))
8983 + (((0.55) * x1021))
8984 + (((-1.0) * x1036))
8985 + ((x1019 * x1028))
8986 + ((x1017 * x1021))
8987 + ((x1018 * x1021))
8988 + (((-1.0) * x1029 * x1035))
8989 + (((-1.0) * x1034)));
8990 evalcond[5]
8991 = ((-0.2125) + ((cj4 * px * x1037))
8992 + ((x1023 * x1037))
8993 + (((1.1) * x1024))
8994 + (((-0.09) * x1032))
8995 + (((0.09) * py * x1027))
8996 + (((1.1) * x1020))
8997 + (((-1.0) * (1.0) * pp))
8998 + (((1.1) * x1022))
8999 + (((-0.09) * x1033)));
9000 if (IKabs(evalcond[0])
9001 > IKFAST_EVALCOND_THRESH
9002 || IKabs(evalcond[1])
9003 > IKFAST_EVALCOND_THRESH
9004 || IKabs(evalcond[2])
9005 > IKFAST_EVALCOND_THRESH
9006 || IKabs(evalcond[3])
9007 > IKFAST_EVALCOND_THRESH
9008 || IKabs(evalcond[4])
9009 > IKFAST_EVALCOND_THRESH
9010 || IKabs(evalcond[5])
9011 > IKFAST_EVALCOND_THRESH)
9012 {
9013 continue;
9014 }
9015 }
9016
9017 rotationfunction0(solutions);
9018 }
9019 }
9020 }
9021 }
9022 }
9023 }
9024 }
9025 }
9026 }
9027 else
9028 {
9029 {
9030 IkReal j6array[2], cj6array[2], sj6array[2];
9031 bool j6valid[2] = {false};
9032 _nj6 = 2;
9033 IkReal x1038
9034 = ((-0.55) + (((-1.0) * (0.3) * cj9))
9035 + (((-1.0) * (0.045) * sj9)));
9036 IkReal x1039 = ((0.045) * cj8);
9037 IkReal x1040
9038 = ((((-1.0) * cj9 * x1039)) + x1039
9039 + (((0.3) * cj8 * sj9)));
9040 CheckValue<IkReal> x1043 = IKatan2WithCheck(
9041 IkReal(x1038), x1040, IKFAST_ATAN2_MAGTHRESH);
9042 if (!x1043.valid)
9043 {
9044 continue;
9045 }
9046 IkReal x1041 = ((-1.0) * (x1043.value));
9047 if ((((x1040 * x1040) + (x1038 * x1038))) < -0.00001)
9048 continue;
9049 CheckValue<IkReal> x1044 = IKPowWithIntegerCheck(
9050 IKabs(IKsqrt(((x1040 * x1040) + (x1038 * x1038)))), -1);
9051 if (!x1044.valid)
9052 {
9053 continue;
9054 }
9055 if (((pz * (x1044.value))) < -1 - IKFAST_SINCOS_THRESH
9056 || ((pz * (x1044.value))) > 1 + IKFAST_SINCOS_THRESH)
9057 continue;
9058 IkReal x1042 = IKasin((pz * (x1044.value)));
9059 j6array[0] = ((((-1.0) * x1042)) + x1041);
9060 sj6array[0] = IKsin(j6array[0]);
9061 cj6array[0] = IKcos(j6array[0]);
9062 j6array[1] = ((3.14159265358979) + x1042 + x1041);
9063 sj6array[1] = IKsin(j6array[1]);
9064 cj6array[1] = IKcos(j6array[1]);
9065 if (j6array[0] > IKPI)
9066 {
9067 j6array[0] -= IK2PI;
9068 }
9069 else if (j6array[0] < -IKPI)
9070 {
9071 j6array[0] += IK2PI;
9072 }
9073 j6valid[0] = true;
9074 if (j6array[1] > IKPI)
9075 {
9076 j6array[1] -= IK2PI;
9077 }
9078 else if (j6array[1] < -IKPI)
9079 {
9080 j6array[1] += IK2PI;
9081 }
9082 j6valid[1] = true;
9083 for (int ij6 = 0; ij6 < 2; ++ij6)
9084 {
9085 if (!j6valid[ij6])
9086 {
9087 continue;
9088 }
9089 _ij6[0] = ij6;
9090 _ij6[1] = -1;
9091 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
9092 {
9093 if (j6valid[iij6]
9094 && IKabs(cj6array[ij6] - cj6array[iij6])
9095 < IKFAST_SOLUTION_THRESH
9096 && IKabs(sj6array[ij6] - sj6array[iij6])
9097 < IKFAST_SOLUTION_THRESH)
9098 {
9099 j6valid[iij6] = false;
9100 _ij6[1] = iij6;
9101 break;
9102 }
9103 }
9104 j6 = j6array[ij6];
9105 cj6 = cj6array[ij6];
9106 sj6 = sj6array[ij6];
9107
9108 {
9109 IkReal j4eval[2];
9110 IkReal x1045
9111 = ((((-1.0) * (1.0) * sj6 * (pz * pz)))
9112 + ((pp * sj6)));
9113 j4eval[0] = x1045;
9114 j4eval[1] = IKsign(x1045);
9115 if (IKabs(j4eval[0]) < 0.0000010000000000
9116 || IKabs(j4eval[1]) < 0.0000010000000000)
9117 {
9118 {
9119 IkReal j4eval[2];
9120 IkReal x1046 = (cj8 * sj6);
9121 IkReal x1047
9122 = (((x1046 * (pz * pz)))
9123 + (((-1.0) * pp * x1046)));
9124 j4eval[0] = x1047;
9125 j4eval[1] = IKsign(x1047);
9126 if (IKabs(j4eval[0]) < 0.0000010000000000
9127 || IKabs(j4eval[1]) < 0.0000010000000000)
9128 {
9129 {
9130 IkReal j4eval[2];
9131 IkReal x1048
9132 = (pp + (((-1.0) * (1.0) * (pz * pz))));
9133 j4eval[0] = x1048;
9134 j4eval[1] = IKsign(x1048);
9135 if (IKabs(j4eval[0]) < 0.0000010000000000
9136 || IKabs(j4eval[1]) < 0.0000010000000000)
9137 {
9138 {
9139 IkReal evalcond[4];
9140 bool bgotonextstatement = true;
9141 do
9142 {
9143 evalcond[0]
9144 = ((-3.14159265358979)
9145 + (IKfmod(
9146 ((3.14159265358979)
9147 + (IKabs(
9148 ((-1.5707963267949)
9149 + j8)))),
9150 6.28318530717959)));
9151 evalcond[1]
9152 = ((0.39655) + (((0.0765) * sj9))
9153 + (((0.32595) * cj9))
9154 + (((-1.0) * (1.0) * pp)));
9155 evalcond[2]
9156 = ((((-1.0) * (0.3) * cj6 * cj9))
9157 + (((-1.0) * (0.55) * cj6)) + pz
9158 + (((-1.0) * (0.045) * cj6
9159 * sj9)));
9160 if (IKabs(evalcond[0])
9161 < 0.0000010000000000
9162 && IKabs(evalcond[1])
9163 < 0.0000010000000000
9164 && IKabs(evalcond[2])
9165 < 0.0000010000000000)
9166 {
9167 bgotonextstatement = false;
9168 {
9169 IkReal j4eval[3];
9170 sj8 = 1.0;
9171 cj8 = 0;
9172 j8 = 1.5707963267949;
9173 IkReal x1049
9174 = ((((-1.0) * (1.0) * cj6
9175 * (pz * pz)))
9176 + ((cj6 * pp)));
9177 IkReal x1050
9178 = ((1.51009803921569) * cj6);
9179 IkReal x1051 = (pz * sj6);
9180 IkReal x1052
9181 = ((1.32323529411765) * cj6
9182 * cj9);
9183 IkReal x1053
9184 = ((3.92156862745098) * cj6 * pp);
9185 j4eval[0] = x1049;
9186 j4eval[1] = IKsign(x1049);
9187 j4eval[2]
9188 = ((IKabs(
9189 (((py * x1053))
9190 + (((-1.0) * py * x1052))
9191 + (((-1.0) * py * x1050))
9192 + ((px * x1051)))))
9193 + (IKabs(
9194 (((px * x1052))
9195 + ((px * x1050))
9196 + ((py * x1051))
9197 + (((-1.0) * px
9198 * x1053))))));
9199 if (IKabs(j4eval[0])
9200 < 0.0000010000000000
9201 || IKabs(j4eval[1])
9202 < 0.0000010000000000
9203 || IKabs(j4eval[2])
9204 < 0.0000010000000000)
9205 {
9206 {
9207 IkReal j4eval[3];
9208 sj8 = 1.0;
9209 cj8 = 0;
9210 j8 = 1.5707963267949;
9211 IkReal x1054 = (cj6 * pp);
9212 IkReal x1055 = (cj6 * (pz * pz));
9213 IkReal x1056 = ((0.2125) * cj6);
9214 IkReal x1057 = ((1.1) * pz);
9215 IkReal x1058
9216 = ((0.09) * pz * sj6);
9217 j4eval[0]
9218 = ((((-1.0) * x1055))
9219 + x1054);
9220 j4eval[1] = IKsign(
9221 ((((-0.09) * x1055))
9222 + (((0.09) * x1054))));
9223 j4eval[2]
9224 = ((IKabs(
9225 (((py * x1058))
9226 + ((px * x1057))
9227 + (((-1.0) * px
9228 * x1054))
9229 + (((-1.0) * px
9230 * x1056)))))
9231 + (IKabs(
9232 (((py * x1056))
9233 + ((py * x1054))
9234 + (((-1.0) * py
9235 * x1057))
9236 + ((px * x1058))))));
9237 if (IKabs(j4eval[0])
9238 < 0.0000010000000000
9239 || IKabs(j4eval[1])
9240 < 0.0000010000000000
9241 || IKabs(j4eval[2])
9242 < 0.0000010000000000)
9243 {
9244 {
9245 IkReal j4eval[3];
9246 sj8 = 1.0;
9247 cj8 = 0;
9248 j8 = 1.5707963267949;
9249 IkReal x1059
9250 = (pp
9251 + (((-1.0) * (1.0)
9252 * (pz * pz))));
9253 IkReal x1060
9254 = ((1.32323529411765)
9255 * cj9);
9256 IkReal x1061
9257 = ((3.92156862745098)
9258 * pp);
9259 IkReal x1062
9260 = ((0.316735294117647)
9261 * sj6);
9262 IkReal x1063
9263 = ((0.108264705882353)
9264 * cj9 * sj6);
9265 IkReal x1064
9266 = ((0.588235294117647)
9267 * pp * sj6);
9268 j4eval[0] = x1059;
9269 j4eval[1]
9270 = ((IKabs((
9271 (((-1.0)
9272 * (1.51009803921569)
9273 * py))
9274 + ((px * x1064))
9275 + (((-1.0) * py
9276 * x1060))
9277 + ((px * x1063))
9278 + ((py * x1061))
9279 + ((px * x1062)))))
9280 + (IKabs((
9281 ((px * x1060))
9282 + (((-1.0) * px
9283 * x1061))
9284 + (((1.51009803921569)
9285 * px))
9286 + ((py * x1063))
9287 + ((py * x1062))
9288 + ((py
9289 * x1064))))));
9290 j4eval[2] = IKsign(x1059);
9291 if (IKabs(j4eval[0])
9292 < 0.0000010000000000
9293 || IKabs(j4eval[1])
9294 < 0.0000010000000000
9295 || IKabs(j4eval[2])
9296 < 0.0000010000000000)
9297 {
9298 {
9299 IkReal evalcond[7];
9300 bool bgotonextstatement
9301 = true;
9302 do
9303 {
9304 evalcond[0]
9305 = ((-3.14159265358979)
9306 + (IKfmod(
9307 ((3.14159265358979)
9308 + (IKabs((
9309 (-1.5707963267949)
9310 + j6)))),
9311 6.28318530717959)));
9312 evalcond[1]
9313 = ((-1.0)
9314 * (((1.0)
9315 * pz)));
9316 if (IKabs(evalcond[0])
9317 < 0.0000010000000000
9318 && IKabs(
9319 evalcond[1])
9320 < 0.0000010000000000)
9321 {
9322 bgotonextstatement
9323 = false;
9324 {
9325 IkReal j4eval[3];
9326 sj8 = 1.0;
9327 cj8 = 0;
9328 j8 = 1.5707963267949;
9329 sj6 = 1.0;
9330 cj6 = 0;
9331 j6 = 1.5707963267949;
9332 IkReal x1065
9333 = (pp
9334 + (((-1.0)
9335 * (1.0)
9336 * (pz
9337 * pz))));
9338 IkReal x1066
9339 = (cj9 * px);
9340 IkReal x1067
9341 = (cj9 * py);
9342 IkReal x1068
9343 = ((3.92156862745098)
9344 * pp);
9345 IkReal x1069
9346 = ((0.045)
9347 * sj9);
9348 j4eval[0] = x1065;
9349 j4eval[1]
9350 = ((IKabs((
9351 (((1.51009803921569)
9352 * px))
9353 + ((py
9354 * x1069))
9355 + (((0.55)
9356 * py))
9357 + (((0.3)
9358 * x1067))
9359 + (((1.32323529411765)
9360 * x1066))
9361 + (((-1.0)
9362 * px
9363 * x1068)))))
9364 + (IKabs((
9365 (((0.3)
9366 * x1066))
9367 + (((-1.0)
9368 * (1.51009803921569)
9369 * py))
9370 + (((-1.32323529411765)
9371 * x1067))
9372 + (((0.55)
9373 * px))
9374 + ((py
9375 * x1068))
9376 + ((px
9377 * x1069))))));
9378 j4eval[2]
9379 = IKsign(x1065);
9380 if (IKabs(j4eval[0])
9381 < 0.0000010000000000
9382 || IKabs(
9383 j4eval
9384 [1])
9385 < 0.0000010000000000
9386 || IKabs(
9387 j4eval
9388 [2])
9389 < 0.0000010000000000)
9390 {
9391 {
9392 IkReal
9393 j4eval[3];
9394 sj8 = 1.0;
9395 cj8 = 0;
9396 j8 = 1.5707963267949;
9397 sj6 = 1.0;
9398 cj6 = 0;
9399 j6 = 1.5707963267949;
9400 IkReal x1070
9401 = (pp
9402 + (((-1.0)
9403 * (1.0)
9404 * (pz
9405 * pz))));
9406 IkReal x1071
9407 = (cj9
9408 * px);
9409 IkReal x1072
9410 = (cj9
9411 * py);
9412 IkReal x1073
9413 = (pp * px);
9414 IkReal x1074
9415 = (pp * py);
9416 j4eval[0]
9417 = x1070;
9418 j4eval[1]
9419 = ((IKabs((
9420 (((0.108264705882353)
9421 * x1072))
9422 + (((0.588235294117647)
9423 * x1074))
9424 + (((1.32323529411765)
9425 * x1071))
9426 + (((0.316735294117647)
9427 * py))
9428 + (((1.51009803921569)
9429 * px))
9430 + (((-3.92156862745098)
9431 * x1073)))))
9432 + (IKabs((
9433 (((-1.32323529411765)
9434 * x1072))
9435 + (((0.588235294117647)
9436 * x1073))
9437 + (((3.92156862745098)
9438 * x1074))
9439 + (((0.316735294117647)
9440 * px))
9441 + (((0.108264705882353)
9442 * x1071))
9443 + (((-1.0)
9444 * (1.51009803921569)
9445 * py))))));
9446 j4eval[2]
9447 = IKsign(
9448 x1070);
9449 if (IKabs(
9450 j4eval
9451 [0])
9452 < 0.0000010000000000
9453 || IKabs(
9454 j4eval
9455 [1])
9456 < 0.0000010000000000
9457 || IKabs(
9458 j4eval
9459 [2])
9460 < 0.0000010000000000)
9461 {
9462 {
9463 IkReal
9464 j4eval
9465 [3];
9466 sj8 = 1.0;
9467 cj8 = 0;
9468 j8 = 1.5707963267949;
9469 sj6 = 1.0;
9470 cj6 = 0;
9471 j6 = 1.5707963267949;
9472 IkReal x1075
9473 = pz
9474 * pz;
9475 IkReal x1076
9476 = (cj9
9477 * px);
9478 IkReal x1077
9479 = (cj9
9480 * py);
9481 IkReal x1078
9482 = (pp
9483 * px);
9484 IkReal x1079
9485 = (pp
9486 * py);
9487 j4eval[0]
9488 = ((((-1.0)
9489 * x1075))
9490 + pp);
9491 j4eval[1]
9492 = ((IKabs((
9493 (((0.348408823529412)
9494 * py))
9495 + (((1.66110784313725)
9496 * px))
9497 + (((1.45555882352941)
9498 * x1076))
9499 + (((0.647058823529412)
9500 * x1079))
9501 + (((-4.31372549019608)
9502 * x1078))
9503 + (((0.119091176470588)
9504 * x1077)))))
9505 + (IKabs((
9506 (((0.348408823529412)
9507 * px))
9508 + (((-1.45555882352941)
9509 * x1077))
9510 + (((4.31372549019608)
9511 * x1079))
9512 + (((0.647058823529412)
9513 * x1078))
9514 + (((-1.0)
9515 * (1.66110784313725)
9516 * py))
9517 + (((0.119091176470588)
9518 * x1076))))));
9519 j4eval[2] = IKsign((
9520 (((1.1)
9521 * pp))
9522 + (((-1.1)
9523 * x1075))));
9524 if (IKabs(
9525 j4eval
9526 [0])
9527 < 0.0000010000000000
9528 || IKabs(
9529 j4eval
9530 [1])
9531 < 0.0000010000000000
9532 || IKabs(
9533 j4eval
9534 [2])
9535 < 0.0000010000000000)
9536 {
9537 {
9538 IkReal evalcond
9539 [6];
9540 bool
9541 bgotonextstatement
9542 = true;
9543 do
9544 {
9545 IkReal
9546 x1080
9547 = ((-1.51009803921569)
9548 + (((-1.0)
9549 * (1.32323529411765)
9550 * cj9))
9551 + (((3.92156862745098)
9552 * pp)));
9553 evalcond
9554 [0]
9555 = ((IKabs(
9556 py))
9557 + (IKabs(
9558 px)));
9559 evalcond
9560 [1]
9561 = ((-0.55)
9562 + (((-1.0)
9563 * (0.3)
9564 * cj9))
9565 + (((-1.0)
9566 * (0.045)
9567 * sj9)));
9568 evalcond
9569 [2]
9570 = x1080;
9571 evalcond
9572 [3]
9573 = x1080;
9574 evalcond
9575 [4]
9576 = ((0.316735294117647)
9577 + (((0.108264705882353)
9578 * cj9))
9579 + (((0.588235294117647)
9580 * pp)));
9581 evalcond
9582 [5]
9583 = ((-0.2125)
9584 + (((-1.0)
9585 * (1.0)
9586 * pp)));
9587 if (IKabs(
9588 evalcond
9589 [0])
9590 < 0.0000010000000000
9591 && IKabs(
9592 evalcond
9593 [1])
9594 < 0.0000010000000000
9595 && IKabs(
9596 evalcond
9597 [2])
9598 < 0.0000010000000000
9599 && IKabs(
9600 evalcond
9601 [3])
9602 < 0.0000010000000000
9603 && IKabs(
9604 evalcond
9605 [4])
9606 < 0.0000010000000000
9607 && IKabs(
9608 evalcond
9609 [5])
9610 < 0.0000010000000000)
9611 {
9612 bgotonextstatement
9613 = false;
9614 {
9615 IkReal j4array
9616 [4],
9617 cj4array
9618 [4],
9619 sj4array
9620 [4];
9621 bool j4valid
9622 [4]
9623 = {false};
9624 _nj4
9625 = 4;
9626 j4array
9627 [0]
9628 = 0;
9629 sj4array
9630 [0]
9631 = IKsin(
9632 j4array
9633 [0]);
9634 cj4array
9635 [0]
9636 = IKcos(
9637 j4array
9638 [0]);
9639 j4array
9640 [1]
9641 = 1.5707963267949;
9642 sj4array
9643 [1]
9644 = IKsin(
9645 j4array
9646 [1]);
9647 cj4array
9648 [1]
9649 = IKcos(
9650 j4array
9651 [1]);
9652 j4array
9653 [2]
9654 = 3.14159265358979;
9655 sj4array
9656 [2]
9657 = IKsin(
9658 j4array
9659 [2]);
9660 cj4array
9661 [2]
9662 = IKcos(
9663 j4array
9664 [2]);
9665 j4array
9666 [3]
9667 = -1.5707963267949;
9668 sj4array
9669 [3]
9670 = IKsin(
9671 j4array
9672 [3]);
9673 cj4array
9674 [3]
9675 = IKcos(
9676 j4array
9677 [3]);
9678 if (j4array
9679 [0]
9680 > IKPI)
9681 {
9682 j4array
9683 [0]
9684 -= IK2PI;
9685 }
9686 else if (
9687 j4array
9688 [0]
9689 < -IKPI)
9690 {
9691 j4array
9692 [0]
9693 += IK2PI;
9694 }
9695 j4valid
9696 [0]
9697 = true;
9698 if (j4array
9699 [1]
9700 > IKPI)
9701 {
9702 j4array
9703 [1]
9704 -= IK2PI;
9705 }
9706 else if (
9707 j4array
9708 [1]
9709 < -IKPI)
9710 {
9711 j4array
9712 [1]
9713 += IK2PI;
9714 }
9715 j4valid
9716 [1]
9717 = true;
9718 if (j4array
9719 [2]
9720 > IKPI)
9721 {
9722 j4array
9723 [2]
9724 -= IK2PI;
9725 }
9726 else if (
9727 j4array
9728 [2]
9729 < -IKPI)
9730 {
9731 j4array
9732 [2]
9733 += IK2PI;
9734 }
9735 j4valid
9736 [2]
9737 = true;
9738 if (j4array
9739 [3]
9740 > IKPI)
9741 {
9742 j4array
9743 [3]
9744 -= IK2PI;
9745 }
9746 else if (
9747 j4array
9748 [3]
9749 < -IKPI)
9750 {
9751 j4array
9752 [3]
9753 += IK2PI;
9754 }
9755 j4valid
9756 [3]
9757 = true;
9758 for (
9759 int ij4
9760 = 0;
9761 ij4
9762 < 4;
9763 ++ij4)
9764 {
9765 if (!j4valid
9766 [ij4])
9767 {
9768 continue;
9769 }
9770 _ij4[0]
9771 = ij4;
9772 _ij4[1]
9773 = -1;
9774 for (
9775 int iij4
9776 = ij4
9777 + 1;
9778 iij4
9779 < 4;
9780 ++iij4)
9781 {
9782 if (j4valid
9783 [iij4]
9784 && IKabs(
9785 cj4array
9786 [ij4]
9787 - cj4array
9788 [iij4])
9789 < IKFAST_SOLUTION_THRESH
9790 && IKabs(
9791 sj4array
9792 [ij4]
9793 - sj4array
9794 [iij4])
9795 < IKFAST_SOLUTION_THRESH)
9796 {
9797 j4valid
9798 [iij4]
9799 = false;
9800 _ij4[1]
9801 = iij4;
9802 break;
9803 }
9804 }
9805 j4 = j4array
9806 [ij4];
9807 cj4 = cj4array
9808 [ij4];
9809 sj4 = sj4array
9810 [ij4];
9811
9812 rotationfunction0(
9813 solutions);
9814 }
9815 }
9816 }
9817 } while (
9818 0);
9819 if (bgotonextstatement)
9820 {
9821 bool
9822 bgotonextstatement
9823 = true;
9824 do
9825 {
9826 if (1)
9827 {
9828 bgotonextstatement
9829 = false;
9830 continue; // branch miss [j4]
9831 }
9832 } while (
9833 0);
9834 if (bgotonextstatement)
9835 {
9836 }
9837 }
9838 }
9839 }
9840 else
9841 {
9842 {
9843 IkReal j4array
9844 [1],
9845 cj4array
9846 [1],
9847 sj4array
9848 [1];
9849 bool j4valid
9850 [1]
9851 = {false};
9852 _nj4
9853 = 1;
9854 IkReal
9855 x1081
9856 = (cj9
9857 * px);
9858 IkReal
9859 x1082
9860 = (cj9
9861 * py);
9862 IkReal
9863 x1083
9864 = (pp
9865 * px);
9866 IkReal
9867 x1084
9868 = (pp
9869 * py);
9870 CheckValue<IkReal> x1085 = IKatan2WithCheck(
9871 IkReal((
9872 (((0.119091176470588)
9873 * x1082))
9874 + (((0.348408823529412)
9875 * py))
9876 + (((1.66110784313725)
9877 * px))
9878 + (((0.647058823529412)
9879 * x1084))
9880 + (((-4.31372549019608)
9881 * x1083))
9882 + (((1.45555882352941)
9883 * x1081)))),
9884 ((((4.31372549019608)
9885 * x1084))
9886 + (((0.119091176470588)
9887 * x1081))
9888 + (((0.348408823529412)
9889 * px))
9890 + (((0.647058823529412)
9891 * x1083))
9892 + (((-1.45555882352941)
9893 * x1082))
9894 + (((-1.0)
9895 * (1.66110784313725)
9896 * py))),
9897 IKFAST_ATAN2_MAGTHRESH);
9898 if (!x1085
9899 .valid)
9900 {
9901 continue;
9902 }
9903 CheckValue<IkReal> x1086 = IKPowWithIntegerCheck(
9904 IKsign((
9905 (((1.1)
9906 * pp))
9907 + (((-1.0)
9908 * (1.1)
9909 * (pz
9910 * pz))))),
9911 -1);
9912 if (!x1086
9913 .valid)
9914 {
9915 continue;
9916 }
9917 j4array
9918 [0]
9919 = ((-1.5707963267949)
9920 + (x1085
9921 .value)
9922 + (((1.5707963267949)
9923 * (x1086
9924 .value))));
9925 sj4array
9926 [0]
9927 = IKsin(
9928 j4array
9929 [0]);
9930 cj4array
9931 [0]
9932 = IKcos(
9933 j4array
9934 [0]);
9935 if (j4array
9936 [0]
9937 > IKPI)
9938 {
9939 j4array
9940 [0]
9941 -= IK2PI;
9942 }
9943 else if (
9944 j4array
9945 [0]
9946 < -IKPI)
9947 {
9948 j4array
9949 [0]
9950 += IK2PI;
9951 }
9952 j4valid
9953 [0]
9954 = true;
9955 for (
9956 int ij4
9957 = 0;
9958 ij4
9959 < 1;
9960 ++ij4)
9961 {
9962 if (!j4valid
9963 [ij4])
9964 {
9965 continue;
9966 }
9967 _ij4[0]
9968 = ij4;
9969 _ij4[1]
9970 = -1;
9971 for (
9972 int iij4
9973 = ij4
9974 + 1;
9975 iij4
9976 < 1;
9977 ++iij4)
9978 {
9979 if (j4valid
9980 [iij4]
9981 && IKabs(
9982 cj4array
9983 [ij4]
9984 - cj4array
9985 [iij4])
9986 < IKFAST_SOLUTION_THRESH
9987 && IKabs(
9988 sj4array
9989 [ij4]
9990 - sj4array
9991 [iij4])
9992 < IKFAST_SOLUTION_THRESH)
9993 {
9994 j4valid
9995 [iij4]
9996 = false;
9997 _ij4[1]
9998 = iij4;
9999 break;
10000 }
10001 }
10002 j4 = j4array
10003 [ij4];
10004 cj4 = cj4array
10005 [ij4];
10006 sj4 = sj4array
10007 [ij4];
10008 {
10009 IkReal evalcond
10010 [4];
10011 IkReal
10012 x1087
10013 = IKcos(
10014 j4);
10015 IkReal
10016 x1088
10017 = (px
10018 * x1087);
10019 IkReal
10020 x1089
10021 = IKsin(
10022 j4);
10023 IkReal
10024 x1090
10025 = (py
10026 * x1089);
10027 IkReal
10028 x1091
10029 = (px
10030 * x1089);
10031 IkReal
10032 x1092
10033 = (py
10034 * x1087);
10035 evalcond
10036 [0]
10037 = ((-0.55)
10038 + x1088
10039 + (((-1.0)
10040 * (0.3)
10041 * cj9))
10042 + x1090
10043 + (((-1.0)
10044 * (0.045)
10045 * sj9)));
10046 evalcond
10047 [1]
10048 = ((-1.51009803921569)
10049 + (((-1.0)
10050 * x1092))
10051 + (((-1.0)
10052 * (1.32323529411765)
10053 * cj9))
10054 + (((3.92156862745098)
10055 * pp))
10056 + x1091);
10057 evalcond
10058 [2]
10059 = ((0.316735294117647)
10060 + (((-1.0)
10061 * x1090))
10062 + (((-1.0)
10063 * x1088))
10064 + (((0.108264705882353)
10065 * cj9))
10066 + (((0.588235294117647)
10067 * pp)));
10068 evalcond
10069 [3]
10070 = ((-0.2125)
10071 + (((1.1)
10072 * x1090))
10073 + (((0.09)
10074 * x1092))
10075 + (((-0.09)
10076 * x1091))
10077 + (((1.1)
10078 * x1088))
10079 + (((-1.0)
10080 * (1.0)
10081 * pp)));
10082 if (IKabs(
10083 evalcond
10084 [0])
10085 > IKFAST_EVALCOND_THRESH
10086 || IKabs(
10087 evalcond
10088 [1])
10089 > IKFAST_EVALCOND_THRESH
10090 || IKabs(
10091 evalcond
10092 [2])
10093 > IKFAST_EVALCOND_THRESH
10094 || IKabs(
10095 evalcond
10096 [3])
10097 > IKFAST_EVALCOND_THRESH)
10098 {
10099 continue;
10100 }
10101 }
10102
10103 rotationfunction0(
10104 solutions);
10105 }
10106 }
10107 }
10108 }
10109 }
10110 else
10111 {
10112 {
10113 IkReal
10114 j4array
10115 [1],
10116 cj4array
10117 [1],
10118 sj4array
10119 [1];
10120 bool j4valid
10121 [1]
10122 = {false};
10123 _nj4 = 1;
10124 IkReal x1093
10125 = (cj9
10126 * px);
10127 IkReal x1094
10128 = (cj9
10129 * py);
10130 IkReal x1095
10131 = (pp
10132 * px);
10133 IkReal x1096
10134 = (pp
10135 * py);
10136 CheckValue<IkReal> x1097 = IKatan2WithCheck(
10137 IkReal((
10138 (((-3.92156862745098)
10139 * x1095))
10140 + (((0.316735294117647)
10141 * py))
10142 + (((1.51009803921569)
10143 * px))
10144 + (((0.108264705882353)
10145 * x1094))
10146 + (((1.32323529411765)
10147 * x1093))
10148 + (((0.588235294117647)
10149 * x1096)))),
10150 ((((0.108264705882353)
10151 * x1093))
10152 + (((0.588235294117647)
10153 * x1095))
10154 + (((-1.32323529411765)
10155 * x1094))
10156 + (((0.316735294117647)
10157 * px))
10158 + (((-1.0)
10159 * (1.51009803921569)
10160 * py))
10161 + (((3.92156862745098)
10162 * x1096))),
10163 IKFAST_ATAN2_MAGTHRESH);
10164 if (!x1097
10165 .valid)
10166 {
10167 continue;
10168 }
10169 CheckValue<IkReal> x1098 = IKPowWithIntegerCheck(
10170 IKsign((
10171 pp
10172 + (((-1.0)
10173 * (1.0)
10174 * (pz
10175 * pz))))),
10176 -1);
10177 if (!x1098
10178 .valid)
10179 {
10180 continue;
10181 }
10182 j4array[0]
10183 = ((-1.5707963267949)
10184 + (x1097
10185 .value)
10186 + (((1.5707963267949)
10187 * (x1098
10188 .value))));
10189 sj4array[0] = IKsin(
10190 j4array
10191 [0]);
10192 cj4array[0] = IKcos(
10193 j4array
10194 [0]);
10195 if (j4array
10196 [0]
10197 > IKPI)
10198 {
10199 j4array[0]
10200 -= IK2PI;
10201 }
10202 else if (
10203 j4array
10204 [0]
10205 < -IKPI)
10206 {
10207 j4array[0]
10208 += IK2PI;
10209 }
10210 j4valid[0]
10211 = true;
10212 for (int ij4
10213 = 0;
10214 ij4
10215 < 1;
10216 ++ij4)
10217 {
10218 if (!j4valid
10219 [ij4])
10220 {
10221 continue;
10222 }
10223 _ij4[0]
10224 = ij4;
10225 _ij4[1]
10226 = -1;
10227 for (
10228 int iij4
10229 = ij4
10230 + 1;
10231 iij4
10232 < 1;
10233 ++iij4)
10234 {
10235 if (j4valid
10236 [iij4]
10237 && IKabs(
10238 cj4array
10239 [ij4]
10240 - cj4array
10241 [iij4])
10242 < IKFAST_SOLUTION_THRESH
10243 && IKabs(
10244 sj4array
10245 [ij4]
10246 - sj4array
10247 [iij4])
10248 < IKFAST_SOLUTION_THRESH)
10249 {
10250 j4valid
10251 [iij4]
10252 = false;
10253 _ij4[1]
10254 = iij4;
10255 break;
10256 }
10257 }
10258 j4 = j4array
10259 [ij4];
10260 cj4 = cj4array
10261 [ij4];
10262 sj4 = sj4array
10263 [ij4];
10264 {
10265 IkReal evalcond
10266 [4];
10267 IkReal
10268 x1099
10269 = IKcos(
10270 j4);
10271 IkReal
10272 x1100
10273 = (px
10274 * x1099);
10275 IkReal
10276 x1101
10277 = IKsin(
10278 j4);
10279 IkReal
10280 x1102
10281 = (py
10282 * x1101);
10283 IkReal
10284 x1103
10285 = (px
10286 * x1101);
10287 IkReal
10288 x1104
10289 = (py
10290 * x1099);
10291 evalcond
10292 [0]
10293 = ((-0.55)
10294 + x1102
10295 + x1100
10296 + (((-1.0)
10297 * (0.3)
10298 * cj9))
10299 + (((-1.0)
10300 * (0.045)
10301 * sj9)));
10302 evalcond
10303 [1]
10304 = ((-1.51009803921569)
10305 + (((-1.0)
10306 * x1104))
10307 + (((-1.0)
10308 * (1.32323529411765)
10309 * cj9))
10310 + x1103
10311 + (((3.92156862745098)
10312 * pp)));
10313 evalcond
10314 [2]
10315 = ((0.316735294117647)
10316 + (((-1.0)
10317 * x1102))
10318 + (((-1.0)
10319 * x1100))
10320 + (((0.108264705882353)
10321 * cj9))
10322 + (((0.588235294117647)
10323 * pp)));
10324 evalcond
10325 [3]
10326 = ((-0.2125)
10327 + (((-0.09)
10328 * x1103))
10329 + (((1.1)
10330 * x1100))
10331 + (((1.1)
10332 * x1102))
10333 + (((-1.0)
10334 * (1.0)
10335 * pp))
10336 + (((0.09)
10337 * x1104)));
10338 if (IKabs(
10339 evalcond
10340 [0])
10341 > IKFAST_EVALCOND_THRESH
10342 || IKabs(
10343 evalcond
10344 [1])
10345 > IKFAST_EVALCOND_THRESH
10346 || IKabs(
10347 evalcond
10348 [2])
10349 > IKFAST_EVALCOND_THRESH
10350 || IKabs(
10351 evalcond
10352 [3])
10353 > IKFAST_EVALCOND_THRESH)
10354 {
10355 continue;
10356 }
10357 }
10358
10359 rotationfunction0(
10360 solutions);
10361 }
10362 }
10363 }
10364 }
10365 }
10366 else
10367 {
10368 {
10369 IkReal
10370 j4array[1],
10371 cj4array[1],
10372 sj4array[1];
10373 bool j4valid[1]
10374 = {false};
10375 _nj4 = 1;
10376 IkReal x1105
10377 = (cj9
10378 * px);
10379 IkReal x1106
10380 = (cj9
10381 * py);
10382 IkReal x1107
10383 = ((3.92156862745098)
10384 * pp);
10385 IkReal x1108
10386 = ((0.045)
10387 * sj9);
10388 CheckValue<IkReal> x1109 = IKPowWithIntegerCheck(
10389 IKsign((
10390 pp
10391 + (((-1.0)
10392 * (1.0)
10393 * (pz
10394 * pz))))),
10395 -1);
10396 if (!x1109
10397 .valid)
10398 {
10399 continue;
10400 }
10401 CheckValue<IkReal> x1110 = IKatan2WithCheck(
10402 IkReal((
10403 (((-1.0)
10404 * px
10405 * x1107))
10406 + ((py
10407 * x1108))
10408 + (((1.51009803921569)
10409 * px))
10410 + (((0.55)
10411 * py))
10412 + (((1.32323529411765)
10413 * x1105))
10414 + (((0.3)
10415 * x1106)))),
10416 ((((-1.32323529411765)
10417 * x1106))
10418 + (((-1.0)
10419 * (1.51009803921569)
10420 * py))
10421 + ((py
10422 * x1107))
10423 + (((0.55)
10424 * px))
10425 + ((px
10426 * x1108))
10427 + (((0.3)
10428 * x1105))),
10429 IKFAST_ATAN2_MAGTHRESH);
10430 if (!x1110
10431 .valid)
10432 {
10433 continue;
10434 }
10435 j4array[0]
10436 = ((-1.5707963267949)
10437 + (((1.5707963267949)
10438 * (x1109
10439 .value)))
10440 + (x1110
10441 .value));
10442 sj4array[0]
10443 = IKsin(
10444 j4array
10445 [0]);
10446 cj4array[0]
10447 = IKcos(
10448 j4array
10449 [0]);
10450 if (j4array[0]
10451 > IKPI)
10452 {
10453 j4array[0]
10454 -= IK2PI;
10455 }
10456 else if (
10457 j4array[0]
10458 < -IKPI)
10459 {
10460 j4array[0]
10461 += IK2PI;
10462 }
10463 j4valid[0]
10464 = true;
10465 for (int ij4
10466 = 0;
10467 ij4 < 1;
10468 ++ij4)
10469 {
10470 if (!j4valid
10471 [ij4])
10472 {
10473 continue;
10474 }
10475 _ij4[0] = ij4;
10476 _ij4[1] = -1;
10477 for (int iij4
10478 = ij4
10479 + 1;
10480 iij4 < 1;
10481 ++iij4)
10482 {
10483 if (j4valid
10484 [iij4]
10485 && IKabs(
10486 cj4array
10487 [ij4]
10488 - cj4array
10489 [iij4])
10490 < IKFAST_SOLUTION_THRESH
10491 && IKabs(
10492 sj4array
10493 [ij4]
10494 - sj4array
10495 [iij4])
10496 < IKFAST_SOLUTION_THRESH)
10497 {
10498 j4valid
10499 [iij4]
10500 = false;
10501 _ij4[1]
10502 = iij4;
10503 break;
10504 }
10505 }
10506 j4 = j4array
10507 [ij4];
10508 cj4 = cj4array
10509 [ij4];
10510 sj4 = sj4array
10511 [ij4];
10512 {
10513 IkReal
10514 evalcond
10515 [4];
10516 IkReal x1111
10517 = IKcos(
10518 j4);
10519 IkReal x1112
10520 = (px
10521 * x1111);
10522 IkReal x1113
10523 = IKsin(
10524 j4);
10525 IkReal x1114
10526 = (py
10527 * x1113);
10528 IkReal x1115
10529 = (px
10530 * x1113);
10531 IkReal x1116
10532 = (py
10533 * x1111);
10534 evalcond[0]
10535 = ((-0.55)
10536 + x1112
10537 + x1114
10538 + (((-1.0)
10539 * (0.3)
10540 * cj9))
10541 + (((-1.0)
10542 * (0.045)
10543 * sj9)));
10544 evalcond[1]
10545 = ((-1.51009803921569)
10546 + x1115
10547 + (((-1.0)
10548 * (1.32323529411765)
10549 * cj9))
10550 + (((-1.0)
10551 * x1116))
10552 + (((3.92156862745098)
10553 * pp)));
10554 evalcond[2]
10555 = ((0.316735294117647)
10556 + (((-1.0)
10557 * x1114))
10558 + (((0.108264705882353)
10559 * cj9))
10560 + (((0.588235294117647)
10561 * pp))
10562 + (((-1.0)
10563 * x1112)));
10564 evalcond[3]
10565 = ((-0.2125)
10566 + (((-0.09)
10567 * x1115))
10568 + (((1.1)
10569 * x1114))
10570 + (((1.1)
10571 * x1112))
10572 + (((-1.0)
10573 * (1.0)
10574 * pp))
10575 + (((0.09)
10576 * x1116)));
10577 if (IKabs(
10578 evalcond
10579 [0])
10580 > IKFAST_EVALCOND_THRESH
10581 || IKabs(
10582 evalcond
10583 [1])
10584 > IKFAST_EVALCOND_THRESH
10585 || IKabs(
10586 evalcond
10587 [2])
10588 > IKFAST_EVALCOND_THRESH
10589 || IKabs(
10590 evalcond
10591 [3])
10592 > IKFAST_EVALCOND_THRESH)
10593 {
10594 continue;
10595 }
10596 }
10597
10598 rotationfunction0(
10599 solutions);
10600 }
10601 }
10602 }
10603 }
10604 }
10605 } while (0);
10606 if (bgotonextstatement)
10607 {
10608 bool bgotonextstatement
10609 = true;
10610 do
10611 {
10612 evalcond[0]
10613 = ((-3.14159265358979)
10614 + (IKfmod(
10615 ((3.14159265358979)
10616 + (IKabs((
10617 (1.5707963267949)
10618 + j6)))),
10619 6.28318530717959)));
10620 evalcond[1] = pz;
10621 if (IKabs(evalcond[0])
10622 < 0.0000010000000000
10623 && IKabs(evalcond
10624 [1])
10625 < 0.0000010000000000)
10626 {
10627 bgotonextstatement
10628 = false;
10629 {
10630 IkReal j4eval[3];
10631 sj8 = 1.0;
10632 cj8 = 0;
10633 j8 = 1.5707963267949;
10634 sj6 = -1.0;
10635 cj6 = 0;
10636 j6 = -1.5707963267949;
10637 IkReal x1117
10638 = (pp
10639 + (((-1.0)
10640 * (1.0)
10641 * (pz
10642 * pz))));
10643 IkReal x1118
10644 = (cj9 * px);
10645 IkReal x1119
10646 = (cj9 * py);
10647 IkReal x1120
10648 = ((3.92156862745098)
10649 * pp);
10650 IkReal x1121
10651 = ((0.045)
10652 * sj9);
10653 j4eval[0] = x1117;
10654 j4eval[1]
10655 = ((IKabs((
10656 (((-0.3)
10657 * x1119))
10658 + (((1.51009803921569)
10659 * px))
10660 + (((1.32323529411765)
10661 * x1118))
10662 + (((-1.0)
10663 * px
10664 * x1120))
10665 + (((-1.0)
10666 * py
10667 * x1121))
10668 + (((-1.0)
10669 * (0.55)
10670 * py)))))
10671 + (IKabs((
10672 (((-1.0)
10673 * px
10674 * x1121))
10675 + (((-1.0)
10676 * (0.55)
10677 * px))
10678 + (((-0.3)
10679 * x1118))
10680 + (((-1.0)
10681 * (1.51009803921569)
10682 * py))
10683 + ((py
10684 * x1120))
10685 + (((-1.32323529411765)
10686 * x1119))))));
10687 j4eval[2]
10688 = IKsign(
10689 x1117);
10690 if (IKabs(
10691 j4eval[0])
10692 < 0.0000010000000000
10693 || IKabs(
10694 j4eval
10695 [1])
10696 < 0.0000010000000000
10697 || IKabs(
10698 j4eval
10699 [2])
10700 < 0.0000010000000000)
10701 {
10702 {
10703 IkReal
10704 j4eval[3];
10705 sj8 = 1.0;
10706 cj8 = 0;
10707 j8 = 1.5707963267949;
10708 sj6 = -1.0;
10709 cj6 = 0;
10710 j6 = -1.5707963267949;
10711 IkReal x1122
10712 = (pp
10713 + (((-1.0)
10714 * (1.0)
10715 * (pz
10716 * pz))));
10717 IkReal x1123
10718 = (cj9
10719 * px);
10720 IkReal x1124
10721 = (cj9
10722 * py);
10723 IkReal x1125
10724 = (pp
10725 * px);
10726 IkReal x1126
10727 = (pp
10728 * py);
10729 j4eval[0]
10730 = x1122;
10731 j4eval[1]
10732 = ((IKabs((
10733 (((-0.108264705882353)
10734 * x1124))
10735 + (((1.51009803921569)
10736 * px))
10737 + (((-1.0)
10738 * (0.316735294117647)
10739 * py))
10740 + (((-3.92156862745098)
10741 * x1125))
10742 + (((-0.588235294117647)
10743 * x1126))
10744 + (((1.32323529411765)
10745 * x1123)))))
10746 + (IKabs((
10747 (((-0.108264705882353)
10748 * x1123))
10749 + (((-1.0)
10750 * (1.51009803921569)
10751 * py))
10752 + (((-1.0)
10753 * (0.316735294117647)
10754 * px))
10755 + (((-0.588235294117647)
10756 * x1125))
10757 + (((3.92156862745098)
10758 * x1126))
10759 + (((-1.32323529411765)
10760 * x1124))))));
10761 j4eval[2]
10762 = IKsign(
10763 x1122);
10764 if (IKabs(
10765 j4eval
10766 [0])
10767 < 0.0000010000000000
10768 || IKabs(
10769 j4eval
10770 [1])
10771 < 0.0000010000000000
10772 || IKabs(
10773 j4eval
10774 [2])
10775 < 0.0000010000000000)
10776 {
10777 {
10778 IkReal j4eval
10779 [3];
10780 sj8 = 1.0;
10781 cj8 = 0;
10782 j8 = 1.5707963267949;
10783 sj6 = -1.0;
10784 cj6 = 0;
10785 j6 = -1.5707963267949;
10786 IkReal
10787 x1127
10788 = pz
10789 * pz;
10790 IkReal
10791 x1128
10792 = (cj9
10793 * px);
10794 IkReal
10795 x1129
10796 = (cj9
10797 * py);
10798 IkReal
10799 x1130
10800 = (pp
10801 * px);
10802 IkReal
10803 x1131
10804 = (pp
10805 * py);
10806 j4eval[0]
10807 = (x1127
10808 + (((-1.0)
10809 * (1.0)
10810 * pp)));
10811 j4eval[1] = IKsign((
10812 (((1.1)
10813 * x1127))
10814 + (((-1.0)
10815 * (1.1)
10816 * pp))));
10817 j4eval[2]
10818 = ((IKabs((
10819 (((0.348408823529412)
10820 * px))
10821 + (((1.66110784313725)
10822 * py))
10823 + (((0.119091176470588)
10824 * x1128))
10825 + (((0.647058823529412)
10826 * x1130))
10827 + (((1.45555882352941)
10828 * x1129))
10829 + (((-4.31372549019608)
10830 * x1131)))))
10831 + (IKabs((
10832 (((0.647058823529412)
10833 * x1131))
10834 + (((0.348408823529412)
10835 * py))
10836 + (((0.119091176470588)
10837 * x1129))
10838 + (((4.31372549019608)
10839 * x1130))
10840 + (((-1.0)
10841 * (1.66110784313725)
10842 * px))
10843 + (((-1.45555882352941)
10844 * x1128))))));
10845 if (IKabs(
10846 j4eval
10847 [0])
10848 < 0.0000010000000000
10849 || IKabs(
10850 j4eval
10851 [1])
10852 < 0.0000010000000000
10853 || IKabs(
10854 j4eval
10855 [2])
10856 < 0.0000010000000000)
10857 {
10858 {
10859 IkReal evalcond
10860 [6];
10861 bool
10862 bgotonextstatement
10863 = true;
10864 do
10865 {
10866 IkReal
10867 x1132
10868 = ((-1.51009803921569)
10869 + (((-1.0)
10870 * (1.32323529411765)
10871 * cj9))
10872 + (((3.92156862745098)
10873 * pp)));
10874 evalcond
10875 [0]
10876 = ((IKabs(
10877 py))
10878 + (IKabs(
10879 px)));
10880 evalcond
10881 [1]
10882 = ((-0.55)
10883 + (((-1.0)
10884 * (0.3)
10885 * cj9))
10886 + (((-1.0)
10887 * (0.045)
10888 * sj9)));
10889 evalcond
10890 [2]
10891 = x1132;
10892 evalcond
10893 [3]
10894 = x1132;
10895 evalcond
10896 [4]
10897 = ((-0.316735294117647)
10898 + (((-1.0)
10899 * (0.588235294117647)
10900 * pp))
10901 + (((-1.0)
10902 * (0.108264705882353)
10903 * cj9)));
10904 evalcond
10905 [5]
10906 = ((-0.2125)
10907 + (((-1.0)
10908 * (1.0)
10909 * pp)));
10910 if (IKabs(
10911 evalcond
10912 [0])
10913 < 0.0000010000000000
10914 && IKabs(
10915 evalcond
10916 [1])
10917 < 0.0000010000000000
10918 && IKabs(
10919 evalcond
10920 [2])
10921 < 0.0000010000000000
10922 && IKabs(
10923 evalcond
10924 [3])
10925 < 0.0000010000000000
10926 && IKabs(
10927 evalcond
10928 [4])
10929 < 0.0000010000000000
10930 && IKabs(
10931 evalcond
10932 [5])
10933 < 0.0000010000000000)
10934 {
10935 bgotonextstatement
10936 = false;
10937 {
10938 IkReal j4array
10939 [4],
10940 cj4array
10941 [4],
10942 sj4array
10943 [4];
10944 bool j4valid
10945 [4]
10946 = {false};
10947 _nj4
10948 = 4;
10949 j4array
10950 [0]
10951 = 0;
10952 sj4array
10953 [0]
10954 = IKsin(
10955 j4array
10956 [0]);
10957 cj4array
10958 [0]
10959 = IKcos(
10960 j4array
10961 [0]);
10962 j4array
10963 [1]
10964 = 1.5707963267949;
10965 sj4array
10966 [1]
10967 = IKsin(
10968 j4array
10969 [1]);
10970 cj4array
10971 [1]
10972 = IKcos(
10973 j4array
10974 [1]);
10975 j4array
10976 [2]
10977 = 3.14159265358979;
10978 sj4array
10979 [2]
10980 = IKsin(
10981 j4array
10982 [2]);
10983 cj4array
10984 [2]
10985 = IKcos(
10986 j4array
10987 [2]);
10988 j4array
10989 [3]
10990 = -1.5707963267949;
10991 sj4array
10992 [3]
10993 = IKsin(
10994 j4array
10995 [3]);
10996 cj4array
10997 [3]
10998 = IKcos(
10999 j4array
11000 [3]);
11001 if (j4array
11002 [0]
11003 > IKPI)
11004 {
11005 j4array
11006 [0]
11007 -= IK2PI;
11008 }
11009 else if (
11010 j4array
11011 [0]
11012 < -IKPI)
11013 {
11014 j4array
11015 [0]
11016 += IK2PI;
11017 }
11018 j4valid
11019 [0]
11020 = true;
11021 if (j4array
11022 [1]
11023 > IKPI)
11024 {
11025 j4array
11026 [1]
11027 -= IK2PI;
11028 }
11029 else if (
11030 j4array
11031 [1]
11032 < -IKPI)
11033 {
11034 j4array
11035 [1]
11036 += IK2PI;
11037 }
11038 j4valid
11039 [1]
11040 = true;
11041 if (j4array
11042 [2]
11043 > IKPI)
11044 {
11045 j4array
11046 [2]
11047 -= IK2PI;
11048 }
11049 else if (
11050 j4array
11051 [2]
11052 < -IKPI)
11053 {
11054 j4array
11055 [2]
11056 += IK2PI;
11057 }
11058 j4valid
11059 [2]
11060 = true;
11061 if (j4array
11062 [3]
11063 > IKPI)
11064 {
11065 j4array
11066 [3]
11067 -= IK2PI;
11068 }
11069 else if (
11070 j4array
11071 [3]
11072 < -IKPI)
11073 {
11074 j4array
11075 [3]
11076 += IK2PI;
11077 }
11078 j4valid
11079 [3]
11080 = true;
11081 for (
11082 int ij4
11083 = 0;
11084 ij4
11085 < 4;
11086 ++ij4)
11087 {
11088 if (!j4valid
11089 [ij4])
11090 {
11091 continue;
11092 }
11093 _ij4[0]
11094 = ij4;
11095 _ij4[1]
11096 = -1;
11097 for (
11098 int iij4
11099 = ij4
11100 + 1;
11101 iij4
11102 < 4;
11103 ++iij4)
11104 {
11105 if (j4valid
11106 [iij4]
11107 && IKabs(
11108 cj4array
11109 [ij4]
11110 - cj4array
11111 [iij4])
11112 < IKFAST_SOLUTION_THRESH
11113 && IKabs(
11114 sj4array
11115 [ij4]
11116 - sj4array
11117 [iij4])
11118 < IKFAST_SOLUTION_THRESH)
11119 {
11120 j4valid
11121 [iij4]
11122 = false;
11123 _ij4[1]
11124 = iij4;
11125 break;
11126 }
11127 }
11128 j4 = j4array
11129 [ij4];
11130 cj4 = cj4array
11131 [ij4];
11132 sj4 = sj4array
11133 [ij4];
11134
11135 rotationfunction0(
11136 solutions);
11137 }
11138 }
11139 }
11140 } while (
11141 0);
11142 if (bgotonextstatement)
11143 {
11144 bool
11145 bgotonextstatement
11146 = true;
11147 do
11148 {
11149 if (1)
11150 {
11151 bgotonextstatement
11152 = false;
11153 continue; // branch miss [j4]
11154 }
11155 } while (
11156 0);
11157 if (bgotonextstatement)
11158 {
11159 }
11160 }
11161 }
11162 }
11163 else
11164 {
11165 {
11166 IkReal j4array
11167 [1],
11168 cj4array
11169 [1],
11170 sj4array
11171 [1];
11172 bool j4valid
11173 [1]
11174 = {false};
11175 _nj4
11176 = 1;
11177 IkReal
11178 x1133
11179 = (cj9
11180 * px);
11181 IkReal
11182 x1134
11183 = (cj9
11184 * py);
11185 IkReal
11186 x1135
11187 = (pp
11188 * px);
11189 IkReal
11190 x1136
11191 = (pp
11192 * py);
11193 CheckValue<IkReal> x1137 = IKatan2WithCheck(
11194 IkReal((
11195 (((4.31372549019608)
11196 * x1135))
11197 + (((-1.45555882352941)
11198 * x1133))
11199 + (((0.348408823529412)
11200 * py))
11201 + (((0.119091176470588)
11202 * x1134))
11203 + (((-1.0)
11204 * (1.66110784313725)
11205 * px))
11206 + (((0.647058823529412)
11207 * x1136)))),
11208 ((((0.348408823529412)
11209 * px))
11210 + (((0.119091176470588)
11211 * x1133))
11212 + (((1.66110784313725)
11213 * py))
11214 + (((1.45555882352941)
11215 * x1134))
11216 + (((-4.31372549019608)
11217 * x1136))
11218 + (((0.647058823529412)
11219 * x1135))),
11220 IKFAST_ATAN2_MAGTHRESH);
11221 if (!x1137
11222 .valid)
11223 {
11224 continue;
11225 }
11226 CheckValue<IkReal> x1138 = IKPowWithIntegerCheck(
11227 IKsign((
11228 (((1.1)
11229 * (pz
11230 * pz)))
11231 + (((-1.0)
11232 * (1.1)
11233 * pp)))),
11234 -1);
11235 if (!x1138
11236 .valid)
11237 {
11238 continue;
11239 }
11240 j4array
11241 [0]
11242 = ((-1.5707963267949)
11243 + (x1137
11244 .value)
11245 + (((1.5707963267949)
11246 * (x1138
11247 .value))));
11248 sj4array
11249 [0]
11250 = IKsin(
11251 j4array
11252 [0]);
11253 cj4array
11254 [0]
11255 = IKcos(
11256 j4array
11257 [0]);
11258 if (j4array
11259 [0]
11260 > IKPI)
11261 {
11262 j4array
11263 [0]
11264 -= IK2PI;
11265 }
11266 else if (
11267 j4array
11268 [0]
11269 < -IKPI)
11270 {
11271 j4array
11272 [0]
11273 += IK2PI;
11274 }
11275 j4valid
11276 [0]
11277 = true;
11278 for (
11279 int ij4
11280 = 0;
11281 ij4
11282 < 1;
11283 ++ij4)
11284 {
11285 if (!j4valid
11286 [ij4])
11287 {
11288 continue;
11289 }
11290 _ij4[0]
11291 = ij4;
11292 _ij4[1]
11293 = -1;
11294 for (
11295 int iij4
11296 = ij4
11297 + 1;
11298 iij4
11299 < 1;
11300 ++iij4)
11301 {
11302 if (j4valid
11303 [iij4]
11304 && IKabs(
11305 cj4array
11306 [ij4]
11307 - cj4array
11308 [iij4])
11309 < IKFAST_SOLUTION_THRESH
11310 && IKabs(
11311 sj4array
11312 [ij4]
11313 - sj4array
11314 [iij4])
11315 < IKFAST_SOLUTION_THRESH)
11316 {
11317 j4valid
11318 [iij4]
11319 = false;
11320 _ij4[1]
11321 = iij4;
11322 break;
11323 }
11324 }
11325 j4 = j4array
11326 [ij4];
11327 cj4 = cj4array
11328 [ij4];
11329 sj4 = sj4array
11330 [ij4];
11331 {
11332 IkReal evalcond
11333 [4];
11334 IkReal
11335 x1139
11336 = IKsin(
11337 j4);
11338 IkReal
11339 x1140
11340 = (px
11341 * x1139);
11342 IkReal
11343 x1141
11344 = IKcos(
11345 j4);
11346 IkReal
11347 x1142
11348 = (py
11349 * x1141);
11350 IkReal
11351 x1143
11352 = (px
11353 * x1141);
11354 IkReal
11355 x1144
11356 = (py
11357 * x1139);
11358 IkReal
11359 x1145
11360 = ((((-1.0)
11361 * x1144))
11362 + (((-1.0)
11363 * x1143)));
11364 evalcond
11365 [0]
11366 = ((-1.51009803921569)
11367 + (((-1.0)
11368 * (1.32323529411765)
11369 * cj9))
11370 + x1140
11371 + (((3.92156862745098)
11372 * pp))
11373 + (((-1.0)
11374 * x1142)));
11375 evalcond
11376 [1]
11377 = ((-0.55)
11378 + x1145
11379 + (((-1.0)
11380 * (0.3)
11381 * cj9))
11382 + (((-1.0)
11383 * (0.045)
11384 * sj9)));
11385 evalcond
11386 [2]
11387 = ((-0.316735294117647)
11388 + x1145
11389 + (((-1.0)
11390 * (0.588235294117647)
11391 * pp))
11392 + (((-1.0)
11393 * (0.108264705882353)
11394 * cj9)));
11395 evalcond
11396 [3]
11397 = ((-0.2125)
11398 + (((-0.09)
11399 * x1140))
11400 + (((-1.1)
11401 * x1144))
11402 + (((-1.1)
11403 * x1143))
11404 + (((0.09)
11405 * x1142))
11406 + (((-1.0)
11407 * (1.0)
11408 * pp)));
11409 if (IKabs(
11410 evalcond
11411 [0])
11412 > IKFAST_EVALCOND_THRESH
11413 || IKabs(
11414 evalcond
11415 [1])
11416 > IKFAST_EVALCOND_THRESH
11417 || IKabs(
11418 evalcond
11419 [2])
11420 > IKFAST_EVALCOND_THRESH
11421 || IKabs(
11422 evalcond
11423 [3])
11424 > IKFAST_EVALCOND_THRESH)
11425 {
11426 continue;
11427 }
11428 }
11429
11430 rotationfunction0(
11431 solutions);
11432 }
11433 }
11434 }
11435 }
11436 }
11437 else
11438 {
11439 {
11440 IkReal j4array
11441 [1],
11442 cj4array
11443 [1],
11444 sj4array
11445 [1];
11446 bool j4valid
11447 [1]
11448 = {false};
11449 _nj4 = 1;
11450 IkReal
11451 x1146
11452 = (cj9
11453 * px);
11454 IkReal
11455 x1147
11456 = (cj9
11457 * py);
11458 IkReal
11459 x1148
11460 = (pp
11461 * px);
11462 IkReal
11463 x1149
11464 = (pp
11465 * py);
11466 CheckValue<IkReal> x1150 = IKatan2WithCheck(
11467 IkReal((
11468 (((1.51009803921569)
11469 * px))
11470 + (((-1.0)
11471 * (0.316735294117647)
11472 * py))
11473 + (((-0.108264705882353)
11474 * x1147))
11475 + (((-0.588235294117647)
11476 * x1149))
11477 + (((1.32323529411765)
11478 * x1146))
11479 + (((-3.92156862745098)
11480 * x1148)))),
11481 ((((-0.108264705882353)
11482 * x1146))
11483 + (((-1.32323529411765)
11484 * x1147))
11485 + (((3.92156862745098)
11486 * x1149))
11487 + (((-1.0)
11488 * (1.51009803921569)
11489 * py))
11490 + (((-1.0)
11491 * (0.316735294117647)
11492 * px))
11493 + (((-0.588235294117647)
11494 * x1148))),
11495 IKFAST_ATAN2_MAGTHRESH);
11496 if (!x1150
11497 .valid)
11498 {
11499 continue;
11500 }
11501 CheckValue<IkReal> x1151 = IKPowWithIntegerCheck(
11502 IKsign((
11503 pp
11504 + (((-1.0)
11505 * (1.0)
11506 * (pz
11507 * pz))))),
11508 -1);
11509 if (!x1151
11510 .valid)
11511 {
11512 continue;
11513 }
11514 j4array[0]
11515 = ((-1.5707963267949)
11516 + (x1150
11517 .value)
11518 + (((1.5707963267949)
11519 * (x1151
11520 .value))));
11521 sj4array[0] = IKsin(
11522 j4array
11523 [0]);
11524 cj4array[0] = IKcos(
11525 j4array
11526 [0]);
11527 if (j4array
11528 [0]
11529 > IKPI)
11530 {
11531 j4array
11532 [0]
11533 -= IK2PI;
11534 }
11535 else if (
11536 j4array
11537 [0]
11538 < -IKPI)
11539 {
11540 j4array
11541 [0]
11542 += IK2PI;
11543 }
11544 j4valid[0]
11545 = true;
11546 for (
11547 int ij4
11548 = 0;
11549 ij4
11550 < 1;
11551 ++ij4)
11552 {
11553 if (!j4valid
11554 [ij4])
11555 {
11556 continue;
11557 }
11558 _ij4[0]
11559 = ij4;
11560 _ij4[1]
11561 = -1;
11562 for (
11563 int iij4
11564 = ij4
11565 + 1;
11566 iij4
11567 < 1;
11568 ++iij4)
11569 {
11570 if (j4valid
11571 [iij4]
11572 && IKabs(
11573 cj4array
11574 [ij4]
11575 - cj4array
11576 [iij4])
11577 < IKFAST_SOLUTION_THRESH
11578 && IKabs(
11579 sj4array
11580 [ij4]
11581 - sj4array
11582 [iij4])
11583 < IKFAST_SOLUTION_THRESH)
11584 {
11585 j4valid
11586 [iij4]
11587 = false;
11588 _ij4[1]
11589 = iij4;
11590 break;
11591 }
11592 }
11593 j4 = j4array
11594 [ij4];
11595 cj4 = cj4array
11596 [ij4];
11597 sj4 = sj4array
11598 [ij4];
11599 {
11600 IkReal evalcond
11601 [4];
11602 IkReal
11603 x1152
11604 = IKsin(
11605 j4);
11606 IkReal
11607 x1153
11608 = (px
11609 * x1152);
11610 IkReal
11611 x1154
11612 = IKcos(
11613 j4);
11614 IkReal
11615 x1155
11616 = (py
11617 * x1154);
11618 IkReal
11619 x1156
11620 = (px
11621 * x1154);
11622 IkReal
11623 x1157
11624 = (py
11625 * x1152);
11626 IkReal
11627 x1158
11628 = ((((-1.0)
11629 * x1156))
11630 + (((-1.0)
11631 * x1157)));
11632 evalcond
11633 [0]
11634 = ((-1.51009803921569)
11635 + (((-1.0)
11636 * x1155))
11637 + x1153
11638 + (((-1.0)
11639 * (1.32323529411765)
11640 * cj9))
11641 + (((3.92156862745098)
11642 * pp)));
11643 evalcond
11644 [1]
11645 = ((-0.55)
11646 + x1158
11647 + (((-1.0)
11648 * (0.3)
11649 * cj9))
11650 + (((-1.0)
11651 * (0.045)
11652 * sj9)));
11653 evalcond
11654 [2]
11655 = ((-0.316735294117647)
11656 + x1158
11657 + (((-1.0)
11658 * (0.588235294117647)
11659 * pp))
11660 + (((-1.0)
11661 * (0.108264705882353)
11662 * cj9)));
11663 evalcond
11664 [3]
11665 = ((-0.2125)
11666 + (((-1.1)
11667 * x1156))
11668 + (((-1.1)
11669 * x1157))
11670 + (((-0.09)
11671 * x1153))
11672 + (((-1.0)
11673 * (1.0)
11674 * pp))
11675 + (((0.09)
11676 * x1155)));
11677 if (IKabs(
11678 evalcond
11679 [0])
11680 > IKFAST_EVALCOND_THRESH
11681 || IKabs(
11682 evalcond
11683 [1])
11684 > IKFAST_EVALCOND_THRESH
11685 || IKabs(
11686 evalcond
11687 [2])
11688 > IKFAST_EVALCOND_THRESH
11689 || IKabs(
11690 evalcond
11691 [3])
11692 > IKFAST_EVALCOND_THRESH)
11693 {
11694 continue;
11695 }
11696 }
11697
11698 rotationfunction0(
11699 solutions);
11700 }
11701 }
11702 }
11703 }
11704 }
11705 else
11706 {
11707 {
11708 IkReal j4array
11709 [1],
11710 cj4array
11711 [1],
11712 sj4array
11713 [1];
11714 bool
11715 j4valid[1]
11716 = {false};
11717 _nj4 = 1;
11718 IkReal x1159
11719 = (cj9
11720 * px);
11721 IkReal x1160
11722 = (cj9
11723 * py);
11724 IkReal x1161
11725 = ((3.92156862745098)
11726 * pp);
11727 IkReal x1162
11728 = ((0.045)
11729 * sj9);
11730 CheckValue<IkReal> x1163 = IKPowWithIntegerCheck(
11731 IKsign((
11732 pp
11733 + (((-1.0)
11734 * (1.0)
11735 * (pz
11736 * pz))))),
11737 -1);
11738 if (!x1163
11739 .valid)
11740 {
11741 continue;
11742 }
11743 CheckValue<IkReal> x1164 = IKatan2WithCheck(
11744 IkReal((
11745 (((-0.3)
11746 * x1160))
11747 + (((1.51009803921569)
11748 * px))
11749 + (((-1.0)
11750 * py
11751 * x1162))
11752 + (((-1.0)
11753 * px
11754 * x1161))
11755 + (((1.32323529411765)
11756 * x1159))
11757 + (((-1.0)
11758 * (0.55)
11759 * py)))),
11760 ((((-1.0)
11761 * (0.55)
11762 * px))
11763 + (((-1.32323529411765)
11764 * x1160))
11765 + (((-1.0)
11766 * (1.51009803921569)
11767 * py))
11768 + (((-0.3)
11769 * x1159))
11770 + ((py
11771 * x1161))
11772 + (((-1.0)
11773 * px
11774 * x1162))),
11775 IKFAST_ATAN2_MAGTHRESH);
11776 if (!x1164
11777 .valid)
11778 {
11779 continue;
11780 }
11781 j4array[0]
11782 = ((-1.5707963267949)
11783 + (((1.5707963267949)
11784 * (x1163
11785 .value)))
11786 + (x1164
11787 .value));
11788 sj4array[0]
11789 = IKsin(
11790 j4array
11791 [0]);
11792 cj4array[0]
11793 = IKcos(
11794 j4array
11795 [0]);
11796 if (j4array[0]
11797 > IKPI)
11798 {
11799 j4array[0]
11800 -= IK2PI;
11801 }
11802 else if (
11803 j4array[0]
11804 < -IKPI)
11805 {
11806 j4array[0]
11807 += IK2PI;
11808 }
11809 j4valid[0]
11810 = true;
11811 for (int ij4
11812 = 0;
11813 ij4 < 1;
11814 ++ij4)
11815 {
11816 if (!j4valid
11817 [ij4])
11818 {
11819 continue;
11820 }
11821 _ij4[0]
11822 = ij4;
11823 _ij4[1]
11824 = -1;
11825 for (
11826 int iij4
11827 = ij4
11828 + 1;
11829 iij4
11830 < 1;
11831 ++iij4)
11832 {
11833 if (j4valid
11834 [iij4]
11835 && IKabs(
11836 cj4array
11837 [ij4]
11838 - cj4array
11839 [iij4])
11840 < IKFAST_SOLUTION_THRESH
11841 && IKabs(
11842 sj4array
11843 [ij4]
11844 - sj4array
11845 [iij4])
11846 < IKFAST_SOLUTION_THRESH)
11847 {
11848 j4valid
11849 [iij4]
11850 = false;
11851 _ij4[1]
11852 = iij4;
11853 break;
11854 }
11855 }
11856 j4 = j4array
11857 [ij4];
11858 cj4 = cj4array
11859 [ij4];
11860 sj4 = sj4array
11861 [ij4];
11862 {
11863 IkReal evalcond
11864 [4];
11865 IkReal
11866 x1165
11867 = IKsin(
11868 j4);
11869 IkReal
11870 x1166
11871 = (px
11872 * x1165);
11873 IkReal
11874 x1167
11875 = IKcos(
11876 j4);
11877 IkReal
11878 x1168
11879 = (py
11880 * x1167);
11881 IkReal
11882 x1169
11883 = (px
11884 * x1167);
11885 IkReal
11886 x1170
11887 = (py
11888 * x1165);
11889 IkReal
11890 x1171
11891 = ((((-1.0)
11892 * x1169))
11893 + (((-1.0)
11894 * x1170)));
11895 evalcond
11896 [0]
11897 = ((-1.51009803921569)
11898 + (((-1.0)
11899 * (1.32323529411765)
11900 * cj9))
11901 + (((-1.0)
11902 * x1168))
11903 + (((3.92156862745098)
11904 * pp))
11905 + x1166);
11906 evalcond
11907 [1]
11908 = ((-0.55)
11909 + (((-1.0)
11910 * (0.3)
11911 * cj9))
11912 + x1171
11913 + (((-1.0)
11914 * (0.045)
11915 * sj9)));
11916 evalcond
11917 [2]
11918 = ((-0.316735294117647)
11919 + (((-1.0)
11920 * (0.588235294117647)
11921 * pp))
11922 + (((-1.0)
11923 * (0.108264705882353)
11924 * cj9))
11925 + x1171);
11926 evalcond
11927 [3]
11928 = ((-0.2125)
11929 + (((-1.1)
11930 * x1170))
11931 + (((-1.1)
11932 * x1169))
11933 + (((0.09)
11934 * x1168))
11935 + (((-0.09)
11936 * x1166))
11937 + (((-1.0)
11938 * (1.0)
11939 * pp)));
11940 if (IKabs(
11941 evalcond
11942 [0])
11943 > IKFAST_EVALCOND_THRESH
11944 || IKabs(
11945 evalcond
11946 [1])
11947 > IKFAST_EVALCOND_THRESH
11948 || IKabs(
11949 evalcond
11950 [2])
11951 > IKFAST_EVALCOND_THRESH
11952 || IKabs(
11953 evalcond
11954 [3])
11955 > IKFAST_EVALCOND_THRESH)
11956 {
11957 continue;
11958 }
11959 }
11960
11961 rotationfunction0(
11962 solutions);
11963 }
11964 }
11965 }
11966 }
11967 }
11968 } while (0);
11969 if (bgotonextstatement)
11970 {
11971 bool
11972 bgotonextstatement
11973 = true;
11974 do
11975 {
11976 IkReal x1172
11977 = (cj6 * pz);
11978 IkReal x1173
11979 = ((-1.51009803921569)
11980 + (((-1.0)
11981 * (1.32323529411765)
11982 * cj9))
11983 + (((3.92156862745098)
11984 * pp)));
11985 evalcond[0]
11986 = ((IKabs(py))
11987 + (IKabs(
11988 px)));
11989 evalcond[1]
11990 = ((-0.55)
11991 + (((-1.0)
11992 * (0.3)
11993 * cj9))
11994 + x1172
11995 + (((-1.0)
11996 * (0.045)
11997 * sj9)));
11998 evalcond[2] = x1173;
11999 evalcond[3]
12000 = ((-1.0)
12001 * (((1.0)
12002 * pz
12003 * sj6)));
12004 evalcond[4] = x1173;
12005 evalcond[5]
12006 = ((((0.108264705882353)
12007 * cj9
12008 * sj6))
12009 + (((0.588235294117647)
12010 * pp
12011 * sj6))
12012 + (((0.316735294117647)
12013 * sj6)));
12014 evalcond[6]
12015 = ((-0.2125)
12016 + (((1.1)
12017 * x1172))
12018 + (((-1.0)
12019 * (1.0)
12020 * pp)));
12021 if (IKabs(
12022 evalcond[0])
12023 < 0.0000010000000000
12024 && IKabs(
12025 evalcond
12026 [1])
12027 < 0.0000010000000000
12028 && IKabs(
12029 evalcond
12030 [2])
12031 < 0.0000010000000000
12032 && IKabs(
12033 evalcond
12034 [3])
12035 < 0.0000010000000000
12036 && IKabs(
12037 evalcond
12038 [4])
12039 < 0.0000010000000000
12040 && IKabs(
12041 evalcond
12042 [5])
12043 < 0.0000010000000000
12044 && IKabs(
12045 evalcond
12046 [6])
12047 < 0.0000010000000000)
12048 {
12049 bgotonextstatement
12050 = false;
12051 {
12052 IkReal
12053 j4array[4],
12054 cj4array[4],
12055 sj4array[4];
12056 bool j4valid[4]
12057 = {false};
12058 _nj4 = 4;
12059 j4array[0] = 0;
12060 sj4array[0]
12061 = IKsin(
12062 j4array
12063 [0]);
12064 cj4array[0]
12065 = IKcos(
12066 j4array
12067 [0]);
12068 j4array[1]
12069 = 1.5707963267949;
12070 sj4array[1]
12071 = IKsin(
12072 j4array
12073 [1]);
12074 cj4array[1]
12075 = IKcos(
12076 j4array
12077 [1]);
12078 j4array[2]
12079 = 3.14159265358979;
12080 sj4array[2]
12081 = IKsin(
12082 j4array
12083 [2]);
12084 cj4array[2]
12085 = IKcos(
12086 j4array
12087 [2]);
12088 j4array[3]
12089 = -1.5707963267949;
12090 sj4array[3]
12091 = IKsin(
12092 j4array
12093 [3]);
12094 cj4array[3]
12095 = IKcos(
12096 j4array
12097 [3]);
12098 if (j4array[0]
12099 > IKPI)
12100 {
12101 j4array[0]
12102 -= IK2PI;
12103 }
12104 else if (
12105 j4array[0]
12106 < -IKPI)
12107 {
12108 j4array[0]
12109 += IK2PI;
12110 }
12111 j4valid[0]
12112 = true;
12113 if (j4array[1]
12114 > IKPI)
12115 {
12116 j4array[1]
12117 -= IK2PI;
12118 }
12119 else if (
12120 j4array[1]
12121 < -IKPI)
12122 {
12123 j4array[1]
12124 += IK2PI;
12125 }
12126 j4valid[1]
12127 = true;
12128 if (j4array[2]
12129 > IKPI)
12130 {
12131 j4array[2]
12132 -= IK2PI;
12133 }
12134 else if (
12135 j4array[2]
12136 < -IKPI)
12137 {
12138 j4array[2]
12139 += IK2PI;
12140 }
12141 j4valid[2]
12142 = true;
12143 if (j4array[3]
12144 > IKPI)
12145 {
12146 j4array[3]
12147 -= IK2PI;
12148 }
12149 else if (
12150 j4array[3]
12151 < -IKPI)
12152 {
12153 j4array[3]
12154 += IK2PI;
12155 }
12156 j4valid[3]
12157 = true;
12158 for (int ij4
12159 = 0;
12160 ij4 < 4;
12161 ++ij4)
12162 {
12163 if (!j4valid
12164 [ij4])
12165 {
12166 continue;
12167 }
12168 _ij4[0] = ij4;
12169 _ij4[1] = -1;
12170 for (int iij4
12171 = ij4
12172 + 1;
12173 iij4 < 4;
12174 ++iij4)
12175 {
12176 if (j4valid
12177 [iij4]
12178 && IKabs(
12179 cj4array
12180 [ij4]
12181 - cj4array
12182 [iij4])
12183 < IKFAST_SOLUTION_THRESH
12184 && IKabs(
12185 sj4array
12186 [ij4]
12187 - sj4array
12188 [iij4])
12189 < IKFAST_SOLUTION_THRESH)
12190 {
12191 j4valid
12192 [iij4]
12193 = false;
12194 _ij4[1]
12195 = iij4;
12196 break;
12197 }
12198 }
12199 j4 = j4array
12200 [ij4];
12201 cj4 = cj4array
12202 [ij4];
12203 sj4 = sj4array
12204 [ij4];
12205
12206 rotationfunction0(
12207 solutions);
12208 }
12209 }
12210 }
12211 } while (0);
12212 if (bgotonextstatement)
12213 {
12214 bool
12215 bgotonextstatement
12216 = true;
12217 do
12218 {
12219 if (1)
12220 {
12221 bgotonextstatement
12222 = false;
12223 continue; // branch
12224 // miss
12225 // [j4]
12226 }
12227 } while (0);
12228 if (bgotonextstatement)
12229 {
12230 }
12231 }
12232 }
12233 }
12234 }
12235 }
12236 else
12237 {
12238 {
12239 IkReal j4array[1],
12240 cj4array[1],
12241 sj4array[1];
12242 bool j4valid[1] = {false};
12243 _nj4 = 1;
12244 IkReal x1174
12245 = ((1.32323529411765)
12246 * cj9);
12247 IkReal x1175
12248 = ((3.92156862745098)
12249 * pp);
12250 IkReal x1176
12251 = ((0.316735294117647)
12252 * sj6);
12253 IkReal x1177
12254 = ((0.108264705882353)
12255 * cj9 * sj6);
12256 IkReal x1178
12257 = ((0.588235294117647)
12258 * pp * sj6);
12259 CheckValue<IkReal> x1179
12260 = IKatan2WithCheck(
12261 IkReal((
12262 (((-1.0) * px
12263 * x1175))
12264 + ((px
12265 * x1174))
12266 + (((1.51009803921569)
12267 * px))
12268 + ((py
12269 * x1178))
12270 + ((py
12271 * x1177))
12272 + ((py
12273 * x1176)))),
12274 (((px * x1176))
12275 + (((-1.0)
12276 * (1.51009803921569)
12277 * py))
12278 + ((px * x1178))
12279 + ((py * x1175))
12280 + (((-1.0) * py
12281 * x1174))
12282 + ((px
12283 * x1177))),
12284 IKFAST_ATAN2_MAGTHRESH);
12285 if (!x1179.valid)
12286 {
12287 continue;
12288 }
12289 CheckValue<IkReal> x1180
12290 = IKPowWithIntegerCheck(
12291 IKsign((
12292 pp
12293 + (((-1.0)
12294 * (1.0)
12295 * (pz
12296 * pz))))),
12297 -1);
12298 if (!x1180.valid)
12299 {
12300 continue;
12301 }
12302 j4array[0]
12303 = ((-1.5707963267949)
12304 + (x1179.value)
12305 + (((1.5707963267949)
12306 * (x1180
12307 .value))));
12308 sj4array[0]
12309 = IKsin(j4array[0]);
12310 cj4array[0]
12311 = IKcos(j4array[0]);
12312 if (j4array[0] > IKPI)
12313 {
12314 j4array[0] -= IK2PI;
12315 }
12316 else if (
12317 j4array[0] < -IKPI)
12318 {
12319 j4array[0] += IK2PI;
12320 }
12321 j4valid[0] = true;
12322 for (int ij4 = 0; ij4 < 1;
12323 ++ij4)
12324 {
12325 if (!j4valid[ij4])
12326 {
12327 continue;
12328 }
12329 _ij4[0] = ij4;
12330 _ij4[1] = -1;
12331 for (int iij4 = ij4 + 1;
12332 iij4 < 1;
12333 ++iij4)
12334 {
12335 if (j4valid[iij4]
12336 && IKabs(
12337 cj4array
12338 [ij4]
12339 - cj4array
12340 [iij4])
12341 < IKFAST_SOLUTION_THRESH
12342 && IKabs(
12343 sj4array
12344 [ij4]
12345 - sj4array
12346 [iij4])
12347 < IKFAST_SOLUTION_THRESH)
12348 {
12349 j4valid[iij4]
12350 = false;
12351 _ij4[1] = iij4;
12352 break;
12353 }
12354 }
12355 j4 = j4array[ij4];
12356 cj4 = cj4array[ij4];
12357 sj4 = sj4array[ij4];
12358 {
12359 IkReal evalcond[5];
12360 IkReal x1181
12361 = IKcos(j4);
12362 IkReal x1182
12363 = (px * x1181);
12364 IkReal x1183
12365 = IKsin(j4);
12366 IkReal x1184
12367 = (py * x1183);
12368 IkReal x1185
12369 = (px * x1183);
12370 IkReal x1186
12371 = ((1.0) * x1181);
12372 IkReal x1187
12373 = (cj6 * pz);
12374 IkReal x1188
12375 = (sj6 * x1182);
12376 IkReal x1189
12377 = (sj6 * x1184);
12378 evalcond[0]
12379 = (((cj6 * x1184))
12380 + ((cj6
12381 * x1182))
12382 + (((-1.0)
12383 * (1.0)
12384 * pz
12385 * sj6)));
12386 evalcond[1]
12387 = ((-1.51009803921569)
12388 + (((-1.0) * py
12389 * x1186))
12390 + (((-1.0)
12391 * (1.32323529411765)
12392 * cj9))
12393 + (((3.92156862745098)
12394 * pp))
12395 + x1185);
12396 evalcond[2]
12397 = ((-0.55)
12398 + (((-1.0)
12399 * (0.3)
12400 * cj9))
12401 + x1187 + x1188
12402 + x1189
12403 + (((-1.0)
12404 * (0.045)
12405 * sj9)));
12406 evalcond[3]
12407 = ((((0.108264705882353)
12408 * cj9 * sj6))
12409 + (((-1.0)
12410 * x1184))
12411 + (((-1.0) * px
12412 * x1186))
12413 + (((0.588235294117647)
12414 * pp
12415 * sj6))
12416 + (((0.316735294117647)
12417 * sj6)));
12418 evalcond[4]
12419 = ((-0.2125)
12420 + (((1.1)
12421 * x1188))
12422 + (((-0.09)
12423 * x1185))
12424 + (((0.09) * py
12425 * x1181))
12426 + (((-1.0)
12427 * (1.0)
12428 * pp))
12429 + (((1.1)
12430 * x1187))
12431 + (((1.1)
12432 * x1189)));
12433 if (IKabs(evalcond[0])
12434 > IKFAST_EVALCOND_THRESH
12435 || IKabs(evalcond
12436 [1])
12437 > IKFAST_EVALCOND_THRESH
12438 || IKabs(evalcond
12439 [2])
12440 > IKFAST_EVALCOND_THRESH
12441 || IKabs(evalcond
12442 [3])
12443 > IKFAST_EVALCOND_THRESH
12444 || IKabs(evalcond
12445 [4])
12446 > IKFAST_EVALCOND_THRESH)
12447 {
12448 continue;
12449 }
12450 }
12451
12452 rotationfunction0(
12453 solutions);
12454 }
12455 }
12456 }
12457 }
12458 }
12459 else
12460 {
12461 {
12462 IkReal j4array[1],
12463 cj4array[1], sj4array[1];
12464 bool j4valid[1] = {false};
12465 _nj4 = 1;
12466 IkReal x1190 = (cj6 * pp);
12467 IkReal x1191
12468 = ((0.2125) * cj6);
12469 IkReal x1192 = ((1.1) * pz);
12470 IkReal x1193
12471 = ((0.09) * pz * sj6);
12472 CheckValue<IkReal> x1194
12473 = IKatan2WithCheck(
12474 IkReal(
12475 (((px * x1192))
12476 + (((-1.0) * px
12477 * x1190))
12478 + ((py * x1193))
12479 + (((-1.0) * px
12480 * x1191)))),
12481 (((py * x1190))
12482 + (((-1.0) * py
12483 * x1192))
12484 + ((py * x1191))
12485 + ((px * x1193))),
12486 IKFAST_ATAN2_MAGTHRESH);
12487 if (!x1194.valid)
12488 {
12489 continue;
12490 }
12491 CheckValue<IkReal> x1195
12492 = IKPowWithIntegerCheck(
12493 IKsign((
12494 (((0.09) * x1190))
12495 + (((-1.0)
12496 * (0.09) * cj6
12497 * (pz
12498 * pz))))),
12499 -1);
12500 if (!x1195.valid)
12501 {
12502 continue;
12503 }
12504 j4array[0]
12505 = ((-1.5707963267949)
12506 + (x1194.value)
12507 + (((1.5707963267949)
12508 * (x1195.value))));
12509 sj4array[0]
12510 = IKsin(j4array[0]);
12511 cj4array[0]
12512 = IKcos(j4array[0]);
12513 if (j4array[0] > IKPI)
12514 {
12515 j4array[0] -= IK2PI;
12516 }
12517 else if (j4array[0] < -IKPI)
12518 {
12519 j4array[0] += IK2PI;
12520 }
12521 j4valid[0] = true;
12522 for (int ij4 = 0; ij4 < 1;
12523 ++ij4)
12524 {
12525 if (!j4valid[ij4])
12526 {
12527 continue;
12528 }
12529 _ij4[0] = ij4;
12530 _ij4[1] = -1;
12531 for (int iij4 = ij4 + 1;
12532 iij4 < 1;
12533 ++iij4)
12534 {
12535 if (j4valid[iij4]
12536 && IKabs(
12537 cj4array[ij4]
12538 - cj4array
12539 [iij4])
12540 < IKFAST_SOLUTION_THRESH
12541 && IKabs(
12542 sj4array[ij4]
12543 - sj4array
12544 [iij4])
12545 < IKFAST_SOLUTION_THRESH)
12546 {
12547 j4valid[iij4] = false;
12548 _ij4[1] = iij4;
12549 break;
12550 }
12551 }
12552 j4 = j4array[ij4];
12553 cj4 = cj4array[ij4];
12554 sj4 = sj4array[ij4];
12555 {
12556 IkReal evalcond[5];
12557 IkReal x1196 = IKcos(j4);
12558 IkReal x1197
12559 = (px * x1196);
12560 IkReal x1198 = IKsin(j4);
12561 IkReal x1199
12562 = (py * x1198);
12563 IkReal x1200
12564 = (px * x1198);
12565 IkReal x1201
12566 = ((1.0) * x1196);
12567 IkReal x1202 = (cj6 * pz);
12568 IkReal x1203
12569 = (sj6 * x1197);
12570 IkReal x1204
12571 = (sj6 * x1199);
12572 evalcond[0]
12573 = (((cj6 * x1199))
12574 + ((cj6 * x1197))
12575 + (((-1.0) * (1.0)
12576 * pz * sj6)));
12577 evalcond[1]
12578 = ((-1.51009803921569)
12579 + x1200
12580 + (((-1.0)
12581 * (1.32323529411765)
12582 * cj9))
12583 + (((-1.0) * py
12584 * x1201))
12585 + (((3.92156862745098)
12586 * pp)));
12587 evalcond[2]
12588 = ((-0.55) + x1202
12589 + x1203 + x1204
12590 + (((-1.0) * (0.3)
12591 * cj9))
12592 + (((-1.0)
12593 * (0.045)
12594 * sj9)));
12595 evalcond[3]
12596 = ((((0.108264705882353)
12597 * cj9 * sj6))
12598 + (((0.588235294117647)
12599 * pp * sj6))
12600 + (((-1.0) * px
12601 * x1201))
12602 + (((0.316735294117647)
12603 * sj6))
12604 + (((-1.0)
12605 * x1199)));
12606 evalcond[4]
12607 = ((-0.2125)
12608 + (((1.1) * x1204))
12609 + (((1.1) * x1203))
12610 + (((-0.09)
12611 * x1200))
12612 + (((0.09) * py
12613 * x1196))
12614 + (((1.1) * x1202))
12615 + (((-1.0) * (1.0)
12616 * pp)));
12617 if (IKabs(evalcond[0])
12618 > IKFAST_EVALCOND_THRESH
12619 || IKabs(evalcond[1])
12620 > IKFAST_EVALCOND_THRESH
12621 || IKabs(evalcond[2])
12622 > IKFAST_EVALCOND_THRESH
12623 || IKabs(evalcond[3])
12624 > IKFAST_EVALCOND_THRESH
12625 || IKabs(evalcond[4])
12626 > IKFAST_EVALCOND_THRESH)
12627 {
12628 continue;
12629 }
12630 }
12631
12632 rotationfunction0(
12633 solutions);
12634 }
12635 }
12636 }
12637 }
12638 }
12639 else
12640 {
12641 {
12642 IkReal j4array[1], cj4array[1],
12643 sj4array[1];
12644 bool j4valid[1] = {false};
12645 _nj4 = 1;
12646 IkReal x1205
12647 = ((1.51009803921569) * cj6);
12648 IkReal x1206 = (pz * sj6);
12649 IkReal x1207
12650 = ((1.32323529411765) * cj6
12651 * cj9);
12652 IkReal x1208
12653 = ((3.92156862745098) * cj6
12654 * pp);
12655 CheckValue<IkReal> x1209
12656 = IKatan2WithCheck(
12657 IkReal(
12658 (((px * x1205))
12659 + ((px * x1207))
12660 + ((py * x1206))
12661 + (((-1.0) * px
12662 * x1208)))),
12663 ((((-1.0) * py * x1205))
12664 + ((py * x1208))
12665 + ((px * x1206))
12666 + (((-1.0) * py
12667 * x1207))),
12668 IKFAST_ATAN2_MAGTHRESH);
12669 if (!x1209.valid)
12670 {
12671 continue;
12672 }
12673 CheckValue<IkReal> x1210
12674 = IKPowWithIntegerCheck(
12675 IKsign(
12676 ((((-1.0) * (1.0)
12677 * cj6 * (pz * pz)))
12678 + ((cj6 * pp)))),
12679 -1);
12680 if (!x1210.valid)
12681 {
12682 continue;
12683 }
12684 j4array[0]
12685 = ((-1.5707963267949)
12686 + (x1209.value)
12687 + (((1.5707963267949)
12688 * (x1210.value))));
12689 sj4array[0] = IKsin(j4array[0]);
12690 cj4array[0] = IKcos(j4array[0]);
12691 if (j4array[0] > IKPI)
12692 {
12693 j4array[0] -= IK2PI;
12694 }
12695 else if (j4array[0] < -IKPI)
12696 {
12697 j4array[0] += IK2PI;
12698 }
12699 j4valid[0] = true;
12700 for (int ij4 = 0; ij4 < 1; ++ij4)
12701 {
12702 if (!j4valid[ij4])
12703 {
12704 continue;
12705 }
12706 _ij4[0] = ij4;
12707 _ij4[1] = -1;
12708 for (int iij4 = ij4 + 1;
12709 iij4 < 1;
12710 ++iij4)
12711 {
12712 if (j4valid[iij4]
12713 && IKabs(
12714 cj4array[ij4]
12715 - cj4array[iij4])
12716 < IKFAST_SOLUTION_THRESH
12717 && IKabs(
12718 sj4array[ij4]
12719 - sj4array[iij4])
12720 < IKFAST_SOLUTION_THRESH)
12721 {
12722 j4valid[iij4] = false;
12723 _ij4[1] = iij4;
12724 break;
12725 }
12726 }
12727 j4 = j4array[ij4];
12728 cj4 = cj4array[ij4];
12729 sj4 = sj4array[ij4];
12730 {
12731 IkReal evalcond[5];
12732 IkReal x1211 = IKcos(j4);
12733 IkReal x1212 = (px * x1211);
12734 IkReal x1213 = IKsin(j4);
12735 IkReal x1214 = (py * x1213);
12736 IkReal x1215 = (px * x1213);
12737 IkReal x1216
12738 = ((1.0) * x1211);
12739 IkReal x1217 = (cj6 * pz);
12740 IkReal x1218 = (sj6 * x1212);
12741 IkReal x1219 = (sj6 * x1214);
12742 evalcond[0]
12743 = (((cj6 * x1212))
12744 + ((cj6 * x1214))
12745 + (((-1.0) * (1.0) * pz
12746 * sj6)));
12747 evalcond[1]
12748 = ((-1.51009803921569)
12749 + (((-1.0)
12750 * (1.32323529411765)
12751 * cj9))
12752 + (((3.92156862745098)
12753 * pp))
12754 + x1215
12755 + (((-1.0) * py
12756 * x1216)));
12757 evalcond[2]
12758 = ((-0.55)
12759 + (((-1.0) * (0.3)
12760 * cj9))
12761 + x1218 + x1219 + x1217
12762 + (((-1.0) * (0.045)
12763 * sj9)));
12764 evalcond[3]
12765 = ((((0.108264705882353)
12766 * cj9 * sj6))
12767 + (((-1.0) * x1214))
12768 + (((0.588235294117647)
12769 * pp * sj6))
12770 + (((-1.0) * px
12771 * x1216))
12772 + (((0.316735294117647)
12773 * sj6)));
12774 evalcond[4]
12775 = ((-0.2125)
12776 + (((1.1) * x1217))
12777 + (((1.1) * x1218))
12778 + (((-0.09) * x1215))
12779 + (((1.1) * x1219))
12780 + (((-1.0) * (1.0)
12781 * pp))
12782 + (((0.09) * py
12783 * x1211)));
12784 if (IKabs(evalcond[0])
12785 > IKFAST_EVALCOND_THRESH
12786 || IKabs(evalcond[1])
12787 > IKFAST_EVALCOND_THRESH
12788 || IKabs(evalcond[2])
12789 > IKFAST_EVALCOND_THRESH
12790 || IKabs(evalcond[3])
12791 > IKFAST_EVALCOND_THRESH
12792 || IKabs(evalcond[4])
12793 > IKFAST_EVALCOND_THRESH)
12794 {
12795 continue;
12796 }
12797 }
12798
12799 rotationfunction0(solutions);
12800 }
12801 }
12802 }
12803 }
12804 }
12805 } while (0);
12806 if (bgotonextstatement)
12807 {
12808 bool bgotonextstatement = true;
12809 do
12810 {
12811 evalcond[0]
12812 = ((-3.14159265358979)
12813 + (IKfmod(
12814 ((3.14159265358979)
12815 + (IKabs(
12816 ((1.5707963267949)
12817 + j8)))),
12818 6.28318530717959)));
12819 evalcond[1]
12820 = ((0.39655) + (((0.0765) * sj9))
12821 + (((0.32595) * cj9))
12822 + (((-1.0) * (1.0) * pp)));
12823 evalcond[2]
12824 = ((((-1.0) * (0.3) * cj6 * cj9))
12825 + (((-1.0) * (0.55) * cj6)) + pz
12826 + (((-1.0) * (0.045) * cj6
12827 * sj9)));
12828 if (IKabs(evalcond[0])
12829 < 0.0000010000000000
12830 && IKabs(evalcond[1])
12831 < 0.0000010000000000
12832 && IKabs(evalcond[2])
12833 < 0.0000010000000000)
12834 {
12835 bgotonextstatement = false;
12836 {
12837 IkReal j4eval[3];
12838 sj8 = -1.0;
12839 cj8 = 0;
12840 j8 = -1.5707963267949;
12841 IkReal x1220
12842 = ((((-1.0) * (1.0) * cj6
12843 * (pz * pz)))
12844 + ((cj6 * pp)));
12845 IkReal x1221
12846 = ((1.51009803921569) * cj6);
12847 IkReal x1222 = (pz * sj6);
12848 IkReal x1223
12849 = ((1.32323529411765) * cj6
12850 * cj9);
12851 IkReal x1224
12852 = ((3.92156862745098) * cj6
12853 * pp);
12854 j4eval[0] = x1220;
12855 j4eval[1]
12856 = ((IKabs(
12857 (((py * x1221))
12858 + ((py * x1223))
12859 + ((px * x1222))
12860 + (((-1.0) * py
12861 * x1224)))))
12862 + (IKabs((
12863 (((-1.0) * px * x1221))
12864 + ((py * x1222))
12865 + (((-1.0) * px * x1223))
12866 + ((px * x1224))))));
12867 j4eval[2] = IKsign(x1220);
12868 if (IKabs(j4eval[0])
12869 < 0.0000010000000000
12870 || IKabs(j4eval[1])
12871 < 0.0000010000000000
12872 || IKabs(j4eval[2])
12873 < 0.0000010000000000)
12874 {
12875 {
12876 IkReal j4eval[3];
12877 sj8 = -1.0;
12878 cj8 = 0;
12879 j8 = -1.5707963267949;
12880 IkReal x1225 = (cj6 * pp);
12881 IkReal x1226 = ((1.0) * x1225);
12882 IkReal x1227
12883 = (cj6 * (pz * pz));
12884 IkReal x1228 = ((0.2125) * cj6);
12885 IkReal x1229 = ((1.1) * pz);
12886 IkReal x1230
12887 = ((0.09) * pz * sj6);
12888 j4eval[0]
12889 = ((((-1.0) * x1226))
12890 + x1227);
12891 j4eval[1] = IKsign(
12892 ((((-0.09) * x1225))
12893 + (((0.09) * x1227))));
12894 j4eval[2]
12895 = ((IKabs(
12896 (((py * x1228))
12897 + ((py * x1225))
12898 + (((-1.0) * py
12899 * x1229))
12900 + (((-1.0) * px
12901 * x1230)))))
12902 + (IKabs(
12903 ((((-1.0) * px
12904 * x1228))
12905 + (((-1.0) * px
12906 * x1226))
12907 + ((px * x1229))
12908 + (((-1.0) * py
12909 * x1230))))));
12910 if (IKabs(j4eval[0])
12911 < 0.0000010000000000
12912 || IKabs(j4eval[1])
12913 < 0.0000010000000000
12914 || IKabs(j4eval[2])
12915 < 0.0000010000000000)
12916 {
12917 {
12918 IkReal j4eval[3];
12919 sj8 = -1.0;
12920 cj8 = 0;
12921 j8 = -1.5707963267949;
12922 IkReal x1231
12923 = (pp
12924 + (((-1.0) * (1.0)
12925 * (pz * pz))));
12926 IkReal x1232
12927 = ((1.32323529411765)
12928 * cj9);
12929 IkReal x1233
12930 = ((3.92156862745098)
12931 * pp);
12932 IkReal x1234
12933 = ((0.316735294117647)
12934 * sj6);
12935 IkReal x1235
12936 = ((0.108264705882353)
12937 * cj9 * sj6);
12938 IkReal x1236
12939 = ((0.588235294117647)
12940 * pp * sj6);
12941 j4eval[0] = x1231;
12942 j4eval[1]
12943 = ((IKabs((
12944 ((px * x1233))
12945 + (((-1.0)
12946 * (1.51009803921569)
12947 * px))
12948 + (((-1.0) * px
12949 * x1232))
12950 + ((py * x1234))
12951 + ((py * x1236))
12952 + ((py
12953 * x1235)))))
12954 + (IKabs((
12955 (((-1.0) * py
12956 * x1233))
12957 + ((py * x1232))
12958 + ((px * x1236))
12959 + ((px * x1235))
12960 + (((1.51009803921569)
12961 * py))
12962 + ((px
12963 * x1234))))));
12964 j4eval[2] = IKsign(x1231);
12965 if (IKabs(j4eval[0])
12966 < 0.0000010000000000
12967 || IKabs(j4eval[1])
12968 < 0.0000010000000000
12969 || IKabs(j4eval[2])
12970 < 0.0000010000000000)
12971 {
12972 {
12973 IkReal evalcond[7];
12974 bool bgotonextstatement
12975 = true;
12976 do
12977 {
12978 evalcond[0]
12979 = ((-3.14159265358979)
12980 + (IKfmod(
12981 ((3.14159265358979)
12982 + (IKabs((
12983 (-1.5707963267949)
12984 + j6)))),
12985 6.28318530717959)));
12986 evalcond[1] = pz;
12987 if (IKabs(evalcond[0])
12988 < 0.0000010000000000
12989 && IKabs(evalcond
12990 [1])
12991 < 0.0000010000000000)
12992 {
12993 bgotonextstatement
12994 = false;
12995 {
12996 IkReal j4eval[3];
12997 sj8 = -1.0;
12998 cj8 = 0;
12999 j8 = -1.5707963267949;
13000 sj6 = 1.0;
13001 cj6 = 0;
13002 j6 = 1.5707963267949;
13003 IkReal x1237
13004 = (pp
13005 + (((-1.0)
13006 * (1.0)
13007 * (pz
13008 * pz))));
13009 IkReal x1238
13010 = (cj9 * px);
13011 IkReal x1239
13012 = (cj9 * py);
13013 IkReal x1240
13014 = ((3.92156862745098)
13015 * pp);
13016 IkReal x1241
13017 = ((0.045)
13018 * sj9);
13019 j4eval[0] = x1237;
13020 j4eval[1]
13021 = ((IKabs((
13022 ((px
13023 * x1240))
13024 + (((-1.0)
13025 * (1.51009803921569)
13026 * px))
13027 + (((0.55)
13028 * py))
13029 + (((0.3)
13030 * x1239))
13031 + ((py
13032 * x1241))
13033 + (((-1.32323529411765)
13034 * x1238)))))
13035 + (IKabs((
13036 ((px
13037 * x1241))
13038 + (((1.51009803921569)
13039 * py))
13040 + (((1.32323529411765)
13041 * x1239))
13042 + (((-1.0)
13043 * py
13044 * x1240))
13045 + (((0.55)
13046 * px))
13047 + (((0.3)
13048 * x1238))))));
13049 j4eval[2]
13050 = IKsign(
13051 x1237);
13052 if (IKabs(
13053 j4eval[0])
13054 < 0.0000010000000000
13055 || IKabs(
13056 j4eval
13057 [1])
13058 < 0.0000010000000000
13059 || IKabs(
13060 j4eval
13061 [2])
13062 < 0.0000010000000000)
13063 {
13064 {
13065 IkReal
13066 j4eval[3];
13067 sj8 = -1.0;
13068 cj8 = 0;
13069 j8 = -1.5707963267949;
13070 sj6 = 1.0;
13071 cj6 = 0;
13072 j6 = 1.5707963267949;
13073 IkReal x1242
13074 = (pp
13075 + (((-1.0)
13076 * (1.0)
13077 * (pz
13078 * pz))));
13079 IkReal x1243
13080 = (cj9
13081 * px);
13082 IkReal x1244
13083 = (cj9
13084 * py);
13085 IkReal x1245
13086 = (pp
13087 * px);
13088 IkReal x1246
13089 = (pp
13090 * py);
13091 j4eval[0]
13092 = x1242;
13093 j4eval[1]
13094 = ((IKabs((
13095 (((-1.32323529411765)
13096 * x1243))
13097 + (((0.316735294117647)
13098 * py))
13099 + (((-1.0)
13100 * (1.51009803921569)
13101 * px))
13102 + (((0.108264705882353)
13103 * x1244))
13104 + (((0.588235294117647)
13105 * x1246))
13106 + (((3.92156862745098)
13107 * x1245)))))
13108 + (IKabs((
13109 (((0.588235294117647)
13110 * x1245))
13111 + (((1.32323529411765)
13112 * x1244))
13113 + (((-3.92156862745098)
13114 * x1246))
13115 + (((0.316735294117647)
13116 * px))
13117 + (((1.51009803921569)
13118 * py))
13119 + (((0.108264705882353)
13120 * x1243))))));
13121 j4eval[2]
13122 = IKsign(
13123 x1242);
13124 if (IKabs(
13125 j4eval
13126 [0])
13127 < 0.0000010000000000
13128 || IKabs(
13129 j4eval
13130 [1])
13131 < 0.0000010000000000
13132 || IKabs(
13133 j4eval
13134 [2])
13135 < 0.0000010000000000)
13136 {
13137 {
13138 IkReal j4eval
13139 [3];
13140 sj8 = -1.0;
13141 cj8 = 0;
13142 j8 = -1.5707963267949;
13143 sj6 = 1.0;
13144 cj6 = 0;
13145 j6 = 1.5707963267949;
13146 IkReal
13147 x1247
13148 = pz
13149 * pz;
13150 IkReal
13151 x1248
13152 = (cj9
13153 * px);
13154 IkReal
13155 x1249
13156 = (cj9
13157 * py);
13158 IkReal
13159 x1250
13160 = (pp
13161 * px);
13162 IkReal
13163 x1251
13164 = (pp
13165 * py);
13166 j4eval[0]
13167 = (pp
13168 + (((-1.0)
13169 * x1247)));
13170 j4eval[1]
13171 = ((IKabs((
13172 (((0.348408823529412)
13173 * py))
13174 + (((0.119091176470588)
13175 * x1249))
13176 + (((0.647058823529412)
13177 * x1251))
13178 + (((-1.45555882352941)
13179 * x1248))
13180 + (((4.31372549019608)
13181 * x1250))
13182 + (((-1.0)
13183 * (1.66110784313725)
13184 * px)))))
13185 + (IKabs((
13186 (((1.45555882352941)
13187 * x1249))
13188 + (((0.348408823529412)
13189 * px))
13190 + (((1.66110784313725)
13191 * py))
13192 + (((0.647058823529412)
13193 * x1250))
13194 + (((0.119091176470588)
13195 * x1248))
13196 + (((-4.31372549019608)
13197 * x1251))))));
13198 j4eval[2] = IKsign((
13199 (((1.1)
13200 * pp))
13201 + (((-1.1)
13202 * x1247))));
13203 if (IKabs(
13204 j4eval
13205 [0])
13206 < 0.0000010000000000
13207 || IKabs(
13208 j4eval
13209 [1])
13210 < 0.0000010000000000
13211 || IKabs(
13212 j4eval
13213 [2])
13214 < 0.0000010000000000)
13215 {
13216 {
13217 IkReal evalcond
13218 [6];
13219 bool
13220 bgotonextstatement
13221 = true;
13222 do
13223 {
13224 IkReal
13225 x1252
13226 = ((1.32323529411765)
13227 * cj9);
13228 IkReal
13229 x1253
13230 = ((3.92156862745098)
13231 * pp);
13232 evalcond
13233 [0]
13234 = ((IKabs(
13235 py))
13236 + (IKabs(
13237 px)));
13238 evalcond
13239 [1]
13240 = ((-0.55)
13241 + (((-1.0)
13242 * (0.3)
13243 * cj9))
13244 + (((-1.0)
13245 * (0.045)
13246 * sj9)));
13247 evalcond
13248 [2]
13249 = ((1.51009803921569)
13250 + (((-1.0)
13251 * x1253))
13252 + x1252);
13253 evalcond
13254 [3]
13255 = ((-1.51009803921569)
13256 + (((-1.0)
13257 * x1252))
13258 + x1253);
13259 evalcond
13260 [4]
13261 = ((0.316735294117647)
13262 + (((0.108264705882353)
13263 * cj9))
13264 + (((0.588235294117647)
13265 * pp)));
13266 evalcond
13267 [5]
13268 = ((-0.2125)
13269 + (((-1.0)
13270 * (1.0)
13271 * pp)));
13272 if (IKabs(
13273 evalcond
13274 [0])
13275 < 0.0000010000000000
13276 && IKabs(
13277 evalcond
13278 [1])
13279 < 0.0000010000000000
13280 && IKabs(
13281 evalcond
13282 [2])
13283 < 0.0000010000000000
13284 && IKabs(
13285 evalcond
13286 [3])
13287 < 0.0000010000000000
13288 && IKabs(
13289 evalcond
13290 [4])
13291 < 0.0000010000000000
13292 && IKabs(
13293 evalcond
13294 [5])
13295 < 0.0000010000000000)
13296 {
13297 bgotonextstatement
13298 = false;
13299 {
13300 IkReal j4array
13301 [4],
13302 cj4array
13303 [4],
13304 sj4array
13305 [4];
13306 bool j4valid
13307 [4]
13308 = {false};
13309 _nj4
13310 = 4;
13311 j4array
13312 [0]
13313 = 0;
13314 sj4array
13315 [0]
13316 = IKsin(
13317 j4array
13318 [0]);
13319 cj4array
13320 [0]
13321 = IKcos(
13322 j4array
13323 [0]);
13324 j4array
13325 [1]
13326 = 1.5707963267949;
13327 sj4array
13328 [1]
13329 = IKsin(
13330 j4array
13331 [1]);
13332 cj4array
13333 [1]
13334 = IKcos(
13335 j4array
13336 [1]);
13337 j4array
13338 [2]
13339 = 3.14159265358979;
13340 sj4array
13341 [2]
13342 = IKsin(
13343 j4array
13344 [2]);
13345 cj4array
13346 [2]
13347 = IKcos(
13348 j4array
13349 [2]);
13350 j4array
13351 [3]
13352 = -1.5707963267949;
13353 sj4array
13354 [3]
13355 = IKsin(
13356 j4array
13357 [3]);
13358 cj4array
13359 [3]
13360 = IKcos(
13361 j4array
13362 [3]);
13363 if (j4array
13364 [0]
13365 > IKPI)
13366 {
13367 j4array
13368 [0]
13369 -= IK2PI;
13370 }
13371 else if (
13372 j4array
13373 [0]
13374 < -IKPI)
13375 {
13376 j4array
13377 [0]
13378 += IK2PI;
13379 }
13380 j4valid
13381 [0]
13382 = true;
13383 if (j4array
13384 [1]
13385 > IKPI)
13386 {
13387 j4array
13388 [1]
13389 -= IK2PI;
13390 }
13391 else if (
13392 j4array
13393 [1]
13394 < -IKPI)
13395 {
13396 j4array
13397 [1]
13398 += IK2PI;
13399 }
13400 j4valid
13401 [1]
13402 = true;
13403 if (j4array
13404 [2]
13405 > IKPI)
13406 {
13407 j4array
13408 [2]
13409 -= IK2PI;
13410 }
13411 else if (
13412 j4array
13413 [2]
13414 < -IKPI)
13415 {
13416 j4array
13417 [2]
13418 += IK2PI;
13419 }
13420 j4valid
13421 [2]
13422 = true;
13423 if (j4array
13424 [3]
13425 > IKPI)
13426 {
13427 j4array
13428 [3]
13429 -= IK2PI;
13430 }
13431 else if (
13432 j4array
13433 [3]
13434 < -IKPI)
13435 {
13436 j4array
13437 [3]
13438 += IK2PI;
13439 }
13440 j4valid
13441 [3]
13442 = true;
13443 for (
13444 int ij4
13445 = 0;
13446 ij4
13447 < 4;
13448 ++ij4)
13449 {
13450 if (!j4valid
13451 [ij4])
13452 {
13453 continue;
13454 }
13455 _ij4[0]
13456 = ij4;
13457 _ij4[1]
13458 = -1;
13459 for (
13460 int iij4
13461 = ij4
13462 + 1;
13463 iij4
13464 < 4;
13465 ++iij4)
13466 {
13467 if (j4valid
13468 [iij4]
13469 && IKabs(
13470 cj4array
13471 [ij4]
13472 - cj4array
13473 [iij4])
13474 < IKFAST_SOLUTION_THRESH
13475 && IKabs(
13476 sj4array
13477 [ij4]
13478 - sj4array
13479 [iij4])
13480 < IKFAST_SOLUTION_THRESH)
13481 {
13482 j4valid
13483 [iij4]
13484 = false;
13485 _ij4[1]
13486 = iij4;
13487 break;
13488 }
13489 }
13490 j4 = j4array
13491 [ij4];
13492 cj4 = cj4array
13493 [ij4];
13494 sj4 = sj4array
13495 [ij4];
13496
13497 rotationfunction0(
13498 solutions);
13499 }
13500 }
13501 }
13502 } while (
13503 0);
13504 if (bgotonextstatement)
13505 {
13506 bool
13507 bgotonextstatement
13508 = true;
13509 do
13510 {
13511 if (1)
13512 {
13513 bgotonextstatement
13514 = false;
13515 continue; // branch miss [j4]
13516 }
13517 } while (
13518 0);
13519 if (bgotonextstatement)
13520 {
13521 }
13522 }
13523 }
13524 }
13525 else
13526 {
13527 {
13528 IkReal j4array
13529 [1],
13530 cj4array
13531 [1],
13532 sj4array
13533 [1];
13534 bool j4valid
13535 [1]
13536 = {false};
13537 _nj4
13538 = 1;
13539 IkReal
13540 x1254
13541 = (cj9
13542 * px);
13543 IkReal
13544 x1255
13545 = (cj9
13546 * py);
13547 IkReal
13548 x1256
13549 = (pp
13550 * px);
13551 IkReal
13552 x1257
13553 = (pp
13554 * py);
13555 CheckValue<IkReal> x1258 = IKatan2WithCheck(
13556 IkReal((
13557 (((0.348408823529412)
13558 * py))
13559 + (((0.647058823529412)
13560 * x1257))
13561 + (((4.31372549019608)
13562 * x1256))
13563 + (((0.119091176470588)
13564 * x1255))
13565 + (((-1.0)
13566 * (1.66110784313725)
13567 * px))
13568 + (((-1.45555882352941)
13569 * x1254)))),
13570 ((((0.348408823529412)
13571 * px))
13572 + (((1.66110784313725)
13573 * py))
13574 + (((0.119091176470588)
13575 * x1254))
13576 + (((-4.31372549019608)
13577 * x1257))
13578 + (((0.647058823529412)
13579 * x1256))
13580 + (((1.45555882352941)
13581 * x1255))),
13582 IKFAST_ATAN2_MAGTHRESH);
13583 if (!x1258
13584 .valid)
13585 {
13586 continue;
13587 }
13588 CheckValue<IkReal> x1259 = IKPowWithIntegerCheck(
13589 IKsign((
13590 (((1.1)
13591 * pp))
13592 + (((-1.0)
13593 * (1.1)
13594 * (pz
13595 * pz))))),
13596 -1);
13597 if (!x1259
13598 .valid)
13599 {
13600 continue;
13601 }
13602 j4array
13603 [0]
13604 = ((-1.5707963267949)
13605 + (x1258
13606 .value)
13607 + (((1.5707963267949)
13608 * (x1259
13609 .value))));
13610 sj4array
13611 [0]
13612 = IKsin(
13613 j4array
13614 [0]);
13615 cj4array
13616 [0]
13617 = IKcos(
13618 j4array
13619 [0]);
13620 if (j4array
13621 [0]
13622 > IKPI)
13623 {
13624 j4array
13625 [0]
13626 -= IK2PI;
13627 }
13628 else if (
13629 j4array
13630 [0]
13631 < -IKPI)
13632 {
13633 j4array
13634 [0]
13635 += IK2PI;
13636 }
13637 j4valid
13638 [0]
13639 = true;
13640 for (
13641 int ij4
13642 = 0;
13643 ij4
13644 < 1;
13645 ++ij4)
13646 {
13647 if (!j4valid
13648 [ij4])
13649 {
13650 continue;
13651 }
13652 _ij4[0]
13653 = ij4;
13654 _ij4[1]
13655 = -1;
13656 for (
13657 int iij4
13658 = ij4
13659 + 1;
13660 iij4
13661 < 1;
13662 ++iij4)
13663 {
13664 if (j4valid
13665 [iij4]
13666 && IKabs(
13667 cj4array
13668 [ij4]
13669 - cj4array
13670 [iij4])
13671 < IKFAST_SOLUTION_THRESH
13672 && IKabs(
13673 sj4array
13674 [ij4]
13675 - sj4array
13676 [iij4])
13677 < IKFAST_SOLUTION_THRESH)
13678 {
13679 j4valid
13680 [iij4]
13681 = false;
13682 _ij4[1]
13683 = iij4;
13684 break;
13685 }
13686 }
13687 j4 = j4array
13688 [ij4];
13689 cj4 = cj4array
13690 [ij4];
13691 sj4 = sj4array
13692 [ij4];
13693 {
13694 IkReal evalcond
13695 [4];
13696 IkReal
13697 x1260
13698 = IKcos(
13699 j4);
13700 IkReal
13701 x1261
13702 = (px
13703 * x1260);
13704 IkReal
13705 x1262
13706 = IKsin(
13707 j4);
13708 IkReal
13709 x1263
13710 = (py
13711 * x1262);
13712 IkReal
13713 x1264
13714 = (px
13715 * x1262);
13716 IkReal
13717 x1265
13718 = (py
13719 * x1260);
13720 evalcond
13721 [0]
13722 = ((-0.55)
13723 + x1261
13724 + x1263
13725 + (((-1.0)
13726 * (0.3)
13727 * cj9))
13728 + (((-1.0)
13729 * (0.045)
13730 * sj9)));
13731 evalcond
13732 [1]
13733 = ((1.51009803921569)
13734 + (((-1.0)
13735 * (3.92156862745098)
13736 * pp))
13737 + (((-1.0)
13738 * x1265))
13739 + x1264
13740 + (((1.32323529411765)
13741 * cj9)));
13742 evalcond
13743 [2]
13744 = ((0.316735294117647)
13745 + (((-1.0)
13746 * x1261))
13747 + (((-1.0)
13748 * x1263))
13749 + (((0.108264705882353)
13750 * cj9))
13751 + (((0.588235294117647)
13752 * pp)));
13753 evalcond
13754 [3]
13755 = ((-0.2125)
13756 + (((0.09)
13757 * x1264))
13758 + (((-0.09)
13759 * x1265))
13760 + (((-1.0)
13761 * (1.0)
13762 * pp))
13763 + (((1.1)
13764 * x1263))
13765 + (((1.1)
13766 * x1261)));
13767 if (IKabs(
13768 evalcond
13769 [0])
13770 > IKFAST_EVALCOND_THRESH
13771 || IKabs(
13772 evalcond
13773 [1])
13774 > IKFAST_EVALCOND_THRESH
13775 || IKabs(
13776 evalcond
13777 [2])
13778 > IKFAST_EVALCOND_THRESH
13779 || IKabs(
13780 evalcond
13781 [3])
13782 > IKFAST_EVALCOND_THRESH)
13783 {
13784 continue;
13785 }
13786 }
13787
13788 rotationfunction0(
13789 solutions);
13790 }
13791 }
13792 }
13793 }
13794 }
13795 else
13796 {
13797 {
13798 IkReal j4array
13799 [1],
13800 cj4array
13801 [1],
13802 sj4array
13803 [1];
13804 bool j4valid
13805 [1]
13806 = {false};
13807 _nj4 = 1;
13808 IkReal
13809 x1266
13810 = (cj9
13811 * px);
13812 IkReal
13813 x1267
13814 = (cj9
13815 * py);
13816 IkReal
13817 x1268
13818 = (pp
13819 * px);
13820 IkReal
13821 x1269
13822 = (pp
13823 * py);
13824 CheckValue<IkReal> x1270 = IKatan2WithCheck(
13825 IkReal((
13826 (((0.316735294117647)
13827 * py))
13828 + (((0.108264705882353)
13829 * x1267))
13830 + (((-1.32323529411765)
13831 * x1266))
13832 + (((3.92156862745098)
13833 * x1268))
13834 + (((-1.0)
13835 * (1.51009803921569)
13836 * px))
13837 + (((0.588235294117647)
13838 * x1269)))),
13839 ((((0.588235294117647)
13840 * x1268))
13841 + (((1.32323529411765)
13842 * x1267))
13843 + (((0.316735294117647)
13844 * px))
13845 + (((1.51009803921569)
13846 * py))
13847 + (((-3.92156862745098)
13848 * x1269))
13849 + (((0.108264705882353)
13850 * x1266))),
13851 IKFAST_ATAN2_MAGTHRESH);
13852 if (!x1270
13853 .valid)
13854 {
13855 continue;
13856 }
13857 CheckValue<IkReal> x1271 = IKPowWithIntegerCheck(
13858 IKsign((
13859 pp
13860 + (((-1.0)
13861 * (1.0)
13862 * (pz
13863 * pz))))),
13864 -1);
13865 if (!x1271
13866 .valid)
13867 {
13868 continue;
13869 }
13870 j4array[0]
13871 = ((-1.5707963267949)
13872 + (x1270
13873 .value)
13874 + (((1.5707963267949)
13875 * (x1271
13876 .value))));
13877 sj4array[0] = IKsin(
13878 j4array
13879 [0]);
13880 cj4array[0] = IKcos(
13881 j4array
13882 [0]);
13883 if (j4array
13884 [0]
13885 > IKPI)
13886 {
13887 j4array
13888 [0]
13889 -= IK2PI;
13890 }
13891 else if (
13892 j4array
13893 [0]
13894 < -IKPI)
13895 {
13896 j4array
13897 [0]
13898 += IK2PI;
13899 }
13900 j4valid[0]
13901 = true;
13902 for (
13903 int ij4
13904 = 0;
13905 ij4
13906 < 1;
13907 ++ij4)
13908 {
13909 if (!j4valid
13910 [ij4])
13911 {
13912 continue;
13913 }
13914 _ij4[0]
13915 = ij4;
13916 _ij4[1]
13917 = -1;
13918 for (
13919 int iij4
13920 = ij4
13921 + 1;
13922 iij4
13923 < 1;
13924 ++iij4)
13925 {
13926 if (j4valid
13927 [iij4]
13928 && IKabs(
13929 cj4array
13930 [ij4]
13931 - cj4array
13932 [iij4])
13933 < IKFAST_SOLUTION_THRESH
13934 && IKabs(
13935 sj4array
13936 [ij4]
13937 - sj4array
13938 [iij4])
13939 < IKFAST_SOLUTION_THRESH)
13940 {
13941 j4valid
13942 [iij4]
13943 = false;
13944 _ij4[1]
13945 = iij4;
13946 break;
13947 }
13948 }
13949 j4 = j4array
13950 [ij4];
13951 cj4 = cj4array
13952 [ij4];
13953 sj4 = sj4array
13954 [ij4];
13955 {
13956 IkReal evalcond
13957 [4];
13958 IkReal
13959 x1272
13960 = IKcos(
13961 j4);
13962 IkReal
13963 x1273
13964 = (px
13965 * x1272);
13966 IkReal
13967 x1274
13968 = IKsin(
13969 j4);
13970 IkReal
13971 x1275
13972 = (py
13973 * x1274);
13974 IkReal
13975 x1276
13976 = (px
13977 * x1274);
13978 IkReal
13979 x1277
13980 = (py
13981 * x1272);
13982 evalcond
13983 [0]
13984 = ((-0.55)
13985 + x1275
13986 + x1273
13987 + (((-1.0)
13988 * (0.3)
13989 * cj9))
13990 + (((-1.0)
13991 * (0.045)
13992 * sj9)));
13993 evalcond
13994 [1]
13995 = ((1.51009803921569)
13996 + (((-1.0)
13997 * (3.92156862745098)
13998 * pp))
13999 + x1276
14000 + (((1.32323529411765)
14001 * cj9))
14002 + (((-1.0)
14003 * x1277)));
14004 evalcond
14005 [2]
14006 = ((0.316735294117647)
14007 + (((0.108264705882353)
14008 * cj9))
14009 + (((-1.0)
14010 * x1273))
14011 + (((0.588235294117647)
14012 * pp))
14013 + (((-1.0)
14014 * x1275)));
14015 evalcond
14016 [3]
14017 = ((-0.2125)
14018 + (((0.09)
14019 * x1276))
14020 + (((1.1)
14021 * x1275))
14022 + (((1.1)
14023 * x1273))
14024 + (((-1.0)
14025 * (1.0)
14026 * pp))
14027 + (((-0.09)
14028 * x1277)));
14029 if (IKabs(
14030 evalcond
14031 [0])
14032 > IKFAST_EVALCOND_THRESH
14033 || IKabs(
14034 evalcond
14035 [1])
14036 > IKFAST_EVALCOND_THRESH
14037 || IKabs(
14038 evalcond
14039 [2])
14040 > IKFAST_EVALCOND_THRESH
14041 || IKabs(
14042 evalcond
14043 [3])
14044 > IKFAST_EVALCOND_THRESH)
14045 {
14046 continue;
14047 }
14048 }
14049
14050 rotationfunction0(
14051 solutions);
14052 }
14053 }
14054 }
14055 }
14056 }
14057 else
14058 {
14059 {
14060 IkReal j4array
14061 [1],
14062 cj4array
14063 [1],
14064 sj4array
14065 [1];
14066 bool
14067 j4valid[1]
14068 = {false};
14069 _nj4 = 1;
14070 IkReal x1278
14071 = (cj9
14072 * px);
14073 IkReal x1279
14074 = (cj9
14075 * py);
14076 IkReal x1280
14077 = ((3.92156862745098)
14078 * pp);
14079 IkReal x1281
14080 = ((0.045)
14081 * sj9);
14082 CheckValue<IkReal> x1282 = IKPowWithIntegerCheck(
14083 IKsign((
14084 pp
14085 + (((-1.0)
14086 * (1.0)
14087 * (pz
14088 * pz))))),
14089 -1);
14090 if (!x1282
14091 .valid)
14092 {
14093 continue;
14094 }
14095 CheckValue<IkReal> x1283 = IKatan2WithCheck(
14096 IkReal((
14097 (((-1.0)
14098 * (1.51009803921569)
14099 * px))
14100 + (((-1.32323529411765)
14101 * x1278))
14102 + (((0.55)
14103 * py))
14104 + ((px
14105 * x1280))
14106 + ((py
14107 * x1281))
14108 + (((0.3)
14109 * x1279)))),
14110 ((((-1.0)
14111 * py
14112 * x1280))
14113 + (((1.32323529411765)
14114 * x1279))
14115 + (((1.51009803921569)
14116 * py))
14117 + (((0.3)
14118 * x1278))
14119 + (((0.55)
14120 * px))
14121 + ((px
14122 * x1281))),
14123 IKFAST_ATAN2_MAGTHRESH);
14124 if (!x1283
14125 .valid)
14126 {
14127 continue;
14128 }
14129 j4array[0]
14130 = ((-1.5707963267949)
14131 + (((1.5707963267949)
14132 * (x1282
14133 .value)))
14134 + (x1283
14135 .value));
14136 sj4array[0]
14137 = IKsin(
14138 j4array
14139 [0]);
14140 cj4array[0]
14141 = IKcos(
14142 j4array
14143 [0]);
14144 if (j4array[0]
14145 > IKPI)
14146 {
14147 j4array[0]
14148 -= IK2PI;
14149 }
14150 else if (
14151 j4array[0]
14152 < -IKPI)
14153 {
14154 j4array[0]
14155 += IK2PI;
14156 }
14157 j4valid[0]
14158 = true;
14159 for (int ij4
14160 = 0;
14161 ij4 < 1;
14162 ++ij4)
14163 {
14164 if (!j4valid
14165 [ij4])
14166 {
14167 continue;
14168 }
14169 _ij4[0]
14170 = ij4;
14171 _ij4[1]
14172 = -1;
14173 for (
14174 int iij4
14175 = ij4
14176 + 1;
14177 iij4
14178 < 1;
14179 ++iij4)
14180 {
14181 if (j4valid
14182 [iij4]
14183 && IKabs(
14184 cj4array
14185 [ij4]
14186 - cj4array
14187 [iij4])
14188 < IKFAST_SOLUTION_THRESH
14189 && IKabs(
14190 sj4array
14191 [ij4]
14192 - sj4array
14193 [iij4])
14194 < IKFAST_SOLUTION_THRESH)
14195 {
14196 j4valid
14197 [iij4]
14198 = false;
14199 _ij4[1]
14200 = iij4;
14201 break;
14202 }
14203 }
14204 j4 = j4array
14205 [ij4];
14206 cj4 = cj4array
14207 [ij4];
14208 sj4 = sj4array
14209 [ij4];
14210 {
14211 IkReal evalcond
14212 [4];
14213 IkReal
14214 x1284
14215 = IKcos(
14216 j4);
14217 IkReal
14218 x1285
14219 = (px
14220 * x1284);
14221 IkReal
14222 x1286
14223 = IKsin(
14224 j4);
14225 IkReal
14226 x1287
14227 = (py
14228 * x1286);
14229 IkReal
14230 x1288
14231 = (px
14232 * x1286);
14233 IkReal
14234 x1289
14235 = (py
14236 * x1284);
14237 evalcond
14238 [0]
14239 = ((-0.55)
14240 + x1287
14241 + x1285
14242 + (((-1.0)
14243 * (0.3)
14244 * cj9))
14245 + (((-1.0)
14246 * (0.045)
14247 * sj9)));
14248 evalcond
14249 [1]
14250 = ((1.51009803921569)
14251 + (((-1.0)
14252 * x1289))
14253 + (((-1.0)
14254 * (3.92156862745098)
14255 * pp))
14256 + x1288
14257 + (((1.32323529411765)
14258 * cj9)));
14259 evalcond
14260 [2]
14261 = ((0.316735294117647)
14262 + (((-1.0)
14263 * x1285))
14264 + (((0.108264705882353)
14265 * cj9))
14266 + (((0.588235294117647)
14267 * pp))
14268 + (((-1.0)
14269 * x1287)));
14270 evalcond
14271 [3]
14272 = ((-0.2125)
14273 + (((0.09)
14274 * x1288))
14275 + (((-0.09)
14276 * x1289))
14277 + (((1.1)
14278 * x1287))
14279 + (((-1.0)
14280 * (1.0)
14281 * pp))
14282 + (((1.1)
14283 * x1285)));
14284 if (IKabs(
14285 evalcond
14286 [0])
14287 > IKFAST_EVALCOND_THRESH
14288 || IKabs(
14289 evalcond
14290 [1])
14291 > IKFAST_EVALCOND_THRESH
14292 || IKabs(
14293 evalcond
14294 [2])
14295 > IKFAST_EVALCOND_THRESH
14296 || IKabs(
14297 evalcond
14298 [3])
14299 > IKFAST_EVALCOND_THRESH)
14300 {
14301 continue;
14302 }
14303 }
14304
14305 rotationfunction0(
14306 solutions);
14307 }
14308 }
14309 }
14310 }
14311 }
14312 } while (0);
14313 if (bgotonextstatement)
14314 {
14315 bool
14316 bgotonextstatement
14317 = true;
14318 do
14319 {
14320 evalcond[0]
14321 = ((-3.14159265358979)
14322 + (IKfmod(
14323 ((3.14159265358979)
14324 + (IKabs((
14325 (1.5707963267949)
14326 + j6)))),
14327 6.28318530717959)));
14328 evalcond[1]
14329 = ((-1.0)
14330 * (((1.0)
14331 * pz)));
14332 if (IKabs(
14333 evalcond[0])
14334 < 0.0000010000000000
14335 && IKabs(
14336 evalcond
14337 [1])
14338 < 0.0000010000000000)
14339 {
14340 bgotonextstatement
14341 = false;
14342 {
14343 IkReal
14344 j4eval[3];
14345 sj8 = -1.0;
14346 cj8 = 0;
14347 j8 = -1.5707963267949;
14348 sj6 = -1.0;
14349 cj6 = 0;
14350 j6 = -1.5707963267949;
14351 IkReal x1290
14352 = (pp
14353 + (((-1.0)
14354 * (1.0)
14355 * (pz
14356 * pz))));
14357 IkReal x1291
14358 = (cj9
14359 * px);
14360 IkReal x1292
14361 = (cj9
14362 * py);
14363 IkReal x1293
14364 = ((3.92156862745098)
14365 * pp);
14366 IkReal x1294
14367 = ((0.045)
14368 * sj9);
14369 j4eval[0]
14370 = x1290;
14371 j4eval[1]
14372 = ((IKabs((
14373 (((-1.32323529411765)
14374 * x1291))
14375 + ((px
14376 * x1293))
14377 + (((-1.0)
14378 * (1.51009803921569)
14379 * px))
14380 + (((-1.0)
14381 * py
14382 * x1294))
14383 + (((-0.3)
14384 * x1292))
14385 + (((-1.0)
14386 * (0.55)
14387 * py)))))
14388 + (IKabs((
14389 (((-1.0)
14390 * (0.55)
14391 * px))
14392 + (((-0.3)
14393 * x1291))
14394 + (((-1.0)
14395 * px
14396 * x1294))
14397 + (((1.51009803921569)
14398 * py))
14399 + (((-1.0)
14400 * py
14401 * x1293))
14402 + (((1.32323529411765)
14403 * x1292))))));
14404 j4eval[2]
14405 = IKsign(
14406 x1290);
14407 if (IKabs(
14408 j4eval
14409 [0])
14410 < 0.0000010000000000
14411 || IKabs(
14412 j4eval
14413 [1])
14414 < 0.0000010000000000
14415 || IKabs(
14416 j4eval
14417 [2])
14418 < 0.0000010000000000)
14419 {
14420 {
14421 IkReal
14422 j4eval
14423 [3];
14424 sj8 = -1.0;
14425 cj8 = 0;
14426 j8 = -1.5707963267949;
14427 sj6 = -1.0;
14428 cj6 = 0;
14429 j6 = -1.5707963267949;
14430 IkReal x1295
14431 = (pp
14432 + (((-1.0)
14433 * (1.0)
14434 * (pz
14435 * pz))));
14436 IkReal x1296
14437 = (cj9
14438 * px);
14439 IkReal x1297
14440 = (cj9
14441 * py);
14442 IkReal x1298
14443 = (pp
14444 * px);
14445 IkReal x1299
14446 = (pp
14447 * py);
14448 j4eval[0]
14449 = x1295;
14450 j4eval[1]
14451 = ((IKabs((
14452 (((-3.92156862745098)
14453 * x1299))
14454 + (((1.51009803921569)
14455 * py))
14456 + (((-0.108264705882353)
14457 * x1296))
14458 + (((-1.0)
14459 * (0.316735294117647)
14460 * px))
14461 + (((1.32323529411765)
14462 * x1297))
14463 + (((-0.588235294117647)
14464 * x1298)))))
14465 + (IKabs((
14466 (((-0.588235294117647)
14467 * x1299))
14468 + (((-1.32323529411765)
14469 * x1296))
14470 + (((-1.0)
14471 * (1.51009803921569)
14472 * px))
14473 + (((-1.0)
14474 * (0.316735294117647)
14475 * py))
14476 + (((-0.108264705882353)
14477 * x1297))
14478 + (((3.92156862745098)
14479 * x1298))))));
14480 j4eval[2]
14481 = IKsign(
14482 x1295);
14483 if (IKabs(
14484 j4eval
14485 [0])
14486 < 0.0000010000000000
14487 || IKabs(
14488 j4eval
14489 [1])
14490 < 0.0000010000000000
14491 || IKabs(
14492 j4eval
14493 [2])
14494 < 0.0000010000000000)
14495 {
14496 {
14497 IkReal j4eval
14498 [3];
14499 sj8 = -1.0;
14500 cj8 = 0;
14501 j8 = -1.5707963267949;
14502 sj6 = -1.0;
14503 cj6 = 0;
14504 j6 = -1.5707963267949;
14505 IkReal
14506 x1300
14507 = pz
14508 * pz;
14509 IkReal
14510 x1301
14511 = (cj9
14512 * px);
14513 IkReal
14514 x1302
14515 = (cj9
14516 * py);
14517 IkReal
14518 x1303
14519 = (pp
14520 * px);
14521 IkReal
14522 x1304
14523 = (pp
14524 * py);
14525 j4eval
14526 [0]
14527 = (x1300
14528 + (((-1.0)
14529 * (1.0)
14530 * pp)));
14531 j4eval[1] = IKsign((
14532 (((1.1)
14533 * x1300))
14534 + (((-1.0)
14535 * (1.1)
14536 * pp))));
14537 j4eval
14538 [2]
14539 = ((IKabs((
14540 (((4.31372549019608)
14541 * x1304))
14542 + (((0.348408823529412)
14543 * px))
14544 + (((0.119091176470588)
14545 * x1301))
14546 + (((-1.0)
14547 * (1.66110784313725)
14548 * py))
14549 + (((0.647058823529412)
14550 * x1303))
14551 + (((-1.45555882352941)
14552 * x1302)))))
14553 + (IKabs((
14554 (((0.348408823529412)
14555 * py))
14556 + (((1.66110784313725)
14557 * px))
14558 + (((1.45555882352941)
14559 * x1301))
14560 + (((-4.31372549019608)
14561 * x1303))
14562 + (((0.647058823529412)
14563 * x1304))
14564 + (((0.119091176470588)
14565 * x1302))))));
14566 if (IKabs(
14567 j4eval
14568 [0])
14569 < 0.0000010000000000
14570 || IKabs(
14571 j4eval
14572 [1])
14573 < 0.0000010000000000
14574 || IKabs(
14575 j4eval
14576 [2])
14577 < 0.0000010000000000)
14578 {
14579 {
14580 IkReal evalcond
14581 [6];
14582 bool
14583 bgotonextstatement
14584 = true;
14585 do
14586 {
14587 IkReal
14588 x1305
14589 = ((1.32323529411765)
14590 * cj9);
14591 IkReal
14592 x1306
14593 = ((3.92156862745098)
14594 * pp);
14595 evalcond
14596 [0]
14597 = ((IKabs(
14598 py))
14599 + (IKabs(
14600 px)));
14601 evalcond
14602 [1]
14603 = ((-0.55)
14604 + (((-1.0)
14605 * (0.3)
14606 * cj9))
14607 + (((-1.0)
14608 * (0.045)
14609 * sj9)));
14610 evalcond
14611 [2]
14612 = ((1.51009803921569)
14613 + (((-1.0)
14614 * x1306))
14615 + x1305);
14616 evalcond
14617 [3]
14618 = ((-1.51009803921569)
14619 + x1306
14620 + (((-1.0)
14621 * x1305)));
14622 evalcond
14623 [4]
14624 = ((-0.316735294117647)
14625 + (((-1.0)
14626 * (0.588235294117647)
14627 * pp))
14628 + (((-1.0)
14629 * (0.108264705882353)
14630 * cj9)));
14631 evalcond
14632 [5]
14633 = ((-0.2125)
14634 + (((-1.0)
14635 * (1.0)
14636 * pp)));
14637 if (IKabs(
14638 evalcond
14639 [0])
14640 < 0.0000010000000000
14641 && IKabs(
14642 evalcond
14643 [1])
14644 < 0.0000010000000000
14645 && IKabs(
14646 evalcond
14647 [2])
14648 < 0.0000010000000000
14649 && IKabs(
14650 evalcond
14651 [3])
14652 < 0.0000010000000000
14653 && IKabs(
14654 evalcond
14655 [4])
14656 < 0.0000010000000000
14657 && IKabs(
14658 evalcond
14659 [5])
14660 < 0.0000010000000000)
14661 {
14662 bgotonextstatement
14663 = false;
14664 {
14665 IkReal j4array
14666 [4],
14667 cj4array
14668 [4],
14669 sj4array
14670 [4];
14671 bool j4valid
14672 [4]
14673 = {false};
14674 _nj4
14675 = 4;
14676 j4array
14677 [0]
14678 = 0;
14679 sj4array
14680 [0]
14681 = IKsin(
14682 j4array
14683 [0]);
14684 cj4array
14685 [0]
14686 = IKcos(
14687 j4array
14688 [0]);
14689 j4array
14690 [1]
14691 = 1.5707963267949;
14692 sj4array
14693 [1]
14694 = IKsin(
14695 j4array
14696 [1]);
14697 cj4array
14698 [1]
14699 = IKcos(
14700 j4array
14701 [1]);
14702 j4array
14703 [2]
14704 = 3.14159265358979;
14705 sj4array
14706 [2]
14707 = IKsin(
14708 j4array
14709 [2]);
14710 cj4array
14711 [2]
14712 = IKcos(
14713 j4array
14714 [2]);
14715 j4array
14716 [3]
14717 = -1.5707963267949;
14718 sj4array
14719 [3]
14720 = IKsin(
14721 j4array
14722 [3]);
14723 cj4array
14724 [3]
14725 = IKcos(
14726 j4array
14727 [3]);
14728 if (j4array
14729 [0]
14730 > IKPI)
14731 {
14732 j4array
14733 [0]
14734 -= IK2PI;
14735 }
14736 else if (
14737 j4array
14738 [0]
14739 < -IKPI)
14740 {
14741 j4array
14742 [0]
14743 += IK2PI;
14744 }
14745 j4valid
14746 [0]
14747 = true;
14748 if (j4array
14749 [1]
14750 > IKPI)
14751 {
14752 j4array
14753 [1]
14754 -= IK2PI;
14755 }
14756 else if (
14757 j4array
14758 [1]
14759 < -IKPI)
14760 {
14761 j4array
14762 [1]
14763 += IK2PI;
14764 }
14765 j4valid
14766 [1]
14767 = true;
14768 if (j4array
14769 [2]
14770 > IKPI)
14771 {
14772 j4array
14773 [2]
14774 -= IK2PI;
14775 }
14776 else if (
14777 j4array
14778 [2]
14779 < -IKPI)
14780 {
14781 j4array
14782 [2]
14783 += IK2PI;
14784 }
14785 j4valid
14786 [2]
14787 = true;
14788 if (j4array
14789 [3]
14790 > IKPI)
14791 {
14792 j4array
14793 [3]
14794 -= IK2PI;
14795 }
14796 else if (
14797 j4array
14798 [3]
14799 < -IKPI)
14800 {
14801 j4array
14802 [3]
14803 += IK2PI;
14804 }
14805 j4valid
14806 [3]
14807 = true;
14808 for (
14809 int ij4
14810 = 0;
14811 ij4
14812 < 4;
14813 ++ij4)
14814 {
14815 if (!j4valid
14816 [ij4])
14817 {
14818 continue;
14819 }
14820 _ij4[0]
14821 = ij4;
14822 _ij4[1]
14823 = -1;
14824 for (
14825 int iij4
14826 = ij4
14827 + 1;
14828 iij4
14829 < 4;
14830 ++iij4)
14831 {
14832 if (j4valid
14833 [iij4]
14834 && IKabs(
14835 cj4array
14836 [ij4]
14837 - cj4array
14838 [iij4])
14839 < IKFAST_SOLUTION_THRESH
14840 && IKabs(
14841 sj4array
14842 [ij4]
14843 - sj4array
14844 [iij4])
14845 < IKFAST_SOLUTION_THRESH)
14846 {
14847 j4valid
14848 [iij4]
14849 = false;
14850 _ij4[1]
14851 = iij4;
14852 break;
14853 }
14854 }
14855 j4 = j4array
14856 [ij4];
14857 cj4 = cj4array
14858 [ij4];
14859 sj4 = sj4array
14860 [ij4];
14861
14862 rotationfunction0(
14863 solutions);
14864 }
14865 }
14866 }
14867 } while (
14868 0);
14869 if (bgotonextstatement)
14870 {
14871 bool
14872 bgotonextstatement
14873 = true;
14874 do
14875 {
14876 if (1)
14877 {
14878 bgotonextstatement
14879 = false;
14880 continue; // branch miss [j4]
14881 }
14882 } while (
14883 0);
14884 if (bgotonextstatement)
14885 {
14886 }
14887 }
14888 }
14889 }
14890 else
14891 {
14892 {
14893 IkReal j4array
14894 [1],
14895 cj4array
14896 [1],
14897 sj4array
14898 [1];
14899 bool j4valid
14900 [1]
14901 = {false};
14902 _nj4
14903 = 1;
14904 IkReal
14905 x1307
14906 = (cj9
14907 * px);
14908 IkReal
14909 x1308
14910 = (cj9
14911 * py);
14912 IkReal
14913 x1309
14914 = (pp
14915 * px);
14916 IkReal
14917 x1310
14918 = (pp
14919 * py);
14920 CheckValue<IkReal> x1311 = IKPowWithIntegerCheck(
14921 IKsign((
14922 (((1.1)
14923 * (pz
14924 * pz)))
14925 + (((-1.0)
14926 * (1.1)
14927 * pp)))),
14928 -1);
14929 if (!x1311
14930 .valid)
14931 {
14932 continue;
14933 }
14934 CheckValue<IkReal> x1312 = IKatan2WithCheck(
14935 IkReal((
14936 (((1.45555882352941)
14937 * x1307))
14938 + (((0.348408823529412)
14939 * py))
14940 + (((1.66110784313725)
14941 * px))
14942 + (((0.119091176470588)
14943 * x1308))
14944 + (((-4.31372549019608)
14945 * x1309))
14946 + (((0.647058823529412)
14947 * x1310)))),
14948 ((((0.647058823529412)
14949 * x1309))
14950 + (((0.348408823529412)
14951 * px))
14952 + (((4.31372549019608)
14953 * x1310))
14954 + (((-1.45555882352941)
14955 * x1308))
14956 + (((0.119091176470588)
14957 * x1307))
14958 + (((-1.0)
14959 * (1.66110784313725)
14960 * py))),
14961 IKFAST_ATAN2_MAGTHRESH);
14962 if (!x1312
14963 .valid)
14964 {
14965 continue;
14966 }
14967 j4array
14968 [0]
14969 = ((-1.5707963267949)
14970 + (((1.5707963267949)
14971 * (x1311
14972 .value)))
14973 + (x1312
14974 .value));
14975 sj4array
14976 [0]
14977 = IKsin(
14978 j4array
14979 [0]);
14980 cj4array
14981 [0]
14982 = IKcos(
14983 j4array
14984 [0]);
14985 if (j4array
14986 [0]
14987 > IKPI)
14988 {
14989 j4array
14990 [0]
14991 -= IK2PI;
14992 }
14993 else if (
14994 j4array
14995 [0]
14996 < -IKPI)
14997 {
14998 j4array
14999 [0]
15000 += IK2PI;
15001 }
15002 j4valid
15003 [0]
15004 = true;
15005 for (
15006 int ij4
15007 = 0;
15008 ij4
15009 < 1;
15010 ++ij4)
15011 {
15012 if (!j4valid
15013 [ij4])
15014 {
15015 continue;
15016 }
15017 _ij4[0]
15018 = ij4;
15019 _ij4[1]
15020 = -1;
15021 for (
15022 int iij4
15023 = ij4
15024 + 1;
15025 iij4
15026 < 1;
15027 ++iij4)
15028 {
15029 if (j4valid
15030 [iij4]
15031 && IKabs(
15032 cj4array
15033 [ij4]
15034 - cj4array
15035 [iij4])
15036 < IKFAST_SOLUTION_THRESH
15037 && IKabs(
15038 sj4array
15039 [ij4]
15040 - sj4array
15041 [iij4])
15042 < IKFAST_SOLUTION_THRESH)
15043 {
15044 j4valid
15045 [iij4]
15046 = false;
15047 _ij4[1]
15048 = iij4;
15049 break;
15050 }
15051 }
15052 j4 = j4array
15053 [ij4];
15054 cj4 = cj4array
15055 [ij4];
15056 sj4 = sj4array
15057 [ij4];
15058 {
15059 IkReal evalcond
15060 [4];
15061 IkReal
15062 x1313
15063 = IKsin(
15064 j4);
15065 IkReal
15066 x1314
15067 = (px
15068 * x1313);
15069 IkReal
15070 x1315
15071 = IKcos(
15072 j4);
15073 IkReal
15074 x1316
15075 = (py
15076 * x1315);
15077 IkReal
15078 x1317
15079 = (px
15080 * x1315);
15081 IkReal
15082 x1318
15083 = (py
15084 * x1313);
15085 IkReal
15086 x1319
15087 = ((((-1.0)
15088 * x1318))
15089 + (((-1.0)
15090 * x1317)));
15091 evalcond
15092 [0]
15093 = ((1.51009803921569)
15094 + (((-1.0)
15095 * (3.92156862745098)
15096 * pp))
15097 + (((-1.0)
15098 * x1316))
15099 + (((1.32323529411765)
15100 * cj9))
15101 + x1314);
15102 evalcond
15103 [1]
15104 = ((-0.55)
15105 + (((-1.0)
15106 * (0.3)
15107 * cj9))
15108 + (((-1.0)
15109 * (0.045)
15110 * sj9))
15111 + x1319);
15112 evalcond
15113 [2]
15114 = ((-0.316735294117647)
15115 + (((-1.0)
15116 * (0.588235294117647)
15117 * pp))
15118 + (((-1.0)
15119 * (0.108264705882353)
15120 * cj9))
15121 + x1319);
15122 evalcond
15123 [3]
15124 = ((-0.2125)
15125 + (((-1.1)
15126 * x1317))
15127 + (((-1.0)
15128 * (1.0)
15129 * pp))
15130 + (((0.09)
15131 * x1314))
15132 + (((-0.09)
15133 * x1316))
15134 + (((-1.1)
15135 * x1318)));
15136 if (IKabs(
15137 evalcond
15138 [0])
15139 > IKFAST_EVALCOND_THRESH
15140 || IKabs(
15141 evalcond
15142 [1])
15143 > IKFAST_EVALCOND_THRESH
15144 || IKabs(
15145 evalcond
15146 [2])
15147 > IKFAST_EVALCOND_THRESH
15148 || IKabs(
15149 evalcond
15150 [3])
15151 > IKFAST_EVALCOND_THRESH)
15152 {
15153 continue;
15154 }
15155 }
15156
15157 rotationfunction0(
15158 solutions);
15159 }
15160 }
15161 }
15162 }
15163 }
15164 else
15165 {
15166 {
15167 IkReal j4array
15168 [1],
15169 cj4array
15170 [1],
15171 sj4array
15172 [1];
15173 bool j4valid
15174 [1]
15175 = {false};
15176 _nj4
15177 = 1;
15178 IkReal
15179 x1320
15180 = (cj9
15181 * px);
15182 IkReal
15183 x1321
15184 = (cj9
15185 * py);
15186 IkReal
15187 x1322
15188 = (pp
15189 * px);
15190 IkReal
15191 x1323
15192 = (pp
15193 * py);
15194 CheckValue<IkReal> x1324 = IKPowWithIntegerCheck(
15195 IKsign((
15196 pp
15197 + (((-1.0)
15198 * (1.0)
15199 * (pz
15200 * pz))))),
15201 -1);
15202 if (!x1324
15203 .valid)
15204 {
15205 continue;
15206 }
15207 CheckValue<IkReal> x1325 = IKatan2WithCheck(
15208 IkReal((
15209 (((-0.108264705882353)
15210 * x1321))
15211 + (((-1.0)
15212 * (1.51009803921569)
15213 * px))
15214 + (((-1.32323529411765)
15215 * x1320))
15216 + (((-1.0)
15217 * (0.316735294117647)
15218 * py))
15219 + (((3.92156862745098)
15220 * x1322))
15221 + (((-0.588235294117647)
15222 * x1323)))),
15223 ((((-3.92156862745098)
15224 * x1323))
15225 + (((-0.588235294117647)
15226 * x1322))
15227 + (((-0.108264705882353)
15228 * x1320))
15229 + (((1.51009803921569)
15230 * py))
15231 + (((1.32323529411765)
15232 * x1321))
15233 + (((-1.0)
15234 * (0.316735294117647)
15235 * px))),
15236 IKFAST_ATAN2_MAGTHRESH);
15237 if (!x1325
15238 .valid)
15239 {
15240 continue;
15241 }
15242 j4array
15243 [0]
15244 = ((-1.5707963267949)
15245 + (((1.5707963267949)
15246 * (x1324
15247 .value)))
15248 + (x1325
15249 .value));
15250 sj4array
15251 [0]
15252 = IKsin(
15253 j4array
15254 [0]);
15255 cj4array
15256 [0]
15257 = IKcos(
15258 j4array
15259 [0]);
15260 if (j4array
15261 [0]
15262 > IKPI)
15263 {
15264 j4array
15265 [0]
15266 -= IK2PI;
15267 }
15268 else if (
15269 j4array
15270 [0]
15271 < -IKPI)
15272 {
15273 j4array
15274 [0]
15275 += IK2PI;
15276 }
15277 j4valid
15278 [0]
15279 = true;
15280 for (
15281 int ij4
15282 = 0;
15283 ij4
15284 < 1;
15285 ++ij4)
15286 {
15287 if (!j4valid
15288 [ij4])
15289 {
15290 continue;
15291 }
15292 _ij4[0]
15293 = ij4;
15294 _ij4[1]
15295 = -1;
15296 for (
15297 int iij4
15298 = ij4
15299 + 1;
15300 iij4
15301 < 1;
15302 ++iij4)
15303 {
15304 if (j4valid
15305 [iij4]
15306 && IKabs(
15307 cj4array
15308 [ij4]
15309 - cj4array
15310 [iij4])
15311 < IKFAST_SOLUTION_THRESH
15312 && IKabs(
15313 sj4array
15314 [ij4]
15315 - sj4array
15316 [iij4])
15317 < IKFAST_SOLUTION_THRESH)
15318 {
15319 j4valid
15320 [iij4]
15321 = false;
15322 _ij4[1]
15323 = iij4;
15324 break;
15325 }
15326 }
15327 j4 = j4array
15328 [ij4];
15329 cj4 = cj4array
15330 [ij4];
15331 sj4 = sj4array
15332 [ij4];
15333 {
15334 IkReal evalcond
15335 [4];
15336 IkReal
15337 x1326
15338 = IKsin(
15339 j4);
15340 IkReal
15341 x1327
15342 = (px
15343 * x1326);
15344 IkReal
15345 x1328
15346 = IKcos(
15347 j4);
15348 IkReal
15349 x1329
15350 = (py
15351 * x1328);
15352 IkReal
15353 x1330
15354 = (px
15355 * x1328);
15356 IkReal
15357 x1331
15358 = (py
15359 * x1326);
15360 IkReal
15361 x1332
15362 = ((((-1.0)
15363 * x1330))
15364 + (((-1.0)
15365 * x1331)));
15366 evalcond
15367 [0]
15368 = ((1.51009803921569)
15369 + x1327
15370 + (((-1.0)
15371 * x1329))
15372 + (((-1.0)
15373 * (3.92156862745098)
15374 * pp))
15375 + (((1.32323529411765)
15376 * cj9)));
15377 evalcond
15378 [1]
15379 = ((-0.55)
15380 + x1332
15381 + (((-1.0)
15382 * (0.3)
15383 * cj9))
15384 + (((-1.0)
15385 * (0.045)
15386 * sj9)));
15387 evalcond
15388 [2]
15389 = ((-0.316735294117647)
15390 + x1332
15391 + (((-1.0)
15392 * (0.588235294117647)
15393 * pp))
15394 + (((-1.0)
15395 * (0.108264705882353)
15396 * cj9)));
15397 evalcond
15398 [3]
15399 = ((-0.2125)
15400 + (((-1.1)
15401 * x1330))
15402 + (((0.09)
15403 * x1327))
15404 + (((-0.09)
15405 * x1329))
15406 + (((-1.0)
15407 * (1.0)
15408 * pp))
15409 + (((-1.1)
15410 * x1331)));
15411 if (IKabs(
15412 evalcond
15413 [0])
15414 > IKFAST_EVALCOND_THRESH
15415 || IKabs(
15416 evalcond
15417 [1])
15418 > IKFAST_EVALCOND_THRESH
15419 || IKabs(
15420 evalcond
15421 [2])
15422 > IKFAST_EVALCOND_THRESH
15423 || IKabs(
15424 evalcond
15425 [3])
15426 > IKFAST_EVALCOND_THRESH)
15427 {
15428 continue;
15429 }
15430 }
15431
15432 rotationfunction0(
15433 solutions);
15434 }
15435 }
15436 }
15437 }
15438 }
15439 else
15440 {
15441 {
15442 IkReal
15443 j4array
15444 [1],
15445 cj4array
15446 [1],
15447 sj4array
15448 [1];
15449 bool j4valid
15450 [1]
15451 = {false};
15452 _nj4 = 1;
15453 IkReal x1333
15454 = (cj9
15455 * px);
15456 IkReal x1334
15457 = (cj9
15458 * py);
15459 IkReal x1335
15460 = ((3.92156862745098)
15461 * pp);
15462 IkReal x1336
15463 = ((0.045)
15464 * sj9);
15465 CheckValue<IkReal> x1337 = IKatan2WithCheck(
15466 IkReal((
15467 (((-1.32323529411765)
15468 * x1333))
15469 + (((-0.3)
15470 * x1334))
15471 + ((px
15472 * x1335))
15473 + (((-1.0)
15474 * (1.51009803921569)
15475 * px))
15476 + (((-1.0)
15477 * py
15478 * x1336))
15479 + (((-1.0)
15480 * (0.55)
15481 * py)))),
15482 ((((-1.0)
15483 * (0.55)
15484 * px))
15485 + (((-1.0)
15486 * py
15487 * x1335))
15488 + (((-1.0)
15489 * px
15490 * x1336))
15491 + (((1.51009803921569)
15492 * py))
15493 + (((-0.3)
15494 * x1333))
15495 + (((1.32323529411765)
15496 * x1334))),
15497 IKFAST_ATAN2_MAGTHRESH);
15498 if (!x1337
15499 .valid)
15500 {
15501 continue;
15502 }
15503 CheckValue<IkReal> x1338 = IKPowWithIntegerCheck(
15504 IKsign((
15505 pp
15506 + (((-1.0)
15507 * (1.0)
15508 * (pz
15509 * pz))))),
15510 -1);
15511 if (!x1338
15512 .valid)
15513 {
15514 continue;
15515 }
15516 j4array[0]
15517 = ((-1.5707963267949)
15518 + (x1337
15519 .value)
15520 + (((1.5707963267949)
15521 * (x1338
15522 .value))));
15523 sj4array[0] = IKsin(
15524 j4array
15525 [0]);
15526 cj4array[0] = IKcos(
15527 j4array
15528 [0]);
15529 if (j4array
15530 [0]
15531 > IKPI)
15532 {
15533 j4array[0]
15534 -= IK2PI;
15535 }
15536 else if (
15537 j4array
15538 [0]
15539 < -IKPI)
15540 {
15541 j4array[0]
15542 += IK2PI;
15543 }
15544 j4valid[0]
15545 = true;
15546 for (int ij4
15547 = 0;
15548 ij4
15549 < 1;
15550 ++ij4)
15551 {
15552 if (!j4valid
15553 [ij4])
15554 {
15555 continue;
15556 }
15557 _ij4[0]
15558 = ij4;
15559 _ij4[1]
15560 = -1;
15561 for (
15562 int iij4
15563 = ij4
15564 + 1;
15565 iij4
15566 < 1;
15567 ++iij4)
15568 {
15569 if (j4valid
15570 [iij4]
15571 && IKabs(
15572 cj4array
15573 [ij4]
15574 - cj4array
15575 [iij4])
15576 < IKFAST_SOLUTION_THRESH
15577 && IKabs(
15578 sj4array
15579 [ij4]
15580 - sj4array
15581 [iij4])
15582 < IKFAST_SOLUTION_THRESH)
15583 {
15584 j4valid
15585 [iij4]
15586 = false;
15587 _ij4[1]
15588 = iij4;
15589 break;
15590 }
15591 }
15592 j4 = j4array
15593 [ij4];
15594 cj4 = cj4array
15595 [ij4];
15596 sj4 = sj4array
15597 [ij4];
15598 {
15599 IkReal evalcond
15600 [4];
15601 IkReal
15602 x1339
15603 = IKsin(
15604 j4);
15605 IkReal
15606 x1340
15607 = (px
15608 * x1339);
15609 IkReal
15610 x1341
15611 = IKcos(
15612 j4);
15613 IkReal
15614 x1342
15615 = (py
15616 * x1341);
15617 IkReal
15618 x1343
15619 = (px
15620 * x1341);
15621 IkReal
15622 x1344
15623 = (py
15624 * x1339);
15625 IkReal
15626 x1345
15627 = ((((-1.0)
15628 * x1343))
15629 + (((-1.0)
15630 * x1344)));
15631 evalcond
15632 [0]
15633 = ((1.51009803921569)
15634 + (((-1.0)
15635 * (3.92156862745098)
15636 * pp))
15637 + (((-1.0)
15638 * x1342))
15639 + x1340
15640 + (((1.32323529411765)
15641 * cj9)));
15642 evalcond
15643 [1]
15644 = ((-0.55)
15645 + (((-1.0)
15646 * (0.3)
15647 * cj9))
15648 + (((-1.0)
15649 * (0.045)
15650 * sj9))
15651 + x1345);
15652 evalcond
15653 [2]
15654 = ((-0.316735294117647)
15655 + (((-1.0)
15656 * (0.588235294117647)
15657 * pp))
15658 + (((-1.0)
15659 * (0.108264705882353)
15660 * cj9))
15661 + x1345);
15662 evalcond
15663 [3]
15664 = ((-0.2125)
15665 + (((-1.1)
15666 * x1344))
15667 + (((-1.1)
15668 * x1343))
15669 + (((0.09)
15670 * x1340))
15671 + (((-0.09)
15672 * x1342))
15673 + (((-1.0)
15674 * (1.0)
15675 * pp)));
15676 if (IKabs(
15677 evalcond
15678 [0])
15679 > IKFAST_EVALCOND_THRESH
15680 || IKabs(
15681 evalcond
15682 [1])
15683 > IKFAST_EVALCOND_THRESH
15684 || IKabs(
15685 evalcond
15686 [2])
15687 > IKFAST_EVALCOND_THRESH
15688 || IKabs(
15689 evalcond
15690 [3])
15691 > IKFAST_EVALCOND_THRESH)
15692 {
15693 continue;
15694 }
15695 }
15696
15697 rotationfunction0(
15698 solutions);
15699 }
15700 }
15701 }
15702 }
15703 }
15704 } while (0);
15705 if (bgotonextstatement)
15706 {
15707 bool
15708 bgotonextstatement
15709 = true;
15710 do
15711 {
15712 IkReal x1346
15713 = (cj6 * pz);
15714 IkReal x1347
15715 = ((1.32323529411765)
15716 * cj9);
15717 IkReal x1348
15718 = ((3.92156862745098)
15719 * pp);
15720 evalcond[0]
15721 = ((IKabs(py))
15722 + (IKabs(
15723 px)));
15724 evalcond[1]
15725 = ((-0.55)
15726 + (((-1.0)
15727 * (0.3)
15728 * cj9))
15729 + (((-1.0)
15730 * (0.045)
15731 * sj9))
15732 + x1346);
15733 evalcond[2]
15734 = ((1.51009803921569)
15735 + (((-1.0)
15736 * x1348))
15737 + x1347);
15738 evalcond[3]
15739 = (pz * sj6);
15740 evalcond[4]
15741 = ((-1.51009803921569)
15742 + (((-1.0)
15743 * x1347))
15744 + x1348);
15745 evalcond[5]
15746 = ((((0.108264705882353)
15747 * cj9
15748 * sj6))
15749 + (((0.588235294117647)
15750 * pp
15751 * sj6))
15752 + (((0.316735294117647)
15753 * sj6)));
15754 evalcond[6]
15755 = ((-0.2125)
15756 + (((1.1)
15757 * x1346))
15758 + (((-1.0)
15759 * (1.0)
15760 * pp)));
15761 if (IKabs(evalcond
15762 [0])
15763 < 0.0000010000000000
15764 && IKabs(
15765 evalcond
15766 [1])
15767 < 0.0000010000000000
15768 && IKabs(
15769 evalcond
15770 [2])
15771 < 0.0000010000000000
15772 && IKabs(
15773 evalcond
15774 [3])
15775 < 0.0000010000000000
15776 && IKabs(
15777 evalcond
15778 [4])
15779 < 0.0000010000000000
15780 && IKabs(
15781 evalcond
15782 [5])
15783 < 0.0000010000000000
15784 && IKabs(
15785 evalcond
15786 [6])
15787 < 0.0000010000000000)
15788 {
15789 bgotonextstatement
15790 = false;
15791 {
15792 IkReal j4array
15793 [4],
15794 cj4array
15795 [4],
15796 sj4array
15797 [4];
15798 bool
15799 j4valid[4]
15800 = {false};
15801 _nj4 = 4;
15802 j4array[0]
15803 = 0;
15804 sj4array[0]
15805 = IKsin(
15806 j4array
15807 [0]);
15808 cj4array[0]
15809 = IKcos(
15810 j4array
15811 [0]);
15812 j4array[1]
15813 = 1.5707963267949;
15814 sj4array[1]
15815 = IKsin(
15816 j4array
15817 [1]);
15818 cj4array[1]
15819 = IKcos(
15820 j4array
15821 [1]);
15822 j4array[2]
15823 = 3.14159265358979;
15824 sj4array[2]
15825 = IKsin(
15826 j4array
15827 [2]);
15828 cj4array[2]
15829 = IKcos(
15830 j4array
15831 [2]);
15832 j4array[3]
15833 = -1.5707963267949;
15834 sj4array[3]
15835 = IKsin(
15836 j4array
15837 [3]);
15838 cj4array[3]
15839 = IKcos(
15840 j4array
15841 [3]);
15842 if (j4array[0]
15843 > IKPI)
15844 {
15845 j4array[0]
15846 -= IK2PI;
15847 }
15848 else if (
15849 j4array[0]
15850 < -IKPI)
15851 {
15852 j4array[0]
15853 += IK2PI;
15854 }
15855 j4valid[0]
15856 = true;
15857 if (j4array[1]
15858 > IKPI)
15859 {
15860 j4array[1]
15861 -= IK2PI;
15862 }
15863 else if (
15864 j4array[1]
15865 < -IKPI)
15866 {
15867 j4array[1]
15868 += IK2PI;
15869 }
15870 j4valid[1]
15871 = true;
15872 if (j4array[2]
15873 > IKPI)
15874 {
15875 j4array[2]
15876 -= IK2PI;
15877 }
15878 else if (
15879 j4array[2]
15880 < -IKPI)
15881 {
15882 j4array[2]
15883 += IK2PI;
15884 }
15885 j4valid[2]
15886 = true;
15887 if (j4array[3]
15888 > IKPI)
15889 {
15890 j4array[3]
15891 -= IK2PI;
15892 }
15893 else if (
15894 j4array[3]
15895 < -IKPI)
15896 {
15897 j4array[3]
15898 += IK2PI;
15899 }
15900 j4valid[3]
15901 = true;
15902 for (int ij4
15903 = 0;
15904 ij4 < 4;
15905 ++ij4)
15906 {
15907 if (!j4valid
15908 [ij4])
15909 {
15910 continue;
15911 }
15912 _ij4[0]
15913 = ij4;
15914 _ij4[1]
15915 = -1;
15916 for (
15917 int iij4
15918 = ij4
15919 + 1;
15920 iij4
15921 < 4;
15922 ++iij4)
15923 {
15924 if (j4valid
15925 [iij4]
15926 && IKabs(
15927 cj4array
15928 [ij4]
15929 - cj4array
15930 [iij4])
15931 < IKFAST_SOLUTION_THRESH
15932 && IKabs(
15933 sj4array
15934 [ij4]
15935 - sj4array
15936 [iij4])
15937 < IKFAST_SOLUTION_THRESH)
15938 {
15939 j4valid
15940 [iij4]
15941 = false;
15942 _ij4[1]
15943 = iij4;
15944 break;
15945 }
15946 }
15947 j4 = j4array
15948 [ij4];
15949 cj4 = cj4array
15950 [ij4];
15951 sj4 = sj4array
15952 [ij4];
15953
15954 rotationfunction0(
15955 solutions);
15956 }
15957 }
15958 }
15959 } while (0);
15960 if (bgotonextstatement)
15961 {
15962 bool
15963 bgotonextstatement
15964 = true;
15965 do
15966 {
15967 if (1)
15968 {
15969 bgotonextstatement
15970 = false;
15971 continue; // branch miss [j4]
15972 }
15973 } while (0);
15974 if (bgotonextstatement)
15975 {
15976 }
15977 }
15978 }
15979 }
15980 }
15981 }
15982 else
15983 {
15984 {
15985 IkReal j4array[1],
15986 cj4array[1],
15987 sj4array[1];
15988 bool j4valid[1]
15989 = {false};
15990 _nj4 = 1;
15991 IkReal x1349
15992 = ((1.32323529411765)
15993 * cj9);
15994 IkReal x1350
15995 = ((3.92156862745098)
15996 * pp);
15997 IkReal x1351
15998 = ((0.316735294117647)
15999 * sj6);
16000 IkReal x1352
16001 = ((0.108264705882353)
16002 * cj9 * sj6);
16003 IkReal x1353
16004 = ((0.588235294117647)
16005 * pp * sj6);
16006 CheckValue<IkReal> x1354 = IKatan2WithCheck(
16007 IkReal((
16008 ((py * x1353))
16009 + (((-1.0)
16010 * (1.51009803921569)
16011 * px))
16012 + (((-1.0) * px
16013 * x1349))
16014 + ((py * x1352))
16015 + ((px * x1350))
16016 + ((py
16017 * x1351)))),
16018 (((px * x1351))
16019 + ((px * x1353))
16020 + (((1.51009803921569)
16021 * py))
16022 + ((py * x1349))
16023 + ((px * x1352))
16024 + (((-1.0) * py
16025 * x1350))),
16026 IKFAST_ATAN2_MAGTHRESH);
16027 if (!x1354.valid)
16028 {
16029 continue;
16030 }
16031 CheckValue<IkReal> x1355
16032 = IKPowWithIntegerCheck(
16033 IKsign((
16034 pp
16035 + (((-1.0)
16036 * (1.0)
16037 * (pz
16038 * pz))))),
16039 -1);
16040 if (!x1355.valid)
16041 {
16042 continue;
16043 }
16044 j4array[0]
16045 = ((-1.5707963267949)
16046 + (x1354.value)
16047 + (((1.5707963267949)
16048 * (x1355
16049 .value))));
16050 sj4array[0]
16051 = IKsin(j4array[0]);
16052 cj4array[0]
16053 = IKcos(j4array[0]);
16054 if (j4array[0] > IKPI)
16055 {
16056 j4array[0] -= IK2PI;
16057 }
16058 else if (
16059 j4array[0] < -IKPI)
16060 {
16061 j4array[0] += IK2PI;
16062 }
16063 j4valid[0] = true;
16064 for (int ij4 = 0;
16065 ij4 < 1;
16066 ++ij4)
16067 {
16068 if (!j4valid[ij4])
16069 {
16070 continue;
16071 }
16072 _ij4[0] = ij4;
16073 _ij4[1] = -1;
16074 for (int iij4
16075 = ij4 + 1;
16076 iij4 < 1;
16077 ++iij4)
16078 {
16079 if (j4valid[iij4]
16080 && IKabs(
16081 cj4array
16082 [ij4]
16083 - cj4array
16084 [iij4])
16085 < IKFAST_SOLUTION_THRESH
16086 && IKabs(
16087 sj4array
16088 [ij4]
16089 - sj4array
16090 [iij4])
16091 < IKFAST_SOLUTION_THRESH)
16092 {
16093 j4valid[iij4]
16094 = false;
16095 _ij4[1] = iij4;
16096 break;
16097 }
16098 }
16099 j4 = j4array[ij4];
16100 cj4 = cj4array[ij4];
16101 sj4 = sj4array[ij4];
16102 {
16103 IkReal evalcond[5];
16104 IkReal x1356
16105 = IKcos(j4);
16106 IkReal x1357
16107 = ((1.0)
16108 * x1356);
16109 IkReal x1358
16110 = (px * x1357);
16111 IkReal x1359
16112 = IKsin(j4);
16113 IkReal x1360
16114 = (py * x1359);
16115 IkReal x1361
16116 = ((1.0)
16117 * x1360);
16118 IkReal x1362
16119 = (px * x1359);
16120 IkReal x1363
16121 = (cj6 * pz);
16122 IkReal x1364
16123 = (px * sj6
16124 * x1356);
16125 IkReal x1365
16126 = (sj6 * x1360);
16127 evalcond[0]
16128 = ((((-1.0)
16129 * cj6
16130 * x1361))
16131 + (((-1.0)
16132 * cj6
16133 * x1358))
16134 + ((pz
16135 * sj6)));
16136 evalcond[1]
16137 = ((1.51009803921569)
16138 + (((-1.0)
16139 * py
16140 * x1357))
16141 + (((-1.0)
16142 * (3.92156862745098)
16143 * pp))
16144 + x1362
16145 + (((1.32323529411765)
16146 * cj9)));
16147 evalcond[2]
16148 = ((-0.55)
16149 + (((-1.0)
16150 * (0.3)
16151 * cj9))
16152 + x1363
16153 + x1365
16154 + x1364
16155 + (((-1.0)
16156 * (0.045)
16157 * sj9)));
16158 evalcond[3]
16159 = ((((0.108264705882353)
16160 * cj9
16161 * sj6))
16162 + (((0.588235294117647)
16163 * pp
16164 * sj6))
16165 + (((-1.0)
16166 * x1361))
16167 + (((-1.0)
16168 * x1358))
16169 + (((0.316735294117647)
16170 * sj6)));
16171 evalcond[4]
16172 = ((-0.2125)
16173 + (((1.1)
16174 * x1364))
16175 + (((1.1)
16176 * x1365))
16177 + (((-0.09)
16178 * py
16179 * x1356))
16180 + (((-1.0)
16181 * (1.0)
16182 * pp))
16183 + (((1.1)
16184 * x1363))
16185 + (((0.09)
16186 * x1362)));
16187 if (IKabs(
16188 evalcond[0])
16189 > IKFAST_EVALCOND_THRESH
16190 || IKabs(
16191 evalcond
16192 [1])
16193 > IKFAST_EVALCOND_THRESH
16194 || IKabs(
16195 evalcond
16196 [2])
16197 > IKFAST_EVALCOND_THRESH
16198 || IKabs(
16199 evalcond
16200 [3])
16201 > IKFAST_EVALCOND_THRESH
16202 || IKabs(
16203 evalcond
16204 [4])
16205 > IKFAST_EVALCOND_THRESH)
16206 {
16207 continue;
16208 }
16209 }
16210
16211 rotationfunction0(
16212 solutions);
16213 }
16214 }
16215 }
16216 }
16217 }
16218 else
16219 {
16220 {
16221 IkReal j4array[1],
16222 cj4array[1],
16223 sj4array[1];
16224 bool j4valid[1] = {false};
16225 _nj4 = 1;
16226 IkReal x1366 = (cj6 * pp);
16227 IkReal x1367
16228 = ((0.2125) * cj6);
16229 IkReal x1368 = ((1.1) * pz);
16230 IkReal x1369
16231 = ((0.09) * pz * sj6);
16232 CheckValue<IkReal> x1370
16233 = IKatan2WithCheck(
16234 IkReal((
16235 ((px * x1368))
16236 + (((-1.0) * px
16237 * x1367))
16238 + (((-1.0) * px
16239 * x1366))
16240 + (((-1.0) * py
16241 * x1369)))),
16242 ((((-1.0) * px
16243 * x1369))
16244 + (((-1.0) * py
16245 * x1368))
16246 + ((py * x1367))
16247 + ((py * x1366))),
16248 IKFAST_ATAN2_MAGTHRESH);
16249 if (!x1370.valid)
16250 {
16251 continue;
16252 }
16253 CheckValue<IkReal> x1371
16254 = IKPowWithIntegerCheck(
16255 IKsign((
16256 (((0.09) * cj6
16257 * (pz * pz)))
16258 + (((-0.09)
16259 * x1366)))),
16260 -1);
16261 if (!x1371.valid)
16262 {
16263 continue;
16264 }
16265 j4array[0]
16266 = ((-1.5707963267949)
16267 + (x1370.value)
16268 + (((1.5707963267949)
16269 * (x1371
16270 .value))));
16271 sj4array[0]
16272 = IKsin(j4array[0]);
16273 cj4array[0]
16274 = IKcos(j4array[0]);
16275 if (j4array[0] > IKPI)
16276 {
16277 j4array[0] -= IK2PI;
16278 }
16279 else if (j4array[0] < -IKPI)
16280 {
16281 j4array[0] += IK2PI;
16282 }
16283 j4valid[0] = true;
16284 for (int ij4 = 0; ij4 < 1;
16285 ++ij4)
16286 {
16287 if (!j4valid[ij4])
16288 {
16289 continue;
16290 }
16291 _ij4[0] = ij4;
16292 _ij4[1] = -1;
16293 for (int iij4 = ij4 + 1;
16294 iij4 < 1;
16295 ++iij4)
16296 {
16297 if (j4valid[iij4]
16298 && IKabs(
16299 cj4array[ij4]
16300 - cj4array
16301 [iij4])
16302 < IKFAST_SOLUTION_THRESH
16303 && IKabs(
16304 sj4array[ij4]
16305 - sj4array
16306 [iij4])
16307 < IKFAST_SOLUTION_THRESH)
16308 {
16309 j4valid[iij4] = false;
16310 _ij4[1] = iij4;
16311 break;
16312 }
16313 }
16314 j4 = j4array[ij4];
16315 cj4 = cj4array[ij4];
16316 sj4 = sj4array[ij4];
16317 {
16318 IkReal evalcond[5];
16319 IkReal x1372
16320 = IKcos(j4);
16321 IkReal x1373
16322 = ((1.0) * x1372);
16323 IkReal x1374
16324 = (px * x1373);
16325 IkReal x1375
16326 = IKsin(j4);
16327 IkReal x1376
16328 = (py * x1375);
16329 IkReal x1377
16330 = ((1.0) * x1376);
16331 IkReal x1378
16332 = (px * x1375);
16333 IkReal x1379
16334 = (cj6 * pz);
16335 IkReal x1380
16336 = (px * sj6
16337 * x1372);
16338 IkReal x1381
16339 = (sj6 * x1376);
16340 evalcond[0]
16341 = ((((-1.0) * cj6
16342 * x1374))
16343 + ((pz * sj6))
16344 + (((-1.0) * cj6
16345 * x1377)));
16346 evalcond[1]
16347 = ((1.51009803921569)
16348 + x1378
16349 + (((-1.0)
16350 * (3.92156862745098)
16351 * pp))
16352 + (((-1.0) * py
16353 * x1373))
16354 + (((1.32323529411765)
16355 * cj9)));
16356 evalcond[2]
16357 = ((-0.55) + x1379
16358 + x1380 + x1381
16359 + (((-1.0)
16360 * (0.3)
16361 * cj9))
16362 + (((-1.0)
16363 * (0.045)
16364 * sj9)));
16365 evalcond[3]
16366 = ((((0.108264705882353)
16367 * cj9 * sj6))
16368 + (((-1.0)
16369 * x1374))
16370 + (((0.588235294117647)
16371 * pp * sj6))
16372 + (((0.316735294117647)
16373 * sj6))
16374 + (((-1.0)
16375 * x1377)));
16376 evalcond[4]
16377 = ((-0.2125)
16378 + (((-0.09) * py
16379 * x1372))
16380 + (((0.09)
16381 * x1378))
16382 + (((1.1)
16383 * x1379))
16384 + (((1.1)
16385 * x1381))
16386 + (((1.1)
16387 * x1380))
16388 + (((-1.0)
16389 * (1.0)
16390 * pp)));
16391 if (IKabs(evalcond[0])
16392 > IKFAST_EVALCOND_THRESH
16393 || IKabs(
16394 evalcond[1])
16395 > IKFAST_EVALCOND_THRESH
16396 || IKabs(
16397 evalcond[2])
16398 > IKFAST_EVALCOND_THRESH
16399 || IKabs(
16400 evalcond[3])
16401 > IKFAST_EVALCOND_THRESH
16402 || IKabs(
16403 evalcond[4])
16404 > IKFAST_EVALCOND_THRESH)
16405 {
16406 continue;
16407 }
16408 }
16409
16410 rotationfunction0(
16411 solutions);
16412 }
16413 }
16414 }
16415 }
16416 }
16417 else
16418 {
16419 {
16420 IkReal j4array[1], cj4array[1],
16421 sj4array[1];
16422 bool j4valid[1] = {false};
16423 _nj4 = 1;
16424 IkReal x1382
16425 = ((1.51009803921569)
16426 * cj6);
16427 IkReal x1383 = (pz * sj6);
16428 IkReal x1384
16429 = ((1.32323529411765) * cj6
16430 * cj9);
16431 IkReal x1385
16432 = ((3.92156862745098) * cj6
16433 * pp);
16434 CheckValue<IkReal> x1386
16435 = IKPowWithIntegerCheck(
16436 IKsign(
16437 ((((-1.0) * (1.0)
16438 * cj6
16439 * (pz * pz)))
16440 + ((cj6 * pp)))),
16441 -1);
16442 if (!x1386.valid)
16443 {
16444 continue;
16445 }
16446 CheckValue<IkReal> x1387
16447 = IKatan2WithCheck(
16448 IkReal(
16449 ((((-1.0) * px
16450 * x1384))
16451 + (((-1.0) * px
16452 * x1382))
16453 + ((py * x1383))
16454 + ((px * x1385)))),
16455 ((((-1.0) * py * x1385))
16456 + ((py * x1384))
16457 + ((px * x1383))
16458 + ((py * x1382))),
16459 IKFAST_ATAN2_MAGTHRESH);
16460 if (!x1387.valid)
16461 {
16462 continue;
16463 }
16464 j4array[0]
16465 = ((-1.5707963267949)
16466 + (((1.5707963267949)
16467 * (x1386.value)))
16468 + (x1387.value));
16469 sj4array[0] = IKsin(j4array[0]);
16470 cj4array[0] = IKcos(j4array[0]);
16471 if (j4array[0] > IKPI)
16472 {
16473 j4array[0] -= IK2PI;
16474 }
16475 else if (j4array[0] < -IKPI)
16476 {
16477 j4array[0] += IK2PI;
16478 }
16479 j4valid[0] = true;
16480 for (int ij4 = 0; ij4 < 1;
16481 ++ij4)
16482 {
16483 if (!j4valid[ij4])
16484 {
16485 continue;
16486 }
16487 _ij4[0] = ij4;
16488 _ij4[1] = -1;
16489 for (int iij4 = ij4 + 1;
16490 iij4 < 1;
16491 ++iij4)
16492 {
16493 if (j4valid[iij4]
16494 && IKabs(
16495 cj4array[ij4]
16496 - cj4array[iij4])
16497 < IKFAST_SOLUTION_THRESH
16498 && IKabs(
16499 sj4array[ij4]
16500 - sj4array[iij4])
16501 < IKFAST_SOLUTION_THRESH)
16502 {
16503 j4valid[iij4] = false;
16504 _ij4[1] = iij4;
16505 break;
16506 }
16507 }
16508 j4 = j4array[ij4];
16509 cj4 = cj4array[ij4];
16510 sj4 = sj4array[ij4];
16511 {
16512 IkReal evalcond[5];
16513 IkReal x1388 = IKcos(j4);
16514 IkReal x1389
16515 = ((1.0) * x1388);
16516 IkReal x1390 = (px * x1389);
16517 IkReal x1391 = IKsin(j4);
16518 IkReal x1392 = (py * x1391);
16519 IkReal x1393
16520 = ((1.0) * x1392);
16521 IkReal x1394 = (px * x1391);
16522 IkReal x1395 = (cj6 * pz);
16523 IkReal x1396
16524 = (px * sj6 * x1388);
16525 IkReal x1397
16526 = (sj6 * x1392);
16527 evalcond[0]
16528 = ((((-1.0) * cj6
16529 * x1393))
16530 + ((pz * sj6))
16531 + (((-1.0) * cj6
16532 * x1390)));
16533 evalcond[1]
16534 = ((1.51009803921569)
16535 + (((-1.0) * py
16536 * x1389))
16537 + (((-1.0)
16538 * (3.92156862745098)
16539 * pp))
16540 + x1394
16541 + (((1.32323529411765)
16542 * cj9)));
16543 evalcond[2]
16544 = ((-0.55)
16545 + (((-1.0) * (0.3)
16546 * cj9))
16547 + x1397 + x1396
16548 + x1395
16549 + (((-1.0) * (0.045)
16550 * sj9)));
16551 evalcond[3]
16552 = ((((-1.0) * x1393))
16553 + (((0.108264705882353)
16554 * cj9 * sj6))
16555 + (((0.588235294117647)
16556 * pp * sj6))
16557 + (((0.316735294117647)
16558 * sj6))
16559 + (((-1.0)
16560 * x1390)));
16561 evalcond[4]
16562 = ((-0.2125)
16563 + (((1.1) * x1395))
16564 + (((0.09) * x1394))
16565 + (((-0.09) * py
16566 * x1388))
16567 + (((1.1) * x1396))
16568 + (((1.1) * x1397))
16569 + (((-1.0) * (1.0)
16570 * pp)));
16571 if (IKabs(evalcond[0])
16572 > IKFAST_EVALCOND_THRESH
16573 || IKabs(evalcond[1])
16574 > IKFAST_EVALCOND_THRESH
16575 || IKabs(evalcond[2])
16576 > IKFAST_EVALCOND_THRESH
16577 || IKabs(evalcond[3])
16578 > IKFAST_EVALCOND_THRESH
16579 || IKabs(evalcond[4])
16580 > IKFAST_EVALCOND_THRESH)
16581 {
16582 continue;
16583 }
16584 }
16585
16586 rotationfunction0(solutions);
16587 }
16588 }
16589 }
16590 }
16591 }
16592 } while (0);
16593 if (bgotonextstatement)
16594 {
16595 bool bgotonextstatement = true;
16596 do
16597 {
16598 IkReal x1398
16599 = ((-0.55) + pz
16600 + (((-1.0) * (0.3) * cj9))
16601 + (((-1.0) * (0.045) * sj9)));
16602 evalcond[0]
16603 = ((-3.14159265358979)
16604 + (IKfmod(
16605 ((3.14159265358979)
16606 + (IKabs(j6))),
16607 6.28318530717959)));
16608 evalcond[1]
16609 = ((0.39655) + (((0.0765) * sj9))
16610 + (((0.32595) * cj9))
16611 + (((-1.0) * (1.0) * pp)));
16612 evalcond[2] = x1398;
16613 evalcond[3] = x1398;
16614 if (IKabs(evalcond[0])
16615 < 0.0000010000000000
16616 && IKabs(evalcond[1])
16617 < 0.0000010000000000
16618 && IKabs(evalcond[2])
16619 < 0.0000010000000000
16620 && IKabs(evalcond[3])
16621 < 0.0000010000000000)
16622 {
16623 bgotonextstatement = false;
16624 {
16625 IkReal j4eval[3];
16626 sj6 = 0;
16627 cj6 = 1.0;
16628 j6 = 0;
16629 IkReal x1399
16630 = (pp
16631 + (((-1.0) * (1.0)
16632 * (pz * pz))));
16633 IkReal x1400
16634 = ((1.51009803921569) * cj8);
16635 IkReal x1401
16636 = ((1.51009803921569) * sj8);
16637 IkReal x1402
16638 = ((1.32323529411765) * cj8
16639 * cj9);
16640 IkReal x1403
16641 = ((3.92156862745098) * cj8
16642 * pp);
16643 IkReal x1404
16644 = ((1.32323529411765) * cj9
16645 * sj8);
16646 IkReal x1405
16647 = ((3.92156862745098) * pp
16648 * sj8);
16649 j4eval[0] = x1399;
16650 j4eval[1]
16651 = ((IKabs(
16652 (((py * x1403))
16653 + ((px * x1401))
16654 + (((-1.0) * py
16655 * x1402))
16656 + ((px * x1404))
16657 + (((-1.0) * px
16658 * x1405))
16659 + (((-1.0) * py
16660 * x1400)))))
16661 + (IKabs((
16662 (((-1.0) * px * x1400))
16663 + (((-1.0) * py
16664 * x1404))
16665 + (((-1.0) * px
16666 * x1402))
16667 + (((-1.0) * py
16668 * x1401))
16669 + ((py * x1405))
16670 + ((px * x1403))))));
16671 j4eval[2] = IKsign(x1399);
16672 if (IKabs(j4eval[0])
16673 < 0.0000010000000000
16674 || IKabs(j4eval[1])
16675 < 0.0000010000000000
16676 || IKabs(j4eval[2])
16677 < 0.0000010000000000)
16678 {
16679 {
16680 IkReal j4eval[2];
16681 sj6 = 0;
16682 cj6 = 1.0;
16683 j6 = 0;
16684 IkReal x1406
16685 = (((cj8 * (pz * pz)))
16686 + (((-1.0) * (1.0)
16687 * cj8 * pp)));
16688 j4eval[0] = x1406;
16689 j4eval[1] = IKsign(x1406);
16690 if (IKabs(j4eval[0])
16691 < 0.0000010000000000
16692 || IKabs(j4eval[1])
16693 < 0.0000010000000000)
16694 {
16695 {
16696 IkReal j4eval[2];
16697 sj6 = 0;
16698 cj6 = 1.0;
16699 j6 = 0;
16700 IkReal x1407
16701 = ((((-1.0) * (1.0)
16702 * sj8
16703 * (pz * pz)))
16704 + ((pp * sj8)));
16705 j4eval[0] = x1407;
16706 j4eval[1] = IKsign(x1407);
16707 if (IKabs(j4eval[0])
16708 < 0.0000010000000000
16709 || IKabs(j4eval[1])
16710 < 0.0000010000000000)
16711 {
16712 {
16713 IkReal evalcond[6];
16714 bool
16715 bgotonextstatement
16716 = true;
16717 do
16718 {
16719 evalcond[0]
16720 = ((-3.14159265358979)
16721 + (IKfmod(
16722 ((3.14159265358979)
16723 + (IKabs(
16724 j8))),
16725 6.28318530717959)));
16726 if (IKabs(
16727 evalcond[0])
16728 < 0.0000010000000000)
16729 {
16730 bgotonextstatement
16731 = false;
16732 {
16733 IkReal
16734 j4eval[3];
16735 sj6 = 0;
16736 cj6 = 1.0;
16737 j6 = 0;
16738 sj8 = 0;
16739 cj8 = 1.0;
16740 j8 = 0;
16741 IkReal x1408
16742 = (pp
16743 + (((-1.0)
16744 * (1.0)
16745 * (pz
16746 * pz))));
16747 IkReal x1409
16748 = ((13497.0)
16749 * cj9);
16750 IkReal x1410
16751 = ((40000.0)
16752 * pp);
16753 j4eval[0]
16754 = x1408;
16755 j4eval[1]
16756 = ((IKabs((
16757 ((py
16758 * x1410))
16759 + (((-1.0)
16760 * (15403.0)
16761 * py))
16762 + (((-1.0)
16763 * py
16764 * x1409)))))
16765 + (IKabs((
16766 (((-1.0)
16767 * px
16768 * x1409))
16769 + (((-1.0)
16770 * (15403.0)
16771 * px))
16772 + ((px
16773 * x1410))))));
16774 j4eval[2]
16775 = IKsign(
16776 x1408);
16777 if (IKabs(
16778 j4eval
16779 [0])
16780 < 0.0000010000000000
16781 || IKabs(
16782 j4eval
16783 [1])
16784 < 0.0000010000000000
16785 || IKabs(
16786 j4eval
16787 [2])
16788 < 0.0000010000000000)
16789 {
16790 {
16791 IkReal
16792 j4eval
16793 [3];
16794 sj6 = 0;
16795 cj6 = 1.0;
16796 j6 = 0;
16797 sj8 = 0;
16798 cj8 = 1.0;
16799 j8 = 0;
16800 IkReal x1411
16801 = pz
16802 * pz;
16803 IkReal x1412
16804 = ((80.0)
16805 * pp);
16806 IkReal x1413
16807 = ((88.0)
16808 * pz);
16809 j4eval[0]
16810 = ((((-1.0)
16811 * x1411))
16812 + pp);
16813 j4eval[1]
16814 = ((IKabs((
16815 (((-1.0)
16816 * px
16817 * x1413))
16818 + ((px
16819 * x1412))
16820 + (((17.0)
16821 * px)))))
16822 + (IKabs((
16823 ((py
16824 * x1412))
16825 + (((17.0)
16826 * py))
16827 + (((-1.0)
16828 * py
16829 * x1413))))));
16830 j4eval[2] = IKsign((
16831 (((9.0)
16832 * pp))
16833 + (((-9.0)
16834 * x1411))));
16835 if (IKabs(
16836 j4eval
16837 [0])
16838 < 0.0000010000000000
16839 || IKabs(
16840 j4eval
16841 [1])
16842 < 0.0000010000000000
16843 || IKabs(
16844 j4eval
16845 [2])
16846 < 0.0000010000000000)
16847 {
16848 {
16849 IkReal evalcond
16850 [5];
16851 bool
16852 bgotonextstatement
16853 = true;
16854 do
16855 {
16856 IkReal
16857 x1414
16858 = ((-1.51009803921569)
16859 + (((-1.0)
16860 * (1.32323529411765)
16861 * cj9))
16862 + (((3.92156862745098)
16863 * pp)));
16864 evalcond
16865 [0]
16866 = ((IKabs(
16867 py))
16868 + (IKabs(
16869 px)));
16870 evalcond
16871 [1]
16872 = 0;
16873 evalcond
16874 [2]
16875 = x1414;
16876 evalcond
16877 [3]
16878 = x1414;
16879 evalcond
16880 [4]
16881 = ((-0.2125)
16882 + (((1.1)
16883 * pz))
16884 + (((-1.0)
16885 * (1.0)
16886 * pp)));
16887 if (IKabs(
16888 evalcond
16889 [0])
16890 < 0.0000010000000000
16891 && IKabs(
16892 evalcond
16893 [1])
16894 < 0.0000010000000000
16895 && IKabs(
16896 evalcond
16897 [2])
16898 < 0.0000010000000000
16899 && IKabs(
16900 evalcond
16901 [3])
16902 < 0.0000010000000000
16903 && IKabs(
16904 evalcond
16905 [4])
16906 < 0.0000010000000000)
16907 {
16908 bgotonextstatement
16909 = false;
16910 {
16911 IkReal j4array
16912 [4],
16913 cj4array
16914 [4],
16915 sj4array
16916 [4];
16917 bool j4valid
16918 [4]
16919 = {false};
16920 _nj4
16921 = 4;
16922 j4array
16923 [0]
16924 = 0;
16925 sj4array
16926 [0]
16927 = IKsin(
16928 j4array
16929 [0]);
16930 cj4array
16931 [0]
16932 = IKcos(
16933 j4array
16934 [0]);
16935 j4array
16936 [1]
16937 = 1.5707963267949;
16938 sj4array
16939 [1]
16940 = IKsin(
16941 j4array
16942 [1]);
16943 cj4array
16944 [1]
16945 = IKcos(
16946 j4array
16947 [1]);
16948 j4array
16949 [2]
16950 = 3.14159265358979;
16951 sj4array
16952 [2]
16953 = IKsin(
16954 j4array
16955 [2]);
16956 cj4array
16957 [2]
16958 = IKcos(
16959 j4array
16960 [2]);
16961 j4array
16962 [3]
16963 = -1.5707963267949;
16964 sj4array
16965 [3]
16966 = IKsin(
16967 j4array
16968 [3]);
16969 cj4array
16970 [3]
16971 = IKcos(
16972 j4array
16973 [3]);
16974 if (j4array
16975 [0]
16976 > IKPI)
16977 {
16978 j4array
16979 [0]
16980 -= IK2PI;
16981 }
16982 else if (
16983 j4array
16984 [0]
16985 < -IKPI)
16986 {
16987 j4array
16988 [0]
16989 += IK2PI;
16990 }
16991 j4valid
16992 [0]
16993 = true;
16994 if (j4array
16995 [1]
16996 > IKPI)
16997 {
16998 j4array
16999 [1]
17000 -= IK2PI;
17001 }
17002 else if (
17003 j4array
17004 [1]
17005 < -IKPI)
17006 {
17007 j4array
17008 [1]
17009 += IK2PI;
17010 }
17011 j4valid
17012 [1]
17013 = true;
17014 if (j4array
17015 [2]
17016 > IKPI)
17017 {
17018 j4array
17019 [2]
17020 -= IK2PI;
17021 }
17022 else if (
17023 j4array
17024 [2]
17025 < -IKPI)
17026 {
17027 j4array
17028 [2]
17029 += IK2PI;
17030 }
17031 j4valid
17032 [2]
17033 = true;
17034 if (j4array
17035 [3]
17036 > IKPI)
17037 {
17038 j4array
17039 [3]
17040 -= IK2PI;
17041 }
17042 else if (
17043 j4array
17044 [3]
17045 < -IKPI)
17046 {
17047 j4array
17048 [3]
17049 += IK2PI;
17050 }
17051 j4valid
17052 [3]
17053 = true;
17054 for (
17055 int ij4
17056 = 0;
17057 ij4
17058 < 4;
17059 ++ij4)
17060 {
17061 if (!j4valid
17062 [ij4])
17063 {
17064 continue;
17065 }
17066 _ij4[0]
17067 = ij4;
17068 _ij4[1]
17069 = -1;
17070 for (
17071 int iij4
17072 = ij4
17073 + 1;
17074 iij4
17075 < 4;
17076 ++iij4)
17077 {
17078 if (j4valid
17079 [iij4]
17080 && IKabs(
17081 cj4array
17082 [ij4]
17083 - cj4array
17084 [iij4])
17085 < IKFAST_SOLUTION_THRESH
17086 && IKabs(
17087 sj4array
17088 [ij4]
17089 - sj4array
17090 [iij4])
17091 < IKFAST_SOLUTION_THRESH)
17092 {
17093 j4valid
17094 [iij4]
17095 = false;
17096 _ij4[1]
17097 = iij4;
17098 break;
17099 }
17100 }
17101 j4 = j4array
17102 [ij4];
17103 cj4 = cj4array
17104 [ij4];
17105 sj4 = sj4array
17106 [ij4];
17107
17108 rotationfunction0(
17109 solutions);
17110 }
17111 }
17112 }
17113 } while (
17114 0);
17115 if (bgotonextstatement)
17116 {
17117 bool
17118 bgotonextstatement
17119 = true;
17120 do
17121 {
17122 if (1)
17123 {
17124 bgotonextstatement
17125 = false;
17126 continue; // branch miss [j4]
17127 }
17128 } while (
17129 0);
17130 if (bgotonextstatement)
17131 {
17132 }
17133 }
17134 }
17135 }
17136 else
17137 {
17138 {
17139 IkReal j4array
17140 [1],
17141 cj4array
17142 [1],
17143 sj4array
17144 [1];
17145 bool j4valid
17146 [1]
17147 = {false};
17148 _nj4
17149 = 1;
17150 IkReal
17151 x1415
17152 = ((100.0)
17153 * pp);
17154 IkReal
17155 x1416
17156 = ((110.0)
17157 * pz);
17158 CheckValue<IkReal> x1417 = IKPowWithIntegerCheck(
17159 IKsign((
17160 (((-1.0)
17161 * (9.0)
17162 * (pz
17163 * pz)))
17164 + (((9.0)
17165 * pp)))),
17166 -1);
17167 if (!x1417
17168 .valid)
17169 {
17170 continue;
17171 }
17172 CheckValue<IkReal> x1418 = IKatan2WithCheck(
17173 IkReal((
17174 ((py
17175 * x1415))
17176 + (((21.25)
17177 * py))
17178 + (((-1.0)
17179 * py
17180 * x1416)))),
17181 (((px
17182 * x1415))
17183 + (((-1.0)
17184 * px
17185 * x1416))
17186 + (((21.25)
17187 * px))),
17188 IKFAST_ATAN2_MAGTHRESH);
17189 if (!x1418
17190 .valid)
17191 {
17192 continue;
17193 }
17194 j4array
17195 [0]
17196 = ((-1.5707963267949)
17197 + (((1.5707963267949)
17198 * (x1417
17199 .value)))
17200 + (x1418
17201 .value));
17202 sj4array
17203 [0]
17204 = IKsin(
17205 j4array
17206 [0]);
17207 cj4array
17208 [0]
17209 = IKcos(
17210 j4array
17211 [0]);
17212 if (j4array
17213 [0]
17214 > IKPI)
17215 {
17216 j4array
17217 [0]
17218 -= IK2PI;
17219 }
17220 else if (
17221 j4array
17222 [0]
17223 < -IKPI)
17224 {
17225 j4array
17226 [0]
17227 += IK2PI;
17228 }
17229 j4valid
17230 [0]
17231 = true;
17232 for (
17233 int ij4
17234 = 0;
17235 ij4
17236 < 1;
17237 ++ij4)
17238 {
17239 if (!j4valid
17240 [ij4])
17241 {
17242 continue;
17243 }
17244 _ij4[0]
17245 = ij4;
17246 _ij4[1]
17247 = -1;
17248 for (
17249 int iij4
17250 = ij4
17251 + 1;
17252 iij4
17253 < 1;
17254 ++iij4)
17255 {
17256 if (j4valid
17257 [iij4]
17258 && IKabs(
17259 cj4array
17260 [ij4]
17261 - cj4array
17262 [iij4])
17263 < IKFAST_SOLUTION_THRESH
17264 && IKabs(
17265 sj4array
17266 [ij4]
17267 - sj4array
17268 [iij4])
17269 < IKFAST_SOLUTION_THRESH)
17270 {
17271 j4valid
17272 [iij4]
17273 = false;
17274 _ij4[1]
17275 = iij4;
17276 break;
17277 }
17278 }
17279 j4 = j4array
17280 [ij4];
17281 cj4 = cj4array
17282 [ij4];
17283 sj4 = sj4array
17284 [ij4];
17285 {
17286 IkReal evalcond
17287 [3];
17288 IkReal
17289 x1419
17290 = IKsin(
17291 j4);
17292 IkReal
17293 x1420
17294 = IKcos(
17295 j4);
17296 IkReal
17297 x1421
17298 = (px
17299 * x1420);
17300 IkReal
17301 x1422
17302 = (py
17303 * x1419);
17304 evalcond
17305 [0]
17306 = ((((-1.0)
17307 * py
17308 * x1420))
17309 + ((px
17310 * x1419)));
17311 evalcond
17312 [1]
17313 = ((-1.51009803921569)
17314 + (((-1.0)
17315 * (1.32323529411765)
17316 * cj9))
17317 + (((-1.0)
17318 * x1421))
17319 + (((-1.0)
17320 * x1422))
17321 + (((3.92156862745098)
17322 * pp)));
17323 evalcond
17324 [2]
17325 = ((-0.2125)
17326 + (((1.1)
17327 * pz))
17328 + (((0.09)
17329 * x1421))
17330 + (((0.09)
17331 * x1422))
17332 + (((-1.0)
17333 * (1.0)
17334 * pp)));
17335 if (IKabs(
17336 evalcond
17337 [0])
17338 > IKFAST_EVALCOND_THRESH
17339 || IKabs(
17340 evalcond
17341 [1])
17342 > IKFAST_EVALCOND_THRESH
17343 || IKabs(
17344 evalcond
17345 [2])
17346 > IKFAST_EVALCOND_THRESH)
17347 {
17348 continue;
17349 }
17350 }
17351
17352 rotationfunction0(
17353 solutions);
17354 }
17355 }
17356 }
17357 }
17358 }
17359 else
17360 {
17361 {
17362 IkReal
17363 j4array
17364 [1],
17365 cj4array
17366 [1],
17367 sj4array
17368 [1];
17369 bool j4valid
17370 [1]
17371 = {false};
17372 _nj4 = 1;
17373 IkReal x1423
17374 = ((1.32323529411765)
17375 * cj9);
17376 IkReal x1424
17377 = ((3.92156862745098)
17378 * pp);
17379 CheckValue<IkReal> x1425 = IKatan2WithCheck(
17380 IkReal((
17381 ((py
17382 * x1424))
17383 + (((-1.0)
17384 * (1.51009803921569)
17385 * py))
17386 + (((-1.0)
17387 * py
17388 * x1423)))),
17389 ((((-1.0)
17390 * px
17391 * x1423))
17392 + (((-1.0)
17393 * (1.51009803921569)
17394 * px))
17395 + ((px
17396 * x1424))),
17397 IKFAST_ATAN2_MAGTHRESH);
17398 if (!x1425
17399 .valid)
17400 {
17401 continue;
17402 }
17403 CheckValue<IkReal> x1426 = IKPowWithIntegerCheck(
17404 IKsign((
17405 pp
17406 + (((-1.0)
17407 * (1.0)
17408 * (pz
17409 * pz))))),
17410 -1);
17411 if (!x1426
17412 .valid)
17413 {
17414 continue;
17415 }
17416 j4array[0]
17417 = ((-1.5707963267949)
17418 + (x1425
17419 .value)
17420 + (((1.5707963267949)
17421 * (x1426
17422 .value))));
17423 sj4array[0] = IKsin(
17424 j4array
17425 [0]);
17426 cj4array[0] = IKcos(
17427 j4array
17428 [0]);
17429 if (j4array
17430 [0]
17431 > IKPI)
17432 {
17433 j4array[0]
17434 -= IK2PI;
17435 }
17436 else if (
17437 j4array
17438 [0]
17439 < -IKPI)
17440 {
17441 j4array[0]
17442 += IK2PI;
17443 }
17444 j4valid[0]
17445 = true;
17446 for (int ij4
17447 = 0;
17448 ij4
17449 < 1;
17450 ++ij4)
17451 {
17452 if (!j4valid
17453 [ij4])
17454 {
17455 continue;
17456 }
17457 _ij4[0]
17458 = ij4;
17459 _ij4[1]
17460 = -1;
17461 for (
17462 int iij4
17463 = ij4
17464 + 1;
17465 iij4
17466 < 1;
17467 ++iij4)
17468 {
17469 if (j4valid
17470 [iij4]
17471 && IKabs(
17472 cj4array
17473 [ij4]
17474 - cj4array
17475 [iij4])
17476 < IKFAST_SOLUTION_THRESH
17477 && IKabs(
17478 sj4array
17479 [ij4]
17480 - sj4array
17481 [iij4])
17482 < IKFAST_SOLUTION_THRESH)
17483 {
17484 j4valid
17485 [iij4]
17486 = false;
17487 _ij4[1]
17488 = iij4;
17489 break;
17490 }
17491 }
17492 j4 = j4array
17493 [ij4];
17494 cj4 = cj4array
17495 [ij4];
17496 sj4 = sj4array
17497 [ij4];
17498 {
17499 IkReal evalcond
17500 [3];
17501 IkReal
17502 x1427
17503 = IKsin(
17504 j4);
17505 IkReal
17506 x1428
17507 = IKcos(
17508 j4);
17509 IkReal
17510 x1429
17511 = (px
17512 * x1428);
17513 IkReal
17514 x1430
17515 = (py
17516 * x1427);
17517 evalcond
17518 [0]
17519 = ((((-1.0)
17520 * py
17521 * x1428))
17522 + ((px
17523 * x1427)));
17524 evalcond
17525 [1]
17526 = ((-1.51009803921569)
17527 + (((-1.0)
17528 * (1.32323529411765)
17529 * cj9))
17530 + (((3.92156862745098)
17531 * pp))
17532 + (((-1.0)
17533 * x1430))
17534 + (((-1.0)
17535 * x1429)));
17536 evalcond
17537 [2]
17538 = ((-0.2125)
17539 + (((1.1)
17540 * pz))
17541 + (((0.09)
17542 * x1430))
17543 + (((-1.0)
17544 * (1.0)
17545 * pp))
17546 + (((0.09)
17547 * x1429)));
17548 if (IKabs(
17549 evalcond
17550 [0])
17551 > IKFAST_EVALCOND_THRESH
17552 || IKabs(
17553 evalcond
17554 [1])
17555 > IKFAST_EVALCOND_THRESH
17556 || IKabs(
17557 evalcond
17558 [2])
17559 > IKFAST_EVALCOND_THRESH)
17560 {
17561 continue;
17562 }
17563 }
17564
17565 rotationfunction0(
17566 solutions);
17567 }
17568 }
17569 }
17570 }
17571 }
17572 } while (0);
17573 if (bgotonextstatement)
17574 {
17575 bool
17576 bgotonextstatement
17577 = true;
17578 do
17579 {
17580 evalcond[0]
17581 = ((-3.14159265358979)
17582 + (IKfmod(
17583 ((3.14159265358979)
17584 + (IKabs((
17585 (-3.14159265358979)
17586 + j8)))),
17587 6.28318530717959)));
17588 if (IKabs(evalcond
17589 [0])
17590 < 0.0000010000000000)
17591 {
17592 bgotonextstatement
17593 = false;
17594 {
17595 IkReal
17596 j4eval[3];
17597 sj6 = 0;
17598 cj6 = 1.0;
17599 j6 = 0;
17600 sj8 = 0;
17601 cj8 = -1.0;
17602 j8 = 3.14159265358979;
17603 IkReal x1431
17604 = (pp
17605 + (((-1.0)
17606 * (1.0)
17607 * (pz
17608 * pz))));
17609 IkReal x1432
17610 = ((13497.0)
17611 * cj9);
17612 IkReal x1433
17613 = ((40000.0)
17614 * pp);
17615 j4eval[0]
17616 = x1431;
17617 j4eval[1]
17618 = ((IKabs((
17619 (((15403.0)
17620 * px))
17621 + ((px
17622 * x1432))
17623 + (((-1.0)
17624 * px
17625 * x1433)))))
17626 + (IKabs((
17627 (((15403.0)
17628 * py))
17629 + ((py
17630 * x1432))
17631 + (((-1.0)
17632 * py
17633 * x1433))))));
17634 j4eval[2]
17635 = IKsign(
17636 x1431);
17637 if (IKabs(
17638 j4eval
17639 [0])
17640 < 0.0000010000000000
17641 || IKabs(
17642 j4eval
17643 [1])
17644 < 0.0000010000000000
17645 || IKabs(
17646 j4eval
17647 [2])
17648 < 0.0000010000000000)
17649 {
17650 {
17651 IkReal j4eval
17652 [3];
17653 sj6 = 0;
17654 cj6 = 1.0;
17655 j6 = 0;
17656 sj8 = 0;
17657 cj8 = -1.0;
17658 j8 = 3.14159265358979;
17659 IkReal
17660 x1434
17661 = pz
17662 * pz;
17663 IkReal
17664 x1435
17665 = ((80.0)
17666 * pp);
17667 IkReal
17668 x1436
17669 = ((88.0)
17670 * pz);
17671 j4eval[0]
17672 = (pp
17673 + (((-1.0)
17674 * x1434)));
17675 j4eval[1] = IKsign((
17676 (((9.0)
17677 * pp))
17678 + (((-9.0)
17679 * x1434))));
17680 j4eval[2]
17681 = ((IKabs((
17682 (((-1.0)
17683 * (17.0)
17684 * px))
17685 + ((px
17686 * x1436))
17687 + (((-1.0)
17688 * px
17689 * x1435)))))
17690 + (IKabs((
17691 ((py
17692 * x1436))
17693 + (((-1.0)
17694 * py
17695 * x1435))
17696 + (((-1.0)
17697 * (17.0)
17698 * py))))));
17699 if (IKabs(
17700 j4eval
17701 [0])
17702 < 0.0000010000000000
17703 || IKabs(
17704 j4eval
17705 [1])
17706 < 0.0000010000000000
17707 || IKabs(
17708 j4eval
17709 [2])
17710 < 0.0000010000000000)
17711 {
17712 {
17713 IkReal evalcond
17714 [5];
17715 bool
17716 bgotonextstatement
17717 = true;
17718 do
17719 {
17720 IkReal
17721 x1437
17722 = ((1.32323529411765)
17723 * cj9);
17724 IkReal
17725 x1438
17726 = ((3.92156862745098)
17727 * pp);
17728 evalcond
17729 [0]
17730 = ((IKabs(
17731 py))
17732 + (IKabs(
17733 px)));
17734 evalcond
17735 [1]
17736 = 0;
17737 evalcond
17738 [2]
17739 = ((-1.51009803921569)
17740 + x1438
17741 + (((-1.0)
17742 * x1437)));
17743 evalcond
17744 [3]
17745 = ((1.51009803921569)
17746 + (((-1.0)
17747 * x1438))
17748 + x1437);
17749 evalcond
17750 [4]
17751 = ((-0.2125)
17752 + (((1.1)
17753 * pz))
17754 + (((-1.0)
17755 * (1.0)
17756 * pp)));
17757 if (IKabs(
17758 evalcond
17759 [0])
17760 < 0.0000010000000000
17761 && IKabs(
17762 evalcond
17763 [1])
17764 < 0.0000010000000000
17765 && IKabs(
17766 evalcond
17767 [2])
17768 < 0.0000010000000000
17769 && IKabs(
17770 evalcond
17771 [3])
17772 < 0.0000010000000000
17773 && IKabs(
17774 evalcond
17775 [4])
17776 < 0.0000010000000000)
17777 {
17778 bgotonextstatement
17779 = false;
17780 {
17781 IkReal j4array
17782 [4],
17783 cj4array
17784 [4],
17785 sj4array
17786 [4];
17787 bool j4valid
17788 [4]
17789 = {false};
17790 _nj4
17791 = 4;
17792 j4array
17793 [0]
17794 = 0;
17795 sj4array
17796 [0]
17797 = IKsin(
17798 j4array
17799 [0]);
17800 cj4array
17801 [0]
17802 = IKcos(
17803 j4array
17804 [0]);
17805 j4array
17806 [1]
17807 = 1.5707963267949;
17808 sj4array
17809 [1]
17810 = IKsin(
17811 j4array
17812 [1]);
17813 cj4array
17814 [1]
17815 = IKcos(
17816 j4array
17817 [1]);
17818 j4array
17819 [2]
17820 = 3.14159265358979;
17821 sj4array
17822 [2]
17823 = IKsin(
17824 j4array
17825 [2]);
17826 cj4array
17827 [2]
17828 = IKcos(
17829 j4array
17830 [2]);
17831 j4array
17832 [3]
17833 = -1.5707963267949;
17834 sj4array
17835 [3]
17836 = IKsin(
17837 j4array
17838 [3]);
17839 cj4array
17840 [3]
17841 = IKcos(
17842 j4array
17843 [3]);
17844 if (j4array
17845 [0]
17846 > IKPI)
17847 {
17848 j4array
17849 [0]
17850 -= IK2PI;
17851 }
17852 else if (
17853 j4array
17854 [0]
17855 < -IKPI)
17856 {
17857 j4array
17858 [0]
17859 += IK2PI;
17860 }
17861 j4valid
17862 [0]
17863 = true;
17864 if (j4array
17865 [1]
17866 > IKPI)
17867 {
17868 j4array
17869 [1]
17870 -= IK2PI;
17871 }
17872 else if (
17873 j4array
17874 [1]
17875 < -IKPI)
17876 {
17877 j4array
17878 [1]
17879 += IK2PI;
17880 }
17881 j4valid
17882 [1]
17883 = true;
17884 if (j4array
17885 [2]
17886 > IKPI)
17887 {
17888 j4array
17889 [2]
17890 -= IK2PI;
17891 }
17892 else if (
17893 j4array
17894 [2]
17895 < -IKPI)
17896 {
17897 j4array
17898 [2]
17899 += IK2PI;
17900 }
17901 j4valid
17902 [2]
17903 = true;
17904 if (j4array
17905 [3]
17906 > IKPI)
17907 {
17908 j4array
17909 [3]
17910 -= IK2PI;
17911 }
17912 else if (
17913 j4array
17914 [3]
17915 < -IKPI)
17916 {
17917 j4array
17918 [3]
17919 += IK2PI;
17920 }
17921 j4valid
17922 [3]
17923 = true;
17924 for (
17925 int ij4
17926 = 0;
17927 ij4
17928 < 4;
17929 ++ij4)
17930 {
17931 if (!j4valid
17932 [ij4])
17933 {
17934 continue;
17935 }
17936 _ij4[0]
17937 = ij4;
17938 _ij4[1]
17939 = -1;
17940 for (
17941 int iij4
17942 = ij4
17943 + 1;
17944 iij4
17945 < 4;
17946 ++iij4)
17947 {
17948 if (j4valid
17949 [iij4]
17950 && IKabs(
17951 cj4array
17952 [ij4]
17953 - cj4array
17954 [iij4])
17955 < IKFAST_SOLUTION_THRESH
17956 && IKabs(
17957 sj4array
17958 [ij4]
17959 - sj4array
17960 [iij4])
17961 < IKFAST_SOLUTION_THRESH)
17962 {
17963 j4valid
17964 [iij4]
17965 = false;
17966 _ij4[1]
17967 = iij4;
17968 break;
17969 }
17970 }
17971 j4 = j4array
17972 [ij4];
17973 cj4 = cj4array
17974 [ij4];
17975 sj4 = sj4array
17976 [ij4];
17977
17978 rotationfunction0(
17979 solutions);
17980 }
17981 }
17982 }
17983 } while (
17984 0);
17985 if (bgotonextstatement)
17986 {
17987 bool
17988 bgotonextstatement
17989 = true;
17990 do
17991 {
17992 if (1)
17993 {
17994 bgotonextstatement
17995 = false;
17996 continue; // branch miss [j4]
17997 }
17998 } while (
17999 0);
18000 if (bgotonextstatement)
18001 {
18002 }
18003 }
18004 }
18005 }
18006 else
18007 {
18008 {
18009 IkReal j4array
18010 [1],
18011 cj4array
18012 [1],
18013 sj4array
18014 [1];
18015 bool j4valid
18016 [1]
18017 = {false};
18018 _nj4
18019 = 1;
18020 IkReal
18021 x1439
18022 = ((100.0)
18023 * pp);
18024 IkReal
18025 x1440
18026 = ((110.0)
18027 * pz);
18028 CheckValue<IkReal> x1441 = IKPowWithIntegerCheck(
18029 IKsign((
18030 (((-1.0)
18031 * (9.0)
18032 * (pz
18033 * pz)))
18034 + (((9.0)
18035 * pp)))),
18036 -1);
18037 if (!x1441
18038 .valid)
18039 {
18040 continue;
18041 }
18042 CheckValue<IkReal> x1442 = IKatan2WithCheck(
18043 IkReal((
18044 ((py
18045 * x1440))
18046 + (((-1.0)
18047 * (21.25)
18048 * py))
18049 + (((-1.0)
18050 * py
18051 * x1439)))),
18052 (((px
18053 * x1440))
18054 + (((-1.0)
18055 * (21.25)
18056 * px))
18057 + (((-1.0)
18058 * px
18059 * x1439))),
18060 IKFAST_ATAN2_MAGTHRESH);
18061 if (!x1442
18062 .valid)
18063 {
18064 continue;
18065 }
18066 j4array
18067 [0]
18068 = ((-1.5707963267949)
18069 + (((1.5707963267949)
18070 * (x1441
18071 .value)))
18072 + (x1442
18073 .value));
18074 sj4array
18075 [0]
18076 = IKsin(
18077 j4array
18078 [0]);
18079 cj4array
18080 [0]
18081 = IKcos(
18082 j4array
18083 [0]);
18084 if (j4array
18085 [0]
18086 > IKPI)
18087 {
18088 j4array
18089 [0]
18090 -= IK2PI;
18091 }
18092 else if (
18093 j4array
18094 [0]
18095 < -IKPI)
18096 {
18097 j4array
18098 [0]
18099 += IK2PI;
18100 }
18101 j4valid
18102 [0]
18103 = true;
18104 for (
18105 int ij4
18106 = 0;
18107 ij4
18108 < 1;
18109 ++ij4)
18110 {
18111 if (!j4valid
18112 [ij4])
18113 {
18114 continue;
18115 }
18116 _ij4[0]
18117 = ij4;
18118 _ij4[1]
18119 = -1;
18120 for (
18121 int iij4
18122 = ij4
18123 + 1;
18124 iij4
18125 < 1;
18126 ++iij4)
18127 {
18128 if (j4valid
18129 [iij4]
18130 && IKabs(
18131 cj4array
18132 [ij4]
18133 - cj4array
18134 [iij4])
18135 < IKFAST_SOLUTION_THRESH
18136 && IKabs(
18137 sj4array
18138 [ij4]
18139 - sj4array
18140 [iij4])
18141 < IKFAST_SOLUTION_THRESH)
18142 {
18143 j4valid
18144 [iij4]
18145 = false;
18146 _ij4[1]
18147 = iij4;
18148 break;
18149 }
18150 }
18151 j4 = j4array
18152 [ij4];
18153 cj4 = cj4array
18154 [ij4];
18155 sj4 = sj4array
18156 [ij4];
18157 {
18158 IkReal evalcond
18159 [3];
18160 IkReal
18161 x1443
18162 = IKsin(
18163 j4);
18164 IkReal
18165 x1444
18166 = IKcos(
18167 j4);
18168 IkReal
18169 x1445
18170 = (px
18171 * x1444);
18172 IkReal
18173 x1446
18174 = (py
18175 * x1443);
18176 evalcond
18177 [0]
18178 = ((((-1.0)
18179 * py
18180 * x1444))
18181 + ((px
18182 * x1443)));
18183 evalcond
18184 [1]
18185 = ((-1.51009803921569)
18186 + (((-1.0)
18187 * (1.32323529411765)
18188 * cj9))
18189 + (((3.92156862745098)
18190 * pp))
18191 + x1446
18192 + x1445);
18193 evalcond
18194 [2]
18195 = ((-0.2125)
18196 + (((1.1)
18197 * pz))
18198 + (((-0.09)
18199 * x1446))
18200 + (((-1.0)
18201 * (1.0)
18202 * pp))
18203 + (((-0.09)
18204 * x1445)));
18205 if (IKabs(
18206 evalcond
18207 [0])
18208 > IKFAST_EVALCOND_THRESH
18209 || IKabs(
18210 evalcond
18211 [1])
18212 > IKFAST_EVALCOND_THRESH
18213 || IKabs(
18214 evalcond
18215 [2])
18216 > IKFAST_EVALCOND_THRESH)
18217 {
18218 continue;
18219 }
18220 }
18221
18222 rotationfunction0(
18223 solutions);
18224 }
18225 }
18226 }
18227 }
18228 }
18229 else
18230 {
18231 {
18232 IkReal j4array
18233 [1],
18234 cj4array
18235 [1],
18236 sj4array
18237 [1];
18238 bool j4valid
18239 [1]
18240 = {false};
18241 _nj4 = 1;
18242 IkReal
18243 x1447
18244 = ((1.32323529411765)
18245 * cj9);
18246 IkReal
18247 x1448
18248 = ((3.92156862745098)
18249 * pp);
18250 CheckValue<IkReal> x1449 = IKPowWithIntegerCheck(
18251 IKsign((
18252 pp
18253 + (((-1.0)
18254 * (1.0)
18255 * (pz
18256 * pz))))),
18257 -1);
18258 if (!x1449
18259 .valid)
18260 {
18261 continue;
18262 }
18263 CheckValue<IkReal> x1450 = IKatan2WithCheck(
18264 IkReal((
18265 (((-1.0)
18266 * py
18267 * x1448))
18268 + (((1.51009803921569)
18269 * py))
18270 + ((py
18271 * x1447)))),
18272 ((((-1.0)
18273 * px
18274 * x1448))
18275 + (((1.51009803921569)
18276 * px))
18277 + ((px
18278 * x1447))),
18279 IKFAST_ATAN2_MAGTHRESH);
18280 if (!x1450
18281 .valid)
18282 {
18283 continue;
18284 }
18285 j4array[0]
18286 = ((-1.5707963267949)
18287 + (((1.5707963267949)
18288 * (x1449
18289 .value)))
18290 + (x1450
18291 .value));
18292 sj4array[0] = IKsin(
18293 j4array
18294 [0]);
18295 cj4array[0] = IKcos(
18296 j4array
18297 [0]);
18298 if (j4array
18299 [0]
18300 > IKPI)
18301 {
18302 j4array
18303 [0]
18304 -= IK2PI;
18305 }
18306 else if (
18307 j4array
18308 [0]
18309 < -IKPI)
18310 {
18311 j4array
18312 [0]
18313 += IK2PI;
18314 }
18315 j4valid[0]
18316 = true;
18317 for (
18318 int ij4
18319 = 0;
18320 ij4
18321 < 1;
18322 ++ij4)
18323 {
18324 if (!j4valid
18325 [ij4])
18326 {
18327 continue;
18328 }
18329 _ij4[0]
18330 = ij4;
18331 _ij4[1]
18332 = -1;
18333 for (
18334 int iij4
18335 = ij4
18336 + 1;
18337 iij4
18338 < 1;
18339 ++iij4)
18340 {
18341 if (j4valid
18342 [iij4]
18343 && IKabs(
18344 cj4array
18345 [ij4]
18346 - cj4array
18347 [iij4])
18348 < IKFAST_SOLUTION_THRESH
18349 && IKabs(
18350 sj4array
18351 [ij4]
18352 - sj4array
18353 [iij4])
18354 < IKFAST_SOLUTION_THRESH)
18355 {
18356 j4valid
18357 [iij4]
18358 = false;
18359 _ij4[1]
18360 = iij4;
18361 break;
18362 }
18363 }
18364 j4 = j4array
18365 [ij4];
18366 cj4 = cj4array
18367 [ij4];
18368 sj4 = sj4array
18369 [ij4];
18370 {
18371 IkReal evalcond
18372 [3];
18373 IkReal
18374 x1451
18375 = IKsin(
18376 j4);
18377 IkReal
18378 x1452
18379 = IKcos(
18380 j4);
18381 IkReal
18382 x1453
18383 = (px
18384 * x1452);
18385 IkReal
18386 x1454
18387 = (py
18388 * x1451);
18389 evalcond
18390 [0]
18391 = (((px
18392 * x1451))
18393 + (((-1.0)
18394 * py
18395 * x1452)));
18396 evalcond
18397 [1]
18398 = ((-1.51009803921569)
18399 + (((-1.0)
18400 * (1.32323529411765)
18401 * cj9))
18402 + (((3.92156862745098)
18403 * pp))
18404 + x1453
18405 + x1454);
18406 evalcond
18407 [2]
18408 = ((-0.2125)
18409 + (((-0.09)
18410 * x1453))
18411 + (((1.1)
18412 * pz))
18413 + (((-1.0)
18414 * (1.0)
18415 * pp))
18416 + (((-0.09)
18417 * x1454)));
18418 if (IKabs(
18419 evalcond
18420 [0])
18421 > IKFAST_EVALCOND_THRESH
18422 || IKabs(
18423 evalcond
18424 [1])
18425 > IKFAST_EVALCOND_THRESH
18426 || IKabs(
18427 evalcond
18428 [2])
18429 > IKFAST_EVALCOND_THRESH)
18430 {
18431 continue;
18432 }
18433 }
18434
18435 rotationfunction0(
18436 solutions);
18437 }
18438 }
18439 }
18440 }
18441 }
18442 } while (0);
18443 if (bgotonextstatement)
18444 {
18445 bool
18446 bgotonextstatement
18447 = true;
18448 do
18449 {
18450 evalcond[0]
18451 = ((-3.14159265358979)
18452 + (IKfmod(
18453 ((3.14159265358979)
18454 + (IKabs((
18455 (-1.5707963267949)
18456 + j8)))),
18457 6.28318530717959)));
18458 if (IKabs(
18459 evalcond
18460 [0])
18461 < 0.0000010000000000)
18462 {
18463 bgotonextstatement
18464 = false;
18465 {
18466 IkReal
18467 j4eval
18468 [3];
18469 sj6 = 0;
18470 cj6 = 1.0;
18471 j6 = 0;
18472 sj8 = 1.0;
18473 cj8 = 0;
18474 j8 = 1.5707963267949;
18475 IkReal x1455
18476 = (pp
18477 + (((-1.0)
18478 * (1.0)
18479 * (pz
18480 * pz))));
18481 IkReal x1456
18482 = ((13497.0)
18483 * cj9);
18484 IkReal x1457
18485 = ((40000.0)
18486 * pp);
18487 j4eval[0]
18488 = x1455;
18489 j4eval[1]
18490 = ((IKabs((
18491 (((15403.0)
18492 * px))
18493 + (((-1.0)
18494 * px
18495 * x1457))
18496 + ((px
18497 * x1456)))))
18498 + (IKabs((
18499 (((-1.0)
18500 * py
18501 * x1456))
18502 + ((py
18503 * x1457))
18504 + (((-1.0)
18505 * (15403.0)
18506 * py))))));
18507 j4eval[2]
18508 = IKsign(
18509 x1455);
18510 if (IKabs(
18511 j4eval
18512 [0])
18513 < 0.0000010000000000
18514 || IKabs(
18515 j4eval
18516 [1])
18517 < 0.0000010000000000
18518 || IKabs(
18519 j4eval
18520 [2])
18521 < 0.0000010000000000)
18522 {
18523 {
18524 IkReal j4eval
18525 [3];
18526 sj6 = 0;
18527 cj6 = 1.0;
18528 j6 = 0;
18529 sj8 = 1.0;
18530 cj8 = 0;
18531 j8 = 1.5707963267949;
18532 IkReal
18533 x1458
18534 = pz
18535 * pz;
18536 IkReal
18537 x1459
18538 = ((80.0)
18539 * pp);
18540 IkReal
18541 x1460
18542 = ((88.0)
18543 * pz);
18544 j4eval
18545 [0]
18546 = (pp
18547 + (((-1.0)
18548 * x1458)));
18549 j4eval[1] = IKsign((
18550 (((-9.0)
18551 * x1458))
18552 + (((9.0)
18553 * pp))));
18554 j4eval
18555 [2]
18556 = ((IKabs((
18557 (((-1.0)
18558 * py
18559 * x1460))
18560 + ((py
18561 * x1459))
18562 + (((17.0)
18563 * py)))))
18564 + (IKabs((
18565 (((-1.0)
18566 * (17.0)
18567 * px))
18568 + (((-1.0)
18569 * px
18570 * x1459))
18571 + ((px
18572 * x1460))))));
18573 if (IKabs(
18574 j4eval
18575 [0])
18576 < 0.0000010000000000
18577 || IKabs(
18578 j4eval
18579 [1])
18580 < 0.0000010000000000
18581 || IKabs(
18582 j4eval
18583 [2])
18584 < 0.0000010000000000)
18585 {
18586 {
18587 IkReal evalcond
18588 [5];
18589 bool
18590 bgotonextstatement
18591 = true;
18592 do
18593 {
18594 IkReal
18595 x1461
18596 = ((-1.51009803921569)
18597 + (((-1.0)
18598 * (1.32323529411765)
18599 * cj9))
18600 + (((3.92156862745098)
18601 * pp)));
18602 evalcond
18603 [0]
18604 = ((IKabs(
18605 py))
18606 + (IKabs(
18607 px)));
18608 evalcond
18609 [1]
18610 = x1461;
18611 evalcond
18612 [2]
18613 = 0;
18614 evalcond
18615 [3]
18616 = x1461;
18617 evalcond
18618 [4]
18619 = ((-0.2125)
18620 + (((1.1)
18621 * pz))
18622 + (((-1.0)
18623 * (1.0)
18624 * pp)));
18625 if (IKabs(
18626 evalcond
18627 [0])
18628 < 0.0000010000000000
18629 && IKabs(
18630 evalcond
18631 [1])
18632 < 0.0000010000000000
18633 && IKabs(
18634 evalcond
18635 [2])
18636 < 0.0000010000000000
18637 && IKabs(
18638 evalcond
18639 [3])
18640 < 0.0000010000000000
18641 && IKabs(
18642 evalcond
18643 [4])
18644 < 0.0000010000000000)
18645 {
18646 bgotonextstatement
18647 = false;
18648 {
18649 IkReal j4array
18650 [4],
18651 cj4array
18652 [4],
18653 sj4array
18654 [4];
18655 bool j4valid
18656 [4]
18657 = {false};
18658 _nj4
18659 = 4;
18660 j4array
18661 [0]
18662 = 0;
18663 sj4array
18664 [0]
18665 = IKsin(
18666 j4array
18667 [0]);
18668 cj4array
18669 [0]
18670 = IKcos(
18671 j4array
18672 [0]);
18673 j4array
18674 [1]
18675 = 1.5707963267949;
18676 sj4array
18677 [1]
18678 = IKsin(
18679 j4array
18680 [1]);
18681 cj4array
18682 [1]
18683 = IKcos(
18684 j4array
18685 [1]);
18686 j4array
18687 [2]
18688 = 3.14159265358979;
18689 sj4array
18690 [2]
18691 = IKsin(
18692 j4array
18693 [2]);
18694 cj4array
18695 [2]
18696 = IKcos(
18697 j4array
18698 [2]);
18699 j4array
18700 [3]
18701 = -1.5707963267949;
18702 sj4array
18703 [3]
18704 = IKsin(
18705 j4array
18706 [3]);
18707 cj4array
18708 [3]
18709 = IKcos(
18710 j4array
18711 [3]);
18712 if (j4array
18713 [0]
18714 > IKPI)
18715 {
18716 j4array
18717 [0]
18718 -= IK2PI;
18719 }
18720 else if (
18721 j4array
18722 [0]
18723 < -IKPI)
18724 {
18725 j4array
18726 [0]
18727 += IK2PI;
18728 }
18729 j4valid
18730 [0]
18731 = true;
18732 if (j4array
18733 [1]
18734 > IKPI)
18735 {
18736 j4array
18737 [1]
18738 -= IK2PI;
18739 }
18740 else if (
18741 j4array
18742 [1]
18743 < -IKPI)
18744 {
18745 j4array
18746 [1]
18747 += IK2PI;
18748 }
18749 j4valid
18750 [1]
18751 = true;
18752 if (j4array
18753 [2]
18754 > IKPI)
18755 {
18756 j4array
18757 [2]
18758 -= IK2PI;
18759 }
18760 else if (
18761 j4array
18762 [2]
18763 < -IKPI)
18764 {
18765 j4array
18766 [2]
18767 += IK2PI;
18768 }
18769 j4valid
18770 [2]
18771 = true;
18772 if (j4array
18773 [3]
18774 > IKPI)
18775 {
18776 j4array
18777 [3]
18778 -= IK2PI;
18779 }
18780 else if (
18781 j4array
18782 [3]
18783 < -IKPI)
18784 {
18785 j4array
18786 [3]
18787 += IK2PI;
18788 }
18789 j4valid
18790 [3]
18791 = true;
18792 for (
18793 int ij4
18794 = 0;
18795 ij4
18796 < 4;
18797 ++ij4)
18798 {
18799 if (!j4valid
18800 [ij4])
18801 {
18802 continue;
18803 }
18804 _ij4[0]
18805 = ij4;
18806 _ij4[1]
18807 = -1;
18808 for (
18809 int iij4
18810 = ij4
18811 + 1;
18812 iij4
18813 < 4;
18814 ++iij4)
18815 {
18816 if (j4valid
18817 [iij4]
18818 && IKabs(
18819 cj4array
18820 [ij4]
18821 - cj4array
18822 [iij4])
18823 < IKFAST_SOLUTION_THRESH
18824 && IKabs(
18825 sj4array
18826 [ij4]
18827 - sj4array
18828 [iij4])
18829 < IKFAST_SOLUTION_THRESH)
18830 {
18831 j4valid
18832 [iij4]
18833 = false;
18834 _ij4[1]
18835 = iij4;
18836 break;
18837 }
18838 }
18839 j4 = j4array
18840 [ij4];
18841 cj4 = cj4array
18842 [ij4];
18843 sj4 = sj4array
18844 [ij4];
18845
18846 rotationfunction0(
18847 solutions);
18848 }
18849 }
18850 }
18851 } while (
18852 0);
18853 if (bgotonextstatement)
18854 {
18855 bool
18856 bgotonextstatement
18857 = true;
18858 do
18859 {
18860 if (1)
18861 {
18862 bgotonextstatement
18863 = false;
18864 continue; // branch miss [j4]
18865 }
18866 } while (
18867 0);
18868 if (bgotonextstatement)
18869 {
18870 }
18871 }
18872 }
18873 }
18874 else
18875 {
18876 {
18877 IkReal j4array
18878 [1],
18879 cj4array
18880 [1],
18881 sj4array
18882 [1];
18883 bool j4valid
18884 [1]
18885 = {false};
18886 _nj4
18887 = 1;
18888 IkReal
18889 x1462
18890 = ((100.0)
18891 * pp);
18892 IkReal
18893 x1463
18894 = ((110.0)
18895 * pz);
18896 CheckValue<IkReal> x1464 = IKPowWithIntegerCheck(
18897 IKsign((
18898 (((-1.0)
18899 * (9.0)
18900 * (pz
18901 * pz)))
18902 + (((9.0)
18903 * pp)))),
18904 -1);
18905 if (!x1464
18906 .valid)
18907 {
18908 continue;
18909 }
18910 CheckValue<IkReal> x1465 = IKatan2WithCheck(
18911 IkReal((
18912 (((-1.0)
18913 * px
18914 * x1462))
18915 + (((-1.0)
18916 * (21.25)
18917 * px))
18918 + ((px
18919 * x1463)))),
18920 (((py
18921 * x1462))
18922 + (((21.25)
18923 * py))
18924 + (((-1.0)
18925 * py
18926 * x1463))),
18927 IKFAST_ATAN2_MAGTHRESH);
18928 if (!x1465
18929 .valid)
18930 {
18931 continue;
18932 }
18933 j4array
18934 [0]
18935 = ((-1.5707963267949)
18936 + (((1.5707963267949)
18937 * (x1464
18938 .value)))
18939 + (x1465
18940 .value));
18941 sj4array
18942 [0]
18943 = IKsin(
18944 j4array
18945 [0]);
18946 cj4array
18947 [0]
18948 = IKcos(
18949 j4array
18950 [0]);
18951 if (j4array
18952 [0]
18953 > IKPI)
18954 {
18955 j4array
18956 [0]
18957 -= IK2PI;
18958 }
18959 else if (
18960 j4array
18961 [0]
18962 < -IKPI)
18963 {
18964 j4array
18965 [0]
18966 += IK2PI;
18967 }
18968 j4valid
18969 [0]
18970 = true;
18971 for (
18972 int ij4
18973 = 0;
18974 ij4
18975 < 1;
18976 ++ij4)
18977 {
18978 if (!j4valid
18979 [ij4])
18980 {
18981 continue;
18982 }
18983 _ij4[0]
18984 = ij4;
18985 _ij4[1]
18986 = -1;
18987 for (
18988 int iij4
18989 = ij4
18990 + 1;
18991 iij4
18992 < 1;
18993 ++iij4)
18994 {
18995 if (j4valid
18996 [iij4]
18997 && IKabs(
18998 cj4array
18999 [ij4]
19000 - cj4array
19001 [iij4])
19002 < IKFAST_SOLUTION_THRESH
19003 && IKabs(
19004 sj4array
19005 [ij4]
19006 - sj4array
19007 [iij4])
19008 < IKFAST_SOLUTION_THRESH)
19009 {
19010 j4valid
19011 [iij4]
19012 = false;
19013 _ij4[1]
19014 = iij4;
19015 break;
19016 }
19017 }
19018 j4 = j4array
19019 [ij4];
19020 cj4 = cj4array
19021 [ij4];
19022 sj4 = sj4array
19023 [ij4];
19024 {
19025 IkReal evalcond
19026 [3];
19027 IkReal
19028 x1466
19029 = IKcos(
19030 j4);
19031 IkReal
19032 x1467
19033 = IKsin(
19034 j4);
19035 IkReal
19036 x1468
19037 = (px
19038 * x1467);
19039 IkReal
19040 x1469
19041 = (py
19042 * x1466);
19043 evalcond
19044 [0]
19045 = (((py
19046 * x1467))
19047 + ((px
19048 * x1466)));
19049 evalcond
19050 [1]
19051 = ((-1.51009803921569)
19052 + (((-1.0)
19053 * x1469))
19054 + (((-1.0)
19055 * (1.32323529411765)
19056 * cj9))
19057 + (((3.92156862745098)
19058 * pp))
19059 + x1468);
19060 evalcond
19061 [2]
19062 = ((-0.2125)
19063 + (((1.1)
19064 * pz))
19065 + (((0.09)
19066 * x1469))
19067 + (((-0.09)
19068 * x1468))
19069 + (((-1.0)
19070 * (1.0)
19071 * pp)));
19072 if (IKabs(
19073 evalcond
19074 [0])
19075 > IKFAST_EVALCOND_THRESH
19076 || IKabs(
19077 evalcond
19078 [1])
19079 > IKFAST_EVALCOND_THRESH
19080 || IKabs(
19081 evalcond
19082 [2])
19083 > IKFAST_EVALCOND_THRESH)
19084 {
19085 continue;
19086 }
19087 }
19088
19089 rotationfunction0(
19090 solutions);
19091 }
19092 }
19093 }
19094 }
19095 }
19096 else
19097 {
19098 {
19099 IkReal j4array
19100 [1],
19101 cj4array
19102 [1],
19103 sj4array
19104 [1];
19105 bool j4valid
19106 [1]
19107 = {false};
19108 _nj4
19109 = 1;
19110 IkReal
19111 x1470
19112 = ((1.32323529411765)
19113 * cj9);
19114 IkReal
19115 x1471
19116 = ((3.92156862745098)
19117 * pp);
19118 CheckValue<IkReal> x1472 = IKPowWithIntegerCheck(
19119 IKsign((
19120 pp
19121 + (((-1.0)
19122 * (1.0)
19123 * (pz
19124 * pz))))),
19125 -1);
19126 if (!x1472
19127 .valid)
19128 {
19129 continue;
19130 }
19131 CheckValue<IkReal> x1473 = IKatan2WithCheck(
19132 IkReal((
19133 ((px
19134 * x1470))
19135 + (((-1.0)
19136 * px
19137 * x1471))
19138 + (((1.51009803921569)
19139 * px)))),
19140 (((py
19141 * x1471))
19142 + (((-1.0)
19143 * (1.51009803921569)
19144 * py))
19145 + (((-1.0)
19146 * py
19147 * x1470))),
19148 IKFAST_ATAN2_MAGTHRESH);
19149 if (!x1473
19150 .valid)
19151 {
19152 continue;
19153 }
19154 j4array
19155 [0]
19156 = ((-1.5707963267949)
19157 + (((1.5707963267949)
19158 * (x1472
19159 .value)))
19160 + (x1473
19161 .value));
19162 sj4array
19163 [0]
19164 = IKsin(
19165 j4array
19166 [0]);
19167 cj4array
19168 [0]
19169 = IKcos(
19170 j4array
19171 [0]);
19172 if (j4array
19173 [0]
19174 > IKPI)
19175 {
19176 j4array
19177 [0]
19178 -= IK2PI;
19179 }
19180 else if (
19181 j4array
19182 [0]
19183 < -IKPI)
19184 {
19185 j4array
19186 [0]
19187 += IK2PI;
19188 }
19189 j4valid
19190 [0]
19191 = true;
19192 for (
19193 int ij4
19194 = 0;
19195 ij4
19196 < 1;
19197 ++ij4)
19198 {
19199 if (!j4valid
19200 [ij4])
19201 {
19202 continue;
19203 }
19204 _ij4[0]
19205 = ij4;
19206 _ij4[1]
19207 = -1;
19208 for (
19209 int iij4
19210 = ij4
19211 + 1;
19212 iij4
19213 < 1;
19214 ++iij4)
19215 {
19216 if (j4valid
19217 [iij4]
19218 && IKabs(
19219 cj4array
19220 [ij4]
19221 - cj4array
19222 [iij4])
19223 < IKFAST_SOLUTION_THRESH
19224 && IKabs(
19225 sj4array
19226 [ij4]
19227 - sj4array
19228 [iij4])
19229 < IKFAST_SOLUTION_THRESH)
19230 {
19231 j4valid
19232 [iij4]
19233 = false;
19234 _ij4[1]
19235 = iij4;
19236 break;
19237 }
19238 }
19239 j4 = j4array
19240 [ij4];
19241 cj4 = cj4array
19242 [ij4];
19243 sj4 = sj4array
19244 [ij4];
19245 {
19246 IkReal evalcond
19247 [3];
19248 IkReal
19249 x1474
19250 = IKcos(
19251 j4);
19252 IkReal
19253 x1475
19254 = IKsin(
19255 j4);
19256 IkReal
19257 x1476
19258 = (px
19259 * x1475);
19260 IkReal
19261 x1477
19262 = (py
19263 * x1474);
19264 evalcond
19265 [0]
19266 = (((px
19267 * x1474))
19268 + ((py
19269 * x1475)));
19270 evalcond
19271 [1]
19272 = ((-1.51009803921569)
19273 + (((-1.0)
19274 * (1.32323529411765)
19275 * cj9))
19276 + (((3.92156862745098)
19277 * pp))
19278 + (((-1.0)
19279 * x1477))
19280 + x1476);
19281 evalcond
19282 [2]
19283 = ((-0.2125)
19284 + (((-0.09)
19285 * x1476))
19286 + (((0.09)
19287 * x1477))
19288 + (((1.1)
19289 * pz))
19290 + (((-1.0)
19291 * (1.0)
19292 * pp)));
19293 if (IKabs(
19294 evalcond
19295 [0])
19296 > IKFAST_EVALCOND_THRESH
19297 || IKabs(
19298 evalcond
19299 [1])
19300 > IKFAST_EVALCOND_THRESH
19301 || IKabs(
19302 evalcond
19303 [2])
19304 > IKFAST_EVALCOND_THRESH)
19305 {
19306 continue;
19307 }
19308 }
19309
19310 rotationfunction0(
19311 solutions);
19312 }
19313 }
19314 }
19315 }
19316 }
19317 } while (0);
19318 if (bgotonextstatement)
19319 {
19320 bool
19321 bgotonextstatement
19322 = true;
19323 do
19324 {
19325 evalcond[0]
19326 = ((-3.14159265358979)
19327 + (IKfmod(
19328 ((3.14159265358979)
19329 + (IKabs((
19330 (1.5707963267949)
19331 + j8)))),
19332 6.28318530717959)));
19333 if (IKabs(
19334 evalcond
19335 [0])
19336 < 0.0000010000000000)
19337 {
19338 bgotonextstatement
19339 = false;
19340 {
19341 IkReal j4eval
19342 [3];
19343 sj6 = 0;
19344 cj6 = 1.0;
19345 j6 = 0;
19346 sj8 = -1.0;
19347 cj8 = 0;
19348 j8 = -1.5707963267949;
19349 IkReal
19350 x1478
19351 = (pp
19352 + (((-1.0)
19353 * (1.0)
19354 * (pz
19355 * pz))));
19356 IkReal
19357 x1479
19358 = ((13497.0)
19359 * cj9);
19360 IkReal
19361 x1480
19362 = ((40000.0)
19363 * pp);
19364 j4eval[0]
19365 = x1478;
19366 j4eval[1]
19367 = ((IKabs((
19368 (((-1.0)
19369 * py
19370 * x1480))
19371 + (((15403.0)
19372 * py))
19373 + ((py
19374 * x1479)))))
19375 + (IKabs((
19376 (((-1.0)
19377 * px
19378 * x1479))
19379 + ((px
19380 * x1480))
19381 + (((-1.0)
19382 * (15403.0)
19383 * px))))));
19384 j4eval[2]
19385 = IKsign(
19386 x1478);
19387 if (IKabs(
19388 j4eval
19389 [0])
19390 < 0.0000010000000000
19391 || IKabs(
19392 j4eval
19393 [1])
19394 < 0.0000010000000000
19395 || IKabs(
19396 j4eval
19397 [2])
19398 < 0.0000010000000000)
19399 {
19400 {
19401 IkReal j4eval
19402 [3];
19403 sj6 = 0;
19404 cj6 = 1.0;
19405 j6 = 0;
19406 sj8 = -1.0;
19407 cj8 = 0;
19408 j8 = -1.5707963267949;
19409 IkReal
19410 x1481
19411 = pz
19412 * pz;
19413 IkReal
19414 x1482
19415 = ((80.0)
19416 * pp);
19417 IkReal
19418 x1483
19419 = ((88.0)
19420 * pz);
19421 j4eval
19422 [0]
19423 = ((((-1.0)
19424 * x1481))
19425 + pp);
19426 j4eval
19427 [1]
19428 = ((IKabs((
19429 (((-1.0)
19430 * py
19431 * x1482))
19432 + ((py
19433 * x1483))
19434 + (((-1.0)
19435 * (17.0)
19436 * py)))))
19437 + (IKabs((
19438 (((17.0)
19439 * px))
19440 + ((px
19441 * x1482))
19442 + (((-1.0)
19443 * px
19444 * x1483))))));
19445 j4eval[2] = IKsign((
19446 (((-9.0)
19447 * x1481))
19448 + (((9.0)
19449 * pp))));
19450 if (IKabs(
19451 j4eval
19452 [0])
19453 < 0.0000010000000000
19454 || IKabs(
19455 j4eval
19456 [1])
19457 < 0.0000010000000000
19458 || IKabs(
19459 j4eval
19460 [2])
19461 < 0.0000010000000000)
19462 {
19463 {
19464 IkReal evalcond
19465 [5];
19466 bool
19467 bgotonextstatement
19468 = true;
19469 do
19470 {
19471 IkReal
19472 x1484
19473 = ((1.32323529411765)
19474 * cj9);
19475 IkReal
19476 x1485
19477 = ((3.92156862745098)
19478 * pp);
19479 evalcond
19480 [0]
19481 = ((IKabs(
19482 py))
19483 + (IKabs(
19484 px)));
19485 evalcond
19486 [1]
19487 = ((1.51009803921569)
19488 + (((-1.0)
19489 * x1485))
19490 + x1484);
19491 evalcond
19492 [2]
19493 = 0;
19494 evalcond
19495 [3]
19496 = ((-1.51009803921569)
19497 + x1485
19498 + (((-1.0)
19499 * x1484)));
19500 evalcond
19501 [4]
19502 = ((-0.2125)
19503 + (((1.1)
19504 * pz))
19505 + (((-1.0)
19506 * (1.0)
19507 * pp)));
19508 if (IKabs(
19509 evalcond
19510 [0])
19511 < 0.0000010000000000
19512 && IKabs(
19513 evalcond
19514 [1])
19515 < 0.0000010000000000
19516 && IKabs(
19517 evalcond
19518 [2])
19519 < 0.0000010000000000
19520 && IKabs(
19521 evalcond
19522 [3])
19523 < 0.0000010000000000
19524 && IKabs(
19525 evalcond
19526 [4])
19527 < 0.0000010000000000)
19528 {
19529 bgotonextstatement
19530 = false;
19531 {
19532 IkReal j4array
19533 [4],
19534 cj4array
19535 [4],
19536 sj4array
19537 [4];
19538 bool j4valid
19539 [4]
19540 = {false};
19541 _nj4
19542 = 4;
19543 j4array
19544 [0]
19545 = 0;
19546 sj4array
19547 [0]
19548 = IKsin(
19549 j4array
19550 [0]);
19551 cj4array
19552 [0]
19553 = IKcos(
19554 j4array
19555 [0]);
19556 j4array
19557 [1]
19558 = 1.5707963267949;
19559 sj4array
19560 [1]
19561 = IKsin(
19562 j4array
19563 [1]);
19564 cj4array
19565 [1]
19566 = IKcos(
19567 j4array
19568 [1]);
19569 j4array
19570 [2]
19571 = 3.14159265358979;
19572 sj4array
19573 [2]
19574 = IKsin(
19575 j4array
19576 [2]);
19577 cj4array
19578 [2]
19579 = IKcos(
19580 j4array
19581 [2]);
19582 j4array
19583 [3]
19584 = -1.5707963267949;
19585 sj4array
19586 [3]
19587 = IKsin(
19588 j4array
19589 [3]);
19590 cj4array
19591 [3]
19592 = IKcos(
19593 j4array
19594 [3]);
19595 if (j4array
19596 [0]
19597 > IKPI)
19598 {
19599 j4array
19600 [0]
19601 -= IK2PI;
19602 }
19603 else if (
19604 j4array
19605 [0]
19606 < -IKPI)
19607 {
19608 j4array
19609 [0]
19610 += IK2PI;
19611 }
19612 j4valid
19613 [0]
19614 = true;
19615 if (j4array
19616 [1]
19617 > IKPI)
19618 {
19619 j4array
19620 [1]
19621 -= IK2PI;
19622 }
19623 else if (
19624 j4array
19625 [1]
19626 < -IKPI)
19627 {
19628 j4array
19629 [1]
19630 += IK2PI;
19631 }
19632 j4valid
19633 [1]
19634 = true;
19635 if (j4array
19636 [2]
19637 > IKPI)
19638 {
19639 j4array
19640 [2]
19641 -= IK2PI;
19642 }
19643 else if (
19644 j4array
19645 [2]
19646 < -IKPI)
19647 {
19648 j4array
19649 [2]
19650 += IK2PI;
19651 }
19652 j4valid
19653 [2]
19654 = true;
19655 if (j4array
19656 [3]
19657 > IKPI)
19658 {
19659 j4array
19660 [3]
19661 -= IK2PI;
19662 }
19663 else if (
19664 j4array
19665 [3]
19666 < -IKPI)
19667 {
19668 j4array
19669 [3]
19670 += IK2PI;
19671 }
19672 j4valid
19673 [3]
19674 = true;
19675 for (
19676 int ij4
19677 = 0;
19678 ij4
19679 < 4;
19680 ++ij4)
19681 {
19682 if (!j4valid
19683 [ij4])
19684 {
19685 continue;
19686 }
19687 _ij4[0]
19688 = ij4;
19689 _ij4[1]
19690 = -1;
19691 for (
19692 int iij4
19693 = ij4
19694 + 1;
19695 iij4
19696 < 4;
19697 ++iij4)
19698 {
19699 if (j4valid
19700 [iij4]
19701 && IKabs(
19702 cj4array
19703 [ij4]
19704 - cj4array
19705 [iij4])
19706 < IKFAST_SOLUTION_THRESH
19707 && IKabs(
19708 sj4array
19709 [ij4]
19710 - sj4array
19711 [iij4])
19712 < IKFAST_SOLUTION_THRESH)
19713 {
19714 j4valid
19715 [iij4]
19716 = false;
19717 _ij4[1]
19718 = iij4;
19719 break;
19720 }
19721 }
19722 j4 = j4array
19723 [ij4];
19724 cj4 = cj4array
19725 [ij4];
19726 sj4 = sj4array
19727 [ij4];
19728
19729 rotationfunction0(
19730 solutions);
19731 }
19732 }
19733 }
19734 } while (
19735 0);
19736 if (bgotonextstatement)
19737 {
19738 bool
19739 bgotonextstatement
19740 = true;
19741 do
19742 {
19743 if (1)
19744 {
19745 bgotonextstatement
19746 = false;
19747 continue; // branch miss [j4]
19748 }
19749 } while (
19750 0);
19751 if (bgotonextstatement)
19752 {
19753 }
19754 }
19755 }
19756 }
19757 else
19758 {
19759 {
19760 IkReal j4array
19761 [1],
19762 cj4array
19763 [1],
19764 sj4array
19765 [1];
19766 bool j4valid
19767 [1]
19768 = {false};
19769 _nj4
19770 = 1;
19771 IkReal
19772 x1486
19773 = ((100.0)
19774 * pp);
19775 IkReal
19776 x1487
19777 = ((110.0)
19778 * pz);
19779 CheckValue<IkReal> x1488 = IKatan2WithCheck(
19780 IkReal((
19781 (((-1.0)
19782 * px
19783 * x1487))
19784 + ((px
19785 * x1486))
19786 + (((21.25)
19787 * px)))),
19788 ((((-1.0)
19789 * py
19790 * x1486))
19791 + (((-1.0)
19792 * (21.25)
19793 * py))
19794 + ((py
19795 * x1487))),
19796 IKFAST_ATAN2_MAGTHRESH);
19797 if (!x1488
19798 .valid)
19799 {
19800 continue;
19801 }
19802 CheckValue<IkReal> x1489 = IKPowWithIntegerCheck(
19803 IKsign((
19804 (((-1.0)
19805 * (9.0)
19806 * (pz
19807 * pz)))
19808 + (((9.0)
19809 * pp)))),
19810 -1);
19811 if (!x1489
19812 .valid)
19813 {
19814 continue;
19815 }
19816 j4array
19817 [0]
19818 = ((-1.5707963267949)
19819 + (x1488
19820 .value)
19821 + (((1.5707963267949)
19822 * (x1489
19823 .value))));
19824 sj4array
19825 [0]
19826 = IKsin(
19827 j4array
19828 [0]);
19829 cj4array
19830 [0]
19831 = IKcos(
19832 j4array
19833 [0]);
19834 if (j4array
19835 [0]
19836 > IKPI)
19837 {
19838 j4array
19839 [0]
19840 -= IK2PI;
19841 }
19842 else if (
19843 j4array
19844 [0]
19845 < -IKPI)
19846 {
19847 j4array
19848 [0]
19849 += IK2PI;
19850 }
19851 j4valid
19852 [0]
19853 = true;
19854 for (
19855 int ij4
19856 = 0;
19857 ij4
19858 < 1;
19859 ++ij4)
19860 {
19861 if (!j4valid
19862 [ij4])
19863 {
19864 continue;
19865 }
19866 _ij4[0]
19867 = ij4;
19868 _ij4[1]
19869 = -1;
19870 for (
19871 int iij4
19872 = ij4
19873 + 1;
19874 iij4
19875 < 1;
19876 ++iij4)
19877 {
19878 if (j4valid
19879 [iij4]
19880 && IKabs(
19881 cj4array
19882 [ij4]
19883 - cj4array
19884 [iij4])
19885 < IKFAST_SOLUTION_THRESH
19886 && IKabs(
19887 sj4array
19888 [ij4]
19889 - sj4array
19890 [iij4])
19891 < IKFAST_SOLUTION_THRESH)
19892 {
19893 j4valid
19894 [iij4]
19895 = false;
19896 _ij4[1]
19897 = iij4;
19898 break;
19899 }
19900 }
19901 j4 = j4array
19902 [ij4];
19903 cj4 = cj4array
19904 [ij4];
19905 sj4 = sj4array
19906 [ij4];
19907 {
19908 IkReal evalcond
19909 [3];
19910 IkReal
19911 x1490
19912 = IKcos(
19913 j4);
19914 IkReal
19915 x1491
19916 = ((1.0)
19917 * x1490);
19918 IkReal
19919 x1492
19920 = IKsin(
19921 j4);
19922 IkReal
19923 x1493
19924 = (px
19925 * x1492);
19926 evalcond
19927 [0]
19928 = ((((-1.0)
19929 * py
19930 * x1492))
19931 + (((-1.0)
19932 * px
19933 * x1491)));
19934 evalcond
19935 [1]
19936 = ((1.51009803921569)
19937 + x1493
19938 + (((-1.0)
19939 * py
19940 * x1491))
19941 + (((-1.0)
19942 * (3.92156862745098)
19943 * pp))
19944 + (((1.32323529411765)
19945 * cj9)));
19946 evalcond
19947 [2]
19948 = ((-0.2125)
19949 + (((1.1)
19950 * pz))
19951 + (((-1.0)
19952 * (1.0)
19953 * pp))
19954 + (((-0.09)
19955 * py
19956 * x1490))
19957 + (((0.09)
19958 * x1493)));
19959 if (IKabs(
19960 evalcond
19961 [0])
19962 > IKFAST_EVALCOND_THRESH
19963 || IKabs(
19964 evalcond
19965 [1])
19966 > IKFAST_EVALCOND_THRESH
19967 || IKabs(
19968 evalcond
19969 [2])
19970 > IKFAST_EVALCOND_THRESH)
19971 {
19972 continue;
19973 }
19974 }
19975
19976 rotationfunction0(
19977 solutions);
19978 }
19979 }
19980 }
19981 }
19982 }
19983 else
19984 {
19985 {
19986 IkReal j4array
19987 [1],
19988 cj4array
19989 [1],
19990 sj4array
19991 [1];
19992 bool j4valid
19993 [1]
19994 = {false};
19995 _nj4
19996 = 1;
19997 IkReal
19998 x1494
19999 = ((1.32323529411765)
20000 * cj9);
20001 IkReal
20002 x1495
20003 = ((3.92156862745098)
20004 * pp);
20005 CheckValue<IkReal> x1496 = IKatan2WithCheck(
20006 IkReal((
20007 (((-1.0)
20008 * (1.51009803921569)
20009 * px))
20010 + (((-1.0)
20011 * px
20012 * x1494))
20013 + ((px
20014 * x1495)))),
20015 ((((-1.0)
20016 * py
20017 * x1495))
20018 + (((1.51009803921569)
20019 * py))
20020 + ((py
20021 * x1494))),
20022 IKFAST_ATAN2_MAGTHRESH);
20023 if (!x1496
20024 .valid)
20025 {
20026 continue;
20027 }
20028 CheckValue<IkReal> x1497 = IKPowWithIntegerCheck(
20029 IKsign((
20030 pp
20031 + (((-1.0)
20032 * (1.0)
20033 * (pz
20034 * pz))))),
20035 -1);
20036 if (!x1497
20037 .valid)
20038 {
20039 continue;
20040 }
20041 j4array
20042 [0]
20043 = ((-1.5707963267949)
20044 + (x1496
20045 .value)
20046 + (((1.5707963267949)
20047 * (x1497
20048 .value))));
20049 sj4array
20050 [0]
20051 = IKsin(
20052 j4array
20053 [0]);
20054 cj4array
20055 [0]
20056 = IKcos(
20057 j4array
20058 [0]);
20059 if (j4array
20060 [0]
20061 > IKPI)
20062 {
20063 j4array
20064 [0]
20065 -= IK2PI;
20066 }
20067 else if (
20068 j4array
20069 [0]
20070 < -IKPI)
20071 {
20072 j4array
20073 [0]
20074 += IK2PI;
20075 }
20076 j4valid
20077 [0]
20078 = true;
20079 for (
20080 int ij4
20081 = 0;
20082 ij4
20083 < 1;
20084 ++ij4)
20085 {
20086 if (!j4valid
20087 [ij4])
20088 {
20089 continue;
20090 }
20091 _ij4[0]
20092 = ij4;
20093 _ij4[1]
20094 = -1;
20095 for (
20096 int iij4
20097 = ij4
20098 + 1;
20099 iij4
20100 < 1;
20101 ++iij4)
20102 {
20103 if (j4valid
20104 [iij4]
20105 && IKabs(
20106 cj4array
20107 [ij4]
20108 - cj4array
20109 [iij4])
20110 < IKFAST_SOLUTION_THRESH
20111 && IKabs(
20112 sj4array
20113 [ij4]
20114 - sj4array
20115 [iij4])
20116 < IKFAST_SOLUTION_THRESH)
20117 {
20118 j4valid
20119 [iij4]
20120 = false;
20121 _ij4[1]
20122 = iij4;
20123 break;
20124 }
20125 }
20126 j4 = j4array
20127 [ij4];
20128 cj4 = cj4array
20129 [ij4];
20130 sj4 = sj4array
20131 [ij4];
20132 {
20133 IkReal evalcond
20134 [3];
20135 IkReal
20136 x1498
20137 = IKcos(
20138 j4);
20139 IkReal
20140 x1499
20141 = ((1.0)
20142 * x1498);
20143 IkReal
20144 x1500
20145 = IKsin(
20146 j4);
20147 IkReal
20148 x1501
20149 = (px
20150 * x1500);
20151 evalcond
20152 [0]
20153 = ((((-1.0)
20154 * px
20155 * x1499))
20156 + (((-1.0)
20157 * py
20158 * x1500)));
20159 evalcond
20160 [1]
20161 = ((1.51009803921569)
20162 + (((-1.0)
20163 * (3.92156862745098)
20164 * pp))
20165 + (((-1.0)
20166 * py
20167 * x1499))
20168 + x1501
20169 + (((1.32323529411765)
20170 * cj9)));
20171 evalcond
20172 [2]
20173 = ((-0.2125)
20174 + (((-0.09)
20175 * py
20176 * x1498))
20177 + (((1.1)
20178 * pz))
20179 + (((-1.0)
20180 * (1.0)
20181 * pp))
20182 + (((0.09)
20183 * x1501)));
20184 if (IKabs(
20185 evalcond
20186 [0])
20187 > IKFAST_EVALCOND_THRESH
20188 || IKabs(
20189 evalcond
20190 [1])
20191 > IKFAST_EVALCOND_THRESH
20192 || IKabs(
20193 evalcond
20194 [2])
20195 > IKFAST_EVALCOND_THRESH)
20196 {
20197 continue;
20198 }
20199 }
20200
20201 rotationfunction0(
20202 solutions);
20203 }
20204 }
20205 }
20206 }
20207 }
20208 } while (0);
20209 if (bgotonextstatement)
20210 {
20211 bool
20212 bgotonextstatement
20213 = true;
20214 do
20215 {
20216 IkReal x1502
20217 = ((1.32323529411765)
20218 * cj9);
20219 IkReal x1503
20220 = ((3.92156862745098)
20221 * pp);
20222 evalcond[0]
20223 = ((IKabs(
20224 py))
20225 + (IKabs(
20226 px)));
20227 evalcond[1]
20228 = (((sj8
20229 * x1503))
20230 + (((-1.0)
20231 * (1.51009803921569)
20232 * sj8))
20233 + (((-1.0)
20234 * sj8
20235 * x1502)));
20236 evalcond[2]
20237 = 0;
20238 evalcond[3]
20239 = ((-1.51009803921569)
20240 + (((-1.0)
20241 * x1502))
20242 + x1503);
20243 evalcond[4]
20244 = ((((-1.0)
20245 * (1.51009803921569)
20246 * cj8))
20247 + ((cj8
20248 * x1503))
20249 + (((-1.0)
20250 * cj8
20251 * x1502)));
20252 evalcond[5]
20253 = ((-0.2125)
20254 + (((1.1)
20255 * pz))
20256 + (((-1.0)
20257 * (1.0)
20258 * pp)));
20259 if (IKabs(
20260 evalcond
20261 [0])
20262 < 0.0000010000000000
20263 && IKabs(
20264 evalcond
20265 [1])
20266 < 0.0000010000000000
20267 && IKabs(
20268 evalcond
20269 [2])
20270 < 0.0000010000000000
20271 && IKabs(
20272 evalcond
20273 [3])
20274 < 0.0000010000000000
20275 && IKabs(
20276 evalcond
20277 [4])
20278 < 0.0000010000000000
20279 && IKabs(
20280 evalcond
20281 [5])
20282 < 0.0000010000000000)
20283 {
20284 bgotonextstatement
20285 = false;
20286 {
20287 IkReal j4array
20288 [4],
20289 cj4array
20290 [4],
20291 sj4array
20292 [4];
20293 bool j4valid
20294 [4]
20295 = {false};
20296 _nj4
20297 = 4;
20298 j4array
20299 [0]
20300 = 0;
20301 sj4array
20302 [0]
20303 = IKsin(
20304 j4array
20305 [0]);
20306 cj4array
20307 [0]
20308 = IKcos(
20309 j4array
20310 [0]);
20311 j4array
20312 [1]
20313 = 1.5707963267949;
20314 sj4array
20315 [1]
20316 = IKsin(
20317 j4array
20318 [1]);
20319 cj4array
20320 [1]
20321 = IKcos(
20322 j4array
20323 [1]);
20324 j4array
20325 [2]
20326 = 3.14159265358979;
20327 sj4array
20328 [2]
20329 = IKsin(
20330 j4array
20331 [2]);
20332 cj4array
20333 [2]
20334 = IKcos(
20335 j4array
20336 [2]);
20337 j4array
20338 [3]
20339 = -1.5707963267949;
20340 sj4array
20341 [3]
20342 = IKsin(
20343 j4array
20344 [3]);
20345 cj4array
20346 [3]
20347 = IKcos(
20348 j4array
20349 [3]);
20350 if (j4array
20351 [0]
20352 > IKPI)
20353 {
20354 j4array
20355 [0]
20356 -= IK2PI;
20357 }
20358 else if (
20359 j4array
20360 [0]
20361 < -IKPI)
20362 {
20363 j4array
20364 [0]
20365 += IK2PI;
20366 }
20367 j4valid
20368 [0]
20369 = true;
20370 if (j4array
20371 [1]
20372 > IKPI)
20373 {
20374 j4array
20375 [1]
20376 -= IK2PI;
20377 }
20378 else if (
20379 j4array
20380 [1]
20381 < -IKPI)
20382 {
20383 j4array
20384 [1]
20385 += IK2PI;
20386 }
20387 j4valid
20388 [1]
20389 = true;
20390 if (j4array
20391 [2]
20392 > IKPI)
20393 {
20394 j4array
20395 [2]
20396 -= IK2PI;
20397 }
20398 else if (
20399 j4array
20400 [2]
20401 < -IKPI)
20402 {
20403 j4array
20404 [2]
20405 += IK2PI;
20406 }
20407 j4valid
20408 [2]
20409 = true;
20410 if (j4array
20411 [3]
20412 > IKPI)
20413 {
20414 j4array
20415 [3]
20416 -= IK2PI;
20417 }
20418 else if (
20419 j4array
20420 [3]
20421 < -IKPI)
20422 {
20423 j4array
20424 [3]
20425 += IK2PI;
20426 }
20427 j4valid
20428 [3]
20429 = true;
20430 for (
20431 int ij4
20432 = 0;
20433 ij4
20434 < 4;
20435 ++ij4)
20436 {
20437 if (!j4valid
20438 [ij4])
20439 {
20440 continue;
20441 }
20442 _ij4[0]
20443 = ij4;
20444 _ij4[1]
20445 = -1;
20446 for (
20447 int iij4
20448 = ij4
20449 + 1;
20450 iij4
20451 < 4;
20452 ++iij4)
20453 {
20454 if (j4valid
20455 [iij4]
20456 && IKabs(
20457 cj4array
20458 [ij4]
20459 - cj4array
20460 [iij4])
20461 < IKFAST_SOLUTION_THRESH
20462 && IKabs(
20463 sj4array
20464 [ij4]
20465 - sj4array
20466 [iij4])
20467 < IKFAST_SOLUTION_THRESH)
20468 {
20469 j4valid
20470 [iij4]
20471 = false;
20472 _ij4[1]
20473 = iij4;
20474 break;
20475 }
20476 }
20477 j4 = j4array
20478 [ij4];
20479 cj4 = cj4array
20480 [ij4];
20481 sj4 = sj4array
20482 [ij4];
20483
20484 rotationfunction0(
20485 solutions);
20486 }
20487 }
20488 }
20489 } while (0);
20490 if (bgotonextstatement)
20491 {
20492 bool
20493 bgotonextstatement
20494 = true;
20495 do
20496 {
20497 if (1)
20498 {
20499 bgotonextstatement
20500 = false;
20501 continue; // branch miss [j4]
20502 }
20503 } while (0);
20504 if (bgotonextstatement)
20505 {
20506 }
20507 }
20508 }
20509 }
20510 }
20511 }
20512 }
20513 }
20514 else
20515 {
20516 {
20517 IkReal j4array[1],
20518 cj4array[1],
20519 sj4array[1];
20520 bool j4valid[1]
20521 = {false};
20522 _nj4 = 1;
20523 IkReal x1504
20524 = ((1.51009803921569)
20525 * px);
20526 IkReal x1505
20527 = ((1.32323529411765)
20528 * cj9);
20529 IkReal x1506
20530 = (px * x1505);
20531 IkReal x1507
20532 = ((3.92156862745098)
20533 * pp);
20534 IkReal x1508
20535 = (px * x1507);
20536 IkReal x1509
20537 = ((1.51009803921569)
20538 * py);
20539 IkReal x1510
20540 = (cj8 * sj8);
20541 IkReal x1511
20542 = cj8 * cj8;
20543 IkReal x1512
20544 = (py * x1505);
20545 IkReal x1513
20546 = ((3.92156862745098)
20547 * cj8 * pp
20548 * sj8);
20549 IkReal x1514
20550 = (py * x1507);
20551 CheckValue<IkReal>
20552 x1515
20553 = IKPowWithIntegerCheck(
20554 IKsign((
20555 (((-1.0)
20556 * (1.0)
20557 * sj8
20558 * (pz
20559 * pz)))
20560 + ((pp
20561 * sj8)))),
20562 -1);
20563 if (!x1515.valid)
20564 {
20565 continue;
20566 }
20567 CheckValue<IkReal> x1516 = IKatan2WithCheck(
20568 IkReal(
20569 ((((-1.0)
20570 * x1506
20571 * x1511))
20572 + (((-1.0)
20573 * x1510
20574 * x1512))
20575 + (((-1.0)
20576 * x1508))
20577 + (((-1.0)
20578 * x1504
20579 * x1511))
20580 + ((x1508
20581 * x1511))
20582 + ((py
20583 * x1513))
20584 + (((-1.0)
20585 * x1509
20586 * x1510))
20587 + x1504
20588 + x1506)),
20589 ((((-1.0)
20590 * x1512))
20591 + ((px * x1513))
20592 + (((-1.0)
20593 * x1506
20594 * x1510))
20595 + x1514
20596 + (((-1.0)
20597 * x1511
20598 * x1514))
20599 + ((x1509
20600 * x1511))
20601 + (((-1.0)
20602 * x1504
20603 * x1510))
20604 + (((-1.0)
20605 * x1509))
20606 + ((x1511
20607 * x1512))),
20608 IKFAST_ATAN2_MAGTHRESH);
20609 if (!x1516.valid)
20610 {
20611 continue;
20612 }
20613 j4array[0]
20614 = ((-1.5707963267949)
20615 + (((1.5707963267949)
20616 * (x1515
20617 .value)))
20618 + (x1516
20619 .value));
20620 sj4array[0] = IKsin(
20621 j4array[0]);
20622 cj4array[0] = IKcos(
20623 j4array[0]);
20624 if (j4array[0] > IKPI)
20625 {
20626 j4array[0] -= IK2PI;
20627 }
20628 else if (
20629 j4array[0]
20630 < -IKPI)
20631 {
20632 j4array[0] += IK2PI;
20633 }
20634 j4valid[0] = true;
20635 for (int ij4 = 0;
20636 ij4 < 1;
20637 ++ij4)
20638 {
20639 if (!j4valid[ij4])
20640 {
20641 continue;
20642 }
20643 _ij4[0] = ij4;
20644 _ij4[1] = -1;
20645 for (int iij4
20646 = ij4 + 1;
20647 iij4 < 1;
20648 ++iij4)
20649 {
20650 if (j4valid[iij4]
20651 && IKabs(
20652 cj4array
20653 [ij4]
20654 - cj4array
20655 [iij4])
20656 < IKFAST_SOLUTION_THRESH
20657 && IKabs(
20658 sj4array
20659 [ij4]
20660 - sj4array
20661 [iij4])
20662 < IKFAST_SOLUTION_THRESH)
20663 {
20664 j4valid[iij4]
20665 = false;
20666 _ij4[1] = iij4;
20667 break;
20668 }
20669 }
20670 j4 = j4array[ij4];
20671 cj4 = cj4array[ij4];
20672 sj4 = sj4array[ij4];
20673 {
20674 IkReal
20675 evalcond[5];
20676 IkReal x1517
20677 = ((1.32323529411765)
20678 * cj9);
20679 IkReal x1518
20680 = ((3.92156862745098)
20681 * pp);
20682 IkReal x1519
20683 = IKsin(j4);
20684 IkReal x1520
20685 = (px
20686 * x1519);
20687 IkReal x1521
20688 = IKcos(j4);
20689 IkReal x1522
20690 = ((1.0)
20691 * x1521);
20692 IkReal x1523
20693 = (py
20694 * x1522);
20695 IkReal x1524
20696 = (px
20697 * x1522);
20698 IkReal x1525
20699 = (py
20700 * x1519);
20701 IkReal x1526
20702 = ((1.0)
20703 * x1525);
20704 IkReal x1527
20705 = (px
20706 * x1521);
20707 IkReal x1528
20708 = (sj8
20709 * x1520);
20710 IkReal x1529
20711 = ((0.09)
20712 * cj8);
20713 evalcond[0]
20714 = ((((-1.0)
20715 * sj8
20716 * x1517))
20717 + (((-1.0)
20718 * x1523))
20719 + ((sj8
20720 * x1518))
20721 + x1520
20722 + (((-1.0)
20723 * (1.51009803921569)
20724 * sj8)));
20725 evalcond[1]
20726 = ((((-1.0)
20727 * (1.51009803921569)
20728 * cj8))
20729 + (((-1.0)
20730 * x1526))
20731 + ((cj8
20732 * x1518))
20733 + (((-1.0)
20734 * x1524))
20735 + (((-1.0)
20736 * cj8
20737 * x1517)));
20738 evalcond[2]
20739 = (((sj8
20740 * x1527))
20741 + (((-1.0)
20742 * cj8
20743 * x1523))
20744 + ((cj8
20745 * x1520))
20746 + ((sj8
20747 * x1525)));
20748 evalcond[3]
20749 = ((-1.51009803921569)
20750 + x1518
20751 + (((-1.0)
20752 * cj8
20753 * x1526))
20754 + (((-1.0)
20755 * sj8
20756 * x1523))
20757 + (((-1.0)
20758 * x1517))
20759 + (((-1.0)
20760 * cj8
20761 * x1524))
20762 + x1528);
20763 evalcond[4]
20764 = ((-0.2125)
20765 + ((x1527
20766 * x1529))
20767 + (((0.09)
20768 * py
20769 * sj8
20770 * x1521))
20771 + ((x1525
20772 * x1529))
20773 + (((1.1)
20774 * pz))
20775 + (((-1.0)
20776 * (1.0)
20777 * pp))
20778 + (((-0.09)
20779 * x1528)));
20780 if (IKabs(evalcond
20781 [0])
20782 > IKFAST_EVALCOND_THRESH
20783 || IKabs(
20784 evalcond
20785 [1])
20786 > IKFAST_EVALCOND_THRESH
20787 || IKabs(
20788 evalcond
20789 [2])
20790 > IKFAST_EVALCOND_THRESH
20791 || IKabs(
20792 evalcond
20793 [3])
20794 > IKFAST_EVALCOND_THRESH
20795 || IKabs(
20796 evalcond
20797 [4])
20798 > IKFAST_EVALCOND_THRESH)
20799 {
20800 continue;
20801 }
20802 }
20803
20804 rotationfunction0(
20805 solutions);
20806 }
20807 }
20808 }
20809 }
20810 }
20811 else
20812 {
20813 {
20814 IkReal j4array[1],
20815 cj4array[1],
20816 sj4array[1];
20817 bool j4valid[1] = {false};
20818 _nj4 = 1;
20819 IkReal x1530
20820 = ((1.51009803921569)
20821 * cj8 * sj8);
20822 IkReal x1531 = cj8 * cj8;
20823 IkReal x1532
20824 = ((1.51009803921569)
20825 * x1531);
20826 IkReal x1533
20827 = ((1.32323529411765)
20828 * cj8 * cj9 * sj8);
20829 IkReal x1534
20830 = ((3.92156862745098)
20831 * cj8 * pp * sj8);
20832 IkReal x1535
20833 = ((1.32323529411765)
20834 * cj9 * x1531);
20835 IkReal x1536
20836 = ((3.92156862745098)
20837 * pp * x1531);
20838 CheckValue<IkReal> x1537
20839 = IKatan2WithCheck(
20840 IkReal((
20841 (((-1.0) * py
20842 * x1536))
20843 + ((px
20844 * x1534))
20845 + (((-1.0)
20846 * px
20847 * x1530))
20848 + (((-1.0)
20849 * px
20850 * x1533))
20851 + ((py
20852 * x1535))
20853 + ((py
20854 * x1532)))),
20855 (((px * x1532))
20856 + (((-1.0) * py
20857 * x1534))
20858 + (((-1.0) * px
20859 * x1536))
20860 + ((px * x1535))
20861 + ((py * x1533))
20862 + ((py
20863 * x1530))),
20864 IKFAST_ATAN2_MAGTHRESH);
20865 if (!x1537.valid)
20866 {
20867 continue;
20868 }
20869 CheckValue<IkReal> x1538
20870 = IKPowWithIntegerCheck(
20871 IKsign(
20872 (((cj8
20873 * (pz
20874 * pz)))
20875 + (((-1.0)
20876 * cj8
20877 * pp)))),
20878 -1);
20879 if (!x1538.valid)
20880 {
20881 continue;
20882 }
20883 j4array[0]
20884 = ((-1.5707963267949)
20885 + (x1537.value)
20886 + (((1.5707963267949)
20887 * (x1538
20888 .value))));
20889 sj4array[0]
20890 = IKsin(j4array[0]);
20891 cj4array[0]
20892 = IKcos(j4array[0]);
20893 if (j4array[0] > IKPI)
20894 {
20895 j4array[0] -= IK2PI;
20896 }
20897 else if (
20898 j4array[0] < -IKPI)
20899 {
20900 j4array[0] += IK2PI;
20901 }
20902 j4valid[0] = true;
20903 for (int ij4 = 0; ij4 < 1;
20904 ++ij4)
20905 {
20906 if (!j4valid[ij4])
20907 {
20908 continue;
20909 }
20910 _ij4[0] = ij4;
20911 _ij4[1] = -1;
20912 for (int iij4 = ij4 + 1;
20913 iij4 < 1;
20914 ++iij4)
20915 {
20916 if (j4valid[iij4]
20917 && IKabs(
20918 cj4array
20919 [ij4]
20920 - cj4array
20921 [iij4])
20922 < IKFAST_SOLUTION_THRESH
20923 && IKabs(
20924 sj4array
20925 [ij4]
20926 - sj4array
20927 [iij4])
20928 < IKFAST_SOLUTION_THRESH)
20929 {
20930 j4valid[iij4]
20931 = false;
20932 _ij4[1] = iij4;
20933 break;
20934 }
20935 }
20936 j4 = j4array[ij4];
20937 cj4 = cj4array[ij4];
20938 sj4 = sj4array[ij4];
20939 {
20940 IkReal evalcond[5];
20941 IkReal x1539
20942 = ((1.32323529411765)
20943 * cj9);
20944 IkReal x1540
20945 = ((3.92156862745098)
20946 * pp);
20947 IkReal x1541
20948 = IKsin(j4);
20949 IkReal x1542
20950 = (px * x1541);
20951 IkReal x1543
20952 = IKcos(j4);
20953 IkReal x1544
20954 = ((1.0) * x1543);
20955 IkReal x1545
20956 = (py * x1544);
20957 IkReal x1546
20958 = (px * x1544);
20959 IkReal x1547
20960 = (py * x1541);
20961 IkReal x1548
20962 = ((1.0) * x1547);
20963 IkReal x1549
20964 = (px * x1543);
20965 IkReal x1550
20966 = (sj8 * x1542);
20967 IkReal x1551
20968 = ((0.09) * cj8);
20969 evalcond[0]
20970 = (x1542
20971 + (((-1.0)
20972 * sj8
20973 * x1539))
20974 + ((sj8
20975 * x1540))
20976 + (((-1.0)
20977 * x1545))
20978 + (((-1.0)
20979 * (1.51009803921569)
20980 * sj8)));
20981 evalcond[1]
20982 = ((((-1.0)
20983 * (1.51009803921569)
20984 * cj8))
20985 + ((cj8
20986 * x1540))
20987 + (((-1.0)
20988 * x1546))
20989 + (((-1.0)
20990 * cj8
20991 * x1539))
20992 + (((-1.0)
20993 * x1548)));
20994 evalcond[2]
20995 = (((sj8 * x1547))
20996 + (((-1.0)
20997 * cj8
20998 * x1545))
20999 + ((cj8
21000 * x1542))
21001 + ((sj8
21002 * x1549)));
21003 evalcond[3]
21004 = ((-1.51009803921569)
21005 + (((-1.0)
21006 * cj8
21007 * x1546))
21008 + x1540 + x1550
21009 + (((-1.0)
21010 * x1539))
21011 + (((-1.0)
21012 * cj8
21013 * x1548))
21014 + (((-1.0)
21015 * sj8
21016 * x1545)));
21017 evalcond[4]
21018 = ((-0.2125)
21019 + (((1.1)
21020 * pz))
21021 + ((x1549
21022 * x1551))
21023 + (((-1.0)
21024 * (1.0)
21025 * pp))
21026 + (((-0.09)
21027 * x1550))
21028 + (((0.09) * py
21029 * sj8
21030 * x1543))
21031 + ((x1547
21032 * x1551)));
21033 if (IKabs(evalcond[0])
21034 > IKFAST_EVALCOND_THRESH
21035 || IKabs(evalcond
21036 [1])
21037 > IKFAST_EVALCOND_THRESH
21038 || IKabs(evalcond
21039 [2])
21040 > IKFAST_EVALCOND_THRESH
21041 || IKabs(evalcond
21042 [3])
21043 > IKFAST_EVALCOND_THRESH
21044 || IKabs(evalcond
21045 [4])
21046 > IKFAST_EVALCOND_THRESH)
21047 {
21048 continue;
21049 }
21050 }
21051
21052 rotationfunction0(
21053 solutions);
21054 }
21055 }
21056 }
21057 }
21058 }
21059 else
21060 {
21061 {
21062 IkReal j4array[1],
21063 cj4array[1], sj4array[1];
21064 bool j4valid[1] = {false};
21065 _nj4 = 1;
21066 IkReal x1552
21067 = ((1.51009803921569)
21068 * cj8);
21069 IkReal x1553
21070 = ((1.51009803921569)
21071 * sj8);
21072 IkReal x1554
21073 = ((1.32323529411765)
21074 * cj8 * cj9);
21075 IkReal x1555
21076 = ((3.92156862745098)
21077 * cj8 * pp);
21078 IkReal x1556
21079 = ((1.32323529411765)
21080 * cj9 * sj8);
21081 IkReal x1557
21082 = ((3.92156862745098) * pp
21083 * sj8);
21084 CheckValue<IkReal> x1558
21085 = IKatan2WithCheck(
21086 IkReal(
21087 ((((-1.0) * py
21088 * x1554))
21089 + (((-1.0) * py
21090 * x1552))
21091 + (((-1.0) * px
21092 * x1557))
21093 + ((py * x1555))
21094 + ((px * x1556))
21095 + ((px
21096 * x1553)))),
21097 ((((-1.0) * px
21098 * x1552))
21099 + ((py * x1557))
21100 + (((-1.0) * py
21101 * x1553))
21102 + (((-1.0) * py
21103 * x1556))
21104 + (((-1.0) * px
21105 * x1554))
21106 + ((px * x1555))),
21107 IKFAST_ATAN2_MAGTHRESH);
21108 if (!x1558.valid)
21109 {
21110 continue;
21111 }
21112 CheckValue<IkReal> x1559
21113 = IKPowWithIntegerCheck(
21114 IKsign((
21115 pp
21116 + (((-1.0) * (1.0)
21117 * (pz
21118 * pz))))),
21119 -1);
21120 if (!x1559.valid)
21121 {
21122 continue;
21123 }
21124 j4array[0]
21125 = ((-1.5707963267949)
21126 + (x1558.value)
21127 + (((1.5707963267949)
21128 * (x1559.value))));
21129 sj4array[0]
21130 = IKsin(j4array[0]);
21131 cj4array[0]
21132 = IKcos(j4array[0]);
21133 if (j4array[0] > IKPI)
21134 {
21135 j4array[0] -= IK2PI;
21136 }
21137 else if (j4array[0] < -IKPI)
21138 {
21139 j4array[0] += IK2PI;
21140 }
21141 j4valid[0] = true;
21142 for (int ij4 = 0; ij4 < 1;
21143 ++ij4)
21144 {
21145 if (!j4valid[ij4])
21146 {
21147 continue;
21148 }
21149 _ij4[0] = ij4;
21150 _ij4[1] = -1;
21151 for (int iij4 = ij4 + 1;
21152 iij4 < 1;
21153 ++iij4)
21154 {
21155 if (j4valid[iij4]
21156 && IKabs(
21157 cj4array[ij4]
21158 - cj4array
21159 [iij4])
21160 < IKFAST_SOLUTION_THRESH
21161 && IKabs(
21162 sj4array[ij4]
21163 - sj4array
21164 [iij4])
21165 < IKFAST_SOLUTION_THRESH)
21166 {
21167 j4valid[iij4] = false;
21168 _ij4[1] = iij4;
21169 break;
21170 }
21171 }
21172 j4 = j4array[ij4];
21173 cj4 = cj4array[ij4];
21174 sj4 = sj4array[ij4];
21175 {
21176 IkReal evalcond[5];
21177 IkReal x1560
21178 = ((1.32323529411765)
21179 * cj9);
21180 IkReal x1561
21181 = ((3.92156862745098)
21182 * pp);
21183 IkReal x1562 = IKsin(j4);
21184 IkReal x1563
21185 = (px * x1562);
21186 IkReal x1564 = IKcos(j4);
21187 IkReal x1565
21188 = ((1.0) * x1564);
21189 IkReal x1566
21190 = (py * x1565);
21191 IkReal x1567
21192 = (px * x1565);
21193 IkReal x1568
21194 = (py * x1562);
21195 IkReal x1569
21196 = ((1.0) * x1568);
21197 IkReal x1570
21198 = (px * x1564);
21199 IkReal x1571
21200 = (sj8 * x1563);
21201 IkReal x1572
21202 = ((0.09) * cj8);
21203 evalcond[0]
21204 = (((sj8 * x1561))
21205 + x1563
21206 + (((-1.0)
21207 * (1.51009803921569)
21208 * sj8))
21209 + (((-1.0) * sj8
21210 * x1560))
21211 + (((-1.0)
21212 * x1566)));
21213 evalcond[1]
21214 = ((((-1.0)
21215 * (1.51009803921569)
21216 * cj8))
21217 + (((-1.0) * cj8
21218 * x1560))
21219 + (((-1.0)
21220 * x1569))
21221 + ((cj8 * x1561))
21222 + (((-1.0)
21223 * x1567)));
21224 evalcond[2]
21225 = ((((-1.0) * cj8
21226 * x1566))
21227 + ((sj8 * x1570))
21228 + ((cj8 * x1563))
21229 + ((sj8 * x1568)));
21230 evalcond[3]
21231 = ((-1.51009803921569)
21232 + (((-1.0)
21233 * x1560))
21234 + (((-1.0) * sj8
21235 * x1566))
21236 + x1571 + x1561
21237 + (((-1.0) * cj8
21238 * x1569))
21239 + (((-1.0) * cj8
21240 * x1567)));
21241 evalcond[4]
21242 = ((-0.2125)
21243 + (((-0.09)
21244 * x1571))
21245 + ((x1570 * x1572))
21246 + (((0.09) * py
21247 * sj8 * x1564))
21248 + (((1.1) * pz))
21249 + (((-1.0) * (1.0)
21250 * pp))
21251 + ((x1568
21252 * x1572)));
21253 if (IKabs(evalcond[0])
21254 > IKFAST_EVALCOND_THRESH
21255 || IKabs(evalcond[1])
21256 > IKFAST_EVALCOND_THRESH
21257 || IKabs(evalcond[2])
21258 > IKFAST_EVALCOND_THRESH
21259 || IKabs(evalcond[3])
21260 > IKFAST_EVALCOND_THRESH
21261 || IKabs(evalcond[4])
21262 > IKFAST_EVALCOND_THRESH)
21263 {
21264 continue;
21265 }
21266 }
21267
21268 rotationfunction0(
21269 solutions);
21270 }
21271 }
21272 }
21273 }
21274 }
21275 } while (0);
21276 if (bgotonextstatement)
21277 {
21278 bool bgotonextstatement = true;
21279 do
21280 {
21281 IkReal x1573 = ((0.3) * cj9);
21282 IkReal x1574 = ((0.045) * sj9);
21283 evalcond[0]
21284 = ((-3.14159265358979)
21285 + (IKfmod(
21286 ((3.14159265358979)
21287 + (IKabs(
21288 ((-3.14159265358979)
21289 + j6)))),
21290 6.28318530717959)));
21291 evalcond[1]
21292 = ((0.39655)
21293 + (((0.0765) * sj9))
21294 + (((0.32595) * cj9))
21295 + (((-1.0) * (1.0) * pp)));
21296 evalcond[2]
21297 = ((-0.55)
21298 + (((-1.0) * (1.0) * pz))
21299 + (((-1.0) * x1573))
21300 + (((-1.0) * x1574)));
21301 evalcond[3]
21302 = ((0.55) + x1574 + x1573 + pz);
21303 if (IKabs(evalcond[0])
21304 < 0.0000010000000000
21305 && IKabs(evalcond[1])
21306 < 0.0000010000000000
21307 && IKabs(evalcond[2])
21308 < 0.0000010000000000
21309 && IKabs(evalcond[3])
21310 < 0.0000010000000000)
21311 {
21312 bgotonextstatement = false;
21313 {
21314 IkReal j4eval[3];
21315 sj6 = 0;
21316 cj6 = -1.0;
21317 j6 = 3.14159265358979;
21318 IkReal x1575
21319 = (pp
21320 + (((-1.0) * (1.0)
21321 * (pz * pz))));
21322 IkReal x1576
21323 = ((1.51009803921569)
21324 * cj8);
21325 IkReal x1577
21326 = ((1.51009803921569)
21327 * sj8);
21328 IkReal x1578
21329 = ((1.32323529411765) * cj8
21330 * cj9);
21331 IkReal x1579
21332 = ((3.92156862745098) * cj8
21333 * pp);
21334 IkReal x1580
21335 = ((1.32323529411765) * cj9
21336 * sj8);
21337 IkReal x1581
21338 = ((3.92156862745098) * pp
21339 * sj8);
21340 j4eval[0] = x1575;
21341 j4eval[1]
21342 = ((IKabs(
21343 ((((-1.0) * py
21344 * x1579))
21345 + ((px * x1577))
21346 + ((py * x1578))
21347 + (((-1.0) * px
21348 * x1581))
21349 + ((py * x1576))
21350 + ((px * x1580)))))
21351 + (IKabs(
21352 (((px * x1578))
21353 + (((-1.0) * py
21354 * x1580))
21355 + ((px * x1576))
21356 + (((-1.0) * px
21357 * x1579))
21358 + ((py * x1581))
21359 + (((-1.0) * py
21360 * x1577))))));
21361 j4eval[2] = IKsign(x1575);
21362 if (IKabs(j4eval[0])
21363 < 0.0000010000000000
21364 || IKabs(j4eval[1])
21365 < 0.0000010000000000
21366 || IKabs(j4eval[2])
21367 < 0.0000010000000000)
21368 {
21369 {
21370 IkReal j4eval[2];
21371 sj6 = 0;
21372 cj6 = -1.0;
21373 j6 = 3.14159265358979;
21374 IkReal x1582
21375 = (((cj8 * (pz * pz)))
21376 + (((-1.0) * (1.0)
21377 * cj8 * pp)));
21378 j4eval[0] = x1582;
21379 j4eval[1] = IKsign(x1582);
21380 if (IKabs(j4eval[0])
21381 < 0.0000010000000000
21382 || IKabs(j4eval[1])
21383 < 0.0000010000000000)
21384 {
21385 {
21386 IkReal j4eval[2];
21387 sj6 = 0;
21388 cj6 = -1.0;
21389 j6 = 3.14159265358979;
21390 IkReal x1583
21391 = ((((-1.0) * (1.0)
21392 * cj8
21393 * (pz * pz)))
21394 + ((cj8 * pp)));
21395 j4eval[0] = x1583;
21396 j4eval[1]
21397 = IKsign(x1583);
21398 if (IKabs(j4eval[0])
21399 < 0.0000010000000000
21400 || IKabs(j4eval[1])
21401 < 0.0000010000000000)
21402 {
21403 {
21404 IkReal evalcond[6];
21405 bool
21406 bgotonextstatement
21407 = true;
21408 do
21409 {
21410 evalcond[0]
21411 = ((-3.14159265358979)
21412 + (IKfmod(
21413 ((3.14159265358979)
21414 + (IKabs((
21415 (-1.5707963267949)
21416 + j8)))),
21417 6.28318530717959)));
21418 if (IKabs(evalcond
21419 [0])
21420 < 0.0000010000000000)
21421 {
21422 bgotonextstatement
21423 = false;
21424 {
21425 IkReal
21426 j4eval[3];
21427 sj6 = 0;
21428 cj6 = -1.0;
21429 j6 = 3.14159265358979;
21430 sj8 = 1.0;
21431 cj8 = 0;
21432 j8 = 1.5707963267949;
21433 IkReal x1584
21434 = (pp
21435 + (((-1.0)
21436 * (1.0)
21437 * (pz
21438 * pz))));
21439 IkReal x1585
21440 = ((13497.0)
21441 * cj9);
21442 IkReal x1586
21443 = ((40000.0)
21444 * pp);
21445 j4eval[0]
21446 = x1584;
21447 j4eval[1]
21448 = ((IKabs((
21449 (((15403.0)
21450 * px))
21451 + ((px
21452 * x1585))
21453 + (((-1.0)
21454 * px
21455 * x1586)))))
21456 + (IKabs((
21457 (((-1.0)
21458 * py
21459 * x1585))
21460 + ((py
21461 * x1586))
21462 + (((-1.0)
21463 * (15403.0)
21464 * py))))));
21465 j4eval[2]
21466 = IKsign(
21467 x1584);
21468 if (IKabs(
21469 j4eval
21470 [0])
21471 < 0.0000010000000000
21472 || IKabs(
21473 j4eval
21474 [1])
21475 < 0.0000010000000000
21476 || IKabs(
21477 j4eval
21478 [2])
21479 < 0.0000010000000000)
21480 {
21481 {
21482 IkReal j4eval
21483 [3];
21484 sj6 = 0;
21485 cj6 = -1.0;
21486 j6 = 3.14159265358979;
21487 sj8 = 1.0;
21488 cj8 = 0;
21489 j8 = 1.5707963267949;
21490 IkReal
21491 x1587
21492 = pz
21493 * pz;
21494 IkReal
21495 x1588
21496 = ((80.0)
21497 * pp);
21498 IkReal
21499 x1589
21500 = ((88.0)
21501 * pz);
21502 j4eval[0]
21503 = (pp
21504 + (((-1.0)
21505 * x1587)));
21506 j4eval[1]
21507 = ((IKabs((
21508 ((py
21509 * x1589))
21510 + (((17.0)
21511 * py))
21512 + ((py
21513 * x1588)))))
21514 + (IKabs((
21515 ((px
21516 * x1588))
21517 + (((17.0)
21518 * px))
21519 + ((px
21520 * x1589))))));
21521 j4eval[2] = IKsign((
21522 (((9.0)
21523 * pp))
21524 + (((-9.0)
21525 * x1587))));
21526 if (IKabs(
21527 j4eval
21528 [0])
21529 < 0.0000010000000000
21530 || IKabs(
21531 j4eval
21532 [1])
21533 < 0.0000010000000000
21534 || IKabs(
21535 j4eval
21536 [2])
21537 < 0.0000010000000000)
21538 {
21539 {
21540 IkReal evalcond
21541 [5];
21542 bool
21543 bgotonextstatement
21544 = true;
21545 do
21546 {
21547 IkReal
21548 x1590
21549 = ((-1.51009803921569)
21550 + (((-1.0)
21551 * (1.32323529411765)
21552 * cj9))
21553 + (((3.92156862745098)
21554 * pp)));
21555 evalcond
21556 [0]
21557 = ((IKabs(
21558 py))
21559 + (IKabs(
21560 px)));
21561 evalcond
21562 [1]
21563 = x1590;
21564 evalcond
21565 [2]
21566 = 0;
21567 evalcond
21568 [3]
21569 = x1590;
21570 evalcond
21571 [4]
21572 = ((-0.2125)
21573 + (((-1.0)
21574 * (1.1)
21575 * pz))
21576 + (((-1.0)
21577 * (1.0)
21578 * pp)));
21579 if (IKabs(
21580 evalcond
21581 [0])
21582 < 0.0000010000000000
21583 && IKabs(
21584 evalcond
21585 [1])
21586 < 0.0000010000000000
21587 && IKabs(
21588 evalcond
21589 [2])
21590 < 0.0000010000000000
21591 && IKabs(
21592 evalcond
21593 [3])
21594 < 0.0000010000000000
21595 && IKabs(
21596 evalcond
21597 [4])
21598 < 0.0000010000000000)
21599 {
21600 bgotonextstatement
21601 = false;
21602 {
21603 IkReal j4array
21604 [4],
21605 cj4array
21606 [4],
21607 sj4array
21608 [4];
21609 bool j4valid
21610 [4]
21611 = {false};
21612 _nj4
21613 = 4;
21614 j4array
21615 [0]
21616 = 0;
21617 sj4array
21618 [0]
21619 = IKsin(
21620 j4array
21621 [0]);
21622 cj4array
21623 [0]
21624 = IKcos(
21625 j4array
21626 [0]);
21627 j4array
21628 [1]
21629 = 1.5707963267949;
21630 sj4array
21631 [1]
21632 = IKsin(
21633 j4array
21634 [1]);
21635 cj4array
21636 [1]
21637 = IKcos(
21638 j4array
21639 [1]);
21640 j4array
21641 [2]
21642 = 3.14159265358979;
21643 sj4array
21644 [2]
21645 = IKsin(
21646 j4array
21647 [2]);
21648 cj4array
21649 [2]
21650 = IKcos(
21651 j4array
21652 [2]);
21653 j4array
21654 [3]
21655 = -1.5707963267949;
21656 sj4array
21657 [3]
21658 = IKsin(
21659 j4array
21660 [3]);
21661 cj4array
21662 [3]
21663 = IKcos(
21664 j4array
21665 [3]);
21666 if (j4array
21667 [0]
21668 > IKPI)
21669 {
21670 j4array
21671 [0]
21672 -= IK2PI;
21673 }
21674 else if (
21675 j4array
21676 [0]
21677 < -IKPI)
21678 {
21679 j4array
21680 [0]
21681 += IK2PI;
21682 }
21683 j4valid
21684 [0]
21685 = true;
21686 if (j4array
21687 [1]
21688 > IKPI)
21689 {
21690 j4array
21691 [1]
21692 -= IK2PI;
21693 }
21694 else if (
21695 j4array
21696 [1]
21697 < -IKPI)
21698 {
21699 j4array
21700 [1]
21701 += IK2PI;
21702 }
21703 j4valid
21704 [1]
21705 = true;
21706 if (j4array
21707 [2]
21708 > IKPI)
21709 {
21710 j4array
21711 [2]
21712 -= IK2PI;
21713 }
21714 else if (
21715 j4array
21716 [2]
21717 < -IKPI)
21718 {
21719 j4array
21720 [2]
21721 += IK2PI;
21722 }
21723 j4valid
21724 [2]
21725 = true;
21726 if (j4array
21727 [3]
21728 > IKPI)
21729 {
21730 j4array
21731 [3]
21732 -= IK2PI;
21733 }
21734 else if (
21735 j4array
21736 [3]
21737 < -IKPI)
21738 {
21739 j4array
21740 [3]
21741 += IK2PI;
21742 }
21743 j4valid
21744 [3]
21745 = true;
21746 for (
21747 int ij4
21748 = 0;
21749 ij4
21750 < 4;
21751 ++ij4)
21752 {
21753 if (!j4valid
21754 [ij4])
21755 {
21756 continue;
21757 }
21758 _ij4[0]
21759 = ij4;
21760 _ij4[1]
21761 = -1;
21762 for (
21763 int iij4
21764 = ij4
21765 + 1;
21766 iij4
21767 < 4;
21768 ++iij4)
21769 {
21770 if (j4valid
21771 [iij4]
21772 && IKabs(
21773 cj4array
21774 [ij4]
21775 - cj4array
21776 [iij4])
21777 < IKFAST_SOLUTION_THRESH
21778 && IKabs(
21779 sj4array
21780 [ij4]
21781 - sj4array
21782 [iij4])
21783 < IKFAST_SOLUTION_THRESH)
21784 {
21785 j4valid
21786 [iij4]
21787 = false;
21788 _ij4[1]
21789 = iij4;
21790 break;
21791 }
21792 }
21793 j4 = j4array
21794 [ij4];
21795 cj4 = cj4array
21796 [ij4];
21797 sj4 = sj4array
21798 [ij4];
21799
21800 rotationfunction0(
21801 solutions);
21802 }
21803 }
21804 }
21805 } while (
21806 0);
21807 if (bgotonextstatement)
21808 {
21809 bool
21810 bgotonextstatement
21811 = true;
21812 do
21813 {
21814 if (1)
21815 {
21816 bgotonextstatement
21817 = false;
21818 continue; // branch miss [j4]
21819 }
21820 } while (
21821 0);
21822 if (bgotonextstatement)
21823 {
21824 }
21825 }
21826 }
21827 }
21828 else
21829 {
21830 {
21831 IkReal j4array
21832 [1],
21833 cj4array
21834 [1],
21835 sj4array
21836 [1];
21837 bool j4valid
21838 [1]
21839 = {false};
21840 _nj4
21841 = 1;
21842 IkReal
21843 x1591
21844 = ((100.0)
21845 * pp);
21846 IkReal
21847 x1592
21848 = ((110.0)
21849 * pz);
21850 CheckValue<IkReal> x1593 = IKatan2WithCheck(
21851 IkReal((
21852 (((-1.0)
21853 * px
21854 * x1591))
21855 + (((-1.0)
21856 * (21.25)
21857 * px))
21858 + (((-1.0)
21859 * px
21860 * x1592)))),
21861 (((py
21862 * x1591))
21863 + ((py
21864 * x1592))
21865 + (((21.25)
21866 * py))),
21867 IKFAST_ATAN2_MAGTHRESH);
21868 if (!x1593
21869 .valid)
21870 {
21871 continue;
21872 }
21873 CheckValue<IkReal> x1594 = IKPowWithIntegerCheck(
21874 IKsign((
21875 (((-1.0)
21876 * (9.0)
21877 * (pz
21878 * pz)))
21879 + (((9.0)
21880 * pp)))),
21881 -1);
21882 if (!x1594
21883 .valid)
21884 {
21885 continue;
21886 }
21887 j4array
21888 [0]
21889 = ((-1.5707963267949)
21890 + (x1593
21891 .value)
21892 + (((1.5707963267949)
21893 * (x1594
21894 .value))));
21895 sj4array
21896 [0]
21897 = IKsin(
21898 j4array
21899 [0]);
21900 cj4array
21901 [0]
21902 = IKcos(
21903 j4array
21904 [0]);
21905 if (j4array
21906 [0]
21907 > IKPI)
21908 {
21909 j4array
21910 [0]
21911 -= IK2PI;
21912 }
21913 else if (
21914 j4array
21915 [0]
21916 < -IKPI)
21917 {
21918 j4array
21919 [0]
21920 += IK2PI;
21921 }
21922 j4valid
21923 [0]
21924 = true;
21925 for (
21926 int ij4
21927 = 0;
21928 ij4
21929 < 1;
21930 ++ij4)
21931 {
21932 if (!j4valid
21933 [ij4])
21934 {
21935 continue;
21936 }
21937 _ij4[0]
21938 = ij4;
21939 _ij4[1]
21940 = -1;
21941 for (
21942 int iij4
21943 = ij4
21944 + 1;
21945 iij4
21946 < 1;
21947 ++iij4)
21948 {
21949 if (j4valid
21950 [iij4]
21951 && IKabs(
21952 cj4array
21953 [ij4]
21954 - cj4array
21955 [iij4])
21956 < IKFAST_SOLUTION_THRESH
21957 && IKabs(
21958 sj4array
21959 [ij4]
21960 - sj4array
21961 [iij4])
21962 < IKFAST_SOLUTION_THRESH)
21963 {
21964 j4valid
21965 [iij4]
21966 = false;
21967 _ij4[1]
21968 = iij4;
21969 break;
21970 }
21971 }
21972 j4 = j4array
21973 [ij4];
21974 cj4 = cj4array
21975 [ij4];
21976 sj4 = sj4array
21977 [ij4];
21978 {
21979 IkReal evalcond
21980 [3];
21981 IkReal
21982 x1595
21983 = IKcos(
21984 j4);
21985 IkReal
21986 x1596
21987 = ((1.0)
21988 * x1595);
21989 IkReal
21990 x1597
21991 = IKsin(
21992 j4);
21993 IkReal
21994 x1598
21995 = (px
21996 * x1597);
21997 evalcond
21998 [0]
21999 = ((((-1.0)
22000 * px
22001 * x1596))
22002 + (((-1.0)
22003 * py
22004 * x1597)));
22005 evalcond
22006 [1]
22007 = ((-1.51009803921569)
22008 + (((-1.0)
22009 * py
22010 * x1596))
22011 + (((-1.0)
22012 * (1.32323529411765)
22013 * cj9))
22014 + (((3.92156862745098)
22015 * pp))
22016 + x1598);
22017 evalcond
22018 [2]
22019 = ((-0.2125)
22020 + (((-1.0)
22021 * (1.1)
22022 * pz))
22023 + (((-0.09)
22024 * x1598))
22025 + (((0.09)
22026 * py
22027 * x1595))
22028 + (((-1.0)
22029 * (1.0)
22030 * pp)));
22031 if (IKabs(
22032 evalcond
22033 [0])
22034 > IKFAST_EVALCOND_THRESH
22035 || IKabs(
22036 evalcond
22037 [1])
22038 > IKFAST_EVALCOND_THRESH
22039 || IKabs(
22040 evalcond
22041 [2])
22042 > IKFAST_EVALCOND_THRESH)
22043 {
22044 continue;
22045 }
22046 }
22047
22048 rotationfunction0(
22049 solutions);
22050 }
22051 }
22052 }
22053 }
22054 }
22055 else
22056 {
22057 {
22058 IkReal j4array
22059 [1],
22060 cj4array
22061 [1],
22062 sj4array
22063 [1];
22064 bool j4valid
22065 [1]
22066 = {false};
22067 _nj4 = 1;
22068 IkReal
22069 x1599
22070 = ((1.32323529411765)
22071 * cj9);
22072 IkReal
22073 x1600
22074 = ((3.92156862745098)
22075 * pp);
22076 CheckValue<IkReal> x1601 = IKPowWithIntegerCheck(
22077 IKsign((
22078 pp
22079 + (((-1.0)
22080 * (1.0)
22081 * (pz
22082 * pz))))),
22083 -1);
22084 if (!x1601
22085 .valid)
22086 {
22087 continue;
22088 }
22089 CheckValue<IkReal> x1602 = IKatan2WithCheck(
22090 IkReal((
22091 ((px
22092 * x1599))
22093 + (((1.51009803921569)
22094 * px))
22095 + (((-1.0)
22096 * px
22097 * x1600)))),
22098 ((((-1.0)
22099 * (1.51009803921569)
22100 * py))
22101 + (((-1.0)
22102 * py
22103 * x1599))
22104 + ((py
22105 * x1600))),
22106 IKFAST_ATAN2_MAGTHRESH);
22107 if (!x1602
22108 .valid)
22109 {
22110 continue;
22111 }
22112 j4array[0]
22113 = ((-1.5707963267949)
22114 + (((1.5707963267949)
22115 * (x1601
22116 .value)))
22117 + (x1602
22118 .value));
22119 sj4array[0] = IKsin(
22120 j4array
22121 [0]);
22122 cj4array[0] = IKcos(
22123 j4array
22124 [0]);
22125 if (j4array
22126 [0]
22127 > IKPI)
22128 {
22129 j4array
22130 [0]
22131 -= IK2PI;
22132 }
22133 else if (
22134 j4array
22135 [0]
22136 < -IKPI)
22137 {
22138 j4array
22139 [0]
22140 += IK2PI;
22141 }
22142 j4valid[0]
22143 = true;
22144 for (
22145 int ij4
22146 = 0;
22147 ij4
22148 < 1;
22149 ++ij4)
22150 {
22151 if (!j4valid
22152 [ij4])
22153 {
22154 continue;
22155 }
22156 _ij4[0]
22157 = ij4;
22158 _ij4[1]
22159 = -1;
22160 for (
22161 int iij4
22162 = ij4
22163 + 1;
22164 iij4
22165 < 1;
22166 ++iij4)
22167 {
22168 if (j4valid
22169 [iij4]
22170 && IKabs(
22171 cj4array
22172 [ij4]
22173 - cj4array
22174 [iij4])
22175 < IKFAST_SOLUTION_THRESH
22176 && IKabs(
22177 sj4array
22178 [ij4]
22179 - sj4array
22180 [iij4])
22181 < IKFAST_SOLUTION_THRESH)
22182 {
22183 j4valid
22184 [iij4]
22185 = false;
22186 _ij4[1]
22187 = iij4;
22188 break;
22189 }
22190 }
22191 j4 = j4array
22192 [ij4];
22193 cj4 = cj4array
22194 [ij4];
22195 sj4 = sj4array
22196 [ij4];
22197 {
22198 IkReal evalcond
22199 [3];
22200 IkReal
22201 x1603
22202 = IKcos(
22203 j4);
22204 IkReal
22205 x1604
22206 = ((1.0)
22207 * x1603);
22208 IkReal
22209 x1605
22210 = IKsin(
22211 j4);
22212 IkReal
22213 x1606
22214 = (px
22215 * x1605);
22216 evalcond
22217 [0]
22218 = ((((-1.0)
22219 * px
22220 * x1604))
22221 + (((-1.0)
22222 * py
22223 * x1605)));
22224 evalcond
22225 [1]
22226 = ((-1.51009803921569)
22227 + (((-1.0)
22228 * (1.32323529411765)
22229 * cj9))
22230 + (((3.92156862745098)
22231 * pp))
22232 + x1606
22233 + (((-1.0)
22234 * py
22235 * x1604)));
22236 evalcond
22237 [2]
22238 = ((-0.2125)
22239 + (((-1.0)
22240 * (1.1)
22241 * pz))
22242 + (((-0.09)
22243 * x1606))
22244 + (((-1.0)
22245 * (1.0)
22246 * pp))
22247 + (((0.09)
22248 * py
22249 * x1603)));
22250 if (IKabs(
22251 evalcond
22252 [0])
22253 > IKFAST_EVALCOND_THRESH
22254 || IKabs(
22255 evalcond
22256 [1])
22257 > IKFAST_EVALCOND_THRESH
22258 || IKabs(
22259 evalcond
22260 [2])
22261 > IKFAST_EVALCOND_THRESH)
22262 {
22263 continue;
22264 }
22265 }
22266
22267 rotationfunction0(
22268 solutions);
22269 }
22270 }
22271 }
22272 }
22273 }
22274 } while (0);
22275 if (bgotonextstatement)
22276 {
22277 bool
22278 bgotonextstatement
22279 = true;
22280 do
22281 {
22282 evalcond[0]
22283 = ((-3.14159265358979)
22284 + (IKfmod(
22285 ((3.14159265358979)
22286 + (IKabs((
22287 (1.5707963267949)
22288 + j8)))),
22289 6.28318530717959)));
22290 if (IKabs(
22291 evalcond
22292 [0])
22293 < 0.0000010000000000)
22294 {
22295 bgotonextstatement
22296 = false;
22297 {
22298 IkReal
22299 j4eval
22300 [3];
22301 sj6 = 0;
22302 cj6 = -1.0;
22303 j6 = 3.14159265358979;
22304 sj8 = -1.0;
22305 cj8 = 0;
22306 j8 = -1.5707963267949;
22307 IkReal x1607
22308 = (pp
22309 + (((-1.0)
22310 * (1.0)
22311 * (pz
22312 * pz))));
22313 IkReal x1608
22314 = ((13497.0)
22315 * cj9);
22316 IkReal x1609
22317 = ((40000.0)
22318 * pp);
22319 j4eval[0]
22320 = x1607;
22321 j4eval[1]
22322 = ((IKabs((
22323 (((-1.0)
22324 * py
22325 * x1609))
22326 + (((15403.0)
22327 * py))
22328 + ((py
22329 * x1608)))))
22330 + (IKabs((
22331 (((-1.0)
22332 * px
22333 * x1608))
22334 + (((-1.0)
22335 * (15403.0)
22336 * px))
22337 + ((px
22338 * x1609))))));
22339 j4eval[2]
22340 = IKsign(
22341 x1607);
22342 if (IKabs(
22343 j4eval
22344 [0])
22345 < 0.0000010000000000
22346 || IKabs(
22347 j4eval
22348 [1])
22349 < 0.0000010000000000
22350 || IKabs(
22351 j4eval
22352 [2])
22353 < 0.0000010000000000)
22354 {
22355 {
22356 IkReal j4eval
22357 [3];
22358 sj6 = 0;
22359 cj6 = -1.0;
22360 j6 = 3.14159265358979;
22361 sj8 = -1.0;
22362 cj8 = 0;
22363 j8 = -1.5707963267949;
22364 IkReal
22365 x1610
22366 = pz
22367 * pz;
22368 IkReal
22369 x1611
22370 = ((80.0)
22371 * pp);
22372 IkReal
22373 x1612
22374 = ((88.0)
22375 * pz);
22376 j4eval
22377 [0]
22378 = (pp
22379 + (((-1.0)
22380 * x1610)));
22381 j4eval
22382 [1]
22383 = ((IKabs((
22384 (((17.0)
22385 * py))
22386 + ((py
22387 * x1612))
22388 + ((py
22389 * x1611)))))
22390 + (IKabs((
22391 (((17.0)
22392 * px))
22393 + ((px
22394 * x1612))
22395 + ((px
22396 * x1611))))));
22397 j4eval[2] = IKsign((
22398 (((-9.0)
22399 * x1610))
22400 + (((9.0)
22401 * pp))));
22402 if (IKabs(
22403 j4eval
22404 [0])
22405 < 0.0000010000000000
22406 || IKabs(
22407 j4eval
22408 [1])
22409 < 0.0000010000000000
22410 || IKabs(
22411 j4eval
22412 [2])
22413 < 0.0000010000000000)
22414 {
22415 {
22416 IkReal evalcond
22417 [5];
22418 bool
22419 bgotonextstatement
22420 = true;
22421 do
22422 {
22423 IkReal
22424 x1613
22425 = ((1.32323529411765)
22426 * cj9);
22427 IkReal
22428 x1614
22429 = ((3.92156862745098)
22430 * pp);
22431 evalcond
22432 [0]
22433 = ((IKabs(
22434 py))
22435 + (IKabs(
22436 px)));
22437 evalcond
22438 [1]
22439 = ((1.51009803921569)
22440 + (((-1.0)
22441 * x1614))
22442 + x1613);
22443 evalcond
22444 [2]
22445 = 0;
22446 evalcond
22447 [3]
22448 = ((-1.51009803921569)
22449 + (((-1.0)
22450 * x1613))
22451 + x1614);
22452 evalcond
22453 [4]
22454 = ((-0.2125)
22455 + (((-1.0)
22456 * (1.1)
22457 * pz))
22458 + (((-1.0)
22459 * (1.0)
22460 * pp)));
22461 if (IKabs(
22462 evalcond
22463 [0])
22464 < 0.0000010000000000
22465 && IKabs(
22466 evalcond
22467 [1])
22468 < 0.0000010000000000
22469 && IKabs(
22470 evalcond
22471 [2])
22472 < 0.0000010000000000
22473 && IKabs(
22474 evalcond
22475 [3])
22476 < 0.0000010000000000
22477 && IKabs(
22478 evalcond
22479 [4])
22480 < 0.0000010000000000)
22481 {
22482 bgotonextstatement
22483 = false;
22484 {
22485 IkReal j4array
22486 [4],
22487 cj4array
22488 [4],
22489 sj4array
22490 [4];
22491 bool j4valid
22492 [4]
22493 = {false};
22494 _nj4
22495 = 4;
22496 j4array
22497 [0]
22498 = 0;
22499 sj4array
22500 [0]
22501 = IKsin(
22502 j4array
22503 [0]);
22504 cj4array
22505 [0]
22506 = IKcos(
22507 j4array
22508 [0]);
22509 j4array
22510 [1]
22511 = 1.5707963267949;
22512 sj4array
22513 [1]
22514 = IKsin(
22515 j4array
22516 [1]);
22517 cj4array
22518 [1]
22519 = IKcos(
22520 j4array
22521 [1]);
22522 j4array
22523 [2]
22524 = 3.14159265358979;
22525 sj4array
22526 [2]
22527 = IKsin(
22528 j4array
22529 [2]);
22530 cj4array
22531 [2]
22532 = IKcos(
22533 j4array
22534 [2]);
22535 j4array
22536 [3]
22537 = -1.5707963267949;
22538 sj4array
22539 [3]
22540 = IKsin(
22541 j4array
22542 [3]);
22543 cj4array
22544 [3]
22545 = IKcos(
22546 j4array
22547 [3]);
22548 if (j4array
22549 [0]
22550 > IKPI)
22551 {
22552 j4array
22553 [0]
22554 -= IK2PI;
22555 }
22556 else if (
22557 j4array
22558 [0]
22559 < -IKPI)
22560 {
22561 j4array
22562 [0]
22563 += IK2PI;
22564 }
22565 j4valid
22566 [0]
22567 = true;
22568 if (j4array
22569 [1]
22570 > IKPI)
22571 {
22572 j4array
22573 [1]
22574 -= IK2PI;
22575 }
22576 else if (
22577 j4array
22578 [1]
22579 < -IKPI)
22580 {
22581 j4array
22582 [1]
22583 += IK2PI;
22584 }
22585 j4valid
22586 [1]
22587 = true;
22588 if (j4array
22589 [2]
22590 > IKPI)
22591 {
22592 j4array
22593 [2]
22594 -= IK2PI;
22595 }
22596 else if (
22597 j4array
22598 [2]
22599 < -IKPI)
22600 {
22601 j4array
22602 [2]
22603 += IK2PI;
22604 }
22605 j4valid
22606 [2]
22607 = true;
22608 if (j4array
22609 [3]
22610 > IKPI)
22611 {
22612 j4array
22613 [3]
22614 -= IK2PI;
22615 }
22616 else if (
22617 j4array
22618 [3]
22619 < -IKPI)
22620 {
22621 j4array
22622 [3]
22623 += IK2PI;
22624 }
22625 j4valid
22626 [3]
22627 = true;
22628 for (
22629 int ij4
22630 = 0;
22631 ij4
22632 < 4;
22633 ++ij4)
22634 {
22635 if (!j4valid
22636 [ij4])
22637 {
22638 continue;
22639 }
22640 _ij4[0]
22641 = ij4;
22642 _ij4[1]
22643 = -1;
22644 for (
22645 int iij4
22646 = ij4
22647 + 1;
22648 iij4
22649 < 4;
22650 ++iij4)
22651 {
22652 if (j4valid
22653 [iij4]
22654 && IKabs(
22655 cj4array
22656 [ij4]
22657 - cj4array
22658 [iij4])
22659 < IKFAST_SOLUTION_THRESH
22660 && IKabs(
22661 sj4array
22662 [ij4]
22663 - sj4array
22664 [iij4])
22665 < IKFAST_SOLUTION_THRESH)
22666 {
22667 j4valid
22668 [iij4]
22669 = false;
22670 _ij4[1]
22671 = iij4;
22672 break;
22673 }
22674 }
22675 j4 = j4array
22676 [ij4];
22677 cj4 = cj4array
22678 [ij4];
22679 sj4 = sj4array
22680 [ij4];
22681
22682 rotationfunction0(
22683 solutions);
22684 }
22685 }
22686 }
22687 } while (
22688 0);
22689 if (bgotonextstatement)
22690 {
22691 bool
22692 bgotonextstatement
22693 = true;
22694 do
22695 {
22696 if (1)
22697 {
22698 bgotonextstatement
22699 = false;
22700 continue; // branch miss [j4]
22701 }
22702 } while (
22703 0);
22704 if (bgotonextstatement)
22705 {
22706 }
22707 }
22708 }
22709 }
22710 else
22711 {
22712 {
22713 IkReal j4array
22714 [1],
22715 cj4array
22716 [1],
22717 sj4array
22718 [1];
22719 bool j4valid
22720 [1]
22721 = {false};
22722 _nj4
22723 = 1;
22724 IkReal
22725 x1615
22726 = ((100.0)
22727 * pp);
22728 IkReal
22729 x1616
22730 = ((110.0)
22731 * pz);
22732 CheckValue<IkReal> x1617 = IKatan2WithCheck(
22733 IkReal((
22734 ((px
22735 * x1616))
22736 + (((21.25)
22737 * px))
22738 + ((px
22739 * x1615)))),
22740 ((((-1.0)
22741 * py
22742 * x1616))
22743 + (((-1.0)
22744 * (21.25)
22745 * py))
22746 + (((-1.0)
22747 * py
22748 * x1615))),
22749 IKFAST_ATAN2_MAGTHRESH);
22750 if (!x1617
22751 .valid)
22752 {
22753 continue;
22754 }
22755 CheckValue<IkReal> x1618 = IKPowWithIntegerCheck(
22756 IKsign((
22757 (((-1.0)
22758 * (9.0)
22759 * (pz
22760 * pz)))
22761 + (((9.0)
22762 * pp)))),
22763 -1);
22764 if (!x1618
22765 .valid)
22766 {
22767 continue;
22768 }
22769 j4array
22770 [0]
22771 = ((-1.5707963267949)
22772 + (x1617
22773 .value)
22774 + (((1.5707963267949)
22775 * (x1618
22776 .value))));
22777 sj4array
22778 [0]
22779 = IKsin(
22780 j4array
22781 [0]);
22782 cj4array
22783 [0]
22784 = IKcos(
22785 j4array
22786 [0]);
22787 if (j4array
22788 [0]
22789 > IKPI)
22790 {
22791 j4array
22792 [0]
22793 -= IK2PI;
22794 }
22795 else if (
22796 j4array
22797 [0]
22798 < -IKPI)
22799 {
22800 j4array
22801 [0]
22802 += IK2PI;
22803 }
22804 j4valid
22805 [0]
22806 = true;
22807 for (
22808 int ij4
22809 = 0;
22810 ij4
22811 < 1;
22812 ++ij4)
22813 {
22814 if (!j4valid
22815 [ij4])
22816 {
22817 continue;
22818 }
22819 _ij4[0]
22820 = ij4;
22821 _ij4[1]
22822 = -1;
22823 for (
22824 int iij4
22825 = ij4
22826 + 1;
22827 iij4
22828 < 1;
22829 ++iij4)
22830 {
22831 if (j4valid
22832 [iij4]
22833 && IKabs(
22834 cj4array
22835 [ij4]
22836 - cj4array
22837 [iij4])
22838 < IKFAST_SOLUTION_THRESH
22839 && IKabs(
22840 sj4array
22841 [ij4]
22842 - sj4array
22843 [iij4])
22844 < IKFAST_SOLUTION_THRESH)
22845 {
22846 j4valid
22847 [iij4]
22848 = false;
22849 _ij4[1]
22850 = iij4;
22851 break;
22852 }
22853 }
22854 j4 = j4array
22855 [ij4];
22856 cj4 = cj4array
22857 [ij4];
22858 sj4 = sj4array
22859 [ij4];
22860 {
22861 IkReal evalcond
22862 [3];
22863 IkReal
22864 x1619
22865 = IKcos(
22866 j4);
22867 IkReal
22868 x1620
22869 = IKsin(
22870 j4);
22871 IkReal
22872 x1621
22873 = (px
22874 * x1620);
22875 IkReal
22876 x1622
22877 = (py
22878 * x1619);
22879 evalcond
22880 [0]
22881 = (((px
22882 * x1619))
22883 + ((py
22884 * x1620)));
22885 evalcond
22886 [1]
22887 = ((1.51009803921569)
22888 + (((-1.0)
22889 * x1622))
22890 + (((-1.0)
22891 * (3.92156862745098)
22892 * pp))
22893 + x1621
22894 + (((1.32323529411765)
22895 * cj9)));
22896 evalcond
22897 [2]
22898 = ((-0.2125)
22899 + (((-1.0)
22900 * (1.1)
22901 * pz))
22902 + (((0.09)
22903 * x1621))
22904 + (((-0.09)
22905 * x1622))
22906 + (((-1.0)
22907 * (1.0)
22908 * pp)));
22909 if (IKabs(
22910 evalcond
22911 [0])
22912 > IKFAST_EVALCOND_THRESH
22913 || IKabs(
22914 evalcond
22915 [1])
22916 > IKFAST_EVALCOND_THRESH
22917 || IKabs(
22918 evalcond
22919 [2])
22920 > IKFAST_EVALCOND_THRESH)
22921 {
22922 continue;
22923 }
22924 }
22925
22926 rotationfunction0(
22927 solutions);
22928 }
22929 }
22930 }
22931 }
22932 }
22933 else
22934 {
22935 {
22936 IkReal j4array
22937 [1],
22938 cj4array
22939 [1],
22940 sj4array
22941 [1];
22942 bool j4valid
22943 [1]
22944 = {false};
22945 _nj4
22946 = 1;
22947 IkReal
22948 x1623
22949 = ((1.32323529411765)
22950 * cj9);
22951 IkReal
22952 x1624
22953 = ((3.92156862745098)
22954 * pp);
22955 CheckValue<IkReal> x1625 = IKatan2WithCheck(
22956 IkReal((
22957 (((-1.0)
22958 * (1.51009803921569)
22959 * px))
22960 + (((-1.0)
22961 * px
22962 * x1623))
22963 + ((px
22964 * x1624)))),
22965 (((py
22966 * x1623))
22967 + (((1.51009803921569)
22968 * py))
22969 + (((-1.0)
22970 * py
22971 * x1624))),
22972 IKFAST_ATAN2_MAGTHRESH);
22973 if (!x1625
22974 .valid)
22975 {
22976 continue;
22977 }
22978 CheckValue<IkReal> x1626 = IKPowWithIntegerCheck(
22979 IKsign((
22980 pp
22981 + (((-1.0)
22982 * (1.0)
22983 * (pz
22984 * pz))))),
22985 -1);
22986 if (!x1626
22987 .valid)
22988 {
22989 continue;
22990 }
22991 j4array
22992 [0]
22993 = ((-1.5707963267949)
22994 + (x1625
22995 .value)
22996 + (((1.5707963267949)
22997 * (x1626
22998 .value))));
22999 sj4array
23000 [0]
23001 = IKsin(
23002 j4array
23003 [0]);
23004 cj4array
23005 [0]
23006 = IKcos(
23007 j4array
23008 [0]);
23009 if (j4array
23010 [0]
23011 > IKPI)
23012 {
23013 j4array
23014 [0]
23015 -= IK2PI;
23016 }
23017 else if (
23018 j4array
23019 [0]
23020 < -IKPI)
23021 {
23022 j4array
23023 [0]
23024 += IK2PI;
23025 }
23026 j4valid
23027 [0]
23028 = true;
23029 for (
23030 int ij4
23031 = 0;
23032 ij4
23033 < 1;
23034 ++ij4)
23035 {
23036 if (!j4valid
23037 [ij4])
23038 {
23039 continue;
23040 }
23041 _ij4[0]
23042 = ij4;
23043 _ij4[1]
23044 = -1;
23045 for (
23046 int iij4
23047 = ij4
23048 + 1;
23049 iij4
23050 < 1;
23051 ++iij4)
23052 {
23053 if (j4valid
23054 [iij4]
23055 && IKabs(
23056 cj4array
23057 [ij4]
23058 - cj4array
23059 [iij4])
23060 < IKFAST_SOLUTION_THRESH
23061 && IKabs(
23062 sj4array
23063 [ij4]
23064 - sj4array
23065 [iij4])
23066 < IKFAST_SOLUTION_THRESH)
23067 {
23068 j4valid
23069 [iij4]
23070 = false;
23071 _ij4[1]
23072 = iij4;
23073 break;
23074 }
23075 }
23076 j4 = j4array
23077 [ij4];
23078 cj4 = cj4array
23079 [ij4];
23080 sj4 = sj4array
23081 [ij4];
23082 {
23083 IkReal evalcond
23084 [3];
23085 IkReal
23086 x1627
23087 = IKcos(
23088 j4);
23089 IkReal
23090 x1628
23091 = IKsin(
23092 j4);
23093 IkReal
23094 x1629
23095 = (px
23096 * x1628);
23097 IkReal
23098 x1630
23099 = (py
23100 * x1627);
23101 evalcond
23102 [0]
23103 = (((py
23104 * x1628))
23105 + ((px
23106 * x1627)));
23107 evalcond
23108 [1]
23109 = ((1.51009803921569)
23110 + (((-1.0)
23111 * (3.92156862745098)
23112 * pp))
23113 + (((-1.0)
23114 * x1630))
23115 + x1629
23116 + (((1.32323529411765)
23117 * cj9)));
23118 evalcond
23119 [2]
23120 = ((-0.2125)
23121 + (((-1.0)
23122 * (1.1)
23123 * pz))
23124 + (((0.09)
23125 * x1629))
23126 + (((-0.09)
23127 * x1630))
23128 + (((-1.0)
23129 * (1.0)
23130 * pp)));
23131 if (IKabs(
23132 evalcond
23133 [0])
23134 > IKFAST_EVALCOND_THRESH
23135 || IKabs(
23136 evalcond
23137 [1])
23138 > IKFAST_EVALCOND_THRESH
23139 || IKabs(
23140 evalcond
23141 [2])
23142 > IKFAST_EVALCOND_THRESH)
23143 {
23144 continue;
23145 }
23146 }
23147
23148 rotationfunction0(
23149 solutions);
23150 }
23151 }
23152 }
23153 }
23154 }
23155 } while (0);
23156 if (bgotonextstatement)
23157 {
23158 bool
23159 bgotonextstatement
23160 = true;
23161 do
23162 {
23163 IkReal x1631
23164 = ((1.32323529411765)
23165 * cj9);
23166 IkReal x1632
23167 = ((3.92156862745098)
23168 * pp);
23169 evalcond[0]
23170 = ((IKabs(
23171 py))
23172 + (IKabs(
23173 px)));
23174 evalcond[1]
23175 = ((((-1.0)
23176 * sj8
23177 * x1631))
23178 + ((sj8
23179 * x1632))
23180 + (((-1.0)
23181 * (1.51009803921569)
23182 * sj8)));
23183 evalcond[2]
23184 = 0;
23185 evalcond[3]
23186 = ((-1.51009803921569)
23187 + (((-1.0)
23188 * x1631))
23189 + x1632);
23190 evalcond[4]
23191 = ((((1.51009803921569)
23192 * cj8))
23193 + (((-1.0)
23194 * cj8
23195 * x1632))
23196 + ((cj8
23197 * x1631)));
23198 evalcond[5]
23199 = ((-0.2125)
23200 + (((-1.0)
23201 * (1.1)
23202 * pz))
23203 + (((-1.0)
23204 * (1.0)
23205 * pp)));
23206 if (IKabs(
23207 evalcond
23208 [0])
23209 < 0.0000010000000000
23210 && IKabs(
23211 evalcond
23212 [1])
23213 < 0.0000010000000000
23214 && IKabs(
23215 evalcond
23216 [2])
23217 < 0.0000010000000000
23218 && IKabs(
23219 evalcond
23220 [3])
23221 < 0.0000010000000000
23222 && IKabs(
23223 evalcond
23224 [4])
23225 < 0.0000010000000000
23226 && IKabs(
23227 evalcond
23228 [5])
23229 < 0.0000010000000000)
23230 {
23231 bgotonextstatement
23232 = false;
23233 {
23234 IkReal j4array
23235 [4],
23236 cj4array
23237 [4],
23238 sj4array
23239 [4];
23240 bool j4valid
23241 [4]
23242 = {false};
23243 _nj4 = 4;
23244 j4array[0]
23245 = 0;
23246 sj4array[0] = IKsin(
23247 j4array
23248 [0]);
23249 cj4array[0] = IKcos(
23250 j4array
23251 [0]);
23252 j4array[1]
23253 = 1.5707963267949;
23254 sj4array[1] = IKsin(
23255 j4array
23256 [1]);
23257 cj4array[1] = IKcos(
23258 j4array
23259 [1]);
23260 j4array[2]
23261 = 3.14159265358979;
23262 sj4array[2] = IKsin(
23263 j4array
23264 [2]);
23265 cj4array[2] = IKcos(
23266 j4array
23267 [2]);
23268 j4array[3]
23269 = -1.5707963267949;
23270 sj4array[3] = IKsin(
23271 j4array
23272 [3]);
23273 cj4array[3] = IKcos(
23274 j4array
23275 [3]);
23276 if (j4array
23277 [0]
23278 > IKPI)
23279 {
23280 j4array
23281 [0]
23282 -= IK2PI;
23283 }
23284 else if (
23285 j4array
23286 [0]
23287 < -IKPI)
23288 {
23289 j4array
23290 [0]
23291 += IK2PI;
23292 }
23293 j4valid[0]
23294 = true;
23295 if (j4array
23296 [1]
23297 > IKPI)
23298 {
23299 j4array
23300 [1]
23301 -= IK2PI;
23302 }
23303 else if (
23304 j4array
23305 [1]
23306 < -IKPI)
23307 {
23308 j4array
23309 [1]
23310 += IK2PI;
23311 }
23312 j4valid[1]
23313 = true;
23314 if (j4array
23315 [2]
23316 > IKPI)
23317 {
23318 j4array
23319 [2]
23320 -= IK2PI;
23321 }
23322 else if (
23323 j4array
23324 [2]
23325 < -IKPI)
23326 {
23327 j4array
23328 [2]
23329 += IK2PI;
23330 }
23331 j4valid[2]
23332 = true;
23333 if (j4array
23334 [3]
23335 > IKPI)
23336 {
23337 j4array
23338 [3]
23339 -= IK2PI;
23340 }
23341 else if (
23342 j4array
23343 [3]
23344 < -IKPI)
23345 {
23346 j4array
23347 [3]
23348 += IK2PI;
23349 }
23350 j4valid[3]
23351 = true;
23352 for (
23353 int ij4
23354 = 0;
23355 ij4
23356 < 4;
23357 ++ij4)
23358 {
23359 if (!j4valid
23360 [ij4])
23361 {
23362 continue;
23363 }
23364 _ij4[0]
23365 = ij4;
23366 _ij4[1]
23367 = -1;
23368 for (
23369 int iij4
23370 = ij4
23371 + 1;
23372 iij4
23373 < 4;
23374 ++iij4)
23375 {
23376 if (j4valid
23377 [iij4]
23378 && IKabs(
23379 cj4array
23380 [ij4]
23381 - cj4array
23382 [iij4])
23383 < IKFAST_SOLUTION_THRESH
23384 && IKabs(
23385 sj4array
23386 [ij4]
23387 - sj4array
23388 [iij4])
23389 < IKFAST_SOLUTION_THRESH)
23390 {
23391 j4valid
23392 [iij4]
23393 = false;
23394 _ij4[1]
23395 = iij4;
23396 break;
23397 }
23398 }
23399 j4 = j4array
23400 [ij4];
23401 cj4 = cj4array
23402 [ij4];
23403 sj4 = sj4array
23404 [ij4];
23405
23406 rotationfunction0(
23407 solutions);
23408 }
23409 }
23410 }
23411 } while (0);
23412 if (bgotonextstatement)
23413 {
23414 bool
23415 bgotonextstatement
23416 = true;
23417 do
23418 {
23419 if (1)
23420 {
23421 bgotonextstatement
23422 = false;
23423 continue; // branch miss [j4]
23424 }
23425 } while (0);
23426 if (bgotonextstatement)
23427 {
23428 }
23429 }
23430 }
23431 }
23432 }
23433 }
23434 else
23435 {
23436 {
23437 IkReal j4array[1],
23438 cj4array[1],
23439 sj4array[1];
23440 bool j4valid[1]
23441 = {false};
23442 _nj4 = 1;
23443 IkReal x1633
23444 = ((1.51009803921569)
23445 * cj8 * sj8);
23446 IkReal x1634
23447 = cj8 * cj8;
23448 IkReal x1635
23449 = ((1.51009803921569)
23450 * x1634);
23451 IkReal x1636
23452 = ((1.32323529411765)
23453 * cj8 * cj9
23454 * sj8);
23455 IkReal x1637
23456 = ((3.92156862745098)
23457 * cj8 * pp
23458 * sj8);
23459 IkReal x1638
23460 = ((1.32323529411765)
23461 * cj9
23462 * x1634);
23463 IkReal x1639
23464 = ((3.92156862745098)
23465 * pp
23466 * x1634);
23467 CheckValue<IkReal> x1640 = IKatan2WithCheck(
23468 IkReal((
23469 (((-1.0)
23470 * px
23471 * x1637))
23472 + ((py
23473 * x1635))
23474 + ((py
23475 * x1638))
23476 + ((px
23477 * x1633))
23478 + ((px
23479 * x1636))
23480 + (((-1.0)
23481 * py
23482 * x1639)))),
23483 (((px * x1638))
23484 + ((py
23485 * x1637))
23486 + (((-1.0) * px
23487 * x1639))
23488 + (((-1.0) * py
23489 * x1633))
23490 + ((px
23491 * x1635))
23492 + (((-1.0) * py
23493 * x1636))),
23494 IKFAST_ATAN2_MAGTHRESH);
23495 if (!x1640.valid)
23496 {
23497 continue;
23498 }
23499 CheckValue<IkReal>
23500 x1641
23501 = IKPowWithIntegerCheck(
23502 IKsign((
23503 (((-1.0)
23504 * (1.0)
23505 * cj8
23506 * (pz
23507 * pz)))
23508 + ((cj8
23509 * pp)))),
23510 -1);
23511 if (!x1641.valid)
23512 {
23513 continue;
23514 }
23515 j4array[0]
23516 = ((-1.5707963267949)
23517 + (x1640
23518 .value)
23519 + (((1.5707963267949)
23520 * (x1641
23521 .value))));
23522 sj4array[0] = IKsin(
23523 j4array[0]);
23524 cj4array[0] = IKcos(
23525 j4array[0]);
23526 if (j4array[0]
23527 > IKPI)
23528 {
23529 j4array[0]
23530 -= IK2PI;
23531 }
23532 else if (
23533 j4array[0]
23534 < -IKPI)
23535 {
23536 j4array[0]
23537 += IK2PI;
23538 }
23539 j4valid[0] = true;
23540 for (int ij4 = 0;
23541 ij4 < 1;
23542 ++ij4)
23543 {
23544 if (!j4valid[ij4])
23545 {
23546 continue;
23547 }
23548 _ij4[0] = ij4;
23549 _ij4[1] = -1;
23550 for (int iij4
23551 = ij4 + 1;
23552 iij4 < 1;
23553 ++iij4)
23554 {
23555 if (j4valid
23556 [iij4]
23557 && IKabs(
23558 cj4array
23559 [ij4]
23560 - cj4array
23561 [iij4])
23562 < IKFAST_SOLUTION_THRESH
23563 && IKabs(
23564 sj4array
23565 [ij4]
23566 - sj4array
23567 [iij4])
23568 < IKFAST_SOLUTION_THRESH)
23569 {
23570 j4valid[iij4]
23571 = false;
23572 _ij4[1]
23573 = iij4;
23574 break;
23575 }
23576 }
23577 j4 = j4array[ij4];
23578 cj4 = cj4array
23579 [ij4];
23580 sj4 = sj4array
23581 [ij4];
23582 {
23583 IkReal
23584 evalcond[5];
23585 IkReal x1642
23586 = ((1.32323529411765)
23587 * cj9);
23588 IkReal x1643
23589 = ((3.92156862745098)
23590 * pp);
23591 IkReal x1644
23592 = IKsin(j4);
23593 IkReal x1645
23594 = (px
23595 * x1644);
23596 IkReal x1646
23597 = IKcos(j4);
23598 IkReal x1647
23599 = ((1.0)
23600 * x1646);
23601 IkReal x1648
23602 = (py
23603 * x1647);
23604 IkReal x1649
23605 = (px
23606 * x1647);
23607 IkReal x1650
23608 = (py
23609 * x1644);
23610 IkReal x1651
23611 = ((1.0)
23612 * x1650);
23613 IkReal x1652
23614 = (cj8 * px
23615 * x1646);
23616 IkReal x1653
23617 = (cj8
23618 * x1650);
23619 IkReal x1654
23620 = (sj8
23621 * x1645);
23622 evalcond[0]
23623 = (((sj8
23624 * x1643))
23625 + (((-1.0)
23626 * sj8
23627 * x1642))
23628 + (((-1.0)
23629 * x1648))
23630 + (((-1.0)
23631 * (1.51009803921569)
23632 * sj8))
23633 + x1645);
23634 evalcond[1]
23635 = (((cj8
23636 * x1642))
23637 + (((1.51009803921569)
23638 * cj8))
23639 + (((-1.0)
23640 * cj8
23641 * x1643))
23642 + (((-1.0)
23643 * x1651))
23644 + (((-1.0)
23645 * x1649)));
23646 evalcond[2]
23647 = ((((-1.0)
23648 * sj8
23649 * x1649))
23650 + (((-1.0)
23651 * sj8
23652 * x1651))
23653 + ((cj8
23654 * x1645))
23655 + (((-1.0)
23656 * cj8
23657 * x1648)));
23658 evalcond[3]
23659 = ((-1.51009803921569)
23660 + (((-1.0)
23661 * sj8
23662 * x1648))
23663 + x1654
23664 + x1652
23665 + x1653
23666 + (((-1.0)
23667 * x1642))
23668 + x1643);
23669 evalcond[4]
23670 = ((-0.2125)
23671 + (((-1.0)
23672 * (1.1)
23673 * pz))
23674 + (((-0.09)
23675 * x1654))
23676 + (((0.09)
23677 * py
23678 * sj8
23679 * x1646))
23680 + (((-1.0)
23681 * (1.0)
23682 * pp))
23683 + (((-0.09)
23684 * x1652))
23685 + (((-0.09)
23686 * x1653)));
23687 if (IKabs(
23688 evalcond
23689 [0])
23690 > IKFAST_EVALCOND_THRESH
23691 || IKabs(
23692 evalcond
23693 [1])
23694 > IKFAST_EVALCOND_THRESH
23695 || IKabs(
23696 evalcond
23697 [2])
23698 > IKFAST_EVALCOND_THRESH
23699 || IKabs(
23700 evalcond
23701 [3])
23702 > IKFAST_EVALCOND_THRESH
23703 || IKabs(
23704 evalcond
23705 [4])
23706 > IKFAST_EVALCOND_THRESH)
23707 {
23708 continue;
23709 }
23710 }
23711
23712 rotationfunction0(
23713 solutions);
23714 }
23715 }
23716 }
23717 }
23718 }
23719 else
23720 {
23721 {
23722 IkReal j4array[1],
23723 cj4array[1],
23724 sj4array[1];
23725 bool j4valid[1]
23726 = {false};
23727 _nj4 = 1;
23728 IkReal x1655
23729 = ((1.51009803921569)
23730 * cj8 * sj8);
23731 IkReal x1656
23732 = cj8 * cj8;
23733 IkReal x1657
23734 = ((1.51009803921569)
23735 * x1656);
23736 IkReal x1658
23737 = ((1.32323529411765)
23738 * cj8 * cj9
23739 * sj8);
23740 IkReal x1659
23741 = ((3.92156862745098)
23742 * cj8 * pp
23743 * sj8);
23744 IkReal x1660
23745 = ((1.32323529411765)
23746 * cj9 * x1656);
23747 IkReal x1661
23748 = ((3.92156862745098)
23749 * pp * x1656);
23750 CheckValue<IkReal> x1662
23751 = IKPowWithIntegerCheck(
23752 IKsign((
23753 ((cj8
23754 * (pz
23755 * pz)))
23756 + (((-1.0)
23757 * cj8
23758 * pp)))),
23759 -1);
23760 if (!x1662.valid)
23761 {
23762 continue;
23763 }
23764 CheckValue<IkReal> x1663
23765 = IKatan2WithCheck(
23766 IkReal((
23767 (((-1.0)
23768 * px
23769 * x1658))
23770 + ((py
23771 * x1661))
23772 + ((px
23773 * x1659))
23774 + (((-1.0)
23775 * py
23776 * x1660))
23777 + (((-1.0)
23778 * px
23779 * x1655))
23780 + (((-1.0)
23781 * py
23782 * x1657)))),
23783 ((((-1.0) * px
23784 * x1660))
23785 + ((py
23786 * x1658))
23787 + (((-1.0) * py
23788 * x1659))
23789 + ((px
23790 * x1661))
23791 + (((-1.0) * px
23792 * x1657))
23793 + ((py
23794 * x1655))),
23795 IKFAST_ATAN2_MAGTHRESH);
23796 if (!x1663.valid)
23797 {
23798 continue;
23799 }
23800 j4array[0]
23801 = ((-1.5707963267949)
23802 + (((1.5707963267949)
23803 * (x1662
23804 .value)))
23805 + (x1663.value));
23806 sj4array[0]
23807 = IKsin(j4array[0]);
23808 cj4array[0]
23809 = IKcos(j4array[0]);
23810 if (j4array[0] > IKPI)
23811 {
23812 j4array[0] -= IK2PI;
23813 }
23814 else if (
23815 j4array[0] < -IKPI)
23816 {
23817 j4array[0] += IK2PI;
23818 }
23819 j4valid[0] = true;
23820 for (int ij4 = 0;
23821 ij4 < 1;
23822 ++ij4)
23823 {
23824 if (!j4valid[ij4])
23825 {
23826 continue;
23827 }
23828 _ij4[0] = ij4;
23829 _ij4[1] = -1;
23830 for (int iij4
23831 = ij4 + 1;
23832 iij4 < 1;
23833 ++iij4)
23834 {
23835 if (j4valid[iij4]
23836 && IKabs(
23837 cj4array
23838 [ij4]
23839 - cj4array
23840 [iij4])
23841 < IKFAST_SOLUTION_THRESH
23842 && IKabs(
23843 sj4array
23844 [ij4]
23845 - sj4array
23846 [iij4])
23847 < IKFAST_SOLUTION_THRESH)
23848 {
23849 j4valid[iij4]
23850 = false;
23851 _ij4[1] = iij4;
23852 break;
23853 }
23854 }
23855 j4 = j4array[ij4];
23856 cj4 = cj4array[ij4];
23857 sj4 = sj4array[ij4];
23858 {
23859 IkReal evalcond[5];
23860 IkReal x1664
23861 = ((1.32323529411765)
23862 * cj9);
23863 IkReal x1665
23864 = ((3.92156862745098)
23865 * pp);
23866 IkReal x1666
23867 = IKsin(j4);
23868 IkReal x1667
23869 = (px * x1666);
23870 IkReal x1668
23871 = IKcos(j4);
23872 IkReal x1669
23873 = ((1.0)
23874 * x1668);
23875 IkReal x1670
23876 = (py * x1669);
23877 IkReal x1671
23878 = (px * x1669);
23879 IkReal x1672
23880 = (py * x1666);
23881 IkReal x1673
23882 = ((1.0)
23883 * x1672);
23884 IkReal x1674
23885 = (cj8 * px
23886 * x1668);
23887 IkReal x1675
23888 = (cj8 * x1672);
23889 IkReal x1676
23890 = (sj8 * x1667);
23891 evalcond[0]
23892 = (((sj8
23893 * x1665))
23894 + (((-1.0)
23895 * sj8
23896 * x1664))
23897 + (((-1.0)
23898 * x1670))
23899 + (((-1.0)
23900 * (1.51009803921569)
23901 * sj8))
23902 + x1667);
23903 evalcond[1]
23904 = ((((-1.0)
23905 * cj8
23906 * x1665))
23907 + (((1.51009803921569)
23908 * cj8))
23909 + (((-1.0)
23910 * x1673))
23911 + ((cj8
23912 * x1664))
23913 + (((-1.0)
23914 * x1671)));
23915 evalcond[2]
23916 = (((cj8
23917 * x1667))
23918 + (((-1.0)
23919 * sj8
23920 * x1673))
23921 + (((-1.0)
23922 * cj8
23923 * x1670))
23924 + (((-1.0)
23925 * sj8
23926 * x1671)));
23927 evalcond[3]
23928 = ((-1.51009803921569)
23929 + x1676
23930 + x1674
23931 + x1675
23932 + (((-1.0)
23933 * x1664))
23934 + (((-1.0)
23935 * sj8
23936 * x1670))
23937 + x1665);
23938 evalcond[4]
23939 = ((-0.2125)
23940 + (((-1.0)
23941 * (1.1)
23942 * pz))
23943 + (((0.09)
23944 * py
23945 * sj8
23946 * x1668))
23947 + (((-0.09)
23948 * x1674))
23949 + (((-0.09)
23950 * x1676))
23951 + (((-1.0)
23952 * (1.0)
23953 * pp))
23954 + (((-0.09)
23955 * x1675)));
23956 if (IKabs(
23957 evalcond[0])
23958 > IKFAST_EVALCOND_THRESH
23959 || IKabs(
23960 evalcond
23961 [1])
23962 > IKFAST_EVALCOND_THRESH
23963 || IKabs(
23964 evalcond
23965 [2])
23966 > IKFAST_EVALCOND_THRESH
23967 || IKabs(
23968 evalcond
23969 [3])
23970 > IKFAST_EVALCOND_THRESH
23971 || IKabs(
23972 evalcond
23973 [4])
23974 > IKFAST_EVALCOND_THRESH)
23975 {
23976 continue;
23977 }
23978 }
23979
23980 rotationfunction0(
23981 solutions);
23982 }
23983 }
23984 }
23985 }
23986 }
23987 else
23988 {
23989 {
23990 IkReal j4array[1],
23991 cj4array[1],
23992 sj4array[1];
23993 bool j4valid[1] = {false};
23994 _nj4 = 1;
23995 IkReal x1677
23996 = ((1.51009803921569)
23997 * cj8);
23998 IkReal x1678
23999 = ((1.51009803921569)
24000 * sj8);
24001 IkReal x1679
24002 = ((1.32323529411765)
24003 * cj8 * cj9);
24004 IkReal x1680
24005 = ((3.92156862745098)
24006 * cj8 * pp);
24007 IkReal x1681
24008 = ((1.32323529411765)
24009 * cj9 * sj8);
24010 IkReal x1682
24011 = ((3.92156862745098)
24012 * pp * sj8);
24013 CheckValue<IkReal> x1683
24014 = IKPowWithIntegerCheck(
24015 IKsign((
24016 pp
24017 + (((-1.0)
24018 * (1.0)
24019 * (pz
24020 * pz))))),
24021 -1);
24022 if (!x1683.valid)
24023 {
24024 continue;
24025 }
24026 CheckValue<IkReal> x1684
24027 = IKatan2WithCheck(
24028 IkReal((
24029 ((py * x1677))
24030 + (((-1.0) * px
24031 * x1682))
24032 + ((px * x1678))
24033 + ((py * x1679))
24034 + ((px * x1681))
24035 + (((-1.0) * py
24036 * x1680)))),
24037 ((((-1.0) * py
24038 * x1678))
24039 + ((py * x1682))
24040 + (((-1.0) * px
24041 * x1680))
24042 + ((px * x1679))
24043 + ((px * x1677))
24044 + (((-1.0) * py
24045 * x1681))),
24046 IKFAST_ATAN2_MAGTHRESH);
24047 if (!x1684.valid)
24048 {
24049 continue;
24050 }
24051 j4array[0]
24052 = ((-1.5707963267949)
24053 + (((1.5707963267949)
24054 * (x1683.value)))
24055 + (x1684.value));
24056 sj4array[0]
24057 = IKsin(j4array[0]);
24058 cj4array[0]
24059 = IKcos(j4array[0]);
24060 if (j4array[0] > IKPI)
24061 {
24062 j4array[0] -= IK2PI;
24063 }
24064 else if (j4array[0] < -IKPI)
24065 {
24066 j4array[0] += IK2PI;
24067 }
24068 j4valid[0] = true;
24069 for (int ij4 = 0; ij4 < 1;
24070 ++ij4)
24071 {
24072 if (!j4valid[ij4])
24073 {
24074 continue;
24075 }
24076 _ij4[0] = ij4;
24077 _ij4[1] = -1;
24078 for (int iij4 = ij4 + 1;
24079 iij4 < 1;
24080 ++iij4)
24081 {
24082 if (j4valid[iij4]
24083 && IKabs(
24084 cj4array[ij4]
24085 - cj4array
24086 [iij4])
24087 < IKFAST_SOLUTION_THRESH
24088 && IKabs(
24089 sj4array[ij4]
24090 - sj4array
24091 [iij4])
24092 < IKFAST_SOLUTION_THRESH)
24093 {
24094 j4valid[iij4] = false;
24095 _ij4[1] = iij4;
24096 break;
24097 }
24098 }
24099 j4 = j4array[ij4];
24100 cj4 = cj4array[ij4];
24101 sj4 = sj4array[ij4];
24102 {
24103 IkReal evalcond[5];
24104 IkReal x1685
24105 = ((1.32323529411765)
24106 * cj9);
24107 IkReal x1686
24108 = ((3.92156862745098)
24109 * pp);
24110 IkReal x1687
24111 = IKsin(j4);
24112 IkReal x1688
24113 = (px * x1687);
24114 IkReal x1689
24115 = IKcos(j4);
24116 IkReal x1690
24117 = ((1.0) * x1689);
24118 IkReal x1691
24119 = (py * x1690);
24120 IkReal x1692
24121 = (px * x1690);
24122 IkReal x1693
24123 = (py * x1687);
24124 IkReal x1694
24125 = ((1.0) * x1693);
24126 IkReal x1695
24127 = (cj8 * px
24128 * x1689);
24129 IkReal x1696
24130 = (cj8 * x1693);
24131 IkReal x1697
24132 = (sj8 * x1688);
24133 evalcond[0]
24134 = (x1688
24135 + (((-1.0)
24136 * x1691))
24137 + (((-1.0) * sj8
24138 * x1685))
24139 + ((sj8 * x1686))
24140 + (((-1.0)
24141 * (1.51009803921569)
24142 * sj8)));
24143 evalcond[1]
24144 = (((cj8 * x1685))
24145 + (((-1.0) * cj8
24146 * x1686))
24147 + (((-1.0)
24148 * x1692))
24149 + (((1.51009803921569)
24150 * cj8))
24151 + (((-1.0)
24152 * x1694)));
24153 evalcond[2]
24154 = ((((-1.0) * cj8
24155 * x1691))
24156 + (((-1.0) * sj8
24157 * x1692))
24158 + ((cj8 * x1688))
24159 + (((-1.0) * sj8
24160 * x1694)));
24161 evalcond[3]
24162 = ((-1.51009803921569)
24163 + x1695 + x1696
24164 + x1697 + x1686
24165 + (((-1.0) * sj8
24166 * x1691))
24167 + (((-1.0)
24168 * x1685)));
24169 evalcond[4]
24170 = ((-0.2125)
24171 + (((-1.0)
24172 * (1.1)
24173 * pz))
24174 + (((-0.09)
24175 * x1697))
24176 + (((-0.09)
24177 * x1696))
24178 + (((-0.09)
24179 * x1695))
24180 + (((0.09) * py
24181 * sj8
24182 * x1689))
24183 + (((-1.0)
24184 * (1.0)
24185 * pp)));
24186 if (IKabs(evalcond[0])
24187 > IKFAST_EVALCOND_THRESH
24188 || IKabs(
24189 evalcond[1])
24190 > IKFAST_EVALCOND_THRESH
24191 || IKabs(
24192 evalcond[2])
24193 > IKFAST_EVALCOND_THRESH
24194 || IKabs(
24195 evalcond[3])
24196 > IKFAST_EVALCOND_THRESH
24197 || IKabs(
24198 evalcond[4])
24199 > IKFAST_EVALCOND_THRESH)
24200 {
24201 continue;
24202 }
24203 }
24204
24205 rotationfunction0(
24206 solutions);
24207 }
24208 }
24209 }
24210 }
24211 }
24212 } while (0);
24213 if (bgotonextstatement)
24214 {
24215 bool bgotonextstatement = true;
24216 do
24217 {
24218 if (1)
24219 {
24220 bgotonextstatement = false;
24221 continue; // branch miss [j4]
24222 }
24223 } while (0);
24224 if (bgotonextstatement)
24225 {
24226 }
24227 }
24228 }
24229 }
24230 }
24231 }
24232 }
24233 else
24234 {
24235 {
24236 IkReal j4array[1], cj4array[1], sj4array[1];
24237 bool j4valid[1] = {false};
24238 _nj4 = 1;
24239 IkReal x1698 = ((0.045) * sj8);
24240 IkReal x1699 = (px * x1698);
24241 IkReal x1700 = ((0.55) * sj6);
24242 IkReal x1701 = ((0.045) * cj6 * cj8);
24243 IkReal x1702 = (py * x1701);
24244 IkReal x1703 = ((0.3) * cj9 * sj6);
24245 IkReal x1704 = ((0.3) * sj8);
24246 IkReal x1705 = (px * sj9);
24247 IkReal x1706 = ((0.045) * sj6);
24248 IkReal x1707 = (py * sj9);
24249 IkReal x1708 = ((0.3) * cj6 * cj8);
24250 IkReal x1709 = (py * x1698);
24251 IkReal x1710 = (px * x1701);
24252 CheckValue<IkReal> x1711 = IKatan2WithCheck(
24253 IkReal(
24254 ((((-1.0) * x1699))
24255 + (((-1.0) * x1704 * x1705))
24256 + (((-1.0) * cj9 * x1702))
24257 + ((py * x1700)) + x1702
24258 + ((x1706 * x1707))
24259 + ((py * x1703)) + ((cj9 * x1699))
24260 + ((x1707 * x1708)))),
24261 (x1709 + (((-1.0) * cj9 * x1709))
24262 + x1710 + ((px * x1700))
24263 + ((px * x1703)) + ((x1705 * x1708))
24264 + ((x1704 * x1707))
24265 + (((-1.0) * cj9 * x1710))
24266 + ((x1705 * x1706))),
24267 IKFAST_ATAN2_MAGTHRESH);
24268 if (!x1711.valid)
24269 {
24270 continue;
24271 }
24272 CheckValue<IkReal> x1712
24273 = IKPowWithIntegerCheck(
24274 IKsign(
24275 (pp
24276 + (((-1.0) * (1.0)
24277 * (pz * pz))))),
24278 -1);
24279 if (!x1712.valid)
24280 {
24281 continue;
24282 }
24283 j4array[0]
24284 = ((-1.5707963267949) + (x1711.value)
24285 + (((1.5707963267949)
24286 * (x1712.value))));
24287 sj4array[0] = IKsin(j4array[0]);
24288 cj4array[0] = IKcos(j4array[0]);
24289 if (j4array[0] > IKPI)
24290 {
24291 j4array[0] -= IK2PI;
24292 }
24293 else if (j4array[0] < -IKPI)
24294 {
24295 j4array[0] += IK2PI;
24296 }
24297 j4valid[0] = true;
24298 for (int ij4 = 0; ij4 < 1; ++ij4)
24299 {
24300 if (!j4valid[ij4])
24301 {
24302 continue;
24303 }
24304 _ij4[0] = ij4;
24305 _ij4[1] = -1;
24306 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24307 {
24308 if (j4valid[iij4]
24309 && IKabs(
24310 cj4array[ij4]
24311 - cj4array[iij4])
24312 < IKFAST_SOLUTION_THRESH
24313 && IKabs(
24314 sj4array[ij4]
24315 - sj4array[iij4])
24316 < IKFAST_SOLUTION_THRESH)
24317 {
24318 j4valid[iij4] = false;
24319 _ij4[1] = iij4;
24320 break;
24321 }
24322 }
24323 j4 = j4array[ij4];
24324 cj4 = cj4array[ij4];
24325 sj4 = sj4array[ij4];
24326 {
24327 IkReal evalcond[6];
24328 IkReal x1713 = ((0.3) * cj9);
24329 IkReal x1714 = ((0.045) * sj9);
24330 IkReal x1715 = (cj6 * pz);
24331 IkReal x1716 = IKcos(j4);
24332 IkReal x1717 = (px * sj6 * x1716);
24333 IkReal x1718 = IKsin(j4);
24334 IkReal x1719 = (py * x1718);
24335 IkReal x1720 = (sj6 * x1719);
24336 IkReal x1721 = ((0.045) * cj9);
24337 IkReal x1722 = (px * x1718);
24338 IkReal x1723 = ((0.3) * sj9);
24339 IkReal x1724 = ((1.0) * x1716);
24340 IkReal x1725 = (py * x1724);
24341 IkReal x1726 = (sj8 * x1716);
24342 IkReal x1727 = (cj6 * cj8);
24343 IkReal x1728 = (px * x1724);
24344 IkReal x1729 = ((1.0) * x1719);
24345 IkReal x1730 = (cj8 * pz * sj6);
24346 IkReal x1731 = (sj8 * x1722);
24347 IkReal x1732 = ((0.09) * cj6 * cj8);
24348 evalcond[0]
24349 = ((-0.55) + (((-1.0) * x1714))
24350 + x1715 + x1717
24351 + (((-1.0) * x1713)) + x1720);
24352 evalcond[1]
24353 = ((((-1.0) * sj8 * x1721))
24354 + (((-1.0) * x1725))
24355 + (((0.045) * sj8)) + x1722
24356 + ((sj8 * x1723)));
24357 evalcond[2]
24358 = (((cj6 * sj8 * x1719))
24359 + ((cj6 * px * x1726))
24360 + (((-1.0) * (1.0) * pz * sj6
24361 * sj8))
24362 + (((-1.0) * cj8 * x1725))
24363 + ((cj8 * x1722)));
24364 evalcond[3]
24365 = ((((0.045) * x1727))
24366 + ((x1723 * x1727))
24367 + ((sj6 * x1714))
24368 + (((0.55) * sj6))
24369 + (((-1.0) * x1721 * x1727))
24370 + (((-1.0) * x1729))
24371 + (((-1.0) * x1728))
24372 + ((sj6 * x1713)));
24373 evalcond[4]
24374 = ((0.045) + (((-1.0) * x1721))
24375 + (((-1.0) * x1727 * x1729))
24376 + (((-1.0) * x1727 * x1728))
24377 + (((-1.0) * sj8 * x1725))
24378 + x1723 + x1731 + x1730);
24379 evalcond[5]
24380 = ((-0.2125)
24381 + (((0.09) * py * x1726))
24382 + (((-0.09) * x1731))
24383 + ((x1719 * x1732))
24384 + (((1.1) * x1717))
24385 + ((px * x1716 * x1732))
24386 + (((1.1) * x1720))
24387 + (((-1.0) * (1.0) * pp))
24388 + (((1.1) * x1715))
24389 + (((-0.09) * x1730)));
24390 if (IKabs(evalcond[0])
24391 > IKFAST_EVALCOND_THRESH
24392 || IKabs(evalcond[1])
24393 > IKFAST_EVALCOND_THRESH
24394 || IKabs(evalcond[2])
24395 > IKFAST_EVALCOND_THRESH
24396 || IKabs(evalcond[3])
24397 > IKFAST_EVALCOND_THRESH
24398 || IKabs(evalcond[4])
24399 > IKFAST_EVALCOND_THRESH
24400 || IKabs(evalcond[5])
24401 > IKFAST_EVALCOND_THRESH)
24402 {
24403 continue;
24404 }
24405 }
24406
24407 rotationfunction0(solutions);
24408 }
24409 }
24410 }
24411 }
24412 }
24413 else
24414 {
24415 {
24416 IkReal j4array[1], cj4array[1], sj4array[1];
24417 bool j4valid[1] = {false};
24418 _nj4 = 1;
24419 IkReal x1733 = (cj8 * sj6);
24420 IkReal x1734 = ((0.55) * cj8);
24421 IkReal x1735 = ((0.55) * cj6);
24422 IkReal x1736 = (px * sj8);
24423 IkReal x1737 = ((0.3) * cj8 * cj9);
24424 IkReal x1738 = ((0.045) * cj8 * sj9);
24425 IkReal x1739 = (cj6 * cj8 * pz);
24426 IkReal x1740 = ((0.3) * cj6 * cj9);
24427 IkReal x1741 = ((0.045) * cj6 * sj9);
24428 IkReal x1742 = (py * sj8);
24429 CheckValue<IkReal> x1743 = IKatan2WithCheck(
24430 IkReal(
24431 ((((-1.0) * pz * x1736))
24432 + ((x1735 * x1736)) + ((py * x1739))
24433 + (((-1.0) * py * x1738))
24434 + ((x1736 * x1740))
24435 + (((-1.0) * py * x1737))
24436 + (((-1.0) * py * x1734))
24437 + ((x1736 * x1741)))),
24438 ((((-1.0) * px * x1738)) + ((px * x1739))
24439 + (((-1.0) * px * x1737))
24440 + (((-1.0) * px * x1734))
24441 + (((-1.0) * x1740 * x1742))
24442 + (((-1.0) * x1735 * x1742))
24443 + (((-1.0) * x1741 * x1742))
24444 + ((pz * x1742))),
24445 IKFAST_ATAN2_MAGTHRESH);
24446 if (!x1743.valid)
24447 {
24448 continue;
24449 }
24450 CheckValue<IkReal> x1744
24451 = IKPowWithIntegerCheck(
24452 IKsign(
24453 (((x1733 * (pz * pz)))
24454 + (((-1.0) * pp * x1733)))),
24455 -1);
24456 if (!x1744.valid)
24457 {
24458 continue;
24459 }
24460 j4array[0]
24461 = ((-1.5707963267949) + (x1743.value)
24462 + (((1.5707963267949) * (x1744.value))));
24463 sj4array[0] = IKsin(j4array[0]);
24464 cj4array[0] = IKcos(j4array[0]);
24465 if (j4array[0] > IKPI)
24466 {
24467 j4array[0] -= IK2PI;
24468 }
24469 else if (j4array[0] < -IKPI)
24470 {
24471 j4array[0] += IK2PI;
24472 }
24473 j4valid[0] = true;
24474 for (int ij4 = 0; ij4 < 1; ++ij4)
24475 {
24476 if (!j4valid[ij4])
24477 {
24478 continue;
24479 }
24480 _ij4[0] = ij4;
24481 _ij4[1] = -1;
24482 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24483 {
24484 if (j4valid[iij4]
24485 && IKabs(cj4array[ij4] - cj4array[iij4])
24486 < IKFAST_SOLUTION_THRESH
24487 && IKabs(sj4array[ij4] - sj4array[iij4])
24488 < IKFAST_SOLUTION_THRESH)
24489 {
24490 j4valid[iij4] = false;
24491 _ij4[1] = iij4;
24492 break;
24493 }
24494 }
24495 j4 = j4array[ij4];
24496 cj4 = cj4array[ij4];
24497 sj4 = sj4array[ij4];
24498 {
24499 IkReal evalcond[6];
24500 IkReal x1745 = ((0.3) * cj9);
24501 IkReal x1746 = ((0.045) * sj9);
24502 IkReal x1747 = (cj6 * pz);
24503 IkReal x1748 = IKcos(j4);
24504 IkReal x1749 = (px * sj6 * x1748);
24505 IkReal x1750 = IKsin(j4);
24506 IkReal x1751 = (py * x1750);
24507 IkReal x1752 = (sj6 * x1751);
24508 IkReal x1753 = ((0.045) * cj9);
24509 IkReal x1754 = (px * x1750);
24510 IkReal x1755 = ((0.3) * sj9);
24511 IkReal x1756 = ((1.0) * x1748);
24512 IkReal x1757 = (py * x1756);
24513 IkReal x1758 = (sj8 * x1748);
24514 IkReal x1759 = (cj6 * cj8);
24515 IkReal x1760 = (px * x1756);
24516 IkReal x1761 = ((1.0) * x1751);
24517 IkReal x1762 = (cj8 * pz * sj6);
24518 IkReal x1763 = (sj8 * x1754);
24519 IkReal x1764 = ((0.09) * cj6 * cj8);
24520 evalcond[0]
24521 = ((-0.55) + (((-1.0) * x1746))
24522 + (((-1.0) * x1745)) + x1747 + x1749
24523 + x1752);
24524 evalcond[1]
24525 = ((((0.045) * sj8))
24526 + (((-1.0) * sj8 * x1753))
24527 + ((sj8 * x1755))
24528 + (((-1.0) * x1757)) + x1754);
24529 evalcond[2]
24530 = (((cj6 * sj8 * x1751))
24531 + (((-1.0) * cj8 * x1757))
24532 + ((cj8 * x1754))
24533 + (((-1.0) * (1.0) * pz * sj6 * sj8))
24534 + ((cj6 * px * x1758)));
24535 evalcond[3]
24536 = ((((-1.0) * x1761))
24537 + (((-1.0) * x1753 * x1759))
24538 + (((0.55) * sj6))
24539 + ((x1755 * x1759)) + ((sj6 * x1746))
24540 + (((0.045) * x1759))
24541 + ((sj6 * x1745))
24542 + (((-1.0) * x1760)));
24543 evalcond[4]
24544 = ((0.045) + (((-1.0) * sj8 * x1757))
24545 + (((-1.0) * x1759 * x1760))
24546 + (((-1.0) * x1759 * x1761)) + x1762
24547 + x1763 + (((-1.0) * x1753))
24548 + x1755);
24549 evalcond[5]
24550 = ((-0.2125) + (((-0.09) * x1762))
24551 + ((px * x1748 * x1764))
24552 + (((1.1) * x1749))
24553 + ((x1751 * x1764))
24554 + (((0.09) * py * x1758))
24555 + (((-1.0) * (1.0) * pp))
24556 + (((1.1) * x1747))
24557 + (((1.1) * x1752))
24558 + (((-0.09) * x1763)));
24559 if (IKabs(evalcond[0])
24560 > IKFAST_EVALCOND_THRESH
24561 || IKabs(evalcond[1])
24562 > IKFAST_EVALCOND_THRESH
24563 || IKabs(evalcond[2])
24564 > IKFAST_EVALCOND_THRESH
24565 || IKabs(evalcond[3])
24566 > IKFAST_EVALCOND_THRESH
24567 || IKabs(evalcond[4])
24568 > IKFAST_EVALCOND_THRESH
24569 || IKabs(evalcond[5])
24570 > IKFAST_EVALCOND_THRESH)
24571 {
24572 continue;
24573 }
24574 }
24575
24576 rotationfunction0(solutions);
24577 }
24578 }
24579 }
24580 }
24581 }
24582 else
24583 {
24584 {
24585 IkReal j4array[1], cj4array[1], sj4array[1];
24586 bool j4valid[1] = {false};
24587 _nj4 = 1;
24588 IkReal x1765 = ((0.3) * cj9);
24589 IkReal x1766 = ((0.045) * sj9);
24590 IkReal x1767 = ((1.0) * cj6 * pz);
24591 IkReal x1768 = ((0.045) * sj6 * sj8);
24592 IkReal x1769 = (px * x1768);
24593 IkReal x1770 = ((0.3) * sj6 * sj8 * sj9);
24594 IkReal x1771 = (py * x1768);
24595 CheckValue<IkReal> x1772 = IKatan2WithCheck(
24596 IkReal(
24597 (((py * x1765)) + ((py * x1766))
24598 + ((cj9 * x1769)) + (((0.55) * py))
24599 + (((-1.0) * x1769))
24600 + (((-1.0) * py * x1767))
24601 + (((-1.0) * px * x1770)))),
24602 (((px * x1766)) + ((py * x1770))
24603 + (((-1.0) * cj9 * x1771)) + (((0.55) * px))
24604 + (((-1.0) * px * x1767)) + ((px * x1765))
24605 + x1771),
24606 IKFAST_ATAN2_MAGTHRESH);
24607 if (!x1772.valid)
24608 {
24609 continue;
24610 }
24611 CheckValue<IkReal> x1773 = IKPowWithIntegerCheck(
24612 IKsign(
24613 ((((-1.0) * (1.0) * sj6 * (pz * pz)))
24614 + ((pp * sj6)))),
24615 -1);
24616 if (!x1773.valid)
24617 {
24618 continue;
24619 }
24620 j4array[0]
24621 = ((-1.5707963267949) + (x1772.value)
24622 + (((1.5707963267949) * (x1773.value))));
24623 sj4array[0] = IKsin(j4array[0]);
24624 cj4array[0] = IKcos(j4array[0]);
24625 if (j4array[0] > IKPI)
24626 {
24627 j4array[0] -= IK2PI;
24628 }
24629 else if (j4array[0] < -IKPI)
24630 {
24631 j4array[0] += IK2PI;
24632 }
24633 j4valid[0] = true;
24634 for (int ij4 = 0; ij4 < 1; ++ij4)
24635 {
24636 if (!j4valid[ij4])
24637 {
24638 continue;
24639 }
24640 _ij4[0] = ij4;
24641 _ij4[1] = -1;
24642 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24643 {
24644 if (j4valid[iij4]
24645 && IKabs(cj4array[ij4] - cj4array[iij4])
24646 < IKFAST_SOLUTION_THRESH
24647 && IKabs(sj4array[ij4] - sj4array[iij4])
24648 < IKFAST_SOLUTION_THRESH)
24649 {
24650 j4valid[iij4] = false;
24651 _ij4[1] = iij4;
24652 break;
24653 }
24654 }
24655 j4 = j4array[ij4];
24656 cj4 = cj4array[ij4];
24657 sj4 = sj4array[ij4];
24658 {
24659 IkReal evalcond[6];
24660 IkReal x1774 = ((0.3) * cj9);
24661 IkReal x1775 = ((0.045) * sj9);
24662 IkReal x1776 = (cj6 * pz);
24663 IkReal x1777 = IKcos(j4);
24664 IkReal x1778 = (px * sj6 * x1777);
24665 IkReal x1779 = IKsin(j4);
24666 IkReal x1780 = (py * x1779);
24667 IkReal x1781 = (sj6 * x1780);
24668 IkReal x1782 = ((0.045) * cj9);
24669 IkReal x1783 = (px * x1779);
24670 IkReal x1784 = ((0.3) * sj9);
24671 IkReal x1785 = ((1.0) * x1777);
24672 IkReal x1786 = (py * x1785);
24673 IkReal x1787 = (sj8 * x1777);
24674 IkReal x1788 = (cj6 * cj8);
24675 IkReal x1789 = (px * x1785);
24676 IkReal x1790 = ((1.0) * x1780);
24677 IkReal x1791 = (cj8 * pz * sj6);
24678 IkReal x1792 = (sj8 * x1783);
24679 IkReal x1793 = ((0.09) * cj6 * cj8);
24680 evalcond[0]
24681 = ((-0.55) + x1781 + (((-1.0) * x1775))
24682 + (((-1.0) * x1774)) + x1778 + x1776);
24683 evalcond[1]
24684 = ((((0.045) * sj8)) + (((-1.0) * x1786))
24685 + x1783 + (((-1.0) * sj8 * x1782))
24686 + ((sj8 * x1784)));
24687 evalcond[2]
24688 = ((((-1.0) * cj8 * x1786))
24689 + ((cj8 * x1783)) + ((cj6 * px * x1787))
24690 + (((-1.0) * (1.0) * pz * sj6 * sj8))
24691 + ((cj6 * sj8 * x1780)));
24692 evalcond[3]
24693 = ((((-1.0) * x1790)) + ((x1784 * x1788))
24694 + (((-1.0) * x1782 * x1788))
24695 + (((0.55) * sj6)) + ((sj6 * x1775))
24696 + (((-1.0) * x1789))
24697 + (((0.045) * x1788)) + ((sj6 * x1774)));
24698 evalcond[4]
24699 = ((0.045) + (((-1.0) * x1782))
24700 + (((-1.0) * x1788 * x1790)) + x1784
24701 + (((-1.0) * x1788 * x1789))
24702 + (((-1.0) * sj8 * x1786)) + x1792
24703 + x1791);
24704 evalcond[5]
24705 = ((-0.2125) + (((1.1) * x1776))
24706 + (((1.1) * x1778)) + ((x1780 * x1793))
24707 + (((1.1) * x1781))
24708 + (((-1.0) * (1.0) * pp))
24709 + (((0.09) * py * x1787))
24710 + (((-0.09) * x1792))
24711 + ((px * x1777 * x1793))
24712 + (((-0.09) * x1791)));
24713 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
24714 || IKabs(evalcond[1])
24715 > IKFAST_EVALCOND_THRESH
24716 || IKabs(evalcond[2])
24717 > IKFAST_EVALCOND_THRESH
24718 || IKabs(evalcond[3])
24719 > IKFAST_EVALCOND_THRESH
24720 || IKabs(evalcond[4])
24721 > IKFAST_EVALCOND_THRESH
24722 || IKabs(evalcond[5])
24723 > IKFAST_EVALCOND_THRESH)
24724 {
24725 continue;
24726 }
24727 }
24728
24729 rotationfunction0(solutions);
24730 }
24731 }
24732 }
24733 }
24734 }
24735 }
24736 }
24737 }
24738 }
24739 else
24740 {
24741 {
24742 IkReal j4array[2], cj4array[2], sj4array[2];
24743 bool j4valid[2] = {false};
24744 _nj4 = 2;
24745 CheckValue<IkReal> x1797 = IKatan2WithCheck(
24746 IkReal(((-1.0) * (((1.0) * py)))),
24747 px,
24748 IKFAST_ATAN2_MAGTHRESH);
24749 if (!x1797.valid)
24750 {
24751 continue;
24752 }
24753 IkReal x1794 = ((-1.0) * (x1797.value));
24754 IkReal x1795 = ((0.045) * sj8);
24755 if ((((py * py) + (px * px))) < -0.00001)
24756 continue;
24757 CheckValue<IkReal> x1798 = IKPowWithIntegerCheck(
24758 IKabs(IKsqrt(((py * py) + (px * px)))), -1);
24759 if (!x1798.valid)
24760 {
24761 continue;
24762 }
24763 if ((((x1798.value)
24764 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24765 + x1795))))
24766 < -1 - IKFAST_SINCOS_THRESH
24767 || (((x1798.value)
24768 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24769 + x1795))))
24770 > 1 + IKFAST_SINCOS_THRESH)
24771 continue;
24772 IkReal x1796 = IKasin(
24773 ((x1798.value)
24774 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24775 + x1795))));
24776 j4array[0] = ((((-1.0) * x1796)) + x1794);
24777 sj4array[0] = IKsin(j4array[0]);
24778 cj4array[0] = IKcos(j4array[0]);
24779 j4array[1] = ((3.14159265358979) + x1794 + x1796);
24780 sj4array[1] = IKsin(j4array[1]);
24781 cj4array[1] = IKcos(j4array[1]);
24782 if (j4array[0] > IKPI)
24783 {
24784 j4array[0] -= IK2PI;
24785 }
24786 else if (j4array[0] < -IKPI)
24787 {
24788 j4array[0] += IK2PI;
24789 }
24790 j4valid[0] = true;
24791 if (j4array[1] > IKPI)
24792 {
24793 j4array[1] -= IK2PI;
24794 }
24795 else if (j4array[1] < -IKPI)
24796 {
24797 j4array[1] += IK2PI;
24798 }
24799 j4valid[1] = true;
24800 for (int ij4 = 0; ij4 < 2; ++ij4)
24801 {
24802 if (!j4valid[ij4])
24803 {
24804 continue;
24805 }
24806 _ij4[0] = ij4;
24807 _ij4[1] = -1;
24808 for (int iij4 = ij4 + 1; iij4 < 2; ++iij4)
24809 {
24810 if (j4valid[iij4]
24811 && IKabs(cj4array[ij4] - cj4array[iij4])
24812 < IKFAST_SOLUTION_THRESH
24813 && IKabs(sj4array[ij4] - sj4array[iij4])
24814 < IKFAST_SOLUTION_THRESH)
24815 {
24816 j4valid[iij4] = false;
24817 _ij4[1] = iij4;
24818 break;
24819 }
24820 }
24821 j4 = j4array[ij4];
24822 cj4 = cj4array[ij4];
24823 sj4 = sj4array[ij4];
24824
24825 {
24826 IkReal j6eval[2];
24827 IkReal x1799 = (cj4 * px);
24828 IkReal x1800 = (cj8 * pz);
24829 IkReal x1801 = (py * sj4);
24830 IkReal x1802 = (cj4 * cj9 * px);
24831 IkReal x1803 = (cj4 * px * sj9);
24832 IkReal x1804 = (cj8 * pz * sj9);
24833 IkReal x1805 = (cj9 * py * sj4);
24834 IkReal x1806 = (py * sj4 * sj9);
24835 IkReal x1807 = ((0.045) * x1800);
24836 j6eval[0]
24837 = ((((-6.66666666666667) * x1802)) + (((-1.0) * x1806))
24838 + (((-12.2222222222222) * x1799))
24839 + (((-1.0) * x1803))
24840 + (((-6.66666666666667) * x1804))
24841 + (((-1.0) * x1800))
24842 + (((-6.66666666666667) * x1805))
24843 + (((-12.2222222222222) * x1801)) + ((cj9 * x1800)));
24844 j6eval[1] = IKsign(
24845 ((((-0.3) * x1804)) + (((-0.3) * x1802))
24846 + (((-0.045) * x1806)) + (((-0.55) * x1799))
24847 + ((cj9 * x1807)) + (((-0.3) * x1805))
24848 + (((-1.0) * x1807)) + (((-0.55) * x1801))
24849 + (((-0.045) * x1803))));
24850 if (IKabs(j6eval[0]) < 0.0000010000000000
24851 || IKabs(j6eval[1]) < 0.0000010000000000)
24852 {
24853 {
24854 IkReal j6eval[2];
24855 IkReal x1808 = (sj8 * (py * py));
24856 IkReal x1809 = cj4 * cj4;
24857 IkReal x1810
24858 = ((((-1.0) * x1808 * x1809)) + ((sj8 * (pz * pz)))
24859 + x1808 + ((sj8 * x1809 * (px * px)))
24860 + (((2.0) * cj4 * px * py * sj4 * sj8)));
24861 j6eval[0] = x1810;
24862 j6eval[1] = IKsign(x1810);
24863 if (IKabs(j6eval[0]) < 0.0000010000000000
24864 || IKabs(j6eval[1]) < 0.0000010000000000)
24865 {
24866 {
24867 IkReal j6eval[2];
24868 IkReal x1811 = (pz * sj8);
24869 IkReal x1812 = (cj9 * pz * sj8);
24870 IkReal x1813 = (pz * sj8 * sj9);
24871 IkReal x1814 = (cj8 * sj8);
24872 IkReal x1815 = (cj4 * px * x1814);
24873 IkReal x1816 = (py * sj4 * x1814);
24874 IkReal x1817 = ((1.0) * cj9);
24875 IkReal x1818 = (cj4 * cj8 * px * sj8 * sj9);
24876 IkReal x1819 = (cj8 * py * sj4 * sj8 * sj9);
24877 IkReal x1820 = ((0.045) * x1815);
24878 IkReal x1821 = ((0.045) * x1816);
24879 j6eval[0]
24880 = ((((-1.0) * x1816 * x1817))
24881 + (((6.66666666666667) * x1818))
24882 + (((-1.0) * x1813)) + x1815 + x1816
24883 + (((-1.0) * x1815 * x1817))
24884 + (((-6.66666666666667) * x1812))
24885 + (((-12.2222222222222) * x1811))
24886 + (((6.66666666666667) * x1819)));
24887 j6eval[1] = IKsign(
24888 ((((-1.0) * cj9 * x1821)) + x1821 + x1820
24889 + (((0.3) * x1819)) + (((-1.0) * cj9 * x1820))
24890 + (((-0.045) * x1813)) + (((0.3) * x1818))
24891 + (((-0.55) * x1811)) + (((-0.3) * x1812))));
24892 if (IKabs(j6eval[0]) < 0.0000010000000000
24893 || IKabs(j6eval[1]) < 0.0000010000000000)
24894 {
24895 {
24896 IkReal evalcond[4];
24897 bool bgotonextstatement = true;
24898 do
24899 {
24900 IkReal x1822
24901 = (((px * sj4))
24902 + (((-1.0) * (1.0) * cj4 * py)));
24903 evalcond[0]
24904 = ((-3.14159265358979)
24905 + (IKfmod(
24906 ((3.14159265358979) + (IKabs(j8))),
24907 6.28318530717959)));
24908 evalcond[1]
24909 = ((0.39655) + (((0.0765) * sj9))
24910 + (((0.32595) * cj9))
24911 + (((-1.0) * (1.0) * pp)));
24912 evalcond[2] = x1822;
24913 evalcond[3] = x1822;
24914 if (IKabs(evalcond[0]) < 0.0000010000000000
24915 && IKabs(evalcond[1]) < 0.0000010000000000
24916 && IKabs(evalcond[2]) < 0.0000010000000000
24917 && IKabs(evalcond[3])
24918 < 0.0000010000000000)
24919 {
24920 bgotonextstatement = false;
24921 {
24922 IkReal j6eval[2];
24923 sj8 = 0;
24924 cj8 = 1.0;
24925 j8 = 0;
24926 IkReal x1823 = (cj9 * pz);
24927 IkReal x1824 = (cj4 * px);
24928 IkReal x1825 = (pp * pz);
24929 IkReal x1826 = (py * sj4);
24930 IkReal x1827 = (cj4 * cj9 * px);
24931 IkReal x1828 = (cj4 * pp * px);
24932 IkReal x1829 = (cj9 * py * sj4);
24933 IkReal x1830 = (pp * py * sj4);
24934 j6eval[0]
24935 = ((((-36.2220411120167) * x1830))
24936 + (((13.9482024812098) * x1826))
24937 + (((5.4333061668025) * x1825))
24938 + x1823
24939 + (((13.9482024812098) * x1824))
24940 + (((-36.2220411120167) * x1828))
24941 + (((12.2222222222222) * x1827))
24942 + (((12.2222222222222) * x1829))
24943 + (((2.92556370551481) * pz)));
24944 j6eval[1] = IKsign(
24945 ((((-3.92156862745098) * x1830))
24946 + (((0.316735294117647) * pz))
24947 + (((-3.92156862745098) * x1828))
24948 + (((1.32323529411765) * x1827))
24949 + (((0.588235294117647) * x1825))
24950 + (((1.51009803921569) * x1824))
24951 + (((1.32323529411765) * x1829))
24952 + (((1.51009803921569) * x1826))
24953 + (((0.108264705882353) * x1823))));
24954 if (IKabs(j6eval[0]) < 0.0000010000000000
24955 || IKabs(j6eval[1])
24956 < 0.0000010000000000)
24957 {
24958 {
24959 IkReal j6eval[2];
24960 sj8 = 0;
24961 cj8 = 1.0;
24962 j8 = 0;
24963 IkReal x1831 = (cj4 * px);
24964 IkReal x1832 = (py * sj4);
24965 IkReal x1833 = (cj9 * pz);
24966 IkReal x1834 = (pz * sj9);
24967 IkReal x1835 = ((1.0) * cj9);
24968 IkReal x1836 = (cj4 * px * sj9);
24969 IkReal x1837 = (py * sj4 * sj9);
24970 IkReal x1838 = ((0.045) * x1831);
24971 IkReal x1839 = ((0.045) * x1832);
24972 j6eval[0]
24973 = ((((-1.0) * (12.2222222222222)
24974 * pz))
24975 + (((-1.0) * x1831 * x1835))
24976 + (((-1.0) * x1834))
24977 + (((-6.66666666666667)
24978 * x1833))
24979 + x1832 + x1831
24980 + (((6.66666666666667)
24981 * x1837))
24982 + (((-1.0) * x1832 * x1835))
24983 + (((6.66666666666667)
24984 * x1836)));
24985 j6eval[1] = IKsign(
24986 ((((-1.0) * (0.55) * pz))
24987 + (((0.3) * x1836))
24988 + (((-1.0) * cj9 * x1839))
24989 + (((-1.0) * cj9 * x1838))
24990 + (((0.3) * x1837))
24991 + (((-0.045) * x1834))
24992 + (((-0.3) * x1833)) + x1839
24993 + x1838));
24994 if (IKabs(j6eval[0])
24995 < 0.0000010000000000
24996 || IKabs(j6eval[1])
24997 < 0.0000010000000000)
24998 {
24999 {
25000 IkReal j6eval[2];
25001 sj8 = 0;
25002 cj8 = 1.0;
25003 j8 = 0;
25004 IkReal x1840 = (cj4 * px);
25005 IkReal x1841 = (cj9 * pz);
25006 IkReal x1842 = (pp * pz);
25007 IkReal x1843 = (py * sj4);
25008 IkReal x1844 = (cj4 * cj9 * px);
25009 IkReal x1845 = (cj4 * pp * px);
25010 IkReal x1846 = (cj9 * py * sj4);
25011 IkReal x1847 = (pp * py * sj4);
25012 j6eval[0]
25013 = ((((-5.4333061668025)
25014 * x1847))
25015 + (((-5.4333061668025)
25016 * x1845))
25017 + (((-36.2220411120167)
25018 * x1842))
25019 + (((-2.92556370551481)
25020 * x1843))
25021 + (((-1.0) * x1846))
25022 + (((-1.0) * x1844))
25023 + (((13.9482024812098)
25024 * pz))
25025 + (((-2.92556370551481)
25026 * x1840))
25027 + (((12.2222222222222)
25028 * x1841)));
25029 j6eval[1] = IKsign(
25030 ((((-0.108264705882353)
25031 * x1844))
25032 + (((1.51009803921569) * pz))
25033 + (((1.32323529411765)
25034 * x1841))
25035 + (((-0.588235294117647)
25036 * x1845))
25037 + (((-0.108264705882353)
25038 * x1846))
25039 + (((-0.316735294117647)
25040 * x1843))
25041 + (((-0.588235294117647)
25042 * x1847))
25043 + (((-0.316735294117647)
25044 * x1840))
25045 + (((-3.92156862745098)
25046 * x1842))));
25047 if (IKabs(j6eval[0])
25048 < 0.0000010000000000
25049 || IKabs(j6eval[1])
25050 < 0.0000010000000000)
25051 {
25052 {
25053 IkReal evalcond[1];
25054 bool bgotonextstatement
25055 = true;
25056 do
25057 {
25058 evalcond[0]
25059 = ((IKabs((
25060 (-3.14159265358979)
25061 + (IKfmod(
25062 ((3.14159265358979)
25063 + j9),
25064 6.28318530717959)))))
25065 + (IKabs(pz)));
25066 if (IKabs(evalcond[0])
25067 < 0.0000010000000000)
25068 {
25069 bgotonextstatement
25070 = false;
25071 {
25072 IkReal j6eval[1];
25073 IkReal x1848
25074 = ((1.0) * py);
25075 sj8 = 0;
25076 cj8 = 1.0;
25077 j8 = 0;
25078 pz = 0;
25079 j9 = 0;
25080 sj9 = 0;
25081 cj9 = 1.0;
25082 pp
25083 = ((py * py)
25084 + (px * px));
25085 npx
25086 = (((py * r10))
25087 + ((px * r00)));
25088 npy
25089 = (((py * r11))
25090 + ((px * r01)));
25091 npz
25092 = (((px * r02))
25093 + ((py * r12)));
25094 rxp0_0
25095 = ((-1.0) * r20
25096 * x1848);
25097 rxp0_1 = (px * r20);
25098 rxp1_0
25099 = ((-1.0) * r21
25100 * x1848);
25101 rxp1_1 = (px * r21);
25102 rxp2_0
25103 = ((-1.0) * r22
25104 * x1848);
25105 rxp2_1 = (px * r22);
25106 j6eval[0]
25107 = (((py * sj4))
25108 + ((cj4 * px)));
25109 if (IKabs(j6eval[0])
25110 < 0.0000010000000000)
25111 {
25112 {
25113 IkReal j6eval[1];
25114 IkReal x1849
25115 = ((1.0) * py);
25116 sj8 = 0;
25117 cj8 = 1.0;
25118 j8 = 0;
25119 pz = 0;
25120 j9 = 0;
25121 sj9 = 0;
25122 cj9 = 1.0;
25123 pp
25124 = ((py * py)
25125 + (px * px));
25126 npx
25127 = (((py * r10))
25128 + ((px
25129 * r00)));
25130 npy
25131 = (((py * r11))
25132 + ((px
25133 * r01)));
25134 npz
25135 = (((px * r02))
25136 + ((py
25137 * r12)));
25138 rxp0_0
25139 = ((-1.0) * r20
25140 * x1849);
25141 rxp0_1 = (px * r20);
25142 rxp1_0
25143 = ((-1.0) * r21
25144 * x1849);
25145 rxp1_1 = (px * r21);
25146 rxp2_0
25147 = ((-1.0) * r22
25148 * x1849);
25149 rxp2_1 = (px * r22);
25150 j6eval[0]
25151 = ((-1.0)
25152 + (((-1.0)
25153 * (1.3840830449827)
25154 * (px
25155 * px)))
25156 + (((-1.0)
25157 * (1.3840830449827)
25158 * (py
25159 * py))));
25160 if (IKabs(j6eval[0])
25161 < 0.0000010000000000)
25162 {
25163 {
25164 IkReal
25165 j6eval[2];
25166 IkReal x1850
25167 = ((1.0)
25168 * py);
25169 sj8 = 0;
25170 cj8 = 1.0;
25171 j8 = 0;
25172 pz = 0;
25173 j9 = 0;
25174 sj9 = 0;
25175 cj9 = 1.0;
25176 pp
25177 = ((py * py)
25178 + (px
25179 * px));
25180 npx
25181 = (((py
25182 * r10))
25183 + ((px
25184 * r00)));
25185 npy
25186 = (((py
25187 * r11))
25188 + ((px
25189 * r01)));
25190 npz
25191 = (((px
25192 * r02))
25193 + ((py
25194 * r12)));
25195 rxp0_0
25196 = ((-1.0)
25197 * r20
25198 * x1850);
25199 rxp0_1
25200 = (px
25201 * r20);
25202 rxp1_0
25203 = ((-1.0)
25204 * r21
25205 * x1850);
25206 rxp1_1
25207 = (px
25208 * r21);
25209 rxp2_0
25210 = ((-1.0)
25211 * r22
25212 * x1850);
25213 rxp2_1
25214 = (px
25215 * r22);
25216 IkReal x1851
25217 = (cj4
25218 * px);
25219 IkReal x1852
25220 = (py
25221 * sj4);
25222 j6eval[0]
25223 = (x1852
25224 + x1851);
25225 j6eval[1]
25226 = ((((-1.0)
25227 * (1.3840830449827)
25228 * cj4
25229 * (px
25230 * px
25231 * px)))
25232 + (((-1.0)
25233 * x1852))
25234 + (((-1.0)
25235 * x1851))
25236 + (((-1.3840830449827)
25237 * x1852
25238 * (px
25239 * px)))
25240 + (((-1.0)
25241 * (1.3840830449827)
25242 * sj4
25243 * (py
25244 * py
25245 * py)))
25246 + (((-1.3840830449827)
25247 * x1851
25248 * (py
25249 * py))));
25250 if (IKabs(
25251 j6eval
25252 [0])
25253 < 0.0000010000000000
25254 || IKabs(
25255 j6eval
25256 [1])
25257 < 0.0000010000000000)
25258 {
25259 {
25260 IkReal
25261 evalcond
25262 [4];
25263 bool
25264 bgotonextstatement
25265 = true;
25266 do
25267 {
25268 evalcond
25269 [0]
25270 = ((IKabs(
25271 py))
25272 + (IKabs(
25273 px)));
25274 evalcond
25275 [1]
25276 = -0.85;
25277 evalcond
25278 [2]
25279 = 0;
25280 evalcond
25281 [3]
25282 = -0.2125;
25283 if (IKabs(
25284 evalcond
25285 [0])
25286 < 0.0000010000000000
25287 && IKabs(
25288 evalcond
25289 [1])
25290 < 0.0000010000000000
25291 && IKabs(
25292 evalcond
25293 [2])
25294 < 0.0000010000000000
25295 && IKabs(
25296 evalcond
25297 [3])
25298 < 0.0000010000000000)
25299 {
25300 bgotonextstatement
25301 = false;
25302 {
25303 IkReal j6array
25304 [2],
25305 cj6array
25306 [2],
25307 sj6array
25308 [2];
25309 bool j6valid
25310 [2]
25311 = {false};
25312 _nj6
25313 = 2;
25314 j6array
25315 [0]
25316 = 2.9927027059803;
25317 sj6array
25318 [0]
25319 = IKsin(
25320 j6array
25321 [0]);
25322 cj6array
25323 [0]
25324 = IKcos(
25325 j6array
25326 [0]);
25327 j6array
25328 [1]
25329 = 6.13429535957009;
25330 sj6array
25331 [1]
25332 = IKsin(
25333 j6array
25334 [1]);
25335 cj6array
25336 [1]
25337 = IKcos(
25338 j6array
25339 [1]);
25340 if (j6array
25341 [0]
25342 > IKPI)
25343 {
25344 j6array
25345 [0]
25346 -= IK2PI;
25347 }
25348 else if (
25349 j6array
25350 [0]
25351 < -IKPI)
25352 {
25353 j6array
25354 [0]
25355 += IK2PI;
25356 }
25357 j6valid
25358 [0]
25359 = true;
25360 if (j6array
25361 [1]
25362 > IKPI)
25363 {
25364 j6array
25365 [1]
25366 -= IK2PI;
25367 }
25368 else if (
25369 j6array
25370 [1]
25371 < -IKPI)
25372 {
25373 j6array
25374 [1]
25375 += IK2PI;
25376 }
25377 j6valid
25378 [1]
25379 = true;
25380 for (
25381 int ij6
25382 = 0;
25383 ij6
25384 < 2;
25385 ++ij6)
25386 {
25387 if (!j6valid
25388 [ij6])
25389 {
25390 continue;
25391 }
25392 _ij6[0]
25393 = ij6;
25394 _ij6[1]
25395 = -1;
25396 for (
25397 int iij6
25398 = ij6
25399 + 1;
25400 iij6
25401 < 2;
25402 ++iij6)
25403 {
25404 if (j6valid
25405 [iij6]
25406 && IKabs(
25407 cj6array
25408 [ij6]
25409 - cj6array
25410 [iij6])
25411 < IKFAST_SOLUTION_THRESH
25412 && IKabs(
25413 sj6array
25414 [ij6]
25415 - sj6array
25416 [iij6])
25417 < IKFAST_SOLUTION_THRESH)
25418 {
25419 j6valid
25420 [iij6]
25421 = false;
25422 _ij6[1]
25423 = iij6;
25424 break;
25425 }
25426 }
25427 j6 = j6array
25428 [ij6];
25429 cj6 = cj6array
25430 [ij6];
25431 sj6 = sj6array
25432 [ij6];
25433 {
25434 IkReal evalcond
25435 [1];
25436 evalcond
25437 [0]
25438 = ((0.85)
25439 * (IKsin(
25440 j6)));
25441 if (IKabs(
25442 evalcond
25443 [0])
25444 > IKFAST_EVALCOND_THRESH)
25445 {
25446 continue;
25447 }
25448 }
25449
25450 rotationfunction0(
25451 solutions);
25452 }
25453 }
25454 }
25455 } while (0);
25456 if (bgotonextstatement)
25457 {
25458 bool
25459 bgotonextstatement
25460 = true;
25461 do
25462 {
25463 evalcond
25464 [0]
25465 = ((IKabs((
25466 (-3.14159265358979)
25467 + (IKfmod(
25468 ((3.14159265358979)
25469 + j4),
25470 6.28318530717959)))))
25471 + (IKabs(
25472 px)));
25473 evalcond
25474 [1]
25475 = -0.85;
25476 evalcond
25477 [2]
25478 = 0;
25479 evalcond
25480 [3]
25481 = ((-0.2125)
25482 + (((-1.0)
25483 * (1.0)
25484 * (py
25485 * py))));
25486 if (IKabs(
25487 evalcond
25488 [0])
25489 < 0.0000010000000000
25490 && IKabs(
25491 evalcond
25492 [1])
25493 < 0.0000010000000000
25494 && IKabs(
25495 evalcond
25496 [2])
25497 < 0.0000010000000000
25498 && IKabs(
25499 evalcond
25500 [3])
25501 < 0.0000010000000000)
25502 {
25503 bgotonextstatement
25504 = false;
25505 {
25506 IkReal j6eval
25507 [1];
25508 IkReal
25509 x1853
25510 = ((1.0)
25511 * py);
25512 sj8 = 0;
25513 cj8 = 1.0;
25514 j8 = 0;
25515 pz = 0;
25516 j9 = 0;
25517 sj9 = 0;
25518 cj9 = 1.0;
25519 pp = py
25520 * py;
25521 npx
25522 = (py
25523 * r10);
25524 npy
25525 = (py
25526 * r11);
25527 npz
25528 = (py
25529 * r12);
25530 rxp0_0
25531 = ((-1.0)
25532 * r20
25533 * x1853);
25534 rxp0_1
25535 = 0;
25536 rxp1_0
25537 = ((-1.0)
25538 * r21
25539 * x1853);
25540 rxp1_1
25541 = 0;
25542 rxp2_0
25543 = ((-1.0)
25544 * r22
25545 * x1853);
25546 rxp2_1
25547 = 0;
25548 px = 0;
25549 j4 = 0;
25550 sj4 = 0;
25551 cj4 = 1.0;
25552 rxp0_2
25553 = (py
25554 * r00);
25555 rxp1_2
25556 = (py
25557 * r01);
25558 rxp2_2
25559 = (py
25560 * r02);
25561 j6eval
25562 [0]
25563 = ((1.0)
25564 + (((1.91568587540858)
25565 * (py
25566 * py
25567 * py
25568 * py)))
25569 + (((-1.0)
25570 * (2.64633970947792)
25571 * (py
25572 * py))));
25573 if (IKabs(
25574 j6eval
25575 [0])
25576 < 0.0000010000000000)
25577 {
25578 continue; // no branches [j6]
25579 }
25580 else
25581 {
25582 {
25583 IkReal j6array
25584 [2],
25585 cj6array
25586 [2],
25587 sj6array
25588 [2];
25589 bool j6valid
25590 [2]
25591 = {false};
25592 _nj6
25593 = 2;
25594 IkReal
25595 x1854
25596 = py
25597 * py;
25598 CheckValue<IkReal> x1856 = IKatan2WithCheck(
25599 IkReal((
25600 (-0.425)
25601 + (((-0.588235294117647)
25602 * x1854)))),
25603 ((-2.83333333333333)
25604 + (((3.92156862745098)
25605 * x1854))),
25606 IKFAST_ATAN2_MAGTHRESH);
25607 if (!x1856
25608 .valid)
25609 {
25610 continue;
25611 }
25612 IkReal
25613 x1855
25614 = ((-1.0)
25615 * (x1856
25616 .value));
25617 j6array
25618 [0]
25619 = x1855;
25620 sj6array
25621 [0]
25622 = IKsin(
25623 j6array
25624 [0]);
25625 cj6array
25626 [0]
25627 = IKcos(
25628 j6array
25629 [0]);
25630 j6array
25631 [1]
25632 = ((3.14159265358979)
25633 + x1855);
25634 sj6array
25635 [1]
25636 = IKsin(
25637 j6array
25638 [1]);
25639 cj6array
25640 [1]
25641 = IKcos(
25642 j6array
25643 [1]);
25644 if (j6array
25645 [0]
25646 > IKPI)
25647 {
25648 j6array
25649 [0]
25650 -= IK2PI;
25651 }
25652 else if (
25653 j6array
25654 [0]
25655 < -IKPI)
25656 {
25657 j6array
25658 [0]
25659 += IK2PI;
25660 }
25661 j6valid
25662 [0]
25663 = true;
25664 if (j6array
25665 [1]
25666 > IKPI)
25667 {
25668 j6array
25669 [1]
25670 -= IK2PI;
25671 }
25672 else if (
25673 j6array
25674 [1]
25675 < -IKPI)
25676 {
25677 j6array
25678 [1]
25679 += IK2PI;
25680 }
25681 j6valid
25682 [1]
25683 = true;
25684 for (
25685 int ij6
25686 = 0;
25687 ij6
25688 < 2;
25689 ++ij6)
25690 {
25691 if (!j6valid
25692 [ij6])
25693 {
25694 continue;
25695 }
25696 _ij6[0]
25697 = ij6;
25698 _ij6[1]
25699 = -1;
25700 for (
25701 int iij6
25702 = ij6
25703 + 1;
25704 iij6
25705 < 2;
25706 ++iij6)
25707 {
25708 if (j6valid
25709 [iij6]
25710 && IKabs(
25711 cj6array
25712 [ij6]
25713 - cj6array
25714 [iij6])
25715 < IKFAST_SOLUTION_THRESH
25716 && IKabs(
25717 sj6array
25718 [ij6]
25719 - sj6array
25720 [iij6])
25721 < IKFAST_SOLUTION_THRESH)
25722 {
25723 j6valid
25724 [iij6]
25725 = false;
25726 _ij6[1]
25727 = iij6;
25728 break;
25729 }
25730 }
25731 j6 = j6array
25732 [ij6];
25733 cj6 = cj6array
25734 [ij6];
25735 sj6 = sj6array
25736 [ij6];
25737 {
25738 IkReal evalcond
25739 [1];
25740 evalcond
25741 [0]
25742 = ((0.85)
25743 * (IKsin(
25744 j6)));
25745 if (IKabs(
25746 evalcond
25747 [0])
25748 > IKFAST_EVALCOND_THRESH)
25749 {
25750 continue;
25751 }
25752 }
25753
25754 rotationfunction0(
25755 solutions);
25756 }
25757 }
25758 }
25759 }
25760 }
25761 } while (
25762 0);
25763 if (bgotonextstatement)
25764 {
25765 bool
25766 bgotonextstatement
25767 = true;
25768 do
25769 {
25770 evalcond
25771 [0]
25772 = ((IKabs((
25773 (-3.14159265358979)
25774 + (IKfmod(
25775 j4,
25776 6.28318530717959)))))
25777 + (IKabs(
25778 px)));
25779 evalcond
25780 [1]
25781 = -0.85;
25782 evalcond
25783 [2]
25784 = 0;
25785 evalcond
25786 [3]
25787 = ((-0.2125)
25788 + (((-1.0)
25789 * (1.0)
25790 * (py
25791 * py))));
25792 if (IKabs(
25793 evalcond
25794 [0])
25795 < 0.0000010000000000
25796 && IKabs(
25797 evalcond
25798 [1])
25799 < 0.0000010000000000
25800 && IKabs(
25801 evalcond
25802 [2])
25803 < 0.0000010000000000
25804 && IKabs(
25805 evalcond
25806 [3])
25807 < 0.0000010000000000)
25808 {
25809 bgotonextstatement
25810 = false;
25811 {
25812 IkReal j6eval
25813 [1];
25814 IkReal
25815 x1857
25816 = ((1.0)
25817 * py);
25818 sj8 = 0;
25819 cj8 = 1.0;
25820 j8 = 0;
25821 pz = 0;
25822 j9 = 0;
25823 sj9 = 0;
25824 cj9 = 1.0;
25825 pp = py
25826 * py;
25827 npx
25828 = (py
25829 * r10);
25830 npy
25831 = (py
25832 * r11);
25833 npz
25834 = (py
25835 * r12);
25836 rxp0_0
25837 = ((-1.0)
25838 * r20
25839 * x1857);
25840 rxp0_1
25841 = 0;
25842 rxp1_0
25843 = ((-1.0)
25844 * r21
25845 * x1857);
25846 rxp1_1
25847 = 0;
25848 rxp2_0
25849 = ((-1.0)
25850 * r22
25851 * x1857);
25852 rxp2_1
25853 = 0;
25854 px = 0;
25855 j4 = 3.14159265358979;
25856 sj4 = 0;
25857 cj4 = -1.0;
25858 rxp0_2
25859 = (py
25860 * r00);
25861 rxp1_2
25862 = (py
25863 * r01);
25864 rxp2_2
25865 = (py
25866 * r02);
25867 j6eval
25868 [0]
25869 = ((1.0)
25870 + (((1.91568587540858)
25871 * (py
25872 * py
25873 * py
25874 * py)))
25875 + (((-1.0)
25876 * (2.64633970947792)
25877 * (py
25878 * py))));
25879 if (IKabs(
25880 j6eval
25881 [0])
25882 < 0.0000010000000000)
25883 {
25884 continue; // no branches [j6]
25885 }
25886 else
25887 {
25888 {
25889 IkReal j6array
25890 [2],
25891 cj6array
25892 [2],
25893 sj6array
25894 [2];
25895 bool j6valid
25896 [2]
25897 = {false};
25898 _nj6
25899 = 2;
25900 IkReal
25901 x1858
25902 = py
25903 * py;
25904 CheckValue<IkReal> x1860 = IKatan2WithCheck(
25905 IkReal((
25906 (-0.425)
25907 + (((-0.588235294117647)
25908 * x1858)))),
25909 ((-2.83333333333333)
25910 + (((3.92156862745098)
25911 * x1858))),
25912 IKFAST_ATAN2_MAGTHRESH);
25913 if (!x1860
25914 .valid)
25915 {
25916 continue;
25917 }
25918 IkReal
25919 x1859
25920 = ((-1.0)
25921 * (x1860
25922 .value));
25923 j6array
25924 [0]
25925 = x1859;
25926 sj6array
25927 [0]
25928 = IKsin(
25929 j6array
25930 [0]);
25931 cj6array
25932 [0]
25933 = IKcos(
25934 j6array
25935 [0]);
25936 j6array
25937 [1]
25938 = ((3.14159265358979)
25939 + x1859);
25940 sj6array
25941 [1]
25942 = IKsin(
25943 j6array
25944 [1]);
25945 cj6array
25946 [1]
25947 = IKcos(
25948 j6array
25949 [1]);
25950 if (j6array
25951 [0]
25952 > IKPI)
25953 {
25954 j6array
25955 [0]
25956 -= IK2PI;
25957 }
25958 else if (
25959 j6array
25960 [0]
25961 < -IKPI)
25962 {
25963 j6array
25964 [0]
25965 += IK2PI;
25966 }
25967 j6valid
25968 [0]
25969 = true;
25970 if (j6array
25971 [1]
25972 > IKPI)
25973 {
25974 j6array
25975 [1]
25976 -= IK2PI;
25977 }
25978 else if (
25979 j6array
25980 [1]
25981 < -IKPI)
25982 {
25983 j6array
25984 [1]
25985 += IK2PI;
25986 }
25987 j6valid
25988 [1]
25989 = true;
25990 for (
25991 int ij6
25992 = 0;
25993 ij6
25994 < 2;
25995 ++ij6)
25996 {
25997 if (!j6valid
25998 [ij6])
25999 {
26000 continue;
26001 }
26002 _ij6[0]
26003 = ij6;
26004 _ij6[1]
26005 = -1;
26006 for (
26007 int iij6
26008 = ij6
26009 + 1;
26010 iij6
26011 < 2;
26012 ++iij6)
26013 {
26014 if (j6valid
26015 [iij6]
26016 && IKabs(
26017 cj6array
26018 [ij6]
26019 - cj6array
26020 [iij6])
26021 < IKFAST_SOLUTION_THRESH
26022 && IKabs(
26023 sj6array
26024 [ij6]
26025 - sj6array
26026 [iij6])
26027 < IKFAST_SOLUTION_THRESH)
26028 {
26029 j6valid
26030 [iij6]
26031 = false;
26032 _ij6[1]
26033 = iij6;
26034 break;
26035 }
26036 }
26037 j6 = j6array
26038 [ij6];
26039 cj6 = cj6array
26040 [ij6];
26041 sj6 = sj6array
26042 [ij6];
26043 {
26044 IkReal evalcond
26045 [1];
26046 evalcond
26047 [0]
26048 = ((0.85)
26049 * (IKsin(
26050 j6)));
26051 if (IKabs(
26052 evalcond
26053 [0])
26054 > IKFAST_EVALCOND_THRESH)
26055 {
26056 continue;
26057 }
26058 }
26059
26060 rotationfunction0(
26061 solutions);
26062 }
26063 }
26064 }
26065 }
26066 }
26067 } while (
26068 0);
26069 if (bgotonextstatement)
26070 {
26071 bool
26072 bgotonextstatement
26073 = true;
26074 do
26075 {
26076 evalcond
26077 [0]
26078 = ((IKabs((
26079 (-3.14159265358979)
26080 + (IKfmod(
26081 ((1.5707963267949)
26082 + j4),
26083 6.28318530717959)))))
26084 + (IKabs(
26085 py)));
26086 evalcond
26087 [1]
26088 = -0.85;
26089 evalcond
26090 [2]
26091 = 0;
26092 evalcond
26093 [3]
26094 = ((-0.2125)
26095 + (((-1.0)
26096 * (1.0)
26097 * (px
26098 * px))));
26099 if (IKabs(
26100 evalcond
26101 [0])
26102 < 0.0000010000000000
26103 && IKabs(
26104 evalcond
26105 [1])
26106 < 0.0000010000000000
26107 && IKabs(
26108 evalcond
26109 [2])
26110 < 0.0000010000000000
26111 && IKabs(
26112 evalcond
26113 [3])
26114 < 0.0000010000000000)
26115 {
26116 bgotonextstatement
26117 = false;
26118 {
26119 IkReal j6eval
26120 [1];
26121 IkReal
26122 x1861
26123 = ((1.0)
26124 * px);
26125 sj8 = 0;
26126 cj8 = 1.0;
26127 j8 = 0;
26128 pz = 0;
26129 j9 = 0;
26130 sj9 = 0;
26131 cj9 = 1.0;
26132 pp = px
26133 * px;
26134 npx
26135 = (px
26136 * r00);
26137 npy
26138 = (px
26139 * r01);
26140 npz
26141 = (px
26142 * r02);
26143 rxp0_0
26144 = 0;
26145 rxp0_1
26146 = (px
26147 * r20);
26148 rxp1_0
26149 = 0;
26150 rxp1_1
26151 = (px
26152 * r21);
26153 rxp2_0
26154 = 0;
26155 rxp2_1
26156 = (px
26157 * r22);
26158 py = 0;
26159 j4 = 1.5707963267949;
26160 sj4 = 1.0;
26161 cj4 = 0;
26162 rxp0_2
26163 = ((-1.0)
26164 * r10
26165 * x1861);
26166 rxp1_2
26167 = ((-1.0)
26168 * r11
26169 * x1861);
26170 rxp2_2
26171 = ((-1.0)
26172 * r12
26173 * x1861);
26174 j6eval
26175 [0]
26176 = ((1.0)
26177 + (((1.91568587540858)
26178 * (px
26179 * px
26180 * px
26181 * px)))
26182 + (((-1.0)
26183 * (2.64633970947792)
26184 * (px
26185 * px))));
26186 if (IKabs(
26187 j6eval
26188 [0])
26189 < 0.0000010000000000)
26190 {
26191 continue; // no branches [j6]
26192 }
26193 else
26194 {
26195 {
26196 IkReal j6array
26197 [2],
26198 cj6array
26199 [2],
26200 sj6array
26201 [2];
26202 bool j6valid
26203 [2]
26204 = {false};
26205 _nj6
26206 = 2;
26207 IkReal
26208 x1862
26209 = px
26210 * px;
26211 CheckValue<IkReal> x1864 = IKatan2WithCheck(
26212 IkReal((
26213 (-0.425)
26214 + (((-0.588235294117647)
26215 * x1862)))),
26216 ((-2.83333333333333)
26217 + (((3.92156862745098)
26218 * x1862))),
26219 IKFAST_ATAN2_MAGTHRESH);
26220 if (!x1864
26221 .valid)
26222 {
26223 continue;
26224 }
26225 IkReal
26226 x1863
26227 = ((-1.0)
26228 * (x1864
26229 .value));
26230 j6array
26231 [0]
26232 = x1863;
26233 sj6array
26234 [0]
26235 = IKsin(
26236 j6array
26237 [0]);
26238 cj6array
26239 [0]
26240 = IKcos(
26241 j6array
26242 [0]);
26243 j6array
26244 [1]
26245 = ((3.14159265358979)
26246 + x1863);
26247 sj6array
26248 [1]
26249 = IKsin(
26250 j6array
26251 [1]);
26252 cj6array
26253 [1]
26254 = IKcos(
26255 j6array
26256 [1]);
26257 if (j6array
26258 [0]
26259 > IKPI)
26260 {
26261 j6array
26262 [0]
26263 -= IK2PI;
26264 }
26265 else if (
26266 j6array
26267 [0]
26268 < -IKPI)
26269 {
26270 j6array
26271 [0]
26272 += IK2PI;
26273 }
26274 j6valid
26275 [0]
26276 = true;
26277 if (j6array
26278 [1]
26279 > IKPI)
26280 {
26281 j6array
26282 [1]
26283 -= IK2PI;
26284 }
26285 else if (
26286 j6array
26287 [1]
26288 < -IKPI)
26289 {
26290 j6array
26291 [1]
26292 += IK2PI;
26293 }
26294 j6valid
26295 [1]
26296 = true;
26297 for (
26298 int ij6
26299 = 0;
26300 ij6
26301 < 2;
26302 ++ij6)
26303 {
26304 if (!j6valid
26305 [ij6])
26306 {
26307 continue;
26308 }
26309 _ij6[0]
26310 = ij6;
26311 _ij6[1]
26312 = -1;
26313 for (
26314 int iij6
26315 = ij6
26316 + 1;
26317 iij6
26318 < 2;
26319 ++iij6)
26320 {
26321 if (j6valid
26322 [iij6]
26323 && IKabs(
26324 cj6array
26325 [ij6]
26326 - cj6array
26327 [iij6])
26328 < IKFAST_SOLUTION_THRESH
26329 && IKabs(
26330 sj6array
26331 [ij6]
26332 - sj6array
26333 [iij6])
26334 < IKFAST_SOLUTION_THRESH)
26335 {
26336 j6valid
26337 [iij6]
26338 = false;
26339 _ij6[1]
26340 = iij6;
26341 break;
26342 }
26343 }
26344 j6 = j6array
26345 [ij6];
26346 cj6 = cj6array
26347 [ij6];
26348 sj6 = sj6array
26349 [ij6];
26350 {
26351 IkReal evalcond
26352 [1];
26353 evalcond
26354 [0]
26355 = ((0.85)
26356 * (IKsin(
26357 j6)));
26358 if (IKabs(
26359 evalcond
26360 [0])
26361 > IKFAST_EVALCOND_THRESH)
26362 {
26363 continue;
26364 }
26365 }
26366
26367 rotationfunction0(
26368 solutions);
26369 }
26370 }
26371 }
26372 }
26373 }
26374 } while (
26375 0);
26376 if (bgotonextstatement)
26377 {
26378 bool
26379 bgotonextstatement
26380 = true;
26381 do
26382 {
26383 evalcond
26384 [0]
26385 = ((IKabs(
26386 py))
26387 + (IKabs((
26388 (-3.14159265358979)
26389 + (IKfmod(
26390 ((4.71238898038469)
26391 + j4),
26392 6.28318530717959))))));
26393 evalcond
26394 [1]
26395 = -0.85;
26396 evalcond
26397 [2]
26398 = 0;
26399 evalcond
26400 [3]
26401 = ((-0.2125)
26402 + (((-1.0)
26403 * (1.0)
26404 * (px
26405 * px))));
26406 if (IKabs(
26407 evalcond
26408 [0])
26409 < 0.0000010000000000
26410 && IKabs(
26411 evalcond
26412 [1])
26413 < 0.0000010000000000
26414 && IKabs(
26415 evalcond
26416 [2])
26417 < 0.0000010000000000
26418 && IKabs(
26419 evalcond
26420 [3])
26421 < 0.0000010000000000)
26422 {
26423 bgotonextstatement
26424 = false;
26425 {
26426 IkReal j6eval
26427 [1];
26428 IkReal
26429 x1865
26430 = ((1.0)
26431 * px);
26432 sj8 = 0;
26433 cj8 = 1.0;
26434 j8 = 0;
26435 pz = 0;
26436 j9 = 0;
26437 sj9 = 0;
26438 cj9 = 1.0;
26439 pp = px
26440 * px;
26441 npx
26442 = (px
26443 * r00);
26444 npy
26445 = (px
26446 * r01);
26447 npz
26448 = (px
26449 * r02);
26450 rxp0_0
26451 = 0;
26452 rxp0_1
26453 = (px
26454 * r20);
26455 rxp1_0
26456 = 0;
26457 rxp1_1
26458 = (px
26459 * r21);
26460 rxp2_0
26461 = 0;
26462 rxp2_1
26463 = (px
26464 * r22);
26465 py = 0;
26466 j4 = -1.5707963267949;
26467 sj4 = -1.0;
26468 cj4 = 0;
26469 rxp0_2
26470 = ((-1.0)
26471 * r10
26472 * x1865);
26473 rxp1_2
26474 = ((-1.0)
26475 * r11
26476 * x1865);
26477 rxp2_2
26478 = ((-1.0)
26479 * r12
26480 * x1865);
26481 j6eval
26482 [0]
26483 = ((1.0)
26484 + (((1.91568587540858)
26485 * (px
26486 * px
26487 * px
26488 * px)))
26489 + (((-1.0)
26490 * (2.64633970947792)
26491 * (px
26492 * px))));
26493 if (IKabs(
26494 j6eval
26495 [0])
26496 < 0.0000010000000000)
26497 {
26498 continue; // no branches [j6]
26499 }
26500 else
26501 {
26502 {
26503 IkReal j6array
26504 [2],
26505 cj6array
26506 [2],
26507 sj6array
26508 [2];
26509 bool j6valid
26510 [2]
26511 = {false};
26512 _nj6
26513 = 2;
26514 IkReal
26515 x1866
26516 = px
26517 * px;
26518 CheckValue<IkReal> x1868 = IKatan2WithCheck(
26519 IkReal((
26520 (-0.425)
26521 + (((-0.588235294117647)
26522 * x1866)))),
26523 ((-2.83333333333333)
26524 + (((3.92156862745098)
26525 * x1866))),
26526 IKFAST_ATAN2_MAGTHRESH);
26527 if (!x1868
26528 .valid)
26529 {
26530 continue;
26531 }
26532 IkReal
26533 x1867
26534 = ((-1.0)
26535 * (x1868
26536 .value));
26537 j6array
26538 [0]
26539 = x1867;
26540 sj6array
26541 [0]
26542 = IKsin(
26543 j6array
26544 [0]);
26545 cj6array
26546 [0]
26547 = IKcos(
26548 j6array
26549 [0]);
26550 j6array
26551 [1]
26552 = ((3.14159265358979)
26553 + x1867);
26554 sj6array
26555 [1]
26556 = IKsin(
26557 j6array
26558 [1]);
26559 cj6array
26560 [1]
26561 = IKcos(
26562 j6array
26563 [1]);
26564 if (j6array
26565 [0]
26566 > IKPI)
26567 {
26568 j6array
26569 [0]
26570 -= IK2PI;
26571 }
26572 else if (
26573 j6array
26574 [0]
26575 < -IKPI)
26576 {
26577 j6array
26578 [0]
26579 += IK2PI;
26580 }
26581 j6valid
26582 [0]
26583 = true;
26584 if (j6array
26585 [1]
26586 > IKPI)
26587 {
26588 j6array
26589 [1]
26590 -= IK2PI;
26591 }
26592 else if (
26593 j6array
26594 [1]
26595 < -IKPI)
26596 {
26597 j6array
26598 [1]
26599 += IK2PI;
26600 }
26601 j6valid
26602 [1]
26603 = true;
26604 for (
26605 int ij6
26606 = 0;
26607 ij6
26608 < 2;
26609 ++ij6)
26610 {
26611 if (!j6valid
26612 [ij6])
26613 {
26614 continue;
26615 }
26616 _ij6[0]
26617 = ij6;
26618 _ij6[1]
26619 = -1;
26620 for (
26621 int iij6
26622 = ij6
26623 + 1;
26624 iij6
26625 < 2;
26626 ++iij6)
26627 {
26628 if (j6valid
26629 [iij6]
26630 && IKabs(
26631 cj6array
26632 [ij6]
26633 - cj6array
26634 [iij6])
26635 < IKFAST_SOLUTION_THRESH
26636 && IKabs(
26637 sj6array
26638 [ij6]
26639 - sj6array
26640 [iij6])
26641 < IKFAST_SOLUTION_THRESH)
26642 {
26643 j6valid
26644 [iij6]
26645 = false;
26646 _ij6[1]
26647 = iij6;
26648 break;
26649 }
26650 }
26651 j6 = j6array
26652 [ij6];
26653 cj6 = cj6array
26654 [ij6];
26655 sj6 = sj6array
26656 [ij6];
26657 {
26658 IkReal evalcond
26659 [1];
26660 evalcond
26661 [0]
26662 = ((0.85)
26663 * (IKsin(
26664 j6)));
26665 if (IKabs(
26666 evalcond
26667 [0])
26668 > IKFAST_EVALCOND_THRESH)
26669 {
26670 continue;
26671 }
26672 }
26673
26674 rotationfunction0(
26675 solutions);
26676 }
26677 }
26678 }
26679 }
26680 }
26681 } while (
26682 0);
26683 if (bgotonextstatement)
26684 {
26685 bool
26686 bgotonextstatement
26687 = true;
26688 do
26689 {
26690 if (1)
26691 {
26692 bgotonextstatement
26693 = false;
26694 continue; // branch miss [j6]
26695 }
26696 } while (
26697 0);
26698 if (bgotonextstatement)
26699 {
26700 }
26701 }
26702 }
26703 }
26704 }
26705 }
26706 }
26707 }
26708 else
26709 {
26710 {
26711 IkReal
26712 j6array
26713 [1],
26714 cj6array
26715 [1],
26716 sj6array
26717 [1];
26718 bool j6valid
26719 [1]
26720 = {false};
26721 _nj6 = 1;
26722 IkReal x1869
26723 = (cj4
26724 * px);
26725 IkReal x1870
26726 = (py
26727 * sj4);
26728 IkReal x1871
26729 = px
26730 * px;
26731 IkReal x1872
26732 = py
26733 * py;
26734 CheckValue<
26735 IkReal>
26736 x1873
26737 = IKPowWithIntegerCheck(
26738 ((((20.0)
26739 * x1870))
26740 + (((20.0)
26741 * x1869))),
26742 -1);
26743 if (!x1873
26744 .valid)
26745 {
26746 continue;
26747 }
26748 CheckValue<IkReal> x1874 = IKPowWithIntegerCheck(
26749 ((((-1.0)
26750 * (11.7647058823529)
26751 * cj4
26752 * (px
26753 * px
26754 * px)))
26755 + (((-11.7647058823529)
26756 * x1869
26757 * x1872))
26758 + (((-8.5)
26759 * x1869))
26760 + (((-8.5)
26761 * x1870))
26762 + (((-11.7647058823529)
26763 * x1870
26764 * x1871))
26765 + (((-1.0)
26766 * (11.7647058823529)
26767 * sj4
26768 * (py
26769 * py
26770 * py)))),
26771 -1);
26772 if (!x1874
26773 .valid)
26774 {
26775 continue;
26776 }
26777 if (IKabs((
26778 (17.0)
26779 * (x1873
26780 .value)))
26781 < IKFAST_ATAN2_MAGTHRESH
26782 && IKabs((
26783 (x1874
26784 .value)
26785 * (((48.1666666666667)
26786 + (((-66.6666666666667)
26787 * x1872))
26788 + (((-66.6666666666667)
26789 * x1871))))))
26790 < IKFAST_ATAN2_MAGTHRESH
26791 && IKabs(
26792 IKsqr((
26793 (17.0)
26794 * (x1873
26795 .value)))
26796 + IKsqr((
26797 (x1874
26798 .value)
26799 * (((48.1666666666667)
26800 + (((-66.6666666666667)
26801 * x1872))
26802 + (((-66.6666666666667)
26803 * x1871))))))
26804 - 1)
26805 <= IKFAST_SINCOS_THRESH)
26806 continue;
26807 j6array[0] = IKatan2(
26808 ((17.0)
26809 * (x1873
26810 .value)),
26811 ((x1874
26812 .value)
26813 * (((48.1666666666667)
26814 + (((-66.6666666666667)
26815 * x1872))
26816 + (((-66.6666666666667)
26817 * x1871))))));
26818 sj6array[0] = IKsin(
26819 j6array
26820 [0]);
26821 cj6array[0] = IKcos(
26822 j6array
26823 [0]);
26824 if (j6array
26825 [0]
26826 > IKPI)
26827 {
26828 j6array[0]
26829 -= IK2PI;
26830 }
26831 else if (
26832 j6array
26833 [0]
26834 < -IKPI)
26835 {
26836 j6array[0]
26837 += IK2PI;
26838 }
26839 j6valid[0]
26840 = true;
26841 for (int ij6
26842 = 0;
26843 ij6
26844 < 1;
26845 ++ij6)
26846 {
26847 if (!j6valid
26848 [ij6])
26849 {
26850 continue;
26851 }
26852 _ij6[0]
26853 = ij6;
26854 _ij6[1]
26855 = -1;
26856 for (
26857 int iij6
26858 = ij6
26859 + 1;
26860 iij6
26861 < 1;
26862 ++iij6)
26863 {
26864 if (j6valid
26865 [iij6]
26866 && IKabs(
26867 cj6array
26868 [ij6]
26869 - cj6array
26870 [iij6])
26871 < IKFAST_SOLUTION_THRESH
26872 && IKabs(
26873 sj6array
26874 [ij6]
26875 - sj6array
26876 [iij6])
26877 < IKFAST_SOLUTION_THRESH)
26878 {
26879 j6valid
26880 [iij6]
26881 = false;
26882 _ij6[1]
26883 = iij6;
26884 break;
26885 }
26886 }
26887 j6 = j6array
26888 [ij6];
26889 cj6 = cj6array
26890 [ij6];
26891 sj6 = sj6array
26892 [ij6];
26893 {
26894 IkReal evalcond
26895 [5];
26896 IkReal
26897 x1875
26898 = IKsin(
26899 j6);
26900 IkReal
26901 x1876
26902 = (cj4
26903 * px);
26904 IkReal
26905 x1877
26906 = (x1875
26907 * x1876);
26908 IkReal
26909 x1878
26910 = (py
26911 * sj4);
26912 IkReal
26913 x1879
26914 = (x1875
26915 * x1878);
26916 IkReal
26917 x1880
26918 = ((1.0)
26919 * x1876);
26920 IkReal
26921 x1881
26922 = ((1.0)
26923 * x1878);
26924 IkReal
26925 x1882
26926 = IKcos(
26927 j6);
26928 IkReal
26929 x1883
26930 = px
26931 * px;
26932 IkReal
26933 x1884
26934 = ((3.92156862745098)
26935 * x1875);
26936 IkReal
26937 x1885
26938 = ((0.588235294117647)
26939 * x1882);
26940 IkReal
26941 x1886
26942 = py
26943 * py;
26944 IkReal
26945 x1887
26946 = ((0.09)
26947 * x1882);
26948 evalcond
26949 [0]
26950 = ((-0.85)
26951 + x1879
26952 + x1877);
26953 evalcond
26954 [1]
26955 = ((((-1.0)
26956 * x1880))
26957 + (((0.85)
26958 * x1875))
26959 + (((-1.0)
26960 * x1881)));
26961 evalcond
26962 [2]
26963 = ((((-1.0)
26964 * x1881
26965 * x1882))
26966 + (((-1.0)
26967 * x1880
26968 * x1882)));
26969 evalcond
26970 [3]
26971 = ((((-0.425)
26972 * x1882))
26973 + (((-1.0)
26974 * x1885
26975 * x1886))
26976 + ((x1884
26977 * x1886))
26978 + (((-1.0)
26979 * x1883
26980 * x1885))
26981 + ((x1883
26982 * x1884))
26983 + (((-2.83333333333333)
26984 * x1875)));
26985 evalcond
26986 [4]
26987 = ((-0.2125)
26988 + (((1.1)
26989 * x1879))
26990 + (((-1.0)
26991 * x1883))
26992 + (((-1.0)
26993 * x1886))
26994 + (((1.1)
26995 * x1877))
26996 + ((x1876
26997 * x1887))
26998 + ((x1878
26999 * x1887)));
27000 if (IKabs(
27001 evalcond
27002 [0])
27003 > IKFAST_EVALCOND_THRESH
27004 || IKabs(
27005 evalcond
27006 [1])
27007 > IKFAST_EVALCOND_THRESH
27008 || IKabs(
27009 evalcond
27010 [2])
27011 > IKFAST_EVALCOND_THRESH
27012 || IKabs(
27013 evalcond
27014 [3])
27015 > IKFAST_EVALCOND_THRESH
27016 || IKabs(
27017 evalcond
27018 [4])
27019 > IKFAST_EVALCOND_THRESH)
27020 {
27021 continue;
27022 }
27023 }
27024
27025 rotationfunction0(
27026 solutions);
27027 }
27028 }
27029 }
27030 }
27031 }
27032 else
27033 {
27034 {
27035 IkReal
27036 j6array[1],
27037 cj6array[1],
27038 sj6array[1];
27039 bool j6valid[1]
27040 = {false};
27041 _nj6 = 1;
27042 IkReal x1888
27043 = (cj4
27044 * px);
27045 IkReal x1889
27046 = (py
27047 * sj4);
27048 IkReal x1890
27049 = px * px;
27050 IkReal x1891
27051 = py * py;
27052 CheckValue<
27053 IkReal>
27054 x1892
27055 = IKPowWithIntegerCheck(
27056 ((-7.225)
27057 + (((-10.0)
27058 * x1891))
27059 + (((-10.0)
27060 * x1890))),
27061 -1);
27062 if (!x1892
27063 .valid)
27064 {
27065 continue;
27066 }
27067 if (IKabs((
27068 (((1.17647058823529)
27069 * x1888))
27070 + (((1.17647058823529)
27071 * x1889))))
27072 < IKFAST_ATAN2_MAGTHRESH
27073 && IKabs((
27074 (x1892
27075 .value)
27076 * (((((56.6666666666667)
27077 * x1889))
27078 + (((56.6666666666667)
27079 * x1888))
27080 + (((-1.0)
27081 * (78.4313725490196)
27082 * sj4
27083 * (py
27084 * py
27085 * py)))
27086 + (((-78.4313725490196)
27087 * x1888
27088 * x1891))
27089 + (((-1.0)
27090 * (78.4313725490196)
27091 * cj4
27092 * (px
27093 * px
27094 * px)))
27095 + (((-78.4313725490196)
27096 * x1889
27097 * x1890))))))
27098 < IKFAST_ATAN2_MAGTHRESH
27099 && IKabs(
27100 IKsqr((
27101 (((1.17647058823529)
27102 * x1888))
27103 + (((1.17647058823529)
27104 * x1889))))
27105 + IKsqr((
27106 (x1892
27107 .value)
27108 * (((((56.6666666666667)
27109 * x1889))
27110 + (((56.6666666666667)
27111 * x1888))
27112 + (((-1.0)
27113 * (78.4313725490196)
27114 * sj4
27115 * (py
27116 * py
27117 * py)))
27118 + (((-78.4313725490196)
27119 * x1888
27120 * x1891))
27121 + (((-1.0)
27122 * (78.4313725490196)
27123 * cj4
27124 * (px
27125 * px
27126 * px)))
27127 + (((-78.4313725490196)
27128 * x1889
27129 * x1890))))))
27130 - 1)
27131 <= IKFAST_SINCOS_THRESH)
27132 continue;
27133 j6array[0] = IKatan2(
27134 ((((1.17647058823529)
27135 * x1888))
27136 + (((1.17647058823529)
27137 * x1889))),
27138 ((x1892
27139 .value)
27140 * (((((56.6666666666667)
27141 * x1889))
27142 + (((56.6666666666667)
27143 * x1888))
27144 + (((-1.0)
27145 * (78.4313725490196)
27146 * sj4
27147 * (py
27148 * py
27149 * py)))
27150 + (((-78.4313725490196)
27151 * x1888
27152 * x1891))
27153 + (((-1.0)
27154 * (78.4313725490196)
27155 * cj4
27156 * (px
27157 * px
27158 * px)))
27159 + (((-78.4313725490196)
27160 * x1889
27161 * x1890))))));
27162 sj6array[0]
27163 = IKsin(
27164 j6array
27165 [0]);
27166 cj6array[0]
27167 = IKcos(
27168 j6array
27169 [0]);
27170 if (j6array[0]
27171 > IKPI)
27172 {
27173 j6array[0]
27174 -= IK2PI;
27175 }
27176 else if (
27177 j6array[0]
27178 < -IKPI)
27179 {
27180 j6array[0]
27181 += IK2PI;
27182 }
27183 j6valid[0]
27184 = true;
27185 for (int ij6
27186 = 0;
27187 ij6 < 1;
27188 ++ij6)
27189 {
27190 if (!j6valid
27191 [ij6])
27192 {
27193 continue;
27194 }
27195 _ij6[0] = ij6;
27196 _ij6[1] = -1;
27197 for (int iij6
27198 = ij6
27199 + 1;
27200 iij6 < 1;
27201 ++iij6)
27202 {
27203 if (j6valid
27204 [iij6]
27205 && IKabs(
27206 cj6array
27207 [ij6]
27208 - cj6array
27209 [iij6])
27210 < IKFAST_SOLUTION_THRESH
27211 && IKabs(
27212 sj6array
27213 [ij6]
27214 - sj6array
27215 [iij6])
27216 < IKFAST_SOLUTION_THRESH)
27217 {
27218 j6valid
27219 [iij6]
27220 = false;
27221 _ij6[1]
27222 = iij6;
27223 break;
27224 }
27225 }
27226 j6 = j6array
27227 [ij6];
27228 cj6 = cj6array
27229 [ij6];
27230 sj6 = sj6array
27231 [ij6];
27232 {
27233 IkReal
27234 evalcond
27235 [5];
27236 IkReal x1893
27237 = IKsin(
27238 j6);
27239 IkReal x1894
27240 = (cj4
27241 * px);
27242 IkReal x1895
27243 = (x1893
27244 * x1894);
27245 IkReal x1896
27246 = (py
27247 * sj4);
27248 IkReal x1897
27249 = (x1893
27250 * x1896);
27251 IkReal x1898
27252 = ((1.0)
27253 * x1894);
27254 IkReal x1899
27255 = ((1.0)
27256 * x1896);
27257 IkReal x1900
27258 = IKcos(
27259 j6);
27260 IkReal x1901
27261 = px
27262 * px;
27263 IkReal x1902
27264 = ((3.92156862745098)
27265 * x1893);
27266 IkReal x1903
27267 = ((0.588235294117647)
27268 * x1900);
27269 IkReal x1904
27270 = py
27271 * py;
27272 IkReal x1905
27273 = ((0.09)
27274 * x1900);
27275 evalcond[0]
27276 = ((-0.85)
27277 + x1897
27278 + x1895);
27279 evalcond[1]
27280 = ((((-1.0)
27281 * x1898))
27282 + (((0.85)
27283 * x1893))
27284 + (((-1.0)
27285 * x1899)));
27286 evalcond[2]
27287 = ((((-1.0)
27288 * x1898
27289 * x1900))
27290 + (((-1.0)
27291 * x1899
27292 * x1900)));
27293 evalcond[3]
27294 = ((((-1.0)
27295 * x1901
27296 * x1903))
27297 + (((-1.0)
27298 * x1903
27299 * x1904))
27300 + ((x1902
27301 * x1904))
27302 + ((x1901
27303 * x1902))
27304 + (((-0.425)
27305 * x1900))
27306 + (((-2.83333333333333)
27307 * x1893)));
27308 evalcond[4]
27309 = ((-0.2125)
27310 + (((-1.0)
27311 * x1901))
27312 + (((1.1)
27313 * x1895))
27314 + (((1.1)
27315 * x1897))
27316 + (((-1.0)
27317 * x1904))
27318 + ((x1894
27319 * x1905))
27320 + ((x1896
27321 * x1905)));
27322 if (IKabs(
27323 evalcond
27324 [0])
27325 > IKFAST_EVALCOND_THRESH
27326 || IKabs(
27327 evalcond
27328 [1])
27329 > IKFAST_EVALCOND_THRESH
27330 || IKabs(
27331 evalcond
27332 [2])
27333 > IKFAST_EVALCOND_THRESH
27334 || IKabs(
27335 evalcond
27336 [3])
27337 > IKFAST_EVALCOND_THRESH
27338 || IKabs(
27339 evalcond
27340 [4])
27341 > IKFAST_EVALCOND_THRESH)
27342 {
27343 continue;
27344 }
27345 }
27346
27347 rotationfunction0(
27348 solutions);
27349 }
27350 }
27351 }
27352 }
27353 }
27354 else
27355 {
27356 {
27357 IkReal j6array[1],
27358 cj6array[1],
27359 sj6array[1];
27360 bool j6valid[1]
27361 = {false};
27362 _nj6 = 1;
27363 IkReal x1906
27364 = (cj4 * px);
27365 IkReal x1907
27366 = (py * sj4);
27367 IkReal x1908
27368 = px * px;
27369 IkReal x1909
27370 = py * py;
27371 IkReal x1910
27372 = ((1.29411764705882)
27373 * (cj4
27374 * cj4));
27375 CheckValue<IkReal>
27376 x1911
27377 = IKPowWithIntegerCheck(
27378 ((((0.09)
27379 * x1907))
27380 + (((0.09)
27381 * x1906))),
27382 -1);
27383 if (!x1911.valid)
27384 {
27385 continue;
27386 }
27387 if (IKabs((
27388 (((1.17647058823529)
27389 * x1907))
27390 + (((1.17647058823529)
27391 * x1906))))
27392 < IKFAST_ATAN2_MAGTHRESH
27393 && IKabs((
27394 (x1911
27395 .value)
27396 * (((0.2125)
27397 + (((-1.0)
27398 * x1908
27399 * x1910))
27400 + ((x1909
27401 * x1910))
27402 + x1908
27403 + (((-0.294117647058824)
27404 * x1909))
27405 + (((-2.58823529411765)
27406 * cj4
27407 * px
27408 * x1907))))))
27409 < IKFAST_ATAN2_MAGTHRESH
27410 && IKabs(
27411 IKsqr((
27412 (((1.17647058823529)
27413 * x1907))
27414 + (((1.17647058823529)
27415 * x1906))))
27416 + IKsqr((
27417 (x1911
27418 .value)
27419 * (((0.2125)
27420 + (((-1.0)
27421 * x1908
27422 * x1910))
27423 + ((x1909
27424 * x1910))
27425 + x1908
27426 + (((-0.294117647058824)
27427 * x1909))
27428 + (((-2.58823529411765)
27429 * cj4
27430 * px
27431 * x1907))))))
27432 - 1)
27433 <= IKFAST_SINCOS_THRESH)
27434 continue;
27435 j6array[0] = IKatan2(
27436 ((((1.17647058823529)
27437 * x1907))
27438 + (((1.17647058823529)
27439 * x1906))),
27440 ((x1911.value)
27441 * (((0.2125)
27442 + (((-1.0)
27443 * x1908
27444 * x1910))
27445 + ((x1909
27446 * x1910))
27447 + x1908
27448 + (((-0.294117647058824)
27449 * x1909))
27450 + (((-2.58823529411765)
27451 * cj4
27452 * px
27453 * x1907))))));
27454 sj6array[0] = IKsin(
27455 j6array[0]);
27456 cj6array[0] = IKcos(
27457 j6array[0]);
27458 if (j6array[0]
27459 > IKPI)
27460 {
27461 j6array[0]
27462 -= IK2PI;
27463 }
27464 else if (
27465 j6array[0]
27466 < -IKPI)
27467 {
27468 j6array[0]
27469 += IK2PI;
27470 }
27471 j6valid[0] = true;
27472 for (int ij6 = 0;
27473 ij6 < 1;
27474 ++ij6)
27475 {
27476 if (!j6valid[ij6])
27477 {
27478 continue;
27479 }
27480 _ij6[0] = ij6;
27481 _ij6[1] = -1;
27482 for (int iij6
27483 = ij6 + 1;
27484 iij6 < 1;
27485 ++iij6)
27486 {
27487 if (j6valid
27488 [iij6]
27489 && IKabs(
27490 cj6array
27491 [ij6]
27492 - cj6array
27493 [iij6])
27494 < IKFAST_SOLUTION_THRESH
27495 && IKabs(
27496 sj6array
27497 [ij6]
27498 - sj6array
27499 [iij6])
27500 < IKFAST_SOLUTION_THRESH)
27501 {
27502 j6valid[iij6]
27503 = false;
27504 _ij6[1]
27505 = iij6;
27506 break;
27507 }
27508 }
27509 j6 = j6array[ij6];
27510 cj6 = cj6array
27511 [ij6];
27512 sj6 = sj6array
27513 [ij6];
27514 {
27515 IkReal
27516 evalcond[5];
27517 IkReal x1912
27518 = IKsin(j6);
27519 IkReal x1913
27520 = (cj4
27521 * px);
27522 IkReal x1914
27523 = (x1912
27524 * x1913);
27525 IkReal x1915
27526 = (py
27527 * sj4);
27528 IkReal x1916
27529 = (x1912
27530 * x1915);
27531 IkReal x1917
27532 = ((1.0)
27533 * x1913);
27534 IkReal x1918
27535 = ((1.0)
27536 * x1915);
27537 IkReal x1919
27538 = IKcos(j6);
27539 IkReal x1920
27540 = px * px;
27541 IkReal x1921
27542 = ((3.92156862745098)
27543 * x1912);
27544 IkReal x1922
27545 = ((0.588235294117647)
27546 * x1919);
27547 IkReal x1923
27548 = py * py;
27549 IkReal x1924
27550 = ((0.09)
27551 * x1919);
27552 evalcond[0]
27553 = ((-0.85)
27554 + x1914
27555 + x1916);
27556 evalcond[1]
27557 = ((((0.85)
27558 * x1912))
27559 + (((-1.0)
27560 * x1918))
27561 + (((-1.0)
27562 * x1917)));
27563 evalcond[2]
27564 = ((((-1.0)
27565 * x1917
27566 * x1919))
27567 + (((-1.0)
27568 * x1918
27569 * x1919)));
27570 evalcond[3]
27571 = ((((-1.0)
27572 * x1920
27573 * x1922))
27574 + (((-1.0)
27575 * x1922
27576 * x1923))
27577 + ((x1921
27578 * x1923))
27579 + ((x1920
27580 * x1921))
27581 + (((-0.425)
27582 * x1919))
27583 + (((-2.83333333333333)
27584 * x1912)));
27585 evalcond[4]
27586 = ((-0.2125)
27587 + ((x1913
27588 * x1924))
27589 + (((-1.0)
27590 * x1923))
27591 + (((1.1)
27592 * x1916))
27593 + ((x1915
27594 * x1924))
27595 + (((1.1)
27596 * x1914))
27597 + (((-1.0)
27598 * x1920)));
27599 if (IKabs(
27600 evalcond
27601 [0])
27602 > IKFAST_EVALCOND_THRESH
27603 || IKabs(
27604 evalcond
27605 [1])
27606 > IKFAST_EVALCOND_THRESH
27607 || IKabs(
27608 evalcond
27609 [2])
27610 > IKFAST_EVALCOND_THRESH
27611 || IKabs(
27612 evalcond
27613 [3])
27614 > IKFAST_EVALCOND_THRESH
27615 || IKabs(
27616 evalcond
27617 [4])
27618 > IKFAST_EVALCOND_THRESH)
27619 {
27620 continue;
27621 }
27622 }
27623
27624 rotationfunction0(
27625 solutions);
27626 }
27627 }
27628 }
27629 }
27630 }
27631 } while (0);
27632 if (bgotonextstatement)
27633 {
27634 bool bgotonextstatement
27635 = true;
27636 do
27637 {
27638 if (1)
27639 {
27640 bgotonextstatement
27641 = false;
27642 continue; // branch miss
27643 // [j6]
27644 }
27645 } while (0);
27646 if (bgotonextstatement)
27647 {
27648 }
27649 }
27650 }
27651 }
27652 else
27653 {
27654 {
27655 IkReal j6array[1],
27656 cj6array[1], sj6array[1];
27657 bool j6valid[1] = {false};
27658 _nj6 = 1;
27659 IkReal x1925 = (cj4 * px);
27660 IkReal x1926 = (py * sj4);
27661 IkReal x1927
27662 = ((0.108264705882353)
27663 * cj9);
27664 IkReal x1928
27665 = ((0.588235294117647)
27666 * pp);
27667 IkReal x1929 = (cj9 * pp);
27668 IkReal x1930 = (cj9 * sj9);
27669 IkReal x1931 = (pp * sj9);
27670 IkReal x1932 = cj9 * cj9;
27671 IkReal x1933 = ((1.0) * pz);
27672 CheckValue<IkReal> x1934
27673 = IKPowWithIntegerCheck(
27674 IKsign((
27675 (((1.51009803921569)
27676 * pz))
27677 + (((1.32323529411765)
27678 * cj9 * pz))
27679 + (((-1.0)
27680 * (3.92156862745098)
27681 * pp * pz))
27682 + (((-1.0) * x1926
27683 * x1928))
27684 + (((-1.0) * x1926
27685 * x1927))
27686 + (((-1.0) * x1925
27687 * x1927))
27688 + (((-0.316735294117647)
27689 * x1925))
27690 + (((-0.316735294117647)
27691 * x1926))
27692 + (((-1.0) * x1925
27693 * x1928)))),
27694 -1);
27695 if (!x1934.valid)
27696 {
27697 continue;
27698 }
27699 CheckValue<IkReal> x1935 = IKatan2WithCheck(
27700 IkReal((
27701 (-0.174204411764706)
27702 + (((-0.00487191176470588)
27703 * x1930))
27704 + (((-0.0264705882352941)
27705 * x1931))
27706 + (pz * pz)
27707 + (((-0.176470588235294)
27708 * x1929))
27709 + (((-1.0)
27710 * (0.154566176470588)
27711 * cj9))
27712 + (((-0.0324794117647059)
27713 * x1932))
27714 + (((-1.0)
27715 * (0.323529411764706)
27716 * pp))
27717 + (((-1.0)
27718 * (0.0142530882352941)
27719 * sj9)))),
27720 ((0.830553921568627)
27721 + (((-1.17647058823529)
27722 * x1929))
27723 + (((-0.176470588235294)
27724 * x1931))
27725 + (((0.396970588235294)
27726 * x1932))
27727 + (((-1.0) * x1926
27728 * x1933))
27729 + (((1.18080882352941)
27730 * cj9))
27731 + (((-1.0)
27732 * (2.15686274509804)
27733 * pp))
27734 + (((-1.0) * x1925
27735 * x1933))
27736 + (((0.0679544117647059)
27737 * sj9))
27738 + (((0.0595455882352941)
27739 * x1930))),
27740 IKFAST_ATAN2_MAGTHRESH);
27741 if (!x1935.valid)
27742 {
27743 continue;
27744 }
27745 j6array[0]
27746 = ((-1.5707963267949)
27747 + (((1.5707963267949)
27748 * (x1934.value)))
27749 + (x1935.value));
27750 sj6array[0]
27751 = IKsin(j6array[0]);
27752 cj6array[0]
27753 = IKcos(j6array[0]);
27754 if (j6array[0] > IKPI)
27755 {
27756 j6array[0] -= IK2PI;
27757 }
27758 else if (j6array[0] < -IKPI)
27759 {
27760 j6array[0] += IK2PI;
27761 }
27762 j6valid[0] = true;
27763 for (int ij6 = 0; ij6 < 1;
27764 ++ij6)
27765 {
27766 if (!j6valid[ij6])
27767 {
27768 continue;
27769 }
27770 _ij6[0] = ij6;
27771 _ij6[1] = -1;
27772 for (int iij6 = ij6 + 1;
27773 iij6 < 1;
27774 ++iij6)
27775 {
27776 if (j6valid[iij6]
27777 && IKabs(
27778 cj6array[ij6]
27779 - cj6array
27780 [iij6])
27781 < IKFAST_SOLUTION_THRESH
27782 && IKabs(
27783 sj6array[ij6]
27784 - sj6array
27785 [iij6])
27786 < IKFAST_SOLUTION_THRESH)
27787 {
27788 j6valid[iij6] = false;
27789 _ij6[1] = iij6;
27790 break;
27791 }
27792 }
27793 j6 = j6array[ij6];
27794 cj6 = cj6array[ij6];
27795 sj6 = sj6array[ij6];
27796 {
27797 IkReal evalcond[5];
27798 IkReal x1936
27799 = ((0.3) * cj9);
27800 IkReal x1937
27801 = ((0.045) * sj9);
27802 IkReal x1938 = IKcos(j6);
27803 IkReal x1939
27804 = (pz * x1938);
27805 IkReal x1940 = IKsin(j6);
27806 IkReal x1941 = (cj4 * px);
27807 IkReal x1942
27808 = (x1940 * x1941);
27809 IkReal x1943 = (py * sj4);
27810 IkReal x1944
27811 = (x1940 * x1943);
27812 IkReal x1945
27813 = ((0.045) * cj9);
27814 IkReal x1946
27815 = ((0.3) * sj9);
27816 IkReal x1947
27817 = (pz * x1940);
27818 IkReal x1948
27819 = ((1.0) * x1941);
27820 IkReal x1949
27821 = ((1.0) * x1943);
27822 IkReal x1950
27823 = ((0.09) * x1938);
27824 evalcond[0]
27825 = ((-0.55) + x1944
27826 + x1942 + x1939
27827 + (((-1.0)
27828 * x1937))
27829 + (((-1.0)
27830 * x1936)));
27831 evalcond[1]
27832 = ((0.045) + x1946
27833 + x1947
27834 + (((-1.0)
27835 * x1945))
27836 + (((-1.0) * x1938
27837 * x1948))
27838 + (((-1.0) * x1938
27839 * x1949)));
27840 evalcond[2]
27841 = ((((-1.51009803921569)
27842 * x1940))
27843 + pz
27844 + (((3.92156862745098)
27845 * pp * x1940))
27846 + (((-0.588235294117647)
27847 * pp * x1938))
27848 + (((-0.108264705882353)
27849 * cj9 * x1938))
27850 + (((-1.32323529411765)
27851 * cj9 * x1940))
27852 + (((-0.316735294117647)
27853 * x1938)));
27854 evalcond[3]
27855 = ((((-1.0) * x1948))
27856 + ((x1936 * x1940))
27857 + (((0.55)
27858 * x1940))
27859 + (((-1.0) * x1938
27860 * x1945))
27861 + (((-1.0)
27862 * x1949))
27863 + (((0.045)
27864 * x1938))
27865 + ((x1937 * x1940))
27866 + ((x1938
27867 * x1946)));
27868 evalcond[4]
27869 = ((-0.2125)
27870 + (((1.1) * x1944))
27871 + ((x1943 * x1950))
27872 + ((x1941 * x1950))
27873 + (((1.1) * x1942))
27874 + (((1.1) * x1939))
27875 + (((-0.09)
27876 * x1947))
27877 + (((-1.0) * (1.0)
27878 * pp)));
27879 if (IKabs(evalcond[0])
27880 > IKFAST_EVALCOND_THRESH
27881 || IKabs(evalcond[1])
27882 > IKFAST_EVALCOND_THRESH
27883 || IKabs(evalcond[2])
27884 > IKFAST_EVALCOND_THRESH
27885 || IKabs(evalcond[3])
27886 > IKFAST_EVALCOND_THRESH
27887 || IKabs(evalcond[4])
27888 > IKFAST_EVALCOND_THRESH)
27889 {
27890 continue;
27891 }
27892 }
27893
27894 rotationfunction0(
27895 solutions);
27896 }
27897 }
27898 }
27899 }
27900 }
27901 else
27902 {
27903 {
27904 IkReal j6array[1], cj6array[1],
27905 sj6array[1];
27906 bool j6valid[1] = {false};
27907 _nj6 = 1;
27908 IkReal x1951
27909 = ((0.045) * cj4 * px);
27910 IkReal x1952
27911 = ((0.045) * py * sj4);
27912 IkReal x1953 = ((0.3) * sj9);
27913 IkReal x1954 = (cj4 * px);
27914 IkReal x1955 = (py * sj4);
27915 IkReal x1956 = (cj9 * sj9);
27916 IkReal x1957 = cj9 * cj9;
27917 IkReal x1958 = ((1.0) * pz);
27918 IkReal x1959 = py * py;
27919 IkReal x1960 = cj4 * cj4;
27920 CheckValue<IkReal> x1961
27921 = IKatan2WithCheck(
27922 IkReal((
27923 (0.03825)
27924 + (((-1.0) * x1955
27925 * x1958))
27926 + (((0.087975)
27927 * x1956))
27928 + (((-0.027) * x1957))
27929 + (((0.167025) * sj9))
27930 + (((-1.0) * (0.01125)
27931 * cj9))
27932 + (((-1.0) * x1954
27933 * x1958)))),
27934 ((-0.304525)
27935 + (((-1.0) * x1959
27936 * x1960))
27937 + ((x1960 * (px * px)))
27938 + (((-1.0) * (0.0495)
27939 * sj9))
27940 + (((-0.087975) * x1957))
27941 + (((-1.0) * (0.33)
27942 * cj9))
27943 + (((2.0) * cj4 * px
27944 * x1955))
27945 + (((-0.027) * x1956))
27946 + x1959),
27947 IKFAST_ATAN2_MAGTHRESH);
27948 if (!x1961.valid)
27949 {
27950 continue;
27951 }
27952 CheckValue<IkReal> x1962
27953 = IKPowWithIntegerCheck(
27954 IKsign(
27955 ((((-1.0) * (0.55)
27956 * pz))
27957 + ((x1953 * x1954))
27958 + (((-1.0) * cj9
27959 * x1952))
27960 + ((x1953 * x1955))
27961 + (((-1.0) * cj9
27962 * x1951))
27963 + x1951 + x1952
27964 + (((-1.0) * (0.045)
27965 * pz * sj9))
27966 + (((-1.0) * (0.3)
27967 * cj9 * pz)))),
27968 -1);
27969 if (!x1962.valid)
27970 {
27971 continue;
27972 }
27973 j6array[0]
27974 = ((-1.5707963267949)
27975 + (x1961.value)
27976 + (((1.5707963267949)
27977 * (x1962.value))));
27978 sj6array[0] = IKsin(j6array[0]);
27979 cj6array[0] = IKcos(j6array[0]);
27980 if (j6array[0] > IKPI)
27981 {
27982 j6array[0] -= IK2PI;
27983 }
27984 else if (j6array[0] < -IKPI)
27985 {
27986 j6array[0] += IK2PI;
27987 }
27988 j6valid[0] = true;
27989 for (int ij6 = 0; ij6 < 1; ++ij6)
27990 {
27991 if (!j6valid[ij6])
27992 {
27993 continue;
27994 }
27995 _ij6[0] = ij6;
27996 _ij6[1] = -1;
27997 for (int iij6 = ij6 + 1;
27998 iij6 < 1;
27999 ++iij6)
28000 {
28001 if (j6valid[iij6]
28002 && IKabs(
28003 cj6array[ij6]
28004 - cj6array[iij6])
28005 < IKFAST_SOLUTION_THRESH
28006 && IKabs(
28007 sj6array[ij6]
28008 - sj6array[iij6])
28009 < IKFAST_SOLUTION_THRESH)
28010 {
28011 j6valid[iij6] = false;
28012 _ij6[1] = iij6;
28013 break;
28014 }
28015 }
28016 j6 = j6array[ij6];
28017 cj6 = cj6array[ij6];
28018 sj6 = sj6array[ij6];
28019 {
28020 IkReal evalcond[5];
28021 IkReal x1963 = ((0.3) * cj9);
28022 IkReal x1964
28023 = ((0.045) * sj9);
28024 IkReal x1965 = IKcos(j6);
28025 IkReal x1966 = (pz * x1965);
28026 IkReal x1967 = IKsin(j6);
28027 IkReal x1968 = (cj4 * px);
28028 IkReal x1969
28029 = (x1967 * x1968);
28030 IkReal x1970 = (py * sj4);
28031 IkReal x1971
28032 = (x1967 * x1970);
28033 IkReal x1972
28034 = ((0.045) * cj9);
28035 IkReal x1973 = ((0.3) * sj9);
28036 IkReal x1974 = (pz * x1967);
28037 IkReal x1975
28038 = ((1.0) * x1968);
28039 IkReal x1976
28040 = ((1.0) * x1970);
28041 IkReal x1977
28042 = ((0.09) * x1965);
28043 evalcond[0]
28044 = ((-0.55) + x1966 + x1969
28045 + (((-1.0) * x1964))
28046 + x1971
28047 + (((-1.0) * x1963)));
28048 evalcond[1]
28049 = ((0.045)
28050 + (((-1.0) * x1965
28051 * x1976))
28052 + (((-1.0) * x1965
28053 * x1975))
28054 + (((-1.0) * x1972))
28055 + x1973 + x1974);
28056 evalcond[2]
28057 = ((((-0.316735294117647)
28058 * x1965))
28059 + (((-0.588235294117647)
28060 * pp * x1965))
28061 + pz
28062 + (((-1.51009803921569)
28063 * x1967))
28064 + (((-0.108264705882353)
28065 * cj9 * x1965))
28066 + (((3.92156862745098)
28067 * pp * x1967))
28068 + (((-1.32323529411765)
28069 * cj9 * x1967)));
28070 evalcond[3]
28071 = ((((0.045) * x1965))
28072 + ((x1964 * x1967))
28073 + ((x1963 * x1967))
28074 + (((-1.0) * x1976))
28075 + (((-1.0) * x1965
28076 * x1972))
28077 + (((-1.0) * x1975))
28078 + (((0.55) * x1967))
28079 + ((x1965 * x1973)));
28080 evalcond[4]
28081 = ((-0.2125)
28082 + (((1.1) * x1966))
28083 + (((1.1) * x1969))
28084 + (((1.1) * x1971))
28085 + (((-0.09) * x1974))
28086 + ((x1970 * x1977))
28087 + ((x1968 * x1977))
28088 + (((-1.0) * (1.0)
28089 * pp)));
28090 if (IKabs(evalcond[0])
28091 > IKFAST_EVALCOND_THRESH
28092 || IKabs(evalcond[1])
28093 > IKFAST_EVALCOND_THRESH
28094 || IKabs(evalcond[2])
28095 > IKFAST_EVALCOND_THRESH
28096 || IKabs(evalcond[3])
28097 > IKFAST_EVALCOND_THRESH
28098 || IKabs(evalcond[4])
28099 > IKFAST_EVALCOND_THRESH)
28100 {
28101 continue;
28102 }
28103 }
28104
28105 rotationfunction0(solutions);
28106 }
28107 }
28108 }
28109 }
28110 }
28111 else
28112 {
28113 {
28114 IkReal j6array[1], cj6array[1],
28115 sj6array[1];
28116 bool j6valid[1] = {false};
28117 _nj6 = 1;
28118 IkReal x1978 = (cj4 * px);
28119 IkReal x1979 = (py * sj4);
28120 IkReal x1980
28121 = ((1.32323529411765) * cj9);
28122 IkReal x1981
28123 = ((3.92156862745098) * pp);
28124 IkReal x1982
28125 = ((0.0264705882352941) * pp);
28126 IkReal x1983 = (cj9 * sj9);
28127 IkReal x1984
28128 = ((0.176470588235294) * pp);
28129 IkReal x1985 = cj9 * cj9;
28130 CheckValue<IkReal> x1986
28131 = IKatan2WithCheck(
28132 IkReal((
28133 (-0.0142530882352941)
28134 + ((pz * x1979))
28135 + (((0.00938117647058823)
28136 * cj9))
28137 + ((cj9 * x1982))
28138 + (((-0.0324794117647059)
28139 * x1983))
28140 + (((0.00487191176470588)
28141 * x1985))
28142 + (((-1.0) * x1982))
28143 + ((pz * x1978))
28144 + (((-1.0) * sj9 * x1984))
28145 + (((-1.0)
28146 * (0.0950205882352941)
28147 * sj9)))),
28148 ((0.0679544117647059)
28149 + (((0.396970588235294)
28150 * x1983))
28151 + (((-0.0595455882352941)
28152 * x1985))
28153 + (pz * pz) + ((cj9 * x1984))
28154 + (((-1.0)
28155 * (0.00840882352941177)
28156 * cj9))
28157 + (((0.453029411764706)
28158 * sj9))
28159 + (((-1.0) * x1984))
28160 + (((-1.0)
28161 * (1.17647058823529) * pp
28162 * sj9))),
28163 IKFAST_ATAN2_MAGTHRESH);
28164 if (!x1986.valid)
28165 {
28166 continue;
28167 }
28168 CheckValue<IkReal> x1987
28169 = IKPowWithIntegerCheck(
28170 IKsign((
28171 (((-1.0) * x1978 * x1981))
28172 + (((1.51009803921569)
28173 * x1978))
28174 + (((1.51009803921569)
28175 * x1979))
28176 + (((0.316735294117647)
28177 * pz))
28178 + (((0.108264705882353)
28179 * cj9 * pz))
28180 + (((0.588235294117647)
28181 * pp * pz))
28182 + ((x1978 * x1980))
28183 + (((-1.0) * x1979
28184 * x1981))
28185 + ((x1979 * x1980)))),
28186 -1);
28187 if (!x1987.valid)
28188 {
28189 continue;
28190 }
28191 j6array[0]
28192 = ((-1.5707963267949)
28193 + (x1986.value)
28194 + (((1.5707963267949)
28195 * (x1987.value))));
28196 sj6array[0] = IKsin(j6array[0]);
28197 cj6array[0] = IKcos(j6array[0]);
28198 if (j6array[0] > IKPI)
28199 {
28200 j6array[0] -= IK2PI;
28201 }
28202 else if (j6array[0] < -IKPI)
28203 {
28204 j6array[0] += IK2PI;
28205 }
28206 j6valid[0] = true;
28207 for (int ij6 = 0; ij6 < 1; ++ij6)
28208 {
28209 if (!j6valid[ij6])
28210 {
28211 continue;
28212 }
28213 _ij6[0] = ij6;
28214 _ij6[1] = -1;
28215 for (int iij6 = ij6 + 1; iij6 < 1;
28216 ++iij6)
28217 {
28218 if (j6valid[iij6]
28219 && IKabs(
28220 cj6array[ij6]
28221 - cj6array[iij6])
28222 < IKFAST_SOLUTION_THRESH
28223 && IKabs(
28224 sj6array[ij6]
28225 - sj6array[iij6])
28226 < IKFAST_SOLUTION_THRESH)
28227 {
28228 j6valid[iij6] = false;
28229 _ij6[1] = iij6;
28230 break;
28231 }
28232 }
28233 j6 = j6array[ij6];
28234 cj6 = cj6array[ij6];
28235 sj6 = sj6array[ij6];
28236 {
28237 IkReal evalcond[5];
28238 IkReal x1988 = ((0.3) * cj9);
28239 IkReal x1989 = ((0.045) * sj9);
28240 IkReal x1990 = IKcos(j6);
28241 IkReal x1991 = (pz * x1990);
28242 IkReal x1992 = IKsin(j6);
28243 IkReal x1993 = (cj4 * px);
28244 IkReal x1994 = (x1992 * x1993);
28245 IkReal x1995 = (py * sj4);
28246 IkReal x1996 = (x1992 * x1995);
28247 IkReal x1997 = ((0.045) * cj9);
28248 IkReal x1998 = ((0.3) * sj9);
28249 IkReal x1999 = (pz * x1992);
28250 IkReal x2000 = ((1.0) * x1993);
28251 IkReal x2001 = ((1.0) * x1995);
28252 IkReal x2002 = ((0.09) * x1990);
28253 evalcond[0]
28254 = ((-0.55)
28255 + (((-1.0) * x1989))
28256 + x1996 + x1994 + x1991
28257 + (((-1.0) * x1988)));
28258 evalcond[1]
28259 = ((0.045)
28260 + (((-1.0) * x1990
28261 * x2000))
28262 + (((-1.0) * x1997))
28263 + x1998 + x1999
28264 + (((-1.0) * x1990
28265 * x2001)));
28266 evalcond[2]
28267 = ((((-0.108264705882353)
28268 * cj9 * x1990))
28269 + (((-0.588235294117647)
28270 * pp * x1990))
28271 + pz
28272 + (((-1.51009803921569)
28273 * x1992))
28274 + (((-1.32323529411765)
28275 * cj9 * x1992))
28276 + (((3.92156862745098) * pp
28277 * x1992))
28278 + (((-0.316735294117647)
28279 * x1990)));
28280 evalcond[3]
28281 = ((((0.045) * x1990))
28282 + (((-1.0) * x2001))
28283 + (((0.55) * x1992))
28284 + (((-1.0) * x2000))
28285 + (((-1.0) * x1990
28286 * x1997))
28287 + ((x1990 * x1998))
28288 + ((x1989 * x1992))
28289 + ((x1988 * x1992)));
28290 evalcond[4]
28291 = ((-0.2125)
28292 + (((1.1) * x1996))
28293 + (((1.1) * x1991))
28294 + ((x1995 * x2002))
28295 + (((-0.09) * x1999))
28296 + ((x1993 * x2002))
28297 + (((-1.0) * (1.0) * pp))
28298 + (((1.1) * x1994)));
28299 if (IKabs(evalcond[0])
28300 > IKFAST_EVALCOND_THRESH
28301 || IKabs(evalcond[1])
28302 > IKFAST_EVALCOND_THRESH
28303 || IKabs(evalcond[2])
28304 > IKFAST_EVALCOND_THRESH
28305 || IKabs(evalcond[3])
28306 > IKFAST_EVALCOND_THRESH
28307 || IKabs(evalcond[4])
28308 > IKFAST_EVALCOND_THRESH)
28309 {
28310 continue;
28311 }
28312 }
28313
28314 rotationfunction0(solutions);
28315 }
28316 }
28317 }
28318 }
28319 }
28320 } while (0);
28321 if (bgotonextstatement)
28322 {
28323 bool bgotonextstatement = true;
28324 do
28325 {
28326 IkReal x2003 = (px * sj4);
28327 IkReal x2004 = (cj4 * py);
28328 evalcond[0]
28329 = ((-3.14159265358979)
28330 + (IKfmod(
28331 ((3.14159265358979)
28332 + (IKabs(
28333 ((-3.14159265358979)
28334 + j8)))),
28335 6.28318530717959)));
28336 evalcond[1]
28337 = ((0.39655) + (((0.0765) * sj9))
28338 + (((0.32595) * cj9))
28339 + (((-1.0) * (1.0) * pp)));
28340 evalcond[2] = (x2003 + (((-1.0) * x2004)));
28341 evalcond[3] = ((((-1.0) * x2003)) + x2004);
28342 if (IKabs(evalcond[0]) < 0.0000010000000000
28343 && IKabs(evalcond[1])
28344 < 0.0000010000000000
28345 && IKabs(evalcond[2])
28346 < 0.0000010000000000
28347 && IKabs(evalcond[3])
28348 < 0.0000010000000000)
28349 {
28350 bgotonextstatement = false;
28351 {
28352 IkReal j6eval[2];
28353 sj8 = 0;
28354 cj8 = -1.0;
28355 j8 = 3.14159265358979;
28356 IkReal x2005 = py * py;
28357 IkReal x2006 = cj4 * cj4;
28358 IkReal x2007
28359 = (x2005
28360 + (((-1.0) * x2005 * x2006))
28361 + (pz * pz)
28362 + (((2.0) * cj4 * px * py * sj4))
28363 + ((x2006 * (px * px))));
28364 j6eval[0] = x2007;
28365 j6eval[1] = IKsign(x2007);
28366 if (IKabs(j6eval[0])
28367 < 0.0000010000000000
28368 || IKabs(j6eval[1])
28369 < 0.0000010000000000)
28370 {
28371 {
28372 IkReal j6eval[2];
28373 sj8 = 0;
28374 cj8 = -1.0;
28375 j8 = 3.14159265358979;
28376 IkReal x2008 = (cj4 * px);
28377 IkReal x2009 = (cj9 * pz);
28378 IkReal x2010 = (py * sj4);
28379 IkReal x2011 = (pz * sj9);
28380 IkReal x2012 = (cj4 * px * sj9);
28381 IkReal x2013 = (py * sj4 * sj9);
28382 IkReal x2014 = ((0.045) * x2008);
28383 IkReal x2015 = ((0.045) * x2010);
28384 j6eval[0]
28385 = (((cj9 * x2010))
28386 + (((-1.0) * x2010))
28387 + ((cj9 * x2008))
28388 + (((-1.0)
28389 * (12.2222222222222)
28390 * pz))
28391 + (((-1.0) * x2011))
28392 + (((-6.66666666666667)
28393 * x2009))
28394 + (((-6.66666666666667)
28395 * x2013))
28396 + (((-1.0) * x2008))
28397 + (((-6.66666666666667)
28398 * x2012)));
28399 j6eval[1] = IKsign(
28400 (((cj9 * x2014))
28401 + (((-0.3) * x2012))
28402 + (((-0.3) * x2013))
28403 + (((-1.0) * (0.55) * pz))
28404 + (((-0.045) * x2011))
28405 + ((cj9 * x2015))
28406 + (((-1.0) * x2014))
28407 + (((-1.0) * x2015))
28408 + (((-0.3) * x2009))));
28409 if (IKabs(j6eval[0])
28410 < 0.0000010000000000
28411 || IKabs(j6eval[1])
28412 < 0.0000010000000000)
28413 {
28414 {
28415 IkReal j6eval[2];
28416 sj8 = 0;
28417 cj8 = -1.0;
28418 j8 = 3.14159265358979;
28419 IkReal x2016 = (cj4 * px);
28420 IkReal x2017 = (cj9 * pz);
28421 IkReal x2018 = (pp * pz);
28422 IkReal x2019 = (py * sj4);
28423 IkReal x2020 = (cj4 * cj9 * px);
28424 IkReal x2021 = (cj4 * pp * px);
28425 IkReal x2022 = (cj9 * py * sj4);
28426 IkReal x2023 = (pp * py * sj4);
28427 j6eval[0]
28428 = ((((-2.92556370551481)
28429 * x2016))
28430 + (((-1.0)
28431 * (13.9482024812098)
28432 * pz))
28433 + (((-5.4333061668025)
28434 * x2021))
28435 + (((-1.0) * x2022))
28436 + (((-12.2222222222222)
28437 * x2017))
28438 + (((-5.4333061668025)
28439 * x2023))
28440 + (((-2.92556370551481)
28441 * x2019))
28442 + (((36.2220411120167)
28443 * x2018))
28444 + (((-1.0) * x2020)));
28445 j6eval[1] = IKsign(
28446 ((((-0.588235294117647)
28447 * x2021))
28448 + (((-1.32323529411765)
28449 * x2017))
28450 + (((3.92156862745098)
28451 * x2018))
28452 + (((-0.316735294117647)
28453 * x2019))
28454 + (((-1.0)
28455 * (1.51009803921569)
28456 * pz))
28457 + (((-0.108264705882353)
28458 * x2020))
28459 + (((-0.108264705882353)
28460 * x2022))
28461 + (((-0.588235294117647)
28462 * x2023))
28463 + (((-0.316735294117647)
28464 * x2016))));
28465 if (IKabs(j6eval[0])
28466 < 0.0000010000000000
28467 || IKabs(j6eval[1])
28468 < 0.0000010000000000)
28469 {
28470 {
28471 IkReal evalcond[1];
28472 bool bgotonextstatement
28473 = true;
28474 do
28475 {
28476 evalcond[0]
28477 = ((IKabs((
28478 (-3.14159265358979)
28479 + (IKfmod(
28480 ((3.14159265358979)
28481 + j9),
28482 6.28318530717959)))))
28483 + (IKabs(pz)));
28484 if (IKabs(evalcond[0])
28485 < 0.0000010000000000)
28486 {
28487 bgotonextstatement
28488 = false;
28489 {
28490 IkReal j6eval[1];
28491 IkReal x2024
28492 = ((1.0) * py);
28493 sj8 = 0;
28494 cj8 = -1.0;
28495 j8 = 3.14159265358979;
28496 pz = 0;
28497 j9 = 0;
28498 sj9 = 0;
28499 cj9 = 1.0;
28500 pp
28501 = ((py * py)
28502 + (px * px));
28503 npx
28504 = (((py * r10))
28505 + ((px
28506 * r00)));
28507 npy
28508 = (((py * r11))
28509 + ((px
28510 * r01)));
28511 npz
28512 = (((px * r02))
28513 + ((py
28514 * r12)));
28515 rxp0_0
28516 = ((-1.0) * r20
28517 * x2024);
28518 rxp0_1 = (px * r20);
28519 rxp1_0
28520 = ((-1.0) * r21
28521 * x2024);
28522 rxp1_1 = (px * r21);
28523 rxp2_0
28524 = ((-1.0) * r22
28525 * x2024);
28526 rxp2_1 = (px * r22);
28527 j6eval[0]
28528 = ((((-1.0)
28529 * (1.0) * cj4
28530 * px))
28531 + (((-1.0)
28532 * (1.0)
28533 * py
28534 * sj4)));
28535 if (IKabs(j6eval[0])
28536 < 0.0000010000000000)
28537 {
28538 {
28539 IkReal j6eval[1];
28540 IkReal x2025
28541 = ((1.0)
28542 * py);
28543 sj8 = 0;
28544 cj8 = -1.0;
28545 j8 = 3.14159265358979;
28546 pz = 0;
28547 j9 = 0;
28548 sj9 = 0;
28549 cj9 = 1.0;
28550 pp
28551 = ((py * py)
28552 + (px
28553 * px));
28554 npx
28555 = (((py
28556 * r10))
28557 + ((px
28558 * r00)));
28559 npy
28560 = (((py
28561 * r11))
28562 + ((px
28563 * r01)));
28564 npz
28565 = (((px
28566 * r02))
28567 + ((py
28568 * r12)));
28569 rxp0_0
28570 = ((-1.0)
28571 * r20
28572 * x2025);
28573 rxp0_1
28574 = (px * r20);
28575 rxp1_0
28576 = ((-1.0)
28577 * r21
28578 * x2025);
28579 rxp1_1
28580 = (px * r21);
28581 rxp2_0
28582 = ((-1.0)
28583 * r22
28584 * x2025);
28585 rxp2_1
28586 = (px * r22);
28587 j6eval[0]
28588 = ((-1.0)
28589 + (((-1.0)
28590 * (1.3840830449827)
28591 * (px
28592 * px)))
28593 + (((-1.0)
28594 * (1.3840830449827)
28595 * (py
28596 * py))));
28597 if (IKabs(
28598 j6eval[0])
28599 < 0.0000010000000000)
28600 {
28601 {
28602 IkReal
28603 j6eval[2];
28604 IkReal x2026
28605 = ((1.0)
28606 * py);
28607 sj8 = 0;
28608 cj8 = -1.0;
28609 j8 = 3.14159265358979;
28610 pz = 0;
28611 j9 = 0;
28612 sj9 = 0;
28613 cj9 = 1.0;
28614 pp
28615 = ((py
28616 * py)
28617 + (px
28618 * px));
28619 npx
28620 = (((py
28621 * r10))
28622 + ((px
28623 * r00)));
28624 npy
28625 = (((py
28626 * r11))
28627 + ((px
28628 * r01)));
28629 npz
28630 = (((px
28631 * r02))
28632 + ((py
28633 * r12)));
28634 rxp0_0
28635 = ((-1.0)
28636 * r20
28637 * x2026);
28638 rxp0_1
28639 = (px
28640 * r20);
28641 rxp1_0
28642 = ((-1.0)
28643 * r21
28644 * x2026);
28645 rxp1_1
28646 = (px
28647 * r21);
28648 rxp2_0
28649 = ((-1.0)
28650 * r22
28651 * x2026);
28652 rxp2_1
28653 = (px
28654 * r22);
28655 IkReal x2027
28656 = (cj4
28657 * px);
28658 IkReal x2028
28659 = (py
28660 * sj4);
28661 j6eval[0]
28662 = (x2027
28663 + x2028);
28664 j6eval[1]
28665 = ((((-1.0)
28666 * (1.3840830449827)
28667 * cj4
28668 * (px
28669 * px
28670 * px)))
28671 + (((-1.0)
28672 * x2028))
28673 + (((-1.3840830449827)
28674 * x2027
28675 * (py
28676 * py)))
28677 + (((-1.0)
28678 * (1.3840830449827)
28679 * sj4
28680 * (py
28681 * py
28682 * py)))
28683 + (((-1.0)
28684 * x2027))
28685 + (((-1.3840830449827)
28686 * x2028
28687 * (px
28688 * px))));
28689 if (IKabs(
28690 j6eval
28691 [0])
28692 < 0.0000010000000000
28693 || IKabs(
28694 j6eval
28695 [1])
28696 < 0.0000010000000000)
28697 {
28698 {
28699 IkReal evalcond
28700 [4];
28701 bool
28702 bgotonextstatement
28703 = true;
28704 do
28705 {
28706 evalcond
28707 [0]
28708 = ((IKabs(
28709 py))
28710 + (IKabs(
28711 px)));
28712 evalcond
28713 [1]
28714 = -0.85;
28715 evalcond
28716 [2]
28717 = 0;
28718 evalcond
28719 [3]
28720 = -0.2125;
28721 if (IKabs(
28722 evalcond
28723 [0])
28724 < 0.0000010000000000
28725 && IKabs(
28726 evalcond
28727 [1])
28728 < 0.0000010000000000
28729 && IKabs(
28730 evalcond
28731 [2])
28732 < 0.0000010000000000
28733 && IKabs(
28734 evalcond
28735 [3])
28736 < 0.0000010000000000)
28737 {
28738 bgotonextstatement
28739 = false;
28740 {
28741 IkReal j6array
28742 [2],
28743 cj6array
28744 [2],
28745 sj6array
28746 [2];
28747 bool j6valid
28748 [2]
28749 = {false};
28750 _nj6
28751 = 2;
28752 j6array
28753 [0]
28754 = 0.148889947609497;
28755 sj6array
28756 [0]
28757 = IKsin(
28758 j6array
28759 [0]);
28760 cj6array
28761 [0]
28762 = IKcos(
28763 j6array
28764 [0]);
28765 j6array
28766 [1]
28767 = 3.29048260119929;
28768 sj6array
28769 [1]
28770 = IKsin(
28771 j6array
28772 [1]);
28773 cj6array
28774 [1]
28775 = IKcos(
28776 j6array
28777 [1]);
28778 if (j6array
28779 [0]
28780 > IKPI)
28781 {
28782 j6array
28783 [0]
28784 -= IK2PI;
28785 }
28786 else if (
28787 j6array
28788 [0]
28789 < -IKPI)
28790 {
28791 j6array
28792 [0]
28793 += IK2PI;
28794 }
28795 j6valid
28796 [0]
28797 = true;
28798 if (j6array
28799 [1]
28800 > IKPI)
28801 {
28802 j6array
28803 [1]
28804 -= IK2PI;
28805 }
28806 else if (
28807 j6array
28808 [1]
28809 < -IKPI)
28810 {
28811 j6array
28812 [1]
28813 += IK2PI;
28814 }
28815 j6valid
28816 [1]
28817 = true;
28818 for (
28819 int ij6
28820 = 0;
28821 ij6
28822 < 2;
28823 ++ij6)
28824 {
28825 if (!j6valid
28826 [ij6])
28827 {
28828 continue;
28829 }
28830 _ij6[0]
28831 = ij6;
28832 _ij6[1]
28833 = -1;
28834 for (
28835 int iij6
28836 = ij6
28837 + 1;
28838 iij6
28839 < 2;
28840 ++iij6)
28841 {
28842 if (j6valid
28843 [iij6]
28844 && IKabs(
28845 cj6array
28846 [ij6]
28847 - cj6array
28848 [iij6])
28849 < IKFAST_SOLUTION_THRESH
28850 && IKabs(
28851 sj6array
28852 [ij6]
28853 - sj6array
28854 [iij6])
28855 < IKFAST_SOLUTION_THRESH)
28856 {
28857 j6valid
28858 [iij6]
28859 = false;
28860 _ij6[1]
28861 = iij6;
28862 break;
28863 }
28864 }
28865 j6 = j6array
28866 [ij6];
28867 cj6 = cj6array
28868 [ij6];
28869 sj6 = sj6array
28870 [ij6];
28871 {
28872 IkReal evalcond
28873 [1];
28874 evalcond
28875 [0]
28876 = ((0.85)
28877 * (IKsin(
28878 j6)));
28879 if (IKabs(
28880 evalcond
28881 [0])
28882 > IKFAST_EVALCOND_THRESH)
28883 {
28884 continue;
28885 }
28886 }
28887
28888 rotationfunction0(
28889 solutions);
28890 }
28891 }
28892 }
28893 } while (
28894 0);
28895 if (bgotonextstatement)
28896 {
28897 bool
28898 bgotonextstatement
28899 = true;
28900 do
28901 {
28902 evalcond
28903 [0]
28904 = ((IKabs((
28905 (-3.14159265358979)
28906 + (IKfmod(
28907 ((3.14159265358979)
28908 + j4),
28909 6.28318530717959)))))
28910 + (IKabs(
28911 px)));
28912 evalcond
28913 [1]
28914 = -0.85;
28915 evalcond
28916 [2]
28917 = 0;
28918 evalcond
28919 [3]
28920 = ((-0.2125)
28921 + (((-1.0)
28922 * (1.0)
28923 * (py
28924 * py))));
28925 if (IKabs(
28926 evalcond
28927 [0])
28928 < 0.0000010000000000
28929 && IKabs(
28930 evalcond
28931 [1])
28932 < 0.0000010000000000
28933 && IKabs(
28934 evalcond
28935 [2])
28936 < 0.0000010000000000
28937 && IKabs(
28938 evalcond
28939 [3])
28940 < 0.0000010000000000)
28941 {
28942 bgotonextstatement
28943 = false;
28944 {
28945 IkReal j6eval
28946 [1];
28947 IkReal
28948 x2029
28949 = ((1.0)
28950 * py);
28951 sj8 = 0;
28952 cj8 = -1.0;
28953 j8 = 3.14159265358979;
28954 pz = 0;
28955 j9 = 0;
28956 sj9 = 0;
28957 cj9 = 1.0;
28958 pp = py
28959 * py;
28960 npx
28961 = (py
28962 * r10);
28963 npy
28964 = (py
28965 * r11);
28966 npz
28967 = (py
28968 * r12);
28969 rxp0_0
28970 = ((-1.0)
28971 * r20
28972 * x2029);
28973 rxp0_1
28974 = 0;
28975 rxp1_0
28976 = ((-1.0)
28977 * r21
28978 * x2029);
28979 rxp1_1
28980 = 0;
28981 rxp2_0
28982 = ((-1.0)
28983 * r22
28984 * x2029);
28985 rxp2_1
28986 = 0;
28987 px = 0;
28988 j4 = 0;
28989 sj4 = 0;
28990 cj4 = 1.0;
28991 rxp0_2
28992 = (py
28993 * r00);
28994 rxp1_2
28995 = (py
28996 * r01);
28997 rxp2_2
28998 = (py
28999 * r02);
29000 j6eval
29001 [0]
29002 = ((1.0)
29003 + (((1.91568587540858)
29004 * (py
29005 * py
29006 * py
29007 * py)))
29008 + (((-1.0)
29009 * (2.64633970947792)
29010 * (py
29011 * py))));
29012 if (IKabs(
29013 j6eval
29014 [0])
29015 < 0.0000010000000000)
29016 {
29017 continue; // no branches [j6]
29018 }
29019 else
29020 {
29021 {
29022 IkReal j6array
29023 [2],
29024 cj6array
29025 [2],
29026 sj6array
29027 [2];
29028 bool j6valid
29029 [2]
29030 = {false};
29031 _nj6
29032 = 2;
29033 IkReal
29034 x2030
29035 = py
29036 * py;
29037 CheckValue<IkReal> x2032 = IKatan2WithCheck(
29038 IkReal((
29039 (-0.425)
29040 + (((-0.588235294117647)
29041 * x2030)))),
29042 ((2.83333333333333)
29043 + (((-3.92156862745098)
29044 * x2030))),
29045 IKFAST_ATAN2_MAGTHRESH);
29046 if (!x2032
29047 .valid)
29048 {
29049 continue;
29050 }
29051 IkReal
29052 x2031
29053 = ((-1.0)
29054 * (x2032
29055 .value));
29056 j6array
29057 [0]
29058 = x2031;
29059 sj6array
29060 [0]
29061 = IKsin(
29062 j6array
29063 [0]);
29064 cj6array
29065 [0]
29066 = IKcos(
29067 j6array
29068 [0]);
29069 j6array
29070 [1]
29071 = ((3.14159265358979)
29072 + x2031);
29073 sj6array
29074 [1]
29075 = IKsin(
29076 j6array
29077 [1]);
29078 cj6array
29079 [1]
29080 = IKcos(
29081 j6array
29082 [1]);
29083 if (j6array
29084 [0]
29085 > IKPI)
29086 {
29087 j6array
29088 [0]
29089 -= IK2PI;
29090 }
29091 else if (
29092 j6array
29093 [0]
29094 < -IKPI)
29095 {
29096 j6array
29097 [0]
29098 += IK2PI;
29099 }
29100 j6valid
29101 [0]
29102 = true;
29103 if (j6array
29104 [1]
29105 > IKPI)
29106 {
29107 j6array
29108 [1]
29109 -= IK2PI;
29110 }
29111 else if (
29112 j6array
29113 [1]
29114 < -IKPI)
29115 {
29116 j6array
29117 [1]
29118 += IK2PI;
29119 }
29120 j6valid
29121 [1]
29122 = true;
29123 for (
29124 int ij6
29125 = 0;
29126 ij6
29127 < 2;
29128 ++ij6)
29129 {
29130 if (!j6valid
29131 [ij6])
29132 {
29133 continue;
29134 }
29135 _ij6[0]
29136 = ij6;
29137 _ij6[1]
29138 = -1;
29139 for (
29140 int iij6
29141 = ij6
29142 + 1;
29143 iij6
29144 < 2;
29145 ++iij6)
29146 {
29147 if (j6valid
29148 [iij6]
29149 && IKabs(
29150 cj6array
29151 [ij6]
29152 - cj6array
29153 [iij6])
29154 < IKFAST_SOLUTION_THRESH
29155 && IKabs(
29156 sj6array
29157 [ij6]
29158 - sj6array
29159 [iij6])
29160 < IKFAST_SOLUTION_THRESH)
29161 {
29162 j6valid
29163 [iij6]
29164 = false;
29165 _ij6[1]
29166 = iij6;
29167 break;
29168 }
29169 }
29170 j6 = j6array
29171 [ij6];
29172 cj6 = cj6array
29173 [ij6];
29174 sj6 = sj6array
29175 [ij6];
29176 {
29177 IkReal evalcond
29178 [1];
29179 evalcond
29180 [0]
29181 = ((0.85)
29182 * (IKsin(
29183 j6)));
29184 if (IKabs(
29185 evalcond
29186 [0])
29187 > IKFAST_EVALCOND_THRESH)
29188 {
29189 continue;
29190 }
29191 }
29192
29193 rotationfunction0(
29194 solutions);
29195 }
29196 }
29197 }
29198 }
29199 }
29200 } while (
29201 0);
29202 if (bgotonextstatement)
29203 {
29204 bool
29205 bgotonextstatement
29206 = true;
29207 do
29208 {
29209 evalcond
29210 [0]
29211 = ((IKabs((
29212 (-3.14159265358979)
29213 + (IKfmod(
29214 j4,
29215 6.28318530717959)))))
29216 + (IKabs(
29217 px)));
29218 evalcond
29219 [1]
29220 = -0.85;
29221 evalcond
29222 [2]
29223 = 0;
29224 evalcond
29225 [3]
29226 = ((-0.2125)
29227 + (((-1.0)
29228 * (1.0)
29229 * (py
29230 * py))));
29231 if (IKabs(
29232 evalcond
29233 [0])
29234 < 0.0000010000000000
29235 && IKabs(
29236 evalcond
29237 [1])
29238 < 0.0000010000000000
29239 && IKabs(
29240 evalcond
29241 [2])
29242 < 0.0000010000000000
29243 && IKabs(
29244 evalcond
29245 [3])
29246 < 0.0000010000000000)
29247 {
29248 bgotonextstatement
29249 = false;
29250 {
29251 IkReal j6eval
29252 [1];
29253 IkReal
29254 x2033
29255 = ((1.0)
29256 * py);
29257 sj8 = 0;
29258 cj8 = -1.0;
29259 j8 = 3.14159265358979;
29260 pz = 0;
29261 j9 = 0;
29262 sj9 = 0;
29263 cj9 = 1.0;
29264 pp = py
29265 * py;
29266 npx
29267 = (py
29268 * r10);
29269 npy
29270 = (py
29271 * r11);
29272 npz
29273 = (py
29274 * r12);
29275 rxp0_0
29276 = ((-1.0)
29277 * r20
29278 * x2033);
29279 rxp0_1
29280 = 0;
29281 rxp1_0
29282 = ((-1.0)
29283 * r21
29284 * x2033);
29285 rxp1_1
29286 = 0;
29287 rxp2_0
29288 = ((-1.0)
29289 * r22
29290 * x2033);
29291 rxp2_1
29292 = 0;
29293 px = 0;
29294 j4 = 3.14159265358979;
29295 sj4 = 0;
29296 cj4 = -1.0;
29297 rxp0_2
29298 = (py
29299 * r00);
29300 rxp1_2
29301 = (py
29302 * r01);
29303 rxp2_2
29304 = (py
29305 * r02);
29306 j6eval
29307 [0]
29308 = ((1.0)
29309 + (((1.91568587540858)
29310 * (py
29311 * py
29312 * py
29313 * py)))
29314 + (((-1.0)
29315 * (2.64633970947792)
29316 * (py
29317 * py))));
29318 if (IKabs(
29319 j6eval
29320 [0])
29321 < 0.0000010000000000)
29322 {
29323 continue; // no branches [j6]
29324 }
29325 else
29326 {
29327 {
29328 IkReal j6array
29329 [2],
29330 cj6array
29331 [2],
29332 sj6array
29333 [2];
29334 bool j6valid
29335 [2]
29336 = {false};
29337 _nj6
29338 = 2;
29339 IkReal
29340 x2034
29341 = py
29342 * py;
29343 CheckValue<IkReal> x2036 = IKatan2WithCheck(
29344 IkReal((
29345 (-0.425)
29346 + (((-0.588235294117647)
29347 * x2034)))),
29348 ((2.83333333333333)
29349 + (((-3.92156862745098)
29350 * x2034))),
29351 IKFAST_ATAN2_MAGTHRESH);
29352 if (!x2036
29353 .valid)
29354 {
29355 continue;
29356 }
29357 IkReal
29358 x2035
29359 = ((-1.0)
29360 * (x2036
29361 .value));
29362 j6array
29363 [0]
29364 = x2035;
29365 sj6array
29366 [0]
29367 = IKsin(
29368 j6array
29369 [0]);
29370 cj6array
29371 [0]
29372 = IKcos(
29373 j6array
29374 [0]);
29375 j6array
29376 [1]
29377 = ((3.14159265358979)
29378 + x2035);
29379 sj6array
29380 [1]
29381 = IKsin(
29382 j6array
29383 [1]);
29384 cj6array
29385 [1]
29386 = IKcos(
29387 j6array
29388 [1]);
29389 if (j6array
29390 [0]
29391 > IKPI)
29392 {
29393 j6array
29394 [0]
29395 -= IK2PI;
29396 }
29397 else if (
29398 j6array
29399 [0]
29400 < -IKPI)
29401 {
29402 j6array
29403 [0]
29404 += IK2PI;
29405 }
29406 j6valid
29407 [0]
29408 = true;
29409 if (j6array
29410 [1]
29411 > IKPI)
29412 {
29413 j6array
29414 [1]
29415 -= IK2PI;
29416 }
29417 else if (
29418 j6array
29419 [1]
29420 < -IKPI)
29421 {
29422 j6array
29423 [1]
29424 += IK2PI;
29425 }
29426 j6valid
29427 [1]
29428 = true;
29429 for (
29430 int ij6
29431 = 0;
29432 ij6
29433 < 2;
29434 ++ij6)
29435 {
29436 if (!j6valid
29437 [ij6])
29438 {
29439 continue;
29440 }
29441 _ij6[0]
29442 = ij6;
29443 _ij6[1]
29444 = -1;
29445 for (
29446 int iij6
29447 = ij6
29448 + 1;
29449 iij6
29450 < 2;
29451 ++iij6)
29452 {
29453 if (j6valid
29454 [iij6]
29455 && IKabs(
29456 cj6array
29457 [ij6]
29458 - cj6array
29459 [iij6])
29460 < IKFAST_SOLUTION_THRESH
29461 && IKabs(
29462 sj6array
29463 [ij6]
29464 - sj6array
29465 [iij6])
29466 < IKFAST_SOLUTION_THRESH)
29467 {
29468 j6valid
29469 [iij6]
29470 = false;
29471 _ij6[1]
29472 = iij6;
29473 break;
29474 }
29475 }
29476 j6 = j6array
29477 [ij6];
29478 cj6 = cj6array
29479 [ij6];
29480 sj6 = sj6array
29481 [ij6];
29482 {
29483 IkReal evalcond
29484 [1];
29485 evalcond
29486 [0]
29487 = ((0.85)
29488 * (IKsin(
29489 j6)));
29490 if (IKabs(
29491 evalcond
29492 [0])
29493 > IKFAST_EVALCOND_THRESH)
29494 {
29495 continue;
29496 }
29497 }
29498
29499 rotationfunction0(
29500 solutions);
29501 }
29502 }
29503 }
29504 }
29505 }
29506 } while (
29507 0);
29508 if (bgotonextstatement)
29509 {
29510 bool
29511 bgotonextstatement
29512 = true;
29513 do
29514 {
29515 evalcond
29516 [0]
29517 = ((IKabs((
29518 (-3.14159265358979)
29519 + (IKfmod(
29520 ((1.5707963267949)
29521 + j4),
29522 6.28318530717959)))))
29523 + (IKabs(
29524 py)));
29525 evalcond
29526 [1]
29527 = -0.85;
29528 evalcond
29529 [2]
29530 = 0;
29531 evalcond
29532 [3]
29533 = ((-0.2125)
29534 + (((-1.0)
29535 * (1.0)
29536 * (px
29537 * px))));
29538 if (IKabs(
29539 evalcond
29540 [0])
29541 < 0.0000010000000000
29542 && IKabs(
29543 evalcond
29544 [1])
29545 < 0.0000010000000000
29546 && IKabs(
29547 evalcond
29548 [2])
29549 < 0.0000010000000000
29550 && IKabs(
29551 evalcond
29552 [3])
29553 < 0.0000010000000000)
29554 {
29555 bgotonextstatement
29556 = false;
29557 {
29558 IkReal j6eval
29559 [1];
29560 IkReal
29561 x2037
29562 = ((1.0)
29563 * px);
29564 sj8 = 0;
29565 cj8 = -1.0;
29566 j8 = 3.14159265358979;
29567 pz = 0;
29568 j9 = 0;
29569 sj9 = 0;
29570 cj9 = 1.0;
29571 pp = px
29572 * px;
29573 npx
29574 = (px
29575 * r00);
29576 npy
29577 = (px
29578 * r01);
29579 npz
29580 = (px
29581 * r02);
29582 rxp0_0
29583 = 0;
29584 rxp0_1
29585 = (px
29586 * r20);
29587 rxp1_0
29588 = 0;
29589 rxp1_1
29590 = (px
29591 * r21);
29592 rxp2_0
29593 = 0;
29594 rxp2_1
29595 = (px
29596 * r22);
29597 py = 0;
29598 j4 = 1.5707963267949;
29599 sj4 = 1.0;
29600 cj4 = 0;
29601 rxp0_2
29602 = ((-1.0)
29603 * r10
29604 * x2037);
29605 rxp1_2
29606 = ((-1.0)
29607 * r11
29608 * x2037);
29609 rxp2_2
29610 = ((-1.0)
29611 * r12
29612 * x2037);
29613 j6eval
29614 [0]
29615 = ((1.0)
29616 + (((1.91568587540858)
29617 * (px
29618 * px
29619 * px
29620 * px)))
29621 + (((-1.0)
29622 * (2.64633970947792)
29623 * (px
29624 * px))));
29625 if (IKabs(
29626 j6eval
29627 [0])
29628 < 0.0000010000000000)
29629 {
29630 continue; // no branches [j6]
29631 }
29632 else
29633 {
29634 {
29635 IkReal j6array
29636 [2],
29637 cj6array
29638 [2],
29639 sj6array
29640 [2];
29641 bool j6valid
29642 [2]
29643 = {false};
29644 _nj6
29645 = 2;
29646 IkReal
29647 x2038
29648 = px
29649 * px;
29650 CheckValue<IkReal> x2040 = IKatan2WithCheck(
29651 IkReal((
29652 (-0.425)
29653 + (((-0.588235294117647)
29654 * x2038)))),
29655 ((2.83333333333333)
29656 + (((-3.92156862745098)
29657 * x2038))),
29658 IKFAST_ATAN2_MAGTHRESH);
29659 if (!x2040
29660 .valid)
29661 {
29662 continue;
29663 }
29664 IkReal
29665 x2039
29666 = ((-1.0)
29667 * (x2040
29668 .value));
29669 j6array
29670 [0]
29671 = x2039;
29672 sj6array
29673 [0]
29674 = IKsin(
29675 j6array
29676 [0]);
29677 cj6array
29678 [0]
29679 = IKcos(
29680 j6array
29681 [0]);
29682 j6array
29683 [1]
29684 = ((3.14159265358979)
29685 + x2039);
29686 sj6array
29687 [1]
29688 = IKsin(
29689 j6array
29690 [1]);
29691 cj6array
29692 [1]
29693 = IKcos(
29694 j6array
29695 [1]);
29696 if (j6array
29697 [0]
29698 > IKPI)
29699 {
29700 j6array
29701 [0]
29702 -= IK2PI;
29703 }
29704 else if (
29705 j6array
29706 [0]
29707 < -IKPI)
29708 {
29709 j6array
29710 [0]
29711 += IK2PI;
29712 }
29713 j6valid
29714 [0]
29715 = true;
29716 if (j6array
29717 [1]
29718 > IKPI)
29719 {
29720 j6array
29721 [1]
29722 -= IK2PI;
29723 }
29724 else if (
29725 j6array
29726 [1]
29727 < -IKPI)
29728 {
29729 j6array
29730 [1]
29731 += IK2PI;
29732 }
29733 j6valid
29734 [1]
29735 = true;
29736 for (
29737 int ij6
29738 = 0;
29739 ij6
29740 < 2;
29741 ++ij6)
29742 {
29743 if (!j6valid
29744 [ij6])
29745 {
29746 continue;
29747 }
29748 _ij6[0]
29749 = ij6;
29750 _ij6[1]
29751 = -1;
29752 for (
29753 int iij6
29754 = ij6
29755 + 1;
29756 iij6
29757 < 2;
29758 ++iij6)
29759 {
29760 if (j6valid
29761 [iij6]
29762 && IKabs(
29763 cj6array
29764 [ij6]
29765 - cj6array
29766 [iij6])
29767 < IKFAST_SOLUTION_THRESH
29768 && IKabs(
29769 sj6array
29770 [ij6]
29771 - sj6array
29772 [iij6])
29773 < IKFAST_SOLUTION_THRESH)
29774 {
29775 j6valid
29776 [iij6]
29777 = false;
29778 _ij6[1]
29779 = iij6;
29780 break;
29781 }
29782 }
29783 j6 = j6array
29784 [ij6];
29785 cj6 = cj6array
29786 [ij6];
29787 sj6 = sj6array
29788 [ij6];
29789 {
29790 IkReal evalcond
29791 [1];
29792 evalcond
29793 [0]
29794 = ((0.85)
29795 * (IKsin(
29796 j6)));
29797 if (IKabs(
29798 evalcond
29799 [0])
29800 > IKFAST_EVALCOND_THRESH)
29801 {
29802 continue;
29803 }
29804 }
29805
29806 rotationfunction0(
29807 solutions);
29808 }
29809 }
29810 }
29811 }
29812 }
29813 } while (
29814 0);
29815 if (bgotonextstatement)
29816 {
29817 bool
29818 bgotonextstatement
29819 = true;
29820 do
29821 {
29822 evalcond
29823 [0]
29824 = ((IKabs(
29825 py))
29826 + (IKabs((
29827 (-3.14159265358979)
29828 + (IKfmod(
29829 ((4.71238898038469)
29830 + j4),
29831 6.28318530717959))))));
29832 evalcond
29833 [1]
29834 = -0.85;
29835 evalcond
29836 [2]
29837 = 0;
29838 evalcond
29839 [3]
29840 = ((-0.2125)
29841 + (((-1.0)
29842 * (1.0)
29843 * (px
29844 * px))));
29845 if (IKabs(
29846 evalcond
29847 [0])
29848 < 0.0000010000000000
29849 && IKabs(
29850 evalcond
29851 [1])
29852 < 0.0000010000000000
29853 && IKabs(
29854 evalcond
29855 [2])
29856 < 0.0000010000000000
29857 && IKabs(
29858 evalcond
29859 [3])
29860 < 0.0000010000000000)
29861 {
29862 bgotonextstatement
29863 = false;
29864 {
29865 IkReal j6eval
29866 [1];
29867 IkReal
29868 x2041
29869 = ((1.0)
29870 * px);
29871 sj8 = 0;
29872 cj8 = -1.0;
29873 j8 = 3.14159265358979;
29874 pz = 0;
29875 j9 = 0;
29876 sj9 = 0;
29877 cj9 = 1.0;
29878 pp = px
29879 * px;
29880 npx
29881 = (px
29882 * r00);
29883 npy
29884 = (px
29885 * r01);
29886 npz
29887 = (px
29888 * r02);
29889 rxp0_0
29890 = 0;
29891 rxp0_1
29892 = (px
29893 * r20);
29894 rxp1_0
29895 = 0;
29896 rxp1_1
29897 = (px
29898 * r21);
29899 rxp2_0
29900 = 0;
29901 rxp2_1
29902 = (px
29903 * r22);
29904 py = 0;
29905 j4 = -1.5707963267949;
29906 sj4 = -1.0;
29907 cj4 = 0;
29908 rxp0_2
29909 = ((-1.0)
29910 * r10
29911 * x2041);
29912 rxp1_2
29913 = ((-1.0)
29914 * r11
29915 * x2041);
29916 rxp2_2
29917 = ((-1.0)
29918 * r12
29919 * x2041);
29920 j6eval
29921 [0]
29922 = ((1.0)
29923 + (((1.91568587540858)
29924 * (px
29925 * px
29926 * px
29927 * px)))
29928 + (((-1.0)
29929 * (2.64633970947792)
29930 * (px
29931 * px))));
29932 if (IKabs(
29933 j6eval
29934 [0])
29935 < 0.0000010000000000)
29936 {
29937 continue; // no branches [j6]
29938 }
29939 else
29940 {
29941 {
29942 IkReal j6array
29943 [2],
29944 cj6array
29945 [2],
29946 sj6array
29947 [2];
29948 bool j6valid
29949 [2]
29950 = {false};
29951 _nj6
29952 = 2;
29953 IkReal
29954 x2042
29955 = px
29956 * px;
29957 CheckValue<IkReal> x2044 = IKatan2WithCheck(
29958 IkReal((
29959 (-0.425)
29960 + (((-0.588235294117647)
29961 * x2042)))),
29962 ((2.83333333333333)
29963 + (((-3.92156862745098)
29964 * x2042))),
29965 IKFAST_ATAN2_MAGTHRESH);
29966 if (!x2044
29967 .valid)
29968 {
29969 continue;
29970 }
29971 IkReal
29972 x2043
29973 = ((-1.0)
29974 * (x2044
29975 .value));
29976 j6array
29977 [0]
29978 = x2043;
29979 sj6array
29980 [0]
29981 = IKsin(
29982 j6array
29983 [0]);
29984 cj6array
29985 [0]
29986 = IKcos(
29987 j6array
29988 [0]);
29989 j6array
29990 [1]
29991 = ((3.14159265358979)
29992 + x2043);
29993 sj6array
29994 [1]
29995 = IKsin(
29996 j6array
29997 [1]);
29998 cj6array
29999 [1]
30000 = IKcos(
30001 j6array
30002 [1]);
30003 if (j6array
30004 [0]
30005 > IKPI)
30006 {
30007 j6array
30008 [0]
30009 -= IK2PI;
30010 }
30011 else if (
30012 j6array
30013 [0]
30014 < -IKPI)
30015 {
30016 j6array
30017 [0]
30018 += IK2PI;
30019 }
30020 j6valid
30021 [0]
30022 = true;
30023 if (j6array
30024 [1]
30025 > IKPI)
30026 {
30027 j6array
30028 [1]
30029 -= IK2PI;
30030 }
30031 else if (
30032 j6array
30033 [1]
30034 < -IKPI)
30035 {
30036 j6array
30037 [1]
30038 += IK2PI;
30039 }
30040 j6valid
30041 [1]
30042 = true;
30043 for (
30044 int ij6
30045 = 0;
30046 ij6
30047 < 2;
30048 ++ij6)
30049 {
30050 if (!j6valid
30051 [ij6])
30052 {
30053 continue;
30054 }
30055 _ij6[0]
30056 = ij6;
30057 _ij6[1]
30058 = -1;
30059 for (
30060 int iij6
30061 = ij6
30062 + 1;
30063 iij6
30064 < 2;
30065 ++iij6)
30066 {
30067 if (j6valid
30068 [iij6]
30069 && IKabs(
30070 cj6array
30071 [ij6]
30072 - cj6array
30073 [iij6])
30074 < IKFAST_SOLUTION_THRESH
30075 && IKabs(
30076 sj6array
30077 [ij6]
30078 - sj6array
30079 [iij6])
30080 < IKFAST_SOLUTION_THRESH)
30081 {
30082 j6valid
30083 [iij6]
30084 = false;
30085 _ij6[1]
30086 = iij6;
30087 break;
30088 }
30089 }
30090 j6 = j6array
30091 [ij6];
30092 cj6 = cj6array
30093 [ij6];
30094 sj6 = sj6array
30095 [ij6];
30096 {
30097 IkReal evalcond
30098 [1];
30099 evalcond
30100 [0]
30101 = ((0.85)
30102 * (IKsin(
30103 j6)));
30104 if (IKabs(
30105 evalcond
30106 [0])
30107 > IKFAST_EVALCOND_THRESH)
30108 {
30109 continue;
30110 }
30111 }
30112
30113 rotationfunction0(
30114 solutions);
30115 }
30116 }
30117 }
30118 }
30119 }
30120 } while (
30121 0);
30122 if (bgotonextstatement)
30123 {
30124 bool
30125 bgotonextstatement
30126 = true;
30127 do
30128 {
30129 if (1)
30130 {
30131 bgotonextstatement
30132 = false;
30133 continue; // branch miss [j6]
30134 }
30135 } while (
30136 0);
30137 if (bgotonextstatement)
30138 {
30139 }
30140 }
30141 }
30142 }
30143 }
30144 }
30145 }
30146 }
30147 else
30148 {
30149 {
30150 IkReal j6array
30151 [1],
30152 cj6array
30153 [1],
30154 sj6array
30155 [1];
30156 bool j6valid
30157 [1]
30158 = {false};
30159 _nj6 = 1;
30160 IkReal
30161 x2045
30162 = (cj4
30163 * px);
30164 IkReal
30165 x2046
30166 = (py
30167 * sj4);
30168 IkReal
30169 x2047
30170 = px
30171 * px;
30172 IkReal
30173 x2048
30174 = py
30175 * py;
30176 CheckValue<
30177 IkReal>
30178 x2049
30179 = IKPowWithIntegerCheck(
30180 ((((20.0)
30181 * x2046))
30182 + (((20.0)
30183 * x2045))),
30184 -1);
30185 if (!x2049
30186 .valid)
30187 {
30188 continue;
30189 }
30190 CheckValue<IkReal> x2050 = IKPowWithIntegerCheck(
30191 ((((-8.5)
30192 * x2046))
30193 + (((-1.0)
30194 * (11.7647058823529)
30195 * cj4
30196 * (px
30197 * px
30198 * px)))
30199 + (((-8.5)
30200 * x2045))
30201 + (((-11.7647058823529)
30202 * x2045
30203 * x2048))
30204 + (((-11.7647058823529)
30205 * x2046
30206 * x2047))
30207 + (((-1.0)
30208 * (11.7647058823529)
30209 * sj4
30210 * (py
30211 * py
30212 * py)))),
30213 -1);
30214 if (!x2050
30215 .valid)
30216 {
30217 continue;
30218 }
30219 if (IKabs((
30220 (17.0)
30221 * (x2049
30222 .value)))
30223 < IKFAST_ATAN2_MAGTHRESH
30224 && IKabs((
30225 (x2050
30226 .value)
30227 * (((-48.1666666666667)
30228 + (((66.6666666666667)
30229 * x2047))
30230 + (((66.6666666666667)
30231 * x2048))))))
30232 < IKFAST_ATAN2_MAGTHRESH
30233 && IKabs(
30234 IKsqr((
30235 (17.0)
30236 * (x2049
30237 .value)))
30238 + IKsqr((
30239 (x2050
30240 .value)
30241 * (((-48.1666666666667)
30242 + (((66.6666666666667)
30243 * x2047))
30244 + (((66.6666666666667)
30245 * x2048))))))
30246 - 1)
30247 <= IKFAST_SINCOS_THRESH)
30248 continue;
30249 j6array[0] = IKatan2(
30250 ((17.0)
30251 * (x2049
30252 .value)),
30253 ((x2050
30254 .value)
30255 * (((-48.1666666666667)
30256 + (((66.6666666666667)
30257 * x2047))
30258 + (((66.6666666666667)
30259 * x2048))))));
30260 sj6array[0] = IKsin(
30261 j6array
30262 [0]);
30263 cj6array[0] = IKcos(
30264 j6array
30265 [0]);
30266 if (j6array
30267 [0]
30268 > IKPI)
30269 {
30270 j6array
30271 [0]
30272 -= IK2PI;
30273 }
30274 else if (
30275 j6array
30276 [0]
30277 < -IKPI)
30278 {
30279 j6array
30280 [0]
30281 += IK2PI;
30282 }
30283 j6valid[0]
30284 = true;
30285 for (
30286 int ij6
30287 = 0;
30288 ij6
30289 < 1;
30290 ++ij6)
30291 {
30292 if (!j6valid
30293 [ij6])
30294 {
30295 continue;
30296 }
30297 _ij6[0]
30298 = ij6;
30299 _ij6[1]
30300 = -1;
30301 for (
30302 int iij6
30303 = ij6
30304 + 1;
30305 iij6
30306 < 1;
30307 ++iij6)
30308 {
30309 if (j6valid
30310 [iij6]
30311 && IKabs(
30312 cj6array
30313 [ij6]
30314 - cj6array
30315 [iij6])
30316 < IKFAST_SOLUTION_THRESH
30317 && IKabs(
30318 sj6array
30319 [ij6]
30320 - sj6array
30321 [iij6])
30322 < IKFAST_SOLUTION_THRESH)
30323 {
30324 j6valid
30325 [iij6]
30326 = false;
30327 _ij6[1]
30328 = iij6;
30329 break;
30330 }
30331 }
30332 j6 = j6array
30333 [ij6];
30334 cj6 = cj6array
30335 [ij6];
30336 sj6 = sj6array
30337 [ij6];
30338 {
30339 IkReal evalcond
30340 [5];
30341 IkReal
30342 x2051
30343 = IKcos(
30344 j6);
30345 IkReal
30346 x2052
30347 = (cj4
30348 * px);
30349 IkReal
30350 x2053
30351 = (x2051
30352 * x2052);
30353 IkReal
30354 x2054
30355 = (py
30356 * sj4);
30357 IkReal
30358 x2055
30359 = (x2051
30360 * x2054);
30361 IkReal
30362 x2056
30363 = IKsin(
30364 j6);
30365 IkReal
30366 x2057
30367 = (x2052
30368 * x2056);
30369 IkReal
30370 x2058
30371 = (x2054
30372 * x2056);
30373 IkReal
30374 x2059
30375 = px
30376 * px;
30377 IkReal
30378 x2060
30379 = ((3.92156862745098)
30380 * x2056);
30381 IkReal
30382 x2061
30383 = ((0.588235294117647)
30384 * x2051);
30385 IkReal
30386 x2062
30387 = py
30388 * py;
30389 evalcond
30390 [0]
30391 = (x2055
30392 + x2053);
30393 evalcond
30394 [1]
30395 = ((-0.85)
30396 + x2057
30397 + x2058);
30398 evalcond
30399 [2]
30400 = ((((0.85)
30401 * x2056))
30402 + (((-1.0)
30403 * x2052))
30404 + (((-1.0)
30405 * x2054)));
30406 evalcond
30407 [3]
30408 = ((((-1.0)
30409 * x2061
30410 * x2062))
30411 + (((-1.0)
30412 * x2059
30413 * x2061))
30414 + (((-0.425)
30415 * x2051))
30416 + (((-1.0)
30417 * x2059
30418 * x2060))
30419 + (((2.83333333333333)
30420 * x2056))
30421 + (((-1.0)
30422 * x2060
30423 * x2062)));
30424 evalcond
30425 [4]
30426 = ((-0.2125)
30427 + (((1.1)
30428 * x2057))
30429 + (((-1.0)
30430 * x2059))
30431 + (((-0.09)
30432 * x2055))
30433 + (((1.1)
30434 * x2058))
30435 + (((-1.0)
30436 * x2062))
30437 + (((-0.09)
30438 * x2053)));
30439 if (IKabs(
30440 evalcond
30441 [0])
30442 > IKFAST_EVALCOND_THRESH
30443 || IKabs(
30444 evalcond
30445 [1])
30446 > IKFAST_EVALCOND_THRESH
30447 || IKabs(
30448 evalcond
30449 [2])
30450 > IKFAST_EVALCOND_THRESH
30451 || IKabs(
30452 evalcond
30453 [3])
30454 > IKFAST_EVALCOND_THRESH
30455 || IKabs(
30456 evalcond
30457 [4])
30458 > IKFAST_EVALCOND_THRESH)
30459 {
30460 continue;
30461 }
30462 }
30463
30464 rotationfunction0(
30465 solutions);
30466 }
30467 }
30468 }
30469 }
30470 }
30471 else
30472 {
30473 {
30474 IkReal j6array
30475 [1],
30476 cj6array
30477 [1],
30478 sj6array
30479 [1];
30480 bool
30481 j6valid[1]
30482 = {false};
30483 _nj6 = 1;
30484 IkReal x2063
30485 = (cj4
30486 * px);
30487 IkReal x2064
30488 = (py
30489 * sj4);
30490 IkReal x2065
30491 = px * px;
30492 IkReal x2066
30493 = py * py;
30494 CheckValue<
30495 IkReal>
30496 x2067
30497 = IKPowWithIntegerCheck(
30498 ((-7.225)
30499 + (((-10.0)
30500 * x2066))
30501 + (((-10.0)
30502 * x2065))),
30503 -1);
30504 if (!x2067
30505 .valid)
30506 {
30507 continue;
30508 }
30509 if (IKabs((
30510 (((1.17647058823529)
30511 * x2063))
30512 + (((1.17647058823529)
30513 * x2064))))
30514 < IKFAST_ATAN2_MAGTHRESH
30515 && IKabs((
30516 (x2067
30517 .value)
30518 * (((((-56.6666666666667)
30519 * x2064))
30520 + (((78.4313725490196)
30521 * sj4
30522 * (py
30523 * py
30524 * py)))
30525 + (((78.4313725490196)
30526 * cj4
30527 * (px
30528 * px
30529 * px)))
30530 + (((78.4313725490196)
30531 * x2063
30532 * x2066))
30533 + (((78.4313725490196)
30534 * x2064
30535 * x2065))
30536 + (((-56.6666666666667)
30537 * x2063))))))
30538 < IKFAST_ATAN2_MAGTHRESH
30539 && IKabs(
30540 IKsqr((
30541 (((1.17647058823529)
30542 * x2063))
30543 + (((1.17647058823529)
30544 * x2064))))
30545 + IKsqr((
30546 (x2067
30547 .value)
30548 * (((((-56.6666666666667)
30549 * x2064))
30550 + (((78.4313725490196)
30551 * sj4
30552 * (py
30553 * py
30554 * py)))
30555 + (((78.4313725490196)
30556 * cj4
30557 * (px
30558 * px
30559 * px)))
30560 + (((78.4313725490196)
30561 * x2063
30562 * x2066))
30563 + (((78.4313725490196)
30564 * x2064
30565 * x2065))
30566 + (((-56.6666666666667)
30567 * x2063))))))
30568 - 1)
30569 <= IKFAST_SINCOS_THRESH)
30570 continue;
30571 j6array[0] = IKatan2(
30572 ((((1.17647058823529)
30573 * x2063))
30574 + (((1.17647058823529)
30575 * x2064))),
30576 ((x2067
30577 .value)
30578 * (((((-56.6666666666667)
30579 * x2064))
30580 + (((78.4313725490196)
30581 * sj4
30582 * (py
30583 * py
30584 * py)))
30585 + (((78.4313725490196)
30586 * cj4
30587 * (px
30588 * px
30589 * px)))
30590 + (((78.4313725490196)
30591 * x2063
30592 * x2066))
30593 + (((78.4313725490196)
30594 * x2064
30595 * x2065))
30596 + (((-56.6666666666667)
30597 * x2063))))));
30598 sj6array[0]
30599 = IKsin(
30600 j6array
30601 [0]);
30602 cj6array[0]
30603 = IKcos(
30604 j6array
30605 [0]);
30606 if (j6array[0]
30607 > IKPI)
30608 {
30609 j6array[0]
30610 -= IK2PI;
30611 }
30612 else if (
30613 j6array[0]
30614 < -IKPI)
30615 {
30616 j6array[0]
30617 += IK2PI;
30618 }
30619 j6valid[0]
30620 = true;
30621 for (int ij6
30622 = 0;
30623 ij6 < 1;
30624 ++ij6)
30625 {
30626 if (!j6valid
30627 [ij6])
30628 {
30629 continue;
30630 }
30631 _ij6[0]
30632 = ij6;
30633 _ij6[1]
30634 = -1;
30635 for (
30636 int iij6
30637 = ij6
30638 + 1;
30639 iij6
30640 < 1;
30641 ++iij6)
30642 {
30643 if (j6valid
30644 [iij6]
30645 && IKabs(
30646 cj6array
30647 [ij6]
30648 - cj6array
30649 [iij6])
30650 < IKFAST_SOLUTION_THRESH
30651 && IKabs(
30652 sj6array
30653 [ij6]
30654 - sj6array
30655 [iij6])
30656 < IKFAST_SOLUTION_THRESH)
30657 {
30658 j6valid
30659 [iij6]
30660 = false;
30661 _ij6[1]
30662 = iij6;
30663 break;
30664 }
30665 }
30666 j6 = j6array
30667 [ij6];
30668 cj6 = cj6array
30669 [ij6];
30670 sj6 = sj6array
30671 [ij6];
30672 {
30673 IkReal evalcond
30674 [5];
30675 IkReal
30676 x2068
30677 = IKcos(
30678 j6);
30679 IkReal
30680 x2069
30681 = (cj4
30682 * px);
30683 IkReal
30684 x2070
30685 = (x2068
30686 * x2069);
30687 IkReal
30688 x2071
30689 = (py
30690 * sj4);
30691 IkReal
30692 x2072
30693 = (x2068
30694 * x2071);
30695 IkReal
30696 x2073
30697 = IKsin(
30698 j6);
30699 IkReal
30700 x2074
30701 = (x2069
30702 * x2073);
30703 IkReal
30704 x2075
30705 = (x2071
30706 * x2073);
30707 IkReal
30708 x2076
30709 = px
30710 * px;
30711 IkReal
30712 x2077
30713 = ((3.92156862745098)
30714 * x2073);
30715 IkReal
30716 x2078
30717 = ((0.588235294117647)
30718 * x2068);
30719 IkReal
30720 x2079
30721 = py
30722 * py;
30723 evalcond
30724 [0]
30725 = (x2070
30726 + x2072);
30727 evalcond
30728 [1]
30729 = ((-0.85)
30730 + x2075
30731 + x2074);
30732 evalcond
30733 [2]
30734 = ((((-1.0)
30735 * x2071))
30736 + (((0.85)
30737 * x2073))
30738 + (((-1.0)
30739 * x2069)));
30740 evalcond
30741 [3]
30742 = ((((-1.0)
30743 * x2076
30744 * x2077))
30745 + (((-1.0)
30746 * x2076
30747 * x2078))
30748 + (((-0.425)
30749 * x2068))
30750 + (((-1.0)
30751 * x2078
30752 * x2079))
30753 + (((2.83333333333333)
30754 * x2073))
30755 + (((-1.0)
30756 * x2077
30757 * x2079)));
30758 evalcond
30759 [4]
30760 = ((-0.2125)
30761 + (((-1.0)
30762 * x2079))
30763 + (((-1.0)
30764 * x2076))
30765 + (((1.1)
30766 * x2075))
30767 + (((-0.09)
30768 * x2070))
30769 + (((1.1)
30770 * x2074))
30771 + (((-0.09)
30772 * x2072)));
30773 if (IKabs(
30774 evalcond
30775 [0])
30776 > IKFAST_EVALCOND_THRESH
30777 || IKabs(
30778 evalcond
30779 [1])
30780 > IKFAST_EVALCOND_THRESH
30781 || IKabs(
30782 evalcond
30783 [2])
30784 > IKFAST_EVALCOND_THRESH
30785 || IKabs(
30786 evalcond
30787 [3])
30788 > IKFAST_EVALCOND_THRESH
30789 || IKabs(
30790 evalcond
30791 [4])
30792 > IKFAST_EVALCOND_THRESH)
30793 {
30794 continue;
30795 }
30796 }
30797
30798 rotationfunction0(
30799 solutions);
30800 }
30801 }
30802 }
30803 }
30804 }
30805 else
30806 {
30807 {
30808 IkReal j6array[1],
30809 cj6array[1],
30810 sj6array[1];
30811 bool j6valid[1]
30812 = {false};
30813 _nj6 = 1;
30814 IkReal x2080
30815 = (cj4 * px);
30816 IkReal x2081
30817 = (py * sj4);
30818 IkReal x2082
30819 = px * px;
30820 IkReal x2083
30821 = py * py;
30822 IkReal x2084
30823 = ((1.29411764705882)
30824 * (cj4
30825 * cj4));
30826 CheckValue<IkReal>
30827 x2085
30828 = IKPowWithIntegerCheck(
30829 ((((-0.09)
30830 * x2080))
30831 + (((-0.09)
30832 * x2081))),
30833 -1);
30834 if (!x2085.valid)
30835 {
30836 continue;
30837 }
30838 if (IKabs((
30839 (((1.17647058823529)
30840 * x2080))
30841 + (((1.17647058823529)
30842 * x2081))))
30843 < IKFAST_ATAN2_MAGTHRESH
30844 && IKabs((
30845 (x2085
30846 .value)
30847 * (((0.2125)
30848 + (((-2.58823529411765)
30849 * cj4
30850 * px
30851 * x2081))
30852 + (((-0.294117647058824)
30853 * x2083))
30854 + ((x2083
30855 * x2084))
30856 + (((-1.0)
30857 * x2082
30858 * x2084))
30859 + x2082))))
30860 < IKFAST_ATAN2_MAGTHRESH
30861 && IKabs(
30862 IKsqr((
30863 (((1.17647058823529)
30864 * x2080))
30865 + (((1.17647058823529)
30866 * x2081))))
30867 + IKsqr((
30868 (x2085
30869 .value)
30870 * (((0.2125)
30871 + (((-2.58823529411765)
30872 * cj4
30873 * px
30874 * x2081))
30875 + (((-0.294117647058824)
30876 * x2083))
30877 + ((x2083
30878 * x2084))
30879 + (((-1.0)
30880 * x2082
30881 * x2084))
30882 + x2082))))
30883 - 1)
30884 <= IKFAST_SINCOS_THRESH)
30885 continue;
30886 j6array[0] = IKatan2(
30887 ((((1.17647058823529)
30888 * x2080))
30889 + (((1.17647058823529)
30890 * x2081))),
30891 ((x2085.value)
30892 * (((0.2125)
30893 + (((-2.58823529411765)
30894 * cj4
30895 * px
30896 * x2081))
30897 + (((-0.294117647058824)
30898 * x2083))
30899 + ((x2083
30900 * x2084))
30901 + (((-1.0)
30902 * x2082
30903 * x2084))
30904 + x2082))));
30905 sj6array[0]
30906 = IKsin(
30907 j6array
30908 [0]);
30909 cj6array[0]
30910 = IKcos(
30911 j6array
30912 [0]);
30913 if (j6array[0]
30914 > IKPI)
30915 {
30916 j6array[0]
30917 -= IK2PI;
30918 }
30919 else if (
30920 j6array[0]
30921 < -IKPI)
30922 {
30923 j6array[0]
30924 += IK2PI;
30925 }
30926 j6valid[0] = true;
30927 for (int ij6 = 0;
30928 ij6 < 1;
30929 ++ij6)
30930 {
30931 if (!j6valid
30932 [ij6])
30933 {
30934 continue;
30935 }
30936 _ij6[0] = ij6;
30937 _ij6[1] = -1;
30938 for (int iij6
30939 = ij6 + 1;
30940 iij6 < 1;
30941 ++iij6)
30942 {
30943 if (j6valid
30944 [iij6]
30945 && IKabs(
30946 cj6array
30947 [ij6]
30948 - cj6array
30949 [iij6])
30950 < IKFAST_SOLUTION_THRESH
30951 && IKabs(
30952 sj6array
30953 [ij6]
30954 - sj6array
30955 [iij6])
30956 < IKFAST_SOLUTION_THRESH)
30957 {
30958 j6valid
30959 [iij6]
30960 = false;
30961 _ij6[1]
30962 = iij6;
30963 break;
30964 }
30965 }
30966 j6 = j6array
30967 [ij6];
30968 cj6 = cj6array
30969 [ij6];
30970 sj6 = sj6array
30971 [ij6];
30972 {
30973 IkReal
30974 evalcond
30975 [5];
30976 IkReal x2086
30977 = IKcos(
30978 j6);
30979 IkReal x2087
30980 = (cj4
30981 * px);
30982 IkReal x2088
30983 = (x2086
30984 * x2087);
30985 IkReal x2089
30986 = (py
30987 * sj4);
30988 IkReal x2090
30989 = (x2086
30990 * x2089);
30991 IkReal x2091
30992 = IKsin(
30993 j6);
30994 IkReal x2092
30995 = (x2087
30996 * x2091);
30997 IkReal x2093
30998 = (x2089
30999 * x2091);
31000 IkReal x2094
31001 = px * px;
31002 IkReal x2095
31003 = ((3.92156862745098)
31004 * x2091);
31005 IkReal x2096
31006 = ((0.588235294117647)
31007 * x2086);
31008 IkReal x2097
31009 = py * py;
31010 evalcond[0]
31011 = (x2090
31012 + x2088);
31013 evalcond[1]
31014 = ((-0.85)
31015 + x2093
31016 + x2092);
31017 evalcond[2]
31018 = ((((-1.0)
31019 * x2089))
31020 + (((-1.0)
31021 * x2087))
31022 + (((0.85)
31023 * x2091)));
31024 evalcond[3]
31025 = ((((-1.0)
31026 * x2095
31027 * x2097))
31028 + (((-1.0)
31029 * x2094
31030 * x2096))
31031 + (((-1.0)
31032 * x2096
31033 * x2097))
31034 + (((2.83333333333333)
31035 * x2091))
31036 + (((-1.0)
31037 * x2094
31038 * x2095))
31039 + (((-0.425)
31040 * x2086)));
31041 evalcond[4]
31042 = ((-0.2125)
31043 + (((-1.0)
31044 * x2094))
31045 + (((1.1)
31046 * x2093))
31047 + (((1.1)
31048 * x2092))
31049 + (((-1.0)
31050 * x2097))
31051 + (((-0.09)
31052 * x2090))
31053 + (((-0.09)
31054 * x2088)));
31055 if (IKabs(
31056 evalcond
31057 [0])
31058 > IKFAST_EVALCOND_THRESH
31059 || IKabs(
31060 evalcond
31061 [1])
31062 > IKFAST_EVALCOND_THRESH
31063 || IKabs(
31064 evalcond
31065 [2])
31066 > IKFAST_EVALCOND_THRESH
31067 || IKabs(
31068 evalcond
31069 [3])
31070 > IKFAST_EVALCOND_THRESH
31071 || IKabs(
31072 evalcond
31073 [4])
31074 > IKFAST_EVALCOND_THRESH)
31075 {
31076 continue;
31077 }
31078 }
31079
31080 rotationfunction0(
31081 solutions);
31082 }
31083 }
31084 }
31085 }
31086 }
31087 } while (0);
31088 if (bgotonextstatement)
31089 {
31090 bool bgotonextstatement
31091 = true;
31092 do
31093 {
31094 if (1)
31095 {
31096 bgotonextstatement
31097 = false;
31098 continue; // branch
31099 // miss [j6]
31100 }
31101 } while (0);
31102 if (bgotonextstatement)
31103 {
31104 }
31105 }
31106 }
31107 }
31108 else
31109 {
31110 {
31111 IkReal j6array[1],
31112 cj6array[1],
31113 sj6array[1];
31114 bool j6valid[1] = {false};
31115 _nj6 = 1;
31116 IkReal x2098 = (cj4 * px);
31117 IkReal x2099 = (py * sj4);
31118 IkReal x2100
31119 = ((0.108264705882353)
31120 * cj9);
31121 IkReal x2101
31122 = ((0.588235294117647)
31123 * pp);
31124 IkReal x2102 = (cj9 * pp);
31125 IkReal x2103 = (cj9 * sj9);
31126 IkReal x2104 = (pp * sj9);
31127 IkReal x2105 = cj9 * cj9;
31128 IkReal x2106 = ((1.0) * pz);
31129 CheckValue<IkReal> x2107
31130 = IKPowWithIntegerCheck(
31131 IKsign((
31132 (((-1.0) * x2099
31133 * x2100))
31134 + (((-1.0)
31135 * x2098
31136 * x2101))
31137 + (((-1.0)
31138 * (1.32323529411765)
31139 * cj9 * pz))
31140 + (((3.92156862745098)
31141 * pp * pz))
31142 + (((-1.0)
31143 * x2098
31144 * x2100))
31145 + (((-1.0)
31146 * (1.51009803921569)
31147 * pz))
31148 + (((-0.316735294117647)
31149 * x2098))
31150 + (((-1.0)
31151 * x2099
31152 * x2101))
31153 + (((-0.316735294117647)
31154 * x2099)))),
31155 -1);
31156 if (!x2107.valid)
31157 {
31158 continue;
31159 }
31160 CheckValue<IkReal> x2108 = IKatan2WithCheck(
31161 IkReal((
31162 (-0.174204411764706)
31163 + (((-0.0324794117647059)
31164 * x2105))
31165 + (pz * pz)
31166 + (((-0.0264705882352941)
31167 * x2104))
31168 + (((-1.0)
31169 * (0.154566176470588)
31170 * cj9))
31171 + (((-1.0)
31172 * (0.323529411764706)
31173 * pp))
31174 + (((-0.00487191176470588)
31175 * x2103))
31176 + (((-1.0)
31177 * (0.0142530882352941)
31178 * sj9))
31179 + (((-0.176470588235294)
31180 * x2102)))),
31181 ((-0.830553921568627)
31182 + (((-1.0)
31183 * (0.0679544117647059)
31184 * sj9))
31185 + (((-1.0)
31186 * (1.18080882352941)
31187 * cj9))
31188 + (((-0.396970588235294)
31189 * x2105))
31190 + (((-1.0) * x2099
31191 * x2106))
31192 + (((-0.0595455882352941)
31193 * x2103))
31194 + (((2.15686274509804)
31195 * pp))
31196 + (((-1.0) * x2098
31197 * x2106))
31198 + (((0.176470588235294)
31199 * x2104))
31200 + (((1.17647058823529)
31201 * x2102))),
31202 IKFAST_ATAN2_MAGTHRESH);
31203 if (!x2108.valid)
31204 {
31205 continue;
31206 }
31207 j6array[0]
31208 = ((-1.5707963267949)
31209 + (((1.5707963267949)
31210 * (x2107.value)))
31211 + (x2108.value));
31212 sj6array[0]
31213 = IKsin(j6array[0]);
31214 cj6array[0]
31215 = IKcos(j6array[0]);
31216 if (j6array[0] > IKPI)
31217 {
31218 j6array[0] -= IK2PI;
31219 }
31220 else if (j6array[0] < -IKPI)
31221 {
31222 j6array[0] += IK2PI;
31223 }
31224 j6valid[0] = true;
31225 for (int ij6 = 0; ij6 < 1;
31226 ++ij6)
31227 {
31228 if (!j6valid[ij6])
31229 {
31230 continue;
31231 }
31232 _ij6[0] = ij6;
31233 _ij6[1] = -1;
31234 for (int iij6 = ij6 + 1;
31235 iij6 < 1;
31236 ++iij6)
31237 {
31238 if (j6valid[iij6]
31239 && IKabs(
31240 cj6array[ij6]
31241 - cj6array
31242 [iij6])
31243 < IKFAST_SOLUTION_THRESH
31244 && IKabs(
31245 sj6array[ij6]
31246 - sj6array
31247 [iij6])
31248 < IKFAST_SOLUTION_THRESH)
31249 {
31250 j6valid[iij6] = false;
31251 _ij6[1] = iij6;
31252 break;
31253 }
31254 }
31255 j6 = j6array[ij6];
31256 cj6 = cj6array[ij6];
31257 sj6 = sj6array[ij6];
31258 {
31259 IkReal evalcond[5];
31260 IkReal x2109
31261 = ((0.3) * cj9);
31262 IkReal x2110
31263 = ((0.045) * sj9);
31264 IkReal x2111
31265 = IKcos(j6);
31266 IkReal x2112
31267 = (pz * x2111);
31268 IkReal x2113
31269 = IKsin(j6);
31270 IkReal x2114
31271 = (cj4 * px);
31272 IkReal x2115
31273 = (x2113 * x2114);
31274 IkReal x2116
31275 = (py * sj4);
31276 IkReal x2117
31277 = (x2113 * x2116);
31278 IkReal x2118
31279 = ((0.045) * cj9);
31280 IkReal x2119
31281 = ((0.3) * sj9);
31282 IkReal x2120
31283 = (pz * x2113);
31284 IkReal x2121
31285 = (x2111 * x2114);
31286 IkReal x2122
31287 = (x2111 * x2116);
31288 evalcond[0]
31289 = ((-0.55) + x2115
31290 + x2117 + x2112
31291 + (((-1.0)
31292 * x2110))
31293 + (((-1.0)
31294 * x2109)));
31295 evalcond[1]
31296 = ((0.045) + x2119
31297 + x2121 + x2122
31298 + (((-1.0)
31299 * x2118))
31300 + (((-1.0)
31301 * x2120)));
31302 evalcond[2]
31303 = ((((-3.92156862745098)
31304 * pp * x2113))
31305 + pz
31306 + (((-0.316735294117647)
31307 * x2111))
31308 + (((-0.588235294117647)
31309 * pp
31310 * x2111))
31311 + (((1.32323529411765)
31312 * cj9
31313 * x2113))
31314 + (((-0.108264705882353)
31315 * cj9
31316 * x2111))
31317 + (((1.51009803921569)
31318 * x2113)));
31319 evalcond[3]
31320 = ((((-1.0)
31321 * x2116))
31322 + (((-1.0)
31323 * x2114))
31324 + ((x2111
31325 * x2118))
31326 + (((-1.0)
31327 * x2111
31328 * x2119))
31329 + ((x2110
31330 * x2113))
31331 + (((0.55)
31332 * x2113))
31333 + (((-0.045)
31334 * x2111))
31335 + ((x2109
31336 * x2113)));
31337 evalcond[4]
31338 = ((-0.2125)
31339 + (((1.1)
31340 * x2115))
31341 + (((1.1)
31342 * x2112))
31343 + (((1.1)
31344 * x2117))
31345 + (((-0.09)
31346 * x2122))
31347 + (((-1.0)
31348 * (1.0)
31349 * pp))
31350 + (((0.09)
31351 * x2120))
31352 + (((-0.09)
31353 * x2121)));
31354 if (IKabs(evalcond[0])
31355 > IKFAST_EVALCOND_THRESH
31356 || IKabs(
31357 evalcond[1])
31358 > IKFAST_EVALCOND_THRESH
31359 || IKabs(
31360 evalcond[2])
31361 > IKFAST_EVALCOND_THRESH
31362 || IKabs(
31363 evalcond[3])
31364 > IKFAST_EVALCOND_THRESH
31365 || IKabs(
31366 evalcond[4])
31367 > IKFAST_EVALCOND_THRESH)
31368 {
31369 continue;
31370 }
31371 }
31372
31373 rotationfunction0(
31374 solutions);
31375 }
31376 }
31377 }
31378 }
31379 }
31380 else
31381 {
31382 {
31383 IkReal j6array[1], cj6array[1],
31384 sj6array[1];
31385 bool j6valid[1] = {false};
31386 _nj6 = 1;
31387 IkReal x2123
31388 = ((0.045) * cj4 * px);
31389 IkReal x2124
31390 = ((0.045) * py * sj4);
31391 IkReal x2125 = ((0.3) * sj9);
31392 IkReal x2126 = (cj4 * px);
31393 IkReal x2127 = (py * sj4);
31394 IkReal x2128 = (cj9 * sj9);
31395 IkReal x2129 = cj9 * cj9;
31396 IkReal x2130 = ((1.0) * pz);
31397 IkReal x2131 = py * py;
31398 IkReal x2132 = cj4 * cj4;
31399 CheckValue<IkReal> x2133
31400 = IKPowWithIntegerCheck(
31401 IKsign((
31402 (((-1.0) * (0.55)
31403 * pz))
31404 + (((-1.0) * x2125
31405 * x2127))
31406 + (((-1.0) * x2125
31407 * x2126))
31408 + ((cj9 * x2123))
31409 + (((-1.0) * x2123))
31410 + ((cj9 * x2124))
31411 + (((-1.0) * x2124))
31412 + (((-1.0) * (0.045)
31413 * pz * sj9))
31414 + (((-1.0) * (0.3)
31415 * cj9 * pz)))),
31416 -1);
31417 if (!x2133.valid)
31418 {
31419 continue;
31420 }
31421 CheckValue<IkReal> x2134
31422 = IKatan2WithCheck(
31423 IkReal(
31424 ((-0.03825)
31425 + (((-0.087975)
31426 * x2128))
31427 + (((0.027)
31428 * x2129))
31429 + (((-1.0)
31430 * (0.167025)
31431 * sj9))
31432 + (((-1.0) * x2126
31433 * x2130))
31434 + (((-1.0) * x2127
31435 * x2130))
31436 + (((0.01125)
31437 * cj9)))),
31438 ((-0.304525)
31439 + ((x2132 * (px * px)))
31440 + (((-1.0) * (0.0495)
31441 * sj9))
31442 + (((-0.027) * x2128))
31443 + (((-1.0) * x2131
31444 * x2132))
31445 + (((-1.0) * (0.33)
31446 * cj9))
31447 + (((-0.087975)
31448 * x2129))
31449 + (((2.0) * cj4 * px
31450 * x2127))
31451 + x2131),
31452 IKFAST_ATAN2_MAGTHRESH);
31453 if (!x2134.valid)
31454 {
31455 continue;
31456 }
31457 j6array[0]
31458 = ((-1.5707963267949)
31459 + (((1.5707963267949)
31460 * (x2133.value)))
31461 + (x2134.value));
31462 sj6array[0] = IKsin(j6array[0]);
31463 cj6array[0] = IKcos(j6array[0]);
31464 if (j6array[0] > IKPI)
31465 {
31466 j6array[0] -= IK2PI;
31467 }
31468 else if (j6array[0] < -IKPI)
31469 {
31470 j6array[0] += IK2PI;
31471 }
31472 j6valid[0] = true;
31473 for (int ij6 = 0; ij6 < 1;
31474 ++ij6)
31475 {
31476 if (!j6valid[ij6])
31477 {
31478 continue;
31479 }
31480 _ij6[0] = ij6;
31481 _ij6[1] = -1;
31482 for (int iij6 = ij6 + 1;
31483 iij6 < 1;
31484 ++iij6)
31485 {
31486 if (j6valid[iij6]
31487 && IKabs(
31488 cj6array[ij6]
31489 - cj6array[iij6])
31490 < IKFAST_SOLUTION_THRESH
31491 && IKabs(
31492 sj6array[ij6]
31493 - sj6array[iij6])
31494 < IKFAST_SOLUTION_THRESH)
31495 {
31496 j6valid[iij6] = false;
31497 _ij6[1] = iij6;
31498 break;
31499 }
31500 }
31501 j6 = j6array[ij6];
31502 cj6 = cj6array[ij6];
31503 sj6 = sj6array[ij6];
31504 {
31505 IkReal evalcond[5];
31506 IkReal x2135
31507 = ((0.3) * cj9);
31508 IkReal x2136
31509 = ((0.045) * sj9);
31510 IkReal x2137 = IKcos(j6);
31511 IkReal x2138 = (pz * x2137);
31512 IkReal x2139 = IKsin(j6);
31513 IkReal x2140 = (cj4 * px);
31514 IkReal x2141
31515 = (x2139 * x2140);
31516 IkReal x2142 = (py * sj4);
31517 IkReal x2143
31518 = (x2139 * x2142);
31519 IkReal x2144
31520 = ((0.045) * cj9);
31521 IkReal x2145
31522 = ((0.3) * sj9);
31523 IkReal x2146 = (pz * x2139);
31524 IkReal x2147
31525 = (x2137 * x2140);
31526 IkReal x2148
31527 = (x2137 * x2142);
31528 evalcond[0]
31529 = ((-0.55)
31530 + (((-1.0) * x2136))
31531 + x2141 + x2143
31532 + (((-1.0) * x2135))
31533 + x2138);
31534 evalcond[1]
31535 = ((0.045) + x2148
31536 + x2145 + x2147
31537 + (((-1.0) * x2144))
31538 + (((-1.0)
31539 * x2146)));
31540 evalcond[2]
31541 = ((((-0.588235294117647)
31542 * pp * x2137))
31543 + (((1.32323529411765)
31544 * cj9 * x2139))
31545 + (((1.51009803921569)
31546 * x2139))
31547 + pz
31548 + (((-3.92156862745098)
31549 * pp * x2139))
31550 + (((-0.316735294117647)
31551 * x2137))
31552 + (((-0.108264705882353)
31553 * cj9 * x2137)));
31554 evalcond[3]
31555 = ((((0.55) * x2139))
31556 + (((-0.045)
31557 * x2137))
31558 + (((-1.0) * x2140))
31559 + ((x2135 * x2139))
31560 + (((-1.0) * x2142))
31561 + ((x2136 * x2139))
31562 + ((x2137 * x2144))
31563 + (((-1.0) * x2137
31564 * x2145)));
31565 evalcond[4]
31566 = ((-0.2125)
31567 + (((-0.09) * x2148))
31568 + (((1.1) * x2143))
31569 + (((-0.09) * x2147))
31570 + (((1.1) * x2141))
31571 + (((0.09) * x2146))
31572 + (((1.1) * x2138))
31573 + (((-1.0) * (1.0)
31574 * pp)));
31575 if (IKabs(evalcond[0])
31576 > IKFAST_EVALCOND_THRESH
31577 || IKabs(evalcond[1])
31578 > IKFAST_EVALCOND_THRESH
31579 || IKabs(evalcond[2])
31580 > IKFAST_EVALCOND_THRESH
31581 || IKabs(evalcond[3])
31582 > IKFAST_EVALCOND_THRESH
31583 || IKabs(evalcond[4])
31584 > IKFAST_EVALCOND_THRESH)
31585 {
31586 continue;
31587 }
31588 }
31589
31590 rotationfunction0(solutions);
31591 }
31592 }
31593 }
31594 }
31595 }
31596 else
31597 {
31598 {
31599 IkReal j6array[1], cj6array[1],
31600 sj6array[1];
31601 bool j6valid[1] = {false};
31602 _nj6 = 1;
31603 IkReal x2149 = py * py;
31604 IkReal x2150 = (py * sj4);
31605 IkReal x2151 = cj4 * cj4;
31606 IkReal x2152 = ((0.045) * pz);
31607 IkReal x2153 = (cj4 * px);
31608 IkReal x2154 = ((0.3) * pz);
31609 IkReal x2155 = ((0.3) * cj4 * px);
31610 IkReal x2156 = ((0.045) * x2153);
31611 IkReal x2157 = ((0.3) * py * sj4);
31612 IkReal x2158 = ((0.045) * x2150);
31613 CheckValue<IkReal> x2159
31614 = IKPowWithIntegerCheck(
31615 IKsign(
31616 ((((-1.0) * x2149
31617 * x2151))
31618 + (pz * pz) + x2149
31619 + (((2.0) * cj4 * px
31620 * x2150))
31621 + ((x2151
31622 * (px * px))))),
31623 -1);
31624 if (!x2159.valid)
31625 {
31626 continue;
31627 }
31628 CheckValue<IkReal> x2160
31629 = IKatan2WithCheck(
31630 IkReal(
31631 (((sj9 * x2158)) + x2152
31632 + (((0.55) * x2150))
31633 + ((cj9 * x2155))
31634 + (((0.55) * x2153))
31635 + ((sj9 * x2156))
31636 + ((sj9 * x2154))
31637 + (((-1.0) * cj9
31638 * x2152))
31639 + ((cj9 * x2157)))),
31640 ((((-1.0) * sj9 * x2157))
31641 + (((-1.0) * x2156))
31642 + ((cj9 * x2154))
31643 + ((sj9 * x2152))
31644 + (((-1.0) * sj9 * x2155))
31645 + ((cj9 * x2156))
31646 + (((-1.0) * x2158))
31647 + (((0.55) * pz))
31648 + ((cj9 * x2158))),
31649 IKFAST_ATAN2_MAGTHRESH);
31650 if (!x2160.valid)
31651 {
31652 continue;
31653 }
31654 j6array[0]
31655 = ((-1.5707963267949)
31656 + (((1.5707963267949)
31657 * (x2159.value)))
31658 + (x2160.value));
31659 sj6array[0] = IKsin(j6array[0]);
31660 cj6array[0] = IKcos(j6array[0]);
31661 if (j6array[0] > IKPI)
31662 {
31663 j6array[0] -= IK2PI;
31664 }
31665 else if (j6array[0] < -IKPI)
31666 {
31667 j6array[0] += IK2PI;
31668 }
31669 j6valid[0] = true;
31670 for (int ij6 = 0; ij6 < 1; ++ij6)
31671 {
31672 if (!j6valid[ij6])
31673 {
31674 continue;
31675 }
31676 _ij6[0] = ij6;
31677 _ij6[1] = -1;
31678 for (int iij6 = ij6 + 1; iij6 < 1;
31679 ++iij6)
31680 {
31681 if (j6valid[iij6]
31682 && IKabs(
31683 cj6array[ij6]
31684 - cj6array[iij6])
31685 < IKFAST_SOLUTION_THRESH
31686 && IKabs(
31687 sj6array[ij6]
31688 - sj6array[iij6])
31689 < IKFAST_SOLUTION_THRESH)
31690 {
31691 j6valid[iij6] = false;
31692 _ij6[1] = iij6;
31693 break;
31694 }
31695 }
31696 j6 = j6array[ij6];
31697 cj6 = cj6array[ij6];
31698 sj6 = sj6array[ij6];
31699 {
31700 IkReal evalcond[5];
31701 IkReal x2161 = ((0.3) * cj9);
31702 IkReal x2162 = ((0.045) * sj9);
31703 IkReal x2163 = IKcos(j6);
31704 IkReal x2164 = (pz * x2163);
31705 IkReal x2165 = IKsin(j6);
31706 IkReal x2166 = (cj4 * px);
31707 IkReal x2167 = (x2165 * x2166);
31708 IkReal x2168 = (py * sj4);
31709 IkReal x2169 = (x2165 * x2168);
31710 IkReal x2170 = ((0.045) * cj9);
31711 IkReal x2171 = ((0.3) * sj9);
31712 IkReal x2172 = (pz * x2165);
31713 IkReal x2173 = (x2163 * x2166);
31714 IkReal x2174 = (x2163 * x2168);
31715 evalcond[0]
31716 = ((-0.55)
31717 + (((-1.0) * x2162))
31718 + x2169 + x2164 + x2167
31719 + (((-1.0) * x2161)));
31720 evalcond[1]
31721 = ((0.045) + x2173 + x2174
31722 + x2171
31723 + (((-1.0) * x2172))
31724 + (((-1.0) * x2170)));
31725 evalcond[2]
31726 = ((((1.32323529411765)
31727 * cj9 * x2165))
31728 + (((-0.108264705882353)
31729 * cj9 * x2163))
31730 + (((-3.92156862745098)
31731 * pp * x2165))
31732 + (((-0.588235294117647)
31733 * pp * x2163))
31734 + pz
31735 + (((1.51009803921569)
31736 * x2165))
31737 + (((-0.316735294117647)
31738 * x2163)));
31739 evalcond[3]
31740 = (((x2163 * x2170))
31741 + (((-1.0) * x2168))
31742 + ((x2161 * x2165))
31743 + (((-1.0) * x2163
31744 * x2171))
31745 + (((-0.045) * x2163))
31746 + ((x2162 * x2165))
31747 + (((-1.0) * x2166))
31748 + (((0.55) * x2165)));
31749 evalcond[4]
31750 = ((-0.2125)
31751 + (((-0.09) * x2174))
31752 + (((0.09) * x2172))
31753 + (((1.1) * x2164))
31754 + (((1.1) * x2167))
31755 + (((1.1) * x2169))
31756 + (((-1.0) * (1.0) * pp))
31757 + (((-0.09) * x2173)));
31758 if (IKabs(evalcond[0])
31759 > IKFAST_EVALCOND_THRESH
31760 || IKabs(evalcond[1])
31761 > IKFAST_EVALCOND_THRESH
31762 || IKabs(evalcond[2])
31763 > IKFAST_EVALCOND_THRESH
31764 || IKabs(evalcond[3])
31765 > IKFAST_EVALCOND_THRESH
31766 || IKabs(evalcond[4])
31767 > IKFAST_EVALCOND_THRESH)
31768 {
31769 continue;
31770 }
31771 }
31772
31773 rotationfunction0(solutions);
31774 }
31775 }
31776 }
31777 }
31778 }
31779 } while (0);
31780 if (bgotonextstatement)
31781 {
31782 bool bgotonextstatement = true;
31783 do
31784 {
31785 if (1)
31786 {
31787 bgotonextstatement = false;
31788 continue; // branch miss [j6]
31789 }
31790 } while (0);
31791 if (bgotonextstatement)
31792 {
31793 }
31794 }
31795 }
31796 }
31797 }
31798 else
31799 {
31800 {
31801 IkReal j6array[1], cj6array[1], sj6array[1];
31802 bool j6valid[1] = {false};
31803 _nj6 = 1;
31804 IkReal x2175 = (pz * sj8);
31805 IkReal x2176 = ((0.3) * cj9);
31806 IkReal x2177 = ((0.045) * sj9);
31807 IkReal x2178 = (cj4 * px);
31808 IkReal x2179 = ((0.045) * cj8 * sj8);
31809 IkReal x2180 = (x2178 * x2179);
31810 IkReal x2181 = (py * sj4);
31811 IkReal x2182 = (x2179 * x2181);
31812 IkReal x2183 = ((0.3) * cj8 * sj8 * sj9);
31813 IkReal x2184 = ((0.55) * cj8);
31814 IkReal x2185 = (cj4 * py);
31815 IkReal x2186 = (px * sj4);
31816 IkReal x2187 = (cj4 * cj8 * py);
31817 IkReal x2188 = ((1.0) * pz * sj8);
31818 IkReal x2189 = (cj8 * px * sj4);
31819 IkReal x2190 = cj8 * cj8;
31820 IkReal x2191 = ((0.045) * x2190);
31821 IkReal x2192 = (x2185 * x2191);
31822 IkReal x2193 = (x2186 * x2191);
31823 IkReal x2194 = ((0.3) * sj9 * x2190);
31824 CheckValue<IkReal> x2195
31825 = IKPowWithIntegerCheck(
31826 IKsign(
31827 ((((-1.0) * cj9 * x2182))
31828 + ((x2181 * x2183))
31829 + (((-0.55) * x2175))
31830 + (((-1.0) * x2175 * x2177))
31831 + (((-1.0) * x2175 * x2176))
31832 + (((-1.0) * cj9 * x2180))
31833 + ((x2178 * x2183)) + x2182
31834 + x2180)),
31835 -1);
31836 if (!x2195.valid)
31837 {
31838 continue;
31839 }
31840 CheckValue<IkReal> x2196 = IKatan2WithCheck(
31841 IkReal(
31842 ((((-1.0) * x2176 * x2189))
31843 + ((x2184 * x2185)) + ((x2176 * x2187))
31844 + (((-1.0) * x2178 * x2188))
31845 + ((x2177 * x2187))
31846 + (((-1.0) * x2181 * x2188))
31847 + (((-1.0) * x2184 * x2186))
31848 + (((-1.0) * x2177 * x2189)))),
31849 ((((-1.0) * (1.0) * sj8 * (pz * pz)))
31850 + (((-1.0) * x2193)) + x2192
31851 + (((-1.0) * x2186 * x2194))
31852 + (((-1.0) * cj9 * x2192))
31853 + ((x2185 * x2194)) + ((cj9 * x2193))),
31854 IKFAST_ATAN2_MAGTHRESH);
31855 if (!x2196.valid)
31856 {
31857 continue;
31858 }
31859 j6array[0]
31860 = ((-1.5707963267949)
31861 + (((1.5707963267949) * (x2195.value)))
31862 + (x2196.value));
31863 sj6array[0] = IKsin(j6array[0]);
31864 cj6array[0] = IKcos(j6array[0]);
31865 if (j6array[0] > IKPI)
31866 {
31867 j6array[0] -= IK2PI;
31868 }
31869 else if (j6array[0] < -IKPI)
31870 {
31871 j6array[0] += IK2PI;
31872 }
31873 j6valid[0] = true;
31874 for (int ij6 = 0; ij6 < 1; ++ij6)
31875 {
31876 if (!j6valid[ij6])
31877 {
31878 continue;
31879 }
31880 _ij6[0] = ij6;
31881 _ij6[1] = -1;
31882 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
31883 {
31884 if (j6valid[iij6]
31885 && IKabs(cj6array[ij6] - cj6array[iij6])
31886 < IKFAST_SOLUTION_THRESH
31887 && IKabs(sj6array[ij6] - sj6array[iij6])
31888 < IKFAST_SOLUTION_THRESH)
31889 {
31890 j6valid[iij6] = false;
31891 _ij6[1] = iij6;
31892 break;
31893 }
31894 }
31895 j6 = j6array[ij6];
31896 cj6 = cj6array[ij6];
31897 sj6 = sj6array[ij6];
31898 {
31899 IkReal evalcond[6];
31900 IkReal x2197 = ((0.3) * cj9);
31901 IkReal x2198 = ((0.045) * sj9);
31902 IkReal x2199 = IKcos(j6);
31903 IkReal x2200 = (pz * x2199);
31904 IkReal x2201 = IKsin(j6);
31905 IkReal x2202 = (cj4 * px * x2201);
31906 IkReal x2203 = (py * sj4);
31907 IkReal x2204 = (x2201 * x2203);
31908 IkReal x2205 = (px * sj4);
31909 IkReal x2206 = ((1.0) * cj4 * py);
31910 IkReal x2207 = (cj4 * sj8);
31911 IkReal x2208 = ((0.045) * cj8);
31912 IkReal x2209 = ((0.045) * cj9);
31913 IkReal x2210 = (cj8 * x2201);
31914 IkReal x2211 = ((0.3) * sj9);
31915 IkReal x2212 = (sj8 * x2205);
31916 IkReal x2213 = (pz * x2210);
31917 IkReal x2214 = (px * (((1.0) * cj4)));
31918 IkReal x2215 = (cj8 * x2199);
31919 IkReal x2216 = ((1.0) * x2203);
31920 IkReal x2217 = ((0.09) * cj8 * x2199);
31921 evalcond[0]
31922 = ((-0.55) + x2204 + x2202 + x2200
31923 + (((-1.0) * x2198))
31924 + (((-1.0) * x2197)));
31925 evalcond[1]
31926 = (((cj8 * x2205))
31927 + (((-1.0) * pz * sj8 * x2201))
31928 + (((-1.0) * cj8 * x2206))
31929 + ((sj8 * x2199 * x2203))
31930 + ((px * x2199 * x2207)));
31931 evalcond[2]
31932 = ((((-0.55) * x2199))
31933 + (((-1.0) * x2197 * x2199))
31934 + ((x2201 * x2208)) + pz
31935 + (((-1.0) * x2209 * x2210))
31936 + (((-1.0) * x2198 * x2199))
31937 + ((x2210 * x2211)));
31938 evalcond[3]
31939 = ((0.045) + (((-1.0) * x2215 * x2216))
31940 + (((-1.0) * x2209)) + x2211 + x2213
31941 + x2212 + (((-1.0) * x2214 * x2215))
31942 + (((-1.0) * sj8 * x2206)));
31943 evalcond[4]
31944 = (((x2211 * x2215)) + ((x2198 * x2201))
31945 + (((-1.0) * x2214))
31946 + (((-1.0) * x2209 * x2215))
31947 + (((0.55) * x2201))
31948 + ((x2199 * x2208))
31949 + (((-1.0) * x2216))
31950 + ((x2197 * x2201)));
31951 evalcond[5]
31952 = ((-0.2125) + (((1.1) * x2204))
31953 + (((1.1) * x2202))
31954 + (((0.09) * py * x2207))
31955 + (((-0.09) * x2213))
31956 + (((-0.09) * x2212))
31957 + ((x2203 * x2217))
31958 + (((-1.0) * (1.0) * pp))
31959 + ((cj4 * px * x2217))
31960 + (((1.1) * x2200)));
31961 if (IKabs(evalcond[0])
31962 > IKFAST_EVALCOND_THRESH
31963 || IKabs(evalcond[1])
31964 > IKFAST_EVALCOND_THRESH
31965 || IKabs(evalcond[2])
31966 > IKFAST_EVALCOND_THRESH
31967 || IKabs(evalcond[3])
31968 > IKFAST_EVALCOND_THRESH
31969 || IKabs(evalcond[4])
31970 > IKFAST_EVALCOND_THRESH
31971 || IKabs(evalcond[5])
31972 > IKFAST_EVALCOND_THRESH)
31973 {
31974 continue;
31975 }
31976 }
31977
31978 rotationfunction0(solutions);
31979 }
31980 }
31981 }
31982 }
31983 }
31984 else
31985 {
31986 {
31987 IkReal j6array[1], cj6array[1], sj6array[1];
31988 bool j6valid[1] = {false};
31989 _nj6 = 1;
31990 IkReal x2218 = py * py;
31991 IkReal x2219 = (sj8 * x2218);
31992 IkReal x2220 = (cj4 * px * sj8);
31993 IkReal x2221 = cj4 * cj4;
31994 IkReal x2222 = px * px;
31995 IkReal x2223 = ((0.55) * sj8);
31996 IkReal x2224 = (cj8 * px);
31997 IkReal x2225 = ((0.3) * cj9);
31998 IkReal x2226 = ((0.045) * sj9);
31999 IkReal x2227 = (py * sj4 * sj8);
32000 IkReal x2228 = (pz * sj8);
32001 IkReal x2229 = (cj4 * cj8 * sj4);
32002 CheckValue<IkReal> x2230 = IKatan2WithCheck(
32003 IkReal(
32004 (((py * sj4 * x2223)) + ((x2220 * x2225))
32005 + ((pz * sj4 * x2224))
32006 + (((-1.0) * cj4 * cj8 * py * pz))
32007 + ((cj4 * px * x2223)) + ((x2226 * x2227))
32008 + ((x2225 * x2227)) + ((x2220 * x2226)))),
32009 ((((2.0) * py * x2221 * x2224))
32010 + ((x2225 * x2228)) + ((x2218 * x2229))
32011 + ((x2226 * x2228)) + ((pz * x2223))
32012 + (((-1.0) * x2222 * x2229))
32013 + (((-1.0) * py * x2224))),
32014 IKFAST_ATAN2_MAGTHRESH);
32015 if (!x2230.valid)
32016 {
32017 continue;
32018 }
32019 CheckValue<IkReal> x2231 = IKPowWithIntegerCheck(
32020 IKsign(
32021 ((((-1.0) * x2219 * x2221))
32022 + (((2.0) * py * sj4 * x2220)) + x2219
32023 + ((sj8 * (pz * pz)))
32024 + ((sj8 * x2221 * x2222)))),
32025 -1);
32026 if (!x2231.valid)
32027 {
32028 continue;
32029 }
32030 j6array[0]
32031 = ((-1.5707963267949) + (x2230.value)
32032 + (((1.5707963267949) * (x2231.value))));
32033 sj6array[0] = IKsin(j6array[0]);
32034 cj6array[0] = IKcos(j6array[0]);
32035 if (j6array[0] > IKPI)
32036 {
32037 j6array[0] -= IK2PI;
32038 }
32039 else if (j6array[0] < -IKPI)
32040 {
32041 j6array[0] += IK2PI;
32042 }
32043 j6valid[0] = true;
32044 for (int ij6 = 0; ij6 < 1; ++ij6)
32045 {
32046 if (!j6valid[ij6])
32047 {
32048 continue;
32049 }
32050 _ij6[0] = ij6;
32051 _ij6[1] = -1;
32052 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
32053 {
32054 if (j6valid[iij6]
32055 && IKabs(cj6array[ij6] - cj6array[iij6])
32056 < IKFAST_SOLUTION_THRESH
32057 && IKabs(sj6array[ij6] - sj6array[iij6])
32058 < IKFAST_SOLUTION_THRESH)
32059 {
32060 j6valid[iij6] = false;
32061 _ij6[1] = iij6;
32062 break;
32063 }
32064 }
32065 j6 = j6array[ij6];
32066 cj6 = cj6array[ij6];
32067 sj6 = sj6array[ij6];
32068 {
32069 IkReal evalcond[6];
32070 IkReal x2232 = ((0.3) * cj9);
32071 IkReal x2233 = ((0.045) * sj9);
32072 IkReal x2234 = IKcos(j6);
32073 IkReal x2235 = (pz * x2234);
32074 IkReal x2236 = IKsin(j6);
32075 IkReal x2237 = (cj4 * px * x2236);
32076 IkReal x2238 = (py * sj4);
32077 IkReal x2239 = (x2236 * x2238);
32078 IkReal x2240 = (px * sj4);
32079 IkReal x2241 = ((1.0) * cj4 * py);
32080 IkReal x2242 = (cj4 * sj8);
32081 IkReal x2243 = ((0.045) * cj8);
32082 IkReal x2244 = ((0.045) * cj9);
32083 IkReal x2245 = (cj8 * x2236);
32084 IkReal x2246 = ((0.3) * sj9);
32085 IkReal x2247 = (sj8 * x2240);
32086 IkReal x2248 = (pz * x2245);
32087 IkReal x2249 = (px * (((1.0) * cj4)));
32088 IkReal x2250 = (cj8 * x2234);
32089 IkReal x2251 = ((1.0) * x2238);
32090 IkReal x2252 = ((0.09) * cj8 * x2234);
32091 evalcond[0]
32092 = ((-0.55) + x2237 + x2235 + x2239
32093 + (((-1.0) * x2232))
32094 + (((-1.0) * x2233)));
32095 evalcond[1]
32096 = (((px * x2234 * x2242))
32097 + ((sj8 * x2234 * x2238))
32098 + (((-1.0) * cj8 * x2241))
32099 + ((cj8 * x2240))
32100 + (((-1.0) * pz * sj8 * x2236)));
32101 evalcond[2]
32102 = ((((-1.0) * x2244 * x2245))
32103 + ((x2236 * x2243)) + pz
32104 + (((-1.0) * x2233 * x2234))
32105 + (((-1.0) * x2232 * x2234))
32106 + (((-0.55) * x2234))
32107 + ((x2245 * x2246)));
32108 evalcond[3]
32109 = ((0.045) + (((-1.0) * x2244))
32110 + (((-1.0) * x2249 * x2250))
32111 + (((-1.0) * sj8 * x2241)) + x2247
32112 + x2246 + x2248
32113 + (((-1.0) * x2250 * x2251)));
32114 evalcond[4]
32115 = ((((0.55) * x2236)) + (((-1.0) * x2249))
32116 + ((x2233 * x2236))
32117 + (((-1.0) * x2244 * x2250))
32118 + ((x2246 * x2250)) + (((-1.0) * x2251))
32119 + ((x2232 * x2236)) + ((x2234 * x2243)));
32120 evalcond[5]
32121 = ((-0.2125) + (((-0.09) * x2247))
32122 + (((-0.09) * x2248)) + ((x2238 * x2252))
32123 + (((1.1) * x2239)) + (((1.1) * x2237))
32124 + (((1.1) * x2235))
32125 + ((cj4 * px * x2252))
32126 + (((-1.0) * (1.0) * pp))
32127 + (((0.09) * py * x2242)));
32128 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
32129 || IKabs(evalcond[1])
32130 > IKFAST_EVALCOND_THRESH
32131 || IKabs(evalcond[2])
32132 > IKFAST_EVALCOND_THRESH
32133 || IKabs(evalcond[3])
32134 > IKFAST_EVALCOND_THRESH
32135 || IKabs(evalcond[4])
32136 > IKFAST_EVALCOND_THRESH
32137 || IKabs(evalcond[5])
32138 > IKFAST_EVALCOND_THRESH)
32139 {
32140 continue;
32141 }
32142 }
32143
32144 rotationfunction0(solutions);
32145 }
32146 }
32147 }
32148 }
32149 }
32150 else
32151 {
32152 {
32153 IkReal j6array[1], cj6array[1], sj6array[1];
32154 bool j6valid[1] = {false};
32155 _nj6 = 1;
32156 IkReal x2253 = (cj4 * px);
32157 IkReal x2254 = ((0.045) * pz);
32158 IkReal x2255 = (py * sj4);
32159 IkReal x2256 = ((0.3) * cj9);
32160 IkReal x2257 = ((0.045) * sj9);
32161 IkReal x2258 = (cj8 * cj9);
32162 IkReal x2259 = (cj8 * sj9);
32163 IkReal x2260 = cj9 * cj9;
32164 IkReal x2261 = ((1.0) * pz);
32165 CheckValue<IkReal> x2262 = IKatan2WithCheck(
32166 IkReal(
32167 ((-0.304525) + (((-1.0) * (0.027) * cj9 * sj9))
32168 + (((-1.0) * (0.0495) * sj9)) + (pz * pz)
32169 + (((-0.087975) * x2260))
32170 + (((-1.0) * (0.33) * cj9)))),
32171 ((((-1.0) * x2255 * x2261))
32172 + (((0.027) * cj8 * x2260)) + (((0.01125) * x2258))
32173 + (((-1.0) * x2253 * x2261))
32174 + (((-1.0) * (0.03825) * cj8))
32175 + (((-0.087975) * sj9 * x2258))
32176 + (((-0.167025) * x2259))),
32177 IKFAST_ATAN2_MAGTHRESH);
32178 if (!x2262.valid)
32179 {
32180 continue;
32181 }
32182 CheckValue<IkReal> x2263 = IKPowWithIntegerCheck(
32183 IKsign((
32184 (((-1.0) * x2255 * x2257))
32185 + (((-1.0) * x2255 * x2256)) + ((x2254 * x2258))
32186 + (((-1.0) * x2253 * x2257))
32187 + (((-0.55) * x2255)) + (((-0.3) * pz * x2259))
32188 + (((-1.0) * x2253 * x2256))
32189 + (((-0.55) * x2253))
32190 + (((-1.0) * cj8 * x2254)))),
32191 -1);
32192 if (!x2263.valid)
32193 {
32194 continue;
32195 }
32196 j6array[0]
32197 = ((-1.5707963267949) + (x2262.value)
32198 + (((1.5707963267949) * (x2263.value))));
32199 sj6array[0] = IKsin(j6array[0]);
32200 cj6array[0] = IKcos(j6array[0]);
32201 if (j6array[0] > IKPI)
32202 {
32203 j6array[0] -= IK2PI;
32204 }
32205 else if (j6array[0] < -IKPI)
32206 {
32207 j6array[0] += IK2PI;
32208 }
32209 j6valid[0] = true;
32210 for (int ij6 = 0; ij6 < 1; ++ij6)
32211 {
32212 if (!j6valid[ij6])
32213 {
32214 continue;
32215 }
32216 _ij6[0] = ij6;
32217 _ij6[1] = -1;
32218 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
32219 {
32220 if (j6valid[iij6]
32221 && IKabs(cj6array[ij6] - cj6array[iij6])
32222 < IKFAST_SOLUTION_THRESH
32223 && IKabs(sj6array[ij6] - sj6array[iij6])
32224 < IKFAST_SOLUTION_THRESH)
32225 {
32226 j6valid[iij6] = false;
32227 _ij6[1] = iij6;
32228 break;
32229 }
32230 }
32231 j6 = j6array[ij6];
32232 cj6 = cj6array[ij6];
32233 sj6 = sj6array[ij6];
32234 {
32235 IkReal evalcond[6];
32236 IkReal x2264 = ((0.3) * cj9);
32237 IkReal x2265 = ((0.045) * sj9);
32238 IkReal x2266 = IKcos(j6);
32239 IkReal x2267 = (pz * x2266);
32240 IkReal x2268 = IKsin(j6);
32241 IkReal x2269 = (cj4 * px * x2268);
32242 IkReal x2270 = (py * sj4);
32243 IkReal x2271 = (x2268 * x2270);
32244 IkReal x2272 = (px * sj4);
32245 IkReal x2273 = ((1.0) * cj4 * py);
32246 IkReal x2274 = (cj4 * sj8);
32247 IkReal x2275 = ((0.045) * cj8);
32248 IkReal x2276 = ((0.045) * cj9);
32249 IkReal x2277 = (cj8 * x2268);
32250 IkReal x2278 = ((0.3) * sj9);
32251 IkReal x2279 = (sj8 * x2272);
32252 IkReal x2280 = (pz * x2277);
32253 IkReal x2281 = (px * (((1.0) * cj4)));
32254 IkReal x2282 = (cj8 * x2266);
32255 IkReal x2283 = ((1.0) * x2270);
32256 IkReal x2284 = ((0.09) * cj8 * x2266);
32257 evalcond[0]
32258 = ((-0.55) + x2267 + x2269 + (((-1.0) * x2265))
32259 + x2271 + (((-1.0) * x2264)));
32260 evalcond[1]
32261 = (((cj8 * x2272)) + (((-1.0) * cj8 * x2273))
32262 + (((-1.0) * pz * sj8 * x2268))
32263 + ((sj8 * x2266 * x2270))
32264 + ((px * x2266 * x2274)));
32265 evalcond[2]
32266 = ((((-1.0) * x2264 * x2266))
32267 + ((x2268 * x2275)) + pz
32268 + (((-1.0) * x2265 * x2266))
32269 + ((x2277 * x2278)) + (((-0.55) * x2266))
32270 + (((-1.0) * x2276 * x2277)));
32271 evalcond[3]
32272 = ((0.045) + (((-1.0) * x2282 * x2283)) + x2280
32273 + (((-1.0) * sj8 * x2273))
32274 + (((-1.0) * x2281 * x2282))
32275 + (((-1.0) * x2276)) + x2278 + x2279);
32276 evalcond[4]
32277 = ((((-1.0) * x2281)) + ((x2266 * x2275))
32278 + (((-1.0) * x2276 * x2282))
32279 + ((x2264 * x2268)) + ((x2278 * x2282))
32280 + ((x2265 * x2268)) + (((-1.0) * x2283))
32281 + (((0.55) * x2268)));
32282 evalcond[5]
32283 = ((-0.2125) + (((-0.09) * x2279))
32284 + (((-0.09) * x2280)) + ((cj4 * px * x2284))
32285 + (((0.09) * py * x2274)) + (((1.1) * x2267))
32286 + ((x2270 * x2284)) + (((-1.0) * (1.0) * pp))
32287 + (((1.1) * x2269)) + (((1.1) * x2271)));
32288 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
32289 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
32290 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
32291 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
32292 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
32293 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH)
32294 {
32295 continue;
32296 }
32297 }
32298
32299 rotationfunction0(solutions);
32300 }
32301 }
32302 }
32303 }
32304 }
32305 }
32306 }
32307 }
32308 }
32309 }
32310 }
32311 return solutions.GetNumSolutions() > 0;
32312 }
32313 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions)
32314 {
32315 for (int rotationiter = 0; rotationiter < 1; ++rotationiter)
32316 {
32317 IkReal x158 = ((1.0) * sj9);
32318 IkReal x159 = (cj9 * sj6);
32319 IkReal x160 = ((((-1.0) * cj6 * x158)) + (((-1.0) * cj8 * x159)));
32320 IkReal x161 = (cj4 * sj8);
32321 IkReal x162 = (sj6 * x158);
32322 IkReal x163 = (cj6 * cj9);
32323 IkReal x164 = (((cj8 * x163)) + (((-1.0) * x162)));
32324 IkReal x165 = (((cj9 * x161)) + ((sj4 * x164)));
32325 IkReal x166 = ((1.0) * sj4 * sj8);
32326 IkReal x167 = ((((-1.0) * cj9 * x166)) + ((cj4 * x164)));
32327 IkReal x168 = (sj6 * sj8);
32328 IkReal x169 = (((cj4 * cj8)) + (((-1.0) * cj6 * x166)));
32329 IkReal x170 = ((((-1.0) * cj6 * x161)) + (((-1.0) * cj8 * sj4)));
32330 IkReal x171 = ((((-1.0) * cj8 * x162)) + x163);
32331 IkReal x172 = (x159 + ((cj6 * cj8 * sj9)));
32332 IkReal x173 = (((sj4 * x172)) + ((sj9 * x161)));
32333 IkReal x174 = (((cj4 * x172)) + (((-1.0) * sj4 * sj8 * x158)));
32334 new_r00 = (((r00 * x167)) + ((r20 * x160)) + ((r10 * x165)));
32335 new_r01 = (((r11 * x165)) + ((r21 * x160)) + ((r01 * x167)));
32336 new_r02 = (((r22 * x160)) + ((r12 * x165)) + ((r02 * x167)));
32337 new_r10 = (((r20 * x168)) + ((r10 * x169)) + ((r00 * x170)));
32338 new_r11 = (((r01 * x170)) + ((r21 * x168)) + ((r11 * x169)));
32339 new_r12 = (((r22 * x168)) + ((r02 * x170)) + ((r12 * x169)));
32340 new_r20 = (((r20 * x171)) + ((r10 * x173)) + ((r00 * x174)));
32341 new_r21 = (((r01 * x174)) + ((r11 * x173)) + ((r21 * x171)));
32342 new_r22 = (((r12 * x173)) + ((r22 * x171)) + ((r02 * x174)));
32343 {
32344 IkReal j11array[2], cj11array[2], sj11array[2];
32345 bool j11valid[2] = {false};
32346 _nj11 = 2;
32347 cj11array[0] = new_r22;
32348 if (cj11array[0] >= -1 - IKFAST_SINCOS_THRESH
32349 && cj11array[0] <= 1 + IKFAST_SINCOS_THRESH)
32350 {
32351 j11valid[0] = j11valid[1] = true;
32352 j11array[0] = IKacos(cj11array[0]);
32353 sj11array[0] = IKsin(j11array[0]);
32354 cj11array[1] = cj11array[0];
32355 j11array[1] = -j11array[0];
32356 sj11array[1] = -sj11array[0];
32357 }
32358 else if (std::isnan(cj11array[0]))
32359 {
32360 // probably any value will work
32361 j11valid[0] = true;
32362 cj11array[0] = 1;
32363 sj11array[0] = 0;
32364 j11array[0] = 0;
32365 }
32366 for (int ij11 = 0; ij11 < 2; ++ij11)
32367 {
32368 if (!j11valid[ij11])
32369 {
32370 continue;
32371 }
32372 _ij11[0] = ij11;
32373 _ij11[1] = -1;
32374 for (int iij11 = ij11 + 1; iij11 < 2; ++iij11)
32375 {
32376 if (j11valid[iij11]
32377 && IKabs(cj11array[ij11] - cj11array[iij11])
32378 < IKFAST_SOLUTION_THRESH
32379 && IKabs(sj11array[ij11] - sj11array[iij11])
32380 < IKFAST_SOLUTION_THRESH)
32381 {
32382 j11valid[iij11] = false;
32383 _ij11[1] = iij11;
32384 break;
32385 }
32386 }
32387 j11 = j11array[ij11];
32388 cj11 = cj11array[ij11];
32389 sj11 = sj11array[ij11];
32390
32391 {
32392 IkReal j10eval[2];
32393 IkReal x175 = ((1.0) * sj9);
32394 IkReal x176 = (cj9 * sj6);
32395 IkReal x177 = x160;
32396 IkReal x178 = (cj4 * sj8);
32397 IkReal x179 = (sj6 * x175);
32398 IkReal x180 = (cj6 * cj9);
32399 IkReal x181 = (((cj8 * x180)) + (((-1.0) * x179)));
32400 IkReal x182 = (((sj4 * x181)) + ((cj9 * x178)));
32401 IkReal x183 = ((1.0) * sj4 * sj8);
32402 IkReal x184 = (((cj4 * x181)) + (((-1.0) * cj9 * x183)));
32403 IkReal x185 = (sj6 * sj8);
32404 IkReal x186 = x169;
32405 IkReal x187 = x170;
32406 IkReal x188 = ((((-1.0) * cj8 * x179)) + x180);
32407 IkReal x189 = x172;
32408 IkReal x190 = (((sj9 * x178)) + ((sj4 * x189)));
32409 IkReal x191 = ((((-1.0) * sj4 * sj8 * x175)) + ((cj4 * x189)));
32410 new_r00 = (((r20 * x177)) + ((r00 * x184)) + ((r10 * x182)));
32411 new_r01 = (((r21 * x177)) + ((r11 * x182)) + ((r01 * x184)));
32412 new_r02 = (((r22 * x177)) + ((r12 * x182)) + ((r02 * x184)));
32413 new_r10 = (((r10 * x186)) + ((r20 * x185)) + ((r00 * x187)));
32414 new_r11 = (((r21 * x185)) + ((r11 * x186)) + ((r01 * x187)));
32415 new_r12 = (((r12 * x186)) + ((r22 * x185)) + ((r02 * x187)));
32416 new_r20 = (((r00 * x191)) + ((r20 * x188)) + ((r10 * x190)));
32417 new_r21 = (((r21 * x188)) + ((r11 * x190)) + ((r01 * x191)));
32418 new_r22 = (((r12 * x190)) + ((r22 * x188)) + ((r02 * x191)));
32419 j10eval[0] = sj11;
32420 j10eval[1] = IKsign(sj11);
32421 if (IKabs(j10eval[0]) < 0.0000010000000000
32422 || IKabs(j10eval[1]) < 0.0000010000000000)
32423 {
32424 {
32425 IkReal j10eval[1];
32426 IkReal x192 = ((1.0) * sj9);
32427 IkReal x193 = (cj9 * sj6);
32428 IkReal x194 = x160;
32429 IkReal x195 = (cj4 * sj8);
32430 IkReal x196 = (sj6 * x192);
32431 IkReal x197 = (cj6 * cj9);
32432 IkReal x198 = (((cj8 * x197)) + (((-1.0) * x196)));
32433 IkReal x199 = (((cj9 * x195)) + ((sj4 * x198)));
32434 IkReal x200 = ((1.0) * sj4 * sj8);
32435 IkReal x201 = ((((-1.0) * cj9 * x200)) + ((cj4 * x198)));
32436 IkReal x202 = (sj6 * sj8);
32437 IkReal x203 = x169;
32438 IkReal x204 = x170;
32439 IkReal x205 = ((((-1.0) * cj8 * x196)) + x197);
32440 IkReal x206 = x172;
32441 IkReal x207 = (((sj9 * x195)) + ((sj4 * x206)));
32442 IkReal x208 = (((cj4 * x206)) + (((-1.0) * sj4 * sj8 * x192)));
32443 new_r00 = (((r20 * x194)) + ((r00 * x201)) + ((r10 * x199)));
32444 new_r01 = (((r11 * x199)) + ((r01 * x201)) + ((r21 * x194)));
32445 new_r02 = (((r02 * x201)) + ((r22 * x194)) + ((r12 * x199)));
32446 new_r10 = (((r00 * x204)) + ((r10 * x203)) + ((r20 * x202)));
32447 new_r11 = (((r21 * x202)) + ((r11 * x203)) + ((r01 * x204)));
32448 new_r12 = (((r12 * x203)) + ((r22 * x202)) + ((r02 * x204)));
32449 new_r20 = (((r20 * x205)) + ((r10 * x207)) + ((r00 * x208)));
32450 new_r21 = (((r11 * x207)) + ((r01 * x208)) + ((r21 * x205)));
32451 new_r22 = (((r12 * x207)) + ((r02 * x208)) + ((r22 * x205)));
32452 j10eval[0] = sj11;
32453 if (IKabs(j10eval[0]) < 0.0000010000000000)
32454 {
32455 {
32456 IkReal evalcond[6];
32457 bool bgotonextstatement = true;
32458 do
32459 {
32460 evalcond[0]
32461 = ((-3.14159265358979)
32462 + (IKfmod(
32463 ((3.14159265358979) + (IKabs(j11))),
32464 6.28318530717959)));
32465 evalcond[1] = ((-1.0) + new_r22);
32466 evalcond[2] = new_r20;
32467 evalcond[3] = new_r02;
32468 evalcond[4] = new_r12;
32469 evalcond[5] = new_r21;
32470 if (IKabs(evalcond[0]) < 0.0000010000000000
32471 && IKabs(evalcond[1]) < 0.0000010000000000
32472 && IKabs(evalcond[2]) < 0.0000010000000000
32473 && IKabs(evalcond[3]) < 0.0000010000000000
32474 && IKabs(evalcond[4]) < 0.0000010000000000
32475 && IKabs(evalcond[5]) < 0.0000010000000000)
32476 {
32477 bgotonextstatement = false;
32478 {
32479 IkReal j10array[2], cj10array[2], sj10array[2];
32480 bool j10valid[2] = {false};
32481 _nj10 = 2;
32482 CheckValue<IkReal> x210 = IKatan2WithCheck(
32483 IkReal(new_r02), new_r12, IKFAST_ATAN2_MAGTHRESH);
32484 if (!x210.valid)
32485 {
32486 continue;
32487 }
32488 IkReal x209 = ((-1.0) * (((1.0) * (x210.value))));
32489 j10array[0] = x209;
32490 sj10array[0] = IKsin(j10array[0]);
32491 cj10array[0] = IKcos(j10array[0]);
32492 j10array[1] = ((3.14159265358979) + x209);
32493 sj10array[1] = IKsin(j10array[1]);
32494 cj10array[1] = IKcos(j10array[1]);
32495 if (j10array[0] > IKPI)
32496 {
32497 j10array[0] -= IK2PI;
32498 }
32499 else if (j10array[0] < -IKPI)
32500 {
32501 j10array[0] += IK2PI;
32502 }
32503 j10valid[0] = true;
32504 if (j10array[1] > IKPI)
32505 {
32506 j10array[1] -= IK2PI;
32507 }
32508 else if (j10array[1] < -IKPI)
32509 {
32510 j10array[1] += IK2PI;
32511 }
32512 j10valid[1] = true;
32513 for (int ij10 = 0; ij10 < 2; ++ij10)
32514 {
32515 if (!j10valid[ij10])
32516 {
32517 continue;
32518 }
32519 _ij10[0] = ij10;
32520 _ij10[1] = -1;
32521 for (int iij10 = ij10 + 1; iij10 < 2; ++iij10)
32522 {
32523 if (j10valid[iij10]
32524 && IKabs(cj10array[ij10] - cj10array[iij10])
32525 < IKFAST_SOLUTION_THRESH
32526 && IKabs(sj10array[ij10] - sj10array[iij10])
32527 < IKFAST_SOLUTION_THRESH)
32528 {
32529 j10valid[iij10] = false;
32530 _ij10[1] = iij10;
32531 break;
32532 }
32533 }
32534 j10 = j10array[ij10];
32535 cj10 = cj10array[ij10];
32536 sj10 = sj10array[ij10];
32537 {
32538 IkReal evalcond[1];
32539 evalcond[0]
32540 = (((new_r12 * (IKcos(j10))))
32541 + (((-1.0) * (1.0) * new_r02
32542 * (IKsin(j10)))));
32543 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
32544 {
32545 continue;
32546 }
32547 }
32548
32549 {
32550 IkReal j12array[1], cj12array[1], sj12array[1];
32551 bool j12valid[1] = {false};
32552 _nj12 = 1;
32553 IkReal x211 = ((1.0) * new_r01);
32554 if (IKabs(
32555 ((((-1.0) * cj10 * x211))
32556 + (((-1.0) * (1.0) * new_r00 * sj10))))
32557 < IKFAST_ATAN2_MAGTHRESH
32558 && IKabs(
32559 ((((-1.0) * sj10 * x211))
32560 + ((cj10 * new_r00))))
32561 < IKFAST_ATAN2_MAGTHRESH
32562 && IKabs(
32563 IKsqr(
32564 ((((-1.0) * cj10 * x211))
32565 + (((-1.0) * (1.0) * new_r00
32566 * sj10))))
32567 + IKsqr(
32568 ((((-1.0) * sj10 * x211))
32569 + ((cj10 * new_r00))))
32570 - 1)
32571 <= IKFAST_SINCOS_THRESH)
32572 continue;
32573 j12array[0] = IKatan2(
32574 ((((-1.0) * cj10 * x211))
32575 + (((-1.0) * (1.0) * new_r00 * sj10))),
32576 ((((-1.0) * sj10 * x211))
32577 + ((cj10 * new_r00))));
32578 sj12array[0] = IKsin(j12array[0]);
32579 cj12array[0] = IKcos(j12array[0]);
32580 if (j12array[0] > IKPI)
32581 {
32582 j12array[0] -= IK2PI;
32583 }
32584 else if (j12array[0] < -IKPI)
32585 {
32586 j12array[0] += IK2PI;
32587 }
32588 j12valid[0] = true;
32589 for (int ij12 = 0; ij12 < 1; ++ij12)
32590 {
32591 if (!j12valid[ij12])
32592 {
32593 continue;
32594 }
32595 _ij12[0] = ij12;
32596 _ij12[1] = -1;
32597 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
32598 {
32599 if (j12valid[iij12]
32600 && IKabs(
32601 cj12array[ij12] - cj12array[iij12])
32602 < IKFAST_SOLUTION_THRESH
32603 && IKabs(
32604 sj12array[ij12] - sj12array[iij12])
32605 < IKFAST_SOLUTION_THRESH)
32606 {
32607 j12valid[iij12] = false;
32608 _ij12[1] = iij12;
32609 break;
32610 }
32611 }
32612 j12 = j12array[ij12];
32613 cj12 = cj12array[ij12];
32614 sj12 = sj12array[ij12];
32615 {
32616 IkReal evalcond[8];
32617 IkReal x212 = IKsin(j12);
32618 IkReal x213 = (cj10 * x212);
32619 IkReal x214 = IKcos(j12);
32620 IkReal x215 = ((1.0) * x214);
32621 IkReal x216 = ((-1.0) * x215);
32622 IkReal x217 = ((1.0) * sj10);
32623 IkReal x218
32624 = ((((-1.0) * cj10 * x215))
32625 + ((sj10 * x212)));
32626 evalcond[0]
32627 = (((cj10 * new_r01)) + x212
32628 + ((new_r11 * sj10)));
32629 evalcond[1]
32630 = (((sj10 * x214)) + x213 + new_r01);
32631 evalcond[2]
32632 = (((cj10 * new_r00)) + ((new_r10 * sj10))
32633 + x216);
32634 evalcond[3]
32635 = ((((-1.0) * new_r00 * x217))
32636 + (((-1.0) * x212))
32637 + ((cj10 * new_r10)));
32638 evalcond[4]
32639 = (((cj10 * new_r11)) + x216
32640 + (((-1.0) * new_r01 * x217)));
32641 evalcond[5] = (x218 + new_r00);
32642 evalcond[6] = (x218 + new_r11);
32643 evalcond[7]
32644 = ((((-1.0) * x213))
32645 + (((-1.0) * x214 * x217)) + new_r10);
32646 if (IKabs(evalcond[0])
32647 > IKFAST_EVALCOND_THRESH
32648 || IKabs(evalcond[1])
32649 > IKFAST_EVALCOND_THRESH
32650 || IKabs(evalcond[2])
32651 > IKFAST_EVALCOND_THRESH
32652 || IKabs(evalcond[3])
32653 > IKFAST_EVALCOND_THRESH
32654 || IKabs(evalcond[4])
32655 > IKFAST_EVALCOND_THRESH
32656 || IKabs(evalcond[5])
32657 > IKFAST_EVALCOND_THRESH
32658 || IKabs(evalcond[6])
32659 > IKFAST_EVALCOND_THRESH
32660 || IKabs(evalcond[7])
32661 > IKFAST_EVALCOND_THRESH)
32662 {
32663 continue;
32664 }
32665 }
32666
32667 {
32668 std::vector<IkSingleDOFSolutionBase<IkReal> >
32669 vinfos(7);
32670 vinfos[0].jointtype = 1;
32671 vinfos[0].foffset = j4;
32672 vinfos[0].indices[0] = _ij4[0];
32673 vinfos[0].indices[1] = _ij4[1];
32674 vinfos[0].maxsolutions = _nj4;
32675 vinfos[1].jointtype = 1;
32676 vinfos[1].foffset = j6;
32677 vinfos[1].indices[0] = _ij6[0];
32678 vinfos[1].indices[1] = _ij6[1];
32679 vinfos[1].maxsolutions = _nj6;
32680 vinfos[2].jointtype = 1;
32681 vinfos[2].foffset = j8;
32682 vinfos[2].indices[0] = _ij8[0];
32683 vinfos[2].indices[1] = _ij8[1];
32684 vinfos[2].maxsolutions = _nj8;
32685 vinfos[3].jointtype = 1;
32686 vinfos[3].foffset = j9;
32687 vinfos[3].indices[0] = _ij9[0];
32688 vinfos[3].indices[1] = _ij9[1];
32689 vinfos[3].maxsolutions = _nj9;
32690 vinfos[4].jointtype = 1;
32691 vinfos[4].foffset = j10;
32692 vinfos[4].indices[0] = _ij10[0];
32693 vinfos[4].indices[1] = _ij10[1];
32694 vinfos[4].maxsolutions = _nj10;
32695 vinfos[5].jointtype = 1;
32696 vinfos[5].foffset = j11;
32697 vinfos[5].indices[0] = _ij11[0];
32698 vinfos[5].indices[1] = _ij11[1];
32699 vinfos[5].maxsolutions = _nj11;
32700 vinfos[6].jointtype = 1;
32701 vinfos[6].foffset = j12;
32702 vinfos[6].indices[0] = _ij12[0];
32703 vinfos[6].indices[1] = _ij12[1];
32704 vinfos[6].maxsolutions = _nj12;
32705 std::vector<int> vfree(0);
32706 solutions.AddSolution(vinfos, vfree);
32707 }
32708 }
32709 }
32710 }
32711 }
32712 }
32713 } while (0);
32714 if (bgotonextstatement)
32715 {
32716 bool bgotonextstatement = true;
32717 do
32718 {
32719 evalcond[0]
32720 = ((-3.14159265358979)
32721 + (IKfmod(
32722 ((3.14159265358979)
32723 + (IKabs(((-3.14159265358979) + j11)))),
32724 6.28318530717959)));
32725 evalcond[1] = ((1.0) + new_r22);
32726 evalcond[2] = new_r20;
32727 evalcond[3] = new_r02;
32728 evalcond[4] = new_r12;
32729 evalcond[5] = new_r21;
32730 if (IKabs(evalcond[0]) < 0.0000010000000000
32731 && IKabs(evalcond[1]) < 0.0000010000000000
32732 && IKabs(evalcond[2]) < 0.0000010000000000
32733 && IKabs(evalcond[3]) < 0.0000010000000000
32734 && IKabs(evalcond[4]) < 0.0000010000000000
32735 && IKabs(evalcond[5]) < 0.0000010000000000)
32736 {
32737 bgotonextstatement = false;
32738 {
32739 IkReal j10array[2], cj10array[2], sj10array[2];
32740 bool j10valid[2] = {false};
32741 _nj10 = 2;
32742 CheckValue<IkReal> x220 = IKatan2WithCheck(
32743 IkReal(new_r02),
32744 new_r12,
32745 IKFAST_ATAN2_MAGTHRESH);
32746 if (!x220.valid)
32747 {
32748 continue;
32749 }
32750 IkReal x219 = ((-1.0) * (((1.0) * (x220.value))));
32751 j10array[0] = x219;
32752 sj10array[0] = IKsin(j10array[0]);
32753 cj10array[0] = IKcos(j10array[0]);
32754 j10array[1] = ((3.14159265358979) + x219);
32755 sj10array[1] = IKsin(j10array[1]);
32756 cj10array[1] = IKcos(j10array[1]);
32757 if (j10array[0] > IKPI)
32758 {
32759 j10array[0] -= IK2PI;
32760 }
32761 else if (j10array[0] < -IKPI)
32762 {
32763 j10array[0] += IK2PI;
32764 }
32765 j10valid[0] = true;
32766 if (j10array[1] > IKPI)
32767 {
32768 j10array[1] -= IK2PI;
32769 }
32770 else if (j10array[1] < -IKPI)
32771 {
32772 j10array[1] += IK2PI;
32773 }
32774 j10valid[1] = true;
32775 for (int ij10 = 0; ij10 < 2; ++ij10)
32776 {
32777 if (!j10valid[ij10])
32778 {
32779 continue;
32780 }
32781 _ij10[0] = ij10;
32782 _ij10[1] = -1;
32783 for (int iij10 = ij10 + 1; iij10 < 2; ++iij10)
32784 {
32785 if (j10valid[iij10]
32786 && IKabs(cj10array[ij10] - cj10array[iij10])
32787 < IKFAST_SOLUTION_THRESH
32788 && IKabs(sj10array[ij10] - sj10array[iij10])
32789 < IKFAST_SOLUTION_THRESH)
32790 {
32791 j10valid[iij10] = false;
32792 _ij10[1] = iij10;
32793 break;
32794 }
32795 }
32796 j10 = j10array[ij10];
32797 cj10 = cj10array[ij10];
32798 sj10 = sj10array[ij10];
32799 {
32800 IkReal evalcond[1];
32801 evalcond[0]
32802 = (((new_r12 * (IKcos(j10))))
32803 + (((-1.0) * (1.0) * new_r02
32804 * (IKsin(j10)))));
32805 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
32806 {
32807 continue;
32808 }
32809 }
32810
32811 {
32812 IkReal j12array[1], cj12array[1], sj12array[1];
32813 bool j12valid[1] = {false};
32814 _nj12 = 1;
32815 IkReal x221 = ((1.0) * new_r00);
32816 if (IKabs(
32817 ((((-1.0) * sj10 * x221))
32818 + ((cj10 * new_r01))))
32819 < IKFAST_ATAN2_MAGTHRESH
32820 && IKabs(
32821 ((((-1.0) * cj10 * x221))
32822 + (((-1.0) * (1.0) * new_r01
32823 * sj10))))
32824 < IKFAST_ATAN2_MAGTHRESH
32825 && IKabs(
32826 IKsqr(
32827 ((((-1.0) * sj10 * x221))
32828 + ((cj10 * new_r01))))
32829 + IKsqr(
32830 ((((-1.0) * cj10 * x221))
32831 + (((-1.0) * (1.0) * new_r01
32832 * sj10))))
32833 - 1)
32834 <= IKFAST_SINCOS_THRESH)
32835 continue;
32836 j12array[0] = IKatan2(
32837 ((((-1.0) * sj10 * x221))
32838 + ((cj10 * new_r01))),
32839 ((((-1.0) * cj10 * x221))
32840 + (((-1.0) * (1.0) * new_r01 * sj10))));
32841 sj12array[0] = IKsin(j12array[0]);
32842 cj12array[0] = IKcos(j12array[0]);
32843 if (j12array[0] > IKPI)
32844 {
32845 j12array[0] -= IK2PI;
32846 }
32847 else if (j12array[0] < -IKPI)
32848 {
32849 j12array[0] += IK2PI;
32850 }
32851 j12valid[0] = true;
32852 for (int ij12 = 0; ij12 < 1; ++ij12)
32853 {
32854 if (!j12valid[ij12])
32855 {
32856 continue;
32857 }
32858 _ij12[0] = ij12;
32859 _ij12[1] = -1;
32860 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
32861 {
32862 if (j12valid[iij12]
32863 && IKabs(
32864 cj12array[ij12]
32865 - cj12array[iij12])
32866 < IKFAST_SOLUTION_THRESH
32867 && IKabs(
32868 sj12array[ij12]
32869 - sj12array[iij12])
32870 < IKFAST_SOLUTION_THRESH)
32871 {
32872 j12valid[iij12] = false;
32873 _ij12[1] = iij12;
32874 break;
32875 }
32876 }
32877 j12 = j12array[ij12];
32878 cj12 = cj12array[ij12];
32879 sj12 = sj12array[ij12];
32880 {
32881 IkReal evalcond[8];
32882 IkReal x222 = IKcos(j12);
32883 IkReal x223 = IKsin(j12);
32884 IkReal x224 = ((1.0) * x223);
32885 IkReal x225 = ((-1.0) * x224);
32886 IkReal x226 = (cj10 * x222);
32887 IkReal x227 = ((1.0) * sj10);
32888 IkReal x228
32889 = (((sj10 * x222))
32890 + (((-1.0) * cj10 * x224)));
32891 evalcond[0]
32892 = (((cj10 * new_r00))
32893 + ((new_r10 * sj10)) + x222);
32894 evalcond[1]
32895 = (((cj10 * new_r01))
32896 + ((new_r11 * sj10)) + x225);
32897 evalcond[2]
32898 = (((sj10 * x223)) + new_r00 + x226);
32899 evalcond[3]
32900 = (((cj10 * new_r10))
32901 + (((-1.0) * new_r00 * x227))
32902 + x225);
32903 evalcond[4]
32904 = (((cj10 * new_r11))
32905 + (((-1.0) * x222))
32906 + (((-1.0) * new_r01 * x227)));
32907 evalcond[5] = (new_r01 + x228);
32908 evalcond[6] = (new_r10 + x228);
32909 evalcond[7]
32910 = ((((-1.0) * x226)) + new_r11
32911 + (((-1.0) * x223 * x227)));
32912 if (IKabs(evalcond[0])
32913 > IKFAST_EVALCOND_THRESH
32914 || IKabs(evalcond[1])
32915 > IKFAST_EVALCOND_THRESH
32916 || IKabs(evalcond[2])
32917 > IKFAST_EVALCOND_THRESH
32918 || IKabs(evalcond[3])
32919 > IKFAST_EVALCOND_THRESH
32920 || IKabs(evalcond[4])
32921 > IKFAST_EVALCOND_THRESH
32922 || IKabs(evalcond[5])
32923 > IKFAST_EVALCOND_THRESH
32924 || IKabs(evalcond[6])
32925 > IKFAST_EVALCOND_THRESH
32926 || IKabs(evalcond[7])
32927 > IKFAST_EVALCOND_THRESH)
32928 {
32929 continue;
32930 }
32931 }
32932
32933 {
32934 std::vector<
32935 IkSingleDOFSolutionBase<IkReal> >
32936 vinfos(7);
32937 vinfos[0].jointtype = 1;
32938 vinfos[0].foffset = j4;
32939 vinfos[0].indices[0] = _ij4[0];
32940 vinfos[0].indices[1] = _ij4[1];
32941 vinfos[0].maxsolutions = _nj4;
32942 vinfos[1].jointtype = 1;
32943 vinfos[1].foffset = j6;
32944 vinfos[1].indices[0] = _ij6[0];
32945 vinfos[1].indices[1] = _ij6[1];
32946 vinfos[1].maxsolutions = _nj6;
32947 vinfos[2].jointtype = 1;
32948 vinfos[2].foffset = j8;
32949 vinfos[2].indices[0] = _ij8[0];
32950 vinfos[2].indices[1] = _ij8[1];
32951 vinfos[2].maxsolutions = _nj8;
32952 vinfos[3].jointtype = 1;
32953 vinfos[3].foffset = j9;
32954 vinfos[3].indices[0] = _ij9[0];
32955 vinfos[3].indices[1] = _ij9[1];
32956 vinfos[3].maxsolutions = _nj9;
32957 vinfos[4].jointtype = 1;
32958 vinfos[4].foffset = j10;
32959 vinfos[4].indices[0] = _ij10[0];
32960 vinfos[4].indices[1] = _ij10[1];
32961 vinfos[4].maxsolutions = _nj10;
32962 vinfos[5].jointtype = 1;
32963 vinfos[5].foffset = j11;
32964 vinfos[5].indices[0] = _ij11[0];
32965 vinfos[5].indices[1] = _ij11[1];
32966 vinfos[5].maxsolutions = _nj11;
32967 vinfos[6].jointtype = 1;
32968 vinfos[6].foffset = j12;
32969 vinfos[6].indices[0] = _ij12[0];
32970 vinfos[6].indices[1] = _ij12[1];
32971 vinfos[6].maxsolutions = _nj12;
32972 std::vector<int> vfree(0);
32973 solutions.AddSolution(vinfos, vfree);
32974 }
32975 }
32976 }
32977 }
32978 }
32979 }
32980 } while (0);
32981 if (bgotonextstatement)
32982 {
32983 bool bgotonextstatement = true;
32984 do
32985 {
32986 if (1)
32987 {
32988 bgotonextstatement = false;
32989 continue; // branch miss [j10, j12]
32990 }
32991 } while (0);
32992 if (bgotonextstatement)
32993 {
32994 }
32995 }
32996 }
32997 }
32998 }
32999 else
33000 {
33001 {
33002 IkReal j10array[1], cj10array[1], sj10array[1];
33003 bool j10valid[1] = {false};
33004 _nj10 = 1;
33005 CheckValue<IkReal> x230 = IKPowWithIntegerCheck(sj11, -1);
33006 if (!x230.valid)
33007 {
33008 continue;
33009 }
33010 IkReal x229 = x230.value;
33011 CheckValue<IkReal> x231
33012 = IKPowWithIntegerCheck(new_r12, -1);
33013 if (!x231.valid)
33014 {
33015 continue;
33016 }
33017 if (IKabs(
33018 (x229 * (x231.value)
33019 * (((1.0) + (((-1.0) * (1.0) * (cj11 * cj11)))
33020 + (((-1.0) * (1.0) * (new_r02 * new_r02)))))))
33021 < IKFAST_ATAN2_MAGTHRESH
33022 && IKabs((new_r02 * x229)) < IKFAST_ATAN2_MAGTHRESH
33023 && IKabs(
33024 IKsqr(
33025 (x229 * (x231.value)
33026 * (((1.0)
33027 + (((-1.0) * (1.0) * (cj11 * cj11)))
33028 + (((-1.0) * (1.0)
33029 * (new_r02 * new_r02)))))))
33030 + IKsqr((new_r02 * x229)) - 1)
33031 <= IKFAST_SINCOS_THRESH)
33032 continue;
33033 j10array[0] = IKatan2(
33034 (x229 * (x231.value)
33035 * (((1.0) + (((-1.0) * (1.0) * (cj11 * cj11)))
33036 + (((-1.0) * (1.0) * (new_r02 * new_r02)))))),
33037 (new_r02 * x229));
33038 sj10array[0] = IKsin(j10array[0]);
33039 cj10array[0] = IKcos(j10array[0]);
33040 if (j10array[0] > IKPI)
33041 {
33042 j10array[0] -= IK2PI;
33043 }
33044 else if (j10array[0] < -IKPI)
33045 {
33046 j10array[0] += IK2PI;
33047 }
33048 j10valid[0] = true;
33049 for (int ij10 = 0; ij10 < 1; ++ij10)
33050 {
33051 if (!j10valid[ij10])
33052 {
33053 continue;
33054 }
33055 _ij10[0] = ij10;
33056 _ij10[1] = -1;
33057 for (int iij10 = ij10 + 1; iij10 < 1; ++iij10)
33058 {
33059 if (j10valid[iij10]
33060 && IKabs(cj10array[ij10] - cj10array[iij10])
33061 < IKFAST_SOLUTION_THRESH
33062 && IKabs(sj10array[ij10] - sj10array[iij10])
33063 < IKFAST_SOLUTION_THRESH)
33064 {
33065 j10valid[iij10] = false;
33066 _ij10[1] = iij10;
33067 break;
33068 }
33069 }
33070 j10 = j10array[ij10];
33071 cj10 = cj10array[ij10];
33072 sj10 = sj10array[ij10];
33073 {
33074 IkReal evalcond[8];
33075 IkReal x232 = IKcos(j10);
33076 IkReal x233 = ((1.0) * sj11);
33077 IkReal x234 = (x232 * x233);
33078 IkReal x235 = IKsin(j10);
33079 IkReal x236 = (x233 * x235);
33080 IkReal x237 = (new_r02 * x232);
33081 IkReal x238 = (new_r12 * x235);
33082 IkReal x239 = ((1.0) * cj11);
33083 evalcond[0] = ((((-1.0) * x234)) + new_r02);
33084 evalcond[1] = (new_r12 + (((-1.0) * x236)));
33085 evalcond[2]
33086 = (((new_r12 * x232))
33087 + (((-1.0) * new_r02 * x235)));
33088 evalcond[3] = ((((-1.0) * x233)) + x237 + x238);
33089 evalcond[4]
33090 = (((cj11 * x238)) + (((-1.0) * new_r22 * x233))
33091 + ((cj11 * x237)));
33092 evalcond[5]
33093 = ((((-1.0) * new_r00 * x234))
33094 + (((-1.0) * new_r10 * x236))
33095 + (((-1.0) * new_r20 * x239)));
33096 evalcond[6]
33097 = ((((-1.0) * new_r01 * x234))
33098 + (((-1.0) * new_r21 * x239))
33099 + (((-1.0) * new_r11 * x236)));
33100 evalcond[7]
33101 = ((1.0) + (((-1.0) * new_r22 * x239))
33102 + (((-1.0) * x233 * x238))
33103 + (((-1.0) * x233 * x237)));
33104 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
33105 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
33106 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
33107 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
33108 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
33109 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
33110 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
33111 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
33112 {
33113 continue;
33114 }
33115 }
33116
33117 {
33118 IkReal j12eval[2];
33119 IkReal x240 = ((1.0) * sj9);
33120 IkReal x241 = (cj9 * sj6);
33121 IkReal x242 = x160;
33122 IkReal x243 = (cj4 * sj8);
33123 IkReal x244 = (sj6 * x240);
33124 IkReal x245 = (cj6 * cj9);
33125 IkReal x246 = (((cj8 * x245)) + (((-1.0) * x244)));
33126 IkReal x247 = (((cj9 * x243)) + ((sj4 * x246)));
33127 IkReal x248 = ((1.0) * sj4 * sj8);
33128 IkReal x249
33129 = ((((-1.0) * cj9 * x248)) + ((cj4 * x246)));
33130 IkReal x250 = (sj6 * sj8);
33131 IkReal x251 = x169;
33132 IkReal x252 = x170;
33133 IkReal x253 = (x245 + (((-1.0) * cj8 * x244)));
33134 IkReal x254 = x172;
33135 IkReal x255 = (((sj9 * x243)) + ((sj4 * x254)));
33136 IkReal x256
33137 = (((cj4 * x254)) + (((-1.0) * sj4 * sj8 * x240)));
33138 new_r00
33139 = (((r20 * x242)) + ((r00 * x249))
33140 + ((r10 * x247)));
33141 new_r01
33142 = (((r21 * x242)) + ((r01 * x249))
33143 + ((r11 * x247)));
33144 new_r02
33145 = (((r22 * x242)) + ((r12 * x247))
33146 + ((r02 * x249)));
33147 new_r10
33148 = (((r10 * x251)) + ((r20 * x250))
33149 + ((r00 * x252)));
33150 new_r11
33151 = (((r11 * x251)) + ((r21 * x250))
33152 + ((r01 * x252)));
33153 new_r12
33154 = (((r22 * x250)) + ((r12 * x251))
33155 + ((r02 * x252)));
33156 new_r20
33157 = (((r20 * x253)) + ((r00 * x256))
33158 + ((r10 * x255)));
33159 new_r21
33160 = (((r21 * x253)) + ((r11 * x255))
33161 + ((r01 * x256)));
33162 new_r22
33163 = (((r02 * x256)) + ((r12 * x255))
33164 + ((r22 * x253)));
33165 j12eval[0] = sj11;
33166 j12eval[1] = IKsign(sj11);
33167 if (IKabs(j12eval[0]) < 0.0000010000000000
33168 || IKabs(j12eval[1]) < 0.0000010000000000)
33169 {
33170 {
33171 IkReal j12eval[2];
33172 IkReal x257 = ((1.0) * sj9);
33173 IkReal x258 = (cj9 * sj6);
33174 IkReal x259 = x160;
33175 IkReal x260 = (cj4 * sj8);
33176 IkReal x261 = (sj6 * x257);
33177 IkReal x262 = (cj6 * cj9);
33178 IkReal x263 = (((cj8 * x262)) + (((-1.0) * x261)));
33179 IkReal x264 = (((sj4 * x263)) + ((cj9 * x260)));
33180 IkReal x265 = ((1.0) * sj4 * sj8);
33181 IkReal x266
33182 = ((((-1.0) * cj9 * x265)) + ((cj4 * x263)));
33183 IkReal x267 = (sj6 * sj8);
33184 IkReal x268 = x169;
33185 IkReal x269 = x170;
33186 IkReal x270 = (x262 + (((-1.0) * cj8 * x261)));
33187 IkReal x271 = x172;
33188 IkReal x272 = (((sj4 * x271)) + ((sj9 * x260)));
33189 IkReal x273
33190 = (((cj4 * x271))
33191 + (((-1.0) * sj4 * sj8 * x257)));
33192 new_r00
33193 = (((r00 * x266)) + ((r20 * x259))
33194 + ((r10 * x264)));
33195 new_r01
33196 = (((r21 * x259)) + ((r01 * x266))
33197 + ((r11 * x264)));
33198 new_r02
33199 = (((r02 * x266)) + ((r12 * x264))
33200 + ((r22 * x259)));
33201 new_r10
33202 = (((r20 * x267)) + ((r00 * x269))
33203 + ((r10 * x268)));
33204 new_r11
33205 = (((r01 * x269)) + ((r21 * x267))
33206 + ((r11 * x268)));
33207 new_r12
33208 = (((r22 * x267)) + ((r12 * x268))
33209 + ((r02 * x269)));
33210 new_r20
33211 = (((r00 * x273)) + ((r10 * x272))
33212 + ((r20 * x270)));
33213 new_r21
33214 = (((r11 * x272)) + ((r01 * x273))
33215 + ((r21 * x270)));
33216 new_r22
33217 = (((r12 * x272)) + ((r02 * x273))
33218 + ((r22 * x270)));
33219 j12eval[0] = sj10;
33220 j12eval[1] = sj11;
33221 if (IKabs(j12eval[0]) < 0.0000010000000000
33222 || IKabs(j12eval[1]) < 0.0000010000000000)
33223 {
33224 {
33225 IkReal j12eval[3];
33226 IkReal x274 = ((1.0) * sj9);
33227 IkReal x275 = (cj9 * sj6);
33228 IkReal x276 = x160;
33229 IkReal x277 = (cj4 * sj8);
33230 IkReal x278 = (sj6 * x274);
33231 IkReal x279 = (cj6 * cj9);
33232 IkReal x280
33233 = (((cj8 * x279)) + (((-1.0) * x278)));
33234 IkReal x281 = (((cj9 * x277)) + ((sj4 * x280)));
33235 IkReal x282 = ((1.0) * sj4 * sj8);
33236 IkReal x283
33237 = (((cj4 * x280))
33238 + (((-1.0) * cj9 * x282)));
33239 IkReal x284 = (sj6 * sj8);
33240 IkReal x285 = x169;
33241 IkReal x286 = x170;
33242 IkReal x287 = ((((-1.0) * cj8 * x278)) + x279);
33243 IkReal x288 = x172;
33244 IkReal x289 = (((sj9 * x277)) + ((sj4 * x288)));
33245 IkReal x290
33246 = (((cj4 * x288))
33247 + (((-1.0) * sj4 * sj8 * x274)));
33248 new_r00
33249 = (((r00 * x283)) + ((r10 * x281))
33250 + ((r20 * x276)));
33251 new_r01
33252 = (((r01 * x283)) + ((r11 * x281))
33253 + ((r21 * x276)));
33254 new_r02
33255 = (((r22 * x276)) + ((r02 * x283))
33256 + ((r12 * x281)));
33257 new_r10
33258 = (((r10 * x285)) + ((r00 * x286))
33259 + ((r20 * x284)));
33260 new_r11
33261 = (((r01 * x286)) + ((r21 * x284))
33262 + ((r11 * x285)));
33263 new_r12
33264 = (((r22 * x284)) + ((r02 * x286))
33265 + ((r12 * x285)));
33266 new_r20
33267 = (((r00 * x290)) + ((r20 * x287))
33268 + ((r10 * x289)));
33269 new_r21
33270 = (((r21 * x287)) + ((r11 * x289))
33271 + ((r01 * x290)));
33272 new_r22
33273 = (((r22 * x287)) + ((r12 * x289))
33274 + ((r02 * x290)));
33275 j12eval[0] = cj10;
33276 j12eval[1] = cj11;
33277 j12eval[2] = sj11;
33278 if (IKabs(j12eval[0]) < 0.0000010000000000
33279 || IKabs(j12eval[1]) < 0.0000010000000000
33280 || IKabs(j12eval[2]) < 0.0000010000000000)
33281 {
33282 {
33283 IkReal evalcond[12];
33284 bool bgotonextstatement = true;
33285 do
33286 {
33287 IkReal x291 = ((1.0) * cj11);
33288 IkReal x292
33289 = ((((-1.0) * x291)) + new_r22);
33290 IkReal x293 = ((1.0) * sj11);
33291 IkReal x294
33292 = ((((-1.0) * x293)) + new_r12);
33293 evalcond[0]
33294 = ((-3.14159265358979)
33295 + (IKfmod(
33296 ((3.14159265358979)
33297 + (IKabs(
33298 ((-1.5707963267949)
33299 + j10)))),
33300 6.28318530717959)));
33301 evalcond[1] = x292;
33302 evalcond[2] = x292;
33303 evalcond[3] = new_r02;
33304 evalcond[4] = x294;
33305 evalcond[5] = x294;
33306 evalcond[6]
33307 = ((((-1.0) * new_r22 * x293))
33308 + ((cj11 * new_r12)));
33309 evalcond[7]
33310 = ((((-1.0) * new_r10 * x293))
33311 + (((-1.0) * new_r20 * x291)));
33312 evalcond[8]
33313 = ((((-1.0) * new_r11 * x293))
33314 + (((-1.0) * new_r21 * x291)));
33315 evalcond[9]
33316 = ((1.0) + (((-1.0) * new_r12 * x293))
33317 + (((-1.0) * new_r22 * x291)));
33318 if (IKabs(evalcond[0])
33319 < 0.0000010000000000
33320 && IKabs(evalcond[1])
33321 < 0.0000010000000000
33322 && IKabs(evalcond[2])
33323 < 0.0000010000000000
33324 && IKabs(evalcond[3])
33325 < 0.0000010000000000
33326 && IKabs(evalcond[4])
33327 < 0.0000010000000000
33328 && IKabs(evalcond[5])
33329 < 0.0000010000000000
33330 && IKabs(evalcond[6])
33331 < 0.0000010000000000
33332 && IKabs(evalcond[7])
33333 < 0.0000010000000000
33334 && IKabs(evalcond[8])
33335 < 0.0000010000000000
33336 && IKabs(evalcond[9])
33337 < 0.0000010000000000)
33338 {
33339 bgotonextstatement = false;
33340 {
33341 IkReal j12array[1], cj12array[1],
33342 sj12array[1];
33343 bool j12valid[1] = {false};
33344 _nj12 = 1;
33345 CheckValue<IkReal> x295
33346 = IKPowWithIntegerCheck(
33347 IKsign(new_r12), -1);
33348 if (!x295.valid)
33349 {
33350 continue;
33351 }
33352 CheckValue<IkReal> x296
33353 = IKatan2WithCheck(
33354 IkReal(new_r21),
33355 ((-1.0)
33356 * (((1.0) * new_r20))),
33357 IKFAST_ATAN2_MAGTHRESH);
33358 if (!x296.valid)
33359 {
33360 continue;
33361 }
33362 j12array[0]
33363 = ((-1.5707963267949)
33364 + (((1.5707963267949)
33365 * (x295.value)))
33366 + (x296.value));
33367 sj12array[0] = IKsin(j12array[0]);
33368 cj12array[0] = IKcos(j12array[0]);
33369 if (j12array[0] > IKPI)
33370 {
33371 j12array[0] -= IK2PI;
33372 }
33373 else if (j12array[0] < -IKPI)
33374 {
33375 j12array[0] += IK2PI;
33376 }
33377 j12valid[0] = true;
33378 for (int ij12 = 0; ij12 < 1; ++ij12)
33379 {
33380 if (!j12valid[ij12])
33381 {
33382 continue;
33383 }
33384 _ij12[0] = ij12;
33385 _ij12[1] = -1;
33386 for (int iij12 = ij12 + 1;
33387 iij12 < 1;
33388 ++iij12)
33389 {
33390 if (j12valid[iij12]
33391 && IKabs(
33392 cj12array[ij12]
33393 - cj12array[iij12])
33394 < IKFAST_SOLUTION_THRESH
33395 && IKabs(
33396 sj12array[ij12]
33397 - sj12array[iij12])
33398 < IKFAST_SOLUTION_THRESH)
33399 {
33400 j12valid[iij12] = false;
33401 _ij12[1] = iij12;
33402 break;
33403 }
33404 }
33405 j12 = j12array[ij12];
33406 cj12 = cj12array[ij12];
33407 sj12 = sj12array[ij12];
33408 {
33409 IkReal evalcond[8];
33410 IkReal x297 = IKcos(j12);
33411 IkReal x298 = IKsin(j12);
33412 IkReal x299 = ((1.0) * new_r12);
33413 IkReal x300 = ((1.0) * x297);
33414 IkReal x301 = ((-1.0) * x300);
33415 evalcond[0]
33416 = (((new_r12 * x297))
33417 + new_r20);
33418 evalcond[1]
33419 = (((new_r22 * x298))
33420 + new_r11);
33421 evalcond[2]
33422 = ((((-1.0) * x298 * x299))
33423 + new_r21);
33424 evalcond[3]
33425 = ((((-1.0) * new_r22 * x300))
33426 + new_r10);
33427 evalcond[4]
33428 = ((((-1.0) * (1.0)
33429 * new_r00))
33430 + (((-1.0) * x298)));
33431 evalcond[5]
33432 = (x301
33433 + (((-1.0) * (1.0)
33434 * new_r01)));
33435 evalcond[6]
33436 = (((new_r11 * new_r22))
33437 + x298
33438 + (((-1.0) * new_r21
33439 * x299)));
33440 evalcond[7]
33441 = ((((-1.0) * new_r20 * x299))
33442 + x301
33443 + ((new_r10 * new_r22)));
33444 if (IKabs(evalcond[0])
33445 > IKFAST_EVALCOND_THRESH
33446 || IKabs(evalcond[1])
33447 > IKFAST_EVALCOND_THRESH
33448 || IKabs(evalcond[2])
33449 > IKFAST_EVALCOND_THRESH
33450 || IKabs(evalcond[3])
33451 > IKFAST_EVALCOND_THRESH
33452 || IKabs(evalcond[4])
33453 > IKFAST_EVALCOND_THRESH
33454 || IKabs(evalcond[5])
33455 > IKFAST_EVALCOND_THRESH
33456 || IKabs(evalcond[6])
33457 > IKFAST_EVALCOND_THRESH
33458 || IKabs(evalcond[7])
33459 > IKFAST_EVALCOND_THRESH)
33460 {
33461 continue;
33462 }
33463 }
33464
33465 {
33466 std::vector<
33467 IkSingleDOFSolutionBase<
33468 IkReal> >
33469 vinfos(7);
33470 vinfos[0].jointtype = 1;
33471 vinfos[0].foffset = j4;
33472 vinfos[0].indices[0] = _ij4[0];
33473 vinfos[0].indices[1] = _ij4[1];
33474 vinfos[0].maxsolutions = _nj4;
33475 vinfos[1].jointtype = 1;
33476 vinfos[1].foffset = j6;
33477 vinfos[1].indices[0] = _ij6[0];
33478 vinfos[1].indices[1] = _ij6[1];
33479 vinfos[1].maxsolutions = _nj6;
33480 vinfos[2].jointtype = 1;
33481 vinfos[2].foffset = j8;
33482 vinfos[2].indices[0] = _ij8[0];
33483 vinfos[2].indices[1] = _ij8[1];
33484 vinfos[2].maxsolutions = _nj8;
33485 vinfos[3].jointtype = 1;
33486 vinfos[3].foffset = j9;
33487 vinfos[3].indices[0] = _ij9[0];
33488 vinfos[3].indices[1] = _ij9[1];
33489 vinfos[3].maxsolutions = _nj9;
33490 vinfos[4].jointtype = 1;
33491 vinfos[4].foffset = j10;
33492 vinfos[4].indices[0] = _ij10[0];
33493 vinfos[4].indices[1] = _ij10[1];
33494 vinfos[4].maxsolutions = _nj10;
33495 vinfos[5].jointtype = 1;
33496 vinfos[5].foffset = j11;
33497 vinfos[5].indices[0] = _ij11[0];
33498 vinfos[5].indices[1] = _ij11[1];
33499 vinfos[5].maxsolutions = _nj11;
33500 vinfos[6].jointtype = 1;
33501 vinfos[6].foffset = j12;
33502 vinfos[6].indices[0] = _ij12[0];
33503 vinfos[6].indices[1] = _ij12[1];
33504 vinfos[6].maxsolutions = _nj12;
33505 std::vector<int> vfree(0);
33506 solutions.AddSolution(
33507 vinfos, vfree);
33508 }
33509 }
33510 }
33511 }
33512 } while (0);
33513 if (bgotonextstatement)
33514 {
33515 bool bgotonextstatement = true;
33516 do
33517 {
33518 IkReal x302 = ((1.0) * cj11);
33519 IkReal x303
33520 = ((((-1.0) * x302)) + new_r22);
33521 IkReal x304 = ((1.0) * sj11);
33522 evalcond[0]
33523 = ((-3.14159265358979)
33524 + (IKfmod(
33525 ((3.14159265358979)
33526 + (IKabs(
33527 ((1.5707963267949)
33528 + j10)))),
33529 6.28318530717959)));
33530 evalcond[1] = x303;
33531 evalcond[2] = x303;
33532 evalcond[3] = new_r02;
33533 evalcond[4] = (sj11 + new_r12);
33534 evalcond[5]
33535 = ((((-1.0) * (1.0) * new_r12))
33536 + (((-1.0) * x304)));
33537 evalcond[6]
33538 = ((((-1.0) * new_r22 * x304))
33539 + (((-1.0) * new_r12 * x302)));
33540 evalcond[7]
33541 = (((new_r10 * sj11))
33542 + (((-1.0) * new_r20 * x302)));
33543 evalcond[8]
33544 = ((((-1.0) * new_r21 * x302))
33545 + ((new_r11 * sj11)));
33546 evalcond[9]
33547 = ((1.0)
33548 + (((-1.0) * new_r22 * x302))
33549 + ((new_r12 * sj11)));
33550 if (IKabs(evalcond[0])
33551 < 0.0000010000000000
33552 && IKabs(evalcond[1])
33553 < 0.0000010000000000
33554 && IKabs(evalcond[2])
33555 < 0.0000010000000000
33556 && IKabs(evalcond[3])
33557 < 0.0000010000000000
33558 && IKabs(evalcond[4])
33559 < 0.0000010000000000
33560 && IKabs(evalcond[5])
33561 < 0.0000010000000000
33562 && IKabs(evalcond[6])
33563 < 0.0000010000000000
33564 && IKabs(evalcond[7])
33565 < 0.0000010000000000
33566 && IKabs(evalcond[8])
33567 < 0.0000010000000000
33568 && IKabs(evalcond[9])
33569 < 0.0000010000000000)
33570 {
33571 bgotonextstatement = false;
33572 {
33573 IkReal j12array[1], cj12array[1],
33574 sj12array[1];
33575 bool j12valid[1] = {false};
33576 _nj12 = 1;
33577 if (IKabs(new_r00)
33578 < IKFAST_ATAN2_MAGTHRESH
33579 && IKabs(new_r01)
33580 < IKFAST_ATAN2_MAGTHRESH
33581 && IKabs(
33582 IKsqr(new_r00)
33583 + IKsqr(new_r01) - 1)
33584 <= IKFAST_SINCOS_THRESH)
33585 continue;
33586 j12array[0]
33587 = IKatan2(new_r00, new_r01);
33588 sj12array[0] = IKsin(j12array[0]);
33589 cj12array[0] = IKcos(j12array[0]);
33590 if (j12array[0] > IKPI)
33591 {
33592 j12array[0] -= IK2PI;
33593 }
33594 else if (j12array[0] < -IKPI)
33595 {
33596 j12array[0] += IK2PI;
33597 }
33598 j12valid[0] = true;
33599 for (int ij12 = 0; ij12 < 1; ++ij12)
33600 {
33601 if (!j12valid[ij12])
33602 {
33603 continue;
33604 }
33605 _ij12[0] = ij12;
33606 _ij12[1] = -1;
33607 for (int iij12 = ij12 + 1;
33608 iij12 < 1;
33609 ++iij12)
33610 {
33611 if (j12valid[iij12]
33612 && IKabs(
33613 cj12array[ij12]
33614 - cj12array[iij12])
33615 < IKFAST_SOLUTION_THRESH
33616 && IKabs(
33617 sj12array[ij12]
33618 - sj12array[iij12])
33619 < IKFAST_SOLUTION_THRESH)
33620 {
33621 j12valid[iij12] = false;
33622 _ij12[1] = iij12;
33623 break;
33624 }
33625 }
33626 j12 = j12array[ij12];
33627 cj12 = cj12array[ij12];
33628 sj12 = sj12array[ij12];
33629 {
33630 IkReal evalcond[8];
33631 IkReal x305 = IKsin(j12);
33632 IkReal x306
33633 = ((1.0) * (IKcos(j12)));
33634 IkReal x307 = ((-1.0) * x306);
33635 IkReal x308 = ((1.0) * new_r11);
33636 IkReal x309 = ((1.0) * new_r10);
33637 evalcond[0]
33638 = (((new_r12 * x305))
33639 + new_r21);
33640 evalcond[1]
33641 = ((((-1.0) * x305))
33642 + new_r00);
33643 evalcond[2] = (x307 + new_r01);
33644 evalcond[3]
33645 = ((((-1.0) * new_r12
33646 * x306))
33647 + new_r20);
33648 evalcond[4]
33649 = ((((-1.0) * x308))
33650 + ((new_r22 * x305)));
33651 evalcond[5]
33652 = ((((-1.0) * x309))
33653 + (((-1.0) * new_r22
33654 * x306)));
33655 evalcond[6]
33656 = (x305
33657 + ((new_r12 * new_r21))
33658 + (((-1.0) * new_r22
33659 * x308)));
33660 evalcond[7]
33661 = (x307
33662 + ((new_r12 * new_r20))
33663 + (((-1.0) * new_r22
33664 * x309)));
33665 if (IKabs(evalcond[0])
33666 > IKFAST_EVALCOND_THRESH
33667 || IKabs(evalcond[1])
33668 > IKFAST_EVALCOND_THRESH
33669 || IKabs(evalcond[2])
33670 > IKFAST_EVALCOND_THRESH
33671 || IKabs(evalcond[3])
33672 > IKFAST_EVALCOND_THRESH
33673 || IKabs(evalcond[4])
33674 > IKFAST_EVALCOND_THRESH
33675 || IKabs(evalcond[5])
33676 > IKFAST_EVALCOND_THRESH
33677 || IKabs(evalcond[6])
33678 > IKFAST_EVALCOND_THRESH
33679 || IKabs(evalcond[7])
33680 > IKFAST_EVALCOND_THRESH)
33681 {
33682 continue;
33683 }
33684 }
33685
33686 {
33687 std::vector<
33688 IkSingleDOFSolutionBase<
33689 IkReal> >
33690 vinfos(7);
33691 vinfos[0].jointtype = 1;
33692 vinfos[0].foffset = j4;
33693 vinfos[0].indices[0] = _ij4[0];
33694 vinfos[0].indices[1] = _ij4[1];
33695 vinfos[0].maxsolutions = _nj4;
33696 vinfos[1].jointtype = 1;
33697 vinfos[1].foffset = j6;
33698 vinfos[1].indices[0] = _ij6[0];
33699 vinfos[1].indices[1] = _ij6[1];
33700 vinfos[1].maxsolutions = _nj6;
33701 vinfos[2].jointtype = 1;
33702 vinfos[2].foffset = j8;
33703 vinfos[2].indices[0] = _ij8[0];
33704 vinfos[2].indices[1] = _ij8[1];
33705 vinfos[2].maxsolutions = _nj8;
33706 vinfos[3].jointtype = 1;
33707 vinfos[3].foffset = j9;
33708 vinfos[3].indices[0] = _ij9[0];
33709 vinfos[3].indices[1] = _ij9[1];
33710 vinfos[3].maxsolutions = _nj9;
33711 vinfos[4].jointtype = 1;
33712 vinfos[4].foffset = j10;
33713 vinfos[4].indices[0] = _ij10[0];
33714 vinfos[4].indices[1] = _ij10[1];
33715 vinfos[4].maxsolutions = _nj10;
33716 vinfos[5].jointtype = 1;
33717 vinfos[5].foffset = j11;
33718 vinfos[5].indices[0] = _ij11[0];
33719 vinfos[5].indices[1] = _ij11[1];
33720 vinfos[5].maxsolutions = _nj11;
33721 vinfos[6].jointtype = 1;
33722 vinfos[6].foffset = j12;
33723 vinfos[6].indices[0] = _ij12[0];
33724 vinfos[6].indices[1] = _ij12[1];
33725 vinfos[6].maxsolutions = _nj12;
33726 std::vector<int> vfree(0);
33727 solutions.AddSolution(
33728 vinfos, vfree);
33729 }
33730 }
33731 }
33732 }
33733 } while (0);
33734 if (bgotonextstatement)
33735 {
33736 bool bgotonextstatement = true;
33737 do
33738 {
33739 IkReal x310 = ((1.0) * cj10);
33740 IkReal x311 = ((1.0) * sj10);
33741 IkReal x312
33742 = ((((-1.0) * new_r02 * x311))
33743 + ((cj10 * new_r12)));
33744 evalcond[0]
33745 = ((-3.14159265358979)
33746 + (IKfmod(
33747 ((3.14159265358979)
33748 + (IKabs(
33749 ((-1.5707963267949)
33750 + j11)))),
33751 6.28318530717959)));
33752 evalcond[1] = new_r22;
33753 evalcond[2]
33754 = ((((-1.0) * x310)) + new_r02);
33755 evalcond[3]
33756 = (new_r12 + (((-1.0) * x311)));
33757 evalcond[4] = x312;
33758 evalcond[5] = x312;
33759 evalcond[6]
33760 = ((-1.0) + ((cj10 * new_r02))
33761 + ((new_r12 * sj10)));
33762 evalcond[7]
33763 = (((cj10 * new_r01))
33764 + ((new_r11 * sj10)));
33765 evalcond[8]
33766 = (((cj10 * new_r00))
33767 + ((new_r10 * sj10)));
33768 evalcond[9]
33769 = ((((-1.0) * new_r10 * x311))
33770 + (((-1.0) * new_r00 * x310)));
33771 evalcond[10]
33772 = ((((-1.0) * new_r01 * x310))
33773 + (((-1.0) * new_r11 * x311)));
33774 evalcond[11]
33775 = ((1.0)
33776 + (((-1.0) * new_r12 * x311))
33777 + (((-1.0) * new_r02 * x310)));
33778 if (IKabs(evalcond[0])
33779 < 0.0000010000000000
33780 && IKabs(evalcond[1])
33781 < 0.0000010000000000
33782 && IKabs(evalcond[2])
33783 < 0.0000010000000000
33784 && IKabs(evalcond[3])
33785 < 0.0000010000000000
33786 && IKabs(evalcond[4])
33787 < 0.0000010000000000
33788 && IKabs(evalcond[5])
33789 < 0.0000010000000000
33790 && IKabs(evalcond[6])
33791 < 0.0000010000000000
33792 && IKabs(evalcond[7])
33793 < 0.0000010000000000
33794 && IKabs(evalcond[8])
33795 < 0.0000010000000000
33796 && IKabs(evalcond[9])
33797 < 0.0000010000000000
33798 && IKabs(evalcond[10])
33799 < 0.0000010000000000
33800 && IKabs(evalcond[11])
33801 < 0.0000010000000000)
33802 {
33803 bgotonextstatement = false;
33804 {
33805 IkReal j12array[1], cj12array[1],
33806 sj12array[1];
33807 bool j12valid[1] = {false};
33808 _nj12 = 1;
33809 if (IKabs(new_r21)
33810 < IKFAST_ATAN2_MAGTHRESH
33811 && IKabs((
33812 (-1.0)
33813 * (((1.0) * new_r20))))
33814 < IKFAST_ATAN2_MAGTHRESH
33815 && IKabs(
33816 IKsqr(new_r21)
33817 + IKsqr(
33818 ((-1.0)
33819 * (((1.0)
33820 * new_r20))))
33821 - 1)
33822 <= IKFAST_SINCOS_THRESH)
33823 continue;
33824 j12array[0] = IKatan2(
33825 new_r21,
33826 ((-1.0)
33827 * (((1.0) * new_r20))));
33828 sj12array[0] = IKsin(j12array[0]);
33829 cj12array[0] = IKcos(j12array[0]);
33830 if (j12array[0] > IKPI)
33831 {
33832 j12array[0] -= IK2PI;
33833 }
33834 else if (j12array[0] < -IKPI)
33835 {
33836 j12array[0] += IK2PI;
33837 }
33838 j12valid[0] = true;
33839 for (int ij12 = 0; ij12 < 1;
33840 ++ij12)
33841 {
33842 if (!j12valid[ij12])
33843 {
33844 continue;
33845 }
33846 _ij12[0] = ij12;
33847 _ij12[1] = -1;
33848 for (int iij12 = ij12 + 1;
33849 iij12 < 1;
33850 ++iij12)
33851 {
33852 if (j12valid[iij12]
33853 && IKabs(
33854 cj12array[ij12]
33855 - cj12array[iij12])
33856 < IKFAST_SOLUTION_THRESH
33857 && IKabs(
33858 sj12array[ij12]
33859 - sj12array[iij12])
33860 < IKFAST_SOLUTION_THRESH)
33861 {
33862 j12valid[iij12] = false;
33863 _ij12[1] = iij12;
33864 break;
33865 }
33866 }
33867 j12 = j12array[ij12];
33868 cj12 = cj12array[ij12];
33869 sj12 = sj12array[ij12];
33870 {
33871 IkReal evalcond[8];
33872 IkReal x313 = IKcos(j12);
33873 IkReal x314 = IKsin(j12);
33874 IkReal x315 = ((1.0) * x314);
33875 IkReal x316 = ((-1.0) * x315);
33876 IkReal x317 = ((1.0) * x313);
33877 IkReal x318
33878 = ((1.0) * new_r12);
33879 evalcond[0]
33880 = (x313 + new_r20);
33881 evalcond[1]
33882 = (x316 + new_r21);
33883 evalcond[2]
33884 = (((new_r12 * x313))
33885 + new_r01);
33886 evalcond[3]
33887 = (((new_r12 * x314))
33888 + new_r00);
33889 evalcond[4]
33890 = ((((-1.0) * new_r02
33891 * x317))
33892 + new_r11);
33893 evalcond[5]
33894 = (new_r10
33895 + (((-1.0) * new_r02
33896 * x315)));
33897 evalcond[6]
33898 = ((((-1.0) * new_r00
33899 * x318))
33900 + x316
33901 + ((new_r02
33902 * new_r10)));
33903 evalcond[7]
33904 = (((new_r02 * new_r11))
33905 + (((-1.0) * new_r01
33906 * x318))
33907 + (((-1.0) * x317)));
33908 if (IKabs(evalcond[0])
33909 > IKFAST_EVALCOND_THRESH
33910 || IKabs(evalcond[1])
33911 > IKFAST_EVALCOND_THRESH
33912 || IKabs(evalcond[2])
33913 > IKFAST_EVALCOND_THRESH
33914 || IKabs(evalcond[3])
33915 > IKFAST_EVALCOND_THRESH
33916 || IKabs(evalcond[4])
33917 > IKFAST_EVALCOND_THRESH
33918 || IKabs(evalcond[5])
33919 > IKFAST_EVALCOND_THRESH
33920 || IKabs(evalcond[6])
33921 > IKFAST_EVALCOND_THRESH
33922 || IKabs(evalcond[7])
33923 > IKFAST_EVALCOND_THRESH)
33924 {
33925 continue;
33926 }
33927 }
33928
33929 {
33930 std::vector<
33931 IkSingleDOFSolutionBase<
33932 IkReal> >
33933 vinfos(7);
33934 vinfos[0].jointtype = 1;
33935 vinfos[0].foffset = j4;
33936 vinfos[0].indices[0]
33937 = _ij4[0];
33938 vinfos[0].indices[1]
33939 = _ij4[1];
33940 vinfos[0].maxsolutions = _nj4;
33941 vinfos[1].jointtype = 1;
33942 vinfos[1].foffset = j6;
33943 vinfos[1].indices[0]
33944 = _ij6[0];
33945 vinfos[1].indices[1]
33946 = _ij6[1];
33947 vinfos[1].maxsolutions = _nj6;
33948 vinfos[2].jointtype = 1;
33949 vinfos[2].foffset = j8;
33950 vinfos[2].indices[0]
33951 = _ij8[0];
33952 vinfos[2].indices[1]
33953 = _ij8[1];
33954 vinfos[2].maxsolutions = _nj8;
33955 vinfos[3].jointtype = 1;
33956 vinfos[3].foffset = j9;
33957 vinfos[3].indices[0]
33958 = _ij9[0];
33959 vinfos[3].indices[1]
33960 = _ij9[1];
33961 vinfos[3].maxsolutions = _nj9;
33962 vinfos[4].jointtype = 1;
33963 vinfos[4].foffset = j10;
33964 vinfos[4].indices[0]
33965 = _ij10[0];
33966 vinfos[4].indices[1]
33967 = _ij10[1];
33968 vinfos[4].maxsolutions
33969 = _nj10;
33970 vinfos[5].jointtype = 1;
33971 vinfos[5].foffset = j11;
33972 vinfos[5].indices[0]
33973 = _ij11[0];
33974 vinfos[5].indices[1]
33975 = _ij11[1];
33976 vinfos[5].maxsolutions
33977 = _nj11;
33978 vinfos[6].jointtype = 1;
33979 vinfos[6].foffset = j12;
33980 vinfos[6].indices[0]
33981 = _ij12[0];
33982 vinfos[6].indices[1]
33983 = _ij12[1];
33984 vinfos[6].maxsolutions
33985 = _nj12;
33986 std::vector<int> vfree(0);
33987 solutions.AddSolution(
33988 vinfos, vfree);
33989 }
33990 }
33991 }
33992 }
33993 } while (0);
33994 if (bgotonextstatement)
33995 {
33996 bool bgotonextstatement = true;
33997 do
33998 {
33999 IkReal x319
34000 = ((((-1.0) * (1.0) * new_r02
34001 * sj10))
34002 + ((cj10 * new_r12)));
34003 IkReal x320
34004 = ((1.0) + ((cj10 * new_r02))
34005 + ((new_r12 * sj10)));
34006 IkReal x321
34007 = (((cj10 * new_r01))
34008 + ((new_r11 * sj10)));
34009 IkReal x322
34010 = (((cj10 * new_r00))
34011 + ((new_r10 * sj10)));
34012 evalcond[0]
34013 = ((-3.14159265358979)
34014 + (IKfmod(
34015 ((3.14159265358979)
34016 + (IKabs(
34017 ((1.5707963267949)
34018 + j11)))),
34019 6.28318530717959)));
34020 evalcond[1] = new_r22;
34021 evalcond[2] = (cj10 + new_r02);
34022 evalcond[3] = (sj10 + new_r12);
34023 evalcond[4] = x319;
34024 evalcond[5] = x319;
34025 evalcond[6] = x320;
34026 evalcond[7] = x321;
34027 evalcond[8] = x322;
34028 evalcond[9] = x322;
34029 evalcond[10] = x321;
34030 evalcond[11] = x320;
34031 if (IKabs(evalcond[0])
34032 < 0.0000010000000000
34033 && IKabs(evalcond[1])
34034 < 0.0000010000000000
34035 && IKabs(evalcond[2])
34036 < 0.0000010000000000
34037 && IKabs(evalcond[3])
34038 < 0.0000010000000000
34039 && IKabs(evalcond[4])
34040 < 0.0000010000000000
34041 && IKabs(evalcond[5])
34042 < 0.0000010000000000
34043 && IKabs(evalcond[6])
34044 < 0.0000010000000000
34045 && IKabs(evalcond[7])
34046 < 0.0000010000000000
34047 && IKabs(evalcond[8])
34048 < 0.0000010000000000
34049 && IKabs(evalcond[9])
34050 < 0.0000010000000000
34051 && IKabs(evalcond[10])
34052 < 0.0000010000000000
34053 && IKabs(evalcond[11])
34054 < 0.0000010000000000)
34055 {
34056 bgotonextstatement = false;
34057 {
34058 IkReal j12array[1],
34059 cj12array[1], sj12array[1];
34060 bool j12valid[1] = {false};
34061 _nj12 = 1;
34062 if (IKabs(
34063 ((-1.0)
34064 * (((1.0) * new_r21))))
34065 < IKFAST_ATAN2_MAGTHRESH
34066 && IKabs(new_r20)
34067 < IKFAST_ATAN2_MAGTHRESH
34068 && IKabs(
34069 IKsqr((
34070 (-1.0)
34071 * (((1.0)
34072 * new_r21))))
34073 + IKsqr(new_r20) - 1)
34074 <= IKFAST_SINCOS_THRESH)
34075 continue;
34076 j12array[0] = IKatan2(
34077 ((-1.0)
34078 * (((1.0) * new_r21))),
34079 new_r20);
34080 sj12array[0]
34081 = IKsin(j12array[0]);
34082 cj12array[0]
34083 = IKcos(j12array[0]);
34084 if (j12array[0] > IKPI)
34085 {
34086 j12array[0] -= IK2PI;
34087 }
34088 else if (j12array[0] < -IKPI)
34089 {
34090 j12array[0] += IK2PI;
34091 }
34092 j12valid[0] = true;
34093 for (int ij12 = 0; ij12 < 1;
34094 ++ij12)
34095 {
34096 if (!j12valid[ij12])
34097 {
34098 continue;
34099 }
34100 _ij12[0] = ij12;
34101 _ij12[1] = -1;
34102 for (int iij12 = ij12 + 1;
34103 iij12 < 1;
34104 ++iij12)
34105 {
34106 if (j12valid[iij12]
34107 && IKabs(
34108 cj12array[ij12]
34109 - cj12array
34110 [iij12])
34111 < IKFAST_SOLUTION_THRESH
34112 && IKabs(
34113 sj12array[ij12]
34114 - sj12array
34115 [iij12])
34116 < IKFAST_SOLUTION_THRESH)
34117 {
34118 j12valid[iij12] = false;
34119 _ij12[1] = iij12;
34120 break;
34121 }
34122 }
34123 j12 = j12array[ij12];
34124 cj12 = cj12array[ij12];
34125 sj12 = sj12array[ij12];
34126 {
34127 IkReal evalcond[8];
34128 IkReal x323 = IKsin(j12);
34129 IkReal x324 = IKcos(j12);
34130 IkReal x325
34131 = ((1.0) * x324);
34132 IkReal x326
34133 = ((-1.0) * x325);
34134 IkReal x327
34135 = ((1.0) * x323);
34136 IkReal x328
34137 = ((1.0) * new_r02);
34138 evalcond[0]
34139 = (x323 + new_r21);
34140 evalcond[1]
34141 = (x326 + new_r20);
34142 evalcond[2]
34143 = (((new_r02 * x324))
34144 + new_r11);
34145 evalcond[3]
34146 = (((new_r02 * x323))
34147 + new_r10);
34148 evalcond[4]
34149 = ((((-1.0) * new_r12
34150 * x325))
34151 + new_r01);
34152 evalcond[5]
34153 = ((((-1.0) * new_r12
34154 * x327))
34155 + new_r00);
34156 evalcond[6]
34157 = (((new_r00 * new_r12))
34158 + (((-1.0) * x327))
34159 + (((-1.0) * new_r10
34160 * x328)));
34161 evalcond[7]
34162 = (((new_r01 * new_r12))
34163 + x326
34164 + (((-1.0) * new_r11
34165 * x328)));
34166 if (IKabs(evalcond[0])
34167 > IKFAST_EVALCOND_THRESH
34168 || IKabs(evalcond[1])
34169 > IKFAST_EVALCOND_THRESH
34170 || IKabs(evalcond[2])
34171 > IKFAST_EVALCOND_THRESH
34172 || IKabs(evalcond[3])
34173 > IKFAST_EVALCOND_THRESH
34174 || IKabs(evalcond[4])
34175 > IKFAST_EVALCOND_THRESH
34176 || IKabs(evalcond[5])
34177 > IKFAST_EVALCOND_THRESH
34178 || IKabs(evalcond[6])
34179 > IKFAST_EVALCOND_THRESH
34180 || IKabs(evalcond[7])
34181 > IKFAST_EVALCOND_THRESH)
34182 {
34183 continue;
34184 }
34185 }
34186
34187 {
34188 std::vector<
34189 IkSingleDOFSolutionBase<
34190 IkReal> >
34191 vinfos(7);
34192 vinfos[0].jointtype = 1;
34193 vinfos[0].foffset = j4;
34194 vinfos[0].indices[0]
34195 = _ij4[0];
34196 vinfos[0].indices[1]
34197 = _ij4[1];
34198 vinfos[0].maxsolutions
34199 = _nj4;
34200 vinfos[1].jointtype = 1;
34201 vinfos[1].foffset = j6;
34202 vinfos[1].indices[0]
34203 = _ij6[0];
34204 vinfos[1].indices[1]
34205 = _ij6[1];
34206 vinfos[1].maxsolutions
34207 = _nj6;
34208 vinfos[2].jointtype = 1;
34209 vinfos[2].foffset = j8;
34210 vinfos[2].indices[0]
34211 = _ij8[0];
34212 vinfos[2].indices[1]
34213 = _ij8[1];
34214 vinfos[2].maxsolutions
34215 = _nj8;
34216 vinfos[3].jointtype = 1;
34217 vinfos[3].foffset = j9;
34218 vinfos[3].indices[0]
34219 = _ij9[0];
34220 vinfos[3].indices[1]
34221 = _ij9[1];
34222 vinfos[3].maxsolutions
34223 = _nj9;
34224 vinfos[4].jointtype = 1;
34225 vinfos[4].foffset = j10;
34226 vinfos[4].indices[0]
34227 = _ij10[0];
34228 vinfos[4].indices[1]
34229 = _ij10[1];
34230 vinfos[4].maxsolutions
34231 = _nj10;
34232 vinfos[5].jointtype = 1;
34233 vinfos[5].foffset = j11;
34234 vinfos[5].indices[0]
34235 = _ij11[0];
34236 vinfos[5].indices[1]
34237 = _ij11[1];
34238 vinfos[5].maxsolutions
34239 = _nj11;
34240 vinfos[6].jointtype = 1;
34241 vinfos[6].foffset = j12;
34242 vinfos[6].indices[0]
34243 = _ij12[0];
34244 vinfos[6].indices[1]
34245 = _ij12[1];
34246 vinfos[6].maxsolutions
34247 = _nj12;
34248 std::vector<int> vfree(0);
34249 solutions.AddSolution(
34250 vinfos, vfree);
34251 }
34252 }
34253 }
34254 }
34255 } while (0);
34256 if (bgotonextstatement)
34257 {
34258 bool bgotonextstatement = true;
34259 do
34260 {
34261 IkReal x329
34262 = ((((-1.0) * (1.0) * new_r02
34263 * sj10))
34264 + ((cj10 * new_r12)));
34265 IkReal x330
34266 = (((cj10 * new_r02))
34267 + ((new_r12 * sj10)));
34268 evalcond[0]
34269 = ((-3.14159265358979)
34270 + (IKfmod(
34271 ((3.14159265358979)
34272 + (IKabs(j11))),
34273 6.28318530717959)));
34274 evalcond[1] = ((-1.0) + new_r22);
34275 evalcond[2] = new_r20;
34276 evalcond[3] = new_r02;
34277 evalcond[4] = new_r12;
34278 evalcond[5] = new_r21;
34279 evalcond[6] = x329;
34280 evalcond[7] = x329;
34281 evalcond[8] = x330;
34282 evalcond[9] = x330;
34283 if (IKabs(evalcond[0])
34284 < 0.0000010000000000
34285 && IKabs(evalcond[1])
34286 < 0.0000010000000000
34287 && IKabs(evalcond[2])
34288 < 0.0000010000000000
34289 && IKabs(evalcond[3])
34290 < 0.0000010000000000
34291 && IKabs(evalcond[4])
34292 < 0.0000010000000000
34293 && IKabs(evalcond[5])
34294 < 0.0000010000000000
34295 && IKabs(evalcond[6])
34296 < 0.0000010000000000
34297 && IKabs(evalcond[7])
34298 < 0.0000010000000000
34299 && IKabs(evalcond[8])
34300 < 0.0000010000000000
34301 && IKabs(evalcond[9])
34302 < 0.0000010000000000)
34303 {
34304 bgotonextstatement = false;
34305 {
34306 IkReal j12array[1],
34307 cj12array[1],
34308 sj12array[1];
34309 bool j12valid[1] = {false};
34310 _nj12 = 1;
34311 IkReal x331
34312 = ((1.0) * new_r01);
34313 if (IKabs(
34314 ((((-1.0) * cj10
34315 * x331))
34316 + (((-1.0) * (1.0)
34317 * new_r00
34318 * sj10))))
34319 < IKFAST_ATAN2_MAGTHRESH
34320 && IKabs(
34321 (((cj10 * new_r00))
34322 + (((-1.0) * sj10
34323 * x331))))
34324 < IKFAST_ATAN2_MAGTHRESH
34325 && IKabs(
34326 IKsqr((
34327 (((-1.0) * cj10
34328 * x331))
34329 + (((-1.0)
34330 * (1.0)
34331 * new_r00
34332 * sj10))))
34333 + IKsqr(
34334 (((cj10
34335 * new_r00))
34336 + (((-1.0)
34337 * sj10
34338 * x331))))
34339 - 1)
34340 <= IKFAST_SINCOS_THRESH)
34341 continue;
34342 j12array[0] = IKatan2(
34343 ((((-1.0) * cj10 * x331))
34344 + (((-1.0) * (1.0)
34345 * new_r00 * sj10))),
34346 (((cj10 * new_r00))
34347 + (((-1.0) * sj10
34348 * x331))));
34349 sj12array[0]
34350 = IKsin(j12array[0]);
34351 cj12array[0]
34352 = IKcos(j12array[0]);
34353 if (j12array[0] > IKPI)
34354 {
34355 j12array[0] -= IK2PI;
34356 }
34357 else if (j12array[0] < -IKPI)
34358 {
34359 j12array[0] += IK2PI;
34360 }
34361 j12valid[0] = true;
34362 for (int ij12 = 0; ij12 < 1;
34363 ++ij12)
34364 {
34365 if (!j12valid[ij12])
34366 {
34367 continue;
34368 }
34369 _ij12[0] = ij12;
34370 _ij12[1] = -1;
34371 for (int iij12 = ij12 + 1;
34372 iij12 < 1;
34373 ++iij12)
34374 {
34375 if (j12valid[iij12]
34376 && IKabs(
34377 cj12array[ij12]
34378 - cj12array
34379 [iij12])
34380 < IKFAST_SOLUTION_THRESH
34381 && IKabs(
34382 sj12array[ij12]
34383 - sj12array
34384 [iij12])
34385 < IKFAST_SOLUTION_THRESH)
34386 {
34387 j12valid[iij12] = false;
34388 _ij12[1] = iij12;
34389 break;
34390 }
34391 }
34392 j12 = j12array[ij12];
34393 cj12 = cj12array[ij12];
34394 sj12 = sj12array[ij12];
34395 {
34396 IkReal evalcond[8];
34397 IkReal x332 = IKsin(j12);
34398 IkReal x333
34399 = (cj10 * x332);
34400 IkReal x334 = IKcos(j12);
34401 IkReal x335
34402 = ((1.0) * x334);
34403 IkReal x336
34404 = ((-1.0) * x335);
34405 IkReal x337
34406 = ((1.0) * sj10);
34407 IkReal x338
34408 = (((sj10 * x332))
34409 + (((-1.0) * cj10
34410 * x335)));
34411 evalcond[0]
34412 = (((cj10 * new_r01))
34413 + x332
34414 + ((new_r11
34415 * sj10)));
34416 evalcond[1]
34417 = (((sj10 * x334))
34418 + x333 + new_r01);
34419 evalcond[2]
34420 = (((cj10 * new_r00))
34421 + ((new_r10
34422 * sj10))
34423 + x336);
34424 evalcond[3]
34425 = (((cj10 * new_r10))
34426 + (((-1.0) * x332))
34427 + (((-1.0)
34428 * new_r00
34429 * x337)));
34430 evalcond[4]
34431 = (((cj10 * new_r11))
34432 + x336
34433 + (((-1.0)
34434 * new_r01
34435 * x337)));
34436 evalcond[5]
34437 = (x338 + new_r00);
34438 evalcond[6]
34439 = (x338 + new_r11);
34440 evalcond[7]
34441 = ((((-1.0) * x334
34442 * x337))
34443 + (((-1.0) * x333))
34444 + new_r10);
34445 if (IKabs(evalcond[0])
34446 > IKFAST_EVALCOND_THRESH
34447 || IKabs(evalcond[1])
34448 > IKFAST_EVALCOND_THRESH
34449 || IKabs(evalcond[2])
34450 > IKFAST_EVALCOND_THRESH
34451 || IKabs(evalcond[3])
34452 > IKFAST_EVALCOND_THRESH
34453 || IKabs(evalcond[4])
34454 > IKFAST_EVALCOND_THRESH
34455 || IKabs(evalcond[5])
34456 > IKFAST_EVALCOND_THRESH
34457 || IKabs(evalcond[6])
34458 > IKFAST_EVALCOND_THRESH
34459 || IKabs(evalcond[7])
34460 > IKFAST_EVALCOND_THRESH)
34461 {
34462 continue;
34463 }
34464 }
34465
34466 {
34467 std::vector<
34468 IkSingleDOFSolutionBase<
34469 IkReal> >
34470 vinfos(7);
34471 vinfos[0].jointtype = 1;
34472 vinfos[0].foffset = j4;
34473 vinfos[0].indices[0]
34474 = _ij4[0];
34475 vinfos[0].indices[1]
34476 = _ij4[1];
34477 vinfos[0].maxsolutions
34478 = _nj4;
34479 vinfos[1].jointtype = 1;
34480 vinfos[1].foffset = j6;
34481 vinfos[1].indices[0]
34482 = _ij6[0];
34483 vinfos[1].indices[1]
34484 = _ij6[1];
34485 vinfos[1].maxsolutions
34486 = _nj6;
34487 vinfos[2].jointtype = 1;
34488 vinfos[2].foffset = j8;
34489 vinfos[2].indices[0]
34490 = _ij8[0];
34491 vinfos[2].indices[1]
34492 = _ij8[1];
34493 vinfos[2].maxsolutions
34494 = _nj8;
34495 vinfos[3].jointtype = 1;
34496 vinfos[3].foffset = j9;
34497 vinfos[3].indices[0]
34498 = _ij9[0];
34499 vinfos[3].indices[1]
34500 = _ij9[1];
34501 vinfos[3].maxsolutions
34502 = _nj9;
34503 vinfos[4].jointtype = 1;
34504 vinfos[4].foffset = j10;
34505 vinfos[4].indices[0]
34506 = _ij10[0];
34507 vinfos[4].indices[1]
34508 = _ij10[1];
34509 vinfos[4].maxsolutions
34510 = _nj10;
34511 vinfos[5].jointtype = 1;
34512 vinfos[5].foffset = j11;
34513 vinfos[5].indices[0]
34514 = _ij11[0];
34515 vinfos[5].indices[1]
34516 = _ij11[1];
34517 vinfos[5].maxsolutions
34518 = _nj11;
34519 vinfos[6].jointtype = 1;
34520 vinfos[6].foffset = j12;
34521 vinfos[6].indices[0]
34522 = _ij12[0];
34523 vinfos[6].indices[1]
34524 = _ij12[1];
34525 vinfos[6].maxsolutions
34526 = _nj12;
34527 std::vector<int> vfree(0);
34528 solutions.AddSolution(
34529 vinfos, vfree);
34530 }
34531 }
34532 }
34533 }
34534 } while (0);
34535 if (bgotonextstatement)
34536 {
34537 bool bgotonextstatement = true;
34538 do
34539 {
34540 IkReal x339
34541 = ((((-1.0) * (1.0)
34542 * new_r02 * sj10))
34543 + ((cj10 * new_r12)));
34544 IkReal x340 = (cj10 * new_r02);
34545 IkReal x341 = (new_r12 * sj10);
34546 evalcond[0]
34547 = ((-3.14159265358979)
34548 + (IKfmod(
34549 ((3.14159265358979)
34550 + (IKabs((
34551 (-3.14159265358979)
34552 + j11)))),
34553 6.28318530717959)));
34554 evalcond[1] = ((1.0) + new_r22);
34555 evalcond[2] = new_r20;
34556 evalcond[3] = new_r02;
34557 evalcond[4] = new_r12;
34558 evalcond[5] = new_r21;
34559 evalcond[6] = x339;
34560 evalcond[7] = x339;
34561 evalcond[8] = (x340 + x341);
34562 evalcond[9]
34563 = ((((-1.0) * x341))
34564 + (((-1.0) * x340)));
34565 if (IKabs(evalcond[0])
34566 < 0.0000010000000000
34567 && IKabs(evalcond[1])
34568 < 0.0000010000000000
34569 && IKabs(evalcond[2])
34570 < 0.0000010000000000
34571 && IKabs(evalcond[3])
34572 < 0.0000010000000000
34573 && IKabs(evalcond[4])
34574 < 0.0000010000000000
34575 && IKabs(evalcond[5])
34576 < 0.0000010000000000
34577 && IKabs(evalcond[6])
34578 < 0.0000010000000000
34579 && IKabs(evalcond[7])
34580 < 0.0000010000000000
34581 && IKabs(evalcond[8])
34582 < 0.0000010000000000
34583 && IKabs(evalcond[9])
34584 < 0.0000010000000000)
34585 {
34586 bgotonextstatement = false;
34587 {
34588 IkReal j12array[1],
34589 cj12array[1],
34590 sj12array[1];
34591 bool j12valid[1] = {false};
34592 _nj12 = 1;
34593 IkReal x342
34594 = ((1.0) * new_r00);
34595 if (IKabs(
34596 (((cj10 * new_r01))
34597 + (((-1.0) * sj10
34598 * x342))))
34599 < IKFAST_ATAN2_MAGTHRESH
34600 && IKabs(
34601 ((((-1.0) * cj10
34602 * x342))
34603 + (((-1.0)
34604 * (1.0)
34605 * new_r01
34606 * sj10))))
34607 < IKFAST_ATAN2_MAGTHRESH
34608 && IKabs(
34609 IKsqr((
34610 ((cj10
34611 * new_r01))
34612 + (((-1.0)
34613 * sj10
34614 * x342))))
34615 + IKsqr((
34616 (((-1.0)
34617 * cj10
34618 * x342))
34619 + (((-1.0)
34620 * (1.0)
34621 * new_r01
34622 * sj10))))
34623 - 1)
34624 <= IKFAST_SINCOS_THRESH)
34625 continue;
34626 j12array[0] = IKatan2(
34627 (((cj10 * new_r01))
34628 + (((-1.0) * sj10
34629 * x342))),
34630 ((((-1.0) * cj10
34631 * x342))
34632 + (((-1.0) * (1.0)
34633 * new_r01
34634 * sj10))));
34635 sj12array[0]
34636 = IKsin(j12array[0]);
34637 cj12array[0]
34638 = IKcos(j12array[0]);
34639 if (j12array[0] > IKPI)
34640 {
34641 j12array[0] -= IK2PI;
34642 }
34643 else if (
34644 j12array[0] < -IKPI)
34645 {
34646 j12array[0] += IK2PI;
34647 }
34648 j12valid[0] = true;
34649 for (int ij12 = 0; ij12 < 1;
34650 ++ij12)
34651 {
34652 if (!j12valid[ij12])
34653 {
34654 continue;
34655 }
34656 _ij12[0] = ij12;
34657 _ij12[1] = -1;
34658 for (int iij12 = ij12 + 1;
34659 iij12 < 1;
34660 ++iij12)
34661 {
34662 if (j12valid[iij12]
34663 && IKabs(
34664 cj12array
34665 [ij12]
34666 - cj12array
34667 [iij12])
34668 < IKFAST_SOLUTION_THRESH
34669 && IKabs(
34670 sj12array
34671 [ij12]
34672 - sj12array
34673 [iij12])
34674 < IKFAST_SOLUTION_THRESH)
34675 {
34676 j12valid[iij12]
34677 = false;
34678 _ij12[1] = iij12;
34679 break;
34680 }
34681 }
34682 j12 = j12array[ij12];
34683 cj12 = cj12array[ij12];
34684 sj12 = sj12array[ij12];
34685 {
34686 IkReal evalcond[8];
34687 IkReal x343
34688 = IKcos(j12);
34689 IkReal x344
34690 = IKsin(j12);
34691 IkReal x345
34692 = ((1.0) * x344);
34693 IkReal x346
34694 = ((-1.0) * x345);
34695 IkReal x347
34696 = (cj10 * x343);
34697 IkReal x348
34698 = ((1.0) * sj10);
34699 IkReal x349
34700 = (((sj10 * x343))
34701 + (((-1.0) * cj10
34702 * x345)));
34703 evalcond[0]
34704 = (((cj10
34705 * new_r00))
34706 + ((new_r10
34707 * sj10))
34708 + x343);
34709 evalcond[1]
34710 = (((cj10
34711 * new_r01))
34712 + ((new_r11
34713 * sj10))
34714 + x346);
34715 evalcond[2]
34716 = (((sj10 * x344))
34717 + x347
34718 + new_r00);
34719 evalcond[3]
34720 = (((cj10
34721 * new_r10))
34722 + (((-1.0)
34723 * new_r00
34724 * x348))
34725 + x346);
34726 evalcond[4]
34727 = (((cj10
34728 * new_r11))
34729 + (((-1.0)
34730 * new_r01
34731 * x348))
34732 + (((-1.0)
34733 * x343)));
34734 evalcond[5]
34735 = (x349 + new_r01);
34736 evalcond[6]
34737 = (new_r10 + x349);
34738 evalcond[7]
34739 = ((((-1.0) * x347))
34740 + (((-1.0) * x344
34741 * x348))
34742 + new_r11);
34743 if (IKabs(evalcond[0])
34744 > IKFAST_EVALCOND_THRESH
34745 || IKabs(
34746 evalcond[1])
34747 > IKFAST_EVALCOND_THRESH
34748 || IKabs(
34749 evalcond[2])
34750 > IKFAST_EVALCOND_THRESH
34751 || IKabs(
34752 evalcond[3])
34753 > IKFAST_EVALCOND_THRESH
34754 || IKabs(
34755 evalcond[4])
34756 > IKFAST_EVALCOND_THRESH
34757 || IKabs(
34758 evalcond[5])
34759 > IKFAST_EVALCOND_THRESH
34760 || IKabs(
34761 evalcond[6])
34762 > IKFAST_EVALCOND_THRESH
34763 || IKabs(
34764 evalcond[7])
34765 > IKFAST_EVALCOND_THRESH)
34766 {
34767 continue;
34768 }
34769 }
34770
34771 {
34772 std::vector<
34773 IkSingleDOFSolutionBase<
34774 IkReal> >
34775 vinfos(7);
34776 vinfos[0].jointtype = 1;
34777 vinfos[0].foffset = j4;
34778 vinfos[0].indices[0]
34779 = _ij4[0];
34780 vinfos[0].indices[1]
34781 = _ij4[1];
34782 vinfos[0].maxsolutions
34783 = _nj4;
34784 vinfos[1].jointtype = 1;
34785 vinfos[1].foffset = j6;
34786 vinfos[1].indices[0]
34787 = _ij6[0];
34788 vinfos[1].indices[1]
34789 = _ij6[1];
34790 vinfos[1].maxsolutions
34791 = _nj6;
34792 vinfos[2].jointtype = 1;
34793 vinfos[2].foffset = j8;
34794 vinfos[2].indices[0]
34795 = _ij8[0];
34796 vinfos[2].indices[1]
34797 = _ij8[1];
34798 vinfos[2].maxsolutions
34799 = _nj8;
34800 vinfos[3].jointtype = 1;
34801 vinfos[3].foffset = j9;
34802 vinfos[3].indices[0]
34803 = _ij9[0];
34804 vinfos[3].indices[1]
34805 = _ij9[1];
34806 vinfos[3].maxsolutions
34807 = _nj9;
34808 vinfos[4].jointtype = 1;
34809 vinfos[4].foffset = j10;
34810 vinfos[4].indices[0]
34811 = _ij10[0];
34812 vinfos[4].indices[1]
34813 = _ij10[1];
34814 vinfos[4].maxsolutions
34815 = _nj10;
34816 vinfos[5].jointtype = 1;
34817 vinfos[5].foffset = j11;
34818 vinfos[5].indices[0]
34819 = _ij11[0];
34820 vinfos[5].indices[1]
34821 = _ij11[1];
34822 vinfos[5].maxsolutions
34823 = _nj11;
34824 vinfos[6].jointtype = 1;
34825 vinfos[6].foffset = j12;
34826 vinfos[6].indices[0]
34827 = _ij12[0];
34828 vinfos[6].indices[1]
34829 = _ij12[1];
34830 vinfos[6].maxsolutions
34831 = _nj12;
34832 std::vector<int> vfree(
34833 0);
34834 solutions.AddSolution(
34835 vinfos, vfree);
34836 }
34837 }
34838 }
34839 }
34840 } while (0);
34841 if (bgotonextstatement)
34842 {
34843 bool bgotonextstatement = true;
34844 do
34845 {
34846 IkReal x350 = ((1.0) * cj11);
34847 IkReal x351
34848 = ((((-1.0) * x350))
34849 + new_r22);
34850 IkReal x352 = ((1.0) * sj11);
34851 IkReal x353
34852 = ((((-1.0) * x352))
34853 + new_r02);
34854 evalcond[0]
34855 = ((-3.14159265358979)
34856 + (IKfmod(
34857 ((3.14159265358979)
34858 + (IKabs(j10))),
34859 6.28318530717959)));
34860 evalcond[1] = x351;
34861 evalcond[2] = x351;
34862 evalcond[3] = x353;
34863 evalcond[4] = new_r12;
34864 evalcond[5] = x353;
34865 evalcond[6]
34866 = ((((-1.0) * new_r22
34867 * x352))
34868 + ((cj11 * new_r02)));
34869 evalcond[7]
34870 = ((((-1.0) * new_r00
34871 * x352))
34872 + (((-1.0) * new_r20
34873 * x350)));
34874 evalcond[8]
34875 = ((((-1.0) * new_r01
34876 * x352))
34877 + (((-1.0) * new_r21
34878 * x350)));
34879 evalcond[9]
34880 = ((1.0)
34881 + (((-1.0) * new_r02
34882 * x352))
34883 + (((-1.0) * new_r22
34884 * x350)));
34885 if (IKabs(evalcond[0])
34886 < 0.0000010000000000
34887 && IKabs(evalcond[1])
34888 < 0.0000010000000000
34889 && IKabs(evalcond[2])
34890 < 0.0000010000000000
34891 && IKabs(evalcond[3])
34892 < 0.0000010000000000
34893 && IKabs(evalcond[4])
34894 < 0.0000010000000000
34895 && IKabs(evalcond[5])
34896 < 0.0000010000000000
34897 && IKabs(evalcond[6])
34898 < 0.0000010000000000
34899 && IKabs(evalcond[7])
34900 < 0.0000010000000000
34901 && IKabs(evalcond[8])
34902 < 0.0000010000000000
34903 && IKabs(evalcond[9])
34904 < 0.0000010000000000)
34905 {
34906 bgotonextstatement = false;
34907 {
34908 IkReal j12array[1],
34909 cj12array[1],
34910 sj12array[1];
34911 bool j12valid[1]
34912 = {false};
34913 _nj12 = 1;
34914 if (IKabs(new_r10)
34915 < IKFAST_ATAN2_MAGTHRESH
34916 && IKabs(new_r11)
34917 < IKFAST_ATAN2_MAGTHRESH
34918 && IKabs(
34919 IKsqr(new_r10)
34920 + IKsqr(
34921 new_r11)
34922 - 1)
34923 <= IKFAST_SINCOS_THRESH)
34924 continue;
34925 j12array[0] = IKatan2(
34926 new_r10, new_r11);
34927 sj12array[0]
34928 = IKsin(j12array[0]);
34929 cj12array[0]
34930 = IKcos(j12array[0]);
34931 if (j12array[0] > IKPI)
34932 {
34933 j12array[0] -= IK2PI;
34934 }
34935 else if (
34936 j12array[0] < -IKPI)
34937 {
34938 j12array[0] += IK2PI;
34939 }
34940 j12valid[0] = true;
34941 for (int ij12 = 0;
34942 ij12 < 1;
34943 ++ij12)
34944 {
34945 if (!j12valid[ij12])
34946 {
34947 continue;
34948 }
34949 _ij12[0] = ij12;
34950 _ij12[1] = -1;
34951 for (int iij12
34952 = ij12 + 1;
34953 iij12 < 1;
34954 ++iij12)
34955 {
34956 if (j12valid[iij12]
34957 && IKabs(
34958 cj12array
34959 [ij12]
34960 - cj12array
34961 [iij12])
34962 < IKFAST_SOLUTION_THRESH
34963 && IKabs(
34964 sj12array
34965 [ij12]
34966 - sj12array
34967 [iij12])
34968 < IKFAST_SOLUTION_THRESH)
34969 {
34970 j12valid[iij12]
34971 = false;
34972 _ij12[1] = iij12;
34973 break;
34974 }
34975 }
34976 j12 = j12array[ij12];
34977 cj12 = cj12array[ij12];
34978 sj12 = sj12array[ij12];
34979 {
34980 IkReal evalcond[8];
34981 IkReal x354
34982 = IKcos(j12);
34983 IkReal x355
34984 = IKsin(j12);
34985 IkReal x356
34986 = ((1.0) * x354);
34987 IkReal x357
34988 = ((-1.0) * x356);
34989 IkReal x358
34990 = ((1.0)
34991 * new_r02);
34992 evalcond[0]
34993 = (((new_r02
34994 * x354))
34995 + new_r20);
34996 evalcond[1]
34997 = ((((-1.0)
34998 * x355))
34999 + new_r10);
35000 evalcond[2]
35001 = (x357
35002 + new_r11);
35003 evalcond[3]
35004 = (((new_r22
35005 * x355))
35006 + new_r01);
35007 evalcond[4]
35008 = ((((-1.0) * x355
35009 * x358))
35010 + new_r21);
35011 evalcond[5]
35012 = ((((-1.0)
35013 * new_r22
35014 * x356))
35015 + new_r00);
35016 evalcond[6]
35017 = (x355
35018 + ((new_r01
35019 * new_r22))
35020 + (((-1.0)
35021 * new_r21
35022 * x358)));
35023 evalcond[7]
35024 = (x357
35025 + ((new_r00
35026 * new_r22))
35027 + (((-1.0)
35028 * new_r20
35029 * x358)));
35030 if (IKabs(evalcond[0])
35031 > IKFAST_EVALCOND_THRESH
35032 || IKabs(evalcond
35033 [1])
35034 > IKFAST_EVALCOND_THRESH
35035 || IKabs(evalcond
35036 [2])
35037 > IKFAST_EVALCOND_THRESH
35038 || IKabs(evalcond
35039 [3])
35040 > IKFAST_EVALCOND_THRESH
35041 || IKabs(evalcond
35042 [4])
35043 > IKFAST_EVALCOND_THRESH
35044 || IKabs(evalcond
35045 [5])
35046 > IKFAST_EVALCOND_THRESH
35047 || IKabs(evalcond
35048 [6])
35049 > IKFAST_EVALCOND_THRESH
35050 || IKabs(evalcond
35051 [7])
35052 > IKFAST_EVALCOND_THRESH)
35053 {
35054 continue;
35055 }
35056 }
35057
35058 {
35059 std::vector<
35060 IkSingleDOFSolutionBase<
35061 IkReal> >
35062 vinfos(7);
35063 vinfos[0].jointtype
35064 = 1;
35065 vinfos[0].foffset
35066 = j4;
35067 vinfos[0].indices[0]
35068 = _ij4[0];
35069 vinfos[0].indices[1]
35070 = _ij4[1];
35071 vinfos[0].maxsolutions
35072 = _nj4;
35073 vinfos[1].jointtype
35074 = 1;
35075 vinfos[1].foffset
35076 = j6;
35077 vinfos[1].indices[0]
35078 = _ij6[0];
35079 vinfos[1].indices[1]
35080 = _ij6[1];
35081 vinfos[1].maxsolutions
35082 = _nj6;
35083 vinfos[2].jointtype
35084 = 1;
35085 vinfos[2].foffset
35086 = j8;
35087 vinfos[2].indices[0]
35088 = _ij8[0];
35089 vinfos[2].indices[1]
35090 = _ij8[1];
35091 vinfos[2].maxsolutions
35092 = _nj8;
35093 vinfos[3].jointtype
35094 = 1;
35095 vinfos[3].foffset
35096 = j9;
35097 vinfos[3].indices[0]
35098 = _ij9[0];
35099 vinfos[3].indices[1]
35100 = _ij9[1];
35101 vinfos[3].maxsolutions
35102 = _nj9;
35103 vinfos[4].jointtype
35104 = 1;
35105 vinfos[4].foffset
35106 = j10;
35107 vinfos[4].indices[0]
35108 = _ij10[0];
35109 vinfos[4].indices[1]
35110 = _ij10[1];
35111 vinfos[4].maxsolutions
35112 = _nj10;
35113 vinfos[5].jointtype
35114 = 1;
35115 vinfos[5].foffset
35116 = j11;
35117 vinfos[5].indices[0]
35118 = _ij11[0];
35119 vinfos[5].indices[1]
35120 = _ij11[1];
35121 vinfos[5].maxsolutions
35122 = _nj11;
35123 vinfos[6].jointtype
35124 = 1;
35125 vinfos[6].foffset
35126 = j12;
35127 vinfos[6].indices[0]
35128 = _ij12[0];
35129 vinfos[6].indices[1]
35130 = _ij12[1];
35131 vinfos[6].maxsolutions
35132 = _nj12;
35133 std::vector<int>
35134 vfree(0);
35135 solutions.AddSolution(
35136 vinfos, vfree);
35137 }
35138 }
35139 }
35140 }
35141 } while (0);
35142 if (bgotonextstatement)
35143 {
35144 bool bgotonextstatement
35145 = true;
35146 do
35147 {
35148 IkReal x359
35149 = ((1.0) * cj11);
35150 IkReal x360
35151 = ((((-1.0) * x359))
35152 + new_r22);
35153 IkReal x361
35154 = ((1.0) * sj11);
35155 evalcond[0]
35156 = ((-3.14159265358979)
35157 + (IKfmod(
35158 ((3.14159265358979)
35159 + (IKabs((
35160 (-3.14159265358979)
35161 + j10)))),
35162 6.28318530717959)));
35163 evalcond[1] = x360;
35164 evalcond[2] = x360;
35165 evalcond[3]
35166 = (sj11 + new_r02);
35167 evalcond[4] = new_r12;
35168 evalcond[5]
35169 = ((((-1.0) * (1.0)
35170 * new_r02))
35171 + (((-1.0) * x361)));
35172 evalcond[6]
35173 = ((((-1.0) * new_r02
35174 * x359))
35175 + (((-1.0) * new_r22
35176 * x361)));
35177 evalcond[7]
35178 = ((((-1.0) * new_r20
35179 * x359))
35180 + ((new_r00
35181 * sj11)));
35182 evalcond[8]
35183 = (((new_r01 * sj11))
35184 + (((-1.0) * new_r21
35185 * x359)));
35186 evalcond[9]
35187 = ((1.0)
35188 + (((-1.0) * new_r22
35189 * x359))
35190 + ((new_r02
35191 * sj11)));
35192 if (IKabs(evalcond[0])
35193 < 0.0000010000000000
35194 && IKabs(evalcond[1])
35195 < 0.0000010000000000
35196 && IKabs(evalcond[2])
35197 < 0.0000010000000000
35198 && IKabs(evalcond[3])
35199 < 0.0000010000000000
35200 && IKabs(evalcond[4])
35201 < 0.0000010000000000
35202 && IKabs(evalcond[5])
35203 < 0.0000010000000000
35204 && IKabs(evalcond[6])
35205 < 0.0000010000000000
35206 && IKabs(evalcond[7])
35207 < 0.0000010000000000
35208 && IKabs(evalcond[8])
35209 < 0.0000010000000000
35210 && IKabs(evalcond[9])
35211 < 0.0000010000000000)
35212 {
35213 bgotonextstatement
35214 = false;
35215 {
35216 IkReal j12array[1],
35217 cj12array[1],
35218 sj12array[1];
35219 bool j12valid[1]
35220 = {false};
35221 _nj12 = 1;
35222 CheckValue<IkReal> x362
35223 = IKatan2WithCheck(
35224 IkReal((
35225 (-1.0)
35226 * (((1.0)
35227 * new_r21)))),
35228 new_r20,
35229 IKFAST_ATAN2_MAGTHRESH);
35230 if (!x362.valid)
35231 {
35232 continue;
35233 }
35234 CheckValue<IkReal> x363
35235 = IKPowWithIntegerCheck(
35236 IKsign(new_r02),
35237 -1);
35238 if (!x363.valid)
35239 {
35240 continue;
35241 }
35242 j12array[0]
35243 = ((-1.5707963267949)
35244 + (x362.value)
35245 + (((1.5707963267949)
35246 * (x363.value))));
35247 sj12array[0] = IKsin(
35248 j12array[0]);
35249 cj12array[0] = IKcos(
35250 j12array[0]);
35251 if (j12array[0] > IKPI)
35252 {
35253 j12array[0] -= IK2PI;
35254 }
35255 else if (
35256 j12array[0] < -IKPI)
35257 {
35258 j12array[0] += IK2PI;
35259 }
35260 j12valid[0] = true;
35261 for (int ij12 = 0;
35262 ij12 < 1;
35263 ++ij12)
35264 {
35265 if (!j12valid[ij12])
35266 {
35267 continue;
35268 }
35269 _ij12[0] = ij12;
35270 _ij12[1] = -1;
35271 for (int iij12
35272 = ij12 + 1;
35273 iij12 < 1;
35274 ++iij12)
35275 {
35276 if (j12valid[iij12]
35277 && IKabs(
35278 cj12array
35279 [ij12]
35280 - cj12array
35281 [iij12])
35282 < IKFAST_SOLUTION_THRESH
35283 && IKabs(
35284 sj12array
35285 [ij12]
35286 - sj12array
35287 [iij12])
35288 < IKFAST_SOLUTION_THRESH)
35289 {
35290 j12valid[iij12]
35291 = false;
35292 _ij12[1] = iij12;
35293 break;
35294 }
35295 }
35296 j12 = j12array[ij12];
35297 cj12
35298 = cj12array[ij12];
35299 sj12
35300 = sj12array[ij12];
35301 {
35302 IkReal evalcond[8];
35303 IkReal x364
35304 = IKsin(j12);
35305 IkReal x365
35306 = ((1.0)
35307 * (IKcos(
35308 j12)));
35309 IkReal x366
35310 = ((-1.0)
35311 * x365);
35312 IkReal x367
35313 = ((1.0)
35314 * new_r01);
35315 IkReal x368
35316 = ((1.0)
35317 * new_r00);
35318 evalcond[0]
35319 = (((new_r02
35320 * x364))
35321 + new_r21);
35322 evalcond[1]
35323 = ((((-1.0)
35324 * new_r02
35325 * x365))
35326 + new_r20);
35327 evalcond[2]
35328 = ((((-1.0)
35329 * x364))
35330 + (((-1.0)
35331 * (1.0)
35332 * new_r10)));
35333 evalcond[3]
35334 = (x366
35335 + (((-1.0)
35336 * (1.0)
35337 * new_r11)));
35338 evalcond[4]
35339 = ((((-1.0)
35340 * x367))
35341 + ((new_r22
35342 * x364)));
35343 evalcond[5]
35344 = ((((-1.0)
35345 * new_r22
35346 * x365))
35347 + (((-1.0)
35348 * x368)));
35349 evalcond[6]
35350 = (((new_r02
35351 * new_r21))
35352 + x364
35353 + (((-1.0)
35354 * new_r22
35355 * x367)));
35356 evalcond[7]
35357 = (((new_r02
35358 * new_r20))
35359 + x366
35360 + (((-1.0)
35361 * new_r22
35362 * x368)));
35363 if (IKabs(
35364 evalcond[0])
35365 > IKFAST_EVALCOND_THRESH
35366 || IKabs(
35367 evalcond
35368 [1])
35369 > IKFAST_EVALCOND_THRESH
35370 || IKabs(
35371 evalcond
35372 [2])
35373 > IKFAST_EVALCOND_THRESH
35374 || IKabs(
35375 evalcond
35376 [3])
35377 > IKFAST_EVALCOND_THRESH
35378 || IKabs(
35379 evalcond
35380 [4])
35381 > IKFAST_EVALCOND_THRESH
35382 || IKabs(
35383 evalcond
35384 [5])
35385 > IKFAST_EVALCOND_THRESH
35386 || IKabs(
35387 evalcond
35388 [6])
35389 > IKFAST_EVALCOND_THRESH
35390 || IKabs(
35391 evalcond
35392 [7])
35393 > IKFAST_EVALCOND_THRESH)
35394 {
35395 continue;
35396 }
35397 }
35398
35399 {
35400 std::vector<
35401 IkSingleDOFSolutionBase<
35402 IkReal> >
35403 vinfos(7);
35404 vinfos[0].jointtype
35405 = 1;
35406 vinfos[0].foffset
35407 = j4;
35408 vinfos[0].indices[0]
35409 = _ij4[0];
35410 vinfos[0].indices[1]
35411 = _ij4[1];
35412 vinfos[0]
35413 .maxsolutions
35414 = _nj4;
35415 vinfos[1].jointtype
35416 = 1;
35417 vinfos[1].foffset
35418 = j6;
35419 vinfos[1].indices[0]
35420 = _ij6[0];
35421 vinfos[1].indices[1]
35422 = _ij6[1];
35423 vinfos[1]
35424 .maxsolutions
35425 = _nj6;
35426 vinfos[2].jointtype
35427 = 1;
35428 vinfos[2].foffset
35429 = j8;
35430 vinfos[2].indices[0]
35431 = _ij8[0];
35432 vinfos[2].indices[1]
35433 = _ij8[1];
35434 vinfos[2]
35435 .maxsolutions
35436 = _nj8;
35437 vinfos[3].jointtype
35438 = 1;
35439 vinfos[3].foffset
35440 = j9;
35441 vinfos[3].indices[0]
35442 = _ij9[0];
35443 vinfos[3].indices[1]
35444 = _ij9[1];
35445 vinfos[3]
35446 .maxsolutions
35447 = _nj9;
35448 vinfos[4].jointtype
35449 = 1;
35450 vinfos[4].foffset
35451 = j10;
35452 vinfos[4].indices[0]
35453 = _ij10[0];
35454 vinfos[4].indices[1]
35455 = _ij10[1];
35456 vinfos[4]
35457 .maxsolutions
35458 = _nj10;
35459 vinfos[5].jointtype
35460 = 1;
35461 vinfos[5].foffset
35462 = j11;
35463 vinfos[5].indices[0]
35464 = _ij11[0];
35465 vinfos[5].indices[1]
35466 = _ij11[1];
35467 vinfos[5]
35468 .maxsolutions
35469 = _nj11;
35470 vinfos[6].jointtype
35471 = 1;
35472 vinfos[6].foffset
35473 = j12;
35474 vinfos[6].indices[0]
35475 = _ij12[0];
35476 vinfos[6].indices[1]
35477 = _ij12[1];
35478 vinfos[6]
35479 .maxsolutions
35480 = _nj12;
35481 std::vector<int>
35482 vfree(0);
35483 solutions
35484 .AddSolution(
35485 vinfos,
35486 vfree);
35487 }
35488 }
35489 }
35490 }
35491 } while (0);
35492 if (bgotonextstatement)
35493 {
35494 bool bgotonextstatement
35495 = true;
35496 do
35497 {
35498 if (1)
35499 {
35500 bgotonextstatement
35501 = false;
35502 continue; // branch miss
35503 // [j12]
35504 }
35505 } while (0);
35506 if (bgotonextstatement)
35507 {
35508 }
35509 }
35510 }
35511 }
35512 }
35513 }
35514 }
35515 }
35516 }
35517 }
35518 }
35519 else
35520 {
35521 {
35522 IkReal j12array[1], cj12array[1],
35523 sj12array[1];
35524 bool j12valid[1] = {false};
35525 _nj12 = 1;
35526 CheckValue<IkReal> x370
35527 = IKPowWithIntegerCheck(sj11, -1);
35528 if (!x370.valid)
35529 {
35530 continue;
35531 }
35532 IkReal x369 = x370.value;
35533 CheckValue<IkReal> x371
35534 = IKPowWithIntegerCheck(cj10, -1);
35535 if (!x371.valid)
35536 {
35537 continue;
35538 }
35539 CheckValue<IkReal> x372
35540 = IKPowWithIntegerCheck(cj11, -1);
35541 if (!x372.valid)
35542 {
35543 continue;
35544 }
35545 if (IKabs(
35546 (x369 * (x371.value) * (x372.value)
35547 * (((((-1.0) * (1.0) * new_r01
35548 * sj11))
35549 + ((new_r20 * sj10))))))
35550 < IKFAST_ATAN2_MAGTHRESH
35551 && IKabs(((-1.0) * new_r20 * x369))
35552 < IKFAST_ATAN2_MAGTHRESH
35553 && IKabs(
35554 IKsqr(
35555 (x369 * (x371.value)
35556 * (x372.value)
35557 * (((((-1.0) * (1.0)
35558 * new_r01 * sj11))
35559 + ((new_r20 * sj10))))))
35560 + IKsqr(
35561 ((-1.0) * new_r20 * x369))
35562 - 1)
35563 <= IKFAST_SINCOS_THRESH)
35564 continue;
35565 j12array[0] = IKatan2(
35566 (x369 * (x371.value) * (x372.value)
35567 * (((((-1.0) * (1.0) * new_r01 * sj11))
35568 + ((new_r20 * sj10))))),
35569 ((-1.0) * new_r20 * x369));
35570 sj12array[0] = IKsin(j12array[0]);
35571 cj12array[0] = IKcos(j12array[0]);
35572 if (j12array[0] > IKPI)
35573 {
35574 j12array[0] -= IK2PI;
35575 }
35576 else if (j12array[0] < -IKPI)
35577 {
35578 j12array[0] += IK2PI;
35579 }
35580 j12valid[0] = true;
35581 for (int ij12 = 0; ij12 < 1; ++ij12)
35582 {
35583 if (!j12valid[ij12])
35584 {
35585 continue;
35586 }
35587 _ij12[0] = ij12;
35588 _ij12[1] = -1;
35589 for (int iij12 = ij12 + 1; iij12 < 1;
35590 ++iij12)
35591 {
35592 if (j12valid[iij12]
35593 && IKabs(
35594 cj12array[ij12]
35595 - cj12array[iij12])
35596 < IKFAST_SOLUTION_THRESH
35597 && IKabs(
35598 sj12array[ij12]
35599 - sj12array[iij12])
35600 < IKFAST_SOLUTION_THRESH)
35601 {
35602 j12valid[iij12] = false;
35603 _ij12[1] = iij12;
35604 break;
35605 }
35606 }
35607 j12 = j12array[ij12];
35608 cj12 = cj12array[ij12];
35609 sj12 = sj12array[ij12];
35610 {
35611 IkReal evalcond[12];
35612 IkReal x373 = IKcos(j12);
35613 IkReal x374 = IKsin(j12);
35614 IkReal x375 = ((1.0) * sj11);
35615 IkReal x376 = (cj10 * new_r01);
35616 IkReal x377 = (new_r11 * sj10);
35617 IkReal x378 = (cj11 * x374);
35618 IkReal x379 = ((1.0) * sj10);
35619 IkReal x380 = ((1.0) * x374);
35620 IkReal x381 = ((1.0) * x373);
35621 IkReal x382 = ((-1.0) * x381);
35622 IkReal x383 = (cj10 * new_r00);
35623 IkReal x384 = (new_r10 * sj10);
35624 IkReal x385 = (cj10 * x381);
35625 evalcond[0]
35626 = (((sj11 * x373)) + new_r20);
35627 evalcond[1]
35628 = ((((-1.0) * x374 * x375))
35629 + new_r21);
35630 evalcond[2] = (x378 + x377 + x376);
35631 evalcond[3]
35632 = (((cj10 * new_r10))
35633 + (((-1.0) * x380))
35634 + (((-1.0) * new_r00 * x379)));
35635 evalcond[4]
35636 = (((cj10 * new_r11)) + x382
35637 + (((-1.0) * new_r01 * x379)));
35638 evalcond[5]
35639 = (((sj10 * x373)) + ((cj10 * x378))
35640 + new_r01);
35641 evalcond[6]
35642 = ((((-1.0) * cj11 * x381)) + x384
35643 + x383);
35644 evalcond[7]
35645 = (((sj10 * x374))
35646 + (((-1.0) * cj11 * x385))
35647 + new_r00);
35648 evalcond[8]
35649 = ((((-1.0) * x385)) + new_r11
35650 + ((sj10 * x378)));
35651 evalcond[9]
35652 = ((((-1.0) * cj10 * x380))
35653 + (((-1.0) * cj11 * x373 * x379))
35654 + new_r10);
35655 evalcond[10]
35656 = (((cj11 * x376)) + ((cj11 * x377))
35657 + (((-1.0) * new_r21 * x375))
35658 + x374);
35659 evalcond[11]
35660 = ((((-1.0) * new_r20 * x375))
35661 + x382 + ((cj11 * x384))
35662 + ((cj11 * x383)));
35663 if (IKabs(evalcond[0])
35664 > IKFAST_EVALCOND_THRESH
35665 || IKabs(evalcond[1])
35666 > IKFAST_EVALCOND_THRESH
35667 || IKabs(evalcond[2])
35668 > IKFAST_EVALCOND_THRESH
35669 || IKabs(evalcond[3])
35670 > IKFAST_EVALCOND_THRESH
35671 || IKabs(evalcond[4])
35672 > IKFAST_EVALCOND_THRESH
35673 || IKabs(evalcond[5])
35674 > IKFAST_EVALCOND_THRESH
35675 || IKabs(evalcond[6])
35676 > IKFAST_EVALCOND_THRESH
35677 || IKabs(evalcond[7])
35678 > IKFAST_EVALCOND_THRESH
35679 || IKabs(evalcond[8])
35680 > IKFAST_EVALCOND_THRESH
35681 || IKabs(evalcond[9])
35682 > IKFAST_EVALCOND_THRESH
35683 || IKabs(evalcond[10])
35684 > IKFAST_EVALCOND_THRESH
35685 || IKabs(evalcond[11])
35686 > IKFAST_EVALCOND_THRESH)
35687 {
35688 continue;
35689 }
35690 }
35691
35692 {
35693 std::vector<
35694 IkSingleDOFSolutionBase<IkReal> >
35695 vinfos(7);
35696 vinfos[0].jointtype = 1;
35697 vinfos[0].foffset = j4;
35698 vinfos[0].indices[0] = _ij4[0];
35699 vinfos[0].indices[1] = _ij4[1];
35700 vinfos[0].maxsolutions = _nj4;
35701 vinfos[1].jointtype = 1;
35702 vinfos[1].foffset = j6;
35703 vinfos[1].indices[0] = _ij6[0];
35704 vinfos[1].indices[1] = _ij6[1];
35705 vinfos[1].maxsolutions = _nj6;
35706 vinfos[2].jointtype = 1;
35707 vinfos[2].foffset = j8;
35708 vinfos[2].indices[0] = _ij8[0];
35709 vinfos[2].indices[1] = _ij8[1];
35710 vinfos[2].maxsolutions = _nj8;
35711 vinfos[3].jointtype = 1;
35712 vinfos[3].foffset = j9;
35713 vinfos[3].indices[0] = _ij9[0];
35714 vinfos[3].indices[1] = _ij9[1];
35715 vinfos[3].maxsolutions = _nj9;
35716 vinfos[4].jointtype = 1;
35717 vinfos[4].foffset = j10;
35718 vinfos[4].indices[0] = _ij10[0];
35719 vinfos[4].indices[1] = _ij10[1];
35720 vinfos[4].maxsolutions = _nj10;
35721 vinfos[5].jointtype = 1;
35722 vinfos[5].foffset = j11;
35723 vinfos[5].indices[0] = _ij11[0];
35724 vinfos[5].indices[1] = _ij11[1];
35725 vinfos[5].maxsolutions = _nj11;
35726 vinfos[6].jointtype = 1;
35727 vinfos[6].foffset = j12;
35728 vinfos[6].indices[0] = _ij12[0];
35729 vinfos[6].indices[1] = _ij12[1];
35730 vinfos[6].maxsolutions = _nj12;
35731 std::vector<int> vfree(0);
35732 solutions.AddSolution(vinfos, vfree);
35733 }
35734 }
35735 }
35736 }
35737 }
35738 }
35739 else
35740 {
35741 {
35742 IkReal j12array[1], cj12array[1], sj12array[1];
35743 bool j12valid[1] = {false};
35744 _nj12 = 1;
35745 CheckValue<IkReal> x388
35746 = IKPowWithIntegerCheck(sj11, -1);
35747 if (!x388.valid)
35748 {
35749 continue;
35750 }
35751 IkReal x386 = x388.value;
35752 IkReal x387 = ((1.0) * new_r20);
35753 CheckValue<IkReal> x389
35754 = IKPowWithIntegerCheck(sj10, -1);
35755 if (!x389.valid)
35756 {
35757 continue;
35758 }
35759 if (IKabs(
35760 (x386 * (x389.value)
35761 * (((((-1.0) * cj10 * cj11 * x387))
35762 + (((-1.0) * (1.0) * new_r00
35763 * sj11))))))
35764 < IKFAST_ATAN2_MAGTHRESH
35765 && IKabs(((-1.0) * x386 * x387))
35766 < IKFAST_ATAN2_MAGTHRESH
35767 && IKabs(
35768 IKsqr(
35769 (x386 * (x389.value)
35770 * (((((-1.0) * cj10 * cj11
35771 * x387))
35772 + (((-1.0) * (1.0) * new_r00
35773 * sj11))))))
35774 + IKsqr(((-1.0) * x386 * x387)) - 1)
35775 <= IKFAST_SINCOS_THRESH)
35776 continue;
35777 j12array[0] = IKatan2(
35778 (x386 * (x389.value)
35779 * (((((-1.0) * cj10 * cj11 * x387))
35780 + (((-1.0) * (1.0) * new_r00
35781 * sj11))))),
35782 ((-1.0) * x386 * x387));
35783 sj12array[0] = IKsin(j12array[0]);
35784 cj12array[0] = IKcos(j12array[0]);
35785 if (j12array[0] > IKPI)
35786 {
35787 j12array[0] -= IK2PI;
35788 }
35789 else if (j12array[0] < -IKPI)
35790 {
35791 j12array[0] += IK2PI;
35792 }
35793 j12valid[0] = true;
35794 for (int ij12 = 0; ij12 < 1; ++ij12)
35795 {
35796 if (!j12valid[ij12])
35797 {
35798 continue;
35799 }
35800 _ij12[0] = ij12;
35801 _ij12[1] = -1;
35802 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
35803 {
35804 if (j12valid[iij12]
35805 && IKabs(
35806 cj12array[ij12]
35807 - cj12array[iij12])
35808 < IKFAST_SOLUTION_THRESH
35809 && IKabs(
35810 sj12array[ij12]
35811 - sj12array[iij12])
35812 < IKFAST_SOLUTION_THRESH)
35813 {
35814 j12valid[iij12] = false;
35815 _ij12[1] = iij12;
35816 break;
35817 }
35818 }
35819 j12 = j12array[ij12];
35820 cj12 = cj12array[ij12];
35821 sj12 = sj12array[ij12];
35822 {
35823 IkReal evalcond[12];
35824 IkReal x390 = IKcos(j12);
35825 IkReal x391 = IKsin(j12);
35826 IkReal x392 = ((1.0) * sj11);
35827 IkReal x393 = (cj10 * new_r01);
35828 IkReal x394 = (new_r11 * sj10);
35829 IkReal x395 = (cj11 * x391);
35830 IkReal x396 = ((1.0) * sj10);
35831 IkReal x397 = ((1.0) * x391);
35832 IkReal x398 = ((1.0) * x390);
35833 IkReal x399 = ((-1.0) * x398);
35834 IkReal x400 = (cj10 * new_r00);
35835 IkReal x401 = (new_r10 * sj10);
35836 IkReal x402 = (cj10 * x398);
35837 evalcond[0] = (((sj11 * x390)) + new_r20);
35838 evalcond[1]
35839 = ((((-1.0) * x391 * x392)) + new_r21);
35840 evalcond[2] = (x394 + x395 + x393);
35841 evalcond[3]
35842 = (((cj10 * new_r10))
35843 + (((-1.0) * x397))
35844 + (((-1.0) * new_r00 * x396)));
35845 evalcond[4]
35846 = (((cj10 * new_r11)) + x399
35847 + (((-1.0) * new_r01 * x396)));
35848 evalcond[5]
35849 = (((cj10 * x395)) + ((sj10 * x390))
35850 + new_r01);
35851 evalcond[6]
35852 = (x401 + x400
35853 + (((-1.0) * cj11 * x398)));
35854 evalcond[7]
35855 = (new_r00 + (((-1.0) * cj11 * x402))
35856 + ((sj10 * x391)));
35857 evalcond[8]
35858 = ((((-1.0) * x402)) + ((sj10 * x395))
35859 + new_r11);
35860 evalcond[9]
35861 = ((((-1.0) * cj10 * x397)) + new_r10
35862 + (((-1.0) * cj11 * x390 * x396)));
35863 evalcond[10]
35864 = (x391 + ((cj11 * x393))
35865 + ((cj11 * x394))
35866 + (((-1.0) * new_r21 * x392)));
35867 evalcond[11]
35868 = (x399 + ((cj11 * x400))
35869 + (((-1.0) * new_r20 * x392))
35870 + ((cj11 * x401)));
35871 if (IKabs(evalcond[0])
35872 > IKFAST_EVALCOND_THRESH
35873 || IKabs(evalcond[1])
35874 > IKFAST_EVALCOND_THRESH
35875 || IKabs(evalcond[2])
35876 > IKFAST_EVALCOND_THRESH
35877 || IKabs(evalcond[3])
35878 > IKFAST_EVALCOND_THRESH
35879 || IKabs(evalcond[4])
35880 > IKFAST_EVALCOND_THRESH
35881 || IKabs(evalcond[5])
35882 > IKFAST_EVALCOND_THRESH
35883 || IKabs(evalcond[6])
35884 > IKFAST_EVALCOND_THRESH
35885 || IKabs(evalcond[7])
35886 > IKFAST_EVALCOND_THRESH
35887 || IKabs(evalcond[8])
35888 > IKFAST_EVALCOND_THRESH
35889 || IKabs(evalcond[9])
35890 > IKFAST_EVALCOND_THRESH
35891 || IKabs(evalcond[10])
35892 > IKFAST_EVALCOND_THRESH
35893 || IKabs(evalcond[11])
35894 > IKFAST_EVALCOND_THRESH)
35895 {
35896 continue;
35897 }
35898 }
35899
35900 {
35901 std::vector<
35902 IkSingleDOFSolutionBase<IkReal> >
35903 vinfos(7);
35904 vinfos[0].jointtype = 1;
35905 vinfos[0].foffset = j4;
35906 vinfos[0].indices[0] = _ij4[0];
35907 vinfos[0].indices[1] = _ij4[1];
35908 vinfos[0].maxsolutions = _nj4;
35909 vinfos[1].jointtype = 1;
35910 vinfos[1].foffset = j6;
35911 vinfos[1].indices[0] = _ij6[0];
35912 vinfos[1].indices[1] = _ij6[1];
35913 vinfos[1].maxsolutions = _nj6;
35914 vinfos[2].jointtype = 1;
35915 vinfos[2].foffset = j8;
35916 vinfos[2].indices[0] = _ij8[0];
35917 vinfos[2].indices[1] = _ij8[1];
35918 vinfos[2].maxsolutions = _nj8;
35919 vinfos[3].jointtype = 1;
35920 vinfos[3].foffset = j9;
35921 vinfos[3].indices[0] = _ij9[0];
35922 vinfos[3].indices[1] = _ij9[1];
35923 vinfos[3].maxsolutions = _nj9;
35924 vinfos[4].jointtype = 1;
35925 vinfos[4].foffset = j10;
35926 vinfos[4].indices[0] = _ij10[0];
35927 vinfos[4].indices[1] = _ij10[1];
35928 vinfos[4].maxsolutions = _nj10;
35929 vinfos[5].jointtype = 1;
35930 vinfos[5].foffset = j11;
35931 vinfos[5].indices[0] = _ij11[0];
35932 vinfos[5].indices[1] = _ij11[1];
35933 vinfos[5].maxsolutions = _nj11;
35934 vinfos[6].jointtype = 1;
35935 vinfos[6].foffset = j12;
35936 vinfos[6].indices[0] = _ij12[0];
35937 vinfos[6].indices[1] = _ij12[1];
35938 vinfos[6].maxsolutions = _nj12;
35939 std::vector<int> vfree(0);
35940 solutions.AddSolution(vinfos, vfree);
35941 }
35942 }
35943 }
35944 }
35945 }
35946 }
35947 else
35948 {
35949 {
35950 IkReal j12array[1], cj12array[1], sj12array[1];
35951 bool j12valid[1] = {false};
35952 _nj12 = 1;
35953 CheckValue<IkReal> x403
35954 = IKPowWithIntegerCheck(IKsign(sj11), -1);
35955 if (!x403.valid)
35956 {
35957 continue;
35958 }
35959 CheckValue<IkReal> x404 = IKatan2WithCheck(
35960 IkReal(new_r21),
35961 ((-1.0) * (((1.0) * new_r20))),
35962 IKFAST_ATAN2_MAGTHRESH);
35963 if (!x404.valid)
35964 {
35965 continue;
35966 }
35967 j12array[0]
35968 = ((-1.5707963267949)
35969 + (((1.5707963267949) * (x403.value)))
35970 + (x404.value));
35971 sj12array[0] = IKsin(j12array[0]);
35972 cj12array[0] = IKcos(j12array[0]);
35973 if (j12array[0] > IKPI)
35974 {
35975 j12array[0] -= IK2PI;
35976 }
35977 else if (j12array[0] < -IKPI)
35978 {
35979 j12array[0] += IK2PI;
35980 }
35981 j12valid[0] = true;
35982 for (int ij12 = 0; ij12 < 1; ++ij12)
35983 {
35984 if (!j12valid[ij12])
35985 {
35986 continue;
35987 }
35988 _ij12[0] = ij12;
35989 _ij12[1] = -1;
35990 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
35991 {
35992 if (j12valid[iij12]
35993 && IKabs(cj12array[ij12] - cj12array[iij12])
35994 < IKFAST_SOLUTION_THRESH
35995 && IKabs(sj12array[ij12] - sj12array[iij12])
35996 < IKFAST_SOLUTION_THRESH)
35997 {
35998 j12valid[iij12] = false;
35999 _ij12[1] = iij12;
36000 break;
36001 }
36002 }
36003 j12 = j12array[ij12];
36004 cj12 = cj12array[ij12];
36005 sj12 = sj12array[ij12];
36006 {
36007 IkReal evalcond[12];
36008 IkReal x405 = IKcos(j12);
36009 IkReal x406 = IKsin(j12);
36010 IkReal x407 = ((1.0) * sj11);
36011 IkReal x408 = (cj10 * new_r01);
36012 IkReal x409 = (new_r11 * sj10);
36013 IkReal x410 = (cj11 * x406);
36014 IkReal x411 = ((1.0) * sj10);
36015 IkReal x412 = ((1.0) * x406);
36016 IkReal x413 = ((1.0) * x405);
36017 IkReal x414 = ((-1.0) * x413);
36018 IkReal x415 = (cj10 * new_r00);
36019 IkReal x416 = (new_r10 * sj10);
36020 IkReal x417 = (cj10 * x413);
36021 evalcond[0] = (((sj11 * x405)) + new_r20);
36022 evalcond[1]
36023 = ((((-1.0) * x406 * x407)) + new_r21);
36024 evalcond[2] = (x410 + x409 + x408);
36025 evalcond[3]
36026 = ((((-1.0) * new_r00 * x411))
36027 + ((cj10 * new_r10))
36028 + (((-1.0) * x412)));
36029 evalcond[4]
36030 = (((cj10 * new_r11)) + x414
36031 + (((-1.0) * new_r01 * x411)));
36032 evalcond[5]
36033 = (((cj10 * x410)) + ((sj10 * x405))
36034 + new_r01);
36035 evalcond[6]
36036 = ((((-1.0) * cj11 * x413)) + x415 + x416);
36037 evalcond[7]
36038 = ((((-1.0) * cj11 * x417))
36039 + ((sj10 * x406)) + new_r00);
36040 evalcond[8]
36041 = (((sj10 * x410)) + (((-1.0) * x417))
36042 + new_r11);
36043 evalcond[9]
36044 = ((((-1.0) * cj10 * x412))
36045 + (((-1.0) * cj11 * x405 * x411))
36046 + new_r10);
36047 evalcond[10]
36048 = (((cj11 * x409))
36049 + (((-1.0) * new_r21 * x407)) + x406
36050 + ((cj11 * x408)));
36051 evalcond[11]
36052 = (x414 + ((cj11 * x416)) + ((cj11 * x415))
36053 + (((-1.0) * new_r20 * x407)));
36054 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
36055 || IKabs(evalcond[1])
36056 > IKFAST_EVALCOND_THRESH
36057 || IKabs(evalcond[2])
36058 > IKFAST_EVALCOND_THRESH
36059 || IKabs(evalcond[3])
36060 > IKFAST_EVALCOND_THRESH
36061 || IKabs(evalcond[4])
36062 > IKFAST_EVALCOND_THRESH
36063 || IKabs(evalcond[5])
36064 > IKFAST_EVALCOND_THRESH
36065 || IKabs(evalcond[6])
36066 > IKFAST_EVALCOND_THRESH
36067 || IKabs(evalcond[7])
36068 > IKFAST_EVALCOND_THRESH
36069 || IKabs(evalcond[8])
36070 > IKFAST_EVALCOND_THRESH
36071 || IKabs(evalcond[9])
36072 > IKFAST_EVALCOND_THRESH
36073 || IKabs(evalcond[10])
36074 > IKFAST_EVALCOND_THRESH
36075 || IKabs(evalcond[11])
36076 > IKFAST_EVALCOND_THRESH)
36077 {
36078 continue;
36079 }
36080 }
36081
36082 {
36083 std::vector<IkSingleDOFSolutionBase<IkReal> >
36084 vinfos(7);
36085 vinfos[0].jointtype = 1;
36086 vinfos[0].foffset = j4;
36087 vinfos[0].indices[0] = _ij4[0];
36088 vinfos[0].indices[1] = _ij4[1];
36089 vinfos[0].maxsolutions = _nj4;
36090 vinfos[1].jointtype = 1;
36091 vinfos[1].foffset = j6;
36092 vinfos[1].indices[0] = _ij6[0];
36093 vinfos[1].indices[1] = _ij6[1];
36094 vinfos[1].maxsolutions = _nj6;
36095 vinfos[2].jointtype = 1;
36096 vinfos[2].foffset = j8;
36097 vinfos[2].indices[0] = _ij8[0];
36098 vinfos[2].indices[1] = _ij8[1];
36099 vinfos[2].maxsolutions = _nj8;
36100 vinfos[3].jointtype = 1;
36101 vinfos[3].foffset = j9;
36102 vinfos[3].indices[0] = _ij9[0];
36103 vinfos[3].indices[1] = _ij9[1];
36104 vinfos[3].maxsolutions = _nj9;
36105 vinfos[4].jointtype = 1;
36106 vinfos[4].foffset = j10;
36107 vinfos[4].indices[0] = _ij10[0];
36108 vinfos[4].indices[1] = _ij10[1];
36109 vinfos[4].maxsolutions = _nj10;
36110 vinfos[5].jointtype = 1;
36111 vinfos[5].foffset = j11;
36112 vinfos[5].indices[0] = _ij11[0];
36113 vinfos[5].indices[1] = _ij11[1];
36114 vinfos[5].maxsolutions = _nj11;
36115 vinfos[6].jointtype = 1;
36116 vinfos[6].foffset = j12;
36117 vinfos[6].indices[0] = _ij12[0];
36118 vinfos[6].indices[1] = _ij12[1];
36119 vinfos[6].maxsolutions = _nj12;
36120 std::vector<int> vfree(0);
36121 solutions.AddSolution(vinfos, vfree);
36122 }
36123 }
36124 }
36125 }
36126 }
36127 }
36128 }
36129 }
36130 }
36131 }
36132 else
36133 {
36134 {
36135 IkReal j10array[1], cj10array[1], sj10array[1];
36136 bool j10valid[1] = {false};
36137 _nj10 = 1;
36138 CheckValue<IkReal> x418 = IKatan2WithCheck(
36139 IkReal(new_r12), new_r02, IKFAST_ATAN2_MAGTHRESH);
36140 if (!x418.valid)
36141 {
36142 continue;
36143 }
36144 CheckValue<IkReal> x419
36145 = IKPowWithIntegerCheck(IKsign(sj11), -1);
36146 if (!x419.valid)
36147 {
36148 continue;
36149 }
36150 j10array[0]
36151 = ((-1.5707963267949) + (x418.value)
36152 + (((1.5707963267949) * (x419.value))));
36153 sj10array[0] = IKsin(j10array[0]);
36154 cj10array[0] = IKcos(j10array[0]);
36155 if (j10array[0] > IKPI)
36156 {
36157 j10array[0] -= IK2PI;
36158 }
36159 else if (j10array[0] < -IKPI)
36160 {
36161 j10array[0] += IK2PI;
36162 }
36163 j10valid[0] = true;
36164 for (int ij10 = 0; ij10 < 1; ++ij10)
36165 {
36166 if (!j10valid[ij10])
36167 {
36168 continue;
36169 }
36170 _ij10[0] = ij10;
36171 _ij10[1] = -1;
36172 for (int iij10 = ij10 + 1; iij10 < 1; ++iij10)
36173 {
36174 if (j10valid[iij10]
36175 && IKabs(cj10array[ij10] - cj10array[iij10])
36176 < IKFAST_SOLUTION_THRESH
36177 && IKabs(sj10array[ij10] - sj10array[iij10])
36178 < IKFAST_SOLUTION_THRESH)
36179 {
36180 j10valid[iij10] = false;
36181 _ij10[1] = iij10;
36182 break;
36183 }
36184 }
36185 j10 = j10array[ij10];
36186 cj10 = cj10array[ij10];
36187 sj10 = sj10array[ij10];
36188 {
36189 IkReal evalcond[8];
36190 IkReal x420 = IKcos(j10);
36191 IkReal x421 = ((1.0) * sj11);
36192 IkReal x422 = (x420 * x421);
36193 IkReal x423 = IKsin(j10);
36194 IkReal x424 = (x421 * x423);
36195 IkReal x425 = (new_r02 * x420);
36196 IkReal x426 = (new_r12 * x423);
36197 IkReal x427 = ((1.0) * cj11);
36198 evalcond[0] = ((((-1.0) * x422)) + new_r02);
36199 evalcond[1] = ((((-1.0) * x424)) + new_r12);
36200 evalcond[2]
36201 = ((((-1.0) * new_r02 * x423)) + ((new_r12 * x420)));
36202 evalcond[3] = ((((-1.0) * x421)) + x426 + x425);
36203 evalcond[4]
36204 = (((cj11 * x426)) + ((cj11 * x425))
36205 + (((-1.0) * new_r22 * x421)));
36206 evalcond[5]
36207 = ((((-1.0) * new_r10 * x424))
36208 + (((-1.0) * new_r20 * x427))
36209 + (((-1.0) * new_r00 * x422)));
36210 evalcond[6]
36211 = ((((-1.0) * new_r21 * x427))
36212 + (((-1.0) * new_r01 * x422))
36213 + (((-1.0) * new_r11 * x424)));
36214 evalcond[7]
36215 = ((1.0) + (((-1.0) * new_r22 * x427))
36216 + (((-1.0) * x421 * x425))
36217 + (((-1.0) * x421 * x426)));
36218 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
36219 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
36220 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
36221 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
36222 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
36223 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
36224 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
36225 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
36226 {
36227 continue;
36228 }
36229 }
36230
36231 {
36232 IkReal j12eval[2];
36233 IkReal x428 = ((1.0) * sj9);
36234 IkReal x429 = (cj9 * sj6);
36235 IkReal x430 = x160;
36236 IkReal x431 = (cj4 * sj8);
36237 IkReal x432 = (sj6 * x428);
36238 IkReal x433 = (cj6 * cj9);
36239 IkReal x434 = ((((-1.0) * x432)) + ((cj8 * x433)));
36240 IkReal x435 = (((cj9 * x431)) + ((sj4 * x434)));
36241 IkReal x436 = ((1.0) * sj4 * sj8);
36242 IkReal x437 = ((((-1.0) * cj9 * x436)) + ((cj4 * x434)));
36243 IkReal x438 = (sj6 * sj8);
36244 IkReal x439 = x169;
36245 IkReal x440 = x170;
36246 IkReal x441 = ((((-1.0) * cj8 * x432)) + x433);
36247 IkReal x442 = x172;
36248 IkReal x443 = (((sj9 * x431)) + ((sj4 * x442)));
36249 IkReal x444
36250 = (((cj4 * x442)) + (((-1.0) * sj4 * sj8 * x428)));
36251 new_r00
36252 = (((r20 * x430)) + ((r10 * x435)) + ((r00 * x437)));
36253 new_r01
36254 = (((r11 * x435)) + ((r01 * x437)) + ((r21 * x430)));
36255 new_r02
36256 = (((r12 * x435)) + ((r02 * x437)) + ((r22 * x430)));
36257 new_r10
36258 = (((r20 * x438)) + ((r00 * x440)) + ((r10 * x439)));
36259 new_r11
36260 = (((r11 * x439)) + ((r21 * x438)) + ((r01 * x440)));
36261 new_r12
36262 = (((r22 * x438)) + ((r12 * x439)) + ((r02 * x440)));
36263 new_r20
36264 = (((r10 * x443)) + ((r20 * x441)) + ((r00 * x444)));
36265 new_r21
36266 = (((r01 * x444)) + ((r21 * x441)) + ((r11 * x443)));
36267 new_r22
36268 = (((r22 * x441)) + ((r12 * x443)) + ((r02 * x444)));
36269 j12eval[0] = sj11;
36270 j12eval[1] = IKsign(sj11);
36271 if (IKabs(j12eval[0]) < 0.0000010000000000
36272 || IKabs(j12eval[1]) < 0.0000010000000000)
36273 {
36274 {
36275 IkReal j12eval[2];
36276 IkReal x445 = ((1.0) * sj9);
36277 IkReal x446 = (cj9 * sj6);
36278 IkReal x447 = x160;
36279 IkReal x448 = (cj4 * sj8);
36280 IkReal x449 = (sj6 * x445);
36281 IkReal x450 = (cj6 * cj9);
36282 IkReal x451 = (((cj8 * x450)) + (((-1.0) * x449)));
36283 IkReal x452 = (((cj9 * x448)) + ((sj4 * x451)));
36284 IkReal x453 = ((1.0) * sj4 * sj8);
36285 IkReal x454
36286 = (((cj4 * x451)) + (((-1.0) * cj9 * x453)));
36287 IkReal x455 = (sj6 * sj8);
36288 IkReal x456 = x169;
36289 IkReal x457 = x170;
36290 IkReal x458 = (x450 + (((-1.0) * cj8 * x449)));
36291 IkReal x459 = x172;
36292 IkReal x460 = (((sj4 * x459)) + ((sj9 * x448)));
36293 IkReal x461
36294 = (((cj4 * x459)) + (((-1.0) * sj4 * sj8 * x445)));
36295 new_r00
36296 = (((r00 * x454)) + ((r20 * x447))
36297 + ((r10 * x452)));
36298 new_r01
36299 = (((r01 * x454)) + ((r21 * x447))
36300 + ((r11 * x452)));
36301 new_r02
36302 = (((r02 * x454)) + ((r12 * x452))
36303 + ((r22 * x447)));
36304 new_r10
36305 = (((r20 * x455)) + ((r00 * x457))
36306 + ((r10 * x456)));
36307 new_r11
36308 = (((r21 * x455)) + ((r11 * x456))
36309 + ((r01 * x457)));
36310 new_r12
36311 = (((r12 * x456)) + ((r22 * x455))
36312 + ((r02 * x457)));
36313 new_r20
36314 = (((r10 * x460)) + ((r20 * x458))
36315 + ((r00 * x461)));
36316 new_r21
36317 = (((r11 * x460)) + ((r21 * x458))
36318 + ((r01 * x461)));
36319 new_r22
36320 = (((r12 * x460)) + ((r22 * x458))
36321 + ((r02 * x461)));
36322 j12eval[0] = sj10;
36323 j12eval[1] = sj11;
36324 if (IKabs(j12eval[0]) < 0.0000010000000000
36325 || IKabs(j12eval[1]) < 0.0000010000000000)
36326 {
36327 {
36328 IkReal j12eval[3];
36329 IkReal x462 = ((1.0) * sj9);
36330 IkReal x463 = (cj9 * sj6);
36331 IkReal x464 = x160;
36332 IkReal x465 = (cj4 * sj8);
36333 IkReal x466 = (sj6 * x462);
36334 IkReal x467 = (cj6 * cj9);
36335 IkReal x468 = ((((-1.0) * x466)) + ((cj8 * x467)));
36336 IkReal x469 = (((cj9 * x465)) + ((sj4 * x468)));
36337 IkReal x470 = ((1.0) * sj4 * sj8);
36338 IkReal x471
36339 = ((((-1.0) * cj9 * x470)) + ((cj4 * x468)));
36340 IkReal x472 = (sj6 * sj8);
36341 IkReal x473 = x169;
36342 IkReal x474 = x170;
36343 IkReal x475 = ((((-1.0) * cj8 * x466)) + x467);
36344 IkReal x476 = x172;
36345 IkReal x477 = (((sj9 * x465)) + ((sj4 * x476)));
36346 IkReal x478
36347 = ((((-1.0) * sj4 * sj8 * x462))
36348 + ((cj4 * x476)));
36349 new_r00
36350 = (((r10 * x469)) + ((r00 * x471))
36351 + ((r20 * x464)));
36352 new_r01
36353 = (((r11 * x469)) + ((r01 * x471))
36354 + ((r21 * x464)));
36355 new_r02
36356 = (((r22 * x464)) + ((r02 * x471))
36357 + ((r12 * x469)));
36358 new_r10
36359 = (((r00 * x474)) + ((r10 * x473))
36360 + ((r20 * x472)));
36361 new_r11
36362 = (((r01 * x474)) + ((r11 * x473))
36363 + ((r21 * x472)));
36364 new_r12
36365 = (((r02 * x474)) + ((r12 * x473))
36366 + ((r22 * x472)));
36367 new_r20
36368 = (((r20 * x475)) + ((r10 * x477))
36369 + ((r00 * x478)));
36370 new_r21
36371 = (((r01 * x478)) + ((r21 * x475))
36372 + ((r11 * x477)));
36373 new_r22
36374 = (((r22 * x475)) + ((r02 * x478))
36375 + ((r12 * x477)));
36376 j12eval[0] = cj10;
36377 j12eval[1] = cj11;
36378 j12eval[2] = sj11;
36379 if (IKabs(j12eval[0]) < 0.0000010000000000
36380 || IKabs(j12eval[1]) < 0.0000010000000000
36381 || IKabs(j12eval[2]) < 0.0000010000000000)
36382 {
36383 {
36384 IkReal evalcond[12];
36385 bool bgotonextstatement = true;
36386 do
36387 {
36388 IkReal x479 = ((1.0) * cj11);
36389 IkReal x480 = ((((-1.0) * x479)) + new_r22);
36390 IkReal x481 = ((1.0) * sj11);
36391 IkReal x482 = ((((-1.0) * x481)) + new_r12);
36392 evalcond[0]
36393 = ((-3.14159265358979)
36394 + (IKfmod(
36395 ((3.14159265358979)
36396 + (IKabs(
36397 ((-1.5707963267949) + j10)))),
36398 6.28318530717959)));
36399 evalcond[1] = x480;
36400 evalcond[2] = x480;
36401 evalcond[3] = new_r02;
36402 evalcond[4] = x482;
36403 evalcond[5] = x482;
36404 evalcond[6]
36405 = ((((-1.0) * new_r22 * x481))
36406 + ((cj11 * new_r12)));
36407 evalcond[7]
36408 = ((((-1.0) * new_r10 * x481))
36409 + (((-1.0) * new_r20 * x479)));
36410 evalcond[8]
36411 = ((((-1.0) * new_r11 * x481))
36412 + (((-1.0) * new_r21 * x479)));
36413 evalcond[9]
36414 = ((1.0) + (((-1.0) * new_r12 * x481))
36415 + (((-1.0) * new_r22 * x479)));
36416 if (IKabs(evalcond[0]) < 0.0000010000000000
36417 && IKabs(evalcond[1]) < 0.0000010000000000
36418 && IKabs(evalcond[2]) < 0.0000010000000000
36419 && IKabs(evalcond[3]) < 0.0000010000000000
36420 && IKabs(evalcond[4]) < 0.0000010000000000
36421 && IKabs(evalcond[5]) < 0.0000010000000000
36422 && IKabs(evalcond[6]) < 0.0000010000000000
36423 && IKabs(evalcond[7]) < 0.0000010000000000
36424 && IKabs(evalcond[8]) < 0.0000010000000000
36425 && IKabs(evalcond[9])
36426 < 0.0000010000000000)
36427 {
36428 bgotonextstatement = false;
36429 {
36430 IkReal j12array[1], cj12array[1],
36431 sj12array[1];
36432 bool j12valid[1] = {false};
36433 _nj12 = 1;
36434 CheckValue<IkReal> x483
36435 = IKPowWithIntegerCheck(
36436 IKsign(new_r12), -1);
36437 if (!x483.valid)
36438 {
36439 continue;
36440 }
36441 CheckValue<IkReal> x484
36442 = IKatan2WithCheck(
36443 IkReal(new_r21),
36444 ((-1.0) * (((1.0) * new_r20))),
36445 IKFAST_ATAN2_MAGTHRESH);
36446 if (!x484.valid)
36447 {
36448 continue;
36449 }
36450 j12array[0]
36451 = ((-1.5707963267949)
36452 + (((1.5707963267949)
36453 * (x483.value)))
36454 + (x484.value));
36455 sj12array[0] = IKsin(j12array[0]);
36456 cj12array[0] = IKcos(j12array[0]);
36457 if (j12array[0] > IKPI)
36458 {
36459 j12array[0] -= IK2PI;
36460 }
36461 else if (j12array[0] < -IKPI)
36462 {
36463 j12array[0] += IK2PI;
36464 }
36465 j12valid[0] = true;
36466 for (int ij12 = 0; ij12 < 1; ++ij12)
36467 {
36468 if (!j12valid[ij12])
36469 {
36470 continue;
36471 }
36472 _ij12[0] = ij12;
36473 _ij12[1] = -1;
36474 for (int iij12 = ij12 + 1; iij12 < 1;
36475 ++iij12)
36476 {
36477 if (j12valid[iij12]
36478 && IKabs(
36479 cj12array[ij12]
36480 - cj12array[iij12])
36481 < IKFAST_SOLUTION_THRESH
36482 && IKabs(
36483 sj12array[ij12]
36484 - sj12array[iij12])
36485 < IKFAST_SOLUTION_THRESH)
36486 {
36487 j12valid[iij12] = false;
36488 _ij12[1] = iij12;
36489 break;
36490 }
36491 }
36492 j12 = j12array[ij12];
36493 cj12 = cj12array[ij12];
36494 sj12 = sj12array[ij12];
36495 {
36496 IkReal evalcond[8];
36497 IkReal x485 = IKcos(j12);
36498 IkReal x486 = IKsin(j12);
36499 IkReal x487 = ((1.0) * new_r12);
36500 IkReal x488 = ((1.0) * x485);
36501 IkReal x489 = ((-1.0) * x488);
36502 evalcond[0]
36503 = (((new_r12 * x485)) + new_r20);
36504 evalcond[1]
36505 = (((new_r22 * x486)) + new_r11);
36506 evalcond[2]
36507 = ((((-1.0) * x486 * x487))
36508 + new_r21);
36509 evalcond[3]
36510 = ((((-1.0) * new_r22 * x488))
36511 + new_r10);
36512 evalcond[4]
36513 = ((((-1.0) * (1.0) * new_r00))
36514 + (((-1.0) * x486)));
36515 evalcond[5]
36516 = ((((-1.0) * (1.0) * new_r01))
36517 + x489);
36518 evalcond[6]
36519 = (((new_r11 * new_r22))
36520 + (((-1.0) * new_r21 * x487))
36521 + x486);
36522 evalcond[7]
36523 = ((((-1.0) * new_r20 * x487))
36524 + ((new_r10 * new_r22))
36525 + x489);
36526 if (IKabs(evalcond[0])
36527 > IKFAST_EVALCOND_THRESH
36528 || IKabs(evalcond[1])
36529 > IKFAST_EVALCOND_THRESH
36530 || IKabs(evalcond[2])
36531 > IKFAST_EVALCOND_THRESH
36532 || IKabs(evalcond[3])
36533 > IKFAST_EVALCOND_THRESH
36534 || IKabs(evalcond[4])
36535 > IKFAST_EVALCOND_THRESH
36536 || IKabs(evalcond[5])
36537 > IKFAST_EVALCOND_THRESH
36538 || IKabs(evalcond[6])
36539 > IKFAST_EVALCOND_THRESH
36540 || IKabs(evalcond[7])
36541 > IKFAST_EVALCOND_THRESH)
36542 {
36543 continue;
36544 }
36545 }
36546
36547 {
36548 std::vector<
36549 IkSingleDOFSolutionBase<IkReal> >
36550 vinfos(7);
36551 vinfos[0].jointtype = 1;
36552 vinfos[0].foffset = j4;
36553 vinfos[0].indices[0] = _ij4[0];
36554 vinfos[0].indices[1] = _ij4[1];
36555 vinfos[0].maxsolutions = _nj4;
36556 vinfos[1].jointtype = 1;
36557 vinfos[1].foffset = j6;
36558 vinfos[1].indices[0] = _ij6[0];
36559 vinfos[1].indices[1] = _ij6[1];
36560 vinfos[1].maxsolutions = _nj6;
36561 vinfos[2].jointtype = 1;
36562 vinfos[2].foffset = j8;
36563 vinfos[2].indices[0] = _ij8[0];
36564 vinfos[2].indices[1] = _ij8[1];
36565 vinfos[2].maxsolutions = _nj8;
36566 vinfos[3].jointtype = 1;
36567 vinfos[3].foffset = j9;
36568 vinfos[3].indices[0] = _ij9[0];
36569 vinfos[3].indices[1] = _ij9[1];
36570 vinfos[3].maxsolutions = _nj9;
36571 vinfos[4].jointtype = 1;
36572 vinfos[4].foffset = j10;
36573 vinfos[4].indices[0] = _ij10[0];
36574 vinfos[4].indices[1] = _ij10[1];
36575 vinfos[4].maxsolutions = _nj10;
36576 vinfos[5].jointtype = 1;
36577 vinfos[5].foffset = j11;
36578 vinfos[5].indices[0] = _ij11[0];
36579 vinfos[5].indices[1] = _ij11[1];
36580 vinfos[5].maxsolutions = _nj11;
36581 vinfos[6].jointtype = 1;
36582 vinfos[6].foffset = j12;
36583 vinfos[6].indices[0] = _ij12[0];
36584 vinfos[6].indices[1] = _ij12[1];
36585 vinfos[6].maxsolutions = _nj12;
36586 std::vector<int> vfree(0);
36587 solutions.AddSolution(vinfos, vfree);
36588 }
36589 }
36590 }
36591 }
36592 } while (0);
36593 if (bgotonextstatement)
36594 {
36595 bool bgotonextstatement = true;
36596 do
36597 {
36598 IkReal x490 = ((1.0) * cj11);
36599 IkReal x491 = ((((-1.0) * x490)) + new_r22);
36600 IkReal x492 = ((1.0) * sj11);
36601 evalcond[0]
36602 = ((-3.14159265358979)
36603 + (IKfmod(
36604 ((3.14159265358979)
36605 + (IKabs((
36606 (1.5707963267949) + j10)))),
36607 6.28318530717959)));
36608 evalcond[1] = x491;
36609 evalcond[2] = x491;
36610 evalcond[3] = new_r02;
36611 evalcond[4] = (sj11 + new_r12);
36612 evalcond[5]
36613 = ((((-1.0) * (1.0) * new_r12))
36614 + (((-1.0) * x492)));
36615 evalcond[6]
36616 = ((((-1.0) * new_r22 * x492))
36617 + (((-1.0) * new_r12 * x490)));
36618 evalcond[7]
36619 = (((new_r10 * sj11))
36620 + (((-1.0) * new_r20 * x490)));
36621 evalcond[8]
36622 = ((((-1.0) * new_r21 * x490))
36623 + ((new_r11 * sj11)));
36624 evalcond[9]
36625 = ((1.0) + (((-1.0) * new_r22 * x490))
36626 + ((new_r12 * sj11)));
36627 if (IKabs(evalcond[0]) < 0.0000010000000000
36628 && IKabs(evalcond[1])
36629 < 0.0000010000000000
36630 && IKabs(evalcond[2])
36631 < 0.0000010000000000
36632 && IKabs(evalcond[3])
36633 < 0.0000010000000000
36634 && IKabs(evalcond[4])
36635 < 0.0000010000000000
36636 && IKabs(evalcond[5])
36637 < 0.0000010000000000
36638 && IKabs(evalcond[6])
36639 < 0.0000010000000000
36640 && IKabs(evalcond[7])
36641 < 0.0000010000000000
36642 && IKabs(evalcond[8])
36643 < 0.0000010000000000
36644 && IKabs(evalcond[9])
36645 < 0.0000010000000000)
36646 {
36647 bgotonextstatement = false;
36648 {
36649 IkReal j12array[1], cj12array[1],
36650 sj12array[1];
36651 bool j12valid[1] = {false};
36652 _nj12 = 1;
36653 if (IKabs(new_r00)
36654 < IKFAST_ATAN2_MAGTHRESH
36655 && IKabs(new_r01)
36656 < IKFAST_ATAN2_MAGTHRESH
36657 && IKabs(
36658 IKsqr(new_r00)
36659 + IKsqr(new_r01) - 1)
36660 <= IKFAST_SINCOS_THRESH)
36661 continue;
36662 j12array[0] = IKatan2(new_r00, new_r01);
36663 sj12array[0] = IKsin(j12array[0]);
36664 cj12array[0] = IKcos(j12array[0]);
36665 if (j12array[0] > IKPI)
36666 {
36667 j12array[0] -= IK2PI;
36668 }
36669 else if (j12array[0] < -IKPI)
36670 {
36671 j12array[0] += IK2PI;
36672 }
36673 j12valid[0] = true;
36674 for (int ij12 = 0; ij12 < 1; ++ij12)
36675 {
36676 if (!j12valid[ij12])
36677 {
36678 continue;
36679 }
36680 _ij12[0] = ij12;
36681 _ij12[1] = -1;
36682 for (int iij12 = ij12 + 1; iij12 < 1;
36683 ++iij12)
36684 {
36685 if (j12valid[iij12]
36686 && IKabs(
36687 cj12array[ij12]
36688 - cj12array[iij12])
36689 < IKFAST_SOLUTION_THRESH
36690 && IKabs(
36691 sj12array[ij12]
36692 - sj12array[iij12])
36693 < IKFAST_SOLUTION_THRESH)
36694 {
36695 j12valid[iij12] = false;
36696 _ij12[1] = iij12;
36697 break;
36698 }
36699 }
36700 j12 = j12array[ij12];
36701 cj12 = cj12array[ij12];
36702 sj12 = sj12array[ij12];
36703 {
36704 IkReal evalcond[8];
36705 IkReal x493 = IKsin(j12);
36706 IkReal x494
36707 = ((1.0) * (IKcos(j12)));
36708 IkReal x495 = ((-1.0) * x494);
36709 IkReal x496 = ((1.0) * new_r11);
36710 IkReal x497 = ((1.0) * new_r10);
36711 evalcond[0]
36712 = (((new_r12 * x493))
36713 + new_r21);
36714 evalcond[1]
36715 = (new_r00 + (((-1.0) * x493)));
36716 evalcond[2] = (new_r01 + x495);
36717 evalcond[3]
36718 = ((((-1.0) * new_r12 * x494))
36719 + new_r20);
36720 evalcond[4]
36721 = (((new_r22 * x493))
36722 + (((-1.0) * x496)));
36723 evalcond[5]
36724 = ((((-1.0) * new_r22 * x494))
36725 + (((-1.0) * x497)));
36726 evalcond[6]
36727 = (((new_r12 * new_r21))
36728 + (((-1.0) * new_r22 * x496))
36729 + x493);
36730 evalcond[7]
36731 = (((new_r12 * new_r20))
36732 + (((-1.0) * new_r22 * x497))
36733 + x495);
36734 if (IKabs(evalcond[0])
36735 > IKFAST_EVALCOND_THRESH
36736 || IKabs(evalcond[1])
36737 > IKFAST_EVALCOND_THRESH
36738 || IKabs(evalcond[2])
36739 > IKFAST_EVALCOND_THRESH
36740 || IKabs(evalcond[3])
36741 > IKFAST_EVALCOND_THRESH
36742 || IKabs(evalcond[4])
36743 > IKFAST_EVALCOND_THRESH
36744 || IKabs(evalcond[5])
36745 > IKFAST_EVALCOND_THRESH
36746 || IKabs(evalcond[6])
36747 > IKFAST_EVALCOND_THRESH
36748 || IKabs(evalcond[7])
36749 > IKFAST_EVALCOND_THRESH)
36750 {
36751 continue;
36752 }
36753 }
36754
36755 {
36756 std::vector<IkSingleDOFSolutionBase<
36757 IkReal> >
36758 vinfos(7);
36759 vinfos[0].jointtype = 1;
36760 vinfos[0].foffset = j4;
36761 vinfos[0].indices[0] = _ij4[0];
36762 vinfos[0].indices[1] = _ij4[1];
36763 vinfos[0].maxsolutions = _nj4;
36764 vinfos[1].jointtype = 1;
36765 vinfos[1].foffset = j6;
36766 vinfos[1].indices[0] = _ij6[0];
36767 vinfos[1].indices[1] = _ij6[1];
36768 vinfos[1].maxsolutions = _nj6;
36769 vinfos[2].jointtype = 1;
36770 vinfos[2].foffset = j8;
36771 vinfos[2].indices[0] = _ij8[0];
36772 vinfos[2].indices[1] = _ij8[1];
36773 vinfos[2].maxsolutions = _nj8;
36774 vinfos[3].jointtype = 1;
36775 vinfos[3].foffset = j9;
36776 vinfos[3].indices[0] = _ij9[0];
36777 vinfos[3].indices[1] = _ij9[1];
36778 vinfos[3].maxsolutions = _nj9;
36779 vinfos[4].jointtype = 1;
36780 vinfos[4].foffset = j10;
36781 vinfos[4].indices[0] = _ij10[0];
36782 vinfos[4].indices[1] = _ij10[1];
36783 vinfos[4].maxsolutions = _nj10;
36784 vinfos[5].jointtype = 1;
36785 vinfos[5].foffset = j11;
36786 vinfos[5].indices[0] = _ij11[0];
36787 vinfos[5].indices[1] = _ij11[1];
36788 vinfos[5].maxsolutions = _nj11;
36789 vinfos[6].jointtype = 1;
36790 vinfos[6].foffset = j12;
36791 vinfos[6].indices[0] = _ij12[0];
36792 vinfos[6].indices[1] = _ij12[1];
36793 vinfos[6].maxsolutions = _nj12;
36794 std::vector<int> vfree(0);
36795 solutions.AddSolution(
36796 vinfos, vfree);
36797 }
36798 }
36799 }
36800 }
36801 } while (0);
36802 if (bgotonextstatement)
36803 {
36804 bool bgotonextstatement = true;
36805 do
36806 {
36807 IkReal x498 = ((1.0) * cj10);
36808 IkReal x499 = ((1.0) * sj10);
36809 IkReal x500
36810 = ((((-1.0) * new_r02 * x499))
36811 + ((cj10 * new_r12)));
36812 evalcond[0]
36813 = ((-3.14159265358979)
36814 + (IKfmod(
36815 ((3.14159265358979)
36816 + (IKabs(
36817 ((-1.5707963267949)
36818 + j11)))),
36819 6.28318530717959)));
36820 evalcond[1] = new_r22;
36821 evalcond[2]
36822 = ((((-1.0) * x498)) + new_r02);
36823 evalcond[3]
36824 = ((((-1.0) * x499)) + new_r12);
36825 evalcond[4] = x500;
36826 evalcond[5] = x500;
36827 evalcond[6]
36828 = ((-1.0) + ((cj10 * new_r02))
36829 + ((new_r12 * sj10)));
36830 evalcond[7]
36831 = (((cj10 * new_r01))
36832 + ((new_r11 * sj10)));
36833 evalcond[8]
36834 = (((cj10 * new_r00))
36835 + ((new_r10 * sj10)));
36836 evalcond[9]
36837 = ((((-1.0) * new_r10 * x499))
36838 + (((-1.0) * new_r00 * x498)));
36839 evalcond[10]
36840 = ((((-1.0) * new_r11 * x499))
36841 + (((-1.0) * new_r01 * x498)));
36842 evalcond[11]
36843 = ((1.0) + (((-1.0) * new_r02 * x498))
36844 + (((-1.0) * new_r12 * x499)));
36845 if (IKabs(evalcond[0])
36846 < 0.0000010000000000
36847 && IKabs(evalcond[1])
36848 < 0.0000010000000000
36849 && IKabs(evalcond[2])
36850 < 0.0000010000000000
36851 && IKabs(evalcond[3])
36852 < 0.0000010000000000
36853 && IKabs(evalcond[4])
36854 < 0.0000010000000000
36855 && IKabs(evalcond[5])
36856 < 0.0000010000000000
36857 && IKabs(evalcond[6])
36858 < 0.0000010000000000
36859 && IKabs(evalcond[7])
36860 < 0.0000010000000000
36861 && IKabs(evalcond[8])
36862 < 0.0000010000000000
36863 && IKabs(evalcond[9])
36864 < 0.0000010000000000
36865 && IKabs(evalcond[10])
36866 < 0.0000010000000000
36867 && IKabs(evalcond[11])
36868 < 0.0000010000000000)
36869 {
36870 bgotonextstatement = false;
36871 {
36872 IkReal j12array[1], cj12array[1],
36873 sj12array[1];
36874 bool j12valid[1] = {false};
36875 _nj12 = 1;
36876 if (IKabs(new_r21)
36877 < IKFAST_ATAN2_MAGTHRESH
36878 && IKabs(
36879 ((-1.0)
36880 * (((1.0) * new_r20))))
36881 < IKFAST_ATAN2_MAGTHRESH
36882 && IKabs(
36883 IKsqr(new_r21)
36884 + IKsqr((
36885 (-1.0)
36886 * (((1.0) * new_r20))))
36887 - 1)
36888 <= IKFAST_SINCOS_THRESH)
36889 continue;
36890 j12array[0] = IKatan2(
36891 new_r21,
36892 ((-1.0) * (((1.0) * new_r20))));
36893 sj12array[0] = IKsin(j12array[0]);
36894 cj12array[0] = IKcos(j12array[0]);
36895 if (j12array[0] > IKPI)
36896 {
36897 j12array[0] -= IK2PI;
36898 }
36899 else if (j12array[0] < -IKPI)
36900 {
36901 j12array[0] += IK2PI;
36902 }
36903 j12valid[0] = true;
36904 for (int ij12 = 0; ij12 < 1; ++ij12)
36905 {
36906 if (!j12valid[ij12])
36907 {
36908 continue;
36909 }
36910 _ij12[0] = ij12;
36911 _ij12[1] = -1;
36912 for (int iij12 = ij12 + 1;
36913 iij12 < 1;
36914 ++iij12)
36915 {
36916 if (j12valid[iij12]
36917 && IKabs(
36918 cj12array[ij12]
36919 - cj12array[iij12])
36920 < IKFAST_SOLUTION_THRESH
36921 && IKabs(
36922 sj12array[ij12]
36923 - sj12array[iij12])
36924 < IKFAST_SOLUTION_THRESH)
36925 {
36926 j12valid[iij12] = false;
36927 _ij12[1] = iij12;
36928 break;
36929 }
36930 }
36931 j12 = j12array[ij12];
36932 cj12 = cj12array[ij12];
36933 sj12 = sj12array[ij12];
36934 {
36935 IkReal evalcond[8];
36936 IkReal x501 = IKcos(j12);
36937 IkReal x502 = IKsin(j12);
36938 IkReal x503 = ((1.0) * x502);
36939 IkReal x504 = ((-1.0) * x503);
36940 IkReal x505 = ((1.0) * x501);
36941 IkReal x506 = ((1.0) * new_r12);
36942 evalcond[0] = (x501 + new_r20);
36943 evalcond[1] = (x504 + new_r21);
36944 evalcond[2]
36945 = (((new_r12 * x501))
36946 + new_r01);
36947 evalcond[3]
36948 = (((new_r12 * x502))
36949 + new_r00);
36950 evalcond[4]
36951 = ((((-1.0) * new_r02 * x505))
36952 + new_r11);
36953 evalcond[5]
36954 = ((((-1.0) * new_r02 * x503))
36955 + new_r10);
36956 evalcond[6]
36957 = ((((-1.0) * new_r00 * x506))
36958 + x504
36959 + ((new_r02 * new_r10)));
36960 evalcond[7]
36961 = ((((-1.0) * new_r01 * x506))
36962 + ((new_r02 * new_r11))
36963 + (((-1.0) * x505)));
36964 if (IKabs(evalcond[0])
36965 > IKFAST_EVALCOND_THRESH
36966 || IKabs(evalcond[1])
36967 > IKFAST_EVALCOND_THRESH
36968 || IKabs(evalcond[2])
36969 > IKFAST_EVALCOND_THRESH
36970 || IKabs(evalcond[3])
36971 > IKFAST_EVALCOND_THRESH
36972 || IKabs(evalcond[4])
36973 > IKFAST_EVALCOND_THRESH
36974 || IKabs(evalcond[5])
36975 > IKFAST_EVALCOND_THRESH
36976 || IKabs(evalcond[6])
36977 > IKFAST_EVALCOND_THRESH
36978 || IKabs(evalcond[7])
36979 > IKFAST_EVALCOND_THRESH)
36980 {
36981 continue;
36982 }
36983 }
36984
36985 {
36986 std::vector<
36987 IkSingleDOFSolutionBase<
36988 IkReal> >
36989 vinfos(7);
36990 vinfos[0].jointtype = 1;
36991 vinfos[0].foffset = j4;
36992 vinfos[0].indices[0] = _ij4[0];
36993 vinfos[0].indices[1] = _ij4[1];
36994 vinfos[0].maxsolutions = _nj4;
36995 vinfos[1].jointtype = 1;
36996 vinfos[1].foffset = j6;
36997 vinfos[1].indices[0] = _ij6[0];
36998 vinfos[1].indices[1] = _ij6[1];
36999 vinfos[1].maxsolutions = _nj6;
37000 vinfos[2].jointtype = 1;
37001 vinfos[2].foffset = j8;
37002 vinfos[2].indices[0] = _ij8[0];
37003 vinfos[2].indices[1] = _ij8[1];
37004 vinfos[2].maxsolutions = _nj8;
37005 vinfos[3].jointtype = 1;
37006 vinfos[3].foffset = j9;
37007 vinfos[3].indices[0] = _ij9[0];
37008 vinfos[3].indices[1] = _ij9[1];
37009 vinfos[3].maxsolutions = _nj9;
37010 vinfos[4].jointtype = 1;
37011 vinfos[4].foffset = j10;
37012 vinfos[4].indices[0] = _ij10[0];
37013 vinfos[4].indices[1] = _ij10[1];
37014 vinfos[4].maxsolutions = _nj10;
37015 vinfos[5].jointtype = 1;
37016 vinfos[5].foffset = j11;
37017 vinfos[5].indices[0] = _ij11[0];
37018 vinfos[5].indices[1] = _ij11[1];
37019 vinfos[5].maxsolutions = _nj11;
37020 vinfos[6].jointtype = 1;
37021 vinfos[6].foffset = j12;
37022 vinfos[6].indices[0] = _ij12[0];
37023 vinfos[6].indices[1] = _ij12[1];
37024 vinfos[6].maxsolutions = _nj12;
37025 std::vector<int> vfree(0);
37026 solutions.AddSolution(
37027 vinfos, vfree);
37028 }
37029 }
37030 }
37031 }
37032 } while (0);
37033 if (bgotonextstatement)
37034 {
37035 bool bgotonextstatement = true;
37036 do
37037 {
37038 IkReal x507
37039 = ((((-1.0) * (1.0) * new_r02
37040 * sj10))
37041 + ((cj10 * new_r12)));
37042 IkReal x508
37043 = ((1.0) + ((cj10 * new_r02))
37044 + ((new_r12 * sj10)));
37045 IkReal x509
37046 = (((cj10 * new_r01))
37047 + ((new_r11 * sj10)));
37048 IkReal x510
37049 = (((cj10 * new_r00))
37050 + ((new_r10 * sj10)));
37051 evalcond[0]
37052 = ((-3.14159265358979)
37053 + (IKfmod(
37054 ((3.14159265358979)
37055 + (IKabs(
37056 ((1.5707963267949)
37057 + j11)))),
37058 6.28318530717959)));
37059 evalcond[1] = new_r22;
37060 evalcond[2] = (cj10 + new_r02);
37061 evalcond[3] = (sj10 + new_r12);
37062 evalcond[4] = x507;
37063 evalcond[5] = x507;
37064 evalcond[6] = x508;
37065 evalcond[7] = x509;
37066 evalcond[8] = x510;
37067 evalcond[9] = x510;
37068 evalcond[10] = x509;
37069 evalcond[11] = x508;
37070 if (IKabs(evalcond[0])
37071 < 0.0000010000000000
37072 && IKabs(evalcond[1])
37073 < 0.0000010000000000
37074 && IKabs(evalcond[2])
37075 < 0.0000010000000000
37076 && IKabs(evalcond[3])
37077 < 0.0000010000000000
37078 && IKabs(evalcond[4])
37079 < 0.0000010000000000
37080 && IKabs(evalcond[5])
37081 < 0.0000010000000000
37082 && IKabs(evalcond[6])
37083 < 0.0000010000000000
37084 && IKabs(evalcond[7])
37085 < 0.0000010000000000
37086 && IKabs(evalcond[8])
37087 < 0.0000010000000000
37088 && IKabs(evalcond[9])
37089 < 0.0000010000000000
37090 && IKabs(evalcond[10])
37091 < 0.0000010000000000
37092 && IKabs(evalcond[11])
37093 < 0.0000010000000000)
37094 {
37095 bgotonextstatement = false;
37096 {
37097 IkReal j12array[1], cj12array[1],
37098 sj12array[1];
37099 bool j12valid[1] = {false};
37100 _nj12 = 1;
37101 if (IKabs(
37102 ((-1.0)
37103 * (((1.0) * new_r21))))
37104 < IKFAST_ATAN2_MAGTHRESH
37105 && IKabs(new_r20)
37106 < IKFAST_ATAN2_MAGTHRESH
37107 && IKabs(
37108 IKsqr(
37109 ((-1.0)
37110 * (((1.0)
37111 * new_r21))))
37112 + IKsqr(new_r20) - 1)
37113 <= IKFAST_SINCOS_THRESH)
37114 continue;
37115 j12array[0] = IKatan2(
37116 ((-1.0) * (((1.0) * new_r21))),
37117 new_r20);
37118 sj12array[0] = IKsin(j12array[0]);
37119 cj12array[0] = IKcos(j12array[0]);
37120 if (j12array[0] > IKPI)
37121 {
37122 j12array[0] -= IK2PI;
37123 }
37124 else if (j12array[0] < -IKPI)
37125 {
37126 j12array[0] += IK2PI;
37127 }
37128 j12valid[0] = true;
37129 for (int ij12 = 0; ij12 < 1; ++ij12)
37130 {
37131 if (!j12valid[ij12])
37132 {
37133 continue;
37134 }
37135 _ij12[0] = ij12;
37136 _ij12[1] = -1;
37137 for (int iij12 = ij12 + 1;
37138 iij12 < 1;
37139 ++iij12)
37140 {
37141 if (j12valid[iij12]
37142 && IKabs(
37143 cj12array[ij12]
37144 - cj12array[iij12])
37145 < IKFAST_SOLUTION_THRESH
37146 && IKabs(
37147 sj12array[ij12]
37148 - sj12array[iij12])
37149 < IKFAST_SOLUTION_THRESH)
37150 {
37151 j12valid[iij12] = false;
37152 _ij12[1] = iij12;
37153 break;
37154 }
37155 }
37156 j12 = j12array[ij12];
37157 cj12 = cj12array[ij12];
37158 sj12 = sj12array[ij12];
37159 {
37160 IkReal evalcond[8];
37161 IkReal x511 = IKsin(j12);
37162 IkReal x512 = IKcos(j12);
37163 IkReal x513 = ((1.0) * x512);
37164 IkReal x514 = ((-1.0) * x513);
37165 IkReal x515 = ((1.0) * x511);
37166 IkReal x516 = ((1.0) * new_r02);
37167 evalcond[0] = (x511 + new_r21);
37168 evalcond[1] = (x514 + new_r20);
37169 evalcond[2]
37170 = (((new_r02 * x512))
37171 + new_r11);
37172 evalcond[3]
37173 = (((new_r02 * x511))
37174 + new_r10);
37175 evalcond[4]
37176 = ((((-1.0) * new_r12
37177 * x513))
37178 + new_r01);
37179 evalcond[5]
37180 = ((((-1.0) * new_r12
37181 * x515))
37182 + new_r00);
37183 evalcond[6]
37184 = ((((-1.0) * new_r10
37185 * x516))
37186 + ((new_r00 * new_r12))
37187 + (((-1.0) * x515)));
37188 evalcond[7]
37189 = ((((-1.0) * new_r11
37190 * x516))
37191 + ((new_r01 * new_r12))
37192 + x514);
37193 if (IKabs(evalcond[0])
37194 > IKFAST_EVALCOND_THRESH
37195 || IKabs(evalcond[1])
37196 > IKFAST_EVALCOND_THRESH
37197 || IKabs(evalcond[2])
37198 > IKFAST_EVALCOND_THRESH
37199 || IKabs(evalcond[3])
37200 > IKFAST_EVALCOND_THRESH
37201 || IKabs(evalcond[4])
37202 > IKFAST_EVALCOND_THRESH
37203 || IKabs(evalcond[5])
37204 > IKFAST_EVALCOND_THRESH
37205 || IKabs(evalcond[6])
37206 > IKFAST_EVALCOND_THRESH
37207 || IKabs(evalcond[7])
37208 > IKFAST_EVALCOND_THRESH)
37209 {
37210 continue;
37211 }
37212 }
37213
37214 {
37215 std::vector<
37216 IkSingleDOFSolutionBase<
37217 IkReal> >
37218 vinfos(7);
37219 vinfos[0].jointtype = 1;
37220 vinfos[0].foffset = j4;
37221 vinfos[0].indices[0] = _ij4[0];
37222 vinfos[0].indices[1] = _ij4[1];
37223 vinfos[0].maxsolutions = _nj4;
37224 vinfos[1].jointtype = 1;
37225 vinfos[1].foffset = j6;
37226 vinfos[1].indices[0] = _ij6[0];
37227 vinfos[1].indices[1] = _ij6[1];
37228 vinfos[1].maxsolutions = _nj6;
37229 vinfos[2].jointtype = 1;
37230 vinfos[2].foffset = j8;
37231 vinfos[2].indices[0] = _ij8[0];
37232 vinfos[2].indices[1] = _ij8[1];
37233 vinfos[2].maxsolutions = _nj8;
37234 vinfos[3].jointtype = 1;
37235 vinfos[3].foffset = j9;
37236 vinfos[3].indices[0] = _ij9[0];
37237 vinfos[3].indices[1] = _ij9[1];
37238 vinfos[3].maxsolutions = _nj9;
37239 vinfos[4].jointtype = 1;
37240 vinfos[4].foffset = j10;
37241 vinfos[4].indices[0] = _ij10[0];
37242 vinfos[4].indices[1] = _ij10[1];
37243 vinfos[4].maxsolutions = _nj10;
37244 vinfos[5].jointtype = 1;
37245 vinfos[5].foffset = j11;
37246 vinfos[5].indices[0] = _ij11[0];
37247 vinfos[5].indices[1] = _ij11[1];
37248 vinfos[5].maxsolutions = _nj11;
37249 vinfos[6].jointtype = 1;
37250 vinfos[6].foffset = j12;
37251 vinfos[6].indices[0] = _ij12[0];
37252 vinfos[6].indices[1] = _ij12[1];
37253 vinfos[6].maxsolutions = _nj12;
37254 std::vector<int> vfree(0);
37255 solutions.AddSolution(
37256 vinfos, vfree);
37257 }
37258 }
37259 }
37260 }
37261 } while (0);
37262 if (bgotonextstatement)
37263 {
37264 bool bgotonextstatement = true;
37265 do
37266 {
37267 IkReal x517
37268 = ((((-1.0) * (1.0) * new_r02
37269 * sj10))
37270 + ((cj10 * new_r12)));
37271 IkReal x518
37272 = (((cj10 * new_r02))
37273 + ((new_r12 * sj10)));
37274 evalcond[0]
37275 = ((-3.14159265358979)
37276 + (IKfmod(
37277 ((3.14159265358979)
37278 + (IKabs(j11))),
37279 6.28318530717959)));
37280 evalcond[1] = ((-1.0) + new_r22);
37281 evalcond[2] = new_r20;
37282 evalcond[3] = new_r02;
37283 evalcond[4] = new_r12;
37284 evalcond[5] = new_r21;
37285 evalcond[6] = x517;
37286 evalcond[7] = x517;
37287 evalcond[8] = x518;
37288 evalcond[9] = x518;
37289 if (IKabs(evalcond[0])
37290 < 0.0000010000000000
37291 && IKabs(evalcond[1])
37292 < 0.0000010000000000
37293 && IKabs(evalcond[2])
37294 < 0.0000010000000000
37295 && IKabs(evalcond[3])
37296 < 0.0000010000000000
37297 && IKabs(evalcond[4])
37298 < 0.0000010000000000
37299 && IKabs(evalcond[5])
37300 < 0.0000010000000000
37301 && IKabs(evalcond[6])
37302 < 0.0000010000000000
37303 && IKabs(evalcond[7])
37304 < 0.0000010000000000
37305 && IKabs(evalcond[8])
37306 < 0.0000010000000000
37307 && IKabs(evalcond[9])
37308 < 0.0000010000000000)
37309 {
37310 bgotonextstatement = false;
37311 {
37312 IkReal j12array[1], cj12array[1],
37313 sj12array[1];
37314 bool j12valid[1] = {false};
37315 _nj12 = 1;
37316 IkReal x519 = ((1.0) * new_r01);
37317 if (IKabs(
37318 ((((-1.0) * cj10 * x519))
37319 + (((-1.0) * (1.0)
37320 * new_r00 * sj10))))
37321 < IKFAST_ATAN2_MAGTHRESH
37322 && IKabs(
37323 (((cj10 * new_r00))
37324 + (((-1.0) * sj10
37325 * x519))))
37326 < IKFAST_ATAN2_MAGTHRESH
37327 && IKabs(
37328 IKsqr(
37329 ((((-1.0) * cj10
37330 * x519))
37331 + (((-1.0) * (1.0)
37332 * new_r00
37333 * sj10))))
37334 + IKsqr(
37335 (((cj10 * new_r00))
37336 + (((-1.0) * sj10
37337 * x519))))
37338 - 1)
37339 <= IKFAST_SINCOS_THRESH)
37340 continue;
37341 j12array[0] = IKatan2(
37342 ((((-1.0) * cj10 * x519))
37343 + (((-1.0) * (1.0) * new_r00
37344 * sj10))),
37345 (((cj10 * new_r00))
37346 + (((-1.0) * sj10 * x519))));
37347 sj12array[0] = IKsin(j12array[0]);
37348 cj12array[0] = IKcos(j12array[0]);
37349 if (j12array[0] > IKPI)
37350 {
37351 j12array[0] -= IK2PI;
37352 }
37353 else if (j12array[0] < -IKPI)
37354 {
37355 j12array[0] += IK2PI;
37356 }
37357 j12valid[0] = true;
37358 for (int ij12 = 0; ij12 < 1;
37359 ++ij12)
37360 {
37361 if (!j12valid[ij12])
37362 {
37363 continue;
37364 }
37365 _ij12[0] = ij12;
37366 _ij12[1] = -1;
37367 for (int iij12 = ij12 + 1;
37368 iij12 < 1;
37369 ++iij12)
37370 {
37371 if (j12valid[iij12]
37372 && IKabs(
37373 cj12array[ij12]
37374 - cj12array[iij12])
37375 < IKFAST_SOLUTION_THRESH
37376 && IKabs(
37377 sj12array[ij12]
37378 - sj12array[iij12])
37379 < IKFAST_SOLUTION_THRESH)
37380 {
37381 j12valid[iij12] = false;
37382 _ij12[1] = iij12;
37383 break;
37384 }
37385 }
37386 j12 = j12array[ij12];
37387 cj12 = cj12array[ij12];
37388 sj12 = sj12array[ij12];
37389 {
37390 IkReal evalcond[8];
37391 IkReal x520 = IKsin(j12);
37392 IkReal x521 = (cj10 * x520);
37393 IkReal x522 = IKcos(j12);
37394 IkReal x523 = ((1.0) * x522);
37395 IkReal x524 = ((-1.0) * x523);
37396 IkReal x525 = ((1.0) * sj10);
37397 IkReal x526
37398 = ((((-1.0) * cj10
37399 * x523))
37400 + ((sj10 * x520)));
37401 evalcond[0]
37402 = (((cj10 * new_r01))
37403 + ((new_r11 * sj10))
37404 + x520);
37405 evalcond[1]
37406 = (((sj10 * x522))
37407 + new_r01 + x521);
37408 evalcond[2]
37409 = (((cj10 * new_r00))
37410 + ((new_r10 * sj10))
37411 + x524);
37412 evalcond[3]
37413 = (((cj10 * new_r10))
37414 + (((-1.0) * new_r00
37415 * x525))
37416 + (((-1.0) * x520)));
37417 evalcond[4]
37418 = (((cj10 * new_r11))
37419 + (((-1.0) * new_r01
37420 * x525))
37421 + x524);
37422 evalcond[5]
37423 = (new_r00 + x526);
37424 evalcond[6]
37425 = (new_r11 + x526);
37426 evalcond[7]
37427 = ((((-1.0) * x521))
37428 + (((-1.0) * x522
37429 * x525))
37430 + new_r10);
37431 if (IKabs(evalcond[0])
37432 > IKFAST_EVALCOND_THRESH
37433 || IKabs(evalcond[1])
37434 > IKFAST_EVALCOND_THRESH
37435 || IKabs(evalcond[2])
37436 > IKFAST_EVALCOND_THRESH
37437 || IKabs(evalcond[3])
37438 > IKFAST_EVALCOND_THRESH
37439 || IKabs(evalcond[4])
37440 > IKFAST_EVALCOND_THRESH
37441 || IKabs(evalcond[5])
37442 > IKFAST_EVALCOND_THRESH
37443 || IKabs(evalcond[6])
37444 > IKFAST_EVALCOND_THRESH
37445 || IKabs(evalcond[7])
37446 > IKFAST_EVALCOND_THRESH)
37447 {
37448 continue;
37449 }
37450 }
37451
37452 {
37453 std::vector<
37454 IkSingleDOFSolutionBase<
37455 IkReal> >
37456 vinfos(7);
37457 vinfos[0].jointtype = 1;
37458 vinfos[0].foffset = j4;
37459 vinfos[0].indices[0]
37460 = _ij4[0];
37461 vinfos[0].indices[1]
37462 = _ij4[1];
37463 vinfos[0].maxsolutions = _nj4;
37464 vinfos[1].jointtype = 1;
37465 vinfos[1].foffset = j6;
37466 vinfos[1].indices[0]
37467 = _ij6[0];
37468 vinfos[1].indices[1]
37469 = _ij6[1];
37470 vinfos[1].maxsolutions = _nj6;
37471 vinfos[2].jointtype = 1;
37472 vinfos[2].foffset = j8;
37473 vinfos[2].indices[0]
37474 = _ij8[0];
37475 vinfos[2].indices[1]
37476 = _ij8[1];
37477 vinfos[2].maxsolutions = _nj8;
37478 vinfos[3].jointtype = 1;
37479 vinfos[3].foffset = j9;
37480 vinfos[3].indices[0]
37481 = _ij9[0];
37482 vinfos[3].indices[1]
37483 = _ij9[1];
37484 vinfos[3].maxsolutions = _nj9;
37485 vinfos[4].jointtype = 1;
37486 vinfos[4].foffset = j10;
37487 vinfos[4].indices[0]
37488 = _ij10[0];
37489 vinfos[4].indices[1]
37490 = _ij10[1];
37491 vinfos[4].maxsolutions
37492 = _nj10;
37493 vinfos[5].jointtype = 1;
37494 vinfos[5].foffset = j11;
37495 vinfos[5].indices[0]
37496 = _ij11[0];
37497 vinfos[5].indices[1]
37498 = _ij11[1];
37499 vinfos[5].maxsolutions
37500 = _nj11;
37501 vinfos[6].jointtype = 1;
37502 vinfos[6].foffset = j12;
37503 vinfos[6].indices[0]
37504 = _ij12[0];
37505 vinfos[6].indices[1]
37506 = _ij12[1];
37507 vinfos[6].maxsolutions
37508 = _nj12;
37509 std::vector<int> vfree(0);
37510 solutions.AddSolution(
37511 vinfos, vfree);
37512 }
37513 }
37514 }
37515 }
37516 } while (0);
37517 if (bgotonextstatement)
37518 {
37519 bool bgotonextstatement = true;
37520 do
37521 {
37522 IkReal x527
37523 = ((((-1.0) * (1.0) * new_r02
37524 * sj10))
37525 + ((cj10 * new_r12)));
37526 IkReal x528 = (cj10 * new_r02);
37527 IkReal x529 = (new_r12 * sj10);
37528 evalcond[0]
37529 = ((-3.14159265358979)
37530 + (IKfmod(
37531 ((3.14159265358979)
37532 + (IKabs(
37533 ((-3.14159265358979)
37534 + j11)))),
37535 6.28318530717959)));
37536 evalcond[1] = ((1.0) + new_r22);
37537 evalcond[2] = new_r20;
37538 evalcond[3] = new_r02;
37539 evalcond[4] = new_r12;
37540 evalcond[5] = new_r21;
37541 evalcond[6] = x527;
37542 evalcond[7] = x527;
37543 evalcond[8] = (x529 + x528);
37544 evalcond[9]
37545 = ((((-1.0) * x528))
37546 + (((-1.0) * x529)));
37547 if (IKabs(evalcond[0])
37548 < 0.0000010000000000
37549 && IKabs(evalcond[1])
37550 < 0.0000010000000000
37551 && IKabs(evalcond[2])
37552 < 0.0000010000000000
37553 && IKabs(evalcond[3])
37554 < 0.0000010000000000
37555 && IKabs(evalcond[4])
37556 < 0.0000010000000000
37557 && IKabs(evalcond[5])
37558 < 0.0000010000000000
37559 && IKabs(evalcond[6])
37560 < 0.0000010000000000
37561 && IKabs(evalcond[7])
37562 < 0.0000010000000000
37563 && IKabs(evalcond[8])
37564 < 0.0000010000000000
37565 && IKabs(evalcond[9])
37566 < 0.0000010000000000)
37567 {
37568 bgotonextstatement = false;
37569 {
37570 IkReal j12array[1],
37571 cj12array[1], sj12array[1];
37572 bool j12valid[1] = {false};
37573 _nj12 = 1;
37574 IkReal x530 = ((1.0) * new_r00);
37575 if (IKabs((
37576 (((-1.0) * sj10 * x530))
37577 + ((cj10 * new_r01))))
37578 < IKFAST_ATAN2_MAGTHRESH
37579 && IKabs(
37580 ((((-1.0) * cj10
37581 * x530))
37582 + (((-1.0) * (1.0)
37583 * new_r01
37584 * sj10))))
37585 < IKFAST_ATAN2_MAGTHRESH
37586 && IKabs(
37587 IKsqr((
37588 (((-1.0) * sj10
37589 * x530))
37590 + ((cj10
37591 * new_r01))))
37592 + IKsqr(
37593 ((((-1.0) * cj10
37594 * x530))
37595 + (((-1.0)
37596 * (1.0)
37597 * new_r01
37598 * sj10))))
37599 - 1)
37600 <= IKFAST_SINCOS_THRESH)
37601 continue;
37602 j12array[0] = IKatan2(
37603 ((((-1.0) * sj10 * x530))
37604 + ((cj10 * new_r01))),
37605 ((((-1.0) * cj10 * x530))
37606 + (((-1.0) * (1.0)
37607 * new_r01 * sj10))));
37608 sj12array[0]
37609 = IKsin(j12array[0]);
37610 cj12array[0]
37611 = IKcos(j12array[0]);
37612 if (j12array[0] > IKPI)
37613 {
37614 j12array[0] -= IK2PI;
37615 }
37616 else if (j12array[0] < -IKPI)
37617 {
37618 j12array[0] += IK2PI;
37619 }
37620 j12valid[0] = true;
37621 for (int ij12 = 0; ij12 < 1;
37622 ++ij12)
37623 {
37624 if (!j12valid[ij12])
37625 {
37626 continue;
37627 }
37628 _ij12[0] = ij12;
37629 _ij12[1] = -1;
37630 for (int iij12 = ij12 + 1;
37631 iij12 < 1;
37632 ++iij12)
37633 {
37634 if (j12valid[iij12]
37635 && IKabs(
37636 cj12array[ij12]
37637 - cj12array
37638 [iij12])
37639 < IKFAST_SOLUTION_THRESH
37640 && IKabs(
37641 sj12array[ij12]
37642 - sj12array
37643 [iij12])
37644 < IKFAST_SOLUTION_THRESH)
37645 {
37646 j12valid[iij12] = false;
37647 _ij12[1] = iij12;
37648 break;
37649 }
37650 }
37651 j12 = j12array[ij12];
37652 cj12 = cj12array[ij12];
37653 sj12 = sj12array[ij12];
37654 {
37655 IkReal evalcond[8];
37656 IkReal x531 = IKcos(j12);
37657 IkReal x532 = IKsin(j12);
37658 IkReal x533
37659 = ((1.0) * x532);
37660 IkReal x534
37661 = ((-1.0) * x533);
37662 IkReal x535 = (cj10 * x531);
37663 IkReal x536
37664 = ((1.0) * sj10);
37665 IkReal x537
37666 = ((((-1.0) * cj10
37667 * x533))
37668 + ((sj10 * x531)));
37669 evalcond[0]
37670 = (x531
37671 + ((cj10 * new_r00))
37672 + ((new_r10
37673 * sj10)));
37674 evalcond[1]
37675 = (x534
37676 + ((cj10 * new_r01))
37677 + ((new_r11
37678 * sj10)));
37679 evalcond[2]
37680 = (x535
37681 + ((sj10 * x532))
37682 + new_r00);
37683 evalcond[3]
37684 = (x534
37685 + ((cj10 * new_r10))
37686 + (((-1.0) * new_r00
37687 * x536)));
37688 evalcond[4]
37689 = (((cj10 * new_r11))
37690 + (((-1.0) * x531))
37691 + (((-1.0) * new_r01
37692 * x536)));
37693 evalcond[5]
37694 = (x537 + new_r01);
37695 evalcond[6]
37696 = (x537 + new_r10);
37697 evalcond[7]
37698 = ((((-1.0) * x532
37699 * x536))
37700 + new_r11
37701 + (((-1.0) * x535)));
37702 if (IKabs(evalcond[0])
37703 > IKFAST_EVALCOND_THRESH
37704 || IKabs(evalcond[1])
37705 > IKFAST_EVALCOND_THRESH
37706 || IKabs(evalcond[2])
37707 > IKFAST_EVALCOND_THRESH
37708 || IKabs(evalcond[3])
37709 > IKFAST_EVALCOND_THRESH
37710 || IKabs(evalcond[4])
37711 > IKFAST_EVALCOND_THRESH
37712 || IKabs(evalcond[5])
37713 > IKFAST_EVALCOND_THRESH
37714 || IKabs(evalcond[6])
37715 > IKFAST_EVALCOND_THRESH
37716 || IKabs(evalcond[7])
37717 > IKFAST_EVALCOND_THRESH)
37718 {
37719 continue;
37720 }
37721 }
37722
37723 {
37724 std::vector<
37725 IkSingleDOFSolutionBase<
37726 IkReal> >
37727 vinfos(7);
37728 vinfos[0].jointtype = 1;
37729 vinfos[0].foffset = j4;
37730 vinfos[0].indices[0]
37731 = _ij4[0];
37732 vinfos[0].indices[1]
37733 = _ij4[1];
37734 vinfos[0].maxsolutions
37735 = _nj4;
37736 vinfos[1].jointtype = 1;
37737 vinfos[1].foffset = j6;
37738 vinfos[1].indices[0]
37739 = _ij6[0];
37740 vinfos[1].indices[1]
37741 = _ij6[1];
37742 vinfos[1].maxsolutions
37743 = _nj6;
37744 vinfos[2].jointtype = 1;
37745 vinfos[2].foffset = j8;
37746 vinfos[2].indices[0]
37747 = _ij8[0];
37748 vinfos[2].indices[1]
37749 = _ij8[1];
37750 vinfos[2].maxsolutions
37751 = _nj8;
37752 vinfos[3].jointtype = 1;
37753 vinfos[3].foffset = j9;
37754 vinfos[3].indices[0]
37755 = _ij9[0];
37756 vinfos[3].indices[1]
37757 = _ij9[1];
37758 vinfos[3].maxsolutions
37759 = _nj9;
37760 vinfos[4].jointtype = 1;
37761 vinfos[4].foffset = j10;
37762 vinfos[4].indices[0]
37763 = _ij10[0];
37764 vinfos[4].indices[1]
37765 = _ij10[1];
37766 vinfos[4].maxsolutions
37767 = _nj10;
37768 vinfos[5].jointtype = 1;
37769 vinfos[5].foffset = j11;
37770 vinfos[5].indices[0]
37771 = _ij11[0];
37772 vinfos[5].indices[1]
37773 = _ij11[1];
37774 vinfos[5].maxsolutions
37775 = _nj11;
37776 vinfos[6].jointtype = 1;
37777 vinfos[6].foffset = j12;
37778 vinfos[6].indices[0]
37779 = _ij12[0];
37780 vinfos[6].indices[1]
37781 = _ij12[1];
37782 vinfos[6].maxsolutions
37783 = _nj12;
37784 std::vector<int> vfree(0);
37785 solutions.AddSolution(
37786 vinfos, vfree);
37787 }
37788 }
37789 }
37790 }
37791 } while (0);
37792 if (bgotonextstatement)
37793 {
37794 bool bgotonextstatement = true;
37795 do
37796 {
37797 IkReal x538 = ((1.0) * cj11);
37798 IkReal x539
37799 = ((((-1.0) * x538))
37800 + new_r22);
37801 IkReal x540 = ((1.0) * sj11);
37802 IkReal x541
37803 = ((((-1.0) * x540))
37804 + new_r02);
37805 evalcond[0]
37806 = ((-3.14159265358979)
37807 + (IKfmod(
37808 ((3.14159265358979)
37809 + (IKabs(j10))),
37810 6.28318530717959)));
37811 evalcond[1] = x539;
37812 evalcond[2] = x539;
37813 evalcond[3] = x541;
37814 evalcond[4] = new_r12;
37815 evalcond[5] = x541;
37816 evalcond[6]
37817 = ((((-1.0) * new_r22 * x540))
37818 + ((cj11 * new_r02)));
37819 evalcond[7]
37820 = ((((-1.0) * new_r20 * x538))
37821 + (((-1.0) * new_r00
37822 * x540)));
37823 evalcond[8]
37824 = ((((-1.0) * new_r21 * x538))
37825 + (((-1.0) * new_r01
37826 * x540)));
37827 evalcond[9]
37828 = ((1.0)
37829 + (((-1.0) * new_r02
37830 * x540))
37831 + (((-1.0) * new_r22
37832 * x538)));
37833 if (IKabs(evalcond[0])
37834 < 0.0000010000000000
37835 && IKabs(evalcond[1])
37836 < 0.0000010000000000
37837 && IKabs(evalcond[2])
37838 < 0.0000010000000000
37839 && IKabs(evalcond[3])
37840 < 0.0000010000000000
37841 && IKabs(evalcond[4])
37842 < 0.0000010000000000
37843 && IKabs(evalcond[5])
37844 < 0.0000010000000000
37845 && IKabs(evalcond[6])
37846 < 0.0000010000000000
37847 && IKabs(evalcond[7])
37848 < 0.0000010000000000
37849 && IKabs(evalcond[8])
37850 < 0.0000010000000000
37851 && IKabs(evalcond[9])
37852 < 0.0000010000000000)
37853 {
37854 bgotonextstatement = false;
37855 {
37856 IkReal j12array[1],
37857 cj12array[1],
37858 sj12array[1];
37859 bool j12valid[1] = {false};
37860 _nj12 = 1;
37861 if (IKabs(new_r10)
37862 < IKFAST_ATAN2_MAGTHRESH
37863 && IKabs(new_r11)
37864 < IKFAST_ATAN2_MAGTHRESH
37865 && IKabs(
37866 IKsqr(new_r10)
37867 + IKsqr(new_r11)
37868 - 1)
37869 <= IKFAST_SINCOS_THRESH)
37870 continue;
37871 j12array[0] = IKatan2(
37872 new_r10, new_r11);
37873 sj12array[0]
37874 = IKsin(j12array[0]);
37875 cj12array[0]
37876 = IKcos(j12array[0]);
37877 if (j12array[0] > IKPI)
37878 {
37879 j12array[0] -= IK2PI;
37880 }
37881 else if (j12array[0] < -IKPI)
37882 {
37883 j12array[0] += IK2PI;
37884 }
37885 j12valid[0] = true;
37886 for (int ij12 = 0; ij12 < 1;
37887 ++ij12)
37888 {
37889 if (!j12valid[ij12])
37890 {
37891 continue;
37892 }
37893 _ij12[0] = ij12;
37894 _ij12[1] = -1;
37895 for (int iij12 = ij12 + 1;
37896 iij12 < 1;
37897 ++iij12)
37898 {
37899 if (j12valid[iij12]
37900 && IKabs(
37901 cj12array[ij12]
37902 - cj12array
37903 [iij12])
37904 < IKFAST_SOLUTION_THRESH
37905 && IKabs(
37906 sj12array[ij12]
37907 - sj12array
37908 [iij12])
37909 < IKFAST_SOLUTION_THRESH)
37910 {
37911 j12valid[iij12] = false;
37912 _ij12[1] = iij12;
37913 break;
37914 }
37915 }
37916 j12 = j12array[ij12];
37917 cj12 = cj12array[ij12];
37918 sj12 = sj12array[ij12];
37919 {
37920 IkReal evalcond[8];
37921 IkReal x542 = IKcos(j12);
37922 IkReal x543 = IKsin(j12);
37923 IkReal x544
37924 = ((1.0) * x542);
37925 IkReal x545
37926 = ((-1.0) * x544);
37927 IkReal x546
37928 = ((1.0) * new_r02);
37929 evalcond[0]
37930 = (((new_r02 * x542))
37931 + new_r20);
37932 evalcond[1]
37933 = ((((-1.0) * x543))
37934 + new_r10);
37935 evalcond[2]
37936 = (new_r11 + x545);
37937 evalcond[3]
37938 = (((new_r22 * x543))
37939 + new_r01);
37940 evalcond[4]
37941 = ((((-1.0) * x543
37942 * x546))
37943 + new_r21);
37944 evalcond[5]
37945 = ((((-1.0) * new_r22
37946 * x544))
37947 + new_r00);
37948 evalcond[6]
37949 = (((new_r01
37950 * new_r22))
37951 + (((-1.0)
37952 * new_r21
37953 * x546))
37954 + x543);
37955 evalcond[7]
37956 = ((((-1.0) * new_r20
37957 * x546))
37958 + ((new_r00
37959 * new_r22))
37960 + x545);
37961 if (IKabs(evalcond[0])
37962 > IKFAST_EVALCOND_THRESH
37963 || IKabs(evalcond[1])
37964 > IKFAST_EVALCOND_THRESH
37965 || IKabs(evalcond[2])
37966 > IKFAST_EVALCOND_THRESH
37967 || IKabs(evalcond[3])
37968 > IKFAST_EVALCOND_THRESH
37969 || IKabs(evalcond[4])
37970 > IKFAST_EVALCOND_THRESH
37971 || IKabs(evalcond[5])
37972 > IKFAST_EVALCOND_THRESH
37973 || IKabs(evalcond[6])
37974 > IKFAST_EVALCOND_THRESH
37975 || IKabs(evalcond[7])
37976 > IKFAST_EVALCOND_THRESH)
37977 {
37978 continue;
37979 }
37980 }
37981
37982 {
37983 std::vector<
37984 IkSingleDOFSolutionBase<
37985 IkReal> >
37986 vinfos(7);
37987 vinfos[0].jointtype = 1;
37988 vinfos[0].foffset = j4;
37989 vinfos[0].indices[0]
37990 = _ij4[0];
37991 vinfos[0].indices[1]
37992 = _ij4[1];
37993 vinfos[0].maxsolutions
37994 = _nj4;
37995 vinfos[1].jointtype = 1;
37996 vinfos[1].foffset = j6;
37997 vinfos[1].indices[0]
37998 = _ij6[0];
37999 vinfos[1].indices[1]
38000 = _ij6[1];
38001 vinfos[1].maxsolutions
38002 = _nj6;
38003 vinfos[2].jointtype = 1;
38004 vinfos[2].foffset = j8;
38005 vinfos[2].indices[0]
38006 = _ij8[0];
38007 vinfos[2].indices[1]
38008 = _ij8[1];
38009 vinfos[2].maxsolutions
38010 = _nj8;
38011 vinfos[3].jointtype = 1;
38012 vinfos[3].foffset = j9;
38013 vinfos[3].indices[0]
38014 = _ij9[0];
38015 vinfos[3].indices[1]
38016 = _ij9[1];
38017 vinfos[3].maxsolutions
38018 = _nj9;
38019 vinfos[4].jointtype = 1;
38020 vinfos[4].foffset = j10;
38021 vinfos[4].indices[0]
38022 = _ij10[0];
38023 vinfos[4].indices[1]
38024 = _ij10[1];
38025 vinfos[4].maxsolutions
38026 = _nj10;
38027 vinfos[5].jointtype = 1;
38028 vinfos[5].foffset = j11;
38029 vinfos[5].indices[0]
38030 = _ij11[0];
38031 vinfos[5].indices[1]
38032 = _ij11[1];
38033 vinfos[5].maxsolutions
38034 = _nj11;
38035 vinfos[6].jointtype = 1;
38036 vinfos[6].foffset = j12;
38037 vinfos[6].indices[0]
38038 = _ij12[0];
38039 vinfos[6].indices[1]
38040 = _ij12[1];
38041 vinfos[6].maxsolutions
38042 = _nj12;
38043 std::vector<int> vfree(0);
38044 solutions.AddSolution(
38045 vinfos, vfree);
38046 }
38047 }
38048 }
38049 }
38050 } while (0);
38051 if (bgotonextstatement)
38052 {
38053 bool bgotonextstatement = true;
38054 do
38055 {
38056 IkReal x547 = ((1.0) * cj11);
38057 IkReal x548
38058 = ((((-1.0) * x547))
38059 + new_r22);
38060 IkReal x549 = ((1.0) * sj11);
38061 evalcond[0]
38062 = ((-3.14159265358979)
38063 + (IKfmod(
38064 ((3.14159265358979)
38065 + (IKabs((
38066 (-3.14159265358979)
38067 + j10)))),
38068 6.28318530717959)));
38069 evalcond[1] = x548;
38070 evalcond[2] = x548;
38071 evalcond[3] = (sj11 + new_r02);
38072 evalcond[4] = new_r12;
38073 evalcond[5]
38074 = ((((-1.0) * x549))
38075 + (((-1.0) * (1.0)
38076 * new_r02)));
38077 evalcond[6]
38078 = ((((-1.0) * new_r22
38079 * x549))
38080 + (((-1.0) * new_r02
38081 * x547)));
38082 evalcond[7]
38083 = ((((-1.0) * new_r20
38084 * x547))
38085 + ((new_r00 * sj11)));
38086 evalcond[8]
38087 = ((((-1.0) * new_r21
38088 * x547))
38089 + ((new_r01 * sj11)));
38090 evalcond[9]
38091 = ((1.0)
38092 + ((new_r02 * sj11))
38093 + (((-1.0) * new_r22
38094 * x547)));
38095 if (IKabs(evalcond[0])
38096 < 0.0000010000000000
38097 && IKabs(evalcond[1])
38098 < 0.0000010000000000
38099 && IKabs(evalcond[2])
38100 < 0.0000010000000000
38101 && IKabs(evalcond[3])
38102 < 0.0000010000000000
38103 && IKabs(evalcond[4])
38104 < 0.0000010000000000
38105 && IKabs(evalcond[5])
38106 < 0.0000010000000000
38107 && IKabs(evalcond[6])
38108 < 0.0000010000000000
38109 && IKabs(evalcond[7])
38110 < 0.0000010000000000
38111 && IKabs(evalcond[8])
38112 < 0.0000010000000000
38113 && IKabs(evalcond[9])
38114 < 0.0000010000000000)
38115 {
38116 bgotonextstatement = false;
38117 {
38118 IkReal j12array[1],
38119 cj12array[1],
38120 sj12array[1];
38121 bool j12valid[1] = {false};
38122 _nj12 = 1;
38123 CheckValue<IkReal> x550
38124 = IKatan2WithCheck(
38125 IkReal((
38126 (-1.0)
38127 * (((1.0)
38128 * new_r21)))),
38129 new_r20,
38130 IKFAST_ATAN2_MAGTHRESH);
38131 if (!x550.valid)
38132 {
38133 continue;
38134 }
38135 CheckValue<IkReal> x551
38136 = IKPowWithIntegerCheck(
38137 IKsign(new_r02),
38138 -1);
38139 if (!x551.valid)
38140 {
38141 continue;
38142 }
38143 j12array[0]
38144 = ((-1.5707963267949)
38145 + (x550.value)
38146 + (((1.5707963267949)
38147 * (x551.value))));
38148 sj12array[0]
38149 = IKsin(j12array[0]);
38150 cj12array[0]
38151 = IKcos(j12array[0]);
38152 if (j12array[0] > IKPI)
38153 {
38154 j12array[0] -= IK2PI;
38155 }
38156 else if (
38157 j12array[0] < -IKPI)
38158 {
38159 j12array[0] += IK2PI;
38160 }
38161 j12valid[0] = true;
38162 for (int ij12 = 0; ij12 < 1;
38163 ++ij12)
38164 {
38165 if (!j12valid[ij12])
38166 {
38167 continue;
38168 }
38169 _ij12[0] = ij12;
38170 _ij12[1] = -1;
38171 for (int iij12 = ij12 + 1;
38172 iij12 < 1;
38173 ++iij12)
38174 {
38175 if (j12valid[iij12]
38176 && IKabs(
38177 cj12array
38178 [ij12]
38179 - cj12array
38180 [iij12])
38181 < IKFAST_SOLUTION_THRESH
38182 && IKabs(
38183 sj12array
38184 [ij12]
38185 - sj12array
38186 [iij12])
38187 < IKFAST_SOLUTION_THRESH)
38188 {
38189 j12valid[iij12]
38190 = false;
38191 _ij12[1] = iij12;
38192 break;
38193 }
38194 }
38195 j12 = j12array[ij12];
38196 cj12 = cj12array[ij12];
38197 sj12 = sj12array[ij12];
38198 {
38199 IkReal evalcond[8];
38200 IkReal x552
38201 = IKsin(j12);
38202 IkReal x553
38203 = ((1.0)
38204 * (IKcos(j12)));
38205 IkReal x554
38206 = ((-1.0) * x553);
38207 IkReal x555
38208 = ((1.0) * new_r01);
38209 IkReal x556
38210 = ((1.0) * new_r00);
38211 evalcond[0]
38212 = (((new_r02
38213 * x552))
38214 + new_r21);
38215 evalcond[1]
38216 = ((((-1.0)
38217 * new_r02
38218 * x553))
38219 + new_r20);
38220 evalcond[2]
38221 = ((((-1.0) * (1.0)
38222 * new_r10))
38223 + (((-1.0)
38224 * x552)));
38225 evalcond[3]
38226 = (x554
38227 + (((-1.0)
38228 * (1.0)
38229 * new_r11)));
38230 evalcond[4]
38231 = (((new_r22
38232 * x552))
38233 + (((-1.0)
38234 * x555)));
38235 evalcond[5]
38236 = ((((-1.0) * x556))
38237 + (((-1.0)
38238 * new_r22
38239 * x553)));
38240 evalcond[6]
38241 = (((new_r02
38242 * new_r21))
38243 + (((-1.0)
38244 * new_r22
38245 * x555))
38246 + x552);
38247 evalcond[7]
38248 = (((new_r02
38249 * new_r20))
38250 + x554
38251 + (((-1.0)
38252 * new_r22
38253 * x556)));
38254 if (IKabs(evalcond[0])
38255 > IKFAST_EVALCOND_THRESH
38256 || IKabs(
38257 evalcond[1])
38258 > IKFAST_EVALCOND_THRESH
38259 || IKabs(
38260 evalcond[2])
38261 > IKFAST_EVALCOND_THRESH
38262 || IKabs(
38263 evalcond[3])
38264 > IKFAST_EVALCOND_THRESH
38265 || IKabs(
38266 evalcond[4])
38267 > IKFAST_EVALCOND_THRESH
38268 || IKabs(
38269 evalcond[5])
38270 > IKFAST_EVALCOND_THRESH
38271 || IKabs(
38272 evalcond[6])
38273 > IKFAST_EVALCOND_THRESH
38274 || IKabs(
38275 evalcond[7])
38276 > IKFAST_EVALCOND_THRESH)
38277 {
38278 continue;
38279 }
38280 }
38281
38282 {
38283 std::vector<
38284 IkSingleDOFSolutionBase<
38285 IkReal> >
38286 vinfos(7);
38287 vinfos[0].jointtype = 1;
38288 vinfos[0].foffset = j4;
38289 vinfos[0].indices[0]
38290 = _ij4[0];
38291 vinfos[0].indices[1]
38292 = _ij4[1];
38293 vinfos[0].maxsolutions
38294 = _nj4;
38295 vinfos[1].jointtype = 1;
38296 vinfos[1].foffset = j6;
38297 vinfos[1].indices[0]
38298 = _ij6[0];
38299 vinfos[1].indices[1]
38300 = _ij6[1];
38301 vinfos[1].maxsolutions
38302 = _nj6;
38303 vinfos[2].jointtype = 1;
38304 vinfos[2].foffset = j8;
38305 vinfos[2].indices[0]
38306 = _ij8[0];
38307 vinfos[2].indices[1]
38308 = _ij8[1];
38309 vinfos[2].maxsolutions
38310 = _nj8;
38311 vinfos[3].jointtype = 1;
38312 vinfos[3].foffset = j9;
38313 vinfos[3].indices[0]
38314 = _ij9[0];
38315 vinfos[3].indices[1]
38316 = _ij9[1];
38317 vinfos[3].maxsolutions
38318 = _nj9;
38319 vinfos[4].jointtype = 1;
38320 vinfos[4].foffset = j10;
38321 vinfos[4].indices[0]
38322 = _ij10[0];
38323 vinfos[4].indices[1]
38324 = _ij10[1];
38325 vinfos[4].maxsolutions
38326 = _nj10;
38327 vinfos[5].jointtype = 1;
38328 vinfos[5].foffset = j11;
38329 vinfos[5].indices[0]
38330 = _ij11[0];
38331 vinfos[5].indices[1]
38332 = _ij11[1];
38333 vinfos[5].maxsolutions
38334 = _nj11;
38335 vinfos[6].jointtype = 1;
38336 vinfos[6].foffset = j12;
38337 vinfos[6].indices[0]
38338 = _ij12[0];
38339 vinfos[6].indices[1]
38340 = _ij12[1];
38341 vinfos[6].maxsolutions
38342 = _nj12;
38343 std::vector<int> vfree(
38344 0);
38345 solutions.AddSolution(
38346 vinfos, vfree);
38347 }
38348 }
38349 }
38350 }
38351 } while (0);
38352 if (bgotonextstatement)
38353 {
38354 bool bgotonextstatement = true;
38355 do
38356 {
38357 if (1)
38358 {
38359 bgotonextstatement = false;
38360 continue; // branch miss
38361 // [j12]
38362 }
38363 } while (0);
38364 if (bgotonextstatement)
38365 {
38366 }
38367 }
38368 }
38369 }
38370 }
38371 }
38372 }
38373 }
38374 }
38375 }
38376 }
38377 else
38378 {
38379 {
38380 IkReal j12array[1], cj12array[1], sj12array[1];
38381 bool j12valid[1] = {false};
38382 _nj12 = 1;
38383 CheckValue<IkReal> x558
38384 = IKPowWithIntegerCheck(sj11, -1);
38385 if (!x558.valid)
38386 {
38387 continue;
38388 }
38389 IkReal x557 = x558.value;
38390 CheckValue<IkReal> x559
38391 = IKPowWithIntegerCheck(cj10, -1);
38392 if (!x559.valid)
38393 {
38394 continue;
38395 }
38396 CheckValue<IkReal> x560
38397 = IKPowWithIntegerCheck(cj11, -1);
38398 if (!x560.valid)
38399 {
38400 continue;
38401 }
38402 if (IKabs(
38403 (x557 * (x559.value) * (x560.value)
38404 * (((((-1.0) * (1.0) * new_r01 * sj11))
38405 + ((new_r20 * sj10))))))
38406 < IKFAST_ATAN2_MAGTHRESH
38407 && IKabs(((-1.0) * new_r20 * x557))
38408 < IKFAST_ATAN2_MAGTHRESH
38409 && IKabs(
38410 IKsqr(
38411 (x557 * (x559.value)
38412 * (x560.value)
38413 * (((((-1.0) * (1.0) * new_r01
38414 * sj11))
38415 + ((new_r20 * sj10))))))
38416 + IKsqr(((-1.0) * new_r20 * x557))
38417 - 1)
38418 <= IKFAST_SINCOS_THRESH)
38419 continue;
38420 j12array[0] = IKatan2(
38421 (x557 * (x559.value) * (x560.value)
38422 * (((((-1.0) * (1.0) * new_r01 * sj11))
38423 + ((new_r20 * sj10))))),
38424 ((-1.0) * new_r20 * x557));
38425 sj12array[0] = IKsin(j12array[0]);
38426 cj12array[0] = IKcos(j12array[0]);
38427 if (j12array[0] > IKPI)
38428 {
38429 j12array[0] -= IK2PI;
38430 }
38431 else if (j12array[0] < -IKPI)
38432 {
38433 j12array[0] += IK2PI;
38434 }
38435 j12valid[0] = true;
38436 for (int ij12 = 0; ij12 < 1; ++ij12)
38437 {
38438 if (!j12valid[ij12])
38439 {
38440 continue;
38441 }
38442 _ij12[0] = ij12;
38443 _ij12[1] = -1;
38444 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38445 {
38446 if (j12valid[iij12]
38447 && IKabs(
38448 cj12array[ij12]
38449 - cj12array[iij12])
38450 < IKFAST_SOLUTION_THRESH
38451 && IKabs(
38452 sj12array[ij12]
38453 - sj12array[iij12])
38454 < IKFAST_SOLUTION_THRESH)
38455 {
38456 j12valid[iij12] = false;
38457 _ij12[1] = iij12;
38458 break;
38459 }
38460 }
38461 j12 = j12array[ij12];
38462 cj12 = cj12array[ij12];
38463 sj12 = sj12array[ij12];
38464 {
38465 IkReal evalcond[12];
38466 IkReal x561 = IKcos(j12);
38467 IkReal x562 = IKsin(j12);
38468 IkReal x563 = ((1.0) * sj11);
38469 IkReal x564 = (cj10 * new_r01);
38470 IkReal x565 = (new_r11 * sj10);
38471 IkReal x566 = (cj11 * x562);
38472 IkReal x567 = ((1.0) * sj10);
38473 IkReal x568 = ((1.0) * x562);
38474 IkReal x569 = ((1.0) * x561);
38475 IkReal x570 = ((-1.0) * x569);
38476 IkReal x571 = (cj10 * new_r00);
38477 IkReal x572 = (new_r10 * sj10);
38478 IkReal x573 = (cj10 * x569);
38479 evalcond[0] = (((sj11 * x561)) + new_r20);
38480 evalcond[1]
38481 = ((((-1.0) * x562 * x563)) + new_r21);
38482 evalcond[2] = (x564 + x565 + x566);
38483 evalcond[3]
38484 = (((cj10 * new_r10))
38485 + (((-1.0) * x568))
38486 + (((-1.0) * new_r00 * x567)));
38487 evalcond[4]
38488 = (((cj10 * new_r11)) + x570
38489 + (((-1.0) * new_r01 * x567)));
38490 evalcond[5]
38491 = (((cj10 * x566)) + new_r01
38492 + ((sj10 * x561)));
38493 evalcond[6]
38494 = (x572 + x571
38495 + (((-1.0) * cj11 * x569)));
38496 evalcond[7]
38497 = (((sj10 * x562))
38498 + (((-1.0) * cj11 * x573))
38499 + new_r00);
38500 evalcond[8]
38501 = ((((-1.0) * x573)) + new_r11
38502 + ((sj10 * x566)));
38503 evalcond[9]
38504 = ((((-1.0) * cj10 * x568))
38505 + (((-1.0) * cj11 * x561 * x567))
38506 + new_r10);
38507 evalcond[10]
38508 = ((((-1.0) * new_r21 * x563))
38509 + ((cj11 * x565)) + ((cj11 * x564))
38510 + x562);
38511 evalcond[11]
38512 = (x570 + ((cj11 * x572))
38513 + ((cj11 * x571))
38514 + (((-1.0) * new_r20 * x563)));
38515 if (IKabs(evalcond[0])
38516 > IKFAST_EVALCOND_THRESH
38517 || IKabs(evalcond[1])
38518 > IKFAST_EVALCOND_THRESH
38519 || IKabs(evalcond[2])
38520 > IKFAST_EVALCOND_THRESH
38521 || IKabs(evalcond[3])
38522 > IKFAST_EVALCOND_THRESH
38523 || IKabs(evalcond[4])
38524 > IKFAST_EVALCOND_THRESH
38525 || IKabs(evalcond[5])
38526 > IKFAST_EVALCOND_THRESH
38527 || IKabs(evalcond[6])
38528 > IKFAST_EVALCOND_THRESH
38529 || IKabs(evalcond[7])
38530 > IKFAST_EVALCOND_THRESH
38531 || IKabs(evalcond[8])
38532 > IKFAST_EVALCOND_THRESH
38533 || IKabs(evalcond[9])
38534 > IKFAST_EVALCOND_THRESH
38535 || IKabs(evalcond[10])
38536 > IKFAST_EVALCOND_THRESH
38537 || IKabs(evalcond[11])
38538 > IKFAST_EVALCOND_THRESH)
38539 {
38540 continue;
38541 }
38542 }
38543
38544 {
38545 std::vector<
38546 IkSingleDOFSolutionBase<IkReal> >
38547 vinfos(7);
38548 vinfos[0].jointtype = 1;
38549 vinfos[0].foffset = j4;
38550 vinfos[0].indices[0] = _ij4[0];
38551 vinfos[0].indices[1] = _ij4[1];
38552 vinfos[0].maxsolutions = _nj4;
38553 vinfos[1].jointtype = 1;
38554 vinfos[1].foffset = j6;
38555 vinfos[1].indices[0] = _ij6[0];
38556 vinfos[1].indices[1] = _ij6[1];
38557 vinfos[1].maxsolutions = _nj6;
38558 vinfos[2].jointtype = 1;
38559 vinfos[2].foffset = j8;
38560 vinfos[2].indices[0] = _ij8[0];
38561 vinfos[2].indices[1] = _ij8[1];
38562 vinfos[2].maxsolutions = _nj8;
38563 vinfos[3].jointtype = 1;
38564 vinfos[3].foffset = j9;
38565 vinfos[3].indices[0] = _ij9[0];
38566 vinfos[3].indices[1] = _ij9[1];
38567 vinfos[3].maxsolutions = _nj9;
38568 vinfos[4].jointtype = 1;
38569 vinfos[4].foffset = j10;
38570 vinfos[4].indices[0] = _ij10[0];
38571 vinfos[4].indices[1] = _ij10[1];
38572 vinfos[4].maxsolutions = _nj10;
38573 vinfos[5].jointtype = 1;
38574 vinfos[5].foffset = j11;
38575 vinfos[5].indices[0] = _ij11[0];
38576 vinfos[5].indices[1] = _ij11[1];
38577 vinfos[5].maxsolutions = _nj11;
38578 vinfos[6].jointtype = 1;
38579 vinfos[6].foffset = j12;
38580 vinfos[6].indices[0] = _ij12[0];
38581 vinfos[6].indices[1] = _ij12[1];
38582 vinfos[6].maxsolutions = _nj12;
38583 std::vector<int> vfree(0);
38584 solutions.AddSolution(vinfos, vfree);
38585 }
38586 }
38587 }
38588 }
38589 }
38590 }
38591 else
38592 {
38593 {
38594 IkReal j12array[1], cj12array[1], sj12array[1];
38595 bool j12valid[1] = {false};
38596 _nj12 = 1;
38597 CheckValue<IkReal> x576
38598 = IKPowWithIntegerCheck(sj11, -1);
38599 if (!x576.valid)
38600 {
38601 continue;
38602 }
38603 IkReal x574 = x576.value;
38604 IkReal x575 = ((1.0) * new_r20);
38605 CheckValue<IkReal> x577
38606 = IKPowWithIntegerCheck(sj10, -1);
38607 if (!x577.valid)
38608 {
38609 continue;
38610 }
38611 if (IKabs(
38612 (x574 * (x577.value)
38613 * (((((-1.0) * cj10 * cj11 * x575))
38614 + (((-1.0) * (1.0) * new_r00
38615 * sj11))))))
38616 < IKFAST_ATAN2_MAGTHRESH
38617 && IKabs(((-1.0) * x574 * x575))
38618 < IKFAST_ATAN2_MAGTHRESH
38619 && IKabs(
38620 IKsqr(
38621 (x574 * (x577.value)
38622 * (((((-1.0) * cj10 * cj11 * x575))
38623 + (((-1.0) * (1.0) * new_r00
38624 * sj11))))))
38625 + IKsqr(((-1.0) * x574 * x575)) - 1)
38626 <= IKFAST_SINCOS_THRESH)
38627 continue;
38628 j12array[0] = IKatan2(
38629 (x574 * (x577.value)
38630 * (((((-1.0) * cj10 * cj11 * x575))
38631 + (((-1.0) * (1.0) * new_r00 * sj11))))),
38632 ((-1.0) * x574 * x575));
38633 sj12array[0] = IKsin(j12array[0]);
38634 cj12array[0] = IKcos(j12array[0]);
38635 if (j12array[0] > IKPI)
38636 {
38637 j12array[0] -= IK2PI;
38638 }
38639 else if (j12array[0] < -IKPI)
38640 {
38641 j12array[0] += IK2PI;
38642 }
38643 j12valid[0] = true;
38644 for (int ij12 = 0; ij12 < 1; ++ij12)
38645 {
38646 if (!j12valid[ij12])
38647 {
38648 continue;
38649 }
38650 _ij12[0] = ij12;
38651 _ij12[1] = -1;
38652 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38653 {
38654 if (j12valid[iij12]
38655 && IKabs(cj12array[ij12] - cj12array[iij12])
38656 < IKFAST_SOLUTION_THRESH
38657 && IKabs(sj12array[ij12] - sj12array[iij12])
38658 < IKFAST_SOLUTION_THRESH)
38659 {
38660 j12valid[iij12] = false;
38661 _ij12[1] = iij12;
38662 break;
38663 }
38664 }
38665 j12 = j12array[ij12];
38666 cj12 = cj12array[ij12];
38667 sj12 = sj12array[ij12];
38668 {
38669 IkReal evalcond[12];
38670 IkReal x578 = IKcos(j12);
38671 IkReal x579 = IKsin(j12);
38672 IkReal x580 = ((1.0) * sj11);
38673 IkReal x581 = (cj10 * new_r01);
38674 IkReal x582 = (new_r11 * sj10);
38675 IkReal x583 = (cj11 * x579);
38676 IkReal x584 = ((1.0) * sj10);
38677 IkReal x585 = ((1.0) * x579);
38678 IkReal x586 = ((1.0) * x578);
38679 IkReal x587 = ((-1.0) * x586);
38680 IkReal x588 = (cj10 * new_r00);
38681 IkReal x589 = (new_r10 * sj10);
38682 IkReal x590 = (cj10 * x586);
38683 evalcond[0] = (((sj11 * x578)) + new_r20);
38684 evalcond[1]
38685 = ((((-1.0) * x579 * x580)) + new_r21);
38686 evalcond[2] = (x581 + x582 + x583);
38687 evalcond[3]
38688 = (((cj10 * new_r10)) + (((-1.0) * x585))
38689 + (((-1.0) * new_r00 * x584)));
38690 evalcond[4]
38691 = (((cj10 * new_r11))
38692 + (((-1.0) * new_r01 * x584)) + x587);
38693 evalcond[5]
38694 = (((sj10 * x578)) + ((cj10 * x583))
38695 + new_r01);
38696 evalcond[6]
38697 = ((((-1.0) * cj11 * x586)) + x589 + x588);
38698 evalcond[7]
38699 = (new_r00 + ((sj10 * x579))
38700 + (((-1.0) * cj11 * x590)));
38701 evalcond[8]
38702 = ((((-1.0) * x590)) + ((sj10 * x583))
38703 + new_r11);
38704 evalcond[9]
38705 = ((((-1.0) * cj10 * x585))
38706 + (((-1.0) * cj11 * x578 * x584))
38707 + new_r10);
38708 evalcond[10]
38709 = (x579 + ((cj11 * x581)) + ((cj11 * x582))
38710 + (((-1.0) * new_r21 * x580)));
38711 evalcond[11]
38712 = (((cj11 * x589))
38713 + (((-1.0) * new_r20 * x580))
38714 + ((cj11 * x588)) + x587);
38715 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
38716 || IKabs(evalcond[1])
38717 > IKFAST_EVALCOND_THRESH
38718 || IKabs(evalcond[2])
38719 > IKFAST_EVALCOND_THRESH
38720 || IKabs(evalcond[3])
38721 > IKFAST_EVALCOND_THRESH
38722 || IKabs(evalcond[4])
38723 > IKFAST_EVALCOND_THRESH
38724 || IKabs(evalcond[5])
38725 > IKFAST_EVALCOND_THRESH
38726 || IKabs(evalcond[6])
38727 > IKFAST_EVALCOND_THRESH
38728 || IKabs(evalcond[7])
38729 > IKFAST_EVALCOND_THRESH
38730 || IKabs(evalcond[8])
38731 > IKFAST_EVALCOND_THRESH
38732 || IKabs(evalcond[9])
38733 > IKFAST_EVALCOND_THRESH
38734 || IKabs(evalcond[10])
38735 > IKFAST_EVALCOND_THRESH
38736 || IKabs(evalcond[11])
38737 > IKFAST_EVALCOND_THRESH)
38738 {
38739 continue;
38740 }
38741 }
38742
38743 {
38744 std::vector<IkSingleDOFSolutionBase<IkReal> >
38745 vinfos(7);
38746 vinfos[0].jointtype = 1;
38747 vinfos[0].foffset = j4;
38748 vinfos[0].indices[0] = _ij4[0];
38749 vinfos[0].indices[1] = _ij4[1];
38750 vinfos[0].maxsolutions = _nj4;
38751 vinfos[1].jointtype = 1;
38752 vinfos[1].foffset = j6;
38753 vinfos[1].indices[0] = _ij6[0];
38754 vinfos[1].indices[1] = _ij6[1];
38755 vinfos[1].maxsolutions = _nj6;
38756 vinfos[2].jointtype = 1;
38757 vinfos[2].foffset = j8;
38758 vinfos[2].indices[0] = _ij8[0];
38759 vinfos[2].indices[1] = _ij8[1];
38760 vinfos[2].maxsolutions = _nj8;
38761 vinfos[3].jointtype = 1;
38762 vinfos[3].foffset = j9;
38763 vinfos[3].indices[0] = _ij9[0];
38764 vinfos[3].indices[1] = _ij9[1];
38765 vinfos[3].maxsolutions = _nj9;
38766 vinfos[4].jointtype = 1;
38767 vinfos[4].foffset = j10;
38768 vinfos[4].indices[0] = _ij10[0];
38769 vinfos[4].indices[1] = _ij10[1];
38770 vinfos[4].maxsolutions = _nj10;
38771 vinfos[5].jointtype = 1;
38772 vinfos[5].foffset = j11;
38773 vinfos[5].indices[0] = _ij11[0];
38774 vinfos[5].indices[1] = _ij11[1];
38775 vinfos[5].maxsolutions = _nj11;
38776 vinfos[6].jointtype = 1;
38777 vinfos[6].foffset = j12;
38778 vinfos[6].indices[0] = _ij12[0];
38779 vinfos[6].indices[1] = _ij12[1];
38780 vinfos[6].maxsolutions = _nj12;
38781 std::vector<int> vfree(0);
38782 solutions.AddSolution(vinfos, vfree);
38783 }
38784 }
38785 }
38786 }
38787 }
38788 }
38789 else
38790 {
38791 {
38792 IkReal j12array[1], cj12array[1], sj12array[1];
38793 bool j12valid[1] = {false};
38794 _nj12 = 1;
38795 CheckValue<IkReal> x591
38796 = IKPowWithIntegerCheck(IKsign(sj11), -1);
38797 if (!x591.valid)
38798 {
38799 continue;
38800 }
38801 CheckValue<IkReal> x592 = IKatan2WithCheck(
38802 IkReal(new_r21),
38803 ((-1.0) * (((1.0) * new_r20))),
38804 IKFAST_ATAN2_MAGTHRESH);
38805 if (!x592.valid)
38806 {
38807 continue;
38808 }
38809 j12array[0]
38810 = ((-1.5707963267949)
38811 + (((1.5707963267949) * (x591.value)))
38812 + (x592.value));
38813 sj12array[0] = IKsin(j12array[0]);
38814 cj12array[0] = IKcos(j12array[0]);
38815 if (j12array[0] > IKPI)
38816 {
38817 j12array[0] -= IK2PI;
38818 }
38819 else if (j12array[0] < -IKPI)
38820 {
38821 j12array[0] += IK2PI;
38822 }
38823 j12valid[0] = true;
38824 for (int ij12 = 0; ij12 < 1; ++ij12)
38825 {
38826 if (!j12valid[ij12])
38827 {
38828 continue;
38829 }
38830 _ij12[0] = ij12;
38831 _ij12[1] = -1;
38832 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38833 {
38834 if (j12valid[iij12]
38835 && IKabs(cj12array[ij12] - cj12array[iij12])
38836 < IKFAST_SOLUTION_THRESH
38837 && IKabs(sj12array[ij12] - sj12array[iij12])
38838 < IKFAST_SOLUTION_THRESH)
38839 {
38840 j12valid[iij12] = false;
38841 _ij12[1] = iij12;
38842 break;
38843 }
38844 }
38845 j12 = j12array[ij12];
38846 cj12 = cj12array[ij12];
38847 sj12 = sj12array[ij12];
38848 {
38849 IkReal evalcond[12];
38850 IkReal x593 = IKcos(j12);
38851 IkReal x594 = IKsin(j12);
38852 IkReal x595 = ((1.0) * sj11);
38853 IkReal x596 = (cj10 * new_r01);
38854 IkReal x597 = (new_r11 * sj10);
38855 IkReal x598 = (cj11 * x594);
38856 IkReal x599 = ((1.0) * sj10);
38857 IkReal x600 = ((1.0) * x594);
38858 IkReal x601 = ((1.0) * x593);
38859 IkReal x602 = ((-1.0) * x601);
38860 IkReal x603 = (cj10 * new_r00);
38861 IkReal x604 = (new_r10 * sj10);
38862 IkReal x605 = (cj10 * x601);
38863 evalcond[0] = (new_r20 + ((sj11 * x593)));
38864 evalcond[1] = ((((-1.0) * x594 * x595)) + new_r21);
38865 evalcond[2] = (x598 + x596 + x597);
38866 evalcond[3]
38867 = (((cj10 * new_r10)) + (((-1.0) * x600))
38868 + (((-1.0) * new_r00 * x599)));
38869 evalcond[4]
38870 = (((cj10 * new_r11))
38871 + (((-1.0) * new_r01 * x599)) + x602);
38872 evalcond[5]
38873 = (((cj10 * x598)) + ((sj10 * x593)) + new_r01);
38874 evalcond[6]
38875 = ((((-1.0) * cj11 * x601)) + x603 + x604);
38876 evalcond[7]
38877 = ((((-1.0) * cj11 * x605)) + ((sj10 * x594))
38878 + new_r00);
38879 evalcond[8]
38880 = (((sj10 * x598)) + new_r11
38881 + (((-1.0) * x605)));
38882 evalcond[9]
38883 = ((((-1.0) * cj10 * x600))
38884 + (((-1.0) * cj11 * x593 * x599)) + new_r10);
38885 evalcond[10]
38886 = (((cj11 * x597)) + x594
38887 + (((-1.0) * new_r21 * x595))
38888 + ((cj11 * x596)));
38889 evalcond[11]
38890 = (((cj11 * x603)) + x602 + ((cj11 * x604))
38891 + (((-1.0) * new_r20 * x595)));
38892 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
38893 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
38894 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
38895 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
38896 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
38897 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
38898 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
38899 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH
38900 || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH
38901 || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH
38902 || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH
38903 || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
38904 {
38905 continue;
38906 }
38907 }
38908
38909 {
38910 std::vector<IkSingleDOFSolutionBase<IkReal> >
38911 vinfos(7);
38912 vinfos[0].jointtype = 1;
38913 vinfos[0].foffset = j4;
38914 vinfos[0].indices[0] = _ij4[0];
38915 vinfos[0].indices[1] = _ij4[1];
38916 vinfos[0].maxsolutions = _nj4;
38917 vinfos[1].jointtype = 1;
38918 vinfos[1].foffset = j6;
38919 vinfos[1].indices[0] = _ij6[0];
38920 vinfos[1].indices[1] = _ij6[1];
38921 vinfos[1].maxsolutions = _nj6;
38922 vinfos[2].jointtype = 1;
38923 vinfos[2].foffset = j8;
38924 vinfos[2].indices[0] = _ij8[0];
38925 vinfos[2].indices[1] = _ij8[1];
38926 vinfos[2].maxsolutions = _nj8;
38927 vinfos[3].jointtype = 1;
38928 vinfos[3].foffset = j9;
38929 vinfos[3].indices[0] = _ij9[0];
38930 vinfos[3].indices[1] = _ij9[1];
38931 vinfos[3].maxsolutions = _nj9;
38932 vinfos[4].jointtype = 1;
38933 vinfos[4].foffset = j10;
38934 vinfos[4].indices[0] = _ij10[0];
38935 vinfos[4].indices[1] = _ij10[1];
38936 vinfos[4].maxsolutions = _nj10;
38937 vinfos[5].jointtype = 1;
38938 vinfos[5].foffset = j11;
38939 vinfos[5].indices[0] = _ij11[0];
38940 vinfos[5].indices[1] = _ij11[1];
38941 vinfos[5].maxsolutions = _nj11;
38942 vinfos[6].jointtype = 1;
38943 vinfos[6].foffset = j12;
38944 vinfos[6].indices[0] = _ij12[0];
38945 vinfos[6].indices[1] = _ij12[1];
38946 vinfos[6].maxsolutions = _nj12;
38947 std::vector<int> vfree(0);
38948 solutions.AddSolution(vinfos, vfree);
38949 }
38950 }
38951 }
38952 }
38953 }
38954 }
38955 }
38956 }
38957 }
38958 }
38959 }
38960 }
38961 }
38962 };
38963
38964 /// solves the inverse kinematics equations.
38965 /// \param pfree is an array specifying the free joints of the chain.
38966 IKFAST_API bool ComputeIk(
38967 const IkReal* eetrans,
38968 const IkReal* eerot,
38969 const IkReal* pfree,
38970 IkSolutionListBase<IkReal>& solutions)
38971 {
38972 IKSolver solver;
38973 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
38974 }
38975
38976 IKFAST_API bool ComputeIk2(
38977 const IkReal* eetrans,
38978 const IkReal* eerot,
38979 const IkReal* pfree,
38980 IkSolutionListBase<IkReal>& solutions,
38981 void* pOpenRAVEManip)
38982 {
38983 IKSolver solver;
38984 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
38985 }
38986
38987 IKFAST_API const char* GetKinematicsHash()
38988 {
38989 return "268c2c509bc1bb657da055f0ef2eb7e1";
38990 }
38991
38992 IKFAST_API const char* GetIkFastVersion()
38993 {
38994 return IKFAST_STRINGIZE(IKFAST_VERSION);
38995 }
38996
38997 #ifdef IKFAST_NAMESPACE
38998 } // end namespace
38999 #endif
39000
39001 //==============================================================================
39002 SharedLibraryWamIkFast::SharedLibraryWamIkFast(
39003 dart::dynamics::InverseKinematics* ik,
39004 const std::vector<std::size_t>& dofMap,
39005 const std::vector<std::size_t>& freeDofMap,
39006 const std::string& methodName,
39007 const dart::dynamics::InverseKinematics::Analytical::Properties& properties)
39008 : IkFast{ik, dofMap, freeDofMap, methodName, properties}
39009 {
39010 // Do nothing
39011 }
39012
39013 //==============================================================================
39014 std::unique_ptr<dart::dynamics::InverseKinematics::GradientMethod>
39015 SharedLibraryWamIkFast::clone(dart::dynamics::InverseKinematics* newIK) const
39016 {
39017 return std::make_unique<SharedLibraryWamIkFast>(
39018 newIK, mDofs, mFreeDofs, getMethodName(), getAnalyticalProperties());
39019 }
39020
39021 //==============================================================================
39022 int SharedLibraryWamIkFast::getNumFreeParameters() const
39023 {
39024 return GetNumFreeParameters();
39025 }
39026
39027 //==============================================================================
39028 int* SharedLibraryWamIkFast::getFreeParameters() const
39029 {
39030 return GetFreeParameters();
39031 }
39032
39033 //==============================================================================
39034 int SharedLibraryWamIkFast::getNumJoints() const
39035 {
39036 return GetNumJoints();
39037 }
39038
39039 //==============================================================================
39040 int SharedLibraryWamIkFast::getIkRealSize() const
39041 {
39042 return GetIkRealSize();
39043 }
39044
39045 //==============================================================================
39046 int SharedLibraryWamIkFast::getIkType() const
39047 {
39048 return GetIkType();
39049 }
39050
39051 //==============================================================================
39052 bool SharedLibraryWamIkFast::computeIk(
39053 const IkReal* targetTranspose,
39054 const IkReal* targetRotation,
39055 const IkReal* freeParams,
39056 ikfast::IkSolutionListBase<IkReal>& solutions)
39057 {
39058 return ComputeIk(targetTranspose, targetRotation, freeParams, solutions);
39059 }
39060
39061 //==============================================================================
39062 void SharedLibraryWamIkFast::computeFk(
39063 const IkReal* parameters, IkReal* targetTranspose, IkReal* targetRotation)
39064 {
39065 ComputeFk(parameters, targetTranspose, targetRotation);
39066 }
39067
39068 //==============================================================================
39069 const char* SharedLibraryWamIkFast::getKinematicsHash()
39070 {
39071 return GetKinematicsHash();
39072 }
39073
39074 //==============================================================================
39075 const char* SharedLibraryWamIkFast::getIkFastVersion()
39076 {
39077 return GetIkFastVersion();
39078 }