1 /*
2 * Copyright (c) 2011-2021, 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
IKabs(float f)155 inline float IKabs(float f)
156 {
157 return fabsf(f);
158 }
IKabs(double f)159 inline double IKabs(double f)
160 {
161 return fabs(f);
162 }
163
IKsqr(float f)164 inline float IKsqr(float f)
165 {
166 return f * f;
167 }
IKsqr(double f)168 inline double IKsqr(double f)
169 {
170 return f * f;
171 }
172
IKlog(float f)173 inline float IKlog(float f)
174 {
175 return logf(f);
176 }
IKlog(double f)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
IKasin(float f)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 }
IKasin(double f)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)
IKfmod(float x,float 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)
IKfmod(double x,double 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
IKacos(float f)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 }
IKacos(double f)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 }
IKsin(float f)272 inline float IKsin(float f)
273 {
274 return sinf(f);
275 }
IKsin(double f)276 inline double IKsin(double f)
277 {
278 return sin(f);
279 }
IKcos(float f)280 inline float IKcos(float f)
281 {
282 return cosf(f);
283 }
IKcos(double f)284 inline double IKcos(double f)
285 {
286 return cos(f);
287 }
IKtan(float f)288 inline float IKtan(float f)
289 {
290 return tanf(f);
291 }
IKtan(double f)292 inline double IKtan(double f)
293 {
294 return tan(f);
295 }
IKsqrt(float f)296 inline float IKsqrt(float f)
297 {
298 if (f <= 0.0f)
299 return 0.0f;
300 return sqrtf(f);
301 }
IKsqrt(double f)302 inline double IKsqrt(double f)
303 {
304 if (f <= 0.0)
305 return 0.0;
306 return sqrt(f);
307 }
IKatan2Simple(float fy,float fx)308 inline float IKatan2Simple(float fy, float fx)
309 {
310 return atan2f(fy, fx);
311 }
IKatan2(float fy,float fx)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 }
IKatan2Simple(double fy,double fx)326 inline double IKatan2Simple(double fy, double fx)
327 {
328 return atan2(fy, fx);
329 }
IKatan2(double fy,double fx)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>
IKatan2WithCheck(T fy,T fx,T epsilon)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
IKsign(float f)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
IKsign(double f)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>
IKPowWithIntegerCheck(T f,int n)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.
ComputeFk(const IkReal * j,IkReal * eetrans,IkReal * eerot)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
GetNumFreeParameters()561 IKFAST_API int GetNumFreeParameters()
562 {
563 return 1;
564 }
GetFreeParameters()565 IKFAST_API int* GetFreeParameters()
566 {
567 static int freeparams[] = {2};
568 return freeparams;
569 }
GetNumJoints()570 IKFAST_API int GetNumJoints()
571 {
572 return 7;
573 }
574
GetIkRealSize()575 IKFAST_API int GetIkRealSize()
576 {
577 return sizeof(IkReal);
578 }
579
GetIkType()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;
ComputeIk(const IkReal * eetrans,const IkReal * eerot,const IkReal * pfree,IkSolutionListBase<IkReal> & solutions)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 * ((
3297 (((-1.0)
3298 * (78.4313725490196)
3299 * sj4
3300 * (py
3301 * py
3302 * py)))
3303 + (((-78.4313725490196)
3304 * x641
3305 * x644))
3306 + (((-78.4313725490196) * x642 * x643)) + (((-1.0) * (78.4313725490196) * cj4 * (px * px * px))) + (((56.6666666666667) * x642))
3307 + (((56.6666666666667)
3308 * x641))))))
3309 - 1)
3310 <= IKFAST_SINCOS_THRESH)
3311 continue;
3312 j6array[0] = IKatan2(
3313 ((((1.17647058823529)
3314 * x642))
3315 + (((1.17647058823529)
3316 * x641))),
3317 ((x645.value)
3318 * (((((-1.0)
3319 * (78.4313725490196)
3320 * sj4
3321 * (py
3322 * py
3323 * py)))
3324 + (((-78.4313725490196)
3325 * x641
3326 * x644))
3327 + (((-78.4313725490196)
3328 * x642
3329 * x643))
3330 + (((-1.0)
3331 * (78.4313725490196)
3332 * cj4
3333 * (px
3334 * px
3335 * px)))
3336 + (((56.6666666666667)
3337 * x642))
3338 + (((56.6666666666667)
3339 * x641))))));
3340 sj6array
3341 [0]
3342 = IKsin(
3343 j6array
3344 [0]);
3345 cj6array
3346 [0]
3347 = IKcos(
3348 j6array
3349 [0]);
3350 if (j6array
3351 [0]
3352 > IKPI)
3353 {
3354 j6array
3355 [0]
3356 -= IK2PI;
3357 }
3358 else if (
3359 j6array
3360 [0]
3361 < -IKPI)
3362 {
3363 j6array
3364 [0]
3365 += IK2PI;
3366 }
3367 j6valid
3368 [0]
3369 = true;
3370 for (
3371 int ij6
3372 = 0;
3373 ij6
3374 < 1;
3375 ++ij6)
3376 {
3377 if (!j6valid
3378 [ij6])
3379 {
3380 continue;
3381 }
3382 _ij6[0]
3383 = ij6;
3384 _ij6[1]
3385 = -1;
3386 for (
3387 int iij6
3388 = ij6
3389 + 1;
3390 iij6
3391 < 1;
3392 ++iij6)
3393 {
3394 if (j6valid
3395 [iij6]
3396 && IKabs(
3397 cj6array
3398 [ij6]
3399 - cj6array
3400 [iij6])
3401 < IKFAST_SOLUTION_THRESH
3402 && IKabs(
3403 sj6array
3404 [ij6]
3405 - sj6array
3406 [iij6])
3407 < IKFAST_SOLUTION_THRESH)
3408 {
3409 j6valid
3410 [iij6]
3411 = false;
3412 _ij6[1]
3413 = iij6;
3414 break;
3415 }
3416 }
3417 j6 = j6array
3418 [ij6];
3419 cj6 = cj6array
3420 [ij6];
3421 sj6 = sj6array
3422 [ij6];
3423 {
3424 IkReal evalcond
3425 [5];
3426 IkReal
3427 x646
3428 = IKsin(
3429 j6);
3430 IkReal
3431 x647
3432 = (cj4
3433 * px);
3434 IkReal
3435 x648
3436 = (x646
3437 * x647);
3438 IkReal
3439 x649
3440 = (py
3441 * sj4);
3442 IkReal
3443 x650
3444 = (x646
3445 * x649);
3446 IkReal
3447 x651
3448 = ((1.0)
3449 * x647);
3450 IkReal
3451 x652
3452 = ((1.0)
3453 * x649);
3454 IkReal
3455 x653
3456 = IKcos(
3457 j6);
3458 IkReal
3459 x654
3460 = px
3461 * px;
3462 IkReal
3463 x655
3464 = ((3.92156862745098)
3465 * x646);
3466 IkReal
3467 x656
3468 = ((0.588235294117647)
3469 * x653);
3470 IkReal
3471 x657
3472 = py
3473 * py;
3474 IkReal
3475 x658
3476 = ((0.09)
3477 * x653);
3478 evalcond
3479 [0]
3480 = ((-0.85)
3481 + x650
3482 + x648);
3483 evalcond
3484 [1]
3485 = ((((-1.0)
3486 * x652))
3487 + (((-1.0)
3488 * x651))
3489 + (((0.85)
3490 * x646)));
3491 evalcond
3492 [2]
3493 = ((((-1.0)
3494 * x652
3495 * x653))
3496 + (((-1.0)
3497 * x651
3498 * x653)));
3499 evalcond
3500 [3]
3501 = (((x654
3502 * x655))
3503 + ((x655
3504 * x657))
3505 + (((-1.0)
3506 * x654
3507 * x656))
3508 + (((-0.425)
3509 * x653))
3510 + (((-1.0)
3511 * x656
3512 * x657))
3513 + (((-2.83333333333333)
3514 * x646)));
3515 evalcond
3516 [4]
3517 = ((-0.2125)
3518 + (((1.1)
3519 * x648))
3520 + (((1.1)
3521 * x650))
3522 + ((x649
3523 * x658))
3524 + (((-1.0)
3525 * x657))
3526 + ((x647
3527 * x658))
3528 + (((-1.0)
3529 * x654)));
3530 if (IKabs(
3531 evalcond
3532 [0])
3533 > IKFAST_EVALCOND_THRESH
3534 || IKabs(
3535 evalcond
3536 [1])
3537 > IKFAST_EVALCOND_THRESH
3538 || IKabs(
3539 evalcond
3540 [2])
3541 > IKFAST_EVALCOND_THRESH
3542 || IKabs(
3543 evalcond
3544 [3])
3545 > IKFAST_EVALCOND_THRESH
3546 || IKabs(
3547 evalcond
3548 [4])
3549 > IKFAST_EVALCOND_THRESH)
3550 {
3551 continue;
3552 }
3553 }
3554
3555 rotationfunction0(
3556 solutions);
3557 }
3558 }
3559 }
3560 }
3561 }
3562 else
3563 {
3564 {
3565 IkReal
3566 j6array
3567 [1],
3568 cj6array
3569 [1],
3570 sj6array
3571 [1];
3572 bool j6valid
3573 [1]
3574 = {false};
3575 _nj6 = 1;
3576 IkReal x659
3577 = (cj4
3578 * px);
3579 IkReal x660
3580 = (py
3581 * sj4);
3582 IkReal x661
3583 = px
3584 * px;
3585 IkReal x662
3586 = py
3587 * py;
3588 IkReal x663
3589 = ((1.29411764705882)
3590 * (cj4
3591 * cj4));
3592 CheckValue<
3593 IkReal>
3594 x664
3595 = IKPowWithIntegerCheck(
3596 ((((0.09)
3597 * x659))
3598 + (((0.09)
3599 * x660))),
3600 -1);
3601 if (!x664.valid)
3602 {
3603 continue;
3604 }
3605 if (IKabs((
3606 (((1.17647058823529)
3607 * x660))
3608 + (((1.17647058823529)
3609 * x659))))
3610 < IKFAST_ATAN2_MAGTHRESH
3611 && IKabs((
3612 (x664.value)
3613 * (((0.2125)
3614 + (((-2.58823529411765)
3615 * cj4
3616 * px
3617 * x660))
3618 + (((-0.294117647058824)
3619 * x662))
3620 + (((-1.0)
3621 * x661
3622 * x663))
3623 + ((x662
3624 * x663))
3625 + x661))))
3626 < IKFAST_ATAN2_MAGTHRESH
3627 && IKabs(
3628 IKsqr((
3629 (((1.17647058823529)
3630 * x660))
3631 + (((1.17647058823529)
3632 * x659))))
3633 + IKsqr((
3634 (x664.value)
3635 * (((0.2125)
3636 + (((-2.58823529411765)
3637 * cj4
3638 * px
3639 * x660))
3640 + (((-0.294117647058824)
3641 * x662))
3642 + (((-1.0)
3643 * x661
3644 * x663))
3645 + ((x662
3646 * x663))
3647 + x661))))
3648 - 1)
3649 <= IKFAST_SINCOS_THRESH)
3650 continue;
3651 j6array[0] = IKatan2(
3652 ((((1.17647058823529)
3653 * x660))
3654 + (((1.17647058823529)
3655 * x659))),
3656 ((x664.value)
3657 * (((0.2125)
3658 + (((-2.58823529411765)
3659 * cj4
3660 * px
3661 * x660))
3662 + (((-0.294117647058824)
3663 * x662))
3664 + (((-1.0)
3665 * x661
3666 * x663))
3667 + ((x662
3668 * x663))
3669 + x661))));
3670 sj6array[0] = IKsin(
3671 j6array
3672 [0]);
3673 cj6array[0] = IKcos(
3674 j6array
3675 [0]);
3676 if (j6array
3677 [0]
3678 > IKPI)
3679 {
3680 j6array[0]
3681 -= IK2PI;
3682 }
3683 else if (
3684 j6array
3685 [0]
3686 < -IKPI)
3687 {
3688 j6array[0]
3689 += IK2PI;
3690 }
3691 j6valid[0]
3692 = true;
3693 for (int ij6
3694 = 0;
3695 ij6
3696 < 1;
3697 ++ij6)
3698 {
3699 if (!j6valid
3700 [ij6])
3701 {
3702 continue;
3703 }
3704 _ij6[0]
3705 = ij6;
3706 _ij6[1]
3707 = -1;
3708 for (
3709 int iij6
3710 = ij6
3711 + 1;
3712 iij6
3713 < 1;
3714 ++iij6)
3715 {
3716 if (j6valid
3717 [iij6]
3718 && IKabs(
3719 cj6array
3720 [ij6]
3721 - cj6array
3722 [iij6])
3723 < IKFAST_SOLUTION_THRESH
3724 && IKabs(
3725 sj6array
3726 [ij6]
3727 - sj6array
3728 [iij6])
3729 < IKFAST_SOLUTION_THRESH)
3730 {
3731 j6valid
3732 [iij6]
3733 = false;
3734 _ij6[1]
3735 = iij6;
3736 break;
3737 }
3738 }
3739 j6 = j6array
3740 [ij6];
3741 cj6 = cj6array
3742 [ij6];
3743 sj6 = sj6array
3744 [ij6];
3745 {
3746 IkReal evalcond
3747 [5];
3748 IkReal
3749 x665
3750 = IKsin(
3751 j6);
3752 IkReal
3753 x666
3754 = (cj4
3755 * px);
3756 IkReal
3757 x667
3758 = (x665
3759 * x666);
3760 IkReal
3761 x668
3762 = (py
3763 * sj4);
3764 IkReal
3765 x669
3766 = (x665
3767 * x668);
3768 IkReal
3769 x670
3770 = ((1.0)
3771 * x666);
3772 IkReal
3773 x671
3774 = ((1.0)
3775 * x668);
3776 IkReal
3777 x672
3778 = IKcos(
3779 j6);
3780 IkReal
3781 x673
3782 = px
3783 * px;
3784 IkReal
3785 x674
3786 = ((3.92156862745098)
3787 * x665);
3788 IkReal
3789 x675
3790 = ((0.588235294117647)
3791 * x672);
3792 IkReal
3793 x676
3794 = py
3795 * py;
3796 IkReal
3797 x677
3798 = ((0.09)
3799 * x672);
3800 evalcond
3801 [0]
3802 = ((-0.85)
3803 + x667
3804 + x669);
3805 evalcond
3806 [1]
3807 = ((((0.85)
3808 * x665))
3809 + (((-1.0)
3810 * x670))
3811 + (((-1.0)
3812 * x671)));
3813 evalcond
3814 [2]
3815 = ((((-1.0)
3816 * x671
3817 * x672))
3818 + (((-1.0)
3819 * x670
3820 * x672)));
3821 evalcond
3822 [3]
3823 = ((((-2.83333333333333)
3824 * x665))
3825 + (((-0.425)
3826 * x672))
3827 + ((x673
3828 * x674))
3829 + (((-1.0)
3830 * x675
3831 * x676))
3832 + ((x674
3833 * x676))
3834 + (((-1.0)
3835 * x673
3836 * x675)));
3837 evalcond
3838 [4]
3839 = ((-0.2125)
3840 + ((x668
3841 * x677))
3842 + (((-1.0)
3843 * x676))
3844 + ((x666
3845 * x677))
3846 + (((1.1)
3847 * x667))
3848 + (((-1.0)
3849 * x673))
3850 + (((1.1)
3851 * x669)));
3852 if (IKabs(
3853 evalcond
3854 [0])
3855 > IKFAST_EVALCOND_THRESH
3856 || IKabs(
3857 evalcond
3858 [1])
3859 > IKFAST_EVALCOND_THRESH
3860 || IKabs(
3861 evalcond
3862 [2])
3863 > IKFAST_EVALCOND_THRESH
3864 || IKabs(
3865 evalcond
3866 [3])
3867 > IKFAST_EVALCOND_THRESH
3868 || IKabs(
3869 evalcond
3870 [4])
3871 > IKFAST_EVALCOND_THRESH)
3872 {
3873 continue;
3874 }
3875 }
3876
3877 rotationfunction0(
3878 solutions);
3879 }
3880 }
3881 }
3882 }
3883 }
3884 } while (0);
3885 if (bgotonextstatement)
3886 {
3887 bool
3888 bgotonextstatement
3889 = true;
3890 do
3891 {
3892 if (1)
3893 {
3894 bgotonextstatement
3895 = false;
3896 continue; // branch
3897 // miss
3898 // [j6]
3899 }
3900 } while (0);
3901 if (bgotonextstatement)
3902 {
3903 }
3904 }
3905 }
3906 }
3907 else
3908 {
3909 {
3910 IkReal j6array[1],
3911 cj6array[1],
3912 sj6array[1];
3913 bool j6valid[1]
3914 = {false};
3915 _nj6 = 1;
3916 IkReal x678
3917 = (cj4 * px);
3918 IkReal x679
3919 = (py * sj4);
3920 IkReal x680
3921 = ((0.108264705882353)
3922 * cj9);
3923 IkReal x681
3924 = ((0.588235294117647)
3925 * pp);
3926 IkReal x682
3927 = (cj9 * pp);
3928 IkReal x683
3929 = (cj9 * sj9);
3930 IkReal x684
3931 = (pp * sj9);
3932 IkReal x685
3933 = cj9 * cj9;
3934 IkReal x686
3935 = ((1.0) * pz);
3936 CheckValue<IkReal> x687 = IKPowWithIntegerCheck(
3937 IKsign((
3938 (((-1.0)
3939 * x678
3940 * x681))
3941 + (((-1.0)
3942 * x679
3943 * x680))
3944 + (((1.51009803921569)
3945 * pz))
3946 + (((1.32323529411765)
3947 * cj9
3948 * pz))
3949 + (((-1.0)
3950 * (3.92156862745098)
3951 * pp
3952 * pz))
3953 + (((-1.0)
3954 * x678
3955 * x680))
3956 + (((-0.316735294117647)
3957 * x679))
3958 + (((-0.316735294117647)
3959 * x678))
3960 + (((-1.0)
3961 * x679
3962 * x681)))),
3963 -1);
3964 if (!x687.valid)
3965 {
3966 continue;
3967 }
3968 CheckValue<IkReal> x688 = IKatan2WithCheck(
3969 IkReal((
3970 (-0.174204411764706)
3971 + (((-0.0264705882352941)
3972 * x684))
3973 + (((-0.00487191176470588)
3974 * x683))
3975 + (pz * pz)
3976 + (((-0.0324794117647059)
3977 * x685))
3978 + (((-1.0)
3979 * (0.154566176470588)
3980 * cj9))
3981 + (((-0.176470588235294)
3982 * x682))
3983 + (((-1.0)
3984 * (0.323529411764706)
3985 * pp))
3986 + (((-1.0)
3987 * (0.0142530882352941)
3988 * sj9)))),
3989 ((0.830553921568627)
3990 + (((-1.17647058823529)
3991 * x682))
3992 + (((0.396970588235294)
3993 * x685))
3994 + (((1.18080882352941)
3995 * cj9))
3996 + (((0.0595455882352941)
3997 * x683))
3998 + (((-1.0)
3999 * (2.15686274509804)
4000 * pp))
4001 + (((-1.0) * x678
4002 * x686))
4003 + (((-1.0) * x679
4004 * x686))
4005 + (((-0.176470588235294)
4006 * x684))
4007 + (((0.0679544117647059)
4008 * sj9))),
4009 IKFAST_ATAN2_MAGTHRESH);
4010 if (!x688.valid)
4011 {
4012 continue;
4013 }
4014 j6array[0]
4015 = ((-1.5707963267949)
4016 + (((1.5707963267949)
4017 * (x687.value)))
4018 + (x688.value));
4019 sj6array[0] = IKsin(
4020 j6array[0]);
4021 cj6array[0] = IKcos(
4022 j6array[0]);
4023 if (j6array[0] > IKPI)
4024 {
4025 j6array[0] -= IK2PI;
4026 }
4027 else if (
4028 j6array[0]
4029 < -IKPI)
4030 {
4031 j6array[0] += IK2PI;
4032 }
4033 j6valid[0] = true;
4034 for (int ij6 = 0;
4035 ij6 < 1;
4036 ++ij6)
4037 {
4038 if (!j6valid[ij6])
4039 {
4040 continue;
4041 }
4042 _ij6[0] = ij6;
4043 _ij6[1] = -1;
4044 for (int iij6
4045 = ij6 + 1;
4046 iij6 < 1;
4047 ++iij6)
4048 {
4049 if (j6valid[iij6]
4050 && IKabs(
4051 cj6array
4052 [ij6]
4053 - cj6array
4054 [iij6])
4055 < IKFAST_SOLUTION_THRESH
4056 && IKabs(
4057 sj6array
4058 [ij6]
4059 - sj6array
4060 [iij6])
4061 < IKFAST_SOLUTION_THRESH)
4062 {
4063 j6valid[iij6]
4064 = false;
4065 _ij6[1] = iij6;
4066 break;
4067 }
4068 }
4069 j6 = j6array[ij6];
4070 cj6 = cj6array[ij6];
4071 sj6 = sj6array[ij6];
4072 {
4073 IkReal
4074 evalcond[5];
4075 IkReal x689
4076 = ((0.3)
4077 * cj9);
4078 IkReal x690
4079 = ((0.045)
4080 * sj9);
4081 IkReal x691
4082 = IKcos(j6);
4083 IkReal x692
4084 = (pz * x691);
4085 IkReal x693
4086 = IKsin(j6);
4087 IkReal x694
4088 = (cj4 * px);
4089 IkReal x695
4090 = (x693
4091 * x694);
4092 IkReal x696
4093 = (py * sj4);
4094 IkReal x697
4095 = (x693
4096 * x696);
4097 IkReal x698
4098 = ((0.045)
4099 * cj9);
4100 IkReal x699
4101 = ((0.3)
4102 * sj9);
4103 IkReal x700
4104 = (pz * x693);
4105 IkReal x701
4106 = ((1.0)
4107 * x694);
4108 IkReal x702
4109 = ((1.0)
4110 * x696);
4111 IkReal x703
4112 = ((0.09)
4113 * x691);
4114 evalcond[0]
4115 = ((-0.55)
4116 + (((-1.0)
4117 * x689))
4118 + (((-1.0)
4119 * x690))
4120 + x692
4121 + x695
4122 + x697);
4123 evalcond[1]
4124 = ((0.045)
4125 + x700
4126 + (((-1.0)
4127 * x691
4128 * x701))
4129 + (((-1.0)
4130 * x698))
4131 + x699
4132 + (((-1.0)
4133 * x691
4134 * x702)));
4135 evalcond[2]
4136 = ((((-1.51009803921569)
4137 * x693))
4138 + (((-0.588235294117647)
4139 * pp
4140 * x691))
4141 + (((3.92156862745098)
4142 * pp
4143 * x693))
4144 + pz
4145 + (((-1.32323529411765)
4146 * cj9
4147 * x693))
4148 + (((-0.316735294117647)
4149 * x691))
4150 + (((-0.108264705882353)
4151 * cj9
4152 * x691)));
4153 evalcond[3]
4154 = ((((-1.0)
4155 * x691
4156 * x698))
4157 + (((-1.0)
4158 * x702))
4159 + ((x690
4160 * x693))
4161 + ((x691
4162 * x699))
4163 + (((0.55)
4164 * x693))
4165 + (((0.045)
4166 * x691))
4167 + ((x689
4168 * x693))
4169 + (((-1.0)
4170 * x701)));
4171 evalcond[4]
4172 = ((-0.2125)
4173 + ((x696
4174 * x703))
4175 + (((1.1)
4176 * x695))
4177 + (((-0.09)
4178 * x700))
4179 + (((1.1)
4180 * x692))
4181 + (((1.1)
4182 * x697))
4183 + ((x694
4184 * x703))
4185 + (((-1.0)
4186 * (1.0)
4187 * pp)));
4188 if (IKabs(evalcond
4189 [0])
4190 > IKFAST_EVALCOND_THRESH
4191 || IKabs(
4192 evalcond
4193 [1])
4194 > IKFAST_EVALCOND_THRESH
4195 || IKabs(
4196 evalcond
4197 [2])
4198 > IKFAST_EVALCOND_THRESH
4199 || IKabs(
4200 evalcond
4201 [3])
4202 > IKFAST_EVALCOND_THRESH
4203 || IKabs(
4204 evalcond
4205 [4])
4206 > IKFAST_EVALCOND_THRESH)
4207 {
4208 continue;
4209 }
4210 }
4211
4212 rotationfunction0(
4213 solutions);
4214 }
4215 }
4216 }
4217 }
4218 }
4219 else
4220 {
4221 {
4222 IkReal j6array[1],
4223 cj6array[1],
4224 sj6array[1];
4225 bool j6valid[1] = {false};
4226 _nj6 = 1;
4227 IkReal x704
4228 = ((0.045) * cj4
4229 * px);
4230 IkReal x705
4231 = ((0.045) * py
4232 * sj4);
4233 IkReal x706
4234 = ((0.3) * sj9);
4235 IkReal x707 = (cj4 * px);
4236 IkReal x708 = (py * sj4);
4237 IkReal x709 = (cj9 * sj9);
4238 IkReal x710 = cj9 * cj9;
4239 IkReal x711
4240 = ((1.0) * pz);
4241 IkReal x712 = py * py;
4242 IkReal x713 = cj4 * cj4;
4243 CheckValue<IkReal> x714
4244 = IKatan2WithCheck(
4245 IkReal((
4246 (0.03825)
4247 + (((0.087975)
4248 * x709))
4249 + (((-0.027)
4250 * x710))
4251 + (((-1.0)
4252 * x707
4253 * x711))
4254 + (((0.167025)
4255 * sj9))
4256 + (((-1.0)
4257 * (0.01125)
4258 * cj9))
4259 + (((-1.0)
4260 * x708
4261 * x711)))),
4262 ((-0.304525)
4263 + (((-1.0)
4264 * (0.0495)
4265 * sj9))
4266 + (((-0.087975)
4267 * x710))
4268 + (((2.0) * cj4
4269 * px * x708))
4270 + x712
4271 + (((-1.0) * x712
4272 * x713))
4273 + (((-1.0)
4274 * (0.33)
4275 * cj9))
4276 + ((x713
4277 * (px * px)))
4278 + (((-0.027)
4279 * x709))),
4280 IKFAST_ATAN2_MAGTHRESH);
4281 if (!x714.valid)
4282 {
4283 continue;
4284 }
4285 CheckValue<IkReal> x715
4286 = IKPowWithIntegerCheck(
4287 IKsign((
4288 (((-1.0) * cj9
4289 * x704))
4290 + (((-1.0)
4291 * (0.55)
4292 * pz))
4293 + ((x706
4294 * x708))
4295 + ((x706
4296 * x707))
4297 + x705 + x704
4298 + (((-1.0)
4299 * cj9
4300 * x705))
4301 + (((-1.0)
4302 * (0.045)
4303 * pz
4304 * sj9))
4305 + (((-1.0)
4306 * (0.3)
4307 * cj9
4308 * pz)))),
4309 -1);
4310 if (!x715.valid)
4311 {
4312 continue;
4313 }
4314 j6array[0]
4315 = ((-1.5707963267949)
4316 + (x714.value)
4317 + (((1.5707963267949)
4318 * (x715.value))));
4319 sj6array[0]
4320 = IKsin(j6array[0]);
4321 cj6array[0]
4322 = IKcos(j6array[0]);
4323 if (j6array[0] > IKPI)
4324 {
4325 j6array[0] -= IK2PI;
4326 }
4327 else if (
4328 j6array[0] < -IKPI)
4329 {
4330 j6array[0] += IK2PI;
4331 }
4332 j6valid[0] = true;
4333 for (int ij6 = 0; ij6 < 1;
4334 ++ij6)
4335 {
4336 if (!j6valid[ij6])
4337 {
4338 continue;
4339 }
4340 _ij6[0] = ij6;
4341 _ij6[1] = -1;
4342 for (int iij6 = ij6 + 1;
4343 iij6 < 1;
4344 ++iij6)
4345 {
4346 if (j6valid[iij6]
4347 && IKabs(
4348 cj6array
4349 [ij6]
4350 - cj6array
4351 [iij6])
4352 < IKFAST_SOLUTION_THRESH
4353 && IKabs(
4354 sj6array
4355 [ij6]
4356 - sj6array
4357 [iij6])
4358 < IKFAST_SOLUTION_THRESH)
4359 {
4360 j6valid[iij6]
4361 = false;
4362 _ij6[1] = iij6;
4363 break;
4364 }
4365 }
4366 j6 = j6array[ij6];
4367 cj6 = cj6array[ij6];
4368 sj6 = sj6array[ij6];
4369 {
4370 IkReal evalcond[5];
4371 IkReal x716
4372 = ((0.3) * cj9);
4373 IkReal x717
4374 = ((0.045) * sj9);
4375 IkReal x718
4376 = IKcos(j6);
4377 IkReal x719
4378 = (pz * x718);
4379 IkReal x720
4380 = IKsin(j6);
4381 IkReal x721
4382 = (cj4 * px);
4383 IkReal x722
4384 = (x720 * x721);
4385 IkReal x723
4386 = (py * sj4);
4387 IkReal x724
4388 = (x720 * x723);
4389 IkReal x725
4390 = ((0.045) * cj9);
4391 IkReal x726
4392 = ((0.3) * sj9);
4393 IkReal x727
4394 = (pz * x720);
4395 IkReal x728
4396 = ((1.0) * x721);
4397 IkReal x729
4398 = ((1.0) * x723);
4399 IkReal x730
4400 = ((0.09) * x718);
4401 evalcond[0]
4402 = ((-0.55) + x719
4403 + (((-1.0)
4404 * x716))
4405 + (((-1.0)
4406 * x717))
4407 + x724 + x722);
4408 evalcond[1]
4409 = ((0.045)
4410 + (((-1.0)
4411 * x718
4412 * x729))
4413 + (((-1.0)
4414 * x725))
4415 + (((-1.0)
4416 * x718
4417 * x728))
4418 + x727 + x726);
4419 evalcond[2]
4420 = ((((3.92156862745098)
4421 * pp * x720))
4422 + (((-1.32323529411765)
4423 * cj9
4424 * x720))
4425 + (((-0.108264705882353)
4426 * cj9
4427 * x718))
4428 + (((-1.51009803921569)
4429 * x720))
4430 + pz
4431 + (((-0.588235294117647)
4432 * pp
4433 * x718))
4434 + (((-0.316735294117647)
4435 * x718)));
4436 evalcond[3]
4437 = (((x717 * x720))
4438 + (((-1.0)
4439 * x718
4440 * x725))
4441 + (((0.045)
4442 * x718))
4443 + ((x716
4444 * x720))
4445 + (((-1.0)
4446 * x728))
4447 + (((0.55)
4448 * x720))
4449 + (((-1.0)
4450 * x729))
4451 + ((x718
4452 * x726)));
4453 evalcond[4]
4454 = ((-0.2125)
4455 + (((1.1)
4456 * x722))
4457 + (((-0.09)
4458 * x727))
4459 + (((1.1)
4460 * x719))
4461 + ((x723
4462 * x730))
4463 + (((1.1)
4464 * x724))
4465 + (((-1.0)
4466 * (1.0)
4467 * pp))
4468 + ((x721
4469 * x730)));
4470 if (IKabs(evalcond[0])
4471 > IKFAST_EVALCOND_THRESH
4472 || IKabs(evalcond
4473 [1])
4474 > IKFAST_EVALCOND_THRESH
4475 || IKabs(evalcond
4476 [2])
4477 > IKFAST_EVALCOND_THRESH
4478 || IKabs(evalcond
4479 [3])
4480 > IKFAST_EVALCOND_THRESH
4481 || IKabs(evalcond
4482 [4])
4483 > IKFAST_EVALCOND_THRESH)
4484 {
4485 continue;
4486 }
4487 }
4488
4489 rotationfunction0(
4490 solutions);
4491 }
4492 }
4493 }
4494 }
4495 }
4496 else
4497 {
4498 {
4499 IkReal j6array[1],
4500 cj6array[1], sj6array[1];
4501 bool j6valid[1] = {false};
4502 _nj6 = 1;
4503 IkReal x731 = (cj4 * px);
4504 IkReal x732 = (py * sj4);
4505 IkReal x733
4506 = ((1.32323529411765)
4507 * cj9);
4508 IkReal x734
4509 = ((3.92156862745098)
4510 * pp);
4511 IkReal x735
4512 = ((0.0264705882352941)
4513 * pp);
4514 IkReal x736 = (cj9 * sj9);
4515 IkReal x737
4516 = ((0.176470588235294)
4517 * pp);
4518 IkReal x738 = cj9 * cj9;
4519 CheckValue<IkReal> x739 = IKatan2WithCheck(
4520 IkReal((
4521 (-0.0142530882352941)
4522 + (((-0.0324794117647059)
4523 * x736))
4524 + (((0.00938117647058823)
4525 * cj9))
4526 + ((pz * x732))
4527 + ((pz * x731))
4528 + (((-1.0) * x735))
4529 + (((0.00487191176470588)
4530 * x738))
4531 + (((-1.0)
4532 * (0.0950205882352941)
4533 * sj9))
4534 + ((cj9 * x735))
4535 + (((-1.0) * sj9
4536 * x737)))),
4537 ((0.0679544117647059)
4538 + (((-1.0) * x737))
4539 + ((cj9 * x737))
4540 + (pz * pz)
4541 + (((-1.0)
4542 * (0.00840882352941177)
4543 * cj9))
4544 + (((-0.0595455882352941)
4545 * x738))
4546 + (((0.453029411764706)
4547 * sj9))
4548 + (((0.396970588235294)
4549 * x736))
4550 + (((-1.0)
4551 * (1.17647058823529)
4552 * pp * sj9))),
4553 IKFAST_ATAN2_MAGTHRESH);
4554 if (!x739.valid)
4555 {
4556 continue;
4557 }
4558 CheckValue<IkReal> x740 = IKPowWithIntegerCheck(
4559 IKsign((
4560 (((0.316735294117647)
4561 * pz))
4562 + (((0.108264705882353)
4563 * cj9 * pz))
4564 + (((0.588235294117647)
4565 * pp * pz))
4566 + ((x732 * x733))
4567 + (((-1.0) * x732
4568 * x734))
4569 + (((1.51009803921569)
4570 * x731))
4571 + (((1.51009803921569)
4572 * x732))
4573 + ((x731 * x733))
4574 + (((-1.0) * x731
4575 * x734)))),
4576 -1);
4577 if (!x740.valid)
4578 {
4579 continue;
4580 }
4581 j6array[0]
4582 = ((-1.5707963267949)
4583 + (x739.value)
4584 + (((1.5707963267949)
4585 * (x740.value))));
4586 sj6array[0]
4587 = IKsin(j6array[0]);
4588 cj6array[0]
4589 = IKcos(j6array[0]);
4590 if (j6array[0] > IKPI)
4591 {
4592 j6array[0] -= IK2PI;
4593 }
4594 else if (j6array[0] < -IKPI)
4595 {
4596 j6array[0] += IK2PI;
4597 }
4598 j6valid[0] = true;
4599 for (int ij6 = 0; ij6 < 1;
4600 ++ij6)
4601 {
4602 if (!j6valid[ij6])
4603 {
4604 continue;
4605 }
4606 _ij6[0] = ij6;
4607 _ij6[1] = -1;
4608 for (int iij6 = ij6 + 1;
4609 iij6 < 1;
4610 ++iij6)
4611 {
4612 if (j6valid[iij6]
4613 && IKabs(
4614 cj6array[ij6]
4615 - cj6array
4616 [iij6])
4617 < IKFAST_SOLUTION_THRESH
4618 && IKabs(
4619 sj6array[ij6]
4620 - sj6array
4621 [iij6])
4622 < IKFAST_SOLUTION_THRESH)
4623 {
4624 j6valid[iij6] = false;
4625 _ij6[1] = iij6;
4626 break;
4627 }
4628 }
4629 j6 = j6array[ij6];
4630 cj6 = cj6array[ij6];
4631 sj6 = sj6array[ij6];
4632 {
4633 IkReal evalcond[5];
4634 IkReal x741
4635 = ((0.3) * cj9);
4636 IkReal x742
4637 = ((0.045) * sj9);
4638 IkReal x743 = IKcos(j6);
4639 IkReal x744 = (pz * x743);
4640 IkReal x745 = IKsin(j6);
4641 IkReal x746 = (cj4 * px);
4642 IkReal x747
4643 = (x745 * x746);
4644 IkReal x748 = (py * sj4);
4645 IkReal x749
4646 = (x745 * x748);
4647 IkReal x750
4648 = ((0.045) * cj9);
4649 IkReal x751
4650 = ((0.3) * sj9);
4651 IkReal x752 = (pz * x745);
4652 IkReal x753
4653 = ((1.0) * x746);
4654 IkReal x754
4655 = ((1.0) * x748);
4656 IkReal x755
4657 = ((0.09) * x743);
4658 evalcond[0]
4659 = ((-0.55)
4660 + (((-1.0) * x741))
4661 + x749 + x744
4662 + x747
4663 + (((-1.0)
4664 * x742)));
4665 evalcond[1]
4666 = ((0.045)
4667 + (((-1.0) * x743
4668 * x753))
4669 + (((-1.0) * x743
4670 * x754))
4671 + x752 + x751
4672 + (((-1.0)
4673 * x750)));
4674 evalcond[2]
4675 = ((((-0.316735294117647)
4676 * x743))
4677 + (((-1.32323529411765)
4678 * cj9 * x745))
4679 + pz
4680 + (((-0.108264705882353)
4681 * cj9 * x743))
4682 + (((-1.51009803921569)
4683 * x745))
4684 + (((-0.588235294117647)
4685 * pp * x743))
4686 + (((3.92156862745098)
4687 * pp * x745)));
4688 evalcond[3]
4689 = ((((-1.0) * x753))
4690 + (((0.55) * x745))
4691 + (((0.045)
4692 * x743))
4693 + ((x741 * x745))
4694 + ((x743 * x751))
4695 + (((-1.0) * x743
4696 * x750))
4697 + (((-1.0) * x754))
4698 + ((x742 * x745)));
4699 evalcond[4]
4700 = ((-0.2125)
4701 + (((1.1) * x749))
4702 + (((-0.09)
4703 * x752))
4704 + ((x748 * x755))
4705 + (((1.1) * x744))
4706 + (((-1.0) * (1.0)
4707 * pp))
4708 + ((x746 * x755))
4709 + (((1.1)
4710 * x747)));
4711 if (IKabs(evalcond[0])
4712 > IKFAST_EVALCOND_THRESH
4713 || IKabs(evalcond[1])
4714 > IKFAST_EVALCOND_THRESH
4715 || IKabs(evalcond[2])
4716 > IKFAST_EVALCOND_THRESH
4717 || IKabs(evalcond[3])
4718 > IKFAST_EVALCOND_THRESH
4719 || IKabs(evalcond[4])
4720 > IKFAST_EVALCOND_THRESH)
4721 {
4722 continue;
4723 }
4724 }
4725
4726 rotationfunction0(
4727 solutions);
4728 }
4729 }
4730 }
4731 }
4732 }
4733 } while (0);
4734 if (bgotonextstatement)
4735 {
4736 bool bgotonextstatement = true;
4737 do
4738 {
4739 IkReal x756 = (px * sj4);
4740 IkReal x757 = (cj4 * py);
4741 evalcond[0]
4742 = ((-3.14159265358979)
4743 + (IKfmod(
4744 ((3.14159265358979)
4745 + (IKabs((
4746 (-3.14159265358979)
4747 + j8)))),
4748 6.28318530717959)));
4749 evalcond[1]
4750 = ((0.39655)
4751 + (((0.0765) * sj9))
4752 + (((0.32595) * cj9))
4753 + (((-1.0) * (1.0) * pp)));
4754 evalcond[2]
4755 = ((((-1.0) * x757)) + x756);
4756 evalcond[3]
4757 = ((((-1.0) * x756)) + x757);
4758 if (IKabs(evalcond[0])
4759 < 0.0000010000000000
4760 && IKabs(evalcond[1])
4761 < 0.0000010000000000
4762 && IKabs(evalcond[2])
4763 < 0.0000010000000000
4764 && IKabs(evalcond[3])
4765 < 0.0000010000000000)
4766 {
4767 bgotonextstatement = false;
4768 {
4769 IkReal j6eval[2];
4770 sj8 = 0;
4771 cj8 = -1.0;
4772 j8 = 3.14159265358979;
4773 IkReal x758 = py * py;
4774 IkReal x759 = cj4 * cj4;
4775 IkReal x760
4776 = ((pz * pz)
4777 + ((x759 * (px * px)))
4778 + (((2.0) * cj4 * px * py
4779 * sj4))
4780 + x758
4781 + (((-1.0) * x758
4782 * x759)));
4783 j6eval[0] = x760;
4784 j6eval[1] = IKsign(x760);
4785 if (IKabs(j6eval[0])
4786 < 0.0000010000000000
4787 || IKabs(j6eval[1])
4788 < 0.0000010000000000)
4789 {
4790 {
4791 IkReal j6eval[2];
4792 sj8 = 0;
4793 cj8 = -1.0;
4794 j8 = 3.14159265358979;
4795 IkReal x761 = (cj4 * px);
4796 IkReal x762 = (cj9 * pz);
4797 IkReal x763 = (py * sj4);
4798 IkReal x764 = (pz * sj9);
4799 IkReal x765
4800 = (cj4 * px * sj9);
4801 IkReal x766
4802 = (py * sj4 * sj9);
4803 IkReal x767
4804 = ((0.045) * x761);
4805 IkReal x768
4806 = ((0.045) * x763);
4807 j6eval[0]
4808 = (((cj9 * x761))
4809 + (((-1.0)
4810 * (12.2222222222222)
4811 * pz))
4812 + (((-1.0) * x764))
4813 + (((-6.66666666666667)
4814 * x765))
4815 + (((-1.0) * x763))
4816 + (((-6.66666666666667)
4817 * x766))
4818 + ((cj9 * x763))
4819 + (((-6.66666666666667)
4820 * x762))
4821 + (((-1.0) * x761)));
4822 j6eval[1] = IKsign(
4823 ((((-0.3) * x766))
4824 + (((-1.0) * x767))
4825 + (((-1.0) * (0.55)
4826 * pz))
4827 + ((cj9 * x768))
4828 + (((-0.3) * x765))
4829 + (((-1.0) * x768))
4830 + (((-0.3) * x762))
4831 + (((-0.045) * x764))
4832 + ((cj9 * x767))));
4833 if (IKabs(j6eval[0])
4834 < 0.0000010000000000
4835 || IKabs(j6eval[1])
4836 < 0.0000010000000000)
4837 {
4838 {
4839 IkReal j6eval[2];
4840 sj8 = 0;
4841 cj8 = -1.0;
4842 j8 = 3.14159265358979;
4843 IkReal x769
4844 = (cj4 * px);
4845 IkReal x770
4846 = (cj9 * pz);
4847 IkReal x771 = (pp * pz);
4848 IkReal x772
4849 = (py * sj4);
4850 IkReal x773
4851 = (cj4 * cj9 * px);
4852 IkReal x774
4853 = (cj4 * pp * px);
4854 IkReal x775
4855 = (cj9 * py * sj4);
4856 IkReal x776
4857 = (pp * py * sj4);
4858 j6eval[0]
4859 = ((((-1.0) * x775))
4860 + (((-1.0)
4861 * (13.9482024812098)
4862 * pz))
4863 + (((-2.92556370551481)
4864 * x772))
4865 + (((-12.2222222222222)
4866 * x770))
4867 + (((-5.4333061668025)
4868 * x776))
4869 + (((-1.0)
4870 * x773))
4871 + (((-5.4333061668025)
4872 * x774))
4873 + (((-2.92556370551481)
4874 * x769))
4875 + (((36.2220411120167)
4876 * x771)));
4877 j6eval[1] = IKsign((
4878 (((-0.588235294117647)
4879 * x776))
4880 + (((-0.108264705882353)
4881 * x773))
4882 + (((3.92156862745098)
4883 * x771))
4884 + (((-0.316735294117647)
4885 * x772))
4886 + (((-1.0)
4887 * (1.51009803921569)
4888 * pz))
4889 + (((-0.316735294117647)
4890 * x769))
4891 + (((-1.32323529411765)
4892 * x770))
4893 + (((-0.588235294117647)
4894 * x774))
4895 + (((-0.108264705882353)
4896 * x775))));
4897 if (IKabs(j6eval[0])
4898 < 0.0000010000000000
4899 || IKabs(j6eval[1])
4900 < 0.0000010000000000)
4901 {
4902 {
4903 IkReal evalcond[1];
4904 bool
4905 bgotonextstatement
4906 = true;
4907 do
4908 {
4909 evalcond[0]
4910 = ((IKabs((
4911 (-3.14159265358979)
4912 + (IKfmod(
4913 ((3.14159265358979)
4914 + j9),
4915 6.28318530717959)))))
4916 + (IKabs(
4917 pz)));
4918 if (IKabs(evalcond
4919 [0])
4920 < 0.0000010000000000)
4921 {
4922 bgotonextstatement
4923 = false;
4924 {
4925 IkReal
4926 j6eval[1];
4927 IkReal x777
4928 = ((1.0)
4929 * py);
4930 sj8 = 0;
4931 cj8 = -1.0;
4932 j8 = 3.14159265358979;
4933 pz = 0;
4934 j9 = 0;
4935 sj9 = 0;
4936 cj9 = 1.0;
4937 pp
4938 = ((py
4939 * py)
4940 + (px
4941 * px));
4942 npx
4943 = (((py
4944 * r10))
4945 + ((px
4946 * r00)));
4947 npy
4948 = (((py
4949 * r11))
4950 + ((px
4951 * r01)));
4952 npz
4953 = (((px
4954 * r02))
4955 + ((py
4956 * r12)));
4957 rxp0_0
4958 = ((-1.0)
4959 * r20
4960 * x777);
4961 rxp0_1
4962 = (px
4963 * r20);
4964 rxp1_0
4965 = ((-1.0)
4966 * r21
4967 * x777);
4968 rxp1_1
4969 = (px
4970 * r21);
4971 rxp2_0
4972 = ((-1.0)
4973 * r22
4974 * x777);
4975 rxp2_1
4976 = (px
4977 * r22);
4978 j6eval[0]
4979 = ((((-1.0)
4980 * (1.0)
4981 * cj4
4982 * px))
4983 + (((-1.0)
4984 * (1.0)
4985 * py
4986 * sj4)));
4987 if (IKabs(
4988 j6eval
4989 [0])
4990 < 0.0000010000000000)
4991 {
4992 {
4993 IkReal j6eval
4994 [1];
4995 IkReal
4996 x778
4997 = ((1.0)
4998 * py);
4999 sj8 = 0;
5000 cj8 = -1.0;
5001 j8 = 3.14159265358979;
5002 pz = 0;
5003 j9 = 0;
5004 sj9 = 0;
5005 cj9 = 1.0;
5006 pp
5007 = ((py
5008 * py)
5009 + (px
5010 * px));
5011 npx
5012 = (((py
5013 * r10))
5014 + ((px
5015 * r00)));
5016 npy
5017 = (((py
5018 * r11))
5019 + ((px
5020 * r01)));
5021 npz
5022 = (((px
5023 * r02))
5024 + ((py
5025 * r12)));
5026 rxp0_0
5027 = ((-1.0)
5028 * r20
5029 * x778);
5030 rxp0_1
5031 = (px
5032 * r20);
5033 rxp1_0
5034 = ((-1.0)
5035 * r21
5036 * x778);
5037 rxp1_1
5038 = (px
5039 * r21);
5040 rxp2_0
5041 = ((-1.0)
5042 * r22
5043 * x778);
5044 rxp2_1
5045 = (px
5046 * r22);
5047 j6eval[0]
5048 = ((-1.0)
5049 + (((-1.0)
5050 * (1.3840830449827)
5051 * (px
5052 * px)))
5053 + (((-1.0)
5054 * (1.3840830449827)
5055 * (py
5056 * py))));
5057 if (IKabs(
5058 j6eval
5059 [0])
5060 < 0.0000010000000000)
5061 {
5062 {
5063 IkReal j6eval
5064 [2];
5065 IkReal
5066 x779
5067 = ((1.0)
5068 * py);
5069 sj8 = 0;
5070 cj8 = -1.0;
5071 j8 = 3.14159265358979;
5072 pz = 0;
5073 j9 = 0;
5074 sj9 = 0;
5075 cj9 = 1.0;
5076 pp
5077 = ((py
5078 * py)
5079 + (px
5080 * px));
5081 npx
5082 = (((py
5083 * r10))
5084 + ((px
5085 * r00)));
5086 npy
5087 = (((py
5088 * r11))
5089 + ((px
5090 * r01)));
5091 npz
5092 = (((px
5093 * r02))
5094 + ((py
5095 * r12)));
5096 rxp0_0
5097 = ((-1.0)
5098 * r20
5099 * x779);
5100 rxp0_1
5101 = (px
5102 * r20);
5103 rxp1_0
5104 = ((-1.0)
5105 * r21
5106 * x779);
5107 rxp1_1
5108 = (px
5109 * r21);
5110 rxp2_0
5111 = ((-1.0)
5112 * r22
5113 * x779);
5114 rxp2_1
5115 = (px
5116 * r22);
5117 IkReal
5118 x780
5119 = (cj4
5120 * px);
5121 IkReal
5122 x781
5123 = (py
5124 * sj4);
5125 j6eval
5126 [0]
5127 = (x780
5128 + x781);
5129 j6eval
5130 [1]
5131 = ((((-1.0)
5132 * (1.3840830449827)
5133 * cj4
5134 * (px
5135 * px
5136 * px)))
5137 + (((-1.0)
5138 * x780))
5139 + (((-1.3840830449827)
5140 * x780
5141 * (py
5142 * py)))
5143 + (((-1.0)
5144 * x781))
5145 + (((-1.0)
5146 * (1.3840830449827)
5147 * sj4
5148 * (py
5149 * py
5150 * py)))
5151 + (((-1.3840830449827)
5152 * x781
5153 * (px
5154 * px))));
5155 if (IKabs(
5156 j6eval
5157 [0])
5158 < 0.0000010000000000
5159 || IKabs(
5160 j6eval
5161 [1])
5162 < 0.0000010000000000)
5163 {
5164 {
5165 IkReal evalcond
5166 [4];
5167 bool
5168 bgotonextstatement
5169 = true;
5170 do
5171 {
5172 evalcond
5173 [0]
5174 = ((IKabs(
5175 py))
5176 + (IKabs(
5177 px)));
5178 evalcond
5179 [1]
5180 = -0.85;
5181 evalcond
5182 [2]
5183 = 0;
5184 evalcond
5185 [3]
5186 = -0.2125;
5187 if (IKabs(
5188 evalcond
5189 [0])
5190 < 0.0000010000000000
5191 && IKabs(
5192 evalcond
5193 [1])
5194 < 0.0000010000000000
5195 && IKabs(
5196 evalcond
5197 [2])
5198 < 0.0000010000000000
5199 && IKabs(
5200 evalcond
5201 [3])
5202 < 0.0000010000000000)
5203 {
5204 bgotonextstatement
5205 = false;
5206 {
5207 IkReal j6array
5208 [2],
5209 cj6array
5210 [2],
5211 sj6array
5212 [2];
5213 bool j6valid
5214 [2]
5215 = {false};
5216 _nj6
5217 = 2;
5218 j6array
5219 [0]
5220 = 0.148889947609497;
5221 sj6array
5222 [0]
5223 = IKsin(
5224 j6array
5225 [0]);
5226 cj6array
5227 [0]
5228 = IKcos(
5229 j6array
5230 [0]);
5231 j6array
5232 [1]
5233 = 3.29048260119929;
5234 sj6array
5235 [1]
5236 = IKsin(
5237 j6array
5238 [1]);
5239 cj6array
5240 [1]
5241 = IKcos(
5242 j6array
5243 [1]);
5244 if (j6array
5245 [0]
5246 > IKPI)
5247 {
5248 j6array
5249 [0]
5250 -= IK2PI;
5251 }
5252 else if (
5253 j6array
5254 [0]
5255 < -IKPI)
5256 {
5257 j6array
5258 [0]
5259 += IK2PI;
5260 }
5261 j6valid
5262 [0]
5263 = true;
5264 if (j6array
5265 [1]
5266 > IKPI)
5267 {
5268 j6array
5269 [1]
5270 -= IK2PI;
5271 }
5272 else if (
5273 j6array
5274 [1]
5275 < -IKPI)
5276 {
5277 j6array
5278 [1]
5279 += IK2PI;
5280 }
5281 j6valid
5282 [1]
5283 = true;
5284 for (
5285 int ij6
5286 = 0;
5287 ij6
5288 < 2;
5289 ++ij6)
5290 {
5291 if (!j6valid
5292 [ij6])
5293 {
5294 continue;
5295 }
5296 _ij6[0]
5297 = ij6;
5298 _ij6[1]
5299 = -1;
5300 for (
5301 int iij6
5302 = ij6
5303 + 1;
5304 iij6
5305 < 2;
5306 ++iij6)
5307 {
5308 if (j6valid
5309 [iij6]
5310 && IKabs(
5311 cj6array
5312 [ij6]
5313 - cj6array
5314 [iij6])
5315 < IKFAST_SOLUTION_THRESH
5316 && IKabs(
5317 sj6array
5318 [ij6]
5319 - sj6array
5320 [iij6])
5321 < IKFAST_SOLUTION_THRESH)
5322 {
5323 j6valid
5324 [iij6]
5325 = false;
5326 _ij6[1]
5327 = iij6;
5328 break;
5329 }
5330 }
5331 j6 = j6array
5332 [ij6];
5333 cj6 = cj6array
5334 [ij6];
5335 sj6 = sj6array
5336 [ij6];
5337 {
5338 IkReal evalcond
5339 [1];
5340 evalcond
5341 [0]
5342 = ((0.85)
5343 * (IKsin(
5344 j6)));
5345 if (IKabs(
5346 evalcond
5347 [0])
5348 > IKFAST_EVALCOND_THRESH)
5349 {
5350 continue;
5351 }
5352 }
5353
5354 rotationfunction0(
5355 solutions);
5356 }
5357 }
5358 }
5359 } while (
5360 0);
5361 if (bgotonextstatement)
5362 {
5363 bool
5364 bgotonextstatement
5365 = true;
5366 do
5367 {
5368 evalcond
5369 [0]
5370 = ((IKabs((
5371 (-3.14159265358979)
5372 + (IKfmod(
5373 ((3.14159265358979)
5374 + j4),
5375 6.28318530717959)))))
5376 + (IKabs(
5377 px)));
5378 evalcond
5379 [1]
5380 = -0.85;
5381 evalcond
5382 [2]
5383 = 0;
5384 evalcond
5385 [3]
5386 = ((-0.2125)
5387 + (((-1.0)
5388 * (1.0)
5389 * (py
5390 * py))));
5391 if (IKabs(
5392 evalcond
5393 [0])
5394 < 0.0000010000000000
5395 && IKabs(
5396 evalcond
5397 [1])
5398 < 0.0000010000000000
5399 && IKabs(
5400 evalcond
5401 [2])
5402 < 0.0000010000000000
5403 && IKabs(
5404 evalcond
5405 [3])
5406 < 0.0000010000000000)
5407 {
5408 bgotonextstatement
5409 = false;
5410 {
5411 IkReal j6eval
5412 [1];
5413 IkReal
5414 x782
5415 = ((1.0)
5416 * py);
5417 sj8 = 0;
5418 cj8 = -1.0;
5419 j8 = 3.14159265358979;
5420 pz = 0;
5421 j9 = 0;
5422 sj9 = 0;
5423 cj9 = 1.0;
5424 pp = py
5425 * py;
5426 npx
5427 = (py
5428 * r10);
5429 npy
5430 = (py
5431 * r11);
5432 npz
5433 = (py
5434 * r12);
5435 rxp0_0
5436 = ((-1.0)
5437 * r20
5438 * x782);
5439 rxp0_1
5440 = 0;
5441 rxp1_0
5442 = ((-1.0)
5443 * r21
5444 * x782);
5445 rxp1_1
5446 = 0;
5447 rxp2_0
5448 = ((-1.0)
5449 * r22
5450 * x782);
5451 rxp2_1
5452 = 0;
5453 px = 0;
5454 j4 = 0;
5455 sj4 = 0;
5456 cj4 = 1.0;
5457 rxp0_2
5458 = (py
5459 * r00);
5460 rxp1_2
5461 = (py
5462 * r01);
5463 rxp2_2
5464 = (py
5465 * r02);
5466 j6eval
5467 [0]
5468 = ((1.0)
5469 + (((1.91568587540858)
5470 * (py
5471 * py
5472 * py
5473 * py)))
5474 + (((-1.0)
5475 * (2.64633970947792)
5476 * (py
5477 * py))));
5478 if (IKabs(
5479 j6eval
5480 [0])
5481 < 0.0000010000000000)
5482 {
5483 continue; // no branches [j6]
5484 }
5485 else
5486 {
5487 {
5488 IkReal j6array
5489 [2],
5490 cj6array
5491 [2],
5492 sj6array
5493 [2];
5494 bool j6valid
5495 [2]
5496 = {false};
5497 _nj6
5498 = 2;
5499 IkReal
5500 x783
5501 = py
5502 * py;
5503 CheckValue<IkReal> x785 = IKatan2WithCheck(
5504 IkReal((
5505 (-0.425)
5506 + (((-0.588235294117647)
5507 * x783)))),
5508 ((2.83333333333333)
5509 + (((-3.92156862745098)
5510 * x783))),
5511 IKFAST_ATAN2_MAGTHRESH);
5512 if (!x785.valid)
5513 {
5514 continue;
5515 }
5516 IkReal
5517 x784
5518 = ((-1.0)
5519 * (x785.value));
5520 j6array
5521 [0]
5522 = x784;
5523 sj6array
5524 [0]
5525 = IKsin(
5526 j6array
5527 [0]);
5528 cj6array
5529 [0]
5530 = IKcos(
5531 j6array
5532 [0]);
5533 j6array
5534 [1]
5535 = ((3.14159265358979)
5536 + x784);
5537 sj6array
5538 [1]
5539 = IKsin(
5540 j6array
5541 [1]);
5542 cj6array
5543 [1]
5544 = IKcos(
5545 j6array
5546 [1]);
5547 if (j6array
5548 [0]
5549 > IKPI)
5550 {
5551 j6array
5552 [0]
5553 -= IK2PI;
5554 }
5555 else if (
5556 j6array
5557 [0]
5558 < -IKPI)
5559 {
5560 j6array
5561 [0]
5562 += IK2PI;
5563 }
5564 j6valid
5565 [0]
5566 = true;
5567 if (j6array
5568 [1]
5569 > IKPI)
5570 {
5571 j6array
5572 [1]
5573 -= IK2PI;
5574 }
5575 else if (
5576 j6array
5577 [1]
5578 < -IKPI)
5579 {
5580 j6array
5581 [1]
5582 += IK2PI;
5583 }
5584 j6valid
5585 [1]
5586 = true;
5587 for (
5588 int ij6
5589 = 0;
5590 ij6
5591 < 2;
5592 ++ij6)
5593 {
5594 if (!j6valid
5595 [ij6])
5596 {
5597 continue;
5598 }
5599 _ij6[0]
5600 = ij6;
5601 _ij6[1]
5602 = -1;
5603 for (
5604 int iij6
5605 = ij6
5606 + 1;
5607 iij6
5608 < 2;
5609 ++iij6)
5610 {
5611 if (j6valid
5612 [iij6]
5613 && IKabs(
5614 cj6array
5615 [ij6]
5616 - cj6array
5617 [iij6])
5618 < IKFAST_SOLUTION_THRESH
5619 && IKabs(
5620 sj6array
5621 [ij6]
5622 - sj6array
5623 [iij6])
5624 < IKFAST_SOLUTION_THRESH)
5625 {
5626 j6valid
5627 [iij6]
5628 = false;
5629 _ij6[1]
5630 = iij6;
5631 break;
5632 }
5633 }
5634 j6 = j6array
5635 [ij6];
5636 cj6 = cj6array
5637 [ij6];
5638 sj6 = sj6array
5639 [ij6];
5640 {
5641 IkReal evalcond
5642 [1];
5643 evalcond
5644 [0]
5645 = ((0.85)
5646 * (IKsin(
5647 j6)));
5648 if (IKabs(
5649 evalcond
5650 [0])
5651 > IKFAST_EVALCOND_THRESH)
5652 {
5653 continue;
5654 }
5655 }
5656
5657 rotationfunction0(
5658 solutions);
5659 }
5660 }
5661 }
5662 }
5663 }
5664 } while (
5665 0);
5666 if (bgotonextstatement)
5667 {
5668 bool
5669 bgotonextstatement
5670 = true;
5671 do
5672 {
5673 evalcond
5674 [0]
5675 = ((IKabs((
5676 (-3.14159265358979)
5677 + (IKfmod(
5678 j4,
5679 6.28318530717959)))))
5680 + (IKabs(
5681 px)));
5682 evalcond
5683 [1]
5684 = -0.85;
5685 evalcond
5686 [2]
5687 = 0;
5688 evalcond
5689 [3]
5690 = ((-0.2125)
5691 + (((-1.0)
5692 * (1.0)
5693 * (py
5694 * py))));
5695 if (IKabs(
5696 evalcond
5697 [0])
5698 < 0.0000010000000000
5699 && IKabs(
5700 evalcond
5701 [1])
5702 < 0.0000010000000000
5703 && IKabs(
5704 evalcond
5705 [2])
5706 < 0.0000010000000000
5707 && IKabs(
5708 evalcond
5709 [3])
5710 < 0.0000010000000000)
5711 {
5712 bgotonextstatement
5713 = false;
5714 {
5715 IkReal j6eval
5716 [1];
5717 IkReal
5718 x786
5719 = ((1.0)
5720 * py);
5721 sj8 = 0;
5722 cj8 = -1.0;
5723 j8 = 3.14159265358979;
5724 pz = 0;
5725 j9 = 0;
5726 sj9 = 0;
5727 cj9 = 1.0;
5728 pp = py
5729 * py;
5730 npx
5731 = (py
5732 * r10);
5733 npy
5734 = (py
5735 * r11);
5736 npz
5737 = (py
5738 * r12);
5739 rxp0_0
5740 = ((-1.0)
5741 * r20
5742 * x786);
5743 rxp0_1
5744 = 0;
5745 rxp1_0
5746 = ((-1.0)
5747 * r21
5748 * x786);
5749 rxp1_1
5750 = 0;
5751 rxp2_0
5752 = ((-1.0)
5753 * r22
5754 * x786);
5755 rxp2_1
5756 = 0;
5757 px = 0;
5758 j4 = 3.14159265358979;
5759 sj4 = 0;
5760 cj4 = -1.0;
5761 rxp0_2
5762 = (py
5763 * r00);
5764 rxp1_2
5765 = (py
5766 * r01);
5767 rxp2_2
5768 = (py
5769 * r02);
5770 j6eval
5771 [0]
5772 = ((1.0)
5773 + (((1.91568587540858)
5774 * (py
5775 * py
5776 * py
5777 * py)))
5778 + (((-1.0)
5779 * (2.64633970947792)
5780 * (py
5781 * py))));
5782 if (IKabs(
5783 j6eval
5784 [0])
5785 < 0.0000010000000000)
5786 {
5787 continue; // no branches [j6]
5788 }
5789 else
5790 {
5791 {
5792 IkReal j6array
5793 [2],
5794 cj6array
5795 [2],
5796 sj6array
5797 [2];
5798 bool j6valid
5799 [2]
5800 = {false};
5801 _nj6
5802 = 2;
5803 IkReal
5804 x787
5805 = py
5806 * py;
5807 CheckValue<IkReal> x789 = IKatan2WithCheck(
5808 IkReal((
5809 (-0.425)
5810 + (((-0.588235294117647)
5811 * x787)))),
5812 ((2.83333333333333)
5813 + (((-3.92156862745098)
5814 * x787))),
5815 IKFAST_ATAN2_MAGTHRESH);
5816 if (!x789.valid)
5817 {
5818 continue;
5819 }
5820 IkReal
5821 x788
5822 = ((-1.0)
5823 * (x789.value));
5824 j6array
5825 [0]
5826 = x788;
5827 sj6array
5828 [0]
5829 = IKsin(
5830 j6array
5831 [0]);
5832 cj6array
5833 [0]
5834 = IKcos(
5835 j6array
5836 [0]);
5837 j6array
5838 [1]
5839 = ((3.14159265358979)
5840 + x788);
5841 sj6array
5842 [1]
5843 = IKsin(
5844 j6array
5845 [1]);
5846 cj6array
5847 [1]
5848 = IKcos(
5849 j6array
5850 [1]);
5851 if (j6array
5852 [0]
5853 > IKPI)
5854 {
5855 j6array
5856 [0]
5857 -= IK2PI;
5858 }
5859 else if (
5860 j6array
5861 [0]
5862 < -IKPI)
5863 {
5864 j6array
5865 [0]
5866 += IK2PI;
5867 }
5868 j6valid
5869 [0]
5870 = true;
5871 if (j6array
5872 [1]
5873 > IKPI)
5874 {
5875 j6array
5876 [1]
5877 -= IK2PI;
5878 }
5879 else if (
5880 j6array
5881 [1]
5882 < -IKPI)
5883 {
5884 j6array
5885 [1]
5886 += IK2PI;
5887 }
5888 j6valid
5889 [1]
5890 = true;
5891 for (
5892 int ij6
5893 = 0;
5894 ij6
5895 < 2;
5896 ++ij6)
5897 {
5898 if (!j6valid
5899 [ij6])
5900 {
5901 continue;
5902 }
5903 _ij6[0]
5904 = ij6;
5905 _ij6[1]
5906 = -1;
5907 for (
5908 int iij6
5909 = ij6
5910 + 1;
5911 iij6
5912 < 2;
5913 ++iij6)
5914 {
5915 if (j6valid
5916 [iij6]
5917 && IKabs(
5918 cj6array
5919 [ij6]
5920 - cj6array
5921 [iij6])
5922 < IKFAST_SOLUTION_THRESH
5923 && IKabs(
5924 sj6array
5925 [ij6]
5926 - sj6array
5927 [iij6])
5928 < IKFAST_SOLUTION_THRESH)
5929 {
5930 j6valid
5931 [iij6]
5932 = false;
5933 _ij6[1]
5934 = iij6;
5935 break;
5936 }
5937 }
5938 j6 = j6array
5939 [ij6];
5940 cj6 = cj6array
5941 [ij6];
5942 sj6 = sj6array
5943 [ij6];
5944 {
5945 IkReal evalcond
5946 [1];
5947 evalcond
5948 [0]
5949 = ((0.85)
5950 * (IKsin(
5951 j6)));
5952 if (IKabs(
5953 evalcond
5954 [0])
5955 > IKFAST_EVALCOND_THRESH)
5956 {
5957 continue;
5958 }
5959 }
5960
5961 rotationfunction0(
5962 solutions);
5963 }
5964 }
5965 }
5966 }
5967 }
5968 } while (
5969 0);
5970 if (bgotonextstatement)
5971 {
5972 bool
5973 bgotonextstatement
5974 = true;
5975 do
5976 {
5977 evalcond
5978 [0]
5979 = ((IKabs((
5980 (-3.14159265358979)
5981 + (IKfmod(
5982 ((1.5707963267949)
5983 + j4),
5984 6.28318530717959)))))
5985 + (IKabs(
5986 py)));
5987 evalcond
5988 [1]
5989 = -0.85;
5990 evalcond
5991 [2]
5992 = 0;
5993 evalcond
5994 [3]
5995 = ((-0.2125)
5996 + (((-1.0)
5997 * (1.0)
5998 * (px
5999 * px))));
6000 if (IKabs(
6001 evalcond
6002 [0])
6003 < 0.0000010000000000
6004 && IKabs(
6005 evalcond
6006 [1])
6007 < 0.0000010000000000
6008 && IKabs(
6009 evalcond
6010 [2])
6011 < 0.0000010000000000
6012 && IKabs(
6013 evalcond
6014 [3])
6015 < 0.0000010000000000)
6016 {
6017 bgotonextstatement
6018 = false;
6019 {
6020 IkReal j6eval
6021 [1];
6022 IkReal
6023 x790
6024 = ((1.0)
6025 * px);
6026 sj8 = 0;
6027 cj8 = -1.0;
6028 j8 = 3.14159265358979;
6029 pz = 0;
6030 j9 = 0;
6031 sj9 = 0;
6032 cj9 = 1.0;
6033 pp = px
6034 * px;
6035 npx
6036 = (px
6037 * r00);
6038 npy
6039 = (px
6040 * r01);
6041 npz
6042 = (px
6043 * r02);
6044 rxp0_0
6045 = 0;
6046 rxp0_1
6047 = (px
6048 * r20);
6049 rxp1_0
6050 = 0;
6051 rxp1_1
6052 = (px
6053 * r21);
6054 rxp2_0
6055 = 0;
6056 rxp2_1
6057 = (px
6058 * r22);
6059 py = 0;
6060 j4 = 1.5707963267949;
6061 sj4 = 1.0;
6062 cj4 = 0;
6063 rxp0_2
6064 = ((-1.0)
6065 * r10
6066 * x790);
6067 rxp1_2
6068 = ((-1.0)
6069 * r11
6070 * x790);
6071 rxp2_2
6072 = ((-1.0)
6073 * r12
6074 * x790);
6075 j6eval
6076 [0]
6077 = ((1.0)
6078 + (((1.91568587540858)
6079 * (px
6080 * px
6081 * px
6082 * px)))
6083 + (((-1.0)
6084 * (2.64633970947792)
6085 * (px
6086 * px))));
6087 if (IKabs(
6088 j6eval
6089 [0])
6090 < 0.0000010000000000)
6091 {
6092 continue; // no branches [j6]
6093 }
6094 else
6095 {
6096 {
6097 IkReal j6array
6098 [2],
6099 cj6array
6100 [2],
6101 sj6array
6102 [2];
6103 bool j6valid
6104 [2]
6105 = {false};
6106 _nj6
6107 = 2;
6108 IkReal
6109 x791
6110 = px
6111 * px;
6112 CheckValue<IkReal> x793 = IKatan2WithCheck(
6113 IkReal((
6114 (-0.425)
6115 + (((-0.588235294117647)
6116 * x791)))),
6117 ((2.83333333333333)
6118 + (((-3.92156862745098)
6119 * x791))),
6120 IKFAST_ATAN2_MAGTHRESH);
6121 if (!x793.valid)
6122 {
6123 continue;
6124 }
6125 IkReal
6126 x792
6127 = ((-1.0)
6128 * (x793.value));
6129 j6array
6130 [0]
6131 = x792;
6132 sj6array
6133 [0]
6134 = IKsin(
6135 j6array
6136 [0]);
6137 cj6array
6138 [0]
6139 = IKcos(
6140 j6array
6141 [0]);
6142 j6array
6143 [1]
6144 = ((3.14159265358979)
6145 + x792);
6146 sj6array
6147 [1]
6148 = IKsin(
6149 j6array
6150 [1]);
6151 cj6array
6152 [1]
6153 = IKcos(
6154 j6array
6155 [1]);
6156 if (j6array
6157 [0]
6158 > IKPI)
6159 {
6160 j6array
6161 [0]
6162 -= IK2PI;
6163 }
6164 else if (
6165 j6array
6166 [0]
6167 < -IKPI)
6168 {
6169 j6array
6170 [0]
6171 += IK2PI;
6172 }
6173 j6valid
6174 [0]
6175 = true;
6176 if (j6array
6177 [1]
6178 > IKPI)
6179 {
6180 j6array
6181 [1]
6182 -= IK2PI;
6183 }
6184 else if (
6185 j6array
6186 [1]
6187 < -IKPI)
6188 {
6189 j6array
6190 [1]
6191 += IK2PI;
6192 }
6193 j6valid
6194 [1]
6195 = true;
6196 for (
6197 int ij6
6198 = 0;
6199 ij6
6200 < 2;
6201 ++ij6)
6202 {
6203 if (!j6valid
6204 [ij6])
6205 {
6206 continue;
6207 }
6208 _ij6[0]
6209 = ij6;
6210 _ij6[1]
6211 = -1;
6212 for (
6213 int iij6
6214 = ij6
6215 + 1;
6216 iij6
6217 < 2;
6218 ++iij6)
6219 {
6220 if (j6valid
6221 [iij6]
6222 && IKabs(
6223 cj6array
6224 [ij6]
6225 - cj6array
6226 [iij6])
6227 < IKFAST_SOLUTION_THRESH
6228 && IKabs(
6229 sj6array
6230 [ij6]
6231 - sj6array
6232 [iij6])
6233 < IKFAST_SOLUTION_THRESH)
6234 {
6235 j6valid
6236 [iij6]
6237 = false;
6238 _ij6[1]
6239 = iij6;
6240 break;
6241 }
6242 }
6243 j6 = j6array
6244 [ij6];
6245 cj6 = cj6array
6246 [ij6];
6247 sj6 = sj6array
6248 [ij6];
6249 {
6250 IkReal evalcond
6251 [1];
6252 evalcond
6253 [0]
6254 = ((0.85)
6255 * (IKsin(
6256 j6)));
6257 if (IKabs(
6258 evalcond
6259 [0])
6260 > IKFAST_EVALCOND_THRESH)
6261 {
6262 continue;
6263 }
6264 }
6265
6266 rotationfunction0(
6267 solutions);
6268 }
6269 }
6270 }
6271 }
6272 }
6273 } while (
6274 0);
6275 if (bgotonextstatement)
6276 {
6277 bool
6278 bgotonextstatement
6279 = true;
6280 do
6281 {
6282 evalcond
6283 [0]
6284 = ((IKabs(
6285 py))
6286 + (IKabs((
6287 (-3.14159265358979)
6288 + (IKfmod(
6289 ((4.71238898038469)
6290 + j4),
6291 6.28318530717959))))));
6292 evalcond
6293 [1]
6294 = -0.85;
6295 evalcond
6296 [2]
6297 = 0;
6298 evalcond
6299 [3]
6300 = ((-0.2125)
6301 + (((-1.0)
6302 * (1.0)
6303 * (px
6304 * px))));
6305 if (IKabs(
6306 evalcond
6307 [0])
6308 < 0.0000010000000000
6309 && IKabs(
6310 evalcond
6311 [1])
6312 < 0.0000010000000000
6313 && IKabs(
6314 evalcond
6315 [2])
6316 < 0.0000010000000000
6317 && IKabs(
6318 evalcond
6319 [3])
6320 < 0.0000010000000000)
6321 {
6322 bgotonextstatement
6323 = false;
6324 {
6325 IkReal j6eval
6326 [1];
6327 IkReal
6328 x794
6329 = ((1.0)
6330 * px);
6331 sj8 = 0;
6332 cj8 = -1.0;
6333 j8 = 3.14159265358979;
6334 pz = 0;
6335 j9 = 0;
6336 sj9 = 0;
6337 cj9 = 1.0;
6338 pp = px
6339 * px;
6340 npx
6341 = (px
6342 * r00);
6343 npy
6344 = (px
6345 * r01);
6346 npz
6347 = (px
6348 * r02);
6349 rxp0_0
6350 = 0;
6351 rxp0_1
6352 = (px
6353 * r20);
6354 rxp1_0
6355 = 0;
6356 rxp1_1
6357 = (px
6358 * r21);
6359 rxp2_0
6360 = 0;
6361 rxp2_1
6362 = (px
6363 * r22);
6364 py = 0;
6365 j4 = -1.5707963267949;
6366 sj4 = -1.0;
6367 cj4 = 0;
6368 rxp0_2
6369 = ((-1.0)
6370 * r10
6371 * x794);
6372 rxp1_2
6373 = ((-1.0)
6374 * r11
6375 * x794);
6376 rxp2_2
6377 = ((-1.0)
6378 * r12
6379 * x794);
6380 j6eval
6381 [0]
6382 = ((1.0)
6383 + (((1.91568587540858)
6384 * (px
6385 * px
6386 * px
6387 * px)))
6388 + (((-1.0)
6389 * (2.64633970947792)
6390 * (px
6391 * px))));
6392 if (IKabs(
6393 j6eval
6394 [0])
6395 < 0.0000010000000000)
6396 {
6397 continue; // no branches [j6]
6398 }
6399 else
6400 {
6401 {
6402 IkReal j6array
6403 [2],
6404 cj6array
6405 [2],
6406 sj6array
6407 [2];
6408 bool j6valid
6409 [2]
6410 = {false};
6411 _nj6
6412 = 2;
6413 IkReal
6414 x795
6415 = px
6416 * px;
6417 CheckValue<IkReal> x797 = IKatan2WithCheck(
6418 IkReal((
6419 (-0.425)
6420 + (((-0.588235294117647)
6421 * x795)))),
6422 ((2.83333333333333)
6423 + (((-3.92156862745098)
6424 * x795))),
6425 IKFAST_ATAN2_MAGTHRESH);
6426 if (!x797.valid)
6427 {
6428 continue;
6429 }
6430 IkReal
6431 x796
6432 = ((-1.0)
6433 * (x797.value));
6434 j6array
6435 [0]
6436 = x796;
6437 sj6array
6438 [0]
6439 = IKsin(
6440 j6array
6441 [0]);
6442 cj6array
6443 [0]
6444 = IKcos(
6445 j6array
6446 [0]);
6447 j6array
6448 [1]
6449 = ((3.14159265358979)
6450 + x796);
6451 sj6array
6452 [1]
6453 = IKsin(
6454 j6array
6455 [1]);
6456 cj6array
6457 [1]
6458 = IKcos(
6459 j6array
6460 [1]);
6461 if (j6array
6462 [0]
6463 > IKPI)
6464 {
6465 j6array
6466 [0]
6467 -= IK2PI;
6468 }
6469 else if (
6470 j6array
6471 [0]
6472 < -IKPI)
6473 {
6474 j6array
6475 [0]
6476 += IK2PI;
6477 }
6478 j6valid
6479 [0]
6480 = true;
6481 if (j6array
6482 [1]
6483 > IKPI)
6484 {
6485 j6array
6486 [1]
6487 -= IK2PI;
6488 }
6489 else if (
6490 j6array
6491 [1]
6492 < -IKPI)
6493 {
6494 j6array
6495 [1]
6496 += IK2PI;
6497 }
6498 j6valid
6499 [1]
6500 = true;
6501 for (
6502 int ij6
6503 = 0;
6504 ij6
6505 < 2;
6506 ++ij6)
6507 {
6508 if (!j6valid
6509 [ij6])
6510 {
6511 continue;
6512 }
6513 _ij6[0]
6514 = ij6;
6515 _ij6[1]
6516 = -1;
6517 for (
6518 int iij6
6519 = ij6
6520 + 1;
6521 iij6
6522 < 2;
6523 ++iij6)
6524 {
6525 if (j6valid
6526 [iij6]
6527 && IKabs(
6528 cj6array
6529 [ij6]
6530 - cj6array
6531 [iij6])
6532 < IKFAST_SOLUTION_THRESH
6533 && IKabs(
6534 sj6array
6535 [ij6]
6536 - sj6array
6537 [iij6])
6538 < IKFAST_SOLUTION_THRESH)
6539 {
6540 j6valid
6541 [iij6]
6542 = false;
6543 _ij6[1]
6544 = iij6;
6545 break;
6546 }
6547 }
6548 j6 = j6array
6549 [ij6];
6550 cj6 = cj6array
6551 [ij6];
6552 sj6 = sj6array
6553 [ij6];
6554 {
6555 IkReal evalcond
6556 [1];
6557 evalcond
6558 [0]
6559 = ((0.85)
6560 * (IKsin(
6561 j6)));
6562 if (IKabs(
6563 evalcond
6564 [0])
6565 > IKFAST_EVALCOND_THRESH)
6566 {
6567 continue;
6568 }
6569 }
6570
6571 rotationfunction0(
6572 solutions);
6573 }
6574 }
6575 }
6576 }
6577 }
6578 } while (
6579 0);
6580 if (bgotonextstatement)
6581 {
6582 bool
6583 bgotonextstatement
6584 = true;
6585 do
6586 {
6587 if (1)
6588 {
6589 bgotonextstatement
6590 = false;
6591 continue; // branch miss [j6]
6592 }
6593 } while (
6594 0);
6595 if (bgotonextstatement)
6596 {
6597 }
6598 }
6599 }
6600 }
6601 }
6602 }
6603 }
6604 }
6605 else
6606 {
6607 {
6608 IkReal j6array
6609 [1],
6610 cj6array
6611 [1],
6612 sj6array
6613 [1];
6614 bool j6valid
6615 [1]
6616 = {false};
6617 _nj6
6618 = 1;
6619 IkReal
6620 x798
6621 = (cj4
6622 * px);
6623 IkReal
6624 x799
6625 = (py
6626 * sj4);
6627 IkReal
6628 x800
6629 = px
6630 * px;
6631 IkReal
6632 x801
6633 = py
6634 * py;
6635 CheckValue<IkReal> x802 = IKPowWithIntegerCheck(
6636 ((((20.0)
6637 * x799))
6638 + (((20.0)
6639 * x798))),
6640 -1);
6641 if (!x802.valid)
6642 {
6643 continue;
6644 }
6645 CheckValue<IkReal> x803 = IKPowWithIntegerCheck(
6646 ((((-8.5)
6647 * x798))
6648 + (((-1.0)
6649 * (11.7647058823529)
6650 * cj4
6651 * (px
6652 * px
6653 * px)))
6654 + (((-11.7647058823529)
6655 * x798
6656 * x801))
6657 + (((-11.7647058823529)
6658 * x799
6659 * x800))
6660 + (((-8.5)
6661 * x799))
6662 + (((-1.0)
6663 * (11.7647058823529)
6664 * sj4
6665 * (py
6666 * py
6667 * py)))),
6668 -1);
6669 if (!x803.valid)
6670 {
6671 continue;
6672 }
6673 if (IKabs((
6674 (17.0)
6675 * (x802.value)))
6676 < IKFAST_ATAN2_MAGTHRESH
6677 && IKabs((
6678 (x803.value)
6679 * (((-48.1666666666667)
6680 + (((66.6666666666667)
6681 * x800))
6682 + (((66.6666666666667)
6683 * x801))))))
6684 < IKFAST_ATAN2_MAGTHRESH
6685 && IKabs(
6686 IKsqr((
6687 (17.0)
6688 * (x802.value)))
6689 + IKsqr((
6690 (x803.value)
6691 * (((-48.1666666666667)
6692 + (((66.6666666666667)
6693 * x800))
6694 + (((66.6666666666667)
6695 * x801))))))
6696 - 1)
6697 <= IKFAST_SINCOS_THRESH)
6698 continue;
6699 j6array[0] = IKatan2(
6700 ((17.0)
6701 * (x802.value)),
6702 ((x803.value)
6703 * (((-48.1666666666667)
6704 + (((66.6666666666667)
6705 * x800))
6706 + (((66.6666666666667)
6707 * x801))))));
6708 sj6array
6709 [0]
6710 = IKsin(
6711 j6array
6712 [0]);
6713 cj6array
6714 [0]
6715 = IKcos(
6716 j6array
6717 [0]);
6718 if (j6array
6719 [0]
6720 > IKPI)
6721 {
6722 j6array
6723 [0]
6724 -= IK2PI;
6725 }
6726 else if (
6727 j6array
6728 [0]
6729 < -IKPI)
6730 {
6731 j6array
6732 [0]
6733 += IK2PI;
6734 }
6735 j6valid
6736 [0]
6737 = true;
6738 for (
6739 int ij6
6740 = 0;
6741 ij6
6742 < 1;
6743 ++ij6)
6744 {
6745 if (!j6valid
6746 [ij6])
6747 {
6748 continue;
6749 }
6750 _ij6[0]
6751 = ij6;
6752 _ij6[1]
6753 = -1;
6754 for (
6755 int iij6
6756 = ij6
6757 + 1;
6758 iij6
6759 < 1;
6760 ++iij6)
6761 {
6762 if (j6valid
6763 [iij6]
6764 && IKabs(
6765 cj6array
6766 [ij6]
6767 - cj6array
6768 [iij6])
6769 < IKFAST_SOLUTION_THRESH
6770 && IKabs(
6771 sj6array
6772 [ij6]
6773 - sj6array
6774 [iij6])
6775 < IKFAST_SOLUTION_THRESH)
6776 {
6777 j6valid
6778 [iij6]
6779 = false;
6780 _ij6[1]
6781 = iij6;
6782 break;
6783 }
6784 }
6785 j6 = j6array
6786 [ij6];
6787 cj6 = cj6array
6788 [ij6];
6789 sj6 = sj6array
6790 [ij6];
6791 {
6792 IkReal evalcond
6793 [5];
6794 IkReal
6795 x804
6796 = IKcos(
6797 j6);
6798 IkReal
6799 x805
6800 = (cj4
6801 * px);
6802 IkReal
6803 x806
6804 = (x804
6805 * x805);
6806 IkReal
6807 x807
6808 = (py
6809 * sj4);
6810 IkReal
6811 x808
6812 = (x804
6813 * x807);
6814 IkReal
6815 x809
6816 = IKsin(
6817 j6);
6818 IkReal
6819 x810
6820 = (x805
6821 * x809);
6822 IkReal
6823 x811
6824 = (x807
6825 * x809);
6826 IkReal
6827 x812
6828 = px
6829 * px;
6830 IkReal
6831 x813
6832 = ((3.92156862745098)
6833 * x809);
6834 IkReal
6835 x814
6836 = ((0.588235294117647)
6837 * x804);
6838 IkReal
6839 x815
6840 = py
6841 * py;
6842 evalcond
6843 [0]
6844 = (x806
6845 + x808);
6846 evalcond
6847 [1]
6848 = ((-0.85)
6849 + x811
6850 + x810);
6851 evalcond
6852 [2]
6853 = ((((-1.0)
6854 * x807))
6855 + (((0.85)
6856 * x809))
6857 + (((-1.0)
6858 * x805)));
6859 evalcond
6860 [3]
6861 = ((((-1.0)
6862 * x812
6863 * x814))
6864 + (((-1.0)
6865 * x814
6866 * x815))
6867 + (((-1.0)
6868 * x812
6869 * x813))
6870 + (((2.83333333333333)
6871 * x809))
6872 + (((-0.425)
6873 * x804))
6874 + (((-1.0)
6875 * x813
6876 * x815)));
6877 evalcond
6878 [4]
6879 = ((-0.2125)
6880 + (((1.1)
6881 * x810))
6882 + (((-0.09)
6883 * x808))
6884 + (((-1.0)
6885 * x815))
6886 + (((1.1)
6887 * x811))
6888 + (((-1.0)
6889 * x812))
6890 + (((-0.09)
6891 * x806)));
6892 if (IKabs(
6893 evalcond
6894 [0])
6895 > IKFAST_EVALCOND_THRESH
6896 || IKabs(
6897 evalcond
6898 [1])
6899 > IKFAST_EVALCOND_THRESH
6900 || IKabs(
6901 evalcond
6902 [2])
6903 > IKFAST_EVALCOND_THRESH
6904 || IKabs(
6905 evalcond
6906 [3])
6907 > IKFAST_EVALCOND_THRESH
6908 || IKabs(
6909 evalcond
6910 [4])
6911 > IKFAST_EVALCOND_THRESH)
6912 {
6913 continue;
6914 }
6915 }
6916
6917 rotationfunction0(
6918 solutions);
6919 }
6920 }
6921 }
6922 }
6923 }
6924 else
6925 {
6926 {
6927 IkReal j6array
6928 [1],
6929 cj6array
6930 [1],
6931 sj6array
6932 [1];
6933 bool j6valid
6934 [1]
6935 = {false};
6936 _nj6
6937 = 1;
6938 IkReal
6939 x816
6940 = (cj4
6941 * px);
6942 IkReal
6943 x817
6944 = (py
6945 * sj4);
6946 IkReal
6947 x818
6948 = px
6949 * px;
6950 IkReal
6951 x819
6952 = py
6953 * py;
6954 CheckValue<IkReal> x820 = IKPowWithIntegerCheck(
6955 ((-7.225)
6956 + (((-10.0)
6957 * x818))
6958 + (((-10.0)
6959 * x819))),
6960 -1);
6961 if (!x820.valid)
6962 {
6963 continue;
6964 }
6965 if (IKabs((
6966 (((1.17647058823529)
6967 * x816))
6968 + (((1.17647058823529)
6969 * x817))))
6970 < IKFAST_ATAN2_MAGTHRESH
6971 && IKabs((
6972 (x820.value)
6973 * (((((78.4313725490196)
6974 * x817
6975 * x818))
6976 + (((78.4313725490196)
6977 * sj4
6978 * (py
6979 * py
6980 * py)))
6981 + (((78.4313725490196)
6982 * cj4
6983 * (px
6984 * px
6985 * px)))
6986 + (((-56.6666666666667)
6987 * x816))
6988 + (((-56.6666666666667)
6989 * x817))
6990 + (((78.4313725490196)
6991 * x816
6992 * x819))))))
6993 < IKFAST_ATAN2_MAGTHRESH
6994 && IKabs(
6995 IKsqr((
6996 (((1.17647058823529)
6997 * x816))
6998 + (((1.17647058823529)
6999 * x817))))
7000 + IKsqr(
7001 (
7002 (x820.value)
7003 * (((((78.4313725490196)
7004 * x817
7005 * x818))
7006 + (((78.4313725490196) * sj4 * (py * py * py))) + (((78.4313725490196) * cj4 * (px * px * px))) + (((-56.6666666666667) * x816))
7007 + (((-56.6666666666667)
7008 * x817))
7009 + (((78.4313725490196)
7010 * x816
7011 * x819))))))
7012 - 1)
7013 <= IKFAST_SINCOS_THRESH)
7014 continue;
7015 j6array[0] = IKatan2(
7016 ((((1.17647058823529)
7017 * x816))
7018 + (((1.17647058823529)
7019 * x817))),
7020 ((x820.value)
7021 * (((((78.4313725490196)
7022 * x817
7023 * x818))
7024 + (((78.4313725490196)
7025 * sj4
7026 * (py
7027 * py
7028 * py)))
7029 + (((78.4313725490196)
7030 * cj4
7031 * (px
7032 * px
7033 * px)))
7034 + (((-56.6666666666667)
7035 * x816))
7036 + (((-56.6666666666667)
7037 * x817))
7038 + (((78.4313725490196)
7039 * x816
7040 * x819))))));
7041 sj6array
7042 [0]
7043 = IKsin(
7044 j6array
7045 [0]);
7046 cj6array
7047 [0]
7048 = IKcos(
7049 j6array
7050 [0]);
7051 if (j6array
7052 [0]
7053 > IKPI)
7054 {
7055 j6array
7056 [0]
7057 -= IK2PI;
7058 }
7059 else if (
7060 j6array
7061 [0]
7062 < -IKPI)
7063 {
7064 j6array
7065 [0]
7066 += IK2PI;
7067 }
7068 j6valid
7069 [0]
7070 = true;
7071 for (
7072 int ij6
7073 = 0;
7074 ij6
7075 < 1;
7076 ++ij6)
7077 {
7078 if (!j6valid
7079 [ij6])
7080 {
7081 continue;
7082 }
7083 _ij6[0]
7084 = ij6;
7085 _ij6[1]
7086 = -1;
7087 for (
7088 int iij6
7089 = ij6
7090 + 1;
7091 iij6
7092 < 1;
7093 ++iij6)
7094 {
7095 if (j6valid
7096 [iij6]
7097 && IKabs(
7098 cj6array
7099 [ij6]
7100 - cj6array
7101 [iij6])
7102 < IKFAST_SOLUTION_THRESH
7103 && IKabs(
7104 sj6array
7105 [ij6]
7106 - sj6array
7107 [iij6])
7108 < IKFAST_SOLUTION_THRESH)
7109 {
7110 j6valid
7111 [iij6]
7112 = false;
7113 _ij6[1]
7114 = iij6;
7115 break;
7116 }
7117 }
7118 j6 = j6array
7119 [ij6];
7120 cj6 = cj6array
7121 [ij6];
7122 sj6 = sj6array
7123 [ij6];
7124 {
7125 IkReal evalcond
7126 [5];
7127 IkReal
7128 x821
7129 = IKcos(
7130 j6);
7131 IkReal
7132 x822
7133 = (cj4
7134 * px);
7135 IkReal
7136 x823
7137 = (x821
7138 * x822);
7139 IkReal
7140 x824
7141 = (py
7142 * sj4);
7143 IkReal
7144 x825
7145 = (x821
7146 * x824);
7147 IkReal
7148 x826
7149 = IKsin(
7150 j6);
7151 IkReal
7152 x827
7153 = (x822
7154 * x826);
7155 IkReal
7156 x828
7157 = (x824
7158 * x826);
7159 IkReal
7160 x829
7161 = px
7162 * px;
7163 IkReal
7164 x830
7165 = ((3.92156862745098)
7166 * x826);
7167 IkReal
7168 x831
7169 = ((0.588235294117647)
7170 * x821);
7171 IkReal
7172 x832
7173 = py
7174 * py;
7175 evalcond
7176 [0]
7177 = (x825
7178 + x823);
7179 evalcond
7180 [1]
7181 = ((-0.85)
7182 + x827
7183 + x828);
7184 evalcond
7185 [2]
7186 = ((((0.85)
7187 * x826))
7188 + (((-1.0)
7189 * x822))
7190 + (((-1.0)
7191 * x824)));
7192 evalcond
7193 [3]
7194 = ((((-1.0)
7195 * x830
7196 * x832))
7197 + (((2.83333333333333)
7198 * x826))
7199 + (((-1.0)
7200 * x829
7201 * x831))
7202 + (((-1.0)
7203 * x831
7204 * x832))
7205 + (((-1.0)
7206 * x829
7207 * x830))
7208 + (((-0.425)
7209 * x821)));
7210 evalcond
7211 [4]
7212 = ((-0.2125)
7213 + (((1.1)
7214 * x828))
7215 + (((-0.09)
7216 * x825))
7217 + (((1.1)
7218 * x827))
7219 + (((-0.09)
7220 * x823))
7221 + (((-1.0)
7222 * x832))
7223 + (((-1.0)
7224 * x829)));
7225 if (IKabs(
7226 evalcond
7227 [0])
7228 > IKFAST_EVALCOND_THRESH
7229 || IKabs(
7230 evalcond
7231 [1])
7232 > IKFAST_EVALCOND_THRESH
7233 || IKabs(
7234 evalcond
7235 [2])
7236 > IKFAST_EVALCOND_THRESH
7237 || IKabs(
7238 evalcond
7239 [3])
7240 > IKFAST_EVALCOND_THRESH
7241 || IKabs(
7242 evalcond
7243 [4])
7244 > IKFAST_EVALCOND_THRESH)
7245 {
7246 continue;
7247 }
7248 }
7249
7250 rotationfunction0(
7251 solutions);
7252 }
7253 }
7254 }
7255 }
7256 }
7257 else
7258 {
7259 {
7260 IkReal j6array
7261 [1],
7262 cj6array
7263 [1],
7264 sj6array
7265 [1];
7266 bool j6valid
7267 [1]
7268 = {false};
7269 _nj6 = 1;
7270 IkReal
7271 x833
7272 = (cj4
7273 * px);
7274 IkReal
7275 x834
7276 = (py
7277 * sj4);
7278 IkReal
7279 x835
7280 = px
7281 * px;
7282 IkReal
7283 x836
7284 = py
7285 * py;
7286 IkReal
7287 x837
7288 = ((1.29411764705882)
7289 * (cj4
7290 * cj4));
7291 CheckValue<
7292 IkReal>
7293 x838
7294 = IKPowWithIntegerCheck(
7295 ((((-0.09)
7296 * x834))
7297 + (((-0.09)
7298 * x833))),
7299 -1);
7300 if (!x838.valid)
7301 {
7302 continue;
7303 }
7304 if (IKabs((
7305 (((1.17647058823529)
7306 * x833))
7307 + (((1.17647058823529)
7308 * x834))))
7309 < IKFAST_ATAN2_MAGTHRESH
7310 && IKabs((
7311 (x838.value)
7312 * (((0.2125)
7313 + (((-0.294117647058824)
7314 * x836))
7315 + x835
7316 + (((-2.58823529411765)
7317 * cj4
7318 * px
7319 * x834))
7320 + ((x836
7321 * x837))
7322 + (((-1.0)
7323 * x835
7324 * x837))))))
7325 < IKFAST_ATAN2_MAGTHRESH
7326 && IKabs(
7327 IKsqr((
7328 (((1.17647058823529)
7329 * x833))
7330 + (((1.17647058823529)
7331 * x834))))
7332 + IKsqr((
7333 (x838.value)
7334 * (((0.2125)
7335 + (((-0.294117647058824)
7336 * x836))
7337 + x835
7338 + (((-2.58823529411765)
7339 * cj4
7340 * px
7341 * x834))
7342 + ((x836
7343 * x837))
7344 + (((-1.0)
7345 * x835
7346 * x837))))))
7347 - 1)
7348 <= IKFAST_SINCOS_THRESH)
7349 continue;
7350 j6array[0] = IKatan2(
7351 ((((1.17647058823529)
7352 * x833))
7353 + (((1.17647058823529)
7354 * x834))),
7355 ((x838.value)
7356 * (((0.2125)
7357 + (((-0.294117647058824)
7358 * x836))
7359 + x835
7360 + (((-2.58823529411765)
7361 * cj4
7362 * px
7363 * x834))
7364 + ((x836
7365 * x837))
7366 + (((-1.0)
7367 * x835
7368 * x837))))));
7369 sj6array[0] = IKsin(
7370 j6array
7371 [0]);
7372 cj6array[0] = IKcos(
7373 j6array
7374 [0]);
7375 if (j6array
7376 [0]
7377 > IKPI)
7378 {
7379 j6array
7380 [0]
7381 -= IK2PI;
7382 }
7383 else if (
7384 j6array
7385 [0]
7386 < -IKPI)
7387 {
7388 j6array
7389 [0]
7390 += IK2PI;
7391 }
7392 j6valid[0]
7393 = true;
7394 for (
7395 int ij6
7396 = 0;
7397 ij6
7398 < 1;
7399 ++ij6)
7400 {
7401 if (!j6valid
7402 [ij6])
7403 {
7404 continue;
7405 }
7406 _ij6[0]
7407 = ij6;
7408 _ij6[1]
7409 = -1;
7410 for (
7411 int iij6
7412 = ij6
7413 + 1;
7414 iij6
7415 < 1;
7416 ++iij6)
7417 {
7418 if (j6valid
7419 [iij6]
7420 && IKabs(
7421 cj6array
7422 [ij6]
7423 - cj6array
7424 [iij6])
7425 < IKFAST_SOLUTION_THRESH
7426 && IKabs(
7427 sj6array
7428 [ij6]
7429 - sj6array
7430 [iij6])
7431 < IKFAST_SOLUTION_THRESH)
7432 {
7433 j6valid
7434 [iij6]
7435 = false;
7436 _ij6[1]
7437 = iij6;
7438 break;
7439 }
7440 }
7441 j6 = j6array
7442 [ij6];
7443 cj6 = cj6array
7444 [ij6];
7445 sj6 = sj6array
7446 [ij6];
7447 {
7448 IkReal evalcond
7449 [5];
7450 IkReal
7451 x839
7452 = IKcos(
7453 j6);
7454 IkReal
7455 x840
7456 = (cj4
7457 * px);
7458 IkReal
7459 x841
7460 = (x839
7461 * x840);
7462 IkReal
7463 x842
7464 = (py
7465 * sj4);
7466 IkReal
7467 x843
7468 = (x839
7469 * x842);
7470 IkReal
7471 x844
7472 = IKsin(
7473 j6);
7474 IkReal
7475 x845
7476 = (x840
7477 * x844);
7478 IkReal
7479 x846
7480 = (x842
7481 * x844);
7482 IkReal
7483 x847
7484 = px
7485 * px;
7486 IkReal
7487 x848
7488 = ((3.92156862745098)
7489 * x844);
7490 IkReal
7491 x849
7492 = ((0.588235294117647)
7493 * x839);
7494 IkReal
7495 x850
7496 = py
7497 * py;
7498 evalcond
7499 [0]
7500 = (x843
7501 + x841);
7502 evalcond
7503 [1]
7504 = ((-0.85)
7505 + x846
7506 + x845);
7507 evalcond
7508 [2]
7509 = ((((0.85)
7510 * x844))
7511 + (((-1.0)
7512 * x842))
7513 + (((-1.0)
7514 * x840)));
7515 evalcond
7516 [3]
7517 = ((((-1.0)
7518 * x847
7519 * x849))
7520 + (((-1.0)
7521 * x847
7522 * x848))
7523 + (((-0.425)
7524 * x839))
7525 + (((-1.0)
7526 * x848
7527 * x850))
7528 + (((-1.0)
7529 * x849
7530 * x850))
7531 + (((2.83333333333333)
7532 * x844)));
7533 evalcond
7534 [4]
7535 = ((-0.2125)
7536 + (((-1.0)
7537 * x850))
7538 + (((-0.09)
7539 * x843))
7540 + (((1.1)
7541 * x846))
7542 + (((1.1)
7543 * x845))
7544 + (((-1.0)
7545 * x847))
7546 + (((-0.09)
7547 * x841)));
7548 if (IKabs(
7549 evalcond
7550 [0])
7551 > IKFAST_EVALCOND_THRESH
7552 || IKabs(
7553 evalcond
7554 [1])
7555 > IKFAST_EVALCOND_THRESH
7556 || IKabs(
7557 evalcond
7558 [2])
7559 > IKFAST_EVALCOND_THRESH
7560 || IKabs(
7561 evalcond
7562 [3])
7563 > IKFAST_EVALCOND_THRESH
7564 || IKabs(
7565 evalcond
7566 [4])
7567 > IKFAST_EVALCOND_THRESH)
7568 {
7569 continue;
7570 }
7571 }
7572
7573 rotationfunction0(
7574 solutions);
7575 }
7576 }
7577 }
7578 }
7579 }
7580 } while (0);
7581 if (bgotonextstatement)
7582 {
7583 bool
7584 bgotonextstatement
7585 = true;
7586 do
7587 {
7588 if (1)
7589 {
7590 bgotonextstatement
7591 = false;
7592 continue; // branch miss [j6]
7593 }
7594 } while (0);
7595 if (bgotonextstatement)
7596 {
7597 }
7598 }
7599 }
7600 }
7601 else
7602 {
7603 {
7604 IkReal j6array[1],
7605 cj6array[1],
7606 sj6array[1];
7607 bool j6valid[1]
7608 = {false};
7609 _nj6 = 1;
7610 IkReal x851
7611 = (cj4 * px);
7612 IkReal x852
7613 = (py * sj4);
7614 IkReal x853
7615 = ((0.108264705882353)
7616 * cj9);
7617 IkReal x854
7618 = ((0.588235294117647)
7619 * pp);
7620 IkReal x855
7621 = (cj9 * pp);
7622 IkReal x856
7623 = (cj9 * sj9);
7624 IkReal x857
7625 = (pp * sj9);
7626 IkReal x858
7627 = cj9 * cj9;
7628 IkReal x859
7629 = ((1.0) * pz);
7630 CheckValue<IkReal> x860 = IKatan2WithCheck(
7631 IkReal((
7632 (-0.174204411764706)
7633 + (pz * pz)
7634 + (((-0.176470588235294)
7635 * x855))
7636 + (((-1.0)
7637 * (0.154566176470588)
7638 * cj9))
7639 + (((-1.0)
7640 * (0.323529411764706)
7641 * pp))
7642 + (((-0.0264705882352941)
7643 * x857))
7644 + (((-0.00487191176470588)
7645 * x856))
7646 + (((-1.0)
7647 * (0.0142530882352941)
7648 * sj9))
7649 + (((-0.0324794117647059)
7650 * x858)))),
7651 ((-0.830553921568627)
7652 + (((-0.396970588235294)
7653 * x858))
7654 + (((-1.0)
7655 * (0.0679544117647059)
7656 * sj9))
7657 + (((-1.0)
7658 * (1.18080882352941)
7659 * cj9))
7660 + (((0.176470588235294)
7661 * x857))
7662 + (((-1.0)
7663 * x852
7664 * x859))
7665 + (((2.15686274509804)
7666 * pp))
7667 + (((1.17647058823529)
7668 * x855))
7669 + (((-1.0)
7670 * x851
7671 * x859))
7672 + (((-0.0595455882352941)
7673 * x856))),
7674 IKFAST_ATAN2_MAGTHRESH);
7675 if (!x860.valid)
7676 {
7677 continue;
7678 }
7679 CheckValue<IkReal> x861 = IKPowWithIntegerCheck(
7680 IKsign((
7681 (((-1.0)
7682 * x851
7683 * x854))
7684 + (((-1.0)
7685 * (1.32323529411765)
7686 * cj9
7687 * pz))
7688 + (((3.92156862745098)
7689 * pp
7690 * pz))
7691 + (((-1.0)
7692 * (1.51009803921569)
7693 * pz))
7694 + (((-0.316735294117647)
7695 * x852))
7696 + (((-1.0)
7697 * x852
7698 * x853))
7699 + (((-0.316735294117647)
7700 * x851))
7701 + (((-1.0)
7702 * x851
7703 * x853))
7704 + (((-1.0)
7705 * x852
7706 * x854)))),
7707 -1);
7708 if (!x861.valid)
7709 {
7710 continue;
7711 }
7712 j6array[0]
7713 = ((-1.5707963267949)
7714 + (x860.value)
7715 + (((1.5707963267949)
7716 * (x861.value))));
7717 sj6array[0] = IKsin(
7718 j6array[0]);
7719 cj6array[0] = IKcos(
7720 j6array[0]);
7721 if (j6array[0]
7722 > IKPI)
7723 {
7724 j6array[0]
7725 -= IK2PI;
7726 }
7727 else if (
7728 j6array[0]
7729 < -IKPI)
7730 {
7731 j6array[0]
7732 += IK2PI;
7733 }
7734 j6valid[0] = true;
7735 for (int ij6 = 0;
7736 ij6 < 1;
7737 ++ij6)
7738 {
7739 if (!j6valid[ij6])
7740 {
7741 continue;
7742 }
7743 _ij6[0] = ij6;
7744 _ij6[1] = -1;
7745 for (int iij6
7746 = ij6 + 1;
7747 iij6 < 1;
7748 ++iij6)
7749 {
7750 if (j6valid
7751 [iij6]
7752 && IKabs(
7753 cj6array
7754 [ij6]
7755 - cj6array
7756 [iij6])
7757 < IKFAST_SOLUTION_THRESH
7758 && IKabs(
7759 sj6array
7760 [ij6]
7761 - sj6array
7762 [iij6])
7763 < IKFAST_SOLUTION_THRESH)
7764 {
7765 j6valid[iij6]
7766 = false;
7767 _ij6[1]
7768 = iij6;
7769 break;
7770 }
7771 }
7772 j6 = j6array[ij6];
7773 cj6 = cj6array
7774 [ij6];
7775 sj6 = sj6array
7776 [ij6];
7777 {
7778 IkReal
7779 evalcond[5];
7780 IkReal x862
7781 = ((0.3)
7782 * cj9);
7783 IkReal x863
7784 = ((0.045)
7785 * sj9);
7786 IkReal x864
7787 = IKcos(j6);
7788 IkReal x865
7789 = (pz
7790 * x864);
7791 IkReal x866
7792 = IKsin(j6);
7793 IkReal x867
7794 = (cj4
7795 * px);
7796 IkReal x868
7797 = (x866
7798 * x867);
7799 IkReal x869
7800 = (py
7801 * sj4);
7802 IkReal x870
7803 = (x866
7804 * x869);
7805 IkReal x871
7806 = ((0.045)
7807 * cj9);
7808 IkReal x872
7809 = ((0.3)
7810 * sj9);
7811 IkReal x873
7812 = (pz
7813 * x866);
7814 IkReal x874
7815 = (x864
7816 * x867);
7817 IkReal x875
7818 = (x864
7819 * x869);
7820 evalcond[0]
7821 = ((-0.55)
7822 + (((-1.0)
7823 * x862))
7824 + x868
7825 + x865
7826 + x870
7827 + (((-1.0)
7828 * x863)));
7829 evalcond[1]
7830 = ((0.045)
7831 + (((-1.0)
7832 * x873))
7833 + x872
7834 + x874
7835 + x875
7836 + (((-1.0)
7837 * x871)));
7838 evalcond[2]
7839 = ((((1.51009803921569)
7840 * x866))
7841 + (((-0.316735294117647)
7842 * x864))
7843 + pz
7844 + (((-3.92156862745098)
7845 * pp
7846 * x866))
7847 + (((-0.108264705882353)
7848 * cj9
7849 * x864))
7850 + (((-0.588235294117647)
7851 * pp
7852 * x864))
7853 + (((1.32323529411765)
7854 * cj9
7855 * x866)));
7856 evalcond[3]
7857 = ((((0.55)
7858 * x866))
7859 + (((-1.0)
7860 * x869))
7861 + (((-0.045)
7862 * x864))
7863 + (((-1.0)
7864 * x867))
7865 + (((-1.0)
7866 * x864
7867 * x872))
7868 + ((x862
7869 * x866))
7870 + ((x864
7871 * x871))
7872 + ((x863
7873 * x866)));
7874 evalcond[4]
7875 = ((-0.2125)
7876 + (((-0.09)
7877 * x875))
7878 + (((1.1)
7879 * x865))
7880 + (((0.09)
7881 * x873))
7882 + (((-0.09)
7883 * x874))
7884 + (((1.1)
7885 * x870))
7886 + (((1.1)
7887 * x868))
7888 + (((-1.0)
7889 * (1.0)
7890 * pp)));
7891 if (IKabs(
7892 evalcond
7893 [0])
7894 > IKFAST_EVALCOND_THRESH
7895 || IKabs(
7896 evalcond
7897 [1])
7898 > IKFAST_EVALCOND_THRESH
7899 || IKabs(
7900 evalcond
7901 [2])
7902 > IKFAST_EVALCOND_THRESH
7903 || IKabs(
7904 evalcond
7905 [3])
7906 > IKFAST_EVALCOND_THRESH
7907 || IKabs(
7908 evalcond
7909 [4])
7910 > IKFAST_EVALCOND_THRESH)
7911 {
7912 continue;
7913 }
7914 }
7915
7916 rotationfunction0(
7917 solutions);
7918 }
7919 }
7920 }
7921 }
7922 }
7923 else
7924 {
7925 {
7926 IkReal j6array[1],
7927 cj6array[1],
7928 sj6array[1];
7929 bool j6valid[1]
7930 = {false};
7931 _nj6 = 1;
7932 IkReal x876
7933 = ((0.045) * cj4
7934 * px);
7935 IkReal x877
7936 = ((0.045) * py
7937 * sj4);
7938 IkReal x878
7939 = ((0.3) * sj9);
7940 IkReal x879
7941 = (cj4 * px);
7942 IkReal x880
7943 = (py * sj4);
7944 IkReal x881
7945 = (cj9 * sj9);
7946 IkReal x882 = cj9 * cj9;
7947 IkReal x883
7948 = ((1.0) * pz);
7949 IkReal x884 = py * py;
7950 IkReal x885 = cj4 * cj4;
7951 CheckValue<IkReal> x886
7952 = IKatan2WithCheck(
7953 IkReal((
7954 (-0.03825)
7955 + (((0.027)
7956 * x882))
7957 + (((-0.087975)
7958 * x881))
7959 + (((-1.0)
7960 * (0.167025)
7961 * sj9))
7962 + (((-1.0)
7963 * x879
7964 * x883))
7965 + (((-1.0)
7966 * x880
7967 * x883))
7968 + (((0.01125)
7969 * cj9)))),
7970 ((-0.304525)
7971 + (((-1.0)
7972 * x884
7973 * x885))
7974 + (((2.0) * cj4
7975 * px
7976 * x880))
7977 + (((-1.0)
7978 * (0.0495)
7979 * sj9))
7980 + (((-0.087975)
7981 * x882))
7982 + ((x885
7983 * (px
7984 * px)))
7985 + x884
7986 + (((-1.0)
7987 * (0.33)
7988 * cj9))
7989 + (((-0.027)
7990 * x881))),
7991 IKFAST_ATAN2_MAGTHRESH);
7992 if (!x886.valid)
7993 {
7994 continue;
7995 }
7996 CheckValue<IkReal> x887
7997 = IKPowWithIntegerCheck(
7998 IKsign((
7999 (((-1.0)
8000 * x878
8001 * x879))
8002 + (((-1.0)
8003 * (0.55)
8004 * pz))
8005 + (((-1.0)
8006 * x877))
8007 + (((-1.0)
8008 * x876))
8009 + (((-1.0)
8010 * x878
8011 * x880))
8012 + ((cj9
8013 * x877))
8014 + ((cj9
8015 * x876))
8016 + (((-1.0)
8017 * (0.045)
8018 * pz
8019 * sj9))
8020 + (((-1.0)
8021 * (0.3)
8022 * cj9
8023 * pz)))),
8024 -1);
8025 if (!x887.valid)
8026 {
8027 continue;
8028 }
8029 j6array[0]
8030 = ((-1.5707963267949)
8031 + (x886.value)
8032 + (((1.5707963267949)
8033 * (x887.value))));
8034 sj6array[0]
8035 = IKsin(j6array[0]);
8036 cj6array[0]
8037 = IKcos(j6array[0]);
8038 if (j6array[0] > IKPI)
8039 {
8040 j6array[0] -= IK2PI;
8041 }
8042 else if (
8043 j6array[0] < -IKPI)
8044 {
8045 j6array[0] += IK2PI;
8046 }
8047 j6valid[0] = true;
8048 for (int ij6 = 0;
8049 ij6 < 1;
8050 ++ij6)
8051 {
8052 if (!j6valid[ij6])
8053 {
8054 continue;
8055 }
8056 _ij6[0] = ij6;
8057 _ij6[1] = -1;
8058 for (int iij6
8059 = ij6 + 1;
8060 iij6 < 1;
8061 ++iij6)
8062 {
8063 if (j6valid[iij6]
8064 && IKabs(
8065 cj6array
8066 [ij6]
8067 - cj6array
8068 [iij6])
8069 < IKFAST_SOLUTION_THRESH
8070 && IKabs(
8071 sj6array
8072 [ij6]
8073 - sj6array
8074 [iij6])
8075 < IKFAST_SOLUTION_THRESH)
8076 {
8077 j6valid[iij6]
8078 = false;
8079 _ij6[1] = iij6;
8080 break;
8081 }
8082 }
8083 j6 = j6array[ij6];
8084 cj6 = cj6array[ij6];
8085 sj6 = sj6array[ij6];
8086 {
8087 IkReal evalcond[5];
8088 IkReal x888
8089 = ((0.3) * cj9);
8090 IkReal x889
8091 = ((0.045)
8092 * sj9);
8093 IkReal x890
8094 = IKcos(j6);
8095 IkReal x891
8096 = (pz * x890);
8097 IkReal x892
8098 = IKsin(j6);
8099 IkReal x893
8100 = (cj4 * px);
8101 IkReal x894
8102 = (x892 * x893);
8103 IkReal x895
8104 = (py * sj4);
8105 IkReal x896
8106 = (x892 * x895);
8107 IkReal x897
8108 = ((0.045)
8109 * cj9);
8110 IkReal x898
8111 = ((0.3) * sj9);
8112 IkReal x899
8113 = (pz * x892);
8114 IkReal x900
8115 = (x890 * x893);
8116 IkReal x901
8117 = (x890 * x895);
8118 evalcond[0]
8119 = ((-0.55)
8120 + (((-1.0)
8121 * x888))
8122 + x894 + x896
8123 + x891
8124 + (((-1.0)
8125 * x889)));
8126 evalcond[1]
8127 = ((0.045)
8128 + (((-1.0)
8129 * x897))
8130 + x900 + x901
8131 + x898
8132 + (((-1.0)
8133 * x899)));
8134 evalcond[2]
8135 = ((((-0.588235294117647)
8136 * pp
8137 * x890))
8138 + (((-0.316735294117647)
8139 * x890))
8140 + (((1.32323529411765)
8141 * cj9
8142 * x892))
8143 + pz
8144 + (((-0.108264705882353)
8145 * cj9
8146 * x890))
8147 + (((-3.92156862745098)
8148 * pp
8149 * x892))
8150 + (((1.51009803921569)
8151 * x892)));
8152 evalcond[3]
8153 = ((((-1.0)
8154 * x893))
8155 + ((x890
8156 * x897))
8157 + (((0.55)
8158 * x892))
8159 + ((x888
8160 * x892))
8161 + (((-1.0)
8162 * x890
8163 * x898))
8164 + ((x889
8165 * x892))
8166 + (((-1.0)
8167 * x895))
8168 + (((-0.045)
8169 * x890)));
8170 evalcond[4]
8171 = ((-0.2125)
8172 + (((-0.09)
8173 * x900))
8174 + (((1.1)
8175 * x891))
8176 + (((0.09)
8177 * x899))
8178 + (((-0.09)
8179 * x901))
8180 + (((1.1)
8181 * x896))
8182 + (((-1.0)
8183 * (1.0)
8184 * pp))
8185 + (((1.1)
8186 * x894)));
8187 if (IKabs(
8188 evalcond[0])
8189 > IKFAST_EVALCOND_THRESH
8190 || IKabs(
8191 evalcond
8192 [1])
8193 > IKFAST_EVALCOND_THRESH
8194 || IKabs(
8195 evalcond
8196 [2])
8197 > IKFAST_EVALCOND_THRESH
8198 || IKabs(
8199 evalcond
8200 [3])
8201 > IKFAST_EVALCOND_THRESH
8202 || IKabs(
8203 evalcond
8204 [4])
8205 > IKFAST_EVALCOND_THRESH)
8206 {
8207 continue;
8208 }
8209 }
8210
8211 rotationfunction0(
8212 solutions);
8213 }
8214 }
8215 }
8216 }
8217 }
8218 else
8219 {
8220 {
8221 IkReal j6array[1],
8222 cj6array[1],
8223 sj6array[1];
8224 bool j6valid[1] = {false};
8225 _nj6 = 1;
8226 IkReal x902 = py * py;
8227 IkReal x903 = (py * sj4);
8228 IkReal x904 = cj4 * cj4;
8229 IkReal x905
8230 = ((0.045) * pz);
8231 IkReal x906 = (cj4 * px);
8232 IkReal x907 = ((0.3) * pz);
8233 IkReal x908
8234 = ((0.3) * cj4 * px);
8235 IkReal x909
8236 = ((0.045) * x906);
8237 IkReal x910
8238 = ((0.3) * py * sj4);
8239 IkReal x911
8240 = ((0.045) * x903);
8241 CheckValue<IkReal> x912
8242 = IKatan2WithCheck(
8243 IkReal((
8244 ((sj9 * x911))
8245 + ((sj9 * x909))
8246 + ((cj9 * x910))
8247 + ((sj9 * x907))
8248 + x905
8249 + (((0.55)
8250 * x906))
8251 + (((0.55)
8252 * x903))
8253 + ((cj9 * x908))
8254 + (((-1.0) * cj9
8255 * x905)))),
8256 ((((-1.0) * x911))
8257 + ((cj9 * x907))
8258 + ((cj9 * x911))
8259 + ((cj9 * x909))
8260 + (((-1.0) * sj9
8261 * x910))
8262 + (((0.55) * pz))
8263 + (((-1.0) * sj9
8264 * x908))
8265 + (((-1.0) * x909))
8266 + ((sj9 * x905))),
8267 IKFAST_ATAN2_MAGTHRESH);
8268 if (!x912.valid)
8269 {
8270 continue;
8271 }
8272 CheckValue<IkReal> x913
8273 = IKPowWithIntegerCheck(
8274 IKsign(
8275 ((((2.0) * cj4
8276 * px * x903))
8277 + (pz * pz)
8278 + x902
8279 + ((x904
8280 * (px
8281 * px)))
8282 + (((-1.0)
8283 * x902
8284 * x904)))),
8285 -1);
8286 if (!x913.valid)
8287 {
8288 continue;
8289 }
8290 j6array[0]
8291 = ((-1.5707963267949)
8292 + (x912.value)
8293 + (((1.5707963267949)
8294 * (x913.value))));
8295 sj6array[0]
8296 = IKsin(j6array[0]);
8297 cj6array[0]
8298 = IKcos(j6array[0]);
8299 if (j6array[0] > IKPI)
8300 {
8301 j6array[0] -= IK2PI;
8302 }
8303 else if (j6array[0] < -IKPI)
8304 {
8305 j6array[0] += IK2PI;
8306 }
8307 j6valid[0] = true;
8308 for (int ij6 = 0; ij6 < 1;
8309 ++ij6)
8310 {
8311 if (!j6valid[ij6])
8312 {
8313 continue;
8314 }
8315 _ij6[0] = ij6;
8316 _ij6[1] = -1;
8317 for (int iij6 = ij6 + 1;
8318 iij6 < 1;
8319 ++iij6)
8320 {
8321 if (j6valid[iij6]
8322 && IKabs(
8323 cj6array[ij6]
8324 - cj6array
8325 [iij6])
8326 < IKFAST_SOLUTION_THRESH
8327 && IKabs(
8328 sj6array[ij6]
8329 - sj6array
8330 [iij6])
8331 < IKFAST_SOLUTION_THRESH)
8332 {
8333 j6valid[iij6] = false;
8334 _ij6[1] = iij6;
8335 break;
8336 }
8337 }
8338 j6 = j6array[ij6];
8339 cj6 = cj6array[ij6];
8340 sj6 = sj6array[ij6];
8341 {
8342 IkReal evalcond[5];
8343 IkReal x914
8344 = ((0.3) * cj9);
8345 IkReal x915
8346 = ((0.045) * sj9);
8347 IkReal x916 = IKcos(j6);
8348 IkReal x917
8349 = (pz * x916);
8350 IkReal x918 = IKsin(j6);
8351 IkReal x919
8352 = (cj4 * px);
8353 IkReal x920
8354 = (x918 * x919);
8355 IkReal x921
8356 = (py * sj4);
8357 IkReal x922
8358 = (x918 * x921);
8359 IkReal x923
8360 = ((0.045) * cj9);
8361 IkReal x924
8362 = ((0.3) * sj9);
8363 IkReal x925
8364 = (pz * x918);
8365 IkReal x926
8366 = (x916 * x919);
8367 IkReal x927
8368 = (x916 * x921);
8369 evalcond[0]
8370 = ((-0.55)
8371 + (((-1.0)
8372 * x914))
8373 + x917
8374 + (((-1.0)
8375 * x915))
8376 + x922 + x920);
8377 evalcond[1]
8378 = ((0.045)
8379 + (((-1.0)
8380 * x925))
8381 + (((-1.0)
8382 * x923))
8383 + x927 + x924
8384 + x926);
8385 evalcond[2]
8386 = ((((-0.316735294117647)
8387 * x916))
8388 + (((-3.92156862745098)
8389 * pp * x918))
8390 + (((-0.588235294117647)
8391 * pp * x916))
8392 + pz
8393 + (((-0.108264705882353)
8394 * cj9
8395 * x916))
8396 + (((1.32323529411765)
8397 * cj9
8398 * x918))
8399 + (((1.51009803921569)
8400 * x918)));
8401 evalcond[3]
8402 = ((((0.55) * x918))
8403 + (((-0.045)
8404 * x916))
8405 + ((x914 * x918))
8406 + ((x915 * x918))
8407 + (((-1.0)
8408 * x921))
8409 + ((x916 * x923))
8410 + (((-1.0)
8411 * x919))
8412 + (((-1.0) * x916
8413 * x924)));
8414 evalcond[4]
8415 = ((-0.2125)
8416 + (((-0.09)
8417 * x927))
8418 + (((1.1)
8419 * x920))
8420 + (((0.09)
8421 * x925))
8422 + (((-0.09)
8423 * x926))
8424 + (((1.1)
8425 * x917))
8426 + (((1.1)
8427 * x922))
8428 + (((-1.0)
8429 * (1.0)
8430 * pp)));
8431 if (IKabs(evalcond[0])
8432 > IKFAST_EVALCOND_THRESH
8433 || IKabs(
8434 evalcond[1])
8435 > IKFAST_EVALCOND_THRESH
8436 || IKabs(
8437 evalcond[2])
8438 > IKFAST_EVALCOND_THRESH
8439 || IKabs(
8440 evalcond[3])
8441 > IKFAST_EVALCOND_THRESH
8442 || IKabs(
8443 evalcond[4])
8444 > IKFAST_EVALCOND_THRESH)
8445 {
8446 continue;
8447 }
8448 }
8449
8450 rotationfunction0(
8451 solutions);
8452 }
8453 }
8454 }
8455 }
8456 }
8457 } while (0);
8458 if (bgotonextstatement)
8459 {
8460 bool bgotonextstatement = true;
8461 do
8462 {
8463 if (1)
8464 {
8465 bgotonextstatement = false;
8466 continue; // branch miss [j6]
8467 }
8468 } while (0);
8469 if (bgotonextstatement)
8470 {
8471 }
8472 }
8473 }
8474 }
8475 }
8476 else
8477 {
8478 {
8479 IkReal j6array[1], cj6array[1],
8480 sj6array[1];
8481 bool j6valid[1] = {false};
8482 _nj6 = 1;
8483 IkReal x928 = (pz * sj8);
8484 IkReal x929 = ((0.3) * cj9);
8485 IkReal x930 = ((0.045) * sj9);
8486 IkReal x931 = (cj4 * px);
8487 IkReal x932 = ((0.045) * cj8 * sj8);
8488 IkReal x933 = (x931 * x932);
8489 IkReal x934 = (py * sj4);
8490 IkReal x935 = (x932 * x934);
8491 IkReal x936 = ((0.3) * cj8 * sj8 * sj9);
8492 IkReal x937 = ((0.55) * cj8);
8493 IkReal x938 = (cj4 * py);
8494 IkReal x939 = (px * sj4);
8495 IkReal x940 = (cj4 * cj8 * py);
8496 IkReal x941 = ((1.0) * pz * sj8);
8497 IkReal x942 = (cj8 * px * sj4);
8498 IkReal x943 = cj8 * cj8;
8499 IkReal x944 = ((0.045) * x943);
8500 IkReal x945 = (x938 * x944);
8501 IkReal x946 = (x939 * x944);
8502 IkReal x947 = ((0.3) * sj9 * x943);
8503 CheckValue<IkReal> x948
8504 = IKPowWithIntegerCheck(
8505 IKsign(
8506 ((((-0.55) * x928))
8507 + (((-1.0) * cj9 * x933))
8508 + (((-1.0) * cj9 * x935))
8509 + (((-1.0) * x928 * x929))
8510 + ((x931 * x936))
8511 + (((-1.0) * x928 * x930))
8512 + x933 + x935
8513 + ((x934 * x936)))),
8514 -1);
8515 if (!x948.valid)
8516 {
8517 continue;
8518 }
8519 CheckValue<IkReal> x949
8520 = IKatan2WithCheck(
8521 IkReal(
8522 ((((-1.0) * x937 * x939))
8523 + ((x937 * x938))
8524 + ((x930 * x940))
8525 + (((-1.0) * x930 * x942))
8526 + (((-1.0) * x931 * x941))
8527 + (((-1.0) * x934 * x941))
8528 + (((-1.0) * x929 * x942))
8529 + ((x929 * x940)))),
8530 (((cj9 * x946))
8531 + (((-1.0) * (1.0) * sj8
8532 * (pz * pz)))
8533 + ((x938 * x947))
8534 + (((-1.0) * x939 * x947))
8535 + (((-1.0) * cj9 * x945))
8536 + (((-1.0) * x946)) + x945),
8537 IKFAST_ATAN2_MAGTHRESH);
8538 if (!x949.valid)
8539 {
8540 continue;
8541 }
8542 j6array[0]
8543 = ((-1.5707963267949)
8544 + (((1.5707963267949)
8545 * (x948.value)))
8546 + (x949.value));
8547 sj6array[0] = IKsin(j6array[0]);
8548 cj6array[0] = IKcos(j6array[0]);
8549 if (j6array[0] > IKPI)
8550 {
8551 j6array[0] -= IK2PI;
8552 }
8553 else if (j6array[0] < -IKPI)
8554 {
8555 j6array[0] += IK2PI;
8556 }
8557 j6valid[0] = true;
8558 for (int ij6 = 0; ij6 < 1; ++ij6)
8559 {
8560 if (!j6valid[ij6])
8561 {
8562 continue;
8563 }
8564 _ij6[0] = ij6;
8565 _ij6[1] = -1;
8566 for (int iij6 = ij6 + 1; iij6 < 1;
8567 ++iij6)
8568 {
8569 if (j6valid[iij6]
8570 && IKabs(
8571 cj6array[ij6]
8572 - cj6array[iij6])
8573 < IKFAST_SOLUTION_THRESH
8574 && IKabs(
8575 sj6array[ij6]
8576 - sj6array[iij6])
8577 < IKFAST_SOLUTION_THRESH)
8578 {
8579 j6valid[iij6] = false;
8580 _ij6[1] = iij6;
8581 break;
8582 }
8583 }
8584 j6 = j6array[ij6];
8585 cj6 = cj6array[ij6];
8586 sj6 = sj6array[ij6];
8587 {
8588 IkReal evalcond[6];
8589 IkReal x950 = ((0.3) * cj9);
8590 IkReal x951 = ((0.045) * sj9);
8591 IkReal x952 = IKcos(j6);
8592 IkReal x953 = (pz * x952);
8593 IkReal x954 = IKsin(j6);
8594 IkReal x955 = (cj4 * px * x954);
8595 IkReal x956 = (py * sj4);
8596 IkReal x957 = (x954 * x956);
8597 IkReal x958 = (px * sj4);
8598 IkReal x959 = ((1.0) * cj4 * py);
8599 IkReal x960 = (cj4 * sj8);
8600 IkReal x961 = ((0.045) * cj8);
8601 IkReal x962 = ((0.045) * cj9);
8602 IkReal x963 = (cj8 * x954);
8603 IkReal x964 = ((0.3) * sj9);
8604 IkReal x965 = (sj8 * x958);
8605 IkReal x966 = (pz * x963);
8606 IkReal x967
8607 = (px * (((1.0) * cj4)));
8608 IkReal x968 = (cj8 * x952);
8609 IkReal x969 = ((1.0) * x956);
8610 IkReal x970 = ((0.09) * cj8 * x952);
8611 evalcond[0]
8612 = ((-0.55) + x953 + x955 + x957
8613 + (((-1.0) * x951))
8614 + (((-1.0) * x950)));
8615 evalcond[1]
8616 = ((((-1.0) * cj8 * x959))
8617 + ((cj8 * x958))
8618 + (((-1.0) * pz * sj8
8619 * x954))
8620 + ((sj8 * x952 * x956))
8621 + ((px * x952 * x960)));
8622 evalcond[2]
8623 = ((((-0.55) * x952)) + pz
8624 + (((-1.0) * x962 * x963))
8625 + (((-1.0) * x951 * x952))
8626 + ((x954 * x961))
8627 + ((x963 * x964))
8628 + (((-1.0) * x950 * x952)));
8629 evalcond[3]
8630 = ((0.045)
8631 + (((-1.0) * x968 * x969))
8632 + (((-1.0) * sj8 * x959))
8633 + x966 + x964 + x965
8634 + (((-1.0) * x962))
8635 + (((-1.0) * x967 * x968)));
8636 evalcond[4]
8637 = (((x952 * x961))
8638 + ((x964 * x968))
8639 + (((0.55) * x954))
8640 + (((-1.0) * x962 * x968))
8641 + (((-1.0) * x969))
8642 + ((x951 * x954))
8643 + ((x950 * x954))
8644 + (((-1.0) * x967)));
8645 evalcond[5]
8646 = ((-0.2125) + (((1.1) * x957))
8647 + (((1.1) * x953))
8648 + (((-0.09) * x965))
8649 + (((1.1) * x955))
8650 + (((0.09) * py * x960))
8651 + ((cj4 * px * x970))
8652 + ((x956 * x970))
8653 + (((-0.09) * x966))
8654 + (((-1.0) * (1.0) * pp)));
8655 if (IKabs(evalcond[0])
8656 > IKFAST_EVALCOND_THRESH
8657 || IKabs(evalcond[1])
8658 > IKFAST_EVALCOND_THRESH
8659 || IKabs(evalcond[2])
8660 > IKFAST_EVALCOND_THRESH
8661 || IKabs(evalcond[3])
8662 > IKFAST_EVALCOND_THRESH
8663 || IKabs(evalcond[4])
8664 > IKFAST_EVALCOND_THRESH
8665 || IKabs(evalcond[5])
8666 > IKFAST_EVALCOND_THRESH)
8667 {
8668 continue;
8669 }
8670 }
8671
8672 rotationfunction0(solutions);
8673 }
8674 }
8675 }
8676 }
8677 }
8678 else
8679 {
8680 {
8681 IkReal j6array[1], cj6array[1], sj6array[1];
8682 bool j6valid[1] = {false};
8683 _nj6 = 1;
8684 IkReal x971 = py * py;
8685 IkReal x972 = (sj8 * x971);
8686 IkReal x973 = (cj4 * px * sj8);
8687 IkReal x974 = cj4 * cj4;
8688 IkReal x975 = px * px;
8689 IkReal x976 = ((0.55) * sj8);
8690 IkReal x977 = (cj8 * px);
8691 IkReal x978 = ((0.3) * cj9);
8692 IkReal x979 = ((0.045) * sj9);
8693 IkReal x980 = (py * sj4 * sj8);
8694 IkReal x981 = (pz * sj8);
8695 IkReal x982 = (cj4 * cj8 * sj4);
8696 CheckValue<IkReal> x983 = IKatan2WithCheck(
8697 IkReal(
8698 (((x973 * x978)) + ((x973 * x979))
8699 + ((pz * sj4 * x977))
8700 + (((-1.0) * cj4 * cj8 * py * pz))
8701 + ((cj4 * px * x976))
8702 + ((x979 * x980)) + ((x978 * x980))
8703 + ((py * sj4 * x976)))),
8704 (((pz * x976))
8705 + (((2.0) * py * x974 * x977))
8706 + ((x979 * x981)) + ((x971 * x982))
8707 + (((-1.0) * py * x977))
8708 + ((x978 * x981))
8709 + (((-1.0) * x975 * x982))),
8710 IKFAST_ATAN2_MAGTHRESH);
8711 if (!x983.valid)
8712 {
8713 continue;
8714 }
8715 CheckValue<IkReal> x984
8716 = IKPowWithIntegerCheck(
8717 IKsign(
8718 ((((2.0) * py * sj4 * x973))
8719 + ((sj8 * (pz * pz))) + x972
8720 + (((-1.0) * x972 * x974))
8721 + ((sj8 * x974 * x975)))),
8722 -1);
8723 if (!x984.valid)
8724 {
8725 continue;
8726 }
8727 j6array[0]
8728 = ((-1.5707963267949) + (x983.value)
8729 + (((1.5707963267949)
8730 * (x984.value))));
8731 sj6array[0] = IKsin(j6array[0]);
8732 cj6array[0] = IKcos(j6array[0]);
8733 if (j6array[0] > IKPI)
8734 {
8735 j6array[0] -= IK2PI;
8736 }
8737 else if (j6array[0] < -IKPI)
8738 {
8739 j6array[0] += IK2PI;
8740 }
8741 j6valid[0] = true;
8742 for (int ij6 = 0; ij6 < 1; ++ij6)
8743 {
8744 if (!j6valid[ij6])
8745 {
8746 continue;
8747 }
8748 _ij6[0] = ij6;
8749 _ij6[1] = -1;
8750 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
8751 {
8752 if (j6valid[iij6]
8753 && IKabs(
8754 cj6array[ij6]
8755 - cj6array[iij6])
8756 < IKFAST_SOLUTION_THRESH
8757 && IKabs(
8758 sj6array[ij6]
8759 - sj6array[iij6])
8760 < IKFAST_SOLUTION_THRESH)
8761 {
8762 j6valid[iij6] = false;
8763 _ij6[1] = iij6;
8764 break;
8765 }
8766 }
8767 j6 = j6array[ij6];
8768 cj6 = cj6array[ij6];
8769 sj6 = sj6array[ij6];
8770 {
8771 IkReal evalcond[6];
8772 IkReal x985 = ((0.3) * cj9);
8773 IkReal x986 = ((0.045) * sj9);
8774 IkReal x987 = IKcos(j6);
8775 IkReal x988 = (pz * x987);
8776 IkReal x989 = IKsin(j6);
8777 IkReal x990 = (cj4 * px * x989);
8778 IkReal x991 = (py * sj4);
8779 IkReal x992 = (x989 * x991);
8780 IkReal x993 = (px * sj4);
8781 IkReal x994 = ((1.0) * cj4 * py);
8782 IkReal x995 = (cj4 * sj8);
8783 IkReal x996 = ((0.045) * cj8);
8784 IkReal x997 = ((0.045) * cj9);
8785 IkReal x998 = (cj8 * x989);
8786 IkReal x999 = ((0.3) * sj9);
8787 IkReal x1000 = (sj8 * x993);
8788 IkReal x1001 = (pz * x998);
8789 IkReal x1002 = (px * (((1.0) * cj4)));
8790 IkReal x1003 = (cj8 * x987);
8791 IkReal x1004 = ((1.0) * x991);
8792 IkReal x1005 = ((0.09) * cj8 * x987);
8793 evalcond[0]
8794 = ((-0.55) + (((-1.0) * x985))
8795 + (((-1.0) * x986)) + x988 + x990
8796 + x992);
8797 evalcond[1]
8798 = ((((-1.0) * cj8 * x994))
8799 + (((-1.0) * pz * sj8 * x989))
8800 + ((px * x987 * x995))
8801 + ((sj8 * x987 * x991))
8802 + ((cj8 * x993)));
8803 evalcond[2]
8804 = ((((-1.0) * x986 * x987)) + pz
8805 + (((-0.55) * x987))
8806 + (((-1.0) * x997 * x998))
8807 + ((x998 * x999))
8808 + (((-1.0) * x985 * x987))
8809 + ((x989 * x996)));
8810 evalcond[3]
8811 = ((0.045) + (((-1.0) * sj8 * x994))
8812 + (((-1.0) * x997)) + x1000
8813 + x1001
8814 + (((-1.0) * x1002 * x1003))
8815 + (((-1.0) * x1003 * x1004))
8816 + x999);
8817 evalcond[4]
8818 = ((((-1.0) * x1003 * x997))
8819 + ((x985 * x989))
8820 + ((x1003 * x999))
8821 + (((0.55) * x989))
8822 + (((-1.0) * x1004))
8823 + (((-1.0) * x1002))
8824 + ((x986 * x989))
8825 + ((x987 * x996)));
8826 evalcond[5]
8827 = ((-0.2125) + (((1.1) * x990))
8828 + ((cj4 * px * x1005))
8829 + ((x1005 * x991))
8830 + (((-0.09) * x1001))
8831 + (((0.09) * py * x995))
8832 + (((-0.09) * x1000))
8833 + (((-1.0) * (1.0) * pp))
8834 + (((1.1) * x988))
8835 + (((1.1) * x992)));
8836 if (IKabs(evalcond[0])
8837 > IKFAST_EVALCOND_THRESH
8838 || IKabs(evalcond[1])
8839 > IKFAST_EVALCOND_THRESH
8840 || IKabs(evalcond[2])
8841 > IKFAST_EVALCOND_THRESH
8842 || IKabs(evalcond[3])
8843 > IKFAST_EVALCOND_THRESH
8844 || IKabs(evalcond[4])
8845 > IKFAST_EVALCOND_THRESH
8846 || IKabs(evalcond[5])
8847 > IKFAST_EVALCOND_THRESH)
8848 {
8849 continue;
8850 }
8851 }
8852
8853 rotationfunction0(solutions);
8854 }
8855 }
8856 }
8857 }
8858 }
8859 else
8860 {
8861 {
8862 IkReal j6array[1], cj6array[1], sj6array[1];
8863 bool j6valid[1] = {false};
8864 _nj6 = 1;
8865 IkReal x1006 = (cj4 * px);
8866 IkReal x1007 = ((0.045) * pz);
8867 IkReal x1008 = (py * sj4);
8868 IkReal x1009 = ((0.3) * cj9);
8869 IkReal x1010 = ((0.045) * sj9);
8870 IkReal x1011 = (cj8 * cj9);
8871 IkReal x1012 = (cj8 * sj9);
8872 IkReal x1013 = cj9 * cj9;
8873 IkReal x1014 = ((1.0) * pz);
8874 CheckValue<IkReal> x1015 = IKatan2WithCheck(
8875 IkReal(
8876 ((-0.304525)
8877 + (((-1.0) * (0.027) * cj9 * sj9))
8878 + (((-1.0) * (0.0495) * sj9))
8879 + (pz * pz) + (((-0.087975) * x1013))
8880 + (((-1.0) * (0.33) * cj9)))),
8881 ((((-0.087975) * sj9 * x1011))
8882 + (((-1.0) * x1008 * x1014))
8883 + (((-1.0) * x1006 * x1014))
8884 + (((0.027) * cj8 * x1013))
8885 + (((0.01125) * x1011))
8886 + (((-0.167025) * x1012))
8887 + (((-1.0) * (0.03825) * cj8))),
8888 IKFAST_ATAN2_MAGTHRESH);
8889 if (!x1015.valid)
8890 {
8891 continue;
8892 }
8893 CheckValue<IkReal> x1016
8894 = IKPowWithIntegerCheck(
8895 IKsign(
8896 ((((-1.0) * cj8 * x1007))
8897 + ((x1007 * x1011))
8898 + (((-0.3) * pz * x1012))
8899 + (((-0.55) * x1008))
8900 + (((-1.0) * x1008 * x1010))
8901 + (((-0.55) * x1006))
8902 + (((-1.0) * x1008 * x1009))
8903 + (((-1.0) * x1006 * x1009))
8904 + (((-1.0) * x1006 * x1010)))),
8905 -1);
8906 if (!x1016.valid)
8907 {
8908 continue;
8909 }
8910 j6array[0]
8911 = ((-1.5707963267949) + (x1015.value)
8912 + (((1.5707963267949) * (x1016.value))));
8913 sj6array[0] = IKsin(j6array[0]);
8914 cj6array[0] = IKcos(j6array[0]);
8915 if (j6array[0] > IKPI)
8916 {
8917 j6array[0] -= IK2PI;
8918 }
8919 else if (j6array[0] < -IKPI)
8920 {
8921 j6array[0] += IK2PI;
8922 }
8923 j6valid[0] = true;
8924 for (int ij6 = 0; ij6 < 1; ++ij6)
8925 {
8926 if (!j6valid[ij6])
8927 {
8928 continue;
8929 }
8930 _ij6[0] = ij6;
8931 _ij6[1] = -1;
8932 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
8933 {
8934 if (j6valid[iij6]
8935 && IKabs(cj6array[ij6] - cj6array[iij6])
8936 < IKFAST_SOLUTION_THRESH
8937 && IKabs(sj6array[ij6] - sj6array[iij6])
8938 < IKFAST_SOLUTION_THRESH)
8939 {
8940 j6valid[iij6] = false;
8941 _ij6[1] = iij6;
8942 break;
8943 }
8944 }
8945 j6 = j6array[ij6];
8946 cj6 = cj6array[ij6];
8947 sj6 = sj6array[ij6];
8948 {
8949 IkReal evalcond[6];
8950 IkReal x1017 = ((0.3) * cj9);
8951 IkReal x1018 = ((0.045) * sj9);
8952 IkReal x1019 = IKcos(j6);
8953 IkReal x1020 = (pz * x1019);
8954 IkReal x1021 = IKsin(j6);
8955 IkReal x1022 = (cj4 * px * x1021);
8956 IkReal x1023 = (py * sj4);
8957 IkReal x1024 = (x1021 * x1023);
8958 IkReal x1025 = (px * sj4);
8959 IkReal x1026 = ((1.0) * cj4 * py);
8960 IkReal x1027 = (cj4 * sj8);
8961 IkReal x1028 = ((0.045) * cj8);
8962 IkReal x1029 = ((0.045) * cj9);
8963 IkReal x1030 = (cj8 * x1021);
8964 IkReal x1031 = ((0.3) * sj9);
8965 IkReal x1032 = (sj8 * x1025);
8966 IkReal x1033 = (pz * x1030);
8967 IkReal x1034 = (px * (((1.0) * cj4)));
8968 IkReal x1035 = (cj8 * x1019);
8969 IkReal x1036 = ((1.0) * x1023);
8970 IkReal x1037 = ((0.09) * cj8 * x1019);
8971 evalcond[0]
8972 = ((-0.55) + x1020 + x1022 + x1024
8973 + (((-1.0) * x1018))
8974 + (((-1.0) * x1017)));
8975 evalcond[1]
8976 = ((((-1.0) * cj8 * x1026))
8977 + (((-1.0) * pz * sj8 * x1021))
8978 + ((px * x1019 * x1027))
8979 + ((cj8 * x1025))
8980 + ((sj8 * x1019 * x1023)));
8981 evalcond[2]
8982 = ((((-1.0) * x1018 * x1019))
8983 + (((-1.0) * x1029 * x1030))
8984 + ((x1030 * x1031))
8985 + ((x1021 * x1028))
8986 + (((-0.55) * x1019)) + pz
8987 + (((-1.0) * x1017 * x1019)));
8988 evalcond[3]
8989 = ((0.045) + (((-1.0) * x1029))
8990 + (((-1.0) * x1035 * x1036))
8991 + (((-1.0) * x1034 * x1035)) + x1031
8992 + x1033 + x1032
8993 + (((-1.0) * sj8 * x1026)));
8994 evalcond[4]
8995 = (((x1031 * x1035))
8996 + (((0.55) * x1021))
8997 + (((-1.0) * x1036))
8998 + ((x1019 * x1028))
8999 + ((x1017 * x1021))
9000 + ((x1018 * x1021))
9001 + (((-1.0) * x1029 * x1035))
9002 + (((-1.0) * x1034)));
9003 evalcond[5]
9004 = ((-0.2125) + ((cj4 * px * x1037))
9005 + ((x1023 * x1037))
9006 + (((1.1) * x1024))
9007 + (((-0.09) * x1032))
9008 + (((0.09) * py * x1027))
9009 + (((1.1) * x1020))
9010 + (((-1.0) * (1.0) * pp))
9011 + (((1.1) * x1022))
9012 + (((-0.09) * x1033)));
9013 if (IKabs(evalcond[0])
9014 > IKFAST_EVALCOND_THRESH
9015 || IKabs(evalcond[1])
9016 > IKFAST_EVALCOND_THRESH
9017 || IKabs(evalcond[2])
9018 > IKFAST_EVALCOND_THRESH
9019 || IKabs(evalcond[3])
9020 > IKFAST_EVALCOND_THRESH
9021 || IKabs(evalcond[4])
9022 > IKFAST_EVALCOND_THRESH
9023 || IKabs(evalcond[5])
9024 > IKFAST_EVALCOND_THRESH)
9025 {
9026 continue;
9027 }
9028 }
9029
9030 rotationfunction0(solutions);
9031 }
9032 }
9033 }
9034 }
9035 }
9036 }
9037 }
9038 }
9039 }
9040 else
9041 {
9042 {
9043 IkReal j6array[2], cj6array[2], sj6array[2];
9044 bool j6valid[2] = {false};
9045 _nj6 = 2;
9046 IkReal x1038
9047 = ((-0.55) + (((-1.0) * (0.3) * cj9))
9048 + (((-1.0) * (0.045) * sj9)));
9049 IkReal x1039 = ((0.045) * cj8);
9050 IkReal x1040
9051 = ((((-1.0) * cj9 * x1039)) + x1039
9052 + (((0.3) * cj8 * sj9)));
9053 CheckValue<IkReal> x1043 = IKatan2WithCheck(
9054 IkReal(x1038), x1040, IKFAST_ATAN2_MAGTHRESH);
9055 if (!x1043.valid)
9056 {
9057 continue;
9058 }
9059 IkReal x1041 = ((-1.0) * (x1043.value));
9060 if ((((x1040 * x1040) + (x1038 * x1038))) < -0.00001)
9061 continue;
9062 CheckValue<IkReal> x1044 = IKPowWithIntegerCheck(
9063 IKabs(IKsqrt(((x1040 * x1040) + (x1038 * x1038)))), -1);
9064 if (!x1044.valid)
9065 {
9066 continue;
9067 }
9068 if (((pz * (x1044.value))) < -1 - IKFAST_SINCOS_THRESH
9069 || ((pz * (x1044.value))) > 1 + IKFAST_SINCOS_THRESH)
9070 continue;
9071 IkReal x1042 = IKasin((pz * (x1044.value)));
9072 j6array[0] = ((((-1.0) * x1042)) + x1041);
9073 sj6array[0] = IKsin(j6array[0]);
9074 cj6array[0] = IKcos(j6array[0]);
9075 j6array[1] = ((3.14159265358979) + x1042 + x1041);
9076 sj6array[1] = IKsin(j6array[1]);
9077 cj6array[1] = IKcos(j6array[1]);
9078 if (j6array[0] > IKPI)
9079 {
9080 j6array[0] -= IK2PI;
9081 }
9082 else if (j6array[0] < -IKPI)
9083 {
9084 j6array[0] += IK2PI;
9085 }
9086 j6valid[0] = true;
9087 if (j6array[1] > IKPI)
9088 {
9089 j6array[1] -= IK2PI;
9090 }
9091 else if (j6array[1] < -IKPI)
9092 {
9093 j6array[1] += IK2PI;
9094 }
9095 j6valid[1] = true;
9096 for (int ij6 = 0; ij6 < 2; ++ij6)
9097 {
9098 if (!j6valid[ij6])
9099 {
9100 continue;
9101 }
9102 _ij6[0] = ij6;
9103 _ij6[1] = -1;
9104 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
9105 {
9106 if (j6valid[iij6]
9107 && IKabs(cj6array[ij6] - cj6array[iij6])
9108 < IKFAST_SOLUTION_THRESH
9109 && IKabs(sj6array[ij6] - sj6array[iij6])
9110 < IKFAST_SOLUTION_THRESH)
9111 {
9112 j6valid[iij6] = false;
9113 _ij6[1] = iij6;
9114 break;
9115 }
9116 }
9117 j6 = j6array[ij6];
9118 cj6 = cj6array[ij6];
9119 sj6 = sj6array[ij6];
9120
9121 {
9122 IkReal j4eval[2];
9123 IkReal x1045
9124 = ((((-1.0) * (1.0) * sj6 * (pz * pz)))
9125 + ((pp * sj6)));
9126 j4eval[0] = x1045;
9127 j4eval[1] = IKsign(x1045);
9128 if (IKabs(j4eval[0]) < 0.0000010000000000
9129 || IKabs(j4eval[1]) < 0.0000010000000000)
9130 {
9131 {
9132 IkReal j4eval[2];
9133 IkReal x1046 = (cj8 * sj6);
9134 IkReal x1047
9135 = (((x1046 * (pz * pz)))
9136 + (((-1.0) * pp * x1046)));
9137 j4eval[0] = x1047;
9138 j4eval[1] = IKsign(x1047);
9139 if (IKabs(j4eval[0]) < 0.0000010000000000
9140 || IKabs(j4eval[1]) < 0.0000010000000000)
9141 {
9142 {
9143 IkReal j4eval[2];
9144 IkReal x1048
9145 = (pp + (((-1.0) * (1.0) * (pz * pz))));
9146 j4eval[0] = x1048;
9147 j4eval[1] = IKsign(x1048);
9148 if (IKabs(j4eval[0]) < 0.0000010000000000
9149 || IKabs(j4eval[1]) < 0.0000010000000000)
9150 {
9151 {
9152 IkReal evalcond[4];
9153 bool bgotonextstatement = true;
9154 do
9155 {
9156 evalcond[0]
9157 = ((-3.14159265358979)
9158 + (IKfmod(
9159 ((3.14159265358979)
9160 + (IKabs(
9161 ((-1.5707963267949)
9162 + j8)))),
9163 6.28318530717959)));
9164 evalcond[1]
9165 = ((0.39655) + (((0.0765) * sj9))
9166 + (((0.32595) * cj9))
9167 + (((-1.0) * (1.0) * pp)));
9168 evalcond[2]
9169 = ((((-1.0) * (0.3) * cj6 * cj9))
9170 + (((-1.0) * (0.55) * cj6)) + pz
9171 + (((-1.0) * (0.045) * cj6
9172 * sj9)));
9173 if (IKabs(evalcond[0])
9174 < 0.0000010000000000
9175 && IKabs(evalcond[1])
9176 < 0.0000010000000000
9177 && IKabs(evalcond[2])
9178 < 0.0000010000000000)
9179 {
9180 bgotonextstatement = false;
9181 {
9182 IkReal j4eval[3];
9183 sj8 = 1.0;
9184 cj8 = 0;
9185 j8 = 1.5707963267949;
9186 IkReal x1049
9187 = ((((-1.0) * (1.0) * cj6
9188 * (pz * pz)))
9189 + ((cj6 * pp)));
9190 IkReal x1050
9191 = ((1.51009803921569) * cj6);
9192 IkReal x1051 = (pz * sj6);
9193 IkReal x1052
9194 = ((1.32323529411765) * cj6
9195 * cj9);
9196 IkReal x1053
9197 = ((3.92156862745098) * cj6 * pp);
9198 j4eval[0] = x1049;
9199 j4eval[1] = IKsign(x1049);
9200 j4eval[2]
9201 = ((IKabs(
9202 (((py * x1053))
9203 + (((-1.0) * py * x1052))
9204 + (((-1.0) * py * x1050))
9205 + ((px * x1051)))))
9206 + (IKabs(
9207 (((px * x1052))
9208 + ((px * x1050))
9209 + ((py * x1051))
9210 + (((-1.0) * px
9211 * x1053))))));
9212 if (IKabs(j4eval[0])
9213 < 0.0000010000000000
9214 || IKabs(j4eval[1])
9215 < 0.0000010000000000
9216 || IKabs(j4eval[2])
9217 < 0.0000010000000000)
9218 {
9219 {
9220 IkReal j4eval[3];
9221 sj8 = 1.0;
9222 cj8 = 0;
9223 j8 = 1.5707963267949;
9224 IkReal x1054 = (cj6 * pp);
9225 IkReal x1055 = (cj6 * (pz * pz));
9226 IkReal x1056 = ((0.2125) * cj6);
9227 IkReal x1057 = ((1.1) * pz);
9228 IkReal x1058
9229 = ((0.09) * pz * sj6);
9230 j4eval[0]
9231 = ((((-1.0) * x1055))
9232 + x1054);
9233 j4eval[1] = IKsign(
9234 ((((-0.09) * x1055))
9235 + (((0.09) * x1054))));
9236 j4eval[2]
9237 = ((IKabs(
9238 (((py * x1058))
9239 + ((px * x1057))
9240 + (((-1.0) * px
9241 * x1054))
9242 + (((-1.0) * px
9243 * x1056)))))
9244 + (IKabs((
9245 ((py * x1056))
9246 + ((py * x1054))
9247 + (((-1.0) * py
9248 * x1057))
9249 + ((px * x1058))))));
9250 if (IKabs(j4eval[0])
9251 < 0.0000010000000000
9252 || IKabs(j4eval[1])
9253 < 0.0000010000000000
9254 || IKabs(j4eval[2])
9255 < 0.0000010000000000)
9256 {
9257 {
9258 IkReal j4eval[3];
9259 sj8 = 1.0;
9260 cj8 = 0;
9261 j8 = 1.5707963267949;
9262 IkReal x1059
9263 = (pp
9264 + (((-1.0) * (1.0)
9265 * (pz * pz))));
9266 IkReal x1060
9267 = ((1.32323529411765)
9268 * cj9);
9269 IkReal x1061
9270 = ((3.92156862745098)
9271 * pp);
9272 IkReal x1062
9273 = ((0.316735294117647)
9274 * sj6);
9275 IkReal x1063
9276 = ((0.108264705882353)
9277 * cj9 * sj6);
9278 IkReal x1064
9279 = ((0.588235294117647)
9280 * pp * sj6);
9281 j4eval[0] = x1059;
9282 j4eval[1]
9283 = ((IKabs((
9284 (((-1.0)
9285 * (1.51009803921569)
9286 * py))
9287 + ((px * x1064))
9288 + (((-1.0) * py
9289 * x1060))
9290 + ((px * x1063))
9291 + ((py * x1061))
9292 + ((px * x1062)))))
9293 + (IKabs((
9294 ((px * x1060))
9295 + (((-1.0) * px
9296 * x1061))
9297 + (((1.51009803921569)
9298 * px))
9299 + ((py * x1063))
9300 + ((py * x1062))
9301 + ((py
9302 * x1064))))));
9303 j4eval[2] = IKsign(x1059);
9304 if (IKabs(j4eval[0])
9305 < 0.0000010000000000
9306 || IKabs(j4eval[1])
9307 < 0.0000010000000000
9308 || IKabs(j4eval[2])
9309 < 0.0000010000000000)
9310 {
9311 {
9312 IkReal evalcond[7];
9313 bool bgotonextstatement
9314 = true;
9315 do
9316 {
9317 evalcond[0]
9318 = ((-3.14159265358979)
9319 + (IKfmod(
9320 ((3.14159265358979)
9321 + (IKabs((
9322 (-1.5707963267949)
9323 + j6)))),
9324 6.28318530717959)));
9325 evalcond[1]
9326 = ((-1.0)
9327 * (((1.0)
9328 * pz)));
9329 if (IKabs(evalcond[0])
9330 < 0.0000010000000000
9331 && IKabs(
9332 evalcond[1])
9333 < 0.0000010000000000)
9334 {
9335 bgotonextstatement
9336 = false;
9337 {
9338 IkReal j4eval[3];
9339 sj8 = 1.0;
9340 cj8 = 0;
9341 j8 = 1.5707963267949;
9342 sj6 = 1.0;
9343 cj6 = 0;
9344 j6 = 1.5707963267949;
9345 IkReal x1065
9346 = (pp
9347 + (((-1.0)
9348 * (1.0)
9349 * (pz
9350 * pz))));
9351 IkReal x1066
9352 = (cj9 * px);
9353 IkReal x1067
9354 = (cj9 * py);
9355 IkReal x1068
9356 = ((3.92156862745098)
9357 * pp);
9358 IkReal x1069
9359 = ((0.045)
9360 * sj9);
9361 j4eval[0] = x1065;
9362 j4eval[1]
9363 = ((IKabs((
9364 (((1.51009803921569)
9365 * px))
9366 + ((py
9367 * x1069))
9368 + (((0.55)
9369 * py))
9370 + (((0.3)
9371 * x1067))
9372 + (((1.32323529411765)
9373 * x1066))
9374 + (((-1.0)
9375 * px
9376 * x1068)))))
9377 + (IKabs((
9378 (((0.3)
9379 * x1066))
9380 + (((-1.0)
9381 * (1.51009803921569)
9382 * py))
9383 + (((-1.32323529411765)
9384 * x1067))
9385 + (((0.55)
9386 * px))
9387 + ((py
9388 * x1068))
9389 + ((px
9390 * x1069))))));
9391 j4eval[2]
9392 = IKsign(x1065);
9393 if (IKabs(j4eval[0])
9394 < 0.0000010000000000
9395 || IKabs(
9396 j4eval
9397 [1])
9398 < 0.0000010000000000
9399 || IKabs(
9400 j4eval
9401 [2])
9402 < 0.0000010000000000)
9403 {
9404 {
9405 IkReal
9406 j4eval[3];
9407 sj8 = 1.0;
9408 cj8 = 0;
9409 j8 = 1.5707963267949;
9410 sj6 = 1.0;
9411 cj6 = 0;
9412 j6 = 1.5707963267949;
9413 IkReal x1070
9414 = (pp
9415 + (((-1.0)
9416 * (1.0)
9417 * (pz
9418 * pz))));
9419 IkReal x1071
9420 = (cj9
9421 * px);
9422 IkReal x1072
9423 = (cj9
9424 * py);
9425 IkReal x1073
9426 = (pp * px);
9427 IkReal x1074
9428 = (pp * py);
9429 j4eval[0]
9430 = x1070;
9431 j4eval[1]
9432 = ((IKabs((
9433 (((0.108264705882353)
9434 * x1072))
9435 + (((0.588235294117647)
9436 * x1074))
9437 + (((1.32323529411765)
9438 * x1071))
9439 + (((0.316735294117647)
9440 * py))
9441 + (((1.51009803921569)
9442 * px))
9443 + (((-3.92156862745098)
9444 * x1073)))))
9445 + (IKabs((
9446 (((-1.32323529411765)
9447 * x1072))
9448 + (((0.588235294117647)
9449 * x1073))
9450 + (((3.92156862745098)
9451 * x1074))
9452 + (((0.316735294117647)
9453 * px))
9454 + (((0.108264705882353)
9455 * x1071))
9456 + (((-1.0)
9457 * (1.51009803921569)
9458 * py))))));
9459 j4eval[2]
9460 = IKsign(
9461 x1070);
9462 if (IKabs(
9463 j4eval
9464 [0])
9465 < 0.0000010000000000
9466 || IKabs(
9467 j4eval
9468 [1])
9469 < 0.0000010000000000
9470 || IKabs(
9471 j4eval
9472 [2])
9473 < 0.0000010000000000)
9474 {
9475 {
9476 IkReal
9477 j4eval
9478 [3];
9479 sj8 = 1.0;
9480 cj8 = 0;
9481 j8 = 1.5707963267949;
9482 sj6 = 1.0;
9483 cj6 = 0;
9484 j6 = 1.5707963267949;
9485 IkReal x1075
9486 = pz
9487 * pz;
9488 IkReal x1076
9489 = (cj9
9490 * px);
9491 IkReal x1077
9492 = (cj9
9493 * py);
9494 IkReal x1078
9495 = (pp
9496 * px);
9497 IkReal x1079
9498 = (pp
9499 * py);
9500 j4eval[0]
9501 = ((((-1.0)
9502 * x1075))
9503 + pp);
9504 j4eval[1]
9505 = ((IKabs((
9506 (((0.348408823529412)
9507 * py))
9508 + (((1.66110784313725)
9509 * px))
9510 + (((1.45555882352941)
9511 * x1076))
9512 + (((0.647058823529412)
9513 * x1079))
9514 + (((-4.31372549019608)
9515 * x1078))
9516 + (((0.119091176470588)
9517 * x1077)))))
9518 + (IKabs((
9519 (((0.348408823529412)
9520 * px))
9521 + (((-1.45555882352941)
9522 * x1077))
9523 + (((4.31372549019608)
9524 * x1079))
9525 + (((0.647058823529412)
9526 * x1078))
9527 + (((-1.0)
9528 * (1.66110784313725)
9529 * py))
9530 + (((0.119091176470588)
9531 * x1076))))));
9532 j4eval[2] = IKsign((
9533 (((1.1)
9534 * pp))
9535 + (((-1.1)
9536 * x1075))));
9537 if (IKabs(
9538 j4eval
9539 [0])
9540 < 0.0000010000000000
9541 || IKabs(
9542 j4eval
9543 [1])
9544 < 0.0000010000000000
9545 || IKabs(
9546 j4eval
9547 [2])
9548 < 0.0000010000000000)
9549 {
9550 {
9551 IkReal evalcond
9552 [6];
9553 bool
9554 bgotonextstatement
9555 = true;
9556 do
9557 {
9558 IkReal
9559 x1080
9560 = ((-1.51009803921569)
9561 + (((-1.0)
9562 * (1.32323529411765)
9563 * cj9))
9564 + (((3.92156862745098)
9565 * pp)));
9566 evalcond
9567 [0]
9568 = ((IKabs(
9569 py))
9570 + (IKabs(
9571 px)));
9572 evalcond
9573 [1]
9574 = ((-0.55)
9575 + (((-1.0)
9576 * (0.3)
9577 * cj9))
9578 + (((-1.0)
9579 * (0.045)
9580 * sj9)));
9581 evalcond
9582 [2]
9583 = x1080;
9584 evalcond
9585 [3]
9586 = x1080;
9587 evalcond
9588 [4]
9589 = ((0.316735294117647)
9590 + (((0.108264705882353)
9591 * cj9))
9592 + (((0.588235294117647)
9593 * pp)));
9594 evalcond
9595 [5]
9596 = ((-0.2125)
9597 + (((-1.0)
9598 * (1.0)
9599 * pp)));
9600 if (IKabs(
9601 evalcond
9602 [0])
9603 < 0.0000010000000000
9604 && IKabs(
9605 evalcond
9606 [1])
9607 < 0.0000010000000000
9608 && IKabs(
9609 evalcond
9610 [2])
9611 < 0.0000010000000000
9612 && IKabs(
9613 evalcond
9614 [3])
9615 < 0.0000010000000000
9616 && IKabs(
9617 evalcond
9618 [4])
9619 < 0.0000010000000000
9620 && IKabs(
9621 evalcond
9622 [5])
9623 < 0.0000010000000000)
9624 {
9625 bgotonextstatement
9626 = false;
9627 {
9628 IkReal j4array
9629 [4],
9630 cj4array
9631 [4],
9632 sj4array
9633 [4];
9634 bool j4valid
9635 [4]
9636 = {false};
9637 _nj4
9638 = 4;
9639 j4array
9640 [0]
9641 = 0;
9642 sj4array
9643 [0]
9644 = IKsin(
9645 j4array
9646 [0]);
9647 cj4array
9648 [0]
9649 = IKcos(
9650 j4array
9651 [0]);
9652 j4array
9653 [1]
9654 = 1.5707963267949;
9655 sj4array
9656 [1]
9657 = IKsin(
9658 j4array
9659 [1]);
9660 cj4array
9661 [1]
9662 = IKcos(
9663 j4array
9664 [1]);
9665 j4array
9666 [2]
9667 = 3.14159265358979;
9668 sj4array
9669 [2]
9670 = IKsin(
9671 j4array
9672 [2]);
9673 cj4array
9674 [2]
9675 = IKcos(
9676 j4array
9677 [2]);
9678 j4array
9679 [3]
9680 = -1.5707963267949;
9681 sj4array
9682 [3]
9683 = IKsin(
9684 j4array
9685 [3]);
9686 cj4array
9687 [3]
9688 = IKcos(
9689 j4array
9690 [3]);
9691 if (j4array
9692 [0]
9693 > IKPI)
9694 {
9695 j4array
9696 [0]
9697 -= IK2PI;
9698 }
9699 else if (
9700 j4array
9701 [0]
9702 < -IKPI)
9703 {
9704 j4array
9705 [0]
9706 += IK2PI;
9707 }
9708 j4valid
9709 [0]
9710 = true;
9711 if (j4array
9712 [1]
9713 > IKPI)
9714 {
9715 j4array
9716 [1]
9717 -= IK2PI;
9718 }
9719 else if (
9720 j4array
9721 [1]
9722 < -IKPI)
9723 {
9724 j4array
9725 [1]
9726 += IK2PI;
9727 }
9728 j4valid
9729 [1]
9730 = true;
9731 if (j4array
9732 [2]
9733 > IKPI)
9734 {
9735 j4array
9736 [2]
9737 -= IK2PI;
9738 }
9739 else if (
9740 j4array
9741 [2]
9742 < -IKPI)
9743 {
9744 j4array
9745 [2]
9746 += IK2PI;
9747 }
9748 j4valid
9749 [2]
9750 = true;
9751 if (j4array
9752 [3]
9753 > IKPI)
9754 {
9755 j4array
9756 [3]
9757 -= IK2PI;
9758 }
9759 else if (
9760 j4array
9761 [3]
9762 < -IKPI)
9763 {
9764 j4array
9765 [3]
9766 += IK2PI;
9767 }
9768 j4valid
9769 [3]
9770 = true;
9771 for (
9772 int ij4
9773 = 0;
9774 ij4
9775 < 4;
9776 ++ij4)
9777 {
9778 if (!j4valid
9779 [ij4])
9780 {
9781 continue;
9782 }
9783 _ij4[0]
9784 = ij4;
9785 _ij4[1]
9786 = -1;
9787 for (
9788 int iij4
9789 = ij4
9790 + 1;
9791 iij4
9792 < 4;
9793 ++iij4)
9794 {
9795 if (j4valid
9796 [iij4]
9797 && IKabs(
9798 cj4array
9799 [ij4]
9800 - cj4array
9801 [iij4])
9802 < IKFAST_SOLUTION_THRESH
9803 && IKabs(
9804 sj4array
9805 [ij4]
9806 - sj4array
9807 [iij4])
9808 < IKFAST_SOLUTION_THRESH)
9809 {
9810 j4valid
9811 [iij4]
9812 = false;
9813 _ij4[1]
9814 = iij4;
9815 break;
9816 }
9817 }
9818 j4 = j4array
9819 [ij4];
9820 cj4 = cj4array
9821 [ij4];
9822 sj4 = sj4array
9823 [ij4];
9824
9825 rotationfunction0(
9826 solutions);
9827 }
9828 }
9829 }
9830 } while (
9831 0);
9832 if (bgotonextstatement)
9833 {
9834 bool
9835 bgotonextstatement
9836 = true;
9837 do
9838 {
9839 if (1)
9840 {
9841 bgotonextstatement
9842 = false;
9843 continue; // branch miss [j4]
9844 }
9845 } while (
9846 0);
9847 if (bgotonextstatement)
9848 {
9849 }
9850 }
9851 }
9852 }
9853 else
9854 {
9855 {
9856 IkReal j4array
9857 [1],
9858 cj4array
9859 [1],
9860 sj4array
9861 [1];
9862 bool j4valid
9863 [1]
9864 = {false};
9865 _nj4
9866 = 1;
9867 IkReal
9868 x1081
9869 = (cj9
9870 * px);
9871 IkReal
9872 x1082
9873 = (cj9
9874 * py);
9875 IkReal
9876 x1083
9877 = (pp
9878 * px);
9879 IkReal
9880 x1084
9881 = (pp
9882 * py);
9883 CheckValue<IkReal> x1085 = IKatan2WithCheck(
9884 IkReal((
9885 (((0.119091176470588)
9886 * x1082))
9887 + (((0.348408823529412)
9888 * py))
9889 + (((1.66110784313725)
9890 * px))
9891 + (((0.647058823529412)
9892 * x1084))
9893 + (((-4.31372549019608)
9894 * x1083))
9895 + (((1.45555882352941)
9896 * x1081)))),
9897 ((((4.31372549019608)
9898 * x1084))
9899 + (((0.119091176470588)
9900 * x1081))
9901 + (((0.348408823529412)
9902 * px))
9903 + (((0.647058823529412)
9904 * x1083))
9905 + (((-1.45555882352941)
9906 * x1082))
9907 + (((-1.0)
9908 * (1.66110784313725)
9909 * py))),
9910 IKFAST_ATAN2_MAGTHRESH);
9911 if (!x1085
9912 .valid)
9913 {
9914 continue;
9915 }
9916 CheckValue<IkReal> x1086 = IKPowWithIntegerCheck(
9917 IKsign((
9918 (((1.1)
9919 * pp))
9920 + (((-1.0)
9921 * (1.1)
9922 * (pz
9923 * pz))))),
9924 -1);
9925 if (!x1086
9926 .valid)
9927 {
9928 continue;
9929 }
9930 j4array
9931 [0]
9932 = ((-1.5707963267949)
9933 + (x1085
9934 .value)
9935 + (((1.5707963267949)
9936 * (x1086
9937 .value))));
9938 sj4array
9939 [0]
9940 = IKsin(
9941 j4array
9942 [0]);
9943 cj4array
9944 [0]
9945 = IKcos(
9946 j4array
9947 [0]);
9948 if (j4array
9949 [0]
9950 > IKPI)
9951 {
9952 j4array
9953 [0]
9954 -= IK2PI;
9955 }
9956 else if (
9957 j4array
9958 [0]
9959 < -IKPI)
9960 {
9961 j4array
9962 [0]
9963 += IK2PI;
9964 }
9965 j4valid
9966 [0]
9967 = true;
9968 for (
9969 int ij4
9970 = 0;
9971 ij4
9972 < 1;
9973 ++ij4)
9974 {
9975 if (!j4valid
9976 [ij4])
9977 {
9978 continue;
9979 }
9980 _ij4[0]
9981 = ij4;
9982 _ij4[1]
9983 = -1;
9984 for (
9985 int iij4
9986 = ij4
9987 + 1;
9988 iij4
9989 < 1;
9990 ++iij4)
9991 {
9992 if (j4valid
9993 [iij4]
9994 && IKabs(
9995 cj4array
9996 [ij4]
9997 - cj4array
9998 [iij4])
9999 < IKFAST_SOLUTION_THRESH
10000 && IKabs(
10001 sj4array
10002 [ij4]
10003 - sj4array
10004 [iij4])
10005 < IKFAST_SOLUTION_THRESH)
10006 {
10007 j4valid
10008 [iij4]
10009 = false;
10010 _ij4[1]
10011 = iij4;
10012 break;
10013 }
10014 }
10015 j4 = j4array
10016 [ij4];
10017 cj4 = cj4array
10018 [ij4];
10019 sj4 = sj4array
10020 [ij4];
10021 {
10022 IkReal evalcond
10023 [4];
10024 IkReal
10025 x1087
10026 = IKcos(
10027 j4);
10028 IkReal
10029 x1088
10030 = (px
10031 * x1087);
10032 IkReal
10033 x1089
10034 = IKsin(
10035 j4);
10036 IkReal
10037 x1090
10038 = (py
10039 * x1089);
10040 IkReal
10041 x1091
10042 = (px
10043 * x1089);
10044 IkReal
10045 x1092
10046 = (py
10047 * x1087);
10048 evalcond
10049 [0]
10050 = ((-0.55)
10051 + x1088
10052 + (((-1.0)
10053 * (0.3)
10054 * cj9))
10055 + x1090
10056 + (((-1.0)
10057 * (0.045)
10058 * sj9)));
10059 evalcond
10060 [1]
10061 = ((-1.51009803921569)
10062 + (((-1.0)
10063 * x1092))
10064 + (((-1.0)
10065 * (1.32323529411765)
10066 * cj9))
10067 + (((3.92156862745098)
10068 * pp))
10069 + x1091);
10070 evalcond
10071 [2]
10072 = ((0.316735294117647)
10073 + (((-1.0)
10074 * x1090))
10075 + (((-1.0)
10076 * x1088))
10077 + (((0.108264705882353)
10078 * cj9))
10079 + (((0.588235294117647)
10080 * pp)));
10081 evalcond
10082 [3]
10083 = ((-0.2125)
10084 + (((1.1)
10085 * x1090))
10086 + (((0.09)
10087 * x1092))
10088 + (((-0.09)
10089 * x1091))
10090 + (((1.1)
10091 * x1088))
10092 + (((-1.0)
10093 * (1.0)
10094 * pp)));
10095 if (IKabs(
10096 evalcond
10097 [0])
10098 > IKFAST_EVALCOND_THRESH
10099 || IKabs(
10100 evalcond
10101 [1])
10102 > IKFAST_EVALCOND_THRESH
10103 || IKabs(
10104 evalcond
10105 [2])
10106 > IKFAST_EVALCOND_THRESH
10107 || IKabs(
10108 evalcond
10109 [3])
10110 > IKFAST_EVALCOND_THRESH)
10111 {
10112 continue;
10113 }
10114 }
10115
10116 rotationfunction0(
10117 solutions);
10118 }
10119 }
10120 }
10121 }
10122 }
10123 else
10124 {
10125 {
10126 IkReal
10127 j4array
10128 [1],
10129 cj4array
10130 [1],
10131 sj4array
10132 [1];
10133 bool j4valid
10134 [1]
10135 = {false};
10136 _nj4 = 1;
10137 IkReal x1093
10138 = (cj9
10139 * px);
10140 IkReal x1094
10141 = (cj9
10142 * py);
10143 IkReal x1095
10144 = (pp
10145 * px);
10146 IkReal x1096
10147 = (pp
10148 * py);
10149 CheckValue<IkReal> x1097 = IKatan2WithCheck(
10150 IkReal((
10151 (((-3.92156862745098)
10152 * x1095))
10153 + (((0.316735294117647)
10154 * py))
10155 + (((1.51009803921569)
10156 * px))
10157 + (((0.108264705882353)
10158 * x1094))
10159 + (((1.32323529411765)
10160 * x1093))
10161 + (((0.588235294117647)
10162 * x1096)))),
10163 ((((0.108264705882353)
10164 * x1093))
10165 + (((0.588235294117647)
10166 * x1095))
10167 + (((-1.32323529411765)
10168 * x1094))
10169 + (((0.316735294117647)
10170 * px))
10171 + (((-1.0)
10172 * (1.51009803921569)
10173 * py))
10174 + (((3.92156862745098)
10175 * x1096))),
10176 IKFAST_ATAN2_MAGTHRESH);
10177 if (!x1097
10178 .valid)
10179 {
10180 continue;
10181 }
10182 CheckValue<IkReal> x1098 = IKPowWithIntegerCheck(
10183 IKsign((
10184 pp
10185 + (((-1.0)
10186 * (1.0)
10187 * (pz
10188 * pz))))),
10189 -1);
10190 if (!x1098
10191 .valid)
10192 {
10193 continue;
10194 }
10195 j4array[0]
10196 = ((-1.5707963267949)
10197 + (x1097
10198 .value)
10199 + (((1.5707963267949)
10200 * (x1098
10201 .value))));
10202 sj4array[0] = IKsin(
10203 j4array
10204 [0]);
10205 cj4array[0] = IKcos(
10206 j4array
10207 [0]);
10208 if (j4array
10209 [0]
10210 > IKPI)
10211 {
10212 j4array[0]
10213 -= IK2PI;
10214 }
10215 else if (
10216 j4array
10217 [0]
10218 < -IKPI)
10219 {
10220 j4array[0]
10221 += IK2PI;
10222 }
10223 j4valid[0]
10224 = true;
10225 for (int ij4
10226 = 0;
10227 ij4
10228 < 1;
10229 ++ij4)
10230 {
10231 if (!j4valid
10232 [ij4])
10233 {
10234 continue;
10235 }
10236 _ij4[0]
10237 = ij4;
10238 _ij4[1]
10239 = -1;
10240 for (
10241 int iij4
10242 = ij4
10243 + 1;
10244 iij4
10245 < 1;
10246 ++iij4)
10247 {
10248 if (j4valid
10249 [iij4]
10250 && IKabs(
10251 cj4array
10252 [ij4]
10253 - cj4array
10254 [iij4])
10255 < IKFAST_SOLUTION_THRESH
10256 && IKabs(
10257 sj4array
10258 [ij4]
10259 - sj4array
10260 [iij4])
10261 < IKFAST_SOLUTION_THRESH)
10262 {
10263 j4valid
10264 [iij4]
10265 = false;
10266 _ij4[1]
10267 = iij4;
10268 break;
10269 }
10270 }
10271 j4 = j4array
10272 [ij4];
10273 cj4 = cj4array
10274 [ij4];
10275 sj4 = sj4array
10276 [ij4];
10277 {
10278 IkReal evalcond
10279 [4];
10280 IkReal
10281 x1099
10282 = IKcos(
10283 j4);
10284 IkReal
10285 x1100
10286 = (px
10287 * x1099);
10288 IkReal
10289 x1101
10290 = IKsin(
10291 j4);
10292 IkReal
10293 x1102
10294 = (py
10295 * x1101);
10296 IkReal
10297 x1103
10298 = (px
10299 * x1101);
10300 IkReal
10301 x1104
10302 = (py
10303 * x1099);
10304 evalcond
10305 [0]
10306 = ((-0.55)
10307 + x1102
10308 + x1100
10309 + (((-1.0)
10310 * (0.3)
10311 * cj9))
10312 + (((-1.0)
10313 * (0.045)
10314 * sj9)));
10315 evalcond
10316 [1]
10317 = ((-1.51009803921569)
10318 + (((-1.0)
10319 * x1104))
10320 + (((-1.0)
10321 * (1.32323529411765)
10322 * cj9))
10323 + x1103
10324 + (((3.92156862745098)
10325 * pp)));
10326 evalcond
10327 [2]
10328 = ((0.316735294117647)
10329 + (((-1.0)
10330 * x1102))
10331 + (((-1.0)
10332 * x1100))
10333 + (((0.108264705882353)
10334 * cj9))
10335 + (((0.588235294117647)
10336 * pp)));
10337 evalcond
10338 [3]
10339 = ((-0.2125)
10340 + (((-0.09)
10341 * x1103))
10342 + (((1.1)
10343 * x1100))
10344 + (((1.1)
10345 * x1102))
10346 + (((-1.0)
10347 * (1.0)
10348 * pp))
10349 + (((0.09)
10350 * x1104)));
10351 if (IKabs(
10352 evalcond
10353 [0])
10354 > IKFAST_EVALCOND_THRESH
10355 || IKabs(
10356 evalcond
10357 [1])
10358 > IKFAST_EVALCOND_THRESH
10359 || IKabs(
10360 evalcond
10361 [2])
10362 > IKFAST_EVALCOND_THRESH
10363 || IKabs(
10364 evalcond
10365 [3])
10366 > IKFAST_EVALCOND_THRESH)
10367 {
10368 continue;
10369 }
10370 }
10371
10372 rotationfunction0(
10373 solutions);
10374 }
10375 }
10376 }
10377 }
10378 }
10379 else
10380 {
10381 {
10382 IkReal
10383 j4array[1],
10384 cj4array[1],
10385 sj4array[1];
10386 bool j4valid[1]
10387 = {false};
10388 _nj4 = 1;
10389 IkReal x1105
10390 = (cj9
10391 * px);
10392 IkReal x1106
10393 = (cj9
10394 * py);
10395 IkReal x1107
10396 = ((3.92156862745098)
10397 * pp);
10398 IkReal x1108
10399 = ((0.045)
10400 * sj9);
10401 CheckValue<IkReal> x1109 = IKPowWithIntegerCheck(
10402 IKsign((
10403 pp
10404 + (((-1.0)
10405 * (1.0)
10406 * (pz
10407 * pz))))),
10408 -1);
10409 if (!x1109
10410 .valid)
10411 {
10412 continue;
10413 }
10414 CheckValue<IkReal> x1110 = IKatan2WithCheck(
10415 IkReal((
10416 (((-1.0)
10417 * px
10418 * x1107))
10419 + ((py
10420 * x1108))
10421 + (((1.51009803921569)
10422 * px))
10423 + (((0.55)
10424 * py))
10425 + (((1.32323529411765)
10426 * x1105))
10427 + (((0.3)
10428 * x1106)))),
10429 ((((-1.32323529411765)
10430 * x1106))
10431 + (((-1.0)
10432 * (1.51009803921569)
10433 * py))
10434 + ((py
10435 * x1107))
10436 + (((0.55)
10437 * px))
10438 + ((px
10439 * x1108))
10440 + (((0.3)
10441 * x1105))),
10442 IKFAST_ATAN2_MAGTHRESH);
10443 if (!x1110
10444 .valid)
10445 {
10446 continue;
10447 }
10448 j4array[0]
10449 = ((-1.5707963267949)
10450 + (((1.5707963267949)
10451 * (x1109
10452 .value)))
10453 + (x1110
10454 .value));
10455 sj4array[0]
10456 = IKsin(
10457 j4array
10458 [0]);
10459 cj4array[0]
10460 = IKcos(
10461 j4array
10462 [0]);
10463 if (j4array[0]
10464 > IKPI)
10465 {
10466 j4array[0]
10467 -= IK2PI;
10468 }
10469 else if (
10470 j4array[0]
10471 < -IKPI)
10472 {
10473 j4array[0]
10474 += IK2PI;
10475 }
10476 j4valid[0]
10477 = true;
10478 for (int ij4
10479 = 0;
10480 ij4 < 1;
10481 ++ij4)
10482 {
10483 if (!j4valid
10484 [ij4])
10485 {
10486 continue;
10487 }
10488 _ij4[0] = ij4;
10489 _ij4[1] = -1;
10490 for (int iij4
10491 = ij4
10492 + 1;
10493 iij4 < 1;
10494 ++iij4)
10495 {
10496 if (j4valid
10497 [iij4]
10498 && IKabs(
10499 cj4array
10500 [ij4]
10501 - cj4array
10502 [iij4])
10503 < IKFAST_SOLUTION_THRESH
10504 && IKabs(
10505 sj4array
10506 [ij4]
10507 - sj4array
10508 [iij4])
10509 < IKFAST_SOLUTION_THRESH)
10510 {
10511 j4valid
10512 [iij4]
10513 = false;
10514 _ij4[1]
10515 = iij4;
10516 break;
10517 }
10518 }
10519 j4 = j4array
10520 [ij4];
10521 cj4 = cj4array
10522 [ij4];
10523 sj4 = sj4array
10524 [ij4];
10525 {
10526 IkReal
10527 evalcond
10528 [4];
10529 IkReal x1111
10530 = IKcos(
10531 j4);
10532 IkReal x1112
10533 = (px
10534 * x1111);
10535 IkReal x1113
10536 = IKsin(
10537 j4);
10538 IkReal x1114
10539 = (py
10540 * x1113);
10541 IkReal x1115
10542 = (px
10543 * x1113);
10544 IkReal x1116
10545 = (py
10546 * x1111);
10547 evalcond[0]
10548 = ((-0.55)
10549 + x1112
10550 + x1114
10551 + (((-1.0)
10552 * (0.3)
10553 * cj9))
10554 + (((-1.0)
10555 * (0.045)
10556 * sj9)));
10557 evalcond[1]
10558 = ((-1.51009803921569)
10559 + x1115
10560 + (((-1.0)
10561 * (1.32323529411765)
10562 * cj9))
10563 + (((-1.0)
10564 * x1116))
10565 + (((3.92156862745098)
10566 * pp)));
10567 evalcond[2]
10568 = ((0.316735294117647)
10569 + (((-1.0)
10570 * x1114))
10571 + (((0.108264705882353)
10572 * cj9))
10573 + (((0.588235294117647)
10574 * pp))
10575 + (((-1.0)
10576 * x1112)));
10577 evalcond[3]
10578 = ((-0.2125)
10579 + (((-0.09)
10580 * x1115))
10581 + (((1.1)
10582 * x1114))
10583 + (((1.1)
10584 * x1112))
10585 + (((-1.0)
10586 * (1.0)
10587 * pp))
10588 + (((0.09)
10589 * x1116)));
10590 if (IKabs(
10591 evalcond
10592 [0])
10593 > IKFAST_EVALCOND_THRESH
10594 || IKabs(
10595 evalcond
10596 [1])
10597 > IKFAST_EVALCOND_THRESH
10598 || IKabs(
10599 evalcond
10600 [2])
10601 > IKFAST_EVALCOND_THRESH
10602 || IKabs(
10603 evalcond
10604 [3])
10605 > IKFAST_EVALCOND_THRESH)
10606 {
10607 continue;
10608 }
10609 }
10610
10611 rotationfunction0(
10612 solutions);
10613 }
10614 }
10615 }
10616 }
10617 }
10618 } while (0);
10619 if (bgotonextstatement)
10620 {
10621 bool bgotonextstatement
10622 = true;
10623 do
10624 {
10625 evalcond[0]
10626 = ((-3.14159265358979)
10627 + (IKfmod(
10628 ((3.14159265358979)
10629 + (IKabs((
10630 (1.5707963267949)
10631 + j6)))),
10632 6.28318530717959)));
10633 evalcond[1] = pz;
10634 if (IKabs(evalcond[0])
10635 < 0.0000010000000000
10636 && IKabs(evalcond
10637 [1])
10638 < 0.0000010000000000)
10639 {
10640 bgotonextstatement
10641 = false;
10642 {
10643 IkReal j4eval[3];
10644 sj8 = 1.0;
10645 cj8 = 0;
10646 j8 = 1.5707963267949;
10647 sj6 = -1.0;
10648 cj6 = 0;
10649 j6 = -1.5707963267949;
10650 IkReal x1117
10651 = (pp
10652 + (((-1.0)
10653 * (1.0)
10654 * (pz
10655 * pz))));
10656 IkReal x1118
10657 = (cj9 * px);
10658 IkReal x1119
10659 = (cj9 * py);
10660 IkReal x1120
10661 = ((3.92156862745098)
10662 * pp);
10663 IkReal x1121
10664 = ((0.045)
10665 * sj9);
10666 j4eval[0] = x1117;
10667 j4eval[1]
10668 = ((IKabs((
10669 (((-0.3)
10670 * x1119))
10671 + (((1.51009803921569)
10672 * px))
10673 + (((1.32323529411765)
10674 * x1118))
10675 + (((-1.0)
10676 * px
10677 * x1120))
10678 + (((-1.0)
10679 * py
10680 * x1121))
10681 + (((-1.0)
10682 * (0.55)
10683 * py)))))
10684 + (IKabs((
10685 (((-1.0)
10686 * px
10687 * x1121))
10688 + (((-1.0)
10689 * (0.55)
10690 * px))
10691 + (((-0.3)
10692 * x1118))
10693 + (((-1.0)
10694 * (1.51009803921569)
10695 * py))
10696 + ((py
10697 * x1120))
10698 + (((-1.32323529411765)
10699 * x1119))))));
10700 j4eval[2]
10701 = IKsign(
10702 x1117);
10703 if (IKabs(
10704 j4eval[0])
10705 < 0.0000010000000000
10706 || IKabs(
10707 j4eval
10708 [1])
10709 < 0.0000010000000000
10710 || IKabs(
10711 j4eval
10712 [2])
10713 < 0.0000010000000000)
10714 {
10715 {
10716 IkReal
10717 j4eval[3];
10718 sj8 = 1.0;
10719 cj8 = 0;
10720 j8 = 1.5707963267949;
10721 sj6 = -1.0;
10722 cj6 = 0;
10723 j6 = -1.5707963267949;
10724 IkReal x1122
10725 = (pp
10726 + (((-1.0)
10727 * (1.0)
10728 * (pz
10729 * pz))));
10730 IkReal x1123
10731 = (cj9
10732 * px);
10733 IkReal x1124
10734 = (cj9
10735 * py);
10736 IkReal x1125
10737 = (pp
10738 * px);
10739 IkReal x1126
10740 = (pp
10741 * py);
10742 j4eval[0]
10743 = x1122;
10744 j4eval[1]
10745 = ((IKabs((
10746 (((-0.108264705882353)
10747 * x1124))
10748 + (((1.51009803921569)
10749 * px))
10750 + (((-1.0)
10751 * (0.316735294117647)
10752 * py))
10753 + (((-3.92156862745098)
10754 * x1125))
10755 + (((-0.588235294117647)
10756 * x1126))
10757 + (((1.32323529411765)
10758 * x1123)))))
10759 + (IKabs((
10760 (((-0.108264705882353)
10761 * x1123))
10762 + (((-1.0)
10763 * (1.51009803921569)
10764 * py))
10765 + (((-1.0)
10766 * (0.316735294117647)
10767 * px))
10768 + (((-0.588235294117647)
10769 * x1125))
10770 + (((3.92156862745098)
10771 * x1126))
10772 + (((-1.32323529411765)
10773 * x1124))))));
10774 j4eval[2]
10775 = IKsign(
10776 x1122);
10777 if (IKabs(
10778 j4eval
10779 [0])
10780 < 0.0000010000000000
10781 || IKabs(
10782 j4eval
10783 [1])
10784 < 0.0000010000000000
10785 || IKabs(
10786 j4eval
10787 [2])
10788 < 0.0000010000000000)
10789 {
10790 {
10791 IkReal j4eval
10792 [3];
10793 sj8 = 1.0;
10794 cj8 = 0;
10795 j8 = 1.5707963267949;
10796 sj6 = -1.0;
10797 cj6 = 0;
10798 j6 = -1.5707963267949;
10799 IkReal
10800 x1127
10801 = pz
10802 * pz;
10803 IkReal
10804 x1128
10805 = (cj9
10806 * px);
10807 IkReal
10808 x1129
10809 = (cj9
10810 * py);
10811 IkReal
10812 x1130
10813 = (pp
10814 * px);
10815 IkReal
10816 x1131
10817 = (pp
10818 * py);
10819 j4eval[0]
10820 = (x1127
10821 + (((-1.0)
10822 * (1.0)
10823 * pp)));
10824 j4eval[1] = IKsign((
10825 (((1.1)
10826 * x1127))
10827 + (((-1.0)
10828 * (1.1)
10829 * pp))));
10830 j4eval[2]
10831 = ((IKabs((
10832 (((0.348408823529412)
10833 * px))
10834 + (((1.66110784313725)
10835 * py))
10836 + (((0.119091176470588)
10837 * x1128))
10838 + (((0.647058823529412)
10839 * x1130))
10840 + (((1.45555882352941)
10841 * x1129))
10842 + (((-4.31372549019608)
10843 * x1131)))))
10844 + (IKabs((
10845 (((0.647058823529412)
10846 * x1131))
10847 + (((0.348408823529412)
10848 * py))
10849 + (((0.119091176470588)
10850 * x1129))
10851 + (((4.31372549019608)
10852 * x1130))
10853 + (((-1.0)
10854 * (1.66110784313725)
10855 * px))
10856 + (((-1.45555882352941)
10857 * x1128))))));
10858 if (IKabs(
10859 j4eval
10860 [0])
10861 < 0.0000010000000000
10862 || IKabs(
10863 j4eval
10864 [1])
10865 < 0.0000010000000000
10866 || IKabs(
10867 j4eval
10868 [2])
10869 < 0.0000010000000000)
10870 {
10871 {
10872 IkReal evalcond
10873 [6];
10874 bool
10875 bgotonextstatement
10876 = true;
10877 do
10878 {
10879 IkReal
10880 x1132
10881 = ((-1.51009803921569)
10882 + (((-1.0)
10883 * (1.32323529411765)
10884 * cj9))
10885 + (((3.92156862745098)
10886 * pp)));
10887 evalcond
10888 [0]
10889 = ((IKabs(
10890 py))
10891 + (IKabs(
10892 px)));
10893 evalcond
10894 [1]
10895 = ((-0.55)
10896 + (((-1.0)
10897 * (0.3)
10898 * cj9))
10899 + (((-1.0)
10900 * (0.045)
10901 * sj9)));
10902 evalcond
10903 [2]
10904 = x1132;
10905 evalcond
10906 [3]
10907 = x1132;
10908 evalcond
10909 [4]
10910 = ((-0.316735294117647)
10911 + (((-1.0)
10912 * (0.588235294117647)
10913 * pp))
10914 + (((-1.0)
10915 * (0.108264705882353)
10916 * cj9)));
10917 evalcond
10918 [5]
10919 = ((-0.2125)
10920 + (((-1.0)
10921 * (1.0)
10922 * pp)));
10923 if (IKabs(
10924 evalcond
10925 [0])
10926 < 0.0000010000000000
10927 && IKabs(
10928 evalcond
10929 [1])
10930 < 0.0000010000000000
10931 && IKabs(
10932 evalcond
10933 [2])
10934 < 0.0000010000000000
10935 && IKabs(
10936 evalcond
10937 [3])
10938 < 0.0000010000000000
10939 && IKabs(
10940 evalcond
10941 [4])
10942 < 0.0000010000000000
10943 && IKabs(
10944 evalcond
10945 [5])
10946 < 0.0000010000000000)
10947 {
10948 bgotonextstatement
10949 = false;
10950 {
10951 IkReal j4array
10952 [4],
10953 cj4array
10954 [4],
10955 sj4array
10956 [4];
10957 bool j4valid
10958 [4]
10959 = {false};
10960 _nj4
10961 = 4;
10962 j4array
10963 [0]
10964 = 0;
10965 sj4array
10966 [0]
10967 = IKsin(
10968 j4array
10969 [0]);
10970 cj4array
10971 [0]
10972 = IKcos(
10973 j4array
10974 [0]);
10975 j4array
10976 [1]
10977 = 1.5707963267949;
10978 sj4array
10979 [1]
10980 = IKsin(
10981 j4array
10982 [1]);
10983 cj4array
10984 [1]
10985 = IKcos(
10986 j4array
10987 [1]);
10988 j4array
10989 [2]
10990 = 3.14159265358979;
10991 sj4array
10992 [2]
10993 = IKsin(
10994 j4array
10995 [2]);
10996 cj4array
10997 [2]
10998 = IKcos(
10999 j4array
11000 [2]);
11001 j4array
11002 [3]
11003 = -1.5707963267949;
11004 sj4array
11005 [3]
11006 = IKsin(
11007 j4array
11008 [3]);
11009 cj4array
11010 [3]
11011 = IKcos(
11012 j4array
11013 [3]);
11014 if (j4array
11015 [0]
11016 > IKPI)
11017 {
11018 j4array
11019 [0]
11020 -= IK2PI;
11021 }
11022 else if (
11023 j4array
11024 [0]
11025 < -IKPI)
11026 {
11027 j4array
11028 [0]
11029 += IK2PI;
11030 }
11031 j4valid
11032 [0]
11033 = true;
11034 if (j4array
11035 [1]
11036 > IKPI)
11037 {
11038 j4array
11039 [1]
11040 -= IK2PI;
11041 }
11042 else if (
11043 j4array
11044 [1]
11045 < -IKPI)
11046 {
11047 j4array
11048 [1]
11049 += IK2PI;
11050 }
11051 j4valid
11052 [1]
11053 = true;
11054 if (j4array
11055 [2]
11056 > IKPI)
11057 {
11058 j4array
11059 [2]
11060 -= IK2PI;
11061 }
11062 else if (
11063 j4array
11064 [2]
11065 < -IKPI)
11066 {
11067 j4array
11068 [2]
11069 += IK2PI;
11070 }
11071 j4valid
11072 [2]
11073 = true;
11074 if (j4array
11075 [3]
11076 > IKPI)
11077 {
11078 j4array
11079 [3]
11080 -= IK2PI;
11081 }
11082 else if (
11083 j4array
11084 [3]
11085 < -IKPI)
11086 {
11087 j4array
11088 [3]
11089 += IK2PI;
11090 }
11091 j4valid
11092 [3]
11093 = true;
11094 for (
11095 int ij4
11096 = 0;
11097 ij4
11098 < 4;
11099 ++ij4)
11100 {
11101 if (!j4valid
11102 [ij4])
11103 {
11104 continue;
11105 }
11106 _ij4[0]
11107 = ij4;
11108 _ij4[1]
11109 = -1;
11110 for (
11111 int iij4
11112 = ij4
11113 + 1;
11114 iij4
11115 < 4;
11116 ++iij4)
11117 {
11118 if (j4valid
11119 [iij4]
11120 && IKabs(
11121 cj4array
11122 [ij4]
11123 - cj4array
11124 [iij4])
11125 < IKFAST_SOLUTION_THRESH
11126 && IKabs(
11127 sj4array
11128 [ij4]
11129 - sj4array
11130 [iij4])
11131 < IKFAST_SOLUTION_THRESH)
11132 {
11133 j4valid
11134 [iij4]
11135 = false;
11136 _ij4[1]
11137 = iij4;
11138 break;
11139 }
11140 }
11141 j4 = j4array
11142 [ij4];
11143 cj4 = cj4array
11144 [ij4];
11145 sj4 = sj4array
11146 [ij4];
11147
11148 rotationfunction0(
11149 solutions);
11150 }
11151 }
11152 }
11153 } while (
11154 0);
11155 if (bgotonextstatement)
11156 {
11157 bool
11158 bgotonextstatement
11159 = true;
11160 do
11161 {
11162 if (1)
11163 {
11164 bgotonextstatement
11165 = false;
11166 continue; // branch miss [j4]
11167 }
11168 } while (
11169 0);
11170 if (bgotonextstatement)
11171 {
11172 }
11173 }
11174 }
11175 }
11176 else
11177 {
11178 {
11179 IkReal j4array
11180 [1],
11181 cj4array
11182 [1],
11183 sj4array
11184 [1];
11185 bool j4valid
11186 [1]
11187 = {false};
11188 _nj4
11189 = 1;
11190 IkReal
11191 x1133
11192 = (cj9
11193 * px);
11194 IkReal
11195 x1134
11196 = (cj9
11197 * py);
11198 IkReal
11199 x1135
11200 = (pp
11201 * px);
11202 IkReal
11203 x1136
11204 = (pp
11205 * py);
11206 CheckValue<IkReal> x1137 = IKatan2WithCheck(
11207 IkReal((
11208 (((4.31372549019608)
11209 * x1135))
11210 + (((-1.45555882352941)
11211 * x1133))
11212 + (((0.348408823529412)
11213 * py))
11214 + (((0.119091176470588)
11215 * x1134))
11216 + (((-1.0)
11217 * (1.66110784313725)
11218 * px))
11219 + (((0.647058823529412)
11220 * x1136)))),
11221 ((((0.348408823529412)
11222 * px))
11223 + (((0.119091176470588)
11224 * x1133))
11225 + (((1.66110784313725)
11226 * py))
11227 + (((1.45555882352941)
11228 * x1134))
11229 + (((-4.31372549019608)
11230 * x1136))
11231 + (((0.647058823529412)
11232 * x1135))),
11233 IKFAST_ATAN2_MAGTHRESH);
11234 if (!x1137
11235 .valid)
11236 {
11237 continue;
11238 }
11239 CheckValue<IkReal> x1138 = IKPowWithIntegerCheck(
11240 IKsign((
11241 (((1.1)
11242 * (pz
11243 * pz)))
11244 + (((-1.0)
11245 * (1.1)
11246 * pp)))),
11247 -1);
11248 if (!x1138
11249 .valid)
11250 {
11251 continue;
11252 }
11253 j4array
11254 [0]
11255 = ((-1.5707963267949)
11256 + (x1137
11257 .value)
11258 + (((1.5707963267949)
11259 * (x1138
11260 .value))));
11261 sj4array
11262 [0]
11263 = IKsin(
11264 j4array
11265 [0]);
11266 cj4array
11267 [0]
11268 = IKcos(
11269 j4array
11270 [0]);
11271 if (j4array
11272 [0]
11273 > IKPI)
11274 {
11275 j4array
11276 [0]
11277 -= IK2PI;
11278 }
11279 else if (
11280 j4array
11281 [0]
11282 < -IKPI)
11283 {
11284 j4array
11285 [0]
11286 += IK2PI;
11287 }
11288 j4valid
11289 [0]
11290 = true;
11291 for (
11292 int ij4
11293 = 0;
11294 ij4
11295 < 1;
11296 ++ij4)
11297 {
11298 if (!j4valid
11299 [ij4])
11300 {
11301 continue;
11302 }
11303 _ij4[0]
11304 = ij4;
11305 _ij4[1]
11306 = -1;
11307 for (
11308 int iij4
11309 = ij4
11310 + 1;
11311 iij4
11312 < 1;
11313 ++iij4)
11314 {
11315 if (j4valid
11316 [iij4]
11317 && IKabs(
11318 cj4array
11319 [ij4]
11320 - cj4array
11321 [iij4])
11322 < IKFAST_SOLUTION_THRESH
11323 && IKabs(
11324 sj4array
11325 [ij4]
11326 - sj4array
11327 [iij4])
11328 < IKFAST_SOLUTION_THRESH)
11329 {
11330 j4valid
11331 [iij4]
11332 = false;
11333 _ij4[1]
11334 = iij4;
11335 break;
11336 }
11337 }
11338 j4 = j4array
11339 [ij4];
11340 cj4 = cj4array
11341 [ij4];
11342 sj4 = sj4array
11343 [ij4];
11344 {
11345 IkReal evalcond
11346 [4];
11347 IkReal
11348 x1139
11349 = IKsin(
11350 j4);
11351 IkReal
11352 x1140
11353 = (px
11354 * x1139);
11355 IkReal
11356 x1141
11357 = IKcos(
11358 j4);
11359 IkReal
11360 x1142
11361 = (py
11362 * x1141);
11363 IkReal
11364 x1143
11365 = (px
11366 * x1141);
11367 IkReal
11368 x1144
11369 = (py
11370 * x1139);
11371 IkReal
11372 x1145
11373 = ((((-1.0)
11374 * x1144))
11375 + (((-1.0)
11376 * x1143)));
11377 evalcond
11378 [0]
11379 = ((-1.51009803921569)
11380 + (((-1.0)
11381 * (1.32323529411765)
11382 * cj9))
11383 + x1140
11384 + (((3.92156862745098)
11385 * pp))
11386 + (((-1.0)
11387 * x1142)));
11388 evalcond
11389 [1]
11390 = ((-0.55)
11391 + x1145
11392 + (((-1.0)
11393 * (0.3)
11394 * cj9))
11395 + (((-1.0)
11396 * (0.045)
11397 * sj9)));
11398 evalcond
11399 [2]
11400 = ((-0.316735294117647)
11401 + x1145
11402 + (((-1.0)
11403 * (0.588235294117647)
11404 * pp))
11405 + (((-1.0)
11406 * (0.108264705882353)
11407 * cj9)));
11408 evalcond
11409 [3]
11410 = ((-0.2125)
11411 + (((-0.09)
11412 * x1140))
11413 + (((-1.1)
11414 * x1144))
11415 + (((-1.1)
11416 * x1143))
11417 + (((0.09)
11418 * x1142))
11419 + (((-1.0)
11420 * (1.0)
11421 * pp)));
11422 if (IKabs(
11423 evalcond
11424 [0])
11425 > IKFAST_EVALCOND_THRESH
11426 || IKabs(
11427 evalcond
11428 [1])
11429 > IKFAST_EVALCOND_THRESH
11430 || IKabs(
11431 evalcond
11432 [2])
11433 > IKFAST_EVALCOND_THRESH
11434 || IKabs(
11435 evalcond
11436 [3])
11437 > IKFAST_EVALCOND_THRESH)
11438 {
11439 continue;
11440 }
11441 }
11442
11443 rotationfunction0(
11444 solutions);
11445 }
11446 }
11447 }
11448 }
11449 }
11450 else
11451 {
11452 {
11453 IkReal j4array
11454 [1],
11455 cj4array
11456 [1],
11457 sj4array
11458 [1];
11459 bool j4valid
11460 [1]
11461 = {false};
11462 _nj4 = 1;
11463 IkReal
11464 x1146
11465 = (cj9
11466 * px);
11467 IkReal
11468 x1147
11469 = (cj9
11470 * py);
11471 IkReal
11472 x1148
11473 = (pp
11474 * px);
11475 IkReal
11476 x1149
11477 = (pp
11478 * py);
11479 CheckValue<IkReal> x1150 = IKatan2WithCheck(
11480 IkReal((
11481 (((1.51009803921569)
11482 * px))
11483 + (((-1.0)
11484 * (0.316735294117647)
11485 * py))
11486 + (((-0.108264705882353)
11487 * x1147))
11488 + (((-0.588235294117647)
11489 * x1149))
11490 + (((1.32323529411765)
11491 * x1146))
11492 + (((-3.92156862745098)
11493 * x1148)))),
11494 ((((-0.108264705882353)
11495 * x1146))
11496 + (((-1.32323529411765)
11497 * x1147))
11498 + (((3.92156862745098)
11499 * x1149))
11500 + (((-1.0)
11501 * (1.51009803921569)
11502 * py))
11503 + (((-1.0)
11504 * (0.316735294117647)
11505 * px))
11506 + (((-0.588235294117647)
11507 * x1148))),
11508 IKFAST_ATAN2_MAGTHRESH);
11509 if (!x1150
11510 .valid)
11511 {
11512 continue;
11513 }
11514 CheckValue<IkReal> x1151 = IKPowWithIntegerCheck(
11515 IKsign((
11516 pp
11517 + (((-1.0)
11518 * (1.0)
11519 * (pz
11520 * pz))))),
11521 -1);
11522 if (!x1151
11523 .valid)
11524 {
11525 continue;
11526 }
11527 j4array[0]
11528 = ((-1.5707963267949)
11529 + (x1150
11530 .value)
11531 + (((1.5707963267949)
11532 * (x1151
11533 .value))));
11534 sj4array[0] = IKsin(
11535 j4array
11536 [0]);
11537 cj4array[0] = IKcos(
11538 j4array
11539 [0]);
11540 if (j4array
11541 [0]
11542 > IKPI)
11543 {
11544 j4array
11545 [0]
11546 -= IK2PI;
11547 }
11548 else if (
11549 j4array
11550 [0]
11551 < -IKPI)
11552 {
11553 j4array
11554 [0]
11555 += IK2PI;
11556 }
11557 j4valid[0]
11558 = true;
11559 for (
11560 int ij4
11561 = 0;
11562 ij4
11563 < 1;
11564 ++ij4)
11565 {
11566 if (!j4valid
11567 [ij4])
11568 {
11569 continue;
11570 }
11571 _ij4[0]
11572 = ij4;
11573 _ij4[1]
11574 = -1;
11575 for (
11576 int iij4
11577 = ij4
11578 + 1;
11579 iij4
11580 < 1;
11581 ++iij4)
11582 {
11583 if (j4valid
11584 [iij4]
11585 && IKabs(
11586 cj4array
11587 [ij4]
11588 - cj4array
11589 [iij4])
11590 < IKFAST_SOLUTION_THRESH
11591 && IKabs(
11592 sj4array
11593 [ij4]
11594 - sj4array
11595 [iij4])
11596 < IKFAST_SOLUTION_THRESH)
11597 {
11598 j4valid
11599 [iij4]
11600 = false;
11601 _ij4[1]
11602 = iij4;
11603 break;
11604 }
11605 }
11606 j4 = j4array
11607 [ij4];
11608 cj4 = cj4array
11609 [ij4];
11610 sj4 = sj4array
11611 [ij4];
11612 {
11613 IkReal evalcond
11614 [4];
11615 IkReal
11616 x1152
11617 = IKsin(
11618 j4);
11619 IkReal
11620 x1153
11621 = (px
11622 * x1152);
11623 IkReal
11624 x1154
11625 = IKcos(
11626 j4);
11627 IkReal
11628 x1155
11629 = (py
11630 * x1154);
11631 IkReal
11632 x1156
11633 = (px
11634 * x1154);
11635 IkReal
11636 x1157
11637 = (py
11638 * x1152);
11639 IkReal
11640 x1158
11641 = ((((-1.0)
11642 * x1156))
11643 + (((-1.0)
11644 * x1157)));
11645 evalcond
11646 [0]
11647 = ((-1.51009803921569)
11648 + (((-1.0)
11649 * x1155))
11650 + x1153
11651 + (((-1.0)
11652 * (1.32323529411765)
11653 * cj9))
11654 + (((3.92156862745098)
11655 * pp)));
11656 evalcond
11657 [1]
11658 = ((-0.55)
11659 + x1158
11660 + (((-1.0)
11661 * (0.3)
11662 * cj9))
11663 + (((-1.0)
11664 * (0.045)
11665 * sj9)));
11666 evalcond
11667 [2]
11668 = ((-0.316735294117647)
11669 + x1158
11670 + (((-1.0)
11671 * (0.588235294117647)
11672 * pp))
11673 + (((-1.0)
11674 * (0.108264705882353)
11675 * cj9)));
11676 evalcond
11677 [3]
11678 = ((-0.2125)
11679 + (((-1.1)
11680 * x1156))
11681 + (((-1.1)
11682 * x1157))
11683 + (((-0.09)
11684 * x1153))
11685 + (((-1.0)
11686 * (1.0)
11687 * pp))
11688 + (((0.09)
11689 * x1155)));
11690 if (IKabs(
11691 evalcond
11692 [0])
11693 > IKFAST_EVALCOND_THRESH
11694 || IKabs(
11695 evalcond
11696 [1])
11697 > IKFAST_EVALCOND_THRESH
11698 || IKabs(
11699 evalcond
11700 [2])
11701 > IKFAST_EVALCOND_THRESH
11702 || IKabs(
11703 evalcond
11704 [3])
11705 > IKFAST_EVALCOND_THRESH)
11706 {
11707 continue;
11708 }
11709 }
11710
11711 rotationfunction0(
11712 solutions);
11713 }
11714 }
11715 }
11716 }
11717 }
11718 else
11719 {
11720 {
11721 IkReal j4array
11722 [1],
11723 cj4array
11724 [1],
11725 sj4array
11726 [1];
11727 bool
11728 j4valid[1]
11729 = {false};
11730 _nj4 = 1;
11731 IkReal x1159
11732 = (cj9
11733 * px);
11734 IkReal x1160
11735 = (cj9
11736 * py);
11737 IkReal x1161
11738 = ((3.92156862745098)
11739 * pp);
11740 IkReal x1162
11741 = ((0.045)
11742 * sj9);
11743 CheckValue<IkReal> x1163 = IKPowWithIntegerCheck(
11744 IKsign((
11745 pp
11746 + (((-1.0)
11747 * (1.0)
11748 * (pz
11749 * pz))))),
11750 -1);
11751 if (!x1163
11752 .valid)
11753 {
11754 continue;
11755 }
11756 CheckValue<IkReal> x1164 = IKatan2WithCheck(
11757 IkReal((
11758 (((-0.3)
11759 * x1160))
11760 + (((1.51009803921569)
11761 * px))
11762 + (((-1.0)
11763 * py
11764 * x1162))
11765 + (((-1.0)
11766 * px
11767 * x1161))
11768 + (((1.32323529411765)
11769 * x1159))
11770 + (((-1.0)
11771 * (0.55)
11772 * py)))),
11773 ((((-1.0)
11774 * (0.55)
11775 * px))
11776 + (((-1.32323529411765)
11777 * x1160))
11778 + (((-1.0)
11779 * (1.51009803921569)
11780 * py))
11781 + (((-0.3)
11782 * x1159))
11783 + ((py
11784 * x1161))
11785 + (((-1.0)
11786 * px
11787 * x1162))),
11788 IKFAST_ATAN2_MAGTHRESH);
11789 if (!x1164
11790 .valid)
11791 {
11792 continue;
11793 }
11794 j4array[0]
11795 = ((-1.5707963267949)
11796 + (((1.5707963267949)
11797 * (x1163
11798 .value)))
11799 + (x1164
11800 .value));
11801 sj4array[0]
11802 = IKsin(
11803 j4array
11804 [0]);
11805 cj4array[0]
11806 = IKcos(
11807 j4array
11808 [0]);
11809 if (j4array[0]
11810 > IKPI)
11811 {
11812 j4array[0]
11813 -= IK2PI;
11814 }
11815 else if (
11816 j4array[0]
11817 < -IKPI)
11818 {
11819 j4array[0]
11820 += IK2PI;
11821 }
11822 j4valid[0]
11823 = true;
11824 for (int ij4
11825 = 0;
11826 ij4 < 1;
11827 ++ij4)
11828 {
11829 if (!j4valid
11830 [ij4])
11831 {
11832 continue;
11833 }
11834 _ij4[0]
11835 = ij4;
11836 _ij4[1]
11837 = -1;
11838 for (
11839 int iij4
11840 = ij4
11841 + 1;
11842 iij4
11843 < 1;
11844 ++iij4)
11845 {
11846 if (j4valid
11847 [iij4]
11848 && IKabs(
11849 cj4array
11850 [ij4]
11851 - cj4array
11852 [iij4])
11853 < IKFAST_SOLUTION_THRESH
11854 && IKabs(
11855 sj4array
11856 [ij4]
11857 - sj4array
11858 [iij4])
11859 < IKFAST_SOLUTION_THRESH)
11860 {
11861 j4valid
11862 [iij4]
11863 = false;
11864 _ij4[1]
11865 = iij4;
11866 break;
11867 }
11868 }
11869 j4 = j4array
11870 [ij4];
11871 cj4 = cj4array
11872 [ij4];
11873 sj4 = sj4array
11874 [ij4];
11875 {
11876 IkReal evalcond
11877 [4];
11878 IkReal
11879 x1165
11880 = IKsin(
11881 j4);
11882 IkReal
11883 x1166
11884 = (px
11885 * x1165);
11886 IkReal
11887 x1167
11888 = IKcos(
11889 j4);
11890 IkReal
11891 x1168
11892 = (py
11893 * x1167);
11894 IkReal
11895 x1169
11896 = (px
11897 * x1167);
11898 IkReal
11899 x1170
11900 = (py
11901 * x1165);
11902 IkReal
11903 x1171
11904 = ((((-1.0)
11905 * x1169))
11906 + (((-1.0)
11907 * x1170)));
11908 evalcond
11909 [0]
11910 = ((-1.51009803921569)
11911 + (((-1.0)
11912 * (1.32323529411765)
11913 * cj9))
11914 + (((-1.0)
11915 * x1168))
11916 + (((3.92156862745098)
11917 * pp))
11918 + x1166);
11919 evalcond
11920 [1]
11921 = ((-0.55)
11922 + (((-1.0)
11923 * (0.3)
11924 * cj9))
11925 + x1171
11926 + (((-1.0)
11927 * (0.045)
11928 * sj9)));
11929 evalcond
11930 [2]
11931 = ((-0.316735294117647)
11932 + (((-1.0)
11933 * (0.588235294117647)
11934 * pp))
11935 + (((-1.0)
11936 * (0.108264705882353)
11937 * cj9))
11938 + x1171);
11939 evalcond
11940 [3]
11941 = ((-0.2125)
11942 + (((-1.1)
11943 * x1170))
11944 + (((-1.1)
11945 * x1169))
11946 + (((0.09)
11947 * x1168))
11948 + (((-0.09)
11949 * x1166))
11950 + (((-1.0)
11951 * (1.0)
11952 * pp)));
11953 if (IKabs(
11954 evalcond
11955 [0])
11956 > IKFAST_EVALCOND_THRESH
11957 || IKabs(
11958 evalcond
11959 [1])
11960 > IKFAST_EVALCOND_THRESH
11961 || IKabs(
11962 evalcond
11963 [2])
11964 > IKFAST_EVALCOND_THRESH
11965 || IKabs(
11966 evalcond
11967 [3])
11968 > IKFAST_EVALCOND_THRESH)
11969 {
11970 continue;
11971 }
11972 }
11973
11974 rotationfunction0(
11975 solutions);
11976 }
11977 }
11978 }
11979 }
11980 }
11981 } while (0);
11982 if (bgotonextstatement)
11983 {
11984 bool
11985 bgotonextstatement
11986 = true;
11987 do
11988 {
11989 IkReal x1172
11990 = (cj6 * pz);
11991 IkReal x1173
11992 = ((-1.51009803921569)
11993 + (((-1.0)
11994 * (1.32323529411765)
11995 * cj9))
11996 + (((3.92156862745098)
11997 * pp)));
11998 evalcond[0]
11999 = ((IKabs(py))
12000 + (IKabs(
12001 px)));
12002 evalcond[1]
12003 = ((-0.55)
12004 + (((-1.0)
12005 * (0.3)
12006 * cj9))
12007 + x1172
12008 + (((-1.0)
12009 * (0.045)
12010 * sj9)));
12011 evalcond[2] = x1173;
12012 evalcond[3]
12013 = ((-1.0)
12014 * (((1.0)
12015 * pz
12016 * sj6)));
12017 evalcond[4] = x1173;
12018 evalcond[5]
12019 = ((((0.108264705882353)
12020 * cj9
12021 * sj6))
12022 + (((0.588235294117647)
12023 * pp
12024 * sj6))
12025 + (((0.316735294117647)
12026 * sj6)));
12027 evalcond[6]
12028 = ((-0.2125)
12029 + (((1.1)
12030 * x1172))
12031 + (((-1.0)
12032 * (1.0)
12033 * pp)));
12034 if (IKabs(
12035 evalcond[0])
12036 < 0.0000010000000000
12037 && IKabs(
12038 evalcond
12039 [1])
12040 < 0.0000010000000000
12041 && IKabs(
12042 evalcond
12043 [2])
12044 < 0.0000010000000000
12045 && IKabs(
12046 evalcond
12047 [3])
12048 < 0.0000010000000000
12049 && IKabs(
12050 evalcond
12051 [4])
12052 < 0.0000010000000000
12053 && IKabs(
12054 evalcond
12055 [5])
12056 < 0.0000010000000000
12057 && IKabs(
12058 evalcond
12059 [6])
12060 < 0.0000010000000000)
12061 {
12062 bgotonextstatement
12063 = false;
12064 {
12065 IkReal
12066 j4array[4],
12067 cj4array[4],
12068 sj4array[4];
12069 bool j4valid[4]
12070 = {false};
12071 _nj4 = 4;
12072 j4array[0] = 0;
12073 sj4array[0]
12074 = IKsin(
12075 j4array
12076 [0]);
12077 cj4array[0]
12078 = IKcos(
12079 j4array
12080 [0]);
12081 j4array[1]
12082 = 1.5707963267949;
12083 sj4array[1]
12084 = IKsin(
12085 j4array
12086 [1]);
12087 cj4array[1]
12088 = IKcos(
12089 j4array
12090 [1]);
12091 j4array[2]
12092 = 3.14159265358979;
12093 sj4array[2]
12094 = IKsin(
12095 j4array
12096 [2]);
12097 cj4array[2]
12098 = IKcos(
12099 j4array
12100 [2]);
12101 j4array[3]
12102 = -1.5707963267949;
12103 sj4array[3]
12104 = IKsin(
12105 j4array
12106 [3]);
12107 cj4array[3]
12108 = IKcos(
12109 j4array
12110 [3]);
12111 if (j4array[0]
12112 > IKPI)
12113 {
12114 j4array[0]
12115 -= IK2PI;
12116 }
12117 else if (
12118 j4array[0]
12119 < -IKPI)
12120 {
12121 j4array[0]
12122 += IK2PI;
12123 }
12124 j4valid[0]
12125 = true;
12126 if (j4array[1]
12127 > IKPI)
12128 {
12129 j4array[1]
12130 -= IK2PI;
12131 }
12132 else if (
12133 j4array[1]
12134 < -IKPI)
12135 {
12136 j4array[1]
12137 += IK2PI;
12138 }
12139 j4valid[1]
12140 = true;
12141 if (j4array[2]
12142 > IKPI)
12143 {
12144 j4array[2]
12145 -= IK2PI;
12146 }
12147 else if (
12148 j4array[2]
12149 < -IKPI)
12150 {
12151 j4array[2]
12152 += IK2PI;
12153 }
12154 j4valid[2]
12155 = true;
12156 if (j4array[3]
12157 > IKPI)
12158 {
12159 j4array[3]
12160 -= IK2PI;
12161 }
12162 else if (
12163 j4array[3]
12164 < -IKPI)
12165 {
12166 j4array[3]
12167 += IK2PI;
12168 }
12169 j4valid[3]
12170 = true;
12171 for (int ij4
12172 = 0;
12173 ij4 < 4;
12174 ++ij4)
12175 {
12176 if (!j4valid
12177 [ij4])
12178 {
12179 continue;
12180 }
12181 _ij4[0] = ij4;
12182 _ij4[1] = -1;
12183 for (int iij4
12184 = ij4
12185 + 1;
12186 iij4 < 4;
12187 ++iij4)
12188 {
12189 if (j4valid
12190 [iij4]
12191 && IKabs(
12192 cj4array
12193 [ij4]
12194 - cj4array
12195 [iij4])
12196 < IKFAST_SOLUTION_THRESH
12197 && IKabs(
12198 sj4array
12199 [ij4]
12200 - sj4array
12201 [iij4])
12202 < IKFAST_SOLUTION_THRESH)
12203 {
12204 j4valid
12205 [iij4]
12206 = false;
12207 _ij4[1]
12208 = iij4;
12209 break;
12210 }
12211 }
12212 j4 = j4array
12213 [ij4];
12214 cj4 = cj4array
12215 [ij4];
12216 sj4 = sj4array
12217 [ij4];
12218
12219 rotationfunction0(
12220 solutions);
12221 }
12222 }
12223 }
12224 } while (0);
12225 if (bgotonextstatement)
12226 {
12227 bool
12228 bgotonextstatement
12229 = true;
12230 do
12231 {
12232 if (1)
12233 {
12234 bgotonextstatement
12235 = false;
12236 continue; // branch
12237 // miss
12238 // [j4]
12239 }
12240 } while (0);
12241 if (bgotonextstatement)
12242 {
12243 }
12244 }
12245 }
12246 }
12247 }
12248 }
12249 else
12250 {
12251 {
12252 IkReal j4array[1],
12253 cj4array[1],
12254 sj4array[1];
12255 bool j4valid[1] = {false};
12256 _nj4 = 1;
12257 IkReal x1174
12258 = ((1.32323529411765)
12259 * cj9);
12260 IkReal x1175
12261 = ((3.92156862745098)
12262 * pp);
12263 IkReal x1176
12264 = ((0.316735294117647)
12265 * sj6);
12266 IkReal x1177
12267 = ((0.108264705882353)
12268 * cj9 * sj6);
12269 IkReal x1178
12270 = ((0.588235294117647)
12271 * pp * sj6);
12272 CheckValue<IkReal> x1179
12273 = IKatan2WithCheck(
12274 IkReal((
12275 (((-1.0) * px
12276 * x1175))
12277 + ((px
12278 * x1174))
12279 + (((1.51009803921569)
12280 * px))
12281 + ((py
12282 * x1178))
12283 + ((py
12284 * x1177))
12285 + ((py
12286 * x1176)))),
12287 (((px * x1176))
12288 + (((-1.0)
12289 * (1.51009803921569)
12290 * py))
12291 + ((px * x1178))
12292 + ((py * x1175))
12293 + (((-1.0) * py
12294 * x1174))
12295 + ((px
12296 * x1177))),
12297 IKFAST_ATAN2_MAGTHRESH);
12298 if (!x1179.valid)
12299 {
12300 continue;
12301 }
12302 CheckValue<IkReal> x1180
12303 = IKPowWithIntegerCheck(
12304 IKsign((
12305 pp
12306 + (((-1.0)
12307 * (1.0)
12308 * (pz
12309 * pz))))),
12310 -1);
12311 if (!x1180.valid)
12312 {
12313 continue;
12314 }
12315 j4array[0]
12316 = ((-1.5707963267949)
12317 + (x1179.value)
12318 + (((1.5707963267949)
12319 * (x1180
12320 .value))));
12321 sj4array[0]
12322 = IKsin(j4array[0]);
12323 cj4array[0]
12324 = IKcos(j4array[0]);
12325 if (j4array[0] > IKPI)
12326 {
12327 j4array[0] -= IK2PI;
12328 }
12329 else if (
12330 j4array[0] < -IKPI)
12331 {
12332 j4array[0] += IK2PI;
12333 }
12334 j4valid[0] = true;
12335 for (int ij4 = 0; ij4 < 1;
12336 ++ij4)
12337 {
12338 if (!j4valid[ij4])
12339 {
12340 continue;
12341 }
12342 _ij4[0] = ij4;
12343 _ij4[1] = -1;
12344 for (int iij4 = ij4 + 1;
12345 iij4 < 1;
12346 ++iij4)
12347 {
12348 if (j4valid[iij4]
12349 && IKabs(
12350 cj4array
12351 [ij4]
12352 - cj4array
12353 [iij4])
12354 < IKFAST_SOLUTION_THRESH
12355 && IKabs(
12356 sj4array
12357 [ij4]
12358 - sj4array
12359 [iij4])
12360 < IKFAST_SOLUTION_THRESH)
12361 {
12362 j4valid[iij4]
12363 = false;
12364 _ij4[1] = iij4;
12365 break;
12366 }
12367 }
12368 j4 = j4array[ij4];
12369 cj4 = cj4array[ij4];
12370 sj4 = sj4array[ij4];
12371 {
12372 IkReal evalcond[5];
12373 IkReal x1181
12374 = IKcos(j4);
12375 IkReal x1182
12376 = (px * x1181);
12377 IkReal x1183
12378 = IKsin(j4);
12379 IkReal x1184
12380 = (py * x1183);
12381 IkReal x1185
12382 = (px * x1183);
12383 IkReal x1186
12384 = ((1.0) * x1181);
12385 IkReal x1187
12386 = (cj6 * pz);
12387 IkReal x1188
12388 = (sj6 * x1182);
12389 IkReal x1189
12390 = (sj6 * x1184);
12391 evalcond[0]
12392 = (((cj6 * x1184))
12393 + ((cj6
12394 * x1182))
12395 + (((-1.0)
12396 * (1.0)
12397 * pz
12398 * sj6)));
12399 evalcond[1]
12400 = ((-1.51009803921569)
12401 + (((-1.0) * py
12402 * x1186))
12403 + (((-1.0)
12404 * (1.32323529411765)
12405 * cj9))
12406 + (((3.92156862745098)
12407 * pp))
12408 + x1185);
12409 evalcond[2]
12410 = ((-0.55)
12411 + (((-1.0)
12412 * (0.3)
12413 * cj9))
12414 + x1187 + x1188
12415 + x1189
12416 + (((-1.0)
12417 * (0.045)
12418 * sj9)));
12419 evalcond[3]
12420 = ((((0.108264705882353)
12421 * cj9 * sj6))
12422 + (((-1.0)
12423 * x1184))
12424 + (((-1.0) * px
12425 * x1186))
12426 + (((0.588235294117647)
12427 * pp
12428 * sj6))
12429 + (((0.316735294117647)
12430 * sj6)));
12431 evalcond[4]
12432 = ((-0.2125)
12433 + (((1.1)
12434 * x1188))
12435 + (((-0.09)
12436 * x1185))
12437 + (((0.09) * py
12438 * x1181))
12439 + (((-1.0)
12440 * (1.0)
12441 * pp))
12442 + (((1.1)
12443 * x1187))
12444 + (((1.1)
12445 * x1189)));
12446 if (IKabs(evalcond[0])
12447 > IKFAST_EVALCOND_THRESH
12448 || IKabs(evalcond
12449 [1])
12450 > IKFAST_EVALCOND_THRESH
12451 || IKabs(evalcond
12452 [2])
12453 > IKFAST_EVALCOND_THRESH
12454 || IKabs(evalcond
12455 [3])
12456 > IKFAST_EVALCOND_THRESH
12457 || IKabs(evalcond
12458 [4])
12459 > IKFAST_EVALCOND_THRESH)
12460 {
12461 continue;
12462 }
12463 }
12464
12465 rotationfunction0(
12466 solutions);
12467 }
12468 }
12469 }
12470 }
12471 }
12472 else
12473 {
12474 {
12475 IkReal j4array[1],
12476 cj4array[1], sj4array[1];
12477 bool j4valid[1] = {false};
12478 _nj4 = 1;
12479 IkReal x1190 = (cj6 * pp);
12480 IkReal x1191
12481 = ((0.2125) * cj6);
12482 IkReal x1192 = ((1.1) * pz);
12483 IkReal x1193
12484 = ((0.09) * pz * sj6);
12485 CheckValue<IkReal> x1194
12486 = IKatan2WithCheck(
12487 IkReal(
12488 (((px * x1192))
12489 + (((-1.0) * px
12490 * x1190))
12491 + ((py * x1193))
12492 + (((-1.0) * px
12493 * x1191)))),
12494 (((py * x1190))
12495 + (((-1.0) * py
12496 * x1192))
12497 + ((py * x1191))
12498 + ((px * x1193))),
12499 IKFAST_ATAN2_MAGTHRESH);
12500 if (!x1194.valid)
12501 {
12502 continue;
12503 }
12504 CheckValue<IkReal> x1195
12505 = IKPowWithIntegerCheck(
12506 IKsign((
12507 (((0.09) * x1190))
12508 + (((-1.0)
12509 * (0.09) * cj6
12510 * (pz
12511 * pz))))),
12512 -1);
12513 if (!x1195.valid)
12514 {
12515 continue;
12516 }
12517 j4array[0]
12518 = ((-1.5707963267949)
12519 + (x1194.value)
12520 + (((1.5707963267949)
12521 * (x1195.value))));
12522 sj4array[0]
12523 = IKsin(j4array[0]);
12524 cj4array[0]
12525 = IKcos(j4array[0]);
12526 if (j4array[0] > IKPI)
12527 {
12528 j4array[0] -= IK2PI;
12529 }
12530 else if (j4array[0] < -IKPI)
12531 {
12532 j4array[0] += IK2PI;
12533 }
12534 j4valid[0] = true;
12535 for (int ij4 = 0; ij4 < 1;
12536 ++ij4)
12537 {
12538 if (!j4valid[ij4])
12539 {
12540 continue;
12541 }
12542 _ij4[0] = ij4;
12543 _ij4[1] = -1;
12544 for (int iij4 = ij4 + 1;
12545 iij4 < 1;
12546 ++iij4)
12547 {
12548 if (j4valid[iij4]
12549 && IKabs(
12550 cj4array[ij4]
12551 - cj4array
12552 [iij4])
12553 < IKFAST_SOLUTION_THRESH
12554 && IKabs(
12555 sj4array[ij4]
12556 - sj4array
12557 [iij4])
12558 < IKFAST_SOLUTION_THRESH)
12559 {
12560 j4valid[iij4] = false;
12561 _ij4[1] = iij4;
12562 break;
12563 }
12564 }
12565 j4 = j4array[ij4];
12566 cj4 = cj4array[ij4];
12567 sj4 = sj4array[ij4];
12568 {
12569 IkReal evalcond[5];
12570 IkReal x1196 = IKcos(j4);
12571 IkReal x1197
12572 = (px * x1196);
12573 IkReal x1198 = IKsin(j4);
12574 IkReal x1199
12575 = (py * x1198);
12576 IkReal x1200
12577 = (px * x1198);
12578 IkReal x1201
12579 = ((1.0) * x1196);
12580 IkReal x1202 = (cj6 * pz);
12581 IkReal x1203
12582 = (sj6 * x1197);
12583 IkReal x1204
12584 = (sj6 * x1199);
12585 evalcond[0]
12586 = (((cj6 * x1199))
12587 + ((cj6 * x1197))
12588 + (((-1.0) * (1.0)
12589 * pz * sj6)));
12590 evalcond[1]
12591 = ((-1.51009803921569)
12592 + x1200
12593 + (((-1.0)
12594 * (1.32323529411765)
12595 * cj9))
12596 + (((-1.0) * py
12597 * x1201))
12598 + (((3.92156862745098)
12599 * pp)));
12600 evalcond[2]
12601 = ((-0.55) + x1202
12602 + x1203 + x1204
12603 + (((-1.0) * (0.3)
12604 * cj9))
12605 + (((-1.0)
12606 * (0.045)
12607 * sj9)));
12608 evalcond[3]
12609 = ((((0.108264705882353)
12610 * cj9 * sj6))
12611 + (((0.588235294117647)
12612 * pp * sj6))
12613 + (((-1.0) * px
12614 * x1201))
12615 + (((0.316735294117647)
12616 * sj6))
12617 + (((-1.0)
12618 * x1199)));
12619 evalcond[4]
12620 = ((-0.2125)
12621 + (((1.1) * x1204))
12622 + (((1.1) * x1203))
12623 + (((-0.09)
12624 * x1200))
12625 + (((0.09) * py
12626 * x1196))
12627 + (((1.1) * x1202))
12628 + (((-1.0) * (1.0)
12629 * pp)));
12630 if (IKabs(evalcond[0])
12631 > IKFAST_EVALCOND_THRESH
12632 || IKabs(evalcond[1])
12633 > IKFAST_EVALCOND_THRESH
12634 || IKabs(evalcond[2])
12635 > IKFAST_EVALCOND_THRESH
12636 || IKabs(evalcond[3])
12637 > IKFAST_EVALCOND_THRESH
12638 || IKabs(evalcond[4])
12639 > IKFAST_EVALCOND_THRESH)
12640 {
12641 continue;
12642 }
12643 }
12644
12645 rotationfunction0(
12646 solutions);
12647 }
12648 }
12649 }
12650 }
12651 }
12652 else
12653 {
12654 {
12655 IkReal j4array[1], cj4array[1],
12656 sj4array[1];
12657 bool j4valid[1] = {false};
12658 _nj4 = 1;
12659 IkReal x1205
12660 = ((1.51009803921569) * cj6);
12661 IkReal x1206 = (pz * sj6);
12662 IkReal x1207
12663 = ((1.32323529411765) * cj6
12664 * cj9);
12665 IkReal x1208
12666 = ((3.92156862745098) * cj6
12667 * pp);
12668 CheckValue<IkReal> x1209
12669 = IKatan2WithCheck(
12670 IkReal(
12671 (((px * x1205))
12672 + ((px * x1207))
12673 + ((py * x1206))
12674 + (((-1.0) * px
12675 * x1208)))),
12676 ((((-1.0) * py * x1205))
12677 + ((py * x1208))
12678 + ((px * x1206))
12679 + (((-1.0) * py
12680 * x1207))),
12681 IKFAST_ATAN2_MAGTHRESH);
12682 if (!x1209.valid)
12683 {
12684 continue;
12685 }
12686 CheckValue<IkReal> x1210
12687 = IKPowWithIntegerCheck(
12688 IKsign(
12689 ((((-1.0) * (1.0)
12690 * cj6 * (pz * pz)))
12691 + ((cj6 * pp)))),
12692 -1);
12693 if (!x1210.valid)
12694 {
12695 continue;
12696 }
12697 j4array[0]
12698 = ((-1.5707963267949)
12699 + (x1209.value)
12700 + (((1.5707963267949)
12701 * (x1210.value))));
12702 sj4array[0] = IKsin(j4array[0]);
12703 cj4array[0] = IKcos(j4array[0]);
12704 if (j4array[0] > IKPI)
12705 {
12706 j4array[0] -= IK2PI;
12707 }
12708 else if (j4array[0] < -IKPI)
12709 {
12710 j4array[0] += IK2PI;
12711 }
12712 j4valid[0] = true;
12713 for (int ij4 = 0; ij4 < 1; ++ij4)
12714 {
12715 if (!j4valid[ij4])
12716 {
12717 continue;
12718 }
12719 _ij4[0] = ij4;
12720 _ij4[1] = -1;
12721 for (int iij4 = ij4 + 1;
12722 iij4 < 1;
12723 ++iij4)
12724 {
12725 if (j4valid[iij4]
12726 && IKabs(
12727 cj4array[ij4]
12728 - cj4array[iij4])
12729 < IKFAST_SOLUTION_THRESH
12730 && IKabs(
12731 sj4array[ij4]
12732 - sj4array[iij4])
12733 < IKFAST_SOLUTION_THRESH)
12734 {
12735 j4valid[iij4] = false;
12736 _ij4[1] = iij4;
12737 break;
12738 }
12739 }
12740 j4 = j4array[ij4];
12741 cj4 = cj4array[ij4];
12742 sj4 = sj4array[ij4];
12743 {
12744 IkReal evalcond[5];
12745 IkReal x1211 = IKcos(j4);
12746 IkReal x1212 = (px * x1211);
12747 IkReal x1213 = IKsin(j4);
12748 IkReal x1214 = (py * x1213);
12749 IkReal x1215 = (px * x1213);
12750 IkReal x1216
12751 = ((1.0) * x1211);
12752 IkReal x1217 = (cj6 * pz);
12753 IkReal x1218 = (sj6 * x1212);
12754 IkReal x1219 = (sj6 * x1214);
12755 evalcond[0]
12756 = (((cj6 * x1212))
12757 + ((cj6 * x1214))
12758 + (((-1.0) * (1.0) * pz
12759 * sj6)));
12760 evalcond[1]
12761 = ((-1.51009803921569)
12762 + (((-1.0)
12763 * (1.32323529411765)
12764 * cj9))
12765 + (((3.92156862745098)
12766 * pp))
12767 + x1215
12768 + (((-1.0) * py
12769 * x1216)));
12770 evalcond[2]
12771 = ((-0.55)
12772 + (((-1.0) * (0.3)
12773 * cj9))
12774 + x1218 + x1219 + x1217
12775 + (((-1.0) * (0.045)
12776 * sj9)));
12777 evalcond[3]
12778 = ((((0.108264705882353)
12779 * cj9 * sj6))
12780 + (((-1.0) * x1214))
12781 + (((0.588235294117647)
12782 * pp * sj6))
12783 + (((-1.0) * px
12784 * x1216))
12785 + (((0.316735294117647)
12786 * sj6)));
12787 evalcond[4]
12788 = ((-0.2125)
12789 + (((1.1) * x1217))
12790 + (((1.1) * x1218))
12791 + (((-0.09) * x1215))
12792 + (((1.1) * x1219))
12793 + (((-1.0) * (1.0)
12794 * pp))
12795 + (((0.09) * py
12796 * x1211)));
12797 if (IKabs(evalcond[0])
12798 > IKFAST_EVALCOND_THRESH
12799 || IKabs(evalcond[1])
12800 > IKFAST_EVALCOND_THRESH
12801 || IKabs(evalcond[2])
12802 > IKFAST_EVALCOND_THRESH
12803 || IKabs(evalcond[3])
12804 > IKFAST_EVALCOND_THRESH
12805 || IKabs(evalcond[4])
12806 > IKFAST_EVALCOND_THRESH)
12807 {
12808 continue;
12809 }
12810 }
12811
12812 rotationfunction0(solutions);
12813 }
12814 }
12815 }
12816 }
12817 }
12818 } while (0);
12819 if (bgotonextstatement)
12820 {
12821 bool bgotonextstatement = true;
12822 do
12823 {
12824 evalcond[0]
12825 = ((-3.14159265358979)
12826 + (IKfmod(
12827 ((3.14159265358979)
12828 + (IKabs(
12829 ((1.5707963267949)
12830 + j8)))),
12831 6.28318530717959)));
12832 evalcond[1]
12833 = ((0.39655) + (((0.0765) * sj9))
12834 + (((0.32595) * cj9))
12835 + (((-1.0) * (1.0) * pp)));
12836 evalcond[2]
12837 = ((((-1.0) * (0.3) * cj6 * cj9))
12838 + (((-1.0) * (0.55) * cj6)) + pz
12839 + (((-1.0) * (0.045) * cj6
12840 * sj9)));
12841 if (IKabs(evalcond[0])
12842 < 0.0000010000000000
12843 && IKabs(evalcond[1])
12844 < 0.0000010000000000
12845 && IKabs(evalcond[2])
12846 < 0.0000010000000000)
12847 {
12848 bgotonextstatement = false;
12849 {
12850 IkReal j4eval[3];
12851 sj8 = -1.0;
12852 cj8 = 0;
12853 j8 = -1.5707963267949;
12854 IkReal x1220
12855 = ((((-1.0) * (1.0) * cj6
12856 * (pz * pz)))
12857 + ((cj6 * pp)));
12858 IkReal x1221
12859 = ((1.51009803921569) * cj6);
12860 IkReal x1222 = (pz * sj6);
12861 IkReal x1223
12862 = ((1.32323529411765) * cj6
12863 * cj9);
12864 IkReal x1224
12865 = ((3.92156862745098) * cj6
12866 * pp);
12867 j4eval[0] = x1220;
12868 j4eval[1]
12869 = ((IKabs(
12870 (((py * x1221))
12871 + ((py * x1223))
12872 + ((px * x1222))
12873 + (((-1.0) * py
12874 * x1224)))))
12875 + (IKabs((
12876 (((-1.0) * px * x1221))
12877 + ((py * x1222))
12878 + (((-1.0) * px
12879 * x1223))
12880 + ((px * x1224))))));
12881 j4eval[2] = IKsign(x1220);
12882 if (IKabs(j4eval[0])
12883 < 0.0000010000000000
12884 || IKabs(j4eval[1])
12885 < 0.0000010000000000
12886 || IKabs(j4eval[2])
12887 < 0.0000010000000000)
12888 {
12889 {
12890 IkReal j4eval[3];
12891 sj8 = -1.0;
12892 cj8 = 0;
12893 j8 = -1.5707963267949;
12894 IkReal x1225 = (cj6 * pp);
12895 IkReal x1226 = ((1.0) * x1225);
12896 IkReal x1227
12897 = (cj6 * (pz * pz));
12898 IkReal x1228 = ((0.2125) * cj6);
12899 IkReal x1229 = ((1.1) * pz);
12900 IkReal x1230
12901 = ((0.09) * pz * sj6);
12902 j4eval[0]
12903 = ((((-1.0) * x1226))
12904 + x1227);
12905 j4eval[1] = IKsign(
12906 ((((-0.09) * x1225))
12907 + (((0.09) * x1227))));
12908 j4eval[2]
12909 = ((IKabs(
12910 (((py * x1228))
12911 + ((py * x1225))
12912 + (((-1.0) * py
12913 * x1229))
12914 + (((-1.0) * px
12915 * x1230)))))
12916 + (IKabs(
12917 ((((-1.0) * px
12918 * x1228))
12919 + (((-1.0) * px
12920 * x1226))
12921 + ((px * x1229))
12922 + (((-1.0) * py
12923 * x1230))))));
12924 if (IKabs(j4eval[0])
12925 < 0.0000010000000000
12926 || IKabs(j4eval[1])
12927 < 0.0000010000000000
12928 || IKabs(j4eval[2])
12929 < 0.0000010000000000)
12930 {
12931 {
12932 IkReal j4eval[3];
12933 sj8 = -1.0;
12934 cj8 = 0;
12935 j8 = -1.5707963267949;
12936 IkReal x1231
12937 = (pp
12938 + (((-1.0) * (1.0)
12939 * (pz * pz))));
12940 IkReal x1232
12941 = ((1.32323529411765)
12942 * cj9);
12943 IkReal x1233
12944 = ((3.92156862745098)
12945 * pp);
12946 IkReal x1234
12947 = ((0.316735294117647)
12948 * sj6);
12949 IkReal x1235
12950 = ((0.108264705882353)
12951 * cj9 * sj6);
12952 IkReal x1236
12953 = ((0.588235294117647)
12954 * pp * sj6);
12955 j4eval[0] = x1231;
12956 j4eval[1]
12957 = ((IKabs((
12958 ((px * x1233))
12959 + (((-1.0)
12960 * (1.51009803921569)
12961 * px))
12962 + (((-1.0) * px
12963 * x1232))
12964 + ((py * x1234))
12965 + ((py * x1236))
12966 + ((py
12967 * x1235)))))
12968 + (IKabs((
12969 (((-1.0) * py
12970 * x1233))
12971 + ((py
12972 * x1232))
12973 + ((px
12974 * x1236))
12975 + ((px
12976 * x1235))
12977 + (((1.51009803921569)
12978 * py))
12979 + ((px
12980 * x1234))))));
12981 j4eval[2] = IKsign(x1231);
12982 if (IKabs(j4eval[0])
12983 < 0.0000010000000000
12984 || IKabs(j4eval[1])
12985 < 0.0000010000000000
12986 || IKabs(j4eval[2])
12987 < 0.0000010000000000)
12988 {
12989 {
12990 IkReal evalcond[7];
12991 bool bgotonextstatement
12992 = true;
12993 do
12994 {
12995 evalcond[0]
12996 = ((-3.14159265358979)
12997 + (IKfmod(
12998 ((3.14159265358979)
12999 + (IKabs((
13000 (-1.5707963267949)
13001 + j6)))),
13002 6.28318530717959)));
13003 evalcond[1] = pz;
13004 if (IKabs(evalcond[0])
13005 < 0.0000010000000000
13006 && IKabs(evalcond
13007 [1])
13008 < 0.0000010000000000)
13009 {
13010 bgotonextstatement
13011 = false;
13012 {
13013 IkReal j4eval[3];
13014 sj8 = -1.0;
13015 cj8 = 0;
13016 j8 = -1.5707963267949;
13017 sj6 = 1.0;
13018 cj6 = 0;
13019 j6 = 1.5707963267949;
13020 IkReal x1237
13021 = (pp
13022 + (((-1.0)
13023 * (1.0)
13024 * (pz
13025 * pz))));
13026 IkReal x1238
13027 = (cj9 * px);
13028 IkReal x1239
13029 = (cj9 * py);
13030 IkReal x1240
13031 = ((3.92156862745098)
13032 * pp);
13033 IkReal x1241
13034 = ((0.045)
13035 * sj9);
13036 j4eval[0] = x1237;
13037 j4eval[1]
13038 = ((IKabs((
13039 ((px
13040 * x1240))
13041 + (((-1.0)
13042 * (1.51009803921569)
13043 * px))
13044 + (((0.55)
13045 * py))
13046 + (((0.3)
13047 * x1239))
13048 + ((py
13049 * x1241))
13050 + (((-1.32323529411765)
13051 * x1238)))))
13052 + (IKabs((
13053 ((px
13054 * x1241))
13055 + (((1.51009803921569)
13056 * py))
13057 + (((1.32323529411765)
13058 * x1239))
13059 + (((-1.0)
13060 * py
13061 * x1240))
13062 + (((0.55)
13063 * px))
13064 + (((0.3)
13065 * x1238))))));
13066 j4eval[2]
13067 = IKsign(
13068 x1237);
13069 if (IKabs(
13070 j4eval[0])
13071 < 0.0000010000000000
13072 || IKabs(
13073 j4eval
13074 [1])
13075 < 0.0000010000000000
13076 || IKabs(
13077 j4eval
13078 [2])
13079 < 0.0000010000000000)
13080 {
13081 {
13082 IkReal
13083 j4eval[3];
13084 sj8 = -1.0;
13085 cj8 = 0;
13086 j8 = -1.5707963267949;
13087 sj6 = 1.0;
13088 cj6 = 0;
13089 j6 = 1.5707963267949;
13090 IkReal x1242
13091 = (pp
13092 + (((-1.0)
13093 * (1.0)
13094 * (pz
13095 * pz))));
13096 IkReal x1243
13097 = (cj9
13098 * px);
13099 IkReal x1244
13100 = (cj9
13101 * py);
13102 IkReal x1245
13103 = (pp
13104 * px);
13105 IkReal x1246
13106 = (pp
13107 * py);
13108 j4eval[0]
13109 = x1242;
13110 j4eval[1]
13111 = ((IKabs((
13112 (((-1.32323529411765)
13113 * x1243))
13114 + (((0.316735294117647)
13115 * py))
13116 + (((-1.0)
13117 * (1.51009803921569)
13118 * px))
13119 + (((0.108264705882353)
13120 * x1244))
13121 + (((0.588235294117647)
13122 * x1246))
13123 + (((3.92156862745098)
13124 * x1245)))))
13125 + (IKabs((
13126 (((0.588235294117647)
13127 * x1245))
13128 + (((1.32323529411765)
13129 * x1244))
13130 + (((-3.92156862745098)
13131 * x1246))
13132 + (((0.316735294117647)
13133 * px))
13134 + (((1.51009803921569)
13135 * py))
13136 + (((0.108264705882353)
13137 * x1243))))));
13138 j4eval[2]
13139 = IKsign(
13140 x1242);
13141 if (IKabs(
13142 j4eval
13143 [0])
13144 < 0.0000010000000000
13145 || IKabs(
13146 j4eval
13147 [1])
13148 < 0.0000010000000000
13149 || IKabs(
13150 j4eval
13151 [2])
13152 < 0.0000010000000000)
13153 {
13154 {
13155 IkReal j4eval
13156 [3];
13157 sj8 = -1.0;
13158 cj8 = 0;
13159 j8 = -1.5707963267949;
13160 sj6 = 1.0;
13161 cj6 = 0;
13162 j6 = 1.5707963267949;
13163 IkReal
13164 x1247
13165 = pz
13166 * pz;
13167 IkReal
13168 x1248
13169 = (cj9
13170 * px);
13171 IkReal
13172 x1249
13173 = (cj9
13174 * py);
13175 IkReal
13176 x1250
13177 = (pp
13178 * px);
13179 IkReal
13180 x1251
13181 = (pp
13182 * py);
13183 j4eval[0]
13184 = (pp
13185 + (((-1.0)
13186 * x1247)));
13187 j4eval[1]
13188 = ((IKabs((
13189 (((0.348408823529412)
13190 * py))
13191 + (((0.119091176470588)
13192 * x1249))
13193 + (((0.647058823529412)
13194 * x1251))
13195 + (((-1.45555882352941)
13196 * x1248))
13197 + (((4.31372549019608)
13198 * x1250))
13199 + (((-1.0)
13200 * (1.66110784313725)
13201 * px)))))
13202 + (IKabs((
13203 (((1.45555882352941)
13204 * x1249))
13205 + (((0.348408823529412)
13206 * px))
13207 + (((1.66110784313725)
13208 * py))
13209 + (((0.647058823529412)
13210 * x1250))
13211 + (((0.119091176470588)
13212 * x1248))
13213 + (((-4.31372549019608)
13214 * x1251))))));
13215 j4eval[2] = IKsign((
13216 (((1.1)
13217 * pp))
13218 + (((-1.1)
13219 * x1247))));
13220 if (IKabs(
13221 j4eval
13222 [0])
13223 < 0.0000010000000000
13224 || IKabs(
13225 j4eval
13226 [1])
13227 < 0.0000010000000000
13228 || IKabs(
13229 j4eval
13230 [2])
13231 < 0.0000010000000000)
13232 {
13233 {
13234 IkReal evalcond
13235 [6];
13236 bool
13237 bgotonextstatement
13238 = true;
13239 do
13240 {
13241 IkReal
13242 x1252
13243 = ((1.32323529411765)
13244 * cj9);
13245 IkReal
13246 x1253
13247 = ((3.92156862745098)
13248 * pp);
13249 evalcond
13250 [0]
13251 = ((IKabs(
13252 py))
13253 + (IKabs(
13254 px)));
13255 evalcond
13256 [1]
13257 = ((-0.55)
13258 + (((-1.0)
13259 * (0.3)
13260 * cj9))
13261 + (((-1.0)
13262 * (0.045)
13263 * sj9)));
13264 evalcond
13265 [2]
13266 = ((1.51009803921569)
13267 + (((-1.0)
13268 * x1253))
13269 + x1252);
13270 evalcond
13271 [3]
13272 = ((-1.51009803921569)
13273 + (((-1.0)
13274 * x1252))
13275 + x1253);
13276 evalcond
13277 [4]
13278 = ((0.316735294117647)
13279 + (((0.108264705882353)
13280 * cj9))
13281 + (((0.588235294117647)
13282 * pp)));
13283 evalcond
13284 [5]
13285 = ((-0.2125)
13286 + (((-1.0)
13287 * (1.0)
13288 * pp)));
13289 if (IKabs(
13290 evalcond
13291 [0])
13292 < 0.0000010000000000
13293 && IKabs(
13294 evalcond
13295 [1])
13296 < 0.0000010000000000
13297 && IKabs(
13298 evalcond
13299 [2])
13300 < 0.0000010000000000
13301 && IKabs(
13302 evalcond
13303 [3])
13304 < 0.0000010000000000
13305 && IKabs(
13306 evalcond
13307 [4])
13308 < 0.0000010000000000
13309 && IKabs(
13310 evalcond
13311 [5])
13312 < 0.0000010000000000)
13313 {
13314 bgotonextstatement
13315 = false;
13316 {
13317 IkReal j4array
13318 [4],
13319 cj4array
13320 [4],
13321 sj4array
13322 [4];
13323 bool j4valid
13324 [4]
13325 = {false};
13326 _nj4
13327 = 4;
13328 j4array
13329 [0]
13330 = 0;
13331 sj4array
13332 [0]
13333 = IKsin(
13334 j4array
13335 [0]);
13336 cj4array
13337 [0]
13338 = IKcos(
13339 j4array
13340 [0]);
13341 j4array
13342 [1]
13343 = 1.5707963267949;
13344 sj4array
13345 [1]
13346 = IKsin(
13347 j4array
13348 [1]);
13349 cj4array
13350 [1]
13351 = IKcos(
13352 j4array
13353 [1]);
13354 j4array
13355 [2]
13356 = 3.14159265358979;
13357 sj4array
13358 [2]
13359 = IKsin(
13360 j4array
13361 [2]);
13362 cj4array
13363 [2]
13364 = IKcos(
13365 j4array
13366 [2]);
13367 j4array
13368 [3]
13369 = -1.5707963267949;
13370 sj4array
13371 [3]
13372 = IKsin(
13373 j4array
13374 [3]);
13375 cj4array
13376 [3]
13377 = IKcos(
13378 j4array
13379 [3]);
13380 if (j4array
13381 [0]
13382 > IKPI)
13383 {
13384 j4array
13385 [0]
13386 -= IK2PI;
13387 }
13388 else if (
13389 j4array
13390 [0]
13391 < -IKPI)
13392 {
13393 j4array
13394 [0]
13395 += IK2PI;
13396 }
13397 j4valid
13398 [0]
13399 = true;
13400 if (j4array
13401 [1]
13402 > IKPI)
13403 {
13404 j4array
13405 [1]
13406 -= IK2PI;
13407 }
13408 else if (
13409 j4array
13410 [1]
13411 < -IKPI)
13412 {
13413 j4array
13414 [1]
13415 += IK2PI;
13416 }
13417 j4valid
13418 [1]
13419 = true;
13420 if (j4array
13421 [2]
13422 > IKPI)
13423 {
13424 j4array
13425 [2]
13426 -= IK2PI;
13427 }
13428 else if (
13429 j4array
13430 [2]
13431 < -IKPI)
13432 {
13433 j4array
13434 [2]
13435 += IK2PI;
13436 }
13437 j4valid
13438 [2]
13439 = true;
13440 if (j4array
13441 [3]
13442 > IKPI)
13443 {
13444 j4array
13445 [3]
13446 -= IK2PI;
13447 }
13448 else if (
13449 j4array
13450 [3]
13451 < -IKPI)
13452 {
13453 j4array
13454 [3]
13455 += IK2PI;
13456 }
13457 j4valid
13458 [3]
13459 = true;
13460 for (
13461 int ij4
13462 = 0;
13463 ij4
13464 < 4;
13465 ++ij4)
13466 {
13467 if (!j4valid
13468 [ij4])
13469 {
13470 continue;
13471 }
13472 _ij4[0]
13473 = ij4;
13474 _ij4[1]
13475 = -1;
13476 for (
13477 int iij4
13478 = ij4
13479 + 1;
13480 iij4
13481 < 4;
13482 ++iij4)
13483 {
13484 if (j4valid
13485 [iij4]
13486 && IKabs(
13487 cj4array
13488 [ij4]
13489 - cj4array
13490 [iij4])
13491 < IKFAST_SOLUTION_THRESH
13492 && IKabs(
13493 sj4array
13494 [ij4]
13495 - sj4array
13496 [iij4])
13497 < IKFAST_SOLUTION_THRESH)
13498 {
13499 j4valid
13500 [iij4]
13501 = false;
13502 _ij4[1]
13503 = iij4;
13504 break;
13505 }
13506 }
13507 j4 = j4array
13508 [ij4];
13509 cj4 = cj4array
13510 [ij4];
13511 sj4 = sj4array
13512 [ij4];
13513
13514 rotationfunction0(
13515 solutions);
13516 }
13517 }
13518 }
13519 } while (
13520 0);
13521 if (bgotonextstatement)
13522 {
13523 bool
13524 bgotonextstatement
13525 = true;
13526 do
13527 {
13528 if (1)
13529 {
13530 bgotonextstatement
13531 = false;
13532 continue; // branch miss [j4]
13533 }
13534 } while (
13535 0);
13536 if (bgotonextstatement)
13537 {
13538 }
13539 }
13540 }
13541 }
13542 else
13543 {
13544 {
13545 IkReal j4array
13546 [1],
13547 cj4array
13548 [1],
13549 sj4array
13550 [1];
13551 bool j4valid
13552 [1]
13553 = {false};
13554 _nj4
13555 = 1;
13556 IkReal
13557 x1254
13558 = (cj9
13559 * px);
13560 IkReal
13561 x1255
13562 = (cj9
13563 * py);
13564 IkReal
13565 x1256
13566 = (pp
13567 * px);
13568 IkReal
13569 x1257
13570 = (pp
13571 * py);
13572 CheckValue<IkReal> x1258 = IKatan2WithCheck(
13573 IkReal((
13574 (((0.348408823529412)
13575 * py))
13576 + (((0.647058823529412)
13577 * x1257))
13578 + (((4.31372549019608)
13579 * x1256))
13580 + (((0.119091176470588)
13581 * x1255))
13582 + (((-1.0)
13583 * (1.66110784313725)
13584 * px))
13585 + (((-1.45555882352941)
13586 * x1254)))),
13587 ((((0.348408823529412)
13588 * px))
13589 + (((1.66110784313725)
13590 * py))
13591 + (((0.119091176470588)
13592 * x1254))
13593 + (((-4.31372549019608)
13594 * x1257))
13595 + (((0.647058823529412)
13596 * x1256))
13597 + (((1.45555882352941)
13598 * x1255))),
13599 IKFAST_ATAN2_MAGTHRESH);
13600 if (!x1258
13601 .valid)
13602 {
13603 continue;
13604 }
13605 CheckValue<IkReal> x1259 = IKPowWithIntegerCheck(
13606 IKsign((
13607 (((1.1)
13608 * pp))
13609 + (((-1.0)
13610 * (1.1)
13611 * (pz
13612 * pz))))),
13613 -1);
13614 if (!x1259
13615 .valid)
13616 {
13617 continue;
13618 }
13619 j4array
13620 [0]
13621 = ((-1.5707963267949)
13622 + (x1258
13623 .value)
13624 + (((1.5707963267949)
13625 * (x1259
13626 .value))));
13627 sj4array
13628 [0]
13629 = IKsin(
13630 j4array
13631 [0]);
13632 cj4array
13633 [0]
13634 = IKcos(
13635 j4array
13636 [0]);
13637 if (j4array
13638 [0]
13639 > IKPI)
13640 {
13641 j4array
13642 [0]
13643 -= IK2PI;
13644 }
13645 else if (
13646 j4array
13647 [0]
13648 < -IKPI)
13649 {
13650 j4array
13651 [0]
13652 += IK2PI;
13653 }
13654 j4valid
13655 [0]
13656 = true;
13657 for (
13658 int ij4
13659 = 0;
13660 ij4
13661 < 1;
13662 ++ij4)
13663 {
13664 if (!j4valid
13665 [ij4])
13666 {
13667 continue;
13668 }
13669 _ij4[0]
13670 = ij4;
13671 _ij4[1]
13672 = -1;
13673 for (
13674 int iij4
13675 = ij4
13676 + 1;
13677 iij4
13678 < 1;
13679 ++iij4)
13680 {
13681 if (j4valid
13682 [iij4]
13683 && IKabs(
13684 cj4array
13685 [ij4]
13686 - cj4array
13687 [iij4])
13688 < IKFAST_SOLUTION_THRESH
13689 && IKabs(
13690 sj4array
13691 [ij4]
13692 - sj4array
13693 [iij4])
13694 < IKFAST_SOLUTION_THRESH)
13695 {
13696 j4valid
13697 [iij4]
13698 = false;
13699 _ij4[1]
13700 = iij4;
13701 break;
13702 }
13703 }
13704 j4 = j4array
13705 [ij4];
13706 cj4 = cj4array
13707 [ij4];
13708 sj4 = sj4array
13709 [ij4];
13710 {
13711 IkReal evalcond
13712 [4];
13713 IkReal
13714 x1260
13715 = IKcos(
13716 j4);
13717 IkReal
13718 x1261
13719 = (px
13720 * x1260);
13721 IkReal
13722 x1262
13723 = IKsin(
13724 j4);
13725 IkReal
13726 x1263
13727 = (py
13728 * x1262);
13729 IkReal
13730 x1264
13731 = (px
13732 * x1262);
13733 IkReal
13734 x1265
13735 = (py
13736 * x1260);
13737 evalcond
13738 [0]
13739 = ((-0.55)
13740 + x1261
13741 + x1263
13742 + (((-1.0)
13743 * (0.3)
13744 * cj9))
13745 + (((-1.0)
13746 * (0.045)
13747 * sj9)));
13748 evalcond
13749 [1]
13750 = ((1.51009803921569)
13751 + (((-1.0)
13752 * (3.92156862745098)
13753 * pp))
13754 + (((-1.0)
13755 * x1265))
13756 + x1264
13757 + (((1.32323529411765)
13758 * cj9)));
13759 evalcond
13760 [2]
13761 = ((0.316735294117647)
13762 + (((-1.0)
13763 * x1261))
13764 + (((-1.0)
13765 * x1263))
13766 + (((0.108264705882353)
13767 * cj9))
13768 + (((0.588235294117647)
13769 * pp)));
13770 evalcond
13771 [3]
13772 = ((-0.2125)
13773 + (((0.09)
13774 * x1264))
13775 + (((-0.09)
13776 * x1265))
13777 + (((-1.0)
13778 * (1.0)
13779 * pp))
13780 + (((1.1)
13781 * x1263))
13782 + (((1.1)
13783 * x1261)));
13784 if (IKabs(
13785 evalcond
13786 [0])
13787 > IKFAST_EVALCOND_THRESH
13788 || IKabs(
13789 evalcond
13790 [1])
13791 > IKFAST_EVALCOND_THRESH
13792 || IKabs(
13793 evalcond
13794 [2])
13795 > IKFAST_EVALCOND_THRESH
13796 || IKabs(
13797 evalcond
13798 [3])
13799 > IKFAST_EVALCOND_THRESH)
13800 {
13801 continue;
13802 }
13803 }
13804
13805 rotationfunction0(
13806 solutions);
13807 }
13808 }
13809 }
13810 }
13811 }
13812 else
13813 {
13814 {
13815 IkReal j4array
13816 [1],
13817 cj4array
13818 [1],
13819 sj4array
13820 [1];
13821 bool j4valid
13822 [1]
13823 = {false};
13824 _nj4 = 1;
13825 IkReal
13826 x1266
13827 = (cj9
13828 * px);
13829 IkReal
13830 x1267
13831 = (cj9
13832 * py);
13833 IkReal
13834 x1268
13835 = (pp
13836 * px);
13837 IkReal
13838 x1269
13839 = (pp
13840 * py);
13841 CheckValue<IkReal> x1270 = IKatan2WithCheck(
13842 IkReal((
13843 (((0.316735294117647)
13844 * py))
13845 + (((0.108264705882353)
13846 * x1267))
13847 + (((-1.32323529411765)
13848 * x1266))
13849 + (((3.92156862745098)
13850 * x1268))
13851 + (((-1.0)
13852 * (1.51009803921569)
13853 * px))
13854 + (((0.588235294117647)
13855 * x1269)))),
13856 ((((0.588235294117647)
13857 * x1268))
13858 + (((1.32323529411765)
13859 * x1267))
13860 + (((0.316735294117647)
13861 * px))
13862 + (((1.51009803921569)
13863 * py))
13864 + (((-3.92156862745098)
13865 * x1269))
13866 + (((0.108264705882353)
13867 * x1266))),
13868 IKFAST_ATAN2_MAGTHRESH);
13869 if (!x1270
13870 .valid)
13871 {
13872 continue;
13873 }
13874 CheckValue<IkReal> x1271 = IKPowWithIntegerCheck(
13875 IKsign((
13876 pp
13877 + (((-1.0)
13878 * (1.0)
13879 * (pz
13880 * pz))))),
13881 -1);
13882 if (!x1271
13883 .valid)
13884 {
13885 continue;
13886 }
13887 j4array[0]
13888 = ((-1.5707963267949)
13889 + (x1270
13890 .value)
13891 + (((1.5707963267949)
13892 * (x1271
13893 .value))));
13894 sj4array[0] = IKsin(
13895 j4array
13896 [0]);
13897 cj4array[0] = IKcos(
13898 j4array
13899 [0]);
13900 if (j4array
13901 [0]
13902 > IKPI)
13903 {
13904 j4array
13905 [0]
13906 -= IK2PI;
13907 }
13908 else if (
13909 j4array
13910 [0]
13911 < -IKPI)
13912 {
13913 j4array
13914 [0]
13915 += IK2PI;
13916 }
13917 j4valid[0]
13918 = true;
13919 for (
13920 int ij4
13921 = 0;
13922 ij4
13923 < 1;
13924 ++ij4)
13925 {
13926 if (!j4valid
13927 [ij4])
13928 {
13929 continue;
13930 }
13931 _ij4[0]
13932 = ij4;
13933 _ij4[1]
13934 = -1;
13935 for (
13936 int iij4
13937 = ij4
13938 + 1;
13939 iij4
13940 < 1;
13941 ++iij4)
13942 {
13943 if (j4valid
13944 [iij4]
13945 && IKabs(
13946 cj4array
13947 [ij4]
13948 - cj4array
13949 [iij4])
13950 < IKFAST_SOLUTION_THRESH
13951 && IKabs(
13952 sj4array
13953 [ij4]
13954 - sj4array
13955 [iij4])
13956 < IKFAST_SOLUTION_THRESH)
13957 {
13958 j4valid
13959 [iij4]
13960 = false;
13961 _ij4[1]
13962 = iij4;
13963 break;
13964 }
13965 }
13966 j4 = j4array
13967 [ij4];
13968 cj4 = cj4array
13969 [ij4];
13970 sj4 = sj4array
13971 [ij4];
13972 {
13973 IkReal evalcond
13974 [4];
13975 IkReal
13976 x1272
13977 = IKcos(
13978 j4);
13979 IkReal
13980 x1273
13981 = (px
13982 * x1272);
13983 IkReal
13984 x1274
13985 = IKsin(
13986 j4);
13987 IkReal
13988 x1275
13989 = (py
13990 * x1274);
13991 IkReal
13992 x1276
13993 = (px
13994 * x1274);
13995 IkReal
13996 x1277
13997 = (py
13998 * x1272);
13999 evalcond
14000 [0]
14001 = ((-0.55)
14002 + x1275
14003 + x1273
14004 + (((-1.0)
14005 * (0.3)
14006 * cj9))
14007 + (((-1.0)
14008 * (0.045)
14009 * sj9)));
14010 evalcond
14011 [1]
14012 = ((1.51009803921569)
14013 + (((-1.0)
14014 * (3.92156862745098)
14015 * pp))
14016 + x1276
14017 + (((1.32323529411765)
14018 * cj9))
14019 + (((-1.0)
14020 * x1277)));
14021 evalcond
14022 [2]
14023 = ((0.316735294117647)
14024 + (((0.108264705882353)
14025 * cj9))
14026 + (((-1.0)
14027 * x1273))
14028 + (((0.588235294117647)
14029 * pp))
14030 + (((-1.0)
14031 * x1275)));
14032 evalcond
14033 [3]
14034 = ((-0.2125)
14035 + (((0.09)
14036 * x1276))
14037 + (((1.1)
14038 * x1275))
14039 + (((1.1)
14040 * x1273))
14041 + (((-1.0)
14042 * (1.0)
14043 * pp))
14044 + (((-0.09)
14045 * x1277)));
14046 if (IKabs(
14047 evalcond
14048 [0])
14049 > IKFAST_EVALCOND_THRESH
14050 || IKabs(
14051 evalcond
14052 [1])
14053 > IKFAST_EVALCOND_THRESH
14054 || IKabs(
14055 evalcond
14056 [2])
14057 > IKFAST_EVALCOND_THRESH
14058 || IKabs(
14059 evalcond
14060 [3])
14061 > IKFAST_EVALCOND_THRESH)
14062 {
14063 continue;
14064 }
14065 }
14066
14067 rotationfunction0(
14068 solutions);
14069 }
14070 }
14071 }
14072 }
14073 }
14074 else
14075 {
14076 {
14077 IkReal j4array
14078 [1],
14079 cj4array
14080 [1],
14081 sj4array
14082 [1];
14083 bool
14084 j4valid[1]
14085 = {false};
14086 _nj4 = 1;
14087 IkReal x1278
14088 = (cj9
14089 * px);
14090 IkReal x1279
14091 = (cj9
14092 * py);
14093 IkReal x1280
14094 = ((3.92156862745098)
14095 * pp);
14096 IkReal x1281
14097 = ((0.045)
14098 * sj9);
14099 CheckValue<IkReal> x1282 = IKPowWithIntegerCheck(
14100 IKsign((
14101 pp
14102 + (((-1.0)
14103 * (1.0)
14104 * (pz
14105 * pz))))),
14106 -1);
14107 if (!x1282
14108 .valid)
14109 {
14110 continue;
14111 }
14112 CheckValue<IkReal> x1283 = IKatan2WithCheck(
14113 IkReal((
14114 (((-1.0)
14115 * (1.51009803921569)
14116 * px))
14117 + (((-1.32323529411765)
14118 * x1278))
14119 + (((0.55)
14120 * py))
14121 + ((px
14122 * x1280))
14123 + ((py
14124 * x1281))
14125 + (((0.3)
14126 * x1279)))),
14127 ((((-1.0)
14128 * py
14129 * x1280))
14130 + (((1.32323529411765)
14131 * x1279))
14132 + (((1.51009803921569)
14133 * py))
14134 + (((0.3)
14135 * x1278))
14136 + (((0.55)
14137 * px))
14138 + ((px
14139 * x1281))),
14140 IKFAST_ATAN2_MAGTHRESH);
14141 if (!x1283
14142 .valid)
14143 {
14144 continue;
14145 }
14146 j4array[0]
14147 = ((-1.5707963267949)
14148 + (((1.5707963267949)
14149 * (x1282
14150 .value)))
14151 + (x1283
14152 .value));
14153 sj4array[0]
14154 = IKsin(
14155 j4array
14156 [0]);
14157 cj4array[0]
14158 = IKcos(
14159 j4array
14160 [0]);
14161 if (j4array[0]
14162 > IKPI)
14163 {
14164 j4array[0]
14165 -= IK2PI;
14166 }
14167 else if (
14168 j4array[0]
14169 < -IKPI)
14170 {
14171 j4array[0]
14172 += IK2PI;
14173 }
14174 j4valid[0]
14175 = true;
14176 for (int ij4
14177 = 0;
14178 ij4 < 1;
14179 ++ij4)
14180 {
14181 if (!j4valid
14182 [ij4])
14183 {
14184 continue;
14185 }
14186 _ij4[0]
14187 = ij4;
14188 _ij4[1]
14189 = -1;
14190 for (
14191 int iij4
14192 = ij4
14193 + 1;
14194 iij4
14195 < 1;
14196 ++iij4)
14197 {
14198 if (j4valid
14199 [iij4]
14200 && IKabs(
14201 cj4array
14202 [ij4]
14203 - cj4array
14204 [iij4])
14205 < IKFAST_SOLUTION_THRESH
14206 && IKabs(
14207 sj4array
14208 [ij4]
14209 - sj4array
14210 [iij4])
14211 < IKFAST_SOLUTION_THRESH)
14212 {
14213 j4valid
14214 [iij4]
14215 = false;
14216 _ij4[1]
14217 = iij4;
14218 break;
14219 }
14220 }
14221 j4 = j4array
14222 [ij4];
14223 cj4 = cj4array
14224 [ij4];
14225 sj4 = sj4array
14226 [ij4];
14227 {
14228 IkReal evalcond
14229 [4];
14230 IkReal
14231 x1284
14232 = IKcos(
14233 j4);
14234 IkReal
14235 x1285
14236 = (px
14237 * x1284);
14238 IkReal
14239 x1286
14240 = IKsin(
14241 j4);
14242 IkReal
14243 x1287
14244 = (py
14245 * x1286);
14246 IkReal
14247 x1288
14248 = (px
14249 * x1286);
14250 IkReal
14251 x1289
14252 = (py
14253 * x1284);
14254 evalcond
14255 [0]
14256 = ((-0.55)
14257 + x1287
14258 + x1285
14259 + (((-1.0)
14260 * (0.3)
14261 * cj9))
14262 + (((-1.0)
14263 * (0.045)
14264 * sj9)));
14265 evalcond
14266 [1]
14267 = ((1.51009803921569)
14268 + (((-1.0)
14269 * x1289))
14270 + (((-1.0)
14271 * (3.92156862745098)
14272 * pp))
14273 + x1288
14274 + (((1.32323529411765)
14275 * cj9)));
14276 evalcond
14277 [2]
14278 = ((0.316735294117647)
14279 + (((-1.0)
14280 * x1285))
14281 + (((0.108264705882353)
14282 * cj9))
14283 + (((0.588235294117647)
14284 * pp))
14285 + (((-1.0)
14286 * x1287)));
14287 evalcond
14288 [3]
14289 = ((-0.2125)
14290 + (((0.09)
14291 * x1288))
14292 + (((-0.09)
14293 * x1289))
14294 + (((1.1)
14295 * x1287))
14296 + (((-1.0)
14297 * (1.0)
14298 * pp))
14299 + (((1.1)
14300 * x1285)));
14301 if (IKabs(
14302 evalcond
14303 [0])
14304 > IKFAST_EVALCOND_THRESH
14305 || IKabs(
14306 evalcond
14307 [1])
14308 > IKFAST_EVALCOND_THRESH
14309 || IKabs(
14310 evalcond
14311 [2])
14312 > IKFAST_EVALCOND_THRESH
14313 || IKabs(
14314 evalcond
14315 [3])
14316 > IKFAST_EVALCOND_THRESH)
14317 {
14318 continue;
14319 }
14320 }
14321
14322 rotationfunction0(
14323 solutions);
14324 }
14325 }
14326 }
14327 }
14328 }
14329 } while (0);
14330 if (bgotonextstatement)
14331 {
14332 bool
14333 bgotonextstatement
14334 = true;
14335 do
14336 {
14337 evalcond[0]
14338 = ((-3.14159265358979)
14339 + (IKfmod(
14340 ((3.14159265358979)
14341 + (IKabs((
14342 (1.5707963267949)
14343 + j6)))),
14344 6.28318530717959)));
14345 evalcond[1]
14346 = ((-1.0)
14347 * (((1.0)
14348 * pz)));
14349 if (IKabs(
14350 evalcond[0])
14351 < 0.0000010000000000
14352 && IKabs(
14353 evalcond
14354 [1])
14355 < 0.0000010000000000)
14356 {
14357 bgotonextstatement
14358 = false;
14359 {
14360 IkReal
14361 j4eval[3];
14362 sj8 = -1.0;
14363 cj8 = 0;
14364 j8 = -1.5707963267949;
14365 sj6 = -1.0;
14366 cj6 = 0;
14367 j6 = -1.5707963267949;
14368 IkReal x1290
14369 = (pp
14370 + (((-1.0)
14371 * (1.0)
14372 * (pz
14373 * pz))));
14374 IkReal x1291
14375 = (cj9
14376 * px);
14377 IkReal x1292
14378 = (cj9
14379 * py);
14380 IkReal x1293
14381 = ((3.92156862745098)
14382 * pp);
14383 IkReal x1294
14384 = ((0.045)
14385 * sj9);
14386 j4eval[0]
14387 = x1290;
14388 j4eval[1]
14389 = ((IKabs((
14390 (((-1.32323529411765)
14391 * x1291))
14392 + ((px
14393 * x1293))
14394 + (((-1.0)
14395 * (1.51009803921569)
14396 * px))
14397 + (((-1.0)
14398 * py
14399 * x1294))
14400 + (((-0.3)
14401 * x1292))
14402 + (((-1.0)
14403 * (0.55)
14404 * py)))))
14405 + (IKabs((
14406 (((-1.0)
14407 * (0.55)
14408 * px))
14409 + (((-0.3)
14410 * x1291))
14411 + (((-1.0)
14412 * px
14413 * x1294))
14414 + (((1.51009803921569)
14415 * py))
14416 + (((-1.0)
14417 * py
14418 * x1293))
14419 + (((1.32323529411765)
14420 * x1292))))));
14421 j4eval[2]
14422 = IKsign(
14423 x1290);
14424 if (IKabs(
14425 j4eval
14426 [0])
14427 < 0.0000010000000000
14428 || IKabs(
14429 j4eval
14430 [1])
14431 < 0.0000010000000000
14432 || IKabs(
14433 j4eval
14434 [2])
14435 < 0.0000010000000000)
14436 {
14437 {
14438 IkReal
14439 j4eval
14440 [3];
14441 sj8 = -1.0;
14442 cj8 = 0;
14443 j8 = -1.5707963267949;
14444 sj6 = -1.0;
14445 cj6 = 0;
14446 j6 = -1.5707963267949;
14447 IkReal x1295
14448 = (pp
14449 + (((-1.0)
14450 * (1.0)
14451 * (pz
14452 * pz))));
14453 IkReal x1296
14454 = (cj9
14455 * px);
14456 IkReal x1297
14457 = (cj9
14458 * py);
14459 IkReal x1298
14460 = (pp
14461 * px);
14462 IkReal x1299
14463 = (pp
14464 * py);
14465 j4eval[0]
14466 = x1295;
14467 j4eval[1]
14468 = ((IKabs((
14469 (((-3.92156862745098)
14470 * x1299))
14471 + (((1.51009803921569)
14472 * py))
14473 + (((-0.108264705882353)
14474 * x1296))
14475 + (((-1.0)
14476 * (0.316735294117647)
14477 * px))
14478 + (((1.32323529411765)
14479 * x1297))
14480 + (((-0.588235294117647)
14481 * x1298)))))
14482 + (IKabs((
14483 (((-0.588235294117647)
14484 * x1299))
14485 + (((-1.32323529411765)
14486 * x1296))
14487 + (((-1.0)
14488 * (1.51009803921569)
14489 * px))
14490 + (((-1.0)
14491 * (0.316735294117647)
14492 * py))
14493 + (((-0.108264705882353)
14494 * x1297))
14495 + (((3.92156862745098)
14496 * x1298))))));
14497 j4eval[2]
14498 = IKsign(
14499 x1295);
14500 if (IKabs(
14501 j4eval
14502 [0])
14503 < 0.0000010000000000
14504 || IKabs(
14505 j4eval
14506 [1])
14507 < 0.0000010000000000
14508 || IKabs(
14509 j4eval
14510 [2])
14511 < 0.0000010000000000)
14512 {
14513 {
14514 IkReal j4eval
14515 [3];
14516 sj8 = -1.0;
14517 cj8 = 0;
14518 j8 = -1.5707963267949;
14519 sj6 = -1.0;
14520 cj6 = 0;
14521 j6 = -1.5707963267949;
14522 IkReal
14523 x1300
14524 = pz
14525 * pz;
14526 IkReal
14527 x1301
14528 = (cj9
14529 * px);
14530 IkReal
14531 x1302
14532 = (cj9
14533 * py);
14534 IkReal
14535 x1303
14536 = (pp
14537 * px);
14538 IkReal
14539 x1304
14540 = (pp
14541 * py);
14542 j4eval
14543 [0]
14544 = (x1300
14545 + (((-1.0)
14546 * (1.0)
14547 * pp)));
14548 j4eval[1] = IKsign((
14549 (((1.1)
14550 * x1300))
14551 + (((-1.0)
14552 * (1.1)
14553 * pp))));
14554 j4eval
14555 [2]
14556 = ((IKabs((
14557 (((4.31372549019608)
14558 * x1304))
14559 + (((0.348408823529412)
14560 * px))
14561 + (((0.119091176470588)
14562 * x1301))
14563 + (((-1.0)
14564 * (1.66110784313725)
14565 * py))
14566 + (((0.647058823529412)
14567 * x1303))
14568 + (((-1.45555882352941)
14569 * x1302)))))
14570 + (IKabs((
14571 (((0.348408823529412)
14572 * py))
14573 + (((1.66110784313725)
14574 * px))
14575 + (((1.45555882352941)
14576 * x1301))
14577 + (((-4.31372549019608)
14578 * x1303))
14579 + (((0.647058823529412)
14580 * x1304))
14581 + (((0.119091176470588)
14582 * x1302))))));
14583 if (IKabs(
14584 j4eval
14585 [0])
14586 < 0.0000010000000000
14587 || IKabs(
14588 j4eval
14589 [1])
14590 < 0.0000010000000000
14591 || IKabs(
14592 j4eval
14593 [2])
14594 < 0.0000010000000000)
14595 {
14596 {
14597 IkReal evalcond
14598 [6];
14599 bool
14600 bgotonextstatement
14601 = true;
14602 do
14603 {
14604 IkReal
14605 x1305
14606 = ((1.32323529411765)
14607 * cj9);
14608 IkReal
14609 x1306
14610 = ((3.92156862745098)
14611 * pp);
14612 evalcond
14613 [0]
14614 = ((IKabs(
14615 py))
14616 + (IKabs(
14617 px)));
14618 evalcond
14619 [1]
14620 = ((-0.55)
14621 + (((-1.0)
14622 * (0.3)
14623 * cj9))
14624 + (((-1.0)
14625 * (0.045)
14626 * sj9)));
14627 evalcond
14628 [2]
14629 = ((1.51009803921569)
14630 + (((-1.0)
14631 * x1306))
14632 + x1305);
14633 evalcond
14634 [3]
14635 = ((-1.51009803921569)
14636 + x1306
14637 + (((-1.0)
14638 * x1305)));
14639 evalcond
14640 [4]
14641 = ((-0.316735294117647)
14642 + (((-1.0)
14643 * (0.588235294117647)
14644 * pp))
14645 + (((-1.0)
14646 * (0.108264705882353)
14647 * cj9)));
14648 evalcond
14649 [5]
14650 = ((-0.2125)
14651 + (((-1.0)
14652 * (1.0)
14653 * pp)));
14654 if (IKabs(
14655 evalcond
14656 [0])
14657 < 0.0000010000000000
14658 && IKabs(
14659 evalcond
14660 [1])
14661 < 0.0000010000000000
14662 && IKabs(
14663 evalcond
14664 [2])
14665 < 0.0000010000000000
14666 && IKabs(
14667 evalcond
14668 [3])
14669 < 0.0000010000000000
14670 && IKabs(
14671 evalcond
14672 [4])
14673 < 0.0000010000000000
14674 && IKabs(
14675 evalcond
14676 [5])
14677 < 0.0000010000000000)
14678 {
14679 bgotonextstatement
14680 = false;
14681 {
14682 IkReal j4array
14683 [4],
14684 cj4array
14685 [4],
14686 sj4array
14687 [4];
14688 bool j4valid
14689 [4]
14690 = {false};
14691 _nj4
14692 = 4;
14693 j4array
14694 [0]
14695 = 0;
14696 sj4array
14697 [0]
14698 = IKsin(
14699 j4array
14700 [0]);
14701 cj4array
14702 [0]
14703 = IKcos(
14704 j4array
14705 [0]);
14706 j4array
14707 [1]
14708 = 1.5707963267949;
14709 sj4array
14710 [1]
14711 = IKsin(
14712 j4array
14713 [1]);
14714 cj4array
14715 [1]
14716 = IKcos(
14717 j4array
14718 [1]);
14719 j4array
14720 [2]
14721 = 3.14159265358979;
14722 sj4array
14723 [2]
14724 = IKsin(
14725 j4array
14726 [2]);
14727 cj4array
14728 [2]
14729 = IKcos(
14730 j4array
14731 [2]);
14732 j4array
14733 [3]
14734 = -1.5707963267949;
14735 sj4array
14736 [3]
14737 = IKsin(
14738 j4array
14739 [3]);
14740 cj4array
14741 [3]
14742 = IKcos(
14743 j4array
14744 [3]);
14745 if (j4array
14746 [0]
14747 > IKPI)
14748 {
14749 j4array
14750 [0]
14751 -= IK2PI;
14752 }
14753 else if (
14754 j4array
14755 [0]
14756 < -IKPI)
14757 {
14758 j4array
14759 [0]
14760 += IK2PI;
14761 }
14762 j4valid
14763 [0]
14764 = true;
14765 if (j4array
14766 [1]
14767 > IKPI)
14768 {
14769 j4array
14770 [1]
14771 -= IK2PI;
14772 }
14773 else if (
14774 j4array
14775 [1]
14776 < -IKPI)
14777 {
14778 j4array
14779 [1]
14780 += IK2PI;
14781 }
14782 j4valid
14783 [1]
14784 = true;
14785 if (j4array
14786 [2]
14787 > IKPI)
14788 {
14789 j4array
14790 [2]
14791 -= IK2PI;
14792 }
14793 else if (
14794 j4array
14795 [2]
14796 < -IKPI)
14797 {
14798 j4array
14799 [2]
14800 += IK2PI;
14801 }
14802 j4valid
14803 [2]
14804 = true;
14805 if (j4array
14806 [3]
14807 > IKPI)
14808 {
14809 j4array
14810 [3]
14811 -= IK2PI;
14812 }
14813 else if (
14814 j4array
14815 [3]
14816 < -IKPI)
14817 {
14818 j4array
14819 [3]
14820 += IK2PI;
14821 }
14822 j4valid
14823 [3]
14824 = true;
14825 for (
14826 int ij4
14827 = 0;
14828 ij4
14829 < 4;
14830 ++ij4)
14831 {
14832 if (!j4valid
14833 [ij4])
14834 {
14835 continue;
14836 }
14837 _ij4[0]
14838 = ij4;
14839 _ij4[1]
14840 = -1;
14841 for (
14842 int iij4
14843 = ij4
14844 + 1;
14845 iij4
14846 < 4;
14847 ++iij4)
14848 {
14849 if (j4valid
14850 [iij4]
14851 && IKabs(
14852 cj4array
14853 [ij4]
14854 - cj4array
14855 [iij4])
14856 < IKFAST_SOLUTION_THRESH
14857 && IKabs(
14858 sj4array
14859 [ij4]
14860 - sj4array
14861 [iij4])
14862 < IKFAST_SOLUTION_THRESH)
14863 {
14864 j4valid
14865 [iij4]
14866 = false;
14867 _ij4[1]
14868 = iij4;
14869 break;
14870 }
14871 }
14872 j4 = j4array
14873 [ij4];
14874 cj4 = cj4array
14875 [ij4];
14876 sj4 = sj4array
14877 [ij4];
14878
14879 rotationfunction0(
14880 solutions);
14881 }
14882 }
14883 }
14884 } while (
14885 0);
14886 if (bgotonextstatement)
14887 {
14888 bool
14889 bgotonextstatement
14890 = true;
14891 do
14892 {
14893 if (1)
14894 {
14895 bgotonextstatement
14896 = false;
14897 continue; // branch miss [j4]
14898 }
14899 } while (
14900 0);
14901 if (bgotonextstatement)
14902 {
14903 }
14904 }
14905 }
14906 }
14907 else
14908 {
14909 {
14910 IkReal j4array
14911 [1],
14912 cj4array
14913 [1],
14914 sj4array
14915 [1];
14916 bool j4valid
14917 [1]
14918 = {false};
14919 _nj4
14920 = 1;
14921 IkReal
14922 x1307
14923 = (cj9
14924 * px);
14925 IkReal
14926 x1308
14927 = (cj9
14928 * py);
14929 IkReal
14930 x1309
14931 = (pp
14932 * px);
14933 IkReal
14934 x1310
14935 = (pp
14936 * py);
14937 CheckValue<IkReal> x1311 = IKPowWithIntegerCheck(
14938 IKsign((
14939 (((1.1)
14940 * (pz
14941 * pz)))
14942 + (((-1.0)
14943 * (1.1)
14944 * pp)))),
14945 -1);
14946 if (!x1311
14947 .valid)
14948 {
14949 continue;
14950 }
14951 CheckValue<IkReal> x1312 = IKatan2WithCheck(
14952 IkReal((
14953 (((1.45555882352941)
14954 * x1307))
14955 + (((0.348408823529412)
14956 * py))
14957 + (((1.66110784313725)
14958 * px))
14959 + (((0.119091176470588)
14960 * x1308))
14961 + (((-4.31372549019608)
14962 * x1309))
14963 + (((0.647058823529412)
14964 * x1310)))),
14965 ((((0.647058823529412)
14966 * x1309))
14967 + (((0.348408823529412)
14968 * px))
14969 + (((4.31372549019608)
14970 * x1310))
14971 + (((-1.45555882352941)
14972 * x1308))
14973 + (((0.119091176470588)
14974 * x1307))
14975 + (((-1.0)
14976 * (1.66110784313725)
14977 * py))),
14978 IKFAST_ATAN2_MAGTHRESH);
14979 if (!x1312
14980 .valid)
14981 {
14982 continue;
14983 }
14984 j4array
14985 [0]
14986 = ((-1.5707963267949)
14987 + (((1.5707963267949)
14988 * (x1311
14989 .value)))
14990 + (x1312
14991 .value));
14992 sj4array
14993 [0]
14994 = IKsin(
14995 j4array
14996 [0]);
14997 cj4array
14998 [0]
14999 = IKcos(
15000 j4array
15001 [0]);
15002 if (j4array
15003 [0]
15004 > IKPI)
15005 {
15006 j4array
15007 [0]
15008 -= IK2PI;
15009 }
15010 else if (
15011 j4array
15012 [0]
15013 < -IKPI)
15014 {
15015 j4array
15016 [0]
15017 += IK2PI;
15018 }
15019 j4valid
15020 [0]
15021 = true;
15022 for (
15023 int ij4
15024 = 0;
15025 ij4
15026 < 1;
15027 ++ij4)
15028 {
15029 if (!j4valid
15030 [ij4])
15031 {
15032 continue;
15033 }
15034 _ij4[0]
15035 = ij4;
15036 _ij4[1]
15037 = -1;
15038 for (
15039 int iij4
15040 = ij4
15041 + 1;
15042 iij4
15043 < 1;
15044 ++iij4)
15045 {
15046 if (j4valid
15047 [iij4]
15048 && IKabs(
15049 cj4array
15050 [ij4]
15051 - cj4array
15052 [iij4])
15053 < IKFAST_SOLUTION_THRESH
15054 && IKabs(
15055 sj4array
15056 [ij4]
15057 - sj4array
15058 [iij4])
15059 < IKFAST_SOLUTION_THRESH)
15060 {
15061 j4valid
15062 [iij4]
15063 = false;
15064 _ij4[1]
15065 = iij4;
15066 break;
15067 }
15068 }
15069 j4 = j4array
15070 [ij4];
15071 cj4 = cj4array
15072 [ij4];
15073 sj4 = sj4array
15074 [ij4];
15075 {
15076 IkReal evalcond
15077 [4];
15078 IkReal
15079 x1313
15080 = IKsin(
15081 j4);
15082 IkReal
15083 x1314
15084 = (px
15085 * x1313);
15086 IkReal
15087 x1315
15088 = IKcos(
15089 j4);
15090 IkReal
15091 x1316
15092 = (py
15093 * x1315);
15094 IkReal
15095 x1317
15096 = (px
15097 * x1315);
15098 IkReal
15099 x1318
15100 = (py
15101 * x1313);
15102 IkReal
15103 x1319
15104 = ((((-1.0)
15105 * x1318))
15106 + (((-1.0)
15107 * x1317)));
15108 evalcond
15109 [0]
15110 = ((1.51009803921569)
15111 + (((-1.0)
15112 * (3.92156862745098)
15113 * pp))
15114 + (((-1.0)
15115 * x1316))
15116 + (((1.32323529411765)
15117 * cj9))
15118 + x1314);
15119 evalcond
15120 [1]
15121 = ((-0.55)
15122 + (((-1.0)
15123 * (0.3)
15124 * cj9))
15125 + (((-1.0)
15126 * (0.045)
15127 * sj9))
15128 + x1319);
15129 evalcond
15130 [2]
15131 = ((-0.316735294117647)
15132 + (((-1.0)
15133 * (0.588235294117647)
15134 * pp))
15135 + (((-1.0)
15136 * (0.108264705882353)
15137 * cj9))
15138 + x1319);
15139 evalcond
15140 [3]
15141 = ((-0.2125)
15142 + (((-1.1)
15143 * x1317))
15144 + (((-1.0)
15145 * (1.0)
15146 * pp))
15147 + (((0.09)
15148 * x1314))
15149 + (((-0.09)
15150 * x1316))
15151 + (((-1.1)
15152 * x1318)));
15153 if (IKabs(
15154 evalcond
15155 [0])
15156 > IKFAST_EVALCOND_THRESH
15157 || IKabs(
15158 evalcond
15159 [1])
15160 > IKFAST_EVALCOND_THRESH
15161 || IKabs(
15162 evalcond
15163 [2])
15164 > IKFAST_EVALCOND_THRESH
15165 || IKabs(
15166 evalcond
15167 [3])
15168 > IKFAST_EVALCOND_THRESH)
15169 {
15170 continue;
15171 }
15172 }
15173
15174 rotationfunction0(
15175 solutions);
15176 }
15177 }
15178 }
15179 }
15180 }
15181 else
15182 {
15183 {
15184 IkReal j4array
15185 [1],
15186 cj4array
15187 [1],
15188 sj4array
15189 [1];
15190 bool j4valid
15191 [1]
15192 = {false};
15193 _nj4
15194 = 1;
15195 IkReal
15196 x1320
15197 = (cj9
15198 * px);
15199 IkReal
15200 x1321
15201 = (cj9
15202 * py);
15203 IkReal
15204 x1322
15205 = (pp
15206 * px);
15207 IkReal
15208 x1323
15209 = (pp
15210 * py);
15211 CheckValue<IkReal> x1324 = IKPowWithIntegerCheck(
15212 IKsign((
15213 pp
15214 + (((-1.0)
15215 * (1.0)
15216 * (pz
15217 * pz))))),
15218 -1);
15219 if (!x1324
15220 .valid)
15221 {
15222 continue;
15223 }
15224 CheckValue<IkReal> x1325 = IKatan2WithCheck(
15225 IkReal((
15226 (((-0.108264705882353)
15227 * x1321))
15228 + (((-1.0)
15229 * (1.51009803921569)
15230 * px))
15231 + (((-1.32323529411765)
15232 * x1320))
15233 + (((-1.0)
15234 * (0.316735294117647)
15235 * py))
15236 + (((3.92156862745098)
15237 * x1322))
15238 + (((-0.588235294117647)
15239 * x1323)))),
15240 ((((-3.92156862745098)
15241 * x1323))
15242 + (((-0.588235294117647)
15243 * x1322))
15244 + (((-0.108264705882353)
15245 * x1320))
15246 + (((1.51009803921569)
15247 * py))
15248 + (((1.32323529411765)
15249 * x1321))
15250 + (((-1.0)
15251 * (0.316735294117647)
15252 * px))),
15253 IKFAST_ATAN2_MAGTHRESH);
15254 if (!x1325
15255 .valid)
15256 {
15257 continue;
15258 }
15259 j4array
15260 [0]
15261 = ((-1.5707963267949)
15262 + (((1.5707963267949)
15263 * (x1324
15264 .value)))
15265 + (x1325
15266 .value));
15267 sj4array
15268 [0]
15269 = IKsin(
15270 j4array
15271 [0]);
15272 cj4array
15273 [0]
15274 = IKcos(
15275 j4array
15276 [0]);
15277 if (j4array
15278 [0]
15279 > IKPI)
15280 {
15281 j4array
15282 [0]
15283 -= IK2PI;
15284 }
15285 else if (
15286 j4array
15287 [0]
15288 < -IKPI)
15289 {
15290 j4array
15291 [0]
15292 += IK2PI;
15293 }
15294 j4valid
15295 [0]
15296 = true;
15297 for (
15298 int ij4
15299 = 0;
15300 ij4
15301 < 1;
15302 ++ij4)
15303 {
15304 if (!j4valid
15305 [ij4])
15306 {
15307 continue;
15308 }
15309 _ij4[0]
15310 = ij4;
15311 _ij4[1]
15312 = -1;
15313 for (
15314 int iij4
15315 = ij4
15316 + 1;
15317 iij4
15318 < 1;
15319 ++iij4)
15320 {
15321 if (j4valid
15322 [iij4]
15323 && IKabs(
15324 cj4array
15325 [ij4]
15326 - cj4array
15327 [iij4])
15328 < IKFAST_SOLUTION_THRESH
15329 && IKabs(
15330 sj4array
15331 [ij4]
15332 - sj4array
15333 [iij4])
15334 < IKFAST_SOLUTION_THRESH)
15335 {
15336 j4valid
15337 [iij4]
15338 = false;
15339 _ij4[1]
15340 = iij4;
15341 break;
15342 }
15343 }
15344 j4 = j4array
15345 [ij4];
15346 cj4 = cj4array
15347 [ij4];
15348 sj4 = sj4array
15349 [ij4];
15350 {
15351 IkReal evalcond
15352 [4];
15353 IkReal
15354 x1326
15355 = IKsin(
15356 j4);
15357 IkReal
15358 x1327
15359 = (px
15360 * x1326);
15361 IkReal
15362 x1328
15363 = IKcos(
15364 j4);
15365 IkReal
15366 x1329
15367 = (py
15368 * x1328);
15369 IkReal
15370 x1330
15371 = (px
15372 * x1328);
15373 IkReal
15374 x1331
15375 = (py
15376 * x1326);
15377 IkReal
15378 x1332
15379 = ((((-1.0)
15380 * x1330))
15381 + (((-1.0)
15382 * x1331)));
15383 evalcond
15384 [0]
15385 = ((1.51009803921569)
15386 + x1327
15387 + (((-1.0)
15388 * x1329))
15389 + (((-1.0)
15390 * (3.92156862745098)
15391 * pp))
15392 + (((1.32323529411765)
15393 * cj9)));
15394 evalcond
15395 [1]
15396 = ((-0.55)
15397 + x1332
15398 + (((-1.0)
15399 * (0.3)
15400 * cj9))
15401 + (((-1.0)
15402 * (0.045)
15403 * sj9)));
15404 evalcond
15405 [2]
15406 = ((-0.316735294117647)
15407 + x1332
15408 + (((-1.0)
15409 * (0.588235294117647)
15410 * pp))
15411 + (((-1.0)
15412 * (0.108264705882353)
15413 * cj9)));
15414 evalcond
15415 [3]
15416 = ((-0.2125)
15417 + (((-1.1)
15418 * x1330))
15419 + (((0.09)
15420 * x1327))
15421 + (((-0.09)
15422 * x1329))
15423 + (((-1.0)
15424 * (1.0)
15425 * pp))
15426 + (((-1.1)
15427 * x1331)));
15428 if (IKabs(
15429 evalcond
15430 [0])
15431 > IKFAST_EVALCOND_THRESH
15432 || IKabs(
15433 evalcond
15434 [1])
15435 > IKFAST_EVALCOND_THRESH
15436 || IKabs(
15437 evalcond
15438 [2])
15439 > IKFAST_EVALCOND_THRESH
15440 || IKabs(
15441 evalcond
15442 [3])
15443 > IKFAST_EVALCOND_THRESH)
15444 {
15445 continue;
15446 }
15447 }
15448
15449 rotationfunction0(
15450 solutions);
15451 }
15452 }
15453 }
15454 }
15455 }
15456 else
15457 {
15458 {
15459 IkReal
15460 j4array
15461 [1],
15462 cj4array
15463 [1],
15464 sj4array
15465 [1];
15466 bool j4valid
15467 [1]
15468 = {false};
15469 _nj4 = 1;
15470 IkReal x1333
15471 = (cj9
15472 * px);
15473 IkReal x1334
15474 = (cj9
15475 * py);
15476 IkReal x1335
15477 = ((3.92156862745098)
15478 * pp);
15479 IkReal x1336
15480 = ((0.045)
15481 * sj9);
15482 CheckValue<IkReal> x1337 = IKatan2WithCheck(
15483 IkReal((
15484 (((-1.32323529411765)
15485 * x1333))
15486 + (((-0.3)
15487 * x1334))
15488 + ((px
15489 * x1335))
15490 + (((-1.0)
15491 * (1.51009803921569)
15492 * px))
15493 + (((-1.0)
15494 * py
15495 * x1336))
15496 + (((-1.0)
15497 * (0.55)
15498 * py)))),
15499 ((((-1.0)
15500 * (0.55)
15501 * px))
15502 + (((-1.0)
15503 * py
15504 * x1335))
15505 + (((-1.0)
15506 * px
15507 * x1336))
15508 + (((1.51009803921569)
15509 * py))
15510 + (((-0.3)
15511 * x1333))
15512 + (((1.32323529411765)
15513 * x1334))),
15514 IKFAST_ATAN2_MAGTHRESH);
15515 if (!x1337
15516 .valid)
15517 {
15518 continue;
15519 }
15520 CheckValue<IkReal> x1338 = IKPowWithIntegerCheck(
15521 IKsign((
15522 pp
15523 + (((-1.0)
15524 * (1.0)
15525 * (pz
15526 * pz))))),
15527 -1);
15528 if (!x1338
15529 .valid)
15530 {
15531 continue;
15532 }
15533 j4array[0]
15534 = ((-1.5707963267949)
15535 + (x1337
15536 .value)
15537 + (((1.5707963267949)
15538 * (x1338
15539 .value))));
15540 sj4array[0] = IKsin(
15541 j4array
15542 [0]);
15543 cj4array[0] = IKcos(
15544 j4array
15545 [0]);
15546 if (j4array
15547 [0]
15548 > IKPI)
15549 {
15550 j4array[0]
15551 -= IK2PI;
15552 }
15553 else if (
15554 j4array
15555 [0]
15556 < -IKPI)
15557 {
15558 j4array[0]
15559 += IK2PI;
15560 }
15561 j4valid[0]
15562 = true;
15563 for (int ij4
15564 = 0;
15565 ij4
15566 < 1;
15567 ++ij4)
15568 {
15569 if (!j4valid
15570 [ij4])
15571 {
15572 continue;
15573 }
15574 _ij4[0]
15575 = ij4;
15576 _ij4[1]
15577 = -1;
15578 for (
15579 int iij4
15580 = ij4
15581 + 1;
15582 iij4
15583 < 1;
15584 ++iij4)
15585 {
15586 if (j4valid
15587 [iij4]
15588 && IKabs(
15589 cj4array
15590 [ij4]
15591 - cj4array
15592 [iij4])
15593 < IKFAST_SOLUTION_THRESH
15594 && IKabs(
15595 sj4array
15596 [ij4]
15597 - sj4array
15598 [iij4])
15599 < IKFAST_SOLUTION_THRESH)
15600 {
15601 j4valid
15602 [iij4]
15603 = false;
15604 _ij4[1]
15605 = iij4;
15606 break;
15607 }
15608 }
15609 j4 = j4array
15610 [ij4];
15611 cj4 = cj4array
15612 [ij4];
15613 sj4 = sj4array
15614 [ij4];
15615 {
15616 IkReal evalcond
15617 [4];
15618 IkReal
15619 x1339
15620 = IKsin(
15621 j4);
15622 IkReal
15623 x1340
15624 = (px
15625 * x1339);
15626 IkReal
15627 x1341
15628 = IKcos(
15629 j4);
15630 IkReal
15631 x1342
15632 = (py
15633 * x1341);
15634 IkReal
15635 x1343
15636 = (px
15637 * x1341);
15638 IkReal
15639 x1344
15640 = (py
15641 * x1339);
15642 IkReal
15643 x1345
15644 = ((((-1.0)
15645 * x1343))
15646 + (((-1.0)
15647 * x1344)));
15648 evalcond
15649 [0]
15650 = ((1.51009803921569)
15651 + (((-1.0)
15652 * (3.92156862745098)
15653 * pp))
15654 + (((-1.0)
15655 * x1342))
15656 + x1340
15657 + (((1.32323529411765)
15658 * cj9)));
15659 evalcond
15660 [1]
15661 = ((-0.55)
15662 + (((-1.0)
15663 * (0.3)
15664 * cj9))
15665 + (((-1.0)
15666 * (0.045)
15667 * sj9))
15668 + x1345);
15669 evalcond
15670 [2]
15671 = ((-0.316735294117647)
15672 + (((-1.0)
15673 * (0.588235294117647)
15674 * pp))
15675 + (((-1.0)
15676 * (0.108264705882353)
15677 * cj9))
15678 + x1345);
15679 evalcond
15680 [3]
15681 = ((-0.2125)
15682 + (((-1.1)
15683 * x1344))
15684 + (((-1.1)
15685 * x1343))
15686 + (((0.09)
15687 * x1340))
15688 + (((-0.09)
15689 * x1342))
15690 + (((-1.0)
15691 * (1.0)
15692 * pp)));
15693 if (IKabs(
15694 evalcond
15695 [0])
15696 > IKFAST_EVALCOND_THRESH
15697 || IKabs(
15698 evalcond
15699 [1])
15700 > IKFAST_EVALCOND_THRESH
15701 || IKabs(
15702 evalcond
15703 [2])
15704 > IKFAST_EVALCOND_THRESH
15705 || IKabs(
15706 evalcond
15707 [3])
15708 > IKFAST_EVALCOND_THRESH)
15709 {
15710 continue;
15711 }
15712 }
15713
15714 rotationfunction0(
15715 solutions);
15716 }
15717 }
15718 }
15719 }
15720 }
15721 } while (0);
15722 if (bgotonextstatement)
15723 {
15724 bool
15725 bgotonextstatement
15726 = true;
15727 do
15728 {
15729 IkReal x1346
15730 = (cj6 * pz);
15731 IkReal x1347
15732 = ((1.32323529411765)
15733 * cj9);
15734 IkReal x1348
15735 = ((3.92156862745098)
15736 * pp);
15737 evalcond[0]
15738 = ((IKabs(py))
15739 + (IKabs(
15740 px)));
15741 evalcond[1]
15742 = ((-0.55)
15743 + (((-1.0)
15744 * (0.3)
15745 * cj9))
15746 + (((-1.0)
15747 * (0.045)
15748 * sj9))
15749 + x1346);
15750 evalcond[2]
15751 = ((1.51009803921569)
15752 + (((-1.0)
15753 * x1348))
15754 + x1347);
15755 evalcond[3]
15756 = (pz * sj6);
15757 evalcond[4]
15758 = ((-1.51009803921569)
15759 + (((-1.0)
15760 * x1347))
15761 + x1348);
15762 evalcond[5]
15763 = ((((0.108264705882353)
15764 * cj9
15765 * sj6))
15766 + (((0.588235294117647)
15767 * pp
15768 * sj6))
15769 + (((0.316735294117647)
15770 * sj6)));
15771 evalcond[6]
15772 = ((-0.2125)
15773 + (((1.1)
15774 * x1346))
15775 + (((-1.0)
15776 * (1.0)
15777 * pp)));
15778 if (IKabs(evalcond
15779 [0])
15780 < 0.0000010000000000
15781 && IKabs(
15782 evalcond
15783 [1])
15784 < 0.0000010000000000
15785 && IKabs(
15786 evalcond
15787 [2])
15788 < 0.0000010000000000
15789 && IKabs(
15790 evalcond
15791 [3])
15792 < 0.0000010000000000
15793 && IKabs(
15794 evalcond
15795 [4])
15796 < 0.0000010000000000
15797 && IKabs(
15798 evalcond
15799 [5])
15800 < 0.0000010000000000
15801 && IKabs(
15802 evalcond
15803 [6])
15804 < 0.0000010000000000)
15805 {
15806 bgotonextstatement
15807 = false;
15808 {
15809 IkReal j4array
15810 [4],
15811 cj4array
15812 [4],
15813 sj4array
15814 [4];
15815 bool
15816 j4valid[4]
15817 = {false};
15818 _nj4 = 4;
15819 j4array[0]
15820 = 0;
15821 sj4array[0]
15822 = IKsin(
15823 j4array
15824 [0]);
15825 cj4array[0]
15826 = IKcos(
15827 j4array
15828 [0]);
15829 j4array[1]
15830 = 1.5707963267949;
15831 sj4array[1]
15832 = IKsin(
15833 j4array
15834 [1]);
15835 cj4array[1]
15836 = IKcos(
15837 j4array
15838 [1]);
15839 j4array[2]
15840 = 3.14159265358979;
15841 sj4array[2]
15842 = IKsin(
15843 j4array
15844 [2]);
15845 cj4array[2]
15846 = IKcos(
15847 j4array
15848 [2]);
15849 j4array[3]
15850 = -1.5707963267949;
15851 sj4array[3]
15852 = IKsin(
15853 j4array
15854 [3]);
15855 cj4array[3]
15856 = IKcos(
15857 j4array
15858 [3]);
15859 if (j4array[0]
15860 > IKPI)
15861 {
15862 j4array[0]
15863 -= IK2PI;
15864 }
15865 else if (
15866 j4array[0]
15867 < -IKPI)
15868 {
15869 j4array[0]
15870 += IK2PI;
15871 }
15872 j4valid[0]
15873 = true;
15874 if (j4array[1]
15875 > IKPI)
15876 {
15877 j4array[1]
15878 -= IK2PI;
15879 }
15880 else if (
15881 j4array[1]
15882 < -IKPI)
15883 {
15884 j4array[1]
15885 += IK2PI;
15886 }
15887 j4valid[1]
15888 = true;
15889 if (j4array[2]
15890 > IKPI)
15891 {
15892 j4array[2]
15893 -= IK2PI;
15894 }
15895 else if (
15896 j4array[2]
15897 < -IKPI)
15898 {
15899 j4array[2]
15900 += IK2PI;
15901 }
15902 j4valid[2]
15903 = true;
15904 if (j4array[3]
15905 > IKPI)
15906 {
15907 j4array[3]
15908 -= IK2PI;
15909 }
15910 else if (
15911 j4array[3]
15912 < -IKPI)
15913 {
15914 j4array[3]
15915 += IK2PI;
15916 }
15917 j4valid[3]
15918 = true;
15919 for (int ij4
15920 = 0;
15921 ij4 < 4;
15922 ++ij4)
15923 {
15924 if (!j4valid
15925 [ij4])
15926 {
15927 continue;
15928 }
15929 _ij4[0]
15930 = ij4;
15931 _ij4[1]
15932 = -1;
15933 for (
15934 int iij4
15935 = ij4
15936 + 1;
15937 iij4
15938 < 4;
15939 ++iij4)
15940 {
15941 if (j4valid
15942 [iij4]
15943 && IKabs(
15944 cj4array
15945 [ij4]
15946 - cj4array
15947 [iij4])
15948 < IKFAST_SOLUTION_THRESH
15949 && IKabs(
15950 sj4array
15951 [ij4]
15952 - sj4array
15953 [iij4])
15954 < IKFAST_SOLUTION_THRESH)
15955 {
15956 j4valid
15957 [iij4]
15958 = false;
15959 _ij4[1]
15960 = iij4;
15961 break;
15962 }
15963 }
15964 j4 = j4array
15965 [ij4];
15966 cj4 = cj4array
15967 [ij4];
15968 sj4 = sj4array
15969 [ij4];
15970
15971 rotationfunction0(
15972 solutions);
15973 }
15974 }
15975 }
15976 } while (0);
15977 if (bgotonextstatement)
15978 {
15979 bool
15980 bgotonextstatement
15981 = true;
15982 do
15983 {
15984 if (1)
15985 {
15986 bgotonextstatement
15987 = false;
15988 continue; // branch miss [j4]
15989 }
15990 } while (0);
15991 if (bgotonextstatement)
15992 {
15993 }
15994 }
15995 }
15996 }
15997 }
15998 }
15999 else
16000 {
16001 {
16002 IkReal j4array[1],
16003 cj4array[1],
16004 sj4array[1];
16005 bool j4valid[1]
16006 = {false};
16007 _nj4 = 1;
16008 IkReal x1349
16009 = ((1.32323529411765)
16010 * cj9);
16011 IkReal x1350
16012 = ((3.92156862745098)
16013 * pp);
16014 IkReal x1351
16015 = ((0.316735294117647)
16016 * sj6);
16017 IkReal x1352
16018 = ((0.108264705882353)
16019 * cj9 * sj6);
16020 IkReal x1353
16021 = ((0.588235294117647)
16022 * pp * sj6);
16023 CheckValue<IkReal> x1354 = IKatan2WithCheck(
16024 IkReal((
16025 ((py * x1353))
16026 + (((-1.0)
16027 * (1.51009803921569)
16028 * px))
16029 + (((-1.0) * px
16030 * x1349))
16031 + ((py * x1352))
16032 + ((px * x1350))
16033 + ((py
16034 * x1351)))),
16035 (((px * x1351))
16036 + ((px * x1353))
16037 + (((1.51009803921569)
16038 * py))
16039 + ((py * x1349))
16040 + ((px * x1352))
16041 + (((-1.0) * py
16042 * x1350))),
16043 IKFAST_ATAN2_MAGTHRESH);
16044 if (!x1354.valid)
16045 {
16046 continue;
16047 }
16048 CheckValue<IkReal> x1355
16049 = IKPowWithIntegerCheck(
16050 IKsign((
16051 pp
16052 + (((-1.0)
16053 * (1.0)
16054 * (pz
16055 * pz))))),
16056 -1);
16057 if (!x1355.valid)
16058 {
16059 continue;
16060 }
16061 j4array[0]
16062 = ((-1.5707963267949)
16063 + (x1354.value)
16064 + (((1.5707963267949)
16065 * (x1355
16066 .value))));
16067 sj4array[0]
16068 = IKsin(j4array[0]);
16069 cj4array[0]
16070 = IKcos(j4array[0]);
16071 if (j4array[0] > IKPI)
16072 {
16073 j4array[0] -= IK2PI;
16074 }
16075 else if (
16076 j4array[0] < -IKPI)
16077 {
16078 j4array[0] += IK2PI;
16079 }
16080 j4valid[0] = true;
16081 for (int ij4 = 0;
16082 ij4 < 1;
16083 ++ij4)
16084 {
16085 if (!j4valid[ij4])
16086 {
16087 continue;
16088 }
16089 _ij4[0] = ij4;
16090 _ij4[1] = -1;
16091 for (int iij4
16092 = ij4 + 1;
16093 iij4 < 1;
16094 ++iij4)
16095 {
16096 if (j4valid[iij4]
16097 && IKabs(
16098 cj4array
16099 [ij4]
16100 - cj4array
16101 [iij4])
16102 < IKFAST_SOLUTION_THRESH
16103 && IKabs(
16104 sj4array
16105 [ij4]
16106 - sj4array
16107 [iij4])
16108 < IKFAST_SOLUTION_THRESH)
16109 {
16110 j4valid[iij4]
16111 = false;
16112 _ij4[1] = iij4;
16113 break;
16114 }
16115 }
16116 j4 = j4array[ij4];
16117 cj4 = cj4array[ij4];
16118 sj4 = sj4array[ij4];
16119 {
16120 IkReal evalcond[5];
16121 IkReal x1356
16122 = IKcos(j4);
16123 IkReal x1357
16124 = ((1.0)
16125 * x1356);
16126 IkReal x1358
16127 = (px * x1357);
16128 IkReal x1359
16129 = IKsin(j4);
16130 IkReal x1360
16131 = (py * x1359);
16132 IkReal x1361
16133 = ((1.0)
16134 * x1360);
16135 IkReal x1362
16136 = (px * x1359);
16137 IkReal x1363
16138 = (cj6 * pz);
16139 IkReal x1364
16140 = (px * sj6
16141 * x1356);
16142 IkReal x1365
16143 = (sj6 * x1360);
16144 evalcond[0]
16145 = ((((-1.0)
16146 * cj6
16147 * x1361))
16148 + (((-1.0)
16149 * cj6
16150 * x1358))
16151 + ((pz
16152 * sj6)));
16153 evalcond[1]
16154 = ((1.51009803921569)
16155 + (((-1.0)
16156 * py
16157 * x1357))
16158 + (((-1.0)
16159 * (3.92156862745098)
16160 * pp))
16161 + x1362
16162 + (((1.32323529411765)
16163 * cj9)));
16164 evalcond[2]
16165 = ((-0.55)
16166 + (((-1.0)
16167 * (0.3)
16168 * cj9))
16169 + x1363
16170 + x1365
16171 + x1364
16172 + (((-1.0)
16173 * (0.045)
16174 * sj9)));
16175 evalcond[3]
16176 = ((((0.108264705882353)
16177 * cj9
16178 * sj6))
16179 + (((0.588235294117647)
16180 * pp
16181 * sj6))
16182 + (((-1.0)
16183 * x1361))
16184 + (((-1.0)
16185 * x1358))
16186 + (((0.316735294117647)
16187 * sj6)));
16188 evalcond[4]
16189 = ((-0.2125)
16190 + (((1.1)
16191 * x1364))
16192 + (((1.1)
16193 * x1365))
16194 + (((-0.09)
16195 * py
16196 * x1356))
16197 + (((-1.0)
16198 * (1.0)
16199 * pp))
16200 + (((1.1)
16201 * x1363))
16202 + (((0.09)
16203 * x1362)));
16204 if (IKabs(
16205 evalcond[0])
16206 > IKFAST_EVALCOND_THRESH
16207 || IKabs(
16208 evalcond
16209 [1])
16210 > IKFAST_EVALCOND_THRESH
16211 || IKabs(
16212 evalcond
16213 [2])
16214 > IKFAST_EVALCOND_THRESH
16215 || IKabs(
16216 evalcond
16217 [3])
16218 > IKFAST_EVALCOND_THRESH
16219 || IKabs(
16220 evalcond
16221 [4])
16222 > IKFAST_EVALCOND_THRESH)
16223 {
16224 continue;
16225 }
16226 }
16227
16228 rotationfunction0(
16229 solutions);
16230 }
16231 }
16232 }
16233 }
16234 }
16235 else
16236 {
16237 {
16238 IkReal j4array[1],
16239 cj4array[1],
16240 sj4array[1];
16241 bool j4valid[1] = {false};
16242 _nj4 = 1;
16243 IkReal x1366 = (cj6 * pp);
16244 IkReal x1367
16245 = ((0.2125) * cj6);
16246 IkReal x1368 = ((1.1) * pz);
16247 IkReal x1369
16248 = ((0.09) * pz * sj6);
16249 CheckValue<IkReal> x1370
16250 = IKatan2WithCheck(
16251 IkReal((
16252 ((px * x1368))
16253 + (((-1.0) * px
16254 * x1367))
16255 + (((-1.0) * px
16256 * x1366))
16257 + (((-1.0) * py
16258 * x1369)))),
16259 ((((-1.0) * px
16260 * x1369))
16261 + (((-1.0) * py
16262 * x1368))
16263 + ((py * x1367))
16264 + ((py * x1366))),
16265 IKFAST_ATAN2_MAGTHRESH);
16266 if (!x1370.valid)
16267 {
16268 continue;
16269 }
16270 CheckValue<IkReal> x1371
16271 = IKPowWithIntegerCheck(
16272 IKsign((
16273 (((0.09) * cj6
16274 * (pz * pz)))
16275 + (((-0.09)
16276 * x1366)))),
16277 -1);
16278 if (!x1371.valid)
16279 {
16280 continue;
16281 }
16282 j4array[0]
16283 = ((-1.5707963267949)
16284 + (x1370.value)
16285 + (((1.5707963267949)
16286 * (x1371
16287 .value))));
16288 sj4array[0]
16289 = IKsin(j4array[0]);
16290 cj4array[0]
16291 = IKcos(j4array[0]);
16292 if (j4array[0] > IKPI)
16293 {
16294 j4array[0] -= IK2PI;
16295 }
16296 else if (j4array[0] < -IKPI)
16297 {
16298 j4array[0] += IK2PI;
16299 }
16300 j4valid[0] = true;
16301 for (int ij4 = 0; ij4 < 1;
16302 ++ij4)
16303 {
16304 if (!j4valid[ij4])
16305 {
16306 continue;
16307 }
16308 _ij4[0] = ij4;
16309 _ij4[1] = -1;
16310 for (int iij4 = ij4 + 1;
16311 iij4 < 1;
16312 ++iij4)
16313 {
16314 if (j4valid[iij4]
16315 && IKabs(
16316 cj4array[ij4]
16317 - cj4array
16318 [iij4])
16319 < IKFAST_SOLUTION_THRESH
16320 && IKabs(
16321 sj4array[ij4]
16322 - sj4array
16323 [iij4])
16324 < IKFAST_SOLUTION_THRESH)
16325 {
16326 j4valid[iij4] = false;
16327 _ij4[1] = iij4;
16328 break;
16329 }
16330 }
16331 j4 = j4array[ij4];
16332 cj4 = cj4array[ij4];
16333 sj4 = sj4array[ij4];
16334 {
16335 IkReal evalcond[5];
16336 IkReal x1372
16337 = IKcos(j4);
16338 IkReal x1373
16339 = ((1.0) * x1372);
16340 IkReal x1374
16341 = (px * x1373);
16342 IkReal x1375
16343 = IKsin(j4);
16344 IkReal x1376
16345 = (py * x1375);
16346 IkReal x1377
16347 = ((1.0) * x1376);
16348 IkReal x1378
16349 = (px * x1375);
16350 IkReal x1379
16351 = (cj6 * pz);
16352 IkReal x1380
16353 = (px * sj6
16354 * x1372);
16355 IkReal x1381
16356 = (sj6 * x1376);
16357 evalcond[0]
16358 = ((((-1.0) * cj6
16359 * x1374))
16360 + ((pz * sj6))
16361 + (((-1.0) * cj6
16362 * x1377)));
16363 evalcond[1]
16364 = ((1.51009803921569)
16365 + x1378
16366 + (((-1.0)
16367 * (3.92156862745098)
16368 * pp))
16369 + (((-1.0) * py
16370 * x1373))
16371 + (((1.32323529411765)
16372 * cj9)));
16373 evalcond[2]
16374 = ((-0.55) + x1379
16375 + x1380 + x1381
16376 + (((-1.0)
16377 * (0.3)
16378 * cj9))
16379 + (((-1.0)
16380 * (0.045)
16381 * sj9)));
16382 evalcond[3]
16383 = ((((0.108264705882353)
16384 * cj9 * sj6))
16385 + (((-1.0)
16386 * x1374))
16387 + (((0.588235294117647)
16388 * pp * sj6))
16389 + (((0.316735294117647)
16390 * sj6))
16391 + (((-1.0)
16392 * x1377)));
16393 evalcond[4]
16394 = ((-0.2125)
16395 + (((-0.09) * py
16396 * x1372))
16397 + (((0.09)
16398 * x1378))
16399 + (((1.1)
16400 * x1379))
16401 + (((1.1)
16402 * x1381))
16403 + (((1.1)
16404 * x1380))
16405 + (((-1.0)
16406 * (1.0)
16407 * pp)));
16408 if (IKabs(evalcond[0])
16409 > IKFAST_EVALCOND_THRESH
16410 || IKabs(
16411 evalcond[1])
16412 > IKFAST_EVALCOND_THRESH
16413 || IKabs(
16414 evalcond[2])
16415 > IKFAST_EVALCOND_THRESH
16416 || IKabs(
16417 evalcond[3])
16418 > IKFAST_EVALCOND_THRESH
16419 || IKabs(
16420 evalcond[4])
16421 > IKFAST_EVALCOND_THRESH)
16422 {
16423 continue;
16424 }
16425 }
16426
16427 rotationfunction0(
16428 solutions);
16429 }
16430 }
16431 }
16432 }
16433 }
16434 else
16435 {
16436 {
16437 IkReal j4array[1], cj4array[1],
16438 sj4array[1];
16439 bool j4valid[1] = {false};
16440 _nj4 = 1;
16441 IkReal x1382
16442 = ((1.51009803921569)
16443 * cj6);
16444 IkReal x1383 = (pz * sj6);
16445 IkReal x1384
16446 = ((1.32323529411765) * cj6
16447 * cj9);
16448 IkReal x1385
16449 = ((3.92156862745098) * cj6
16450 * pp);
16451 CheckValue<IkReal> x1386
16452 = IKPowWithIntegerCheck(
16453 IKsign(
16454 ((((-1.0) * (1.0)
16455 * cj6
16456 * (pz * pz)))
16457 + ((cj6 * pp)))),
16458 -1);
16459 if (!x1386.valid)
16460 {
16461 continue;
16462 }
16463 CheckValue<IkReal> x1387
16464 = IKatan2WithCheck(
16465 IkReal(
16466 ((((-1.0) * px
16467 * x1384))
16468 + (((-1.0) * px
16469 * x1382))
16470 + ((py * x1383))
16471 + ((px * x1385)))),
16472 ((((-1.0) * py * x1385))
16473 + ((py * x1384))
16474 + ((px * x1383))
16475 + ((py * x1382))),
16476 IKFAST_ATAN2_MAGTHRESH);
16477 if (!x1387.valid)
16478 {
16479 continue;
16480 }
16481 j4array[0]
16482 = ((-1.5707963267949)
16483 + (((1.5707963267949)
16484 * (x1386.value)))
16485 + (x1387.value));
16486 sj4array[0] = IKsin(j4array[0]);
16487 cj4array[0] = IKcos(j4array[0]);
16488 if (j4array[0] > IKPI)
16489 {
16490 j4array[0] -= IK2PI;
16491 }
16492 else if (j4array[0] < -IKPI)
16493 {
16494 j4array[0] += IK2PI;
16495 }
16496 j4valid[0] = true;
16497 for (int ij4 = 0; ij4 < 1;
16498 ++ij4)
16499 {
16500 if (!j4valid[ij4])
16501 {
16502 continue;
16503 }
16504 _ij4[0] = ij4;
16505 _ij4[1] = -1;
16506 for (int iij4 = ij4 + 1;
16507 iij4 < 1;
16508 ++iij4)
16509 {
16510 if (j4valid[iij4]
16511 && IKabs(
16512 cj4array[ij4]
16513 - cj4array[iij4])
16514 < IKFAST_SOLUTION_THRESH
16515 && IKabs(
16516 sj4array[ij4]
16517 - sj4array[iij4])
16518 < IKFAST_SOLUTION_THRESH)
16519 {
16520 j4valid[iij4] = false;
16521 _ij4[1] = iij4;
16522 break;
16523 }
16524 }
16525 j4 = j4array[ij4];
16526 cj4 = cj4array[ij4];
16527 sj4 = sj4array[ij4];
16528 {
16529 IkReal evalcond[5];
16530 IkReal x1388 = IKcos(j4);
16531 IkReal x1389
16532 = ((1.0) * x1388);
16533 IkReal x1390 = (px * x1389);
16534 IkReal x1391 = IKsin(j4);
16535 IkReal x1392 = (py * x1391);
16536 IkReal x1393
16537 = ((1.0) * x1392);
16538 IkReal x1394 = (px * x1391);
16539 IkReal x1395 = (cj6 * pz);
16540 IkReal x1396
16541 = (px * sj6 * x1388);
16542 IkReal x1397
16543 = (sj6 * x1392);
16544 evalcond[0]
16545 = ((((-1.0) * cj6
16546 * x1393))
16547 + ((pz * sj6))
16548 + (((-1.0) * cj6
16549 * x1390)));
16550 evalcond[1]
16551 = ((1.51009803921569)
16552 + (((-1.0) * py
16553 * x1389))
16554 + (((-1.0)
16555 * (3.92156862745098)
16556 * pp))
16557 + x1394
16558 + (((1.32323529411765)
16559 * cj9)));
16560 evalcond[2]
16561 = ((-0.55)
16562 + (((-1.0) * (0.3)
16563 * cj9))
16564 + x1397 + x1396
16565 + x1395
16566 + (((-1.0) * (0.045)
16567 * sj9)));
16568 evalcond[3]
16569 = ((((-1.0) * x1393))
16570 + (((0.108264705882353)
16571 * cj9 * sj6))
16572 + (((0.588235294117647)
16573 * pp * sj6))
16574 + (((0.316735294117647)
16575 * sj6))
16576 + (((-1.0)
16577 * x1390)));
16578 evalcond[4]
16579 = ((-0.2125)
16580 + (((1.1) * x1395))
16581 + (((0.09) * x1394))
16582 + (((-0.09) * py
16583 * x1388))
16584 + (((1.1) * x1396))
16585 + (((1.1) * x1397))
16586 + (((-1.0) * (1.0)
16587 * pp)));
16588 if (IKabs(evalcond[0])
16589 > IKFAST_EVALCOND_THRESH
16590 || IKabs(evalcond[1])
16591 > IKFAST_EVALCOND_THRESH
16592 || IKabs(evalcond[2])
16593 > IKFAST_EVALCOND_THRESH
16594 || IKabs(evalcond[3])
16595 > IKFAST_EVALCOND_THRESH
16596 || IKabs(evalcond[4])
16597 > IKFAST_EVALCOND_THRESH)
16598 {
16599 continue;
16600 }
16601 }
16602
16603 rotationfunction0(solutions);
16604 }
16605 }
16606 }
16607 }
16608 }
16609 } while (0);
16610 if (bgotonextstatement)
16611 {
16612 bool bgotonextstatement = true;
16613 do
16614 {
16615 IkReal x1398
16616 = ((-0.55) + pz
16617 + (((-1.0) * (0.3) * cj9))
16618 + (((-1.0) * (0.045) * sj9)));
16619 evalcond[0]
16620 = ((-3.14159265358979)
16621 + (IKfmod(
16622 ((3.14159265358979)
16623 + (IKabs(j6))),
16624 6.28318530717959)));
16625 evalcond[1]
16626 = ((0.39655) + (((0.0765) * sj9))
16627 + (((0.32595) * cj9))
16628 + (((-1.0) * (1.0) * pp)));
16629 evalcond[2] = x1398;
16630 evalcond[3] = x1398;
16631 if (IKabs(evalcond[0])
16632 < 0.0000010000000000
16633 && IKabs(evalcond[1])
16634 < 0.0000010000000000
16635 && IKabs(evalcond[2])
16636 < 0.0000010000000000
16637 && IKabs(evalcond[3])
16638 < 0.0000010000000000)
16639 {
16640 bgotonextstatement = false;
16641 {
16642 IkReal j4eval[3];
16643 sj6 = 0;
16644 cj6 = 1.0;
16645 j6 = 0;
16646 IkReal x1399
16647 = (pp
16648 + (((-1.0) * (1.0)
16649 * (pz * pz))));
16650 IkReal x1400
16651 = ((1.51009803921569) * cj8);
16652 IkReal x1401
16653 = ((1.51009803921569) * sj8);
16654 IkReal x1402
16655 = ((1.32323529411765) * cj8
16656 * cj9);
16657 IkReal x1403
16658 = ((3.92156862745098) * cj8
16659 * pp);
16660 IkReal x1404
16661 = ((1.32323529411765) * cj9
16662 * sj8);
16663 IkReal x1405
16664 = ((3.92156862745098) * pp
16665 * sj8);
16666 j4eval[0] = x1399;
16667 j4eval[1]
16668 = ((IKabs(
16669 (((py * x1403))
16670 + ((px * x1401))
16671 + (((-1.0) * py
16672 * x1402))
16673 + ((px * x1404))
16674 + (((-1.0) * px
16675 * x1405))
16676 + (((-1.0) * py
16677 * x1400)))))
16678 + (IKabs((
16679 (((-1.0) * px
16680 * x1400))
16681 + (((-1.0) * py
16682 * x1404))
16683 + (((-1.0) * px
16684 * x1402))
16685 + (((-1.0) * py
16686 * x1401))
16687 + ((py * x1405))
16688 + ((px * x1403))))));
16689 j4eval[2] = IKsign(x1399);
16690 if (IKabs(j4eval[0])
16691 < 0.0000010000000000
16692 || IKabs(j4eval[1])
16693 < 0.0000010000000000
16694 || IKabs(j4eval[2])
16695 < 0.0000010000000000)
16696 {
16697 {
16698 IkReal j4eval[2];
16699 sj6 = 0;
16700 cj6 = 1.0;
16701 j6 = 0;
16702 IkReal x1406
16703 = (((cj8 * (pz * pz)))
16704 + (((-1.0) * (1.0)
16705 * cj8 * pp)));
16706 j4eval[0] = x1406;
16707 j4eval[1] = IKsign(x1406);
16708 if (IKabs(j4eval[0])
16709 < 0.0000010000000000
16710 || IKabs(j4eval[1])
16711 < 0.0000010000000000)
16712 {
16713 {
16714 IkReal j4eval[2];
16715 sj6 = 0;
16716 cj6 = 1.0;
16717 j6 = 0;
16718 IkReal x1407
16719 = ((((-1.0) * (1.0)
16720 * sj8
16721 * (pz * pz)))
16722 + ((pp * sj8)));
16723 j4eval[0] = x1407;
16724 j4eval[1] = IKsign(x1407);
16725 if (IKabs(j4eval[0])
16726 < 0.0000010000000000
16727 || IKabs(j4eval[1])
16728 < 0.0000010000000000)
16729 {
16730 {
16731 IkReal evalcond[6];
16732 bool
16733 bgotonextstatement
16734 = true;
16735 do
16736 {
16737 evalcond[0]
16738 = ((-3.14159265358979)
16739 + (IKfmod(
16740 ((3.14159265358979)
16741 + (IKabs(
16742 j8))),
16743 6.28318530717959)));
16744 if (IKabs(
16745 evalcond[0])
16746 < 0.0000010000000000)
16747 {
16748 bgotonextstatement
16749 = false;
16750 {
16751 IkReal
16752 j4eval[3];
16753 sj6 = 0;
16754 cj6 = 1.0;
16755 j6 = 0;
16756 sj8 = 0;
16757 cj8 = 1.0;
16758 j8 = 0;
16759 IkReal x1408
16760 = (pp
16761 + (((-1.0)
16762 * (1.0)
16763 * (pz
16764 * pz))));
16765 IkReal x1409
16766 = ((13497.0)
16767 * cj9);
16768 IkReal x1410
16769 = ((40000.0)
16770 * pp);
16771 j4eval[0]
16772 = x1408;
16773 j4eval[1]
16774 = ((IKabs((
16775 ((py
16776 * x1410))
16777 + (((-1.0)
16778 * (15403.0)
16779 * py))
16780 + (((-1.0)
16781 * py
16782 * x1409)))))
16783 + (IKabs((
16784 (((-1.0)
16785 * px
16786 * x1409))
16787 + (((-1.0)
16788 * (15403.0)
16789 * px))
16790 + ((px
16791 * x1410))))));
16792 j4eval[2]
16793 = IKsign(
16794 x1408);
16795 if (IKabs(
16796 j4eval
16797 [0])
16798 < 0.0000010000000000
16799 || IKabs(
16800 j4eval
16801 [1])
16802 < 0.0000010000000000
16803 || IKabs(
16804 j4eval
16805 [2])
16806 < 0.0000010000000000)
16807 {
16808 {
16809 IkReal
16810 j4eval
16811 [3];
16812 sj6 = 0;
16813 cj6 = 1.0;
16814 j6 = 0;
16815 sj8 = 0;
16816 cj8 = 1.0;
16817 j8 = 0;
16818 IkReal x1411
16819 = pz
16820 * pz;
16821 IkReal x1412
16822 = ((80.0)
16823 * pp);
16824 IkReal x1413
16825 = ((88.0)
16826 * pz);
16827 j4eval[0]
16828 = ((((-1.0)
16829 * x1411))
16830 + pp);
16831 j4eval[1]
16832 = ((IKabs((
16833 (((-1.0)
16834 * px
16835 * x1413))
16836 + ((px
16837 * x1412))
16838 + (((17.0)
16839 * px)))))
16840 + (IKabs((
16841 ((py
16842 * x1412))
16843 + (((17.0)
16844 * py))
16845 + (((-1.0)
16846 * py
16847 * x1413))))));
16848 j4eval[2] = IKsign((
16849 (((9.0)
16850 * pp))
16851 + (((-9.0)
16852 * x1411))));
16853 if (IKabs(
16854 j4eval
16855 [0])
16856 < 0.0000010000000000
16857 || IKabs(
16858 j4eval
16859 [1])
16860 < 0.0000010000000000
16861 || IKabs(
16862 j4eval
16863 [2])
16864 < 0.0000010000000000)
16865 {
16866 {
16867 IkReal evalcond
16868 [5];
16869 bool
16870 bgotonextstatement
16871 = true;
16872 do
16873 {
16874 IkReal
16875 x1414
16876 = ((-1.51009803921569)
16877 + (((-1.0)
16878 * (1.32323529411765)
16879 * cj9))
16880 + (((3.92156862745098)
16881 * pp)));
16882 evalcond
16883 [0]
16884 = ((IKabs(
16885 py))
16886 + (IKabs(
16887 px)));
16888 evalcond
16889 [1]
16890 = 0;
16891 evalcond
16892 [2]
16893 = x1414;
16894 evalcond
16895 [3]
16896 = x1414;
16897 evalcond
16898 [4]
16899 = ((-0.2125)
16900 + (((1.1)
16901 * pz))
16902 + (((-1.0)
16903 * (1.0)
16904 * pp)));
16905 if (IKabs(
16906 evalcond
16907 [0])
16908 < 0.0000010000000000
16909 && IKabs(
16910 evalcond
16911 [1])
16912 < 0.0000010000000000
16913 && IKabs(
16914 evalcond
16915 [2])
16916 < 0.0000010000000000
16917 && IKabs(
16918 evalcond
16919 [3])
16920 < 0.0000010000000000
16921 && IKabs(
16922 evalcond
16923 [4])
16924 < 0.0000010000000000)
16925 {
16926 bgotonextstatement
16927 = false;
16928 {
16929 IkReal j4array
16930 [4],
16931 cj4array
16932 [4],
16933 sj4array
16934 [4];
16935 bool j4valid
16936 [4]
16937 = {false};
16938 _nj4
16939 = 4;
16940 j4array
16941 [0]
16942 = 0;
16943 sj4array
16944 [0]
16945 = IKsin(
16946 j4array
16947 [0]);
16948 cj4array
16949 [0]
16950 = IKcos(
16951 j4array
16952 [0]);
16953 j4array
16954 [1]
16955 = 1.5707963267949;
16956 sj4array
16957 [1]
16958 = IKsin(
16959 j4array
16960 [1]);
16961 cj4array
16962 [1]
16963 = IKcos(
16964 j4array
16965 [1]);
16966 j4array
16967 [2]
16968 = 3.14159265358979;
16969 sj4array
16970 [2]
16971 = IKsin(
16972 j4array
16973 [2]);
16974 cj4array
16975 [2]
16976 = IKcos(
16977 j4array
16978 [2]);
16979 j4array
16980 [3]
16981 = -1.5707963267949;
16982 sj4array
16983 [3]
16984 = IKsin(
16985 j4array
16986 [3]);
16987 cj4array
16988 [3]
16989 = IKcos(
16990 j4array
16991 [3]);
16992 if (j4array
16993 [0]
16994 > IKPI)
16995 {
16996 j4array
16997 [0]
16998 -= IK2PI;
16999 }
17000 else if (
17001 j4array
17002 [0]
17003 < -IKPI)
17004 {
17005 j4array
17006 [0]
17007 += IK2PI;
17008 }
17009 j4valid
17010 [0]
17011 = true;
17012 if (j4array
17013 [1]
17014 > IKPI)
17015 {
17016 j4array
17017 [1]
17018 -= IK2PI;
17019 }
17020 else if (
17021 j4array
17022 [1]
17023 < -IKPI)
17024 {
17025 j4array
17026 [1]
17027 += IK2PI;
17028 }
17029 j4valid
17030 [1]
17031 = true;
17032 if (j4array
17033 [2]
17034 > IKPI)
17035 {
17036 j4array
17037 [2]
17038 -= IK2PI;
17039 }
17040 else if (
17041 j4array
17042 [2]
17043 < -IKPI)
17044 {
17045 j4array
17046 [2]
17047 += IK2PI;
17048 }
17049 j4valid
17050 [2]
17051 = true;
17052 if (j4array
17053 [3]
17054 > IKPI)
17055 {
17056 j4array
17057 [3]
17058 -= IK2PI;
17059 }
17060 else if (
17061 j4array
17062 [3]
17063 < -IKPI)
17064 {
17065 j4array
17066 [3]
17067 += IK2PI;
17068 }
17069 j4valid
17070 [3]
17071 = true;
17072 for (
17073 int ij4
17074 = 0;
17075 ij4
17076 < 4;
17077 ++ij4)
17078 {
17079 if (!j4valid
17080 [ij4])
17081 {
17082 continue;
17083 }
17084 _ij4[0]
17085 = ij4;
17086 _ij4[1]
17087 = -1;
17088 for (
17089 int iij4
17090 = ij4
17091 + 1;
17092 iij4
17093 < 4;
17094 ++iij4)
17095 {
17096 if (j4valid
17097 [iij4]
17098 && IKabs(
17099 cj4array
17100 [ij4]
17101 - cj4array
17102 [iij4])
17103 < IKFAST_SOLUTION_THRESH
17104 && IKabs(
17105 sj4array
17106 [ij4]
17107 - sj4array
17108 [iij4])
17109 < IKFAST_SOLUTION_THRESH)
17110 {
17111 j4valid
17112 [iij4]
17113 = false;
17114 _ij4[1]
17115 = iij4;
17116 break;
17117 }
17118 }
17119 j4 = j4array
17120 [ij4];
17121 cj4 = cj4array
17122 [ij4];
17123 sj4 = sj4array
17124 [ij4];
17125
17126 rotationfunction0(
17127 solutions);
17128 }
17129 }
17130 }
17131 } while (
17132 0);
17133 if (bgotonextstatement)
17134 {
17135 bool
17136 bgotonextstatement
17137 = true;
17138 do
17139 {
17140 if (1)
17141 {
17142 bgotonextstatement
17143 = false;
17144 continue; // branch miss [j4]
17145 }
17146 } while (
17147 0);
17148 if (bgotonextstatement)
17149 {
17150 }
17151 }
17152 }
17153 }
17154 else
17155 {
17156 {
17157 IkReal j4array
17158 [1],
17159 cj4array
17160 [1],
17161 sj4array
17162 [1];
17163 bool j4valid
17164 [1]
17165 = {false};
17166 _nj4
17167 = 1;
17168 IkReal
17169 x1415
17170 = ((100.0)
17171 * pp);
17172 IkReal
17173 x1416
17174 = ((110.0)
17175 * pz);
17176 CheckValue<IkReal> x1417 = IKPowWithIntegerCheck(
17177 IKsign((
17178 (((-1.0)
17179 * (9.0)
17180 * (pz
17181 * pz)))
17182 + (((9.0)
17183 * pp)))),
17184 -1);
17185 if (!x1417
17186 .valid)
17187 {
17188 continue;
17189 }
17190 CheckValue<IkReal> x1418 = IKatan2WithCheck(
17191 IkReal((
17192 ((py
17193 * x1415))
17194 + (((21.25)
17195 * py))
17196 + (((-1.0)
17197 * py
17198 * x1416)))),
17199 (((px
17200 * x1415))
17201 + (((-1.0)
17202 * px
17203 * x1416))
17204 + (((21.25)
17205 * px))),
17206 IKFAST_ATAN2_MAGTHRESH);
17207 if (!x1418
17208 .valid)
17209 {
17210 continue;
17211 }
17212 j4array
17213 [0]
17214 = ((-1.5707963267949)
17215 + (((1.5707963267949)
17216 * (x1417
17217 .value)))
17218 + (x1418
17219 .value));
17220 sj4array
17221 [0]
17222 = IKsin(
17223 j4array
17224 [0]);
17225 cj4array
17226 [0]
17227 = IKcos(
17228 j4array
17229 [0]);
17230 if (j4array
17231 [0]
17232 > IKPI)
17233 {
17234 j4array
17235 [0]
17236 -= IK2PI;
17237 }
17238 else if (
17239 j4array
17240 [0]
17241 < -IKPI)
17242 {
17243 j4array
17244 [0]
17245 += IK2PI;
17246 }
17247 j4valid
17248 [0]
17249 = true;
17250 for (
17251 int ij4
17252 = 0;
17253 ij4
17254 < 1;
17255 ++ij4)
17256 {
17257 if (!j4valid
17258 [ij4])
17259 {
17260 continue;
17261 }
17262 _ij4[0]
17263 = ij4;
17264 _ij4[1]
17265 = -1;
17266 for (
17267 int iij4
17268 = ij4
17269 + 1;
17270 iij4
17271 < 1;
17272 ++iij4)
17273 {
17274 if (j4valid
17275 [iij4]
17276 && IKabs(
17277 cj4array
17278 [ij4]
17279 - cj4array
17280 [iij4])
17281 < IKFAST_SOLUTION_THRESH
17282 && IKabs(
17283 sj4array
17284 [ij4]
17285 - sj4array
17286 [iij4])
17287 < IKFAST_SOLUTION_THRESH)
17288 {
17289 j4valid
17290 [iij4]
17291 = false;
17292 _ij4[1]
17293 = iij4;
17294 break;
17295 }
17296 }
17297 j4 = j4array
17298 [ij4];
17299 cj4 = cj4array
17300 [ij4];
17301 sj4 = sj4array
17302 [ij4];
17303 {
17304 IkReal evalcond
17305 [3];
17306 IkReal
17307 x1419
17308 = IKsin(
17309 j4);
17310 IkReal
17311 x1420
17312 = IKcos(
17313 j4);
17314 IkReal
17315 x1421
17316 = (px
17317 * x1420);
17318 IkReal
17319 x1422
17320 = (py
17321 * x1419);
17322 evalcond
17323 [0]
17324 = ((((-1.0)
17325 * py
17326 * x1420))
17327 + ((px
17328 * x1419)));
17329 evalcond
17330 [1]
17331 = ((-1.51009803921569)
17332 + (((-1.0)
17333 * (1.32323529411765)
17334 * cj9))
17335 + (((-1.0)
17336 * x1421))
17337 + (((-1.0)
17338 * x1422))
17339 + (((3.92156862745098)
17340 * pp)));
17341 evalcond
17342 [2]
17343 = ((-0.2125)
17344 + (((1.1)
17345 * pz))
17346 + (((0.09)
17347 * x1421))
17348 + (((0.09)
17349 * x1422))
17350 + (((-1.0)
17351 * (1.0)
17352 * pp)));
17353 if (IKabs(
17354 evalcond
17355 [0])
17356 > IKFAST_EVALCOND_THRESH
17357 || IKabs(
17358 evalcond
17359 [1])
17360 > IKFAST_EVALCOND_THRESH
17361 || IKabs(
17362 evalcond
17363 [2])
17364 > IKFAST_EVALCOND_THRESH)
17365 {
17366 continue;
17367 }
17368 }
17369
17370 rotationfunction0(
17371 solutions);
17372 }
17373 }
17374 }
17375 }
17376 }
17377 else
17378 {
17379 {
17380 IkReal
17381 j4array
17382 [1],
17383 cj4array
17384 [1],
17385 sj4array
17386 [1];
17387 bool j4valid
17388 [1]
17389 = {false};
17390 _nj4 = 1;
17391 IkReal x1423
17392 = ((1.32323529411765)
17393 * cj9);
17394 IkReal x1424
17395 = ((3.92156862745098)
17396 * pp);
17397 CheckValue<IkReal> x1425 = IKatan2WithCheck(
17398 IkReal((
17399 ((py
17400 * x1424))
17401 + (((-1.0)
17402 * (1.51009803921569)
17403 * py))
17404 + (((-1.0)
17405 * py
17406 * x1423)))),
17407 ((((-1.0)
17408 * px
17409 * x1423))
17410 + (((-1.0)
17411 * (1.51009803921569)
17412 * px))
17413 + ((px
17414 * x1424))),
17415 IKFAST_ATAN2_MAGTHRESH);
17416 if (!x1425
17417 .valid)
17418 {
17419 continue;
17420 }
17421 CheckValue<IkReal> x1426 = IKPowWithIntegerCheck(
17422 IKsign((
17423 pp
17424 + (((-1.0)
17425 * (1.0)
17426 * (pz
17427 * pz))))),
17428 -1);
17429 if (!x1426
17430 .valid)
17431 {
17432 continue;
17433 }
17434 j4array[0]
17435 = ((-1.5707963267949)
17436 + (x1425
17437 .value)
17438 + (((1.5707963267949)
17439 * (x1426
17440 .value))));
17441 sj4array[0] = IKsin(
17442 j4array
17443 [0]);
17444 cj4array[0] = IKcos(
17445 j4array
17446 [0]);
17447 if (j4array
17448 [0]
17449 > IKPI)
17450 {
17451 j4array[0]
17452 -= IK2PI;
17453 }
17454 else if (
17455 j4array
17456 [0]
17457 < -IKPI)
17458 {
17459 j4array[0]
17460 += IK2PI;
17461 }
17462 j4valid[0]
17463 = true;
17464 for (int ij4
17465 = 0;
17466 ij4
17467 < 1;
17468 ++ij4)
17469 {
17470 if (!j4valid
17471 [ij4])
17472 {
17473 continue;
17474 }
17475 _ij4[0]
17476 = ij4;
17477 _ij4[1]
17478 = -1;
17479 for (
17480 int iij4
17481 = ij4
17482 + 1;
17483 iij4
17484 < 1;
17485 ++iij4)
17486 {
17487 if (j4valid
17488 [iij4]
17489 && IKabs(
17490 cj4array
17491 [ij4]
17492 - cj4array
17493 [iij4])
17494 < IKFAST_SOLUTION_THRESH
17495 && IKabs(
17496 sj4array
17497 [ij4]
17498 - sj4array
17499 [iij4])
17500 < IKFAST_SOLUTION_THRESH)
17501 {
17502 j4valid
17503 [iij4]
17504 = false;
17505 _ij4[1]
17506 = iij4;
17507 break;
17508 }
17509 }
17510 j4 = j4array
17511 [ij4];
17512 cj4 = cj4array
17513 [ij4];
17514 sj4 = sj4array
17515 [ij4];
17516 {
17517 IkReal evalcond
17518 [3];
17519 IkReal
17520 x1427
17521 = IKsin(
17522 j4);
17523 IkReal
17524 x1428
17525 = IKcos(
17526 j4);
17527 IkReal
17528 x1429
17529 = (px
17530 * x1428);
17531 IkReal
17532 x1430
17533 = (py
17534 * x1427);
17535 evalcond
17536 [0]
17537 = ((((-1.0)
17538 * py
17539 * x1428))
17540 + ((px
17541 * x1427)));
17542 evalcond
17543 [1]
17544 = ((-1.51009803921569)
17545 + (((-1.0)
17546 * (1.32323529411765)
17547 * cj9))
17548 + (((3.92156862745098)
17549 * pp))
17550 + (((-1.0)
17551 * x1430))
17552 + (((-1.0)
17553 * x1429)));
17554 evalcond
17555 [2]
17556 = ((-0.2125)
17557 + (((1.1)
17558 * pz))
17559 + (((0.09)
17560 * x1430))
17561 + (((-1.0)
17562 * (1.0)
17563 * pp))
17564 + (((0.09)
17565 * x1429)));
17566 if (IKabs(
17567 evalcond
17568 [0])
17569 > IKFAST_EVALCOND_THRESH
17570 || IKabs(
17571 evalcond
17572 [1])
17573 > IKFAST_EVALCOND_THRESH
17574 || IKabs(
17575 evalcond
17576 [2])
17577 > IKFAST_EVALCOND_THRESH)
17578 {
17579 continue;
17580 }
17581 }
17582
17583 rotationfunction0(
17584 solutions);
17585 }
17586 }
17587 }
17588 }
17589 }
17590 } while (0);
17591 if (bgotonextstatement)
17592 {
17593 bool
17594 bgotonextstatement
17595 = true;
17596 do
17597 {
17598 evalcond[0]
17599 = ((-3.14159265358979)
17600 + (IKfmod(
17601 ((3.14159265358979)
17602 + (IKabs((
17603 (-3.14159265358979)
17604 + j8)))),
17605 6.28318530717959)));
17606 if (IKabs(evalcond
17607 [0])
17608 < 0.0000010000000000)
17609 {
17610 bgotonextstatement
17611 = false;
17612 {
17613 IkReal
17614 j4eval[3];
17615 sj6 = 0;
17616 cj6 = 1.0;
17617 j6 = 0;
17618 sj8 = 0;
17619 cj8 = -1.0;
17620 j8 = 3.14159265358979;
17621 IkReal x1431
17622 = (pp
17623 + (((-1.0)
17624 * (1.0)
17625 * (pz
17626 * pz))));
17627 IkReal x1432
17628 = ((13497.0)
17629 * cj9);
17630 IkReal x1433
17631 = ((40000.0)
17632 * pp);
17633 j4eval[0]
17634 = x1431;
17635 j4eval[1]
17636 = ((IKabs((
17637 (((15403.0)
17638 * px))
17639 + ((px
17640 * x1432))
17641 + (((-1.0)
17642 * px
17643 * x1433)))))
17644 + (IKabs((
17645 (((15403.0)
17646 * py))
17647 + ((py
17648 * x1432))
17649 + (((-1.0)
17650 * py
17651 * x1433))))));
17652 j4eval[2]
17653 = IKsign(
17654 x1431);
17655 if (IKabs(
17656 j4eval
17657 [0])
17658 < 0.0000010000000000
17659 || IKabs(
17660 j4eval
17661 [1])
17662 < 0.0000010000000000
17663 || IKabs(
17664 j4eval
17665 [2])
17666 < 0.0000010000000000)
17667 {
17668 {
17669 IkReal j4eval
17670 [3];
17671 sj6 = 0;
17672 cj6 = 1.0;
17673 j6 = 0;
17674 sj8 = 0;
17675 cj8 = -1.0;
17676 j8 = 3.14159265358979;
17677 IkReal
17678 x1434
17679 = pz
17680 * pz;
17681 IkReal
17682 x1435
17683 = ((80.0)
17684 * pp);
17685 IkReal
17686 x1436
17687 = ((88.0)
17688 * pz);
17689 j4eval[0]
17690 = (pp
17691 + (((-1.0)
17692 * x1434)));
17693 j4eval[1] = IKsign((
17694 (((9.0)
17695 * pp))
17696 + (((-9.0)
17697 * x1434))));
17698 j4eval[2]
17699 = ((IKabs((
17700 (((-1.0)
17701 * (17.0)
17702 * px))
17703 + ((px
17704 * x1436))
17705 + (((-1.0)
17706 * px
17707 * x1435)))))
17708 + (IKabs((
17709 ((py
17710 * x1436))
17711 + (((-1.0)
17712 * py
17713 * x1435))
17714 + (((-1.0)
17715 * (17.0)
17716 * py))))));
17717 if (IKabs(
17718 j4eval
17719 [0])
17720 < 0.0000010000000000
17721 || IKabs(
17722 j4eval
17723 [1])
17724 < 0.0000010000000000
17725 || IKabs(
17726 j4eval
17727 [2])
17728 < 0.0000010000000000)
17729 {
17730 {
17731 IkReal evalcond
17732 [5];
17733 bool
17734 bgotonextstatement
17735 = true;
17736 do
17737 {
17738 IkReal
17739 x1437
17740 = ((1.32323529411765)
17741 * cj9);
17742 IkReal
17743 x1438
17744 = ((3.92156862745098)
17745 * pp);
17746 evalcond
17747 [0]
17748 = ((IKabs(
17749 py))
17750 + (IKabs(
17751 px)));
17752 evalcond
17753 [1]
17754 = 0;
17755 evalcond
17756 [2]
17757 = ((-1.51009803921569)
17758 + x1438
17759 + (((-1.0)
17760 * x1437)));
17761 evalcond
17762 [3]
17763 = ((1.51009803921569)
17764 + (((-1.0)
17765 * x1438))
17766 + x1437);
17767 evalcond
17768 [4]
17769 = ((-0.2125)
17770 + (((1.1)
17771 * pz))
17772 + (((-1.0)
17773 * (1.0)
17774 * pp)));
17775 if (IKabs(
17776 evalcond
17777 [0])
17778 < 0.0000010000000000
17779 && IKabs(
17780 evalcond
17781 [1])
17782 < 0.0000010000000000
17783 && IKabs(
17784 evalcond
17785 [2])
17786 < 0.0000010000000000
17787 && IKabs(
17788 evalcond
17789 [3])
17790 < 0.0000010000000000
17791 && IKabs(
17792 evalcond
17793 [4])
17794 < 0.0000010000000000)
17795 {
17796 bgotonextstatement
17797 = false;
17798 {
17799 IkReal j4array
17800 [4],
17801 cj4array
17802 [4],
17803 sj4array
17804 [4];
17805 bool j4valid
17806 [4]
17807 = {false};
17808 _nj4
17809 = 4;
17810 j4array
17811 [0]
17812 = 0;
17813 sj4array
17814 [0]
17815 = IKsin(
17816 j4array
17817 [0]);
17818 cj4array
17819 [0]
17820 = IKcos(
17821 j4array
17822 [0]);
17823 j4array
17824 [1]
17825 = 1.5707963267949;
17826 sj4array
17827 [1]
17828 = IKsin(
17829 j4array
17830 [1]);
17831 cj4array
17832 [1]
17833 = IKcos(
17834 j4array
17835 [1]);
17836 j4array
17837 [2]
17838 = 3.14159265358979;
17839 sj4array
17840 [2]
17841 = IKsin(
17842 j4array
17843 [2]);
17844 cj4array
17845 [2]
17846 = IKcos(
17847 j4array
17848 [2]);
17849 j4array
17850 [3]
17851 = -1.5707963267949;
17852 sj4array
17853 [3]
17854 = IKsin(
17855 j4array
17856 [3]);
17857 cj4array
17858 [3]
17859 = IKcos(
17860 j4array
17861 [3]);
17862 if (j4array
17863 [0]
17864 > IKPI)
17865 {
17866 j4array
17867 [0]
17868 -= IK2PI;
17869 }
17870 else if (
17871 j4array
17872 [0]
17873 < -IKPI)
17874 {
17875 j4array
17876 [0]
17877 += IK2PI;
17878 }
17879 j4valid
17880 [0]
17881 = true;
17882 if (j4array
17883 [1]
17884 > IKPI)
17885 {
17886 j4array
17887 [1]
17888 -= IK2PI;
17889 }
17890 else if (
17891 j4array
17892 [1]
17893 < -IKPI)
17894 {
17895 j4array
17896 [1]
17897 += IK2PI;
17898 }
17899 j4valid
17900 [1]
17901 = true;
17902 if (j4array
17903 [2]
17904 > IKPI)
17905 {
17906 j4array
17907 [2]
17908 -= IK2PI;
17909 }
17910 else if (
17911 j4array
17912 [2]
17913 < -IKPI)
17914 {
17915 j4array
17916 [2]
17917 += IK2PI;
17918 }
17919 j4valid
17920 [2]
17921 = true;
17922 if (j4array
17923 [3]
17924 > IKPI)
17925 {
17926 j4array
17927 [3]
17928 -= IK2PI;
17929 }
17930 else if (
17931 j4array
17932 [3]
17933 < -IKPI)
17934 {
17935 j4array
17936 [3]
17937 += IK2PI;
17938 }
17939 j4valid
17940 [3]
17941 = true;
17942 for (
17943 int ij4
17944 = 0;
17945 ij4
17946 < 4;
17947 ++ij4)
17948 {
17949 if (!j4valid
17950 [ij4])
17951 {
17952 continue;
17953 }
17954 _ij4[0]
17955 = ij4;
17956 _ij4[1]
17957 = -1;
17958 for (
17959 int iij4
17960 = ij4
17961 + 1;
17962 iij4
17963 < 4;
17964 ++iij4)
17965 {
17966 if (j4valid
17967 [iij4]
17968 && IKabs(
17969 cj4array
17970 [ij4]
17971 - cj4array
17972 [iij4])
17973 < IKFAST_SOLUTION_THRESH
17974 && IKabs(
17975 sj4array
17976 [ij4]
17977 - sj4array
17978 [iij4])
17979 < IKFAST_SOLUTION_THRESH)
17980 {
17981 j4valid
17982 [iij4]
17983 = false;
17984 _ij4[1]
17985 = iij4;
17986 break;
17987 }
17988 }
17989 j4 = j4array
17990 [ij4];
17991 cj4 = cj4array
17992 [ij4];
17993 sj4 = sj4array
17994 [ij4];
17995
17996 rotationfunction0(
17997 solutions);
17998 }
17999 }
18000 }
18001 } while (
18002 0);
18003 if (bgotonextstatement)
18004 {
18005 bool
18006 bgotonextstatement
18007 = true;
18008 do
18009 {
18010 if (1)
18011 {
18012 bgotonextstatement
18013 = false;
18014 continue; // branch miss [j4]
18015 }
18016 } while (
18017 0);
18018 if (bgotonextstatement)
18019 {
18020 }
18021 }
18022 }
18023 }
18024 else
18025 {
18026 {
18027 IkReal j4array
18028 [1],
18029 cj4array
18030 [1],
18031 sj4array
18032 [1];
18033 bool j4valid
18034 [1]
18035 = {false};
18036 _nj4
18037 = 1;
18038 IkReal
18039 x1439
18040 = ((100.0)
18041 * pp);
18042 IkReal
18043 x1440
18044 = ((110.0)
18045 * pz);
18046 CheckValue<IkReal> x1441 = IKPowWithIntegerCheck(
18047 IKsign((
18048 (((-1.0)
18049 * (9.0)
18050 * (pz
18051 * pz)))
18052 + (((9.0)
18053 * pp)))),
18054 -1);
18055 if (!x1441
18056 .valid)
18057 {
18058 continue;
18059 }
18060 CheckValue<IkReal> x1442 = IKatan2WithCheck(
18061 IkReal((
18062 ((py
18063 * x1440))
18064 + (((-1.0)
18065 * (21.25)
18066 * py))
18067 + (((-1.0)
18068 * py
18069 * x1439)))),
18070 (((px
18071 * x1440))
18072 + (((-1.0)
18073 * (21.25)
18074 * px))
18075 + (((-1.0)
18076 * px
18077 * x1439))),
18078 IKFAST_ATAN2_MAGTHRESH);
18079 if (!x1442
18080 .valid)
18081 {
18082 continue;
18083 }
18084 j4array
18085 [0]
18086 = ((-1.5707963267949)
18087 + (((1.5707963267949)
18088 * (x1441
18089 .value)))
18090 + (x1442
18091 .value));
18092 sj4array
18093 [0]
18094 = IKsin(
18095 j4array
18096 [0]);
18097 cj4array
18098 [0]
18099 = IKcos(
18100 j4array
18101 [0]);
18102 if (j4array
18103 [0]
18104 > IKPI)
18105 {
18106 j4array
18107 [0]
18108 -= IK2PI;
18109 }
18110 else if (
18111 j4array
18112 [0]
18113 < -IKPI)
18114 {
18115 j4array
18116 [0]
18117 += IK2PI;
18118 }
18119 j4valid
18120 [0]
18121 = true;
18122 for (
18123 int ij4
18124 = 0;
18125 ij4
18126 < 1;
18127 ++ij4)
18128 {
18129 if (!j4valid
18130 [ij4])
18131 {
18132 continue;
18133 }
18134 _ij4[0]
18135 = ij4;
18136 _ij4[1]
18137 = -1;
18138 for (
18139 int iij4
18140 = ij4
18141 + 1;
18142 iij4
18143 < 1;
18144 ++iij4)
18145 {
18146 if (j4valid
18147 [iij4]
18148 && IKabs(
18149 cj4array
18150 [ij4]
18151 - cj4array
18152 [iij4])
18153 < IKFAST_SOLUTION_THRESH
18154 && IKabs(
18155 sj4array
18156 [ij4]
18157 - sj4array
18158 [iij4])
18159 < IKFAST_SOLUTION_THRESH)
18160 {
18161 j4valid
18162 [iij4]
18163 = false;
18164 _ij4[1]
18165 = iij4;
18166 break;
18167 }
18168 }
18169 j4 = j4array
18170 [ij4];
18171 cj4 = cj4array
18172 [ij4];
18173 sj4 = sj4array
18174 [ij4];
18175 {
18176 IkReal evalcond
18177 [3];
18178 IkReal
18179 x1443
18180 = IKsin(
18181 j4);
18182 IkReal
18183 x1444
18184 = IKcos(
18185 j4);
18186 IkReal
18187 x1445
18188 = (px
18189 * x1444);
18190 IkReal
18191 x1446
18192 = (py
18193 * x1443);
18194 evalcond
18195 [0]
18196 = ((((-1.0)
18197 * py
18198 * x1444))
18199 + ((px
18200 * x1443)));
18201 evalcond
18202 [1]
18203 = ((-1.51009803921569)
18204 + (((-1.0)
18205 * (1.32323529411765)
18206 * cj9))
18207 + (((3.92156862745098)
18208 * pp))
18209 + x1446
18210 + x1445);
18211 evalcond
18212 [2]
18213 = ((-0.2125)
18214 + (((1.1)
18215 * pz))
18216 + (((-0.09)
18217 * x1446))
18218 + (((-1.0)
18219 * (1.0)
18220 * pp))
18221 + (((-0.09)
18222 * x1445)));
18223 if (IKabs(
18224 evalcond
18225 [0])
18226 > IKFAST_EVALCOND_THRESH
18227 || IKabs(
18228 evalcond
18229 [1])
18230 > IKFAST_EVALCOND_THRESH
18231 || IKabs(
18232 evalcond
18233 [2])
18234 > IKFAST_EVALCOND_THRESH)
18235 {
18236 continue;
18237 }
18238 }
18239
18240 rotationfunction0(
18241 solutions);
18242 }
18243 }
18244 }
18245 }
18246 }
18247 else
18248 {
18249 {
18250 IkReal j4array
18251 [1],
18252 cj4array
18253 [1],
18254 sj4array
18255 [1];
18256 bool j4valid
18257 [1]
18258 = {false};
18259 _nj4 = 1;
18260 IkReal
18261 x1447
18262 = ((1.32323529411765)
18263 * cj9);
18264 IkReal
18265 x1448
18266 = ((3.92156862745098)
18267 * pp);
18268 CheckValue<IkReal> x1449 = IKPowWithIntegerCheck(
18269 IKsign((
18270 pp
18271 + (((-1.0)
18272 * (1.0)
18273 * (pz
18274 * pz))))),
18275 -1);
18276 if (!x1449
18277 .valid)
18278 {
18279 continue;
18280 }
18281 CheckValue<IkReal> x1450 = IKatan2WithCheck(
18282 IkReal((
18283 (((-1.0)
18284 * py
18285 * x1448))
18286 + (((1.51009803921569)
18287 * py))
18288 + ((py
18289 * x1447)))),
18290 ((((-1.0)
18291 * px
18292 * x1448))
18293 + (((1.51009803921569)
18294 * px))
18295 + ((px
18296 * x1447))),
18297 IKFAST_ATAN2_MAGTHRESH);
18298 if (!x1450
18299 .valid)
18300 {
18301 continue;
18302 }
18303 j4array[0]
18304 = ((-1.5707963267949)
18305 + (((1.5707963267949)
18306 * (x1449
18307 .value)))
18308 + (x1450
18309 .value));
18310 sj4array[0] = IKsin(
18311 j4array
18312 [0]);
18313 cj4array[0] = IKcos(
18314 j4array
18315 [0]);
18316 if (j4array
18317 [0]
18318 > IKPI)
18319 {
18320 j4array
18321 [0]
18322 -= IK2PI;
18323 }
18324 else if (
18325 j4array
18326 [0]
18327 < -IKPI)
18328 {
18329 j4array
18330 [0]
18331 += IK2PI;
18332 }
18333 j4valid[0]
18334 = true;
18335 for (
18336 int ij4
18337 = 0;
18338 ij4
18339 < 1;
18340 ++ij4)
18341 {
18342 if (!j4valid
18343 [ij4])
18344 {
18345 continue;
18346 }
18347 _ij4[0]
18348 = ij4;
18349 _ij4[1]
18350 = -1;
18351 for (
18352 int iij4
18353 = ij4
18354 + 1;
18355 iij4
18356 < 1;
18357 ++iij4)
18358 {
18359 if (j4valid
18360 [iij4]
18361 && IKabs(
18362 cj4array
18363 [ij4]
18364 - cj4array
18365 [iij4])
18366 < IKFAST_SOLUTION_THRESH
18367 && IKabs(
18368 sj4array
18369 [ij4]
18370 - sj4array
18371 [iij4])
18372 < IKFAST_SOLUTION_THRESH)
18373 {
18374 j4valid
18375 [iij4]
18376 = false;
18377 _ij4[1]
18378 = iij4;
18379 break;
18380 }
18381 }
18382 j4 = j4array
18383 [ij4];
18384 cj4 = cj4array
18385 [ij4];
18386 sj4 = sj4array
18387 [ij4];
18388 {
18389 IkReal evalcond
18390 [3];
18391 IkReal
18392 x1451
18393 = IKsin(
18394 j4);
18395 IkReal
18396 x1452
18397 = IKcos(
18398 j4);
18399 IkReal
18400 x1453
18401 = (px
18402 * x1452);
18403 IkReal
18404 x1454
18405 = (py
18406 * x1451);
18407 evalcond
18408 [0]
18409 = (((px
18410 * x1451))
18411 + (((-1.0)
18412 * py
18413 * x1452)));
18414 evalcond
18415 [1]
18416 = ((-1.51009803921569)
18417 + (((-1.0)
18418 * (1.32323529411765)
18419 * cj9))
18420 + (((3.92156862745098)
18421 * pp))
18422 + x1453
18423 + x1454);
18424 evalcond
18425 [2]
18426 = ((-0.2125)
18427 + (((-0.09)
18428 * x1453))
18429 + (((1.1)
18430 * pz))
18431 + (((-1.0)
18432 * (1.0)
18433 * pp))
18434 + (((-0.09)
18435 * x1454)));
18436 if (IKabs(
18437 evalcond
18438 [0])
18439 > IKFAST_EVALCOND_THRESH
18440 || IKabs(
18441 evalcond
18442 [1])
18443 > IKFAST_EVALCOND_THRESH
18444 || IKabs(
18445 evalcond
18446 [2])
18447 > IKFAST_EVALCOND_THRESH)
18448 {
18449 continue;
18450 }
18451 }
18452
18453 rotationfunction0(
18454 solutions);
18455 }
18456 }
18457 }
18458 }
18459 }
18460 } while (0);
18461 if (bgotonextstatement)
18462 {
18463 bool
18464 bgotonextstatement
18465 = true;
18466 do
18467 {
18468 evalcond[0]
18469 = ((-3.14159265358979)
18470 + (IKfmod(
18471 ((3.14159265358979)
18472 + (IKabs((
18473 (-1.5707963267949)
18474 + j8)))),
18475 6.28318530717959)));
18476 if (IKabs(
18477 evalcond
18478 [0])
18479 < 0.0000010000000000)
18480 {
18481 bgotonextstatement
18482 = false;
18483 {
18484 IkReal
18485 j4eval
18486 [3];
18487 sj6 = 0;
18488 cj6 = 1.0;
18489 j6 = 0;
18490 sj8 = 1.0;
18491 cj8 = 0;
18492 j8 = 1.5707963267949;
18493 IkReal x1455
18494 = (pp
18495 + (((-1.0)
18496 * (1.0)
18497 * (pz
18498 * pz))));
18499 IkReal x1456
18500 = ((13497.0)
18501 * cj9);
18502 IkReal x1457
18503 = ((40000.0)
18504 * pp);
18505 j4eval[0]
18506 = x1455;
18507 j4eval[1]
18508 = ((IKabs((
18509 (((15403.0)
18510 * px))
18511 + (((-1.0)
18512 * px
18513 * x1457))
18514 + ((px
18515 * x1456)))))
18516 + (IKabs((
18517 (((-1.0)
18518 * py
18519 * x1456))
18520 + ((py
18521 * x1457))
18522 + (((-1.0)
18523 * (15403.0)
18524 * py))))));
18525 j4eval[2]
18526 = IKsign(
18527 x1455);
18528 if (IKabs(
18529 j4eval
18530 [0])
18531 < 0.0000010000000000
18532 || IKabs(
18533 j4eval
18534 [1])
18535 < 0.0000010000000000
18536 || IKabs(
18537 j4eval
18538 [2])
18539 < 0.0000010000000000)
18540 {
18541 {
18542 IkReal j4eval
18543 [3];
18544 sj6 = 0;
18545 cj6 = 1.0;
18546 j6 = 0;
18547 sj8 = 1.0;
18548 cj8 = 0;
18549 j8 = 1.5707963267949;
18550 IkReal
18551 x1458
18552 = pz
18553 * pz;
18554 IkReal
18555 x1459
18556 = ((80.0)
18557 * pp);
18558 IkReal
18559 x1460
18560 = ((88.0)
18561 * pz);
18562 j4eval
18563 [0]
18564 = (pp
18565 + (((-1.0)
18566 * x1458)));
18567 j4eval[1] = IKsign((
18568 (((-9.0)
18569 * x1458))
18570 + (((9.0)
18571 * pp))));
18572 j4eval
18573 [2]
18574 = ((IKabs((
18575 (((-1.0)
18576 * py
18577 * x1460))
18578 + ((py
18579 * x1459))
18580 + (((17.0)
18581 * py)))))
18582 + (IKabs((
18583 (((-1.0)
18584 * (17.0)
18585 * px))
18586 + (((-1.0)
18587 * px
18588 * x1459))
18589 + ((px
18590 * x1460))))));
18591 if (IKabs(
18592 j4eval
18593 [0])
18594 < 0.0000010000000000
18595 || IKabs(
18596 j4eval
18597 [1])
18598 < 0.0000010000000000
18599 || IKabs(
18600 j4eval
18601 [2])
18602 < 0.0000010000000000)
18603 {
18604 {
18605 IkReal evalcond
18606 [5];
18607 bool
18608 bgotonextstatement
18609 = true;
18610 do
18611 {
18612 IkReal
18613 x1461
18614 = ((-1.51009803921569)
18615 + (((-1.0)
18616 * (1.32323529411765)
18617 * cj9))
18618 + (((3.92156862745098)
18619 * pp)));
18620 evalcond
18621 [0]
18622 = ((IKabs(
18623 py))
18624 + (IKabs(
18625 px)));
18626 evalcond
18627 [1]
18628 = x1461;
18629 evalcond
18630 [2]
18631 = 0;
18632 evalcond
18633 [3]
18634 = x1461;
18635 evalcond
18636 [4]
18637 = ((-0.2125)
18638 + (((1.1)
18639 * pz))
18640 + (((-1.0)
18641 * (1.0)
18642 * pp)));
18643 if (IKabs(
18644 evalcond
18645 [0])
18646 < 0.0000010000000000
18647 && IKabs(
18648 evalcond
18649 [1])
18650 < 0.0000010000000000
18651 && IKabs(
18652 evalcond
18653 [2])
18654 < 0.0000010000000000
18655 && IKabs(
18656 evalcond
18657 [3])
18658 < 0.0000010000000000
18659 && IKabs(
18660 evalcond
18661 [4])
18662 < 0.0000010000000000)
18663 {
18664 bgotonextstatement
18665 = false;
18666 {
18667 IkReal j4array
18668 [4],
18669 cj4array
18670 [4],
18671 sj4array
18672 [4];
18673 bool j4valid
18674 [4]
18675 = {false};
18676 _nj4
18677 = 4;
18678 j4array
18679 [0]
18680 = 0;
18681 sj4array
18682 [0]
18683 = IKsin(
18684 j4array
18685 [0]);
18686 cj4array
18687 [0]
18688 = IKcos(
18689 j4array
18690 [0]);
18691 j4array
18692 [1]
18693 = 1.5707963267949;
18694 sj4array
18695 [1]
18696 = IKsin(
18697 j4array
18698 [1]);
18699 cj4array
18700 [1]
18701 = IKcos(
18702 j4array
18703 [1]);
18704 j4array
18705 [2]
18706 = 3.14159265358979;
18707 sj4array
18708 [2]
18709 = IKsin(
18710 j4array
18711 [2]);
18712 cj4array
18713 [2]
18714 = IKcos(
18715 j4array
18716 [2]);
18717 j4array
18718 [3]
18719 = -1.5707963267949;
18720 sj4array
18721 [3]
18722 = IKsin(
18723 j4array
18724 [3]);
18725 cj4array
18726 [3]
18727 = IKcos(
18728 j4array
18729 [3]);
18730 if (j4array
18731 [0]
18732 > IKPI)
18733 {
18734 j4array
18735 [0]
18736 -= IK2PI;
18737 }
18738 else if (
18739 j4array
18740 [0]
18741 < -IKPI)
18742 {
18743 j4array
18744 [0]
18745 += IK2PI;
18746 }
18747 j4valid
18748 [0]
18749 = true;
18750 if (j4array
18751 [1]
18752 > IKPI)
18753 {
18754 j4array
18755 [1]
18756 -= IK2PI;
18757 }
18758 else if (
18759 j4array
18760 [1]
18761 < -IKPI)
18762 {
18763 j4array
18764 [1]
18765 += IK2PI;
18766 }
18767 j4valid
18768 [1]
18769 = true;
18770 if (j4array
18771 [2]
18772 > IKPI)
18773 {
18774 j4array
18775 [2]
18776 -= IK2PI;
18777 }
18778 else if (
18779 j4array
18780 [2]
18781 < -IKPI)
18782 {
18783 j4array
18784 [2]
18785 += IK2PI;
18786 }
18787 j4valid
18788 [2]
18789 = true;
18790 if (j4array
18791 [3]
18792 > IKPI)
18793 {
18794 j4array
18795 [3]
18796 -= IK2PI;
18797 }
18798 else if (
18799 j4array
18800 [3]
18801 < -IKPI)
18802 {
18803 j4array
18804 [3]
18805 += IK2PI;
18806 }
18807 j4valid
18808 [3]
18809 = true;
18810 for (
18811 int ij4
18812 = 0;
18813 ij4
18814 < 4;
18815 ++ij4)
18816 {
18817 if (!j4valid
18818 [ij4])
18819 {
18820 continue;
18821 }
18822 _ij4[0]
18823 = ij4;
18824 _ij4[1]
18825 = -1;
18826 for (
18827 int iij4
18828 = ij4
18829 + 1;
18830 iij4
18831 < 4;
18832 ++iij4)
18833 {
18834 if (j4valid
18835 [iij4]
18836 && IKabs(
18837 cj4array
18838 [ij4]
18839 - cj4array
18840 [iij4])
18841 < IKFAST_SOLUTION_THRESH
18842 && IKabs(
18843 sj4array
18844 [ij4]
18845 - sj4array
18846 [iij4])
18847 < IKFAST_SOLUTION_THRESH)
18848 {
18849 j4valid
18850 [iij4]
18851 = false;
18852 _ij4[1]
18853 = iij4;
18854 break;
18855 }
18856 }
18857 j4 = j4array
18858 [ij4];
18859 cj4 = cj4array
18860 [ij4];
18861 sj4 = sj4array
18862 [ij4];
18863
18864 rotationfunction0(
18865 solutions);
18866 }
18867 }
18868 }
18869 } while (
18870 0);
18871 if (bgotonextstatement)
18872 {
18873 bool
18874 bgotonextstatement
18875 = true;
18876 do
18877 {
18878 if (1)
18879 {
18880 bgotonextstatement
18881 = false;
18882 continue; // branch miss [j4]
18883 }
18884 } while (
18885 0);
18886 if (bgotonextstatement)
18887 {
18888 }
18889 }
18890 }
18891 }
18892 else
18893 {
18894 {
18895 IkReal j4array
18896 [1],
18897 cj4array
18898 [1],
18899 sj4array
18900 [1];
18901 bool j4valid
18902 [1]
18903 = {false};
18904 _nj4
18905 = 1;
18906 IkReal
18907 x1462
18908 = ((100.0)
18909 * pp);
18910 IkReal
18911 x1463
18912 = ((110.0)
18913 * pz);
18914 CheckValue<IkReal> x1464 = IKPowWithIntegerCheck(
18915 IKsign((
18916 (((-1.0)
18917 * (9.0)
18918 * (pz
18919 * pz)))
18920 + (((9.0)
18921 * pp)))),
18922 -1);
18923 if (!x1464
18924 .valid)
18925 {
18926 continue;
18927 }
18928 CheckValue<IkReal> x1465 = IKatan2WithCheck(
18929 IkReal((
18930 (((-1.0)
18931 * px
18932 * x1462))
18933 + (((-1.0)
18934 * (21.25)
18935 * px))
18936 + ((px
18937 * x1463)))),
18938 (((py
18939 * x1462))
18940 + (((21.25)
18941 * py))
18942 + (((-1.0)
18943 * py
18944 * x1463))),
18945 IKFAST_ATAN2_MAGTHRESH);
18946 if (!x1465
18947 .valid)
18948 {
18949 continue;
18950 }
18951 j4array
18952 [0]
18953 = ((-1.5707963267949)
18954 + (((1.5707963267949)
18955 * (x1464
18956 .value)))
18957 + (x1465
18958 .value));
18959 sj4array
18960 [0]
18961 = IKsin(
18962 j4array
18963 [0]);
18964 cj4array
18965 [0]
18966 = IKcos(
18967 j4array
18968 [0]);
18969 if (j4array
18970 [0]
18971 > IKPI)
18972 {
18973 j4array
18974 [0]
18975 -= IK2PI;
18976 }
18977 else if (
18978 j4array
18979 [0]
18980 < -IKPI)
18981 {
18982 j4array
18983 [0]
18984 += IK2PI;
18985 }
18986 j4valid
18987 [0]
18988 = true;
18989 for (
18990 int ij4
18991 = 0;
18992 ij4
18993 < 1;
18994 ++ij4)
18995 {
18996 if (!j4valid
18997 [ij4])
18998 {
18999 continue;
19000 }
19001 _ij4[0]
19002 = ij4;
19003 _ij4[1]
19004 = -1;
19005 for (
19006 int iij4
19007 = ij4
19008 + 1;
19009 iij4
19010 < 1;
19011 ++iij4)
19012 {
19013 if (j4valid
19014 [iij4]
19015 && IKabs(
19016 cj4array
19017 [ij4]
19018 - cj4array
19019 [iij4])
19020 < IKFAST_SOLUTION_THRESH
19021 && IKabs(
19022 sj4array
19023 [ij4]
19024 - sj4array
19025 [iij4])
19026 < IKFAST_SOLUTION_THRESH)
19027 {
19028 j4valid
19029 [iij4]
19030 = false;
19031 _ij4[1]
19032 = iij4;
19033 break;
19034 }
19035 }
19036 j4 = j4array
19037 [ij4];
19038 cj4 = cj4array
19039 [ij4];
19040 sj4 = sj4array
19041 [ij4];
19042 {
19043 IkReal evalcond
19044 [3];
19045 IkReal
19046 x1466
19047 = IKcos(
19048 j4);
19049 IkReal
19050 x1467
19051 = IKsin(
19052 j4);
19053 IkReal
19054 x1468
19055 = (px
19056 * x1467);
19057 IkReal
19058 x1469
19059 = (py
19060 * x1466);
19061 evalcond
19062 [0]
19063 = (((py
19064 * x1467))
19065 + ((px
19066 * x1466)));
19067 evalcond
19068 [1]
19069 = ((-1.51009803921569)
19070 + (((-1.0)
19071 * x1469))
19072 + (((-1.0)
19073 * (1.32323529411765)
19074 * cj9))
19075 + (((3.92156862745098)
19076 * pp))
19077 + x1468);
19078 evalcond
19079 [2]
19080 = ((-0.2125)
19081 + (((1.1)
19082 * pz))
19083 + (((0.09)
19084 * x1469))
19085 + (((-0.09)
19086 * x1468))
19087 + (((-1.0)
19088 * (1.0)
19089 * pp)));
19090 if (IKabs(
19091 evalcond
19092 [0])
19093 > IKFAST_EVALCOND_THRESH
19094 || IKabs(
19095 evalcond
19096 [1])
19097 > IKFAST_EVALCOND_THRESH
19098 || IKabs(
19099 evalcond
19100 [2])
19101 > IKFAST_EVALCOND_THRESH)
19102 {
19103 continue;
19104 }
19105 }
19106
19107 rotationfunction0(
19108 solutions);
19109 }
19110 }
19111 }
19112 }
19113 }
19114 else
19115 {
19116 {
19117 IkReal j4array
19118 [1],
19119 cj4array
19120 [1],
19121 sj4array
19122 [1];
19123 bool j4valid
19124 [1]
19125 = {false};
19126 _nj4
19127 = 1;
19128 IkReal
19129 x1470
19130 = ((1.32323529411765)
19131 * cj9);
19132 IkReal
19133 x1471
19134 = ((3.92156862745098)
19135 * pp);
19136 CheckValue<IkReal> x1472 = IKPowWithIntegerCheck(
19137 IKsign((
19138 pp
19139 + (((-1.0)
19140 * (1.0)
19141 * (pz
19142 * pz))))),
19143 -1);
19144 if (!x1472
19145 .valid)
19146 {
19147 continue;
19148 }
19149 CheckValue<IkReal> x1473 = IKatan2WithCheck(
19150 IkReal((
19151 ((px
19152 * x1470))
19153 + (((-1.0)
19154 * px
19155 * x1471))
19156 + (((1.51009803921569)
19157 * px)))),
19158 (((py
19159 * x1471))
19160 + (((-1.0)
19161 * (1.51009803921569)
19162 * py))
19163 + (((-1.0)
19164 * py
19165 * x1470))),
19166 IKFAST_ATAN2_MAGTHRESH);
19167 if (!x1473
19168 .valid)
19169 {
19170 continue;
19171 }
19172 j4array
19173 [0]
19174 = ((-1.5707963267949)
19175 + (((1.5707963267949)
19176 * (x1472
19177 .value)))
19178 + (x1473
19179 .value));
19180 sj4array
19181 [0]
19182 = IKsin(
19183 j4array
19184 [0]);
19185 cj4array
19186 [0]
19187 = IKcos(
19188 j4array
19189 [0]);
19190 if (j4array
19191 [0]
19192 > IKPI)
19193 {
19194 j4array
19195 [0]
19196 -= IK2PI;
19197 }
19198 else if (
19199 j4array
19200 [0]
19201 < -IKPI)
19202 {
19203 j4array
19204 [0]
19205 += IK2PI;
19206 }
19207 j4valid
19208 [0]
19209 = true;
19210 for (
19211 int ij4
19212 = 0;
19213 ij4
19214 < 1;
19215 ++ij4)
19216 {
19217 if (!j4valid
19218 [ij4])
19219 {
19220 continue;
19221 }
19222 _ij4[0]
19223 = ij4;
19224 _ij4[1]
19225 = -1;
19226 for (
19227 int iij4
19228 = ij4
19229 + 1;
19230 iij4
19231 < 1;
19232 ++iij4)
19233 {
19234 if (j4valid
19235 [iij4]
19236 && IKabs(
19237 cj4array
19238 [ij4]
19239 - cj4array
19240 [iij4])
19241 < IKFAST_SOLUTION_THRESH
19242 && IKabs(
19243 sj4array
19244 [ij4]
19245 - sj4array
19246 [iij4])
19247 < IKFAST_SOLUTION_THRESH)
19248 {
19249 j4valid
19250 [iij4]
19251 = false;
19252 _ij4[1]
19253 = iij4;
19254 break;
19255 }
19256 }
19257 j4 = j4array
19258 [ij4];
19259 cj4 = cj4array
19260 [ij4];
19261 sj4 = sj4array
19262 [ij4];
19263 {
19264 IkReal evalcond
19265 [3];
19266 IkReal
19267 x1474
19268 = IKcos(
19269 j4);
19270 IkReal
19271 x1475
19272 = IKsin(
19273 j4);
19274 IkReal
19275 x1476
19276 = (px
19277 * x1475);
19278 IkReal
19279 x1477
19280 = (py
19281 * x1474);
19282 evalcond
19283 [0]
19284 = (((px
19285 * x1474))
19286 + ((py
19287 * x1475)));
19288 evalcond
19289 [1]
19290 = ((-1.51009803921569)
19291 + (((-1.0)
19292 * (1.32323529411765)
19293 * cj9))
19294 + (((3.92156862745098)
19295 * pp))
19296 + (((-1.0)
19297 * x1477))
19298 + x1476);
19299 evalcond
19300 [2]
19301 = ((-0.2125)
19302 + (((-0.09)
19303 * x1476))
19304 + (((0.09)
19305 * x1477))
19306 + (((1.1)
19307 * pz))
19308 + (((-1.0)
19309 * (1.0)
19310 * pp)));
19311 if (IKabs(
19312 evalcond
19313 [0])
19314 > IKFAST_EVALCOND_THRESH
19315 || IKabs(
19316 evalcond
19317 [1])
19318 > IKFAST_EVALCOND_THRESH
19319 || IKabs(
19320 evalcond
19321 [2])
19322 > IKFAST_EVALCOND_THRESH)
19323 {
19324 continue;
19325 }
19326 }
19327
19328 rotationfunction0(
19329 solutions);
19330 }
19331 }
19332 }
19333 }
19334 }
19335 } while (0);
19336 if (bgotonextstatement)
19337 {
19338 bool
19339 bgotonextstatement
19340 = true;
19341 do
19342 {
19343 evalcond[0]
19344 = ((-3.14159265358979)
19345 + (IKfmod(
19346 ((3.14159265358979)
19347 + (IKabs((
19348 (1.5707963267949)
19349 + j8)))),
19350 6.28318530717959)));
19351 if (IKabs(
19352 evalcond
19353 [0])
19354 < 0.0000010000000000)
19355 {
19356 bgotonextstatement
19357 = false;
19358 {
19359 IkReal j4eval
19360 [3];
19361 sj6 = 0;
19362 cj6 = 1.0;
19363 j6 = 0;
19364 sj8 = -1.0;
19365 cj8 = 0;
19366 j8 = -1.5707963267949;
19367 IkReal
19368 x1478
19369 = (pp
19370 + (((-1.0)
19371 * (1.0)
19372 * (pz
19373 * pz))));
19374 IkReal
19375 x1479
19376 = ((13497.0)
19377 * cj9);
19378 IkReal
19379 x1480
19380 = ((40000.0)
19381 * pp);
19382 j4eval[0]
19383 = x1478;
19384 j4eval[1]
19385 = ((IKabs((
19386 (((-1.0)
19387 * py
19388 * x1480))
19389 + (((15403.0)
19390 * py))
19391 + ((py
19392 * x1479)))))
19393 + (IKabs((
19394 (((-1.0)
19395 * px
19396 * x1479))
19397 + ((px
19398 * x1480))
19399 + (((-1.0)
19400 * (15403.0)
19401 * px))))));
19402 j4eval[2]
19403 = IKsign(
19404 x1478);
19405 if (IKabs(
19406 j4eval
19407 [0])
19408 < 0.0000010000000000
19409 || IKabs(
19410 j4eval
19411 [1])
19412 < 0.0000010000000000
19413 || IKabs(
19414 j4eval
19415 [2])
19416 < 0.0000010000000000)
19417 {
19418 {
19419 IkReal j4eval
19420 [3];
19421 sj6 = 0;
19422 cj6 = 1.0;
19423 j6 = 0;
19424 sj8 = -1.0;
19425 cj8 = 0;
19426 j8 = -1.5707963267949;
19427 IkReal
19428 x1481
19429 = pz
19430 * pz;
19431 IkReal
19432 x1482
19433 = ((80.0)
19434 * pp);
19435 IkReal
19436 x1483
19437 = ((88.0)
19438 * pz);
19439 j4eval
19440 [0]
19441 = ((((-1.0)
19442 * x1481))
19443 + pp);
19444 j4eval
19445 [1]
19446 = ((IKabs((
19447 (((-1.0)
19448 * py
19449 * x1482))
19450 + ((py
19451 * x1483))
19452 + (((-1.0)
19453 * (17.0)
19454 * py)))))
19455 + (IKabs((
19456 (((17.0)
19457 * px))
19458 + ((px
19459 * x1482))
19460 + (((-1.0)
19461 * px
19462 * x1483))))));
19463 j4eval[2] = IKsign((
19464 (((-9.0)
19465 * x1481))
19466 + (((9.0)
19467 * pp))));
19468 if (IKabs(
19469 j4eval
19470 [0])
19471 < 0.0000010000000000
19472 || IKabs(
19473 j4eval
19474 [1])
19475 < 0.0000010000000000
19476 || IKabs(
19477 j4eval
19478 [2])
19479 < 0.0000010000000000)
19480 {
19481 {
19482 IkReal evalcond
19483 [5];
19484 bool
19485 bgotonextstatement
19486 = true;
19487 do
19488 {
19489 IkReal
19490 x1484
19491 = ((1.32323529411765)
19492 * cj9);
19493 IkReal
19494 x1485
19495 = ((3.92156862745098)
19496 * pp);
19497 evalcond
19498 [0]
19499 = ((IKabs(
19500 py))
19501 + (IKabs(
19502 px)));
19503 evalcond
19504 [1]
19505 = ((1.51009803921569)
19506 + (((-1.0)
19507 * x1485))
19508 + x1484);
19509 evalcond
19510 [2]
19511 = 0;
19512 evalcond
19513 [3]
19514 = ((-1.51009803921569)
19515 + x1485
19516 + (((-1.0)
19517 * x1484)));
19518 evalcond
19519 [4]
19520 = ((-0.2125)
19521 + (((1.1)
19522 * pz))
19523 + (((-1.0)
19524 * (1.0)
19525 * pp)));
19526 if (IKabs(
19527 evalcond
19528 [0])
19529 < 0.0000010000000000
19530 && IKabs(
19531 evalcond
19532 [1])
19533 < 0.0000010000000000
19534 && IKabs(
19535 evalcond
19536 [2])
19537 < 0.0000010000000000
19538 && IKabs(
19539 evalcond
19540 [3])
19541 < 0.0000010000000000
19542 && IKabs(
19543 evalcond
19544 [4])
19545 < 0.0000010000000000)
19546 {
19547 bgotonextstatement
19548 = false;
19549 {
19550 IkReal j4array
19551 [4],
19552 cj4array
19553 [4],
19554 sj4array
19555 [4];
19556 bool j4valid
19557 [4]
19558 = {false};
19559 _nj4
19560 = 4;
19561 j4array
19562 [0]
19563 = 0;
19564 sj4array
19565 [0]
19566 = IKsin(
19567 j4array
19568 [0]);
19569 cj4array
19570 [0]
19571 = IKcos(
19572 j4array
19573 [0]);
19574 j4array
19575 [1]
19576 = 1.5707963267949;
19577 sj4array
19578 [1]
19579 = IKsin(
19580 j4array
19581 [1]);
19582 cj4array
19583 [1]
19584 = IKcos(
19585 j4array
19586 [1]);
19587 j4array
19588 [2]
19589 = 3.14159265358979;
19590 sj4array
19591 [2]
19592 = IKsin(
19593 j4array
19594 [2]);
19595 cj4array
19596 [2]
19597 = IKcos(
19598 j4array
19599 [2]);
19600 j4array
19601 [3]
19602 = -1.5707963267949;
19603 sj4array
19604 [3]
19605 = IKsin(
19606 j4array
19607 [3]);
19608 cj4array
19609 [3]
19610 = IKcos(
19611 j4array
19612 [3]);
19613 if (j4array
19614 [0]
19615 > IKPI)
19616 {
19617 j4array
19618 [0]
19619 -= IK2PI;
19620 }
19621 else if (
19622 j4array
19623 [0]
19624 < -IKPI)
19625 {
19626 j4array
19627 [0]
19628 += IK2PI;
19629 }
19630 j4valid
19631 [0]
19632 = true;
19633 if (j4array
19634 [1]
19635 > IKPI)
19636 {
19637 j4array
19638 [1]
19639 -= IK2PI;
19640 }
19641 else if (
19642 j4array
19643 [1]
19644 < -IKPI)
19645 {
19646 j4array
19647 [1]
19648 += IK2PI;
19649 }
19650 j4valid
19651 [1]
19652 = true;
19653 if (j4array
19654 [2]
19655 > IKPI)
19656 {
19657 j4array
19658 [2]
19659 -= IK2PI;
19660 }
19661 else if (
19662 j4array
19663 [2]
19664 < -IKPI)
19665 {
19666 j4array
19667 [2]
19668 += IK2PI;
19669 }
19670 j4valid
19671 [2]
19672 = true;
19673 if (j4array
19674 [3]
19675 > IKPI)
19676 {
19677 j4array
19678 [3]
19679 -= IK2PI;
19680 }
19681 else if (
19682 j4array
19683 [3]
19684 < -IKPI)
19685 {
19686 j4array
19687 [3]
19688 += IK2PI;
19689 }
19690 j4valid
19691 [3]
19692 = true;
19693 for (
19694 int ij4
19695 = 0;
19696 ij4
19697 < 4;
19698 ++ij4)
19699 {
19700 if (!j4valid
19701 [ij4])
19702 {
19703 continue;
19704 }
19705 _ij4[0]
19706 = ij4;
19707 _ij4[1]
19708 = -1;
19709 for (
19710 int iij4
19711 = ij4
19712 + 1;
19713 iij4
19714 < 4;
19715 ++iij4)
19716 {
19717 if (j4valid
19718 [iij4]
19719 && IKabs(
19720 cj4array
19721 [ij4]
19722 - cj4array
19723 [iij4])
19724 < IKFAST_SOLUTION_THRESH
19725 && IKabs(
19726 sj4array
19727 [ij4]
19728 - sj4array
19729 [iij4])
19730 < IKFAST_SOLUTION_THRESH)
19731 {
19732 j4valid
19733 [iij4]
19734 = false;
19735 _ij4[1]
19736 = iij4;
19737 break;
19738 }
19739 }
19740 j4 = j4array
19741 [ij4];
19742 cj4 = cj4array
19743 [ij4];
19744 sj4 = sj4array
19745 [ij4];
19746
19747 rotationfunction0(
19748 solutions);
19749 }
19750 }
19751 }
19752 } while (
19753 0);
19754 if (bgotonextstatement)
19755 {
19756 bool
19757 bgotonextstatement
19758 = true;
19759 do
19760 {
19761 if (1)
19762 {
19763 bgotonextstatement
19764 = false;
19765 continue; // branch miss [j4]
19766 }
19767 } while (
19768 0);
19769 if (bgotonextstatement)
19770 {
19771 }
19772 }
19773 }
19774 }
19775 else
19776 {
19777 {
19778 IkReal j4array
19779 [1],
19780 cj4array
19781 [1],
19782 sj4array
19783 [1];
19784 bool j4valid
19785 [1]
19786 = {false};
19787 _nj4
19788 = 1;
19789 IkReal
19790 x1486
19791 = ((100.0)
19792 * pp);
19793 IkReal
19794 x1487
19795 = ((110.0)
19796 * pz);
19797 CheckValue<IkReal> x1488 = IKatan2WithCheck(
19798 IkReal((
19799 (((-1.0)
19800 * px
19801 * x1487))
19802 + ((px
19803 * x1486))
19804 + (((21.25)
19805 * px)))),
19806 ((((-1.0)
19807 * py
19808 * x1486))
19809 + (((-1.0)
19810 * (21.25)
19811 * py))
19812 + ((py
19813 * x1487))),
19814 IKFAST_ATAN2_MAGTHRESH);
19815 if (!x1488
19816 .valid)
19817 {
19818 continue;
19819 }
19820 CheckValue<IkReal> x1489 = IKPowWithIntegerCheck(
19821 IKsign((
19822 (((-1.0)
19823 * (9.0)
19824 * (pz
19825 * pz)))
19826 + (((9.0)
19827 * pp)))),
19828 -1);
19829 if (!x1489
19830 .valid)
19831 {
19832 continue;
19833 }
19834 j4array
19835 [0]
19836 = ((-1.5707963267949)
19837 + (x1488
19838 .value)
19839 + (((1.5707963267949)
19840 * (x1489
19841 .value))));
19842 sj4array
19843 [0]
19844 = IKsin(
19845 j4array
19846 [0]);
19847 cj4array
19848 [0]
19849 = IKcos(
19850 j4array
19851 [0]);
19852 if (j4array
19853 [0]
19854 > IKPI)
19855 {
19856 j4array
19857 [0]
19858 -= IK2PI;
19859 }
19860 else if (
19861 j4array
19862 [0]
19863 < -IKPI)
19864 {
19865 j4array
19866 [0]
19867 += IK2PI;
19868 }
19869 j4valid
19870 [0]
19871 = true;
19872 for (
19873 int ij4
19874 = 0;
19875 ij4
19876 < 1;
19877 ++ij4)
19878 {
19879 if (!j4valid
19880 [ij4])
19881 {
19882 continue;
19883 }
19884 _ij4[0]
19885 = ij4;
19886 _ij4[1]
19887 = -1;
19888 for (
19889 int iij4
19890 = ij4
19891 + 1;
19892 iij4
19893 < 1;
19894 ++iij4)
19895 {
19896 if (j4valid
19897 [iij4]
19898 && IKabs(
19899 cj4array
19900 [ij4]
19901 - cj4array
19902 [iij4])
19903 < IKFAST_SOLUTION_THRESH
19904 && IKabs(
19905 sj4array
19906 [ij4]
19907 - sj4array
19908 [iij4])
19909 < IKFAST_SOLUTION_THRESH)
19910 {
19911 j4valid
19912 [iij4]
19913 = false;
19914 _ij4[1]
19915 = iij4;
19916 break;
19917 }
19918 }
19919 j4 = j4array
19920 [ij4];
19921 cj4 = cj4array
19922 [ij4];
19923 sj4 = sj4array
19924 [ij4];
19925 {
19926 IkReal evalcond
19927 [3];
19928 IkReal
19929 x1490
19930 = IKcos(
19931 j4);
19932 IkReal
19933 x1491
19934 = ((1.0)
19935 * x1490);
19936 IkReal
19937 x1492
19938 = IKsin(
19939 j4);
19940 IkReal
19941 x1493
19942 = (px
19943 * x1492);
19944 evalcond
19945 [0]
19946 = ((((-1.0)
19947 * py
19948 * x1492))
19949 + (((-1.0)
19950 * px
19951 * x1491)));
19952 evalcond
19953 [1]
19954 = ((1.51009803921569)
19955 + x1493
19956 + (((-1.0)
19957 * py
19958 * x1491))
19959 + (((-1.0)
19960 * (3.92156862745098)
19961 * pp))
19962 + (((1.32323529411765)
19963 * cj9)));
19964 evalcond
19965 [2]
19966 = ((-0.2125)
19967 + (((1.1)
19968 * pz))
19969 + (((-1.0)
19970 * (1.0)
19971 * pp))
19972 + (((-0.09)
19973 * py
19974 * x1490))
19975 + (((0.09)
19976 * x1493)));
19977 if (IKabs(
19978 evalcond
19979 [0])
19980 > IKFAST_EVALCOND_THRESH
19981 || IKabs(
19982 evalcond
19983 [1])
19984 > IKFAST_EVALCOND_THRESH
19985 || IKabs(
19986 evalcond
19987 [2])
19988 > IKFAST_EVALCOND_THRESH)
19989 {
19990 continue;
19991 }
19992 }
19993
19994 rotationfunction0(
19995 solutions);
19996 }
19997 }
19998 }
19999 }
20000 }
20001 else
20002 {
20003 {
20004 IkReal j4array
20005 [1],
20006 cj4array
20007 [1],
20008 sj4array
20009 [1];
20010 bool j4valid
20011 [1]
20012 = {false};
20013 _nj4
20014 = 1;
20015 IkReal
20016 x1494
20017 = ((1.32323529411765)
20018 * cj9);
20019 IkReal
20020 x1495
20021 = ((3.92156862745098)
20022 * pp);
20023 CheckValue<IkReal> x1496 = IKatan2WithCheck(
20024 IkReal((
20025 (((-1.0)
20026 * (1.51009803921569)
20027 * px))
20028 + (((-1.0)
20029 * px
20030 * x1494))
20031 + ((px
20032 * x1495)))),
20033 ((((-1.0)
20034 * py
20035 * x1495))
20036 + (((1.51009803921569)
20037 * py))
20038 + ((py
20039 * x1494))),
20040 IKFAST_ATAN2_MAGTHRESH);
20041 if (!x1496
20042 .valid)
20043 {
20044 continue;
20045 }
20046 CheckValue<IkReal> x1497 = IKPowWithIntegerCheck(
20047 IKsign((
20048 pp
20049 + (((-1.0)
20050 * (1.0)
20051 * (pz
20052 * pz))))),
20053 -1);
20054 if (!x1497
20055 .valid)
20056 {
20057 continue;
20058 }
20059 j4array
20060 [0]
20061 = ((-1.5707963267949)
20062 + (x1496
20063 .value)
20064 + (((1.5707963267949)
20065 * (x1497
20066 .value))));
20067 sj4array
20068 [0]
20069 = IKsin(
20070 j4array
20071 [0]);
20072 cj4array
20073 [0]
20074 = IKcos(
20075 j4array
20076 [0]);
20077 if (j4array
20078 [0]
20079 > IKPI)
20080 {
20081 j4array
20082 [0]
20083 -= IK2PI;
20084 }
20085 else if (
20086 j4array
20087 [0]
20088 < -IKPI)
20089 {
20090 j4array
20091 [0]
20092 += IK2PI;
20093 }
20094 j4valid
20095 [0]
20096 = true;
20097 for (
20098 int ij4
20099 = 0;
20100 ij4
20101 < 1;
20102 ++ij4)
20103 {
20104 if (!j4valid
20105 [ij4])
20106 {
20107 continue;
20108 }
20109 _ij4[0]
20110 = ij4;
20111 _ij4[1]
20112 = -1;
20113 for (
20114 int iij4
20115 = ij4
20116 + 1;
20117 iij4
20118 < 1;
20119 ++iij4)
20120 {
20121 if (j4valid
20122 [iij4]
20123 && IKabs(
20124 cj4array
20125 [ij4]
20126 - cj4array
20127 [iij4])
20128 < IKFAST_SOLUTION_THRESH
20129 && IKabs(
20130 sj4array
20131 [ij4]
20132 - sj4array
20133 [iij4])
20134 < IKFAST_SOLUTION_THRESH)
20135 {
20136 j4valid
20137 [iij4]
20138 = false;
20139 _ij4[1]
20140 = iij4;
20141 break;
20142 }
20143 }
20144 j4 = j4array
20145 [ij4];
20146 cj4 = cj4array
20147 [ij4];
20148 sj4 = sj4array
20149 [ij4];
20150 {
20151 IkReal evalcond
20152 [3];
20153 IkReal
20154 x1498
20155 = IKcos(
20156 j4);
20157 IkReal
20158 x1499
20159 = ((1.0)
20160 * x1498);
20161 IkReal
20162 x1500
20163 = IKsin(
20164 j4);
20165 IkReal
20166 x1501
20167 = (px
20168 * x1500);
20169 evalcond
20170 [0]
20171 = ((((-1.0)
20172 * px
20173 * x1499))
20174 + (((-1.0)
20175 * py
20176 * x1500)));
20177 evalcond
20178 [1]
20179 = ((1.51009803921569)
20180 + (((-1.0)
20181 * (3.92156862745098)
20182 * pp))
20183 + (((-1.0)
20184 * py
20185 * x1499))
20186 + x1501
20187 + (((1.32323529411765)
20188 * cj9)));
20189 evalcond
20190 [2]
20191 = ((-0.2125)
20192 + (((-0.09)
20193 * py
20194 * x1498))
20195 + (((1.1)
20196 * pz))
20197 + (((-1.0)
20198 * (1.0)
20199 * pp))
20200 + (((0.09)
20201 * x1501)));
20202 if (IKabs(
20203 evalcond
20204 [0])
20205 > IKFAST_EVALCOND_THRESH
20206 || IKabs(
20207 evalcond
20208 [1])
20209 > IKFAST_EVALCOND_THRESH
20210 || IKabs(
20211 evalcond
20212 [2])
20213 > IKFAST_EVALCOND_THRESH)
20214 {
20215 continue;
20216 }
20217 }
20218
20219 rotationfunction0(
20220 solutions);
20221 }
20222 }
20223 }
20224 }
20225 }
20226 } while (0);
20227 if (bgotonextstatement)
20228 {
20229 bool
20230 bgotonextstatement
20231 = true;
20232 do
20233 {
20234 IkReal x1502
20235 = ((1.32323529411765)
20236 * cj9);
20237 IkReal x1503
20238 = ((3.92156862745098)
20239 * pp);
20240 evalcond[0]
20241 = ((IKabs(
20242 py))
20243 + (IKabs(
20244 px)));
20245 evalcond[1]
20246 = (((sj8
20247 * x1503))
20248 + (((-1.0)
20249 * (1.51009803921569)
20250 * sj8))
20251 + (((-1.0)
20252 * sj8
20253 * x1502)));
20254 evalcond[2]
20255 = 0;
20256 evalcond[3]
20257 = ((-1.51009803921569)
20258 + (((-1.0)
20259 * x1502))
20260 + x1503);
20261 evalcond[4]
20262 = ((((-1.0)
20263 * (1.51009803921569)
20264 * cj8))
20265 + ((cj8
20266 * x1503))
20267 + (((-1.0)
20268 * cj8
20269 * x1502)));
20270 evalcond[5]
20271 = ((-0.2125)
20272 + (((1.1)
20273 * pz))
20274 + (((-1.0)
20275 * (1.0)
20276 * pp)));
20277 if (IKabs(
20278 evalcond
20279 [0])
20280 < 0.0000010000000000
20281 && IKabs(
20282 evalcond
20283 [1])
20284 < 0.0000010000000000
20285 && IKabs(
20286 evalcond
20287 [2])
20288 < 0.0000010000000000
20289 && IKabs(
20290 evalcond
20291 [3])
20292 < 0.0000010000000000
20293 && IKabs(
20294 evalcond
20295 [4])
20296 < 0.0000010000000000
20297 && IKabs(
20298 evalcond
20299 [5])
20300 < 0.0000010000000000)
20301 {
20302 bgotonextstatement
20303 = false;
20304 {
20305 IkReal j4array
20306 [4],
20307 cj4array
20308 [4],
20309 sj4array
20310 [4];
20311 bool j4valid
20312 [4]
20313 = {false};
20314 _nj4
20315 = 4;
20316 j4array
20317 [0]
20318 = 0;
20319 sj4array
20320 [0]
20321 = IKsin(
20322 j4array
20323 [0]);
20324 cj4array
20325 [0]
20326 = IKcos(
20327 j4array
20328 [0]);
20329 j4array
20330 [1]
20331 = 1.5707963267949;
20332 sj4array
20333 [1]
20334 = IKsin(
20335 j4array
20336 [1]);
20337 cj4array
20338 [1]
20339 = IKcos(
20340 j4array
20341 [1]);
20342 j4array
20343 [2]
20344 = 3.14159265358979;
20345 sj4array
20346 [2]
20347 = IKsin(
20348 j4array
20349 [2]);
20350 cj4array
20351 [2]
20352 = IKcos(
20353 j4array
20354 [2]);
20355 j4array
20356 [3]
20357 = -1.5707963267949;
20358 sj4array
20359 [3]
20360 = IKsin(
20361 j4array
20362 [3]);
20363 cj4array
20364 [3]
20365 = IKcos(
20366 j4array
20367 [3]);
20368 if (j4array
20369 [0]
20370 > IKPI)
20371 {
20372 j4array
20373 [0]
20374 -= IK2PI;
20375 }
20376 else if (
20377 j4array
20378 [0]
20379 < -IKPI)
20380 {
20381 j4array
20382 [0]
20383 += IK2PI;
20384 }
20385 j4valid
20386 [0]
20387 = true;
20388 if (j4array
20389 [1]
20390 > IKPI)
20391 {
20392 j4array
20393 [1]
20394 -= IK2PI;
20395 }
20396 else if (
20397 j4array
20398 [1]
20399 < -IKPI)
20400 {
20401 j4array
20402 [1]
20403 += IK2PI;
20404 }
20405 j4valid
20406 [1]
20407 = true;
20408 if (j4array
20409 [2]
20410 > IKPI)
20411 {
20412 j4array
20413 [2]
20414 -= IK2PI;
20415 }
20416 else if (
20417 j4array
20418 [2]
20419 < -IKPI)
20420 {
20421 j4array
20422 [2]
20423 += IK2PI;
20424 }
20425 j4valid
20426 [2]
20427 = true;
20428 if (j4array
20429 [3]
20430 > IKPI)
20431 {
20432 j4array
20433 [3]
20434 -= IK2PI;
20435 }
20436 else if (
20437 j4array
20438 [3]
20439 < -IKPI)
20440 {
20441 j4array
20442 [3]
20443 += IK2PI;
20444 }
20445 j4valid
20446 [3]
20447 = true;
20448 for (
20449 int ij4
20450 = 0;
20451 ij4
20452 < 4;
20453 ++ij4)
20454 {
20455 if (!j4valid
20456 [ij4])
20457 {
20458 continue;
20459 }
20460 _ij4[0]
20461 = ij4;
20462 _ij4[1]
20463 = -1;
20464 for (
20465 int iij4
20466 = ij4
20467 + 1;
20468 iij4
20469 < 4;
20470 ++iij4)
20471 {
20472 if (j4valid
20473 [iij4]
20474 && IKabs(
20475 cj4array
20476 [ij4]
20477 - cj4array
20478 [iij4])
20479 < IKFAST_SOLUTION_THRESH
20480 && IKabs(
20481 sj4array
20482 [ij4]
20483 - sj4array
20484 [iij4])
20485 < IKFAST_SOLUTION_THRESH)
20486 {
20487 j4valid
20488 [iij4]
20489 = false;
20490 _ij4[1]
20491 = iij4;
20492 break;
20493 }
20494 }
20495 j4 = j4array
20496 [ij4];
20497 cj4 = cj4array
20498 [ij4];
20499 sj4 = sj4array
20500 [ij4];
20501
20502 rotationfunction0(
20503 solutions);
20504 }
20505 }
20506 }
20507 } while (0);
20508 if (bgotonextstatement)
20509 {
20510 bool
20511 bgotonextstatement
20512 = true;
20513 do
20514 {
20515 if (1)
20516 {
20517 bgotonextstatement
20518 = false;
20519 continue; // branch miss [j4]
20520 }
20521 } while (0);
20522 if (bgotonextstatement)
20523 {
20524 }
20525 }
20526 }
20527 }
20528 }
20529 }
20530 }
20531 }
20532 else
20533 {
20534 {
20535 IkReal j4array[1],
20536 cj4array[1],
20537 sj4array[1];
20538 bool j4valid[1]
20539 = {false};
20540 _nj4 = 1;
20541 IkReal x1504
20542 = ((1.51009803921569)
20543 * px);
20544 IkReal x1505
20545 = ((1.32323529411765)
20546 * cj9);
20547 IkReal x1506
20548 = (px * x1505);
20549 IkReal x1507
20550 = ((3.92156862745098)
20551 * pp);
20552 IkReal x1508
20553 = (px * x1507);
20554 IkReal x1509
20555 = ((1.51009803921569)
20556 * py);
20557 IkReal x1510
20558 = (cj8 * sj8);
20559 IkReal x1511
20560 = cj8 * cj8;
20561 IkReal x1512
20562 = (py * x1505);
20563 IkReal x1513
20564 = ((3.92156862745098)
20565 * cj8 * pp
20566 * sj8);
20567 IkReal x1514
20568 = (py * x1507);
20569 CheckValue<IkReal>
20570 x1515
20571 = IKPowWithIntegerCheck(
20572 IKsign((
20573 (((-1.0)
20574 * (1.0)
20575 * sj8
20576 * (pz
20577 * pz)))
20578 + ((pp
20579 * sj8)))),
20580 -1);
20581 if (!x1515.valid)
20582 {
20583 continue;
20584 }
20585 CheckValue<IkReal> x1516 = IKatan2WithCheck(
20586 IkReal(
20587 ((((-1.0)
20588 * x1506
20589 * x1511))
20590 + (((-1.0)
20591 * x1510
20592 * x1512))
20593 + (((-1.0)
20594 * x1508))
20595 + (((-1.0)
20596 * x1504
20597 * x1511))
20598 + ((x1508
20599 * x1511))
20600 + ((py
20601 * x1513))
20602 + (((-1.0)
20603 * x1509
20604 * x1510))
20605 + x1504
20606 + x1506)),
20607 ((((-1.0)
20608 * x1512))
20609 + ((px * x1513))
20610 + (((-1.0)
20611 * x1506
20612 * x1510))
20613 + x1514
20614 + (((-1.0)
20615 * x1511
20616 * x1514))
20617 + ((x1509
20618 * x1511))
20619 + (((-1.0)
20620 * x1504
20621 * x1510))
20622 + (((-1.0)
20623 * x1509))
20624 + ((x1511
20625 * x1512))),
20626 IKFAST_ATAN2_MAGTHRESH);
20627 if (!x1516.valid)
20628 {
20629 continue;
20630 }
20631 j4array[0]
20632 = ((-1.5707963267949)
20633 + (((1.5707963267949)
20634 * (x1515
20635 .value)))
20636 + (x1516
20637 .value));
20638 sj4array[0] = IKsin(
20639 j4array[0]);
20640 cj4array[0] = IKcos(
20641 j4array[0]);
20642 if (j4array[0] > IKPI)
20643 {
20644 j4array[0] -= IK2PI;
20645 }
20646 else if (
20647 j4array[0]
20648 < -IKPI)
20649 {
20650 j4array[0] += IK2PI;
20651 }
20652 j4valid[0] = true;
20653 for (int ij4 = 0;
20654 ij4 < 1;
20655 ++ij4)
20656 {
20657 if (!j4valid[ij4])
20658 {
20659 continue;
20660 }
20661 _ij4[0] = ij4;
20662 _ij4[1] = -1;
20663 for (int iij4
20664 = ij4 + 1;
20665 iij4 < 1;
20666 ++iij4)
20667 {
20668 if (j4valid[iij4]
20669 && IKabs(
20670 cj4array
20671 [ij4]
20672 - cj4array
20673 [iij4])
20674 < IKFAST_SOLUTION_THRESH
20675 && IKabs(
20676 sj4array
20677 [ij4]
20678 - sj4array
20679 [iij4])
20680 < IKFAST_SOLUTION_THRESH)
20681 {
20682 j4valid[iij4]
20683 = false;
20684 _ij4[1] = iij4;
20685 break;
20686 }
20687 }
20688 j4 = j4array[ij4];
20689 cj4 = cj4array[ij4];
20690 sj4 = sj4array[ij4];
20691 {
20692 IkReal
20693 evalcond[5];
20694 IkReal x1517
20695 = ((1.32323529411765)
20696 * cj9);
20697 IkReal x1518
20698 = ((3.92156862745098)
20699 * pp);
20700 IkReal x1519
20701 = IKsin(j4);
20702 IkReal x1520
20703 = (px
20704 * x1519);
20705 IkReal x1521
20706 = IKcos(j4);
20707 IkReal x1522
20708 = ((1.0)
20709 * x1521);
20710 IkReal x1523
20711 = (py
20712 * x1522);
20713 IkReal x1524
20714 = (px
20715 * x1522);
20716 IkReal x1525
20717 = (py
20718 * x1519);
20719 IkReal x1526
20720 = ((1.0)
20721 * x1525);
20722 IkReal x1527
20723 = (px
20724 * x1521);
20725 IkReal x1528
20726 = (sj8
20727 * x1520);
20728 IkReal x1529
20729 = ((0.09)
20730 * cj8);
20731 evalcond[0]
20732 = ((((-1.0)
20733 * sj8
20734 * x1517))
20735 + (((-1.0)
20736 * x1523))
20737 + ((sj8
20738 * x1518))
20739 + x1520
20740 + (((-1.0)
20741 * (1.51009803921569)
20742 * sj8)));
20743 evalcond[1]
20744 = ((((-1.0)
20745 * (1.51009803921569)
20746 * cj8))
20747 + (((-1.0)
20748 * x1526))
20749 + ((cj8
20750 * x1518))
20751 + (((-1.0)
20752 * x1524))
20753 + (((-1.0)
20754 * cj8
20755 * x1517)));
20756 evalcond[2]
20757 = (((sj8
20758 * x1527))
20759 + (((-1.0)
20760 * cj8
20761 * x1523))
20762 + ((cj8
20763 * x1520))
20764 + ((sj8
20765 * x1525)));
20766 evalcond[3]
20767 = ((-1.51009803921569)
20768 + x1518
20769 + (((-1.0)
20770 * cj8
20771 * x1526))
20772 + (((-1.0)
20773 * sj8
20774 * x1523))
20775 + (((-1.0)
20776 * x1517))
20777 + (((-1.0)
20778 * cj8
20779 * x1524))
20780 + x1528);
20781 evalcond[4]
20782 = ((-0.2125)
20783 + ((x1527
20784 * x1529))
20785 + (((0.09)
20786 * py
20787 * sj8
20788 * x1521))
20789 + ((x1525
20790 * x1529))
20791 + (((1.1)
20792 * pz))
20793 + (((-1.0)
20794 * (1.0)
20795 * pp))
20796 + (((-0.09)
20797 * x1528)));
20798 if (IKabs(evalcond
20799 [0])
20800 > IKFAST_EVALCOND_THRESH
20801 || IKabs(
20802 evalcond
20803 [1])
20804 > IKFAST_EVALCOND_THRESH
20805 || IKabs(
20806 evalcond
20807 [2])
20808 > IKFAST_EVALCOND_THRESH
20809 || IKabs(
20810 evalcond
20811 [3])
20812 > IKFAST_EVALCOND_THRESH
20813 || IKabs(
20814 evalcond
20815 [4])
20816 > IKFAST_EVALCOND_THRESH)
20817 {
20818 continue;
20819 }
20820 }
20821
20822 rotationfunction0(
20823 solutions);
20824 }
20825 }
20826 }
20827 }
20828 }
20829 else
20830 {
20831 {
20832 IkReal j4array[1],
20833 cj4array[1],
20834 sj4array[1];
20835 bool j4valid[1] = {false};
20836 _nj4 = 1;
20837 IkReal x1530
20838 = ((1.51009803921569)
20839 * cj8 * sj8);
20840 IkReal x1531 = cj8 * cj8;
20841 IkReal x1532
20842 = ((1.51009803921569)
20843 * x1531);
20844 IkReal x1533
20845 = ((1.32323529411765)
20846 * cj8 * cj9 * sj8);
20847 IkReal x1534
20848 = ((3.92156862745098)
20849 * cj8 * pp * sj8);
20850 IkReal x1535
20851 = ((1.32323529411765)
20852 * cj9 * x1531);
20853 IkReal x1536
20854 = ((3.92156862745098)
20855 * pp * x1531);
20856 CheckValue<IkReal> x1537
20857 = IKatan2WithCheck(
20858 IkReal((
20859 (((-1.0) * py
20860 * x1536))
20861 + ((px
20862 * x1534))
20863 + (((-1.0)
20864 * px
20865 * x1530))
20866 + (((-1.0)
20867 * px
20868 * x1533))
20869 + ((py
20870 * x1535))
20871 + ((py
20872 * x1532)))),
20873 (((px * x1532))
20874 + (((-1.0) * py
20875 * x1534))
20876 + (((-1.0) * px
20877 * x1536))
20878 + ((px * x1535))
20879 + ((py * x1533))
20880 + ((py
20881 * x1530))),
20882 IKFAST_ATAN2_MAGTHRESH);
20883 if (!x1537.valid)
20884 {
20885 continue;
20886 }
20887 CheckValue<IkReal> x1538
20888 = IKPowWithIntegerCheck(
20889 IKsign(
20890 (((cj8
20891 * (pz
20892 * pz)))
20893 + (((-1.0)
20894 * cj8
20895 * pp)))),
20896 -1);
20897 if (!x1538.valid)
20898 {
20899 continue;
20900 }
20901 j4array[0]
20902 = ((-1.5707963267949)
20903 + (x1537.value)
20904 + (((1.5707963267949)
20905 * (x1538
20906 .value))));
20907 sj4array[0]
20908 = IKsin(j4array[0]);
20909 cj4array[0]
20910 = IKcos(j4array[0]);
20911 if (j4array[0] > IKPI)
20912 {
20913 j4array[0] -= IK2PI;
20914 }
20915 else if (
20916 j4array[0] < -IKPI)
20917 {
20918 j4array[0] += IK2PI;
20919 }
20920 j4valid[0] = true;
20921 for (int ij4 = 0; ij4 < 1;
20922 ++ij4)
20923 {
20924 if (!j4valid[ij4])
20925 {
20926 continue;
20927 }
20928 _ij4[0] = ij4;
20929 _ij4[1] = -1;
20930 for (int iij4 = ij4 + 1;
20931 iij4 < 1;
20932 ++iij4)
20933 {
20934 if (j4valid[iij4]
20935 && IKabs(
20936 cj4array
20937 [ij4]
20938 - cj4array
20939 [iij4])
20940 < IKFAST_SOLUTION_THRESH
20941 && IKabs(
20942 sj4array
20943 [ij4]
20944 - sj4array
20945 [iij4])
20946 < IKFAST_SOLUTION_THRESH)
20947 {
20948 j4valid[iij4]
20949 = false;
20950 _ij4[1] = iij4;
20951 break;
20952 }
20953 }
20954 j4 = j4array[ij4];
20955 cj4 = cj4array[ij4];
20956 sj4 = sj4array[ij4];
20957 {
20958 IkReal evalcond[5];
20959 IkReal x1539
20960 = ((1.32323529411765)
20961 * cj9);
20962 IkReal x1540
20963 = ((3.92156862745098)
20964 * pp);
20965 IkReal x1541
20966 = IKsin(j4);
20967 IkReal x1542
20968 = (px * x1541);
20969 IkReal x1543
20970 = IKcos(j4);
20971 IkReal x1544
20972 = ((1.0) * x1543);
20973 IkReal x1545
20974 = (py * x1544);
20975 IkReal x1546
20976 = (px * x1544);
20977 IkReal x1547
20978 = (py * x1541);
20979 IkReal x1548
20980 = ((1.0) * x1547);
20981 IkReal x1549
20982 = (px * x1543);
20983 IkReal x1550
20984 = (sj8 * x1542);
20985 IkReal x1551
20986 = ((0.09) * cj8);
20987 evalcond[0]
20988 = (x1542
20989 + (((-1.0)
20990 * sj8
20991 * x1539))
20992 + ((sj8
20993 * x1540))
20994 + (((-1.0)
20995 * x1545))
20996 + (((-1.0)
20997 * (1.51009803921569)
20998 * sj8)));
20999 evalcond[1]
21000 = ((((-1.0)
21001 * (1.51009803921569)
21002 * cj8))
21003 + ((cj8
21004 * x1540))
21005 + (((-1.0)
21006 * x1546))
21007 + (((-1.0)
21008 * cj8
21009 * x1539))
21010 + (((-1.0)
21011 * x1548)));
21012 evalcond[2]
21013 = (((sj8 * x1547))
21014 + (((-1.0)
21015 * cj8
21016 * x1545))
21017 + ((cj8
21018 * x1542))
21019 + ((sj8
21020 * x1549)));
21021 evalcond[3]
21022 = ((-1.51009803921569)
21023 + (((-1.0)
21024 * cj8
21025 * x1546))
21026 + x1540 + x1550
21027 + (((-1.0)
21028 * x1539))
21029 + (((-1.0)
21030 * cj8
21031 * x1548))
21032 + (((-1.0)
21033 * sj8
21034 * x1545)));
21035 evalcond[4]
21036 = ((-0.2125)
21037 + (((1.1)
21038 * pz))
21039 + ((x1549
21040 * x1551))
21041 + (((-1.0)
21042 * (1.0)
21043 * pp))
21044 + (((-0.09)
21045 * x1550))
21046 + (((0.09) * py
21047 * sj8
21048 * x1543))
21049 + ((x1547
21050 * x1551)));
21051 if (IKabs(evalcond[0])
21052 > IKFAST_EVALCOND_THRESH
21053 || IKabs(evalcond
21054 [1])
21055 > IKFAST_EVALCOND_THRESH
21056 || IKabs(evalcond
21057 [2])
21058 > IKFAST_EVALCOND_THRESH
21059 || IKabs(evalcond
21060 [3])
21061 > IKFAST_EVALCOND_THRESH
21062 || IKabs(evalcond
21063 [4])
21064 > IKFAST_EVALCOND_THRESH)
21065 {
21066 continue;
21067 }
21068 }
21069
21070 rotationfunction0(
21071 solutions);
21072 }
21073 }
21074 }
21075 }
21076 }
21077 else
21078 {
21079 {
21080 IkReal j4array[1],
21081 cj4array[1], sj4array[1];
21082 bool j4valid[1] = {false};
21083 _nj4 = 1;
21084 IkReal x1552
21085 = ((1.51009803921569)
21086 * cj8);
21087 IkReal x1553
21088 = ((1.51009803921569)
21089 * sj8);
21090 IkReal x1554
21091 = ((1.32323529411765)
21092 * cj8 * cj9);
21093 IkReal x1555
21094 = ((3.92156862745098)
21095 * cj8 * pp);
21096 IkReal x1556
21097 = ((1.32323529411765)
21098 * cj9 * sj8);
21099 IkReal x1557
21100 = ((3.92156862745098) * pp
21101 * sj8);
21102 CheckValue<IkReal> x1558
21103 = IKatan2WithCheck(
21104 IkReal(
21105 ((((-1.0) * py
21106 * x1554))
21107 + (((-1.0) * py
21108 * x1552))
21109 + (((-1.0) * px
21110 * x1557))
21111 + ((py * x1555))
21112 + ((px * x1556))
21113 + ((px
21114 * x1553)))),
21115 ((((-1.0) * px
21116 * x1552))
21117 + ((py * x1557))
21118 + (((-1.0) * py
21119 * x1553))
21120 + (((-1.0) * py
21121 * x1556))
21122 + (((-1.0) * px
21123 * x1554))
21124 + ((px * x1555))),
21125 IKFAST_ATAN2_MAGTHRESH);
21126 if (!x1558.valid)
21127 {
21128 continue;
21129 }
21130 CheckValue<IkReal> x1559
21131 = IKPowWithIntegerCheck(
21132 IKsign((
21133 pp
21134 + (((-1.0) * (1.0)
21135 * (pz
21136 * pz))))),
21137 -1);
21138 if (!x1559.valid)
21139 {
21140 continue;
21141 }
21142 j4array[0]
21143 = ((-1.5707963267949)
21144 + (x1558.value)
21145 + (((1.5707963267949)
21146 * (x1559.value))));
21147 sj4array[0]
21148 = IKsin(j4array[0]);
21149 cj4array[0]
21150 = IKcos(j4array[0]);
21151 if (j4array[0] > IKPI)
21152 {
21153 j4array[0] -= IK2PI;
21154 }
21155 else if (j4array[0] < -IKPI)
21156 {
21157 j4array[0] += IK2PI;
21158 }
21159 j4valid[0] = true;
21160 for (int ij4 = 0; ij4 < 1;
21161 ++ij4)
21162 {
21163 if (!j4valid[ij4])
21164 {
21165 continue;
21166 }
21167 _ij4[0] = ij4;
21168 _ij4[1] = -1;
21169 for (int iij4 = ij4 + 1;
21170 iij4 < 1;
21171 ++iij4)
21172 {
21173 if (j4valid[iij4]
21174 && IKabs(
21175 cj4array[ij4]
21176 - cj4array
21177 [iij4])
21178 < IKFAST_SOLUTION_THRESH
21179 && IKabs(
21180 sj4array[ij4]
21181 - sj4array
21182 [iij4])
21183 < IKFAST_SOLUTION_THRESH)
21184 {
21185 j4valid[iij4] = false;
21186 _ij4[1] = iij4;
21187 break;
21188 }
21189 }
21190 j4 = j4array[ij4];
21191 cj4 = cj4array[ij4];
21192 sj4 = sj4array[ij4];
21193 {
21194 IkReal evalcond[5];
21195 IkReal x1560
21196 = ((1.32323529411765)
21197 * cj9);
21198 IkReal x1561
21199 = ((3.92156862745098)
21200 * pp);
21201 IkReal x1562 = IKsin(j4);
21202 IkReal x1563
21203 = (px * x1562);
21204 IkReal x1564 = IKcos(j4);
21205 IkReal x1565
21206 = ((1.0) * x1564);
21207 IkReal x1566
21208 = (py * x1565);
21209 IkReal x1567
21210 = (px * x1565);
21211 IkReal x1568
21212 = (py * x1562);
21213 IkReal x1569
21214 = ((1.0) * x1568);
21215 IkReal x1570
21216 = (px * x1564);
21217 IkReal x1571
21218 = (sj8 * x1563);
21219 IkReal x1572
21220 = ((0.09) * cj8);
21221 evalcond[0]
21222 = (((sj8 * x1561))
21223 + x1563
21224 + (((-1.0)
21225 * (1.51009803921569)
21226 * sj8))
21227 + (((-1.0) * sj8
21228 * x1560))
21229 + (((-1.0)
21230 * x1566)));
21231 evalcond[1]
21232 = ((((-1.0)
21233 * (1.51009803921569)
21234 * cj8))
21235 + (((-1.0) * cj8
21236 * x1560))
21237 + (((-1.0)
21238 * x1569))
21239 + ((cj8 * x1561))
21240 + (((-1.0)
21241 * x1567)));
21242 evalcond[2]
21243 = ((((-1.0) * cj8
21244 * x1566))
21245 + ((sj8 * x1570))
21246 + ((cj8 * x1563))
21247 + ((sj8 * x1568)));
21248 evalcond[3]
21249 = ((-1.51009803921569)
21250 + (((-1.0)
21251 * x1560))
21252 + (((-1.0) * sj8
21253 * x1566))
21254 + x1571 + x1561
21255 + (((-1.0) * cj8
21256 * x1569))
21257 + (((-1.0) * cj8
21258 * x1567)));
21259 evalcond[4]
21260 = ((-0.2125)
21261 + (((-0.09)
21262 * x1571))
21263 + ((x1570 * x1572))
21264 + (((0.09) * py
21265 * sj8 * x1564))
21266 + (((1.1) * pz))
21267 + (((-1.0) * (1.0)
21268 * pp))
21269 + ((x1568
21270 * x1572)));
21271 if (IKabs(evalcond[0])
21272 > IKFAST_EVALCOND_THRESH
21273 || IKabs(evalcond[1])
21274 > IKFAST_EVALCOND_THRESH
21275 || IKabs(evalcond[2])
21276 > IKFAST_EVALCOND_THRESH
21277 || IKabs(evalcond[3])
21278 > IKFAST_EVALCOND_THRESH
21279 || IKabs(evalcond[4])
21280 > IKFAST_EVALCOND_THRESH)
21281 {
21282 continue;
21283 }
21284 }
21285
21286 rotationfunction0(
21287 solutions);
21288 }
21289 }
21290 }
21291 }
21292 }
21293 } while (0);
21294 if (bgotonextstatement)
21295 {
21296 bool bgotonextstatement = true;
21297 do
21298 {
21299 IkReal x1573 = ((0.3) * cj9);
21300 IkReal x1574 = ((0.045) * sj9);
21301 evalcond[0]
21302 = ((-3.14159265358979)
21303 + (IKfmod(
21304 ((3.14159265358979)
21305 + (IKabs((
21306 (-3.14159265358979)
21307 + j6)))),
21308 6.28318530717959)));
21309 evalcond[1]
21310 = ((0.39655)
21311 + (((0.0765) * sj9))
21312 + (((0.32595) * cj9))
21313 + (((-1.0) * (1.0) * pp)));
21314 evalcond[2]
21315 = ((-0.55)
21316 + (((-1.0) * (1.0) * pz))
21317 + (((-1.0) * x1573))
21318 + (((-1.0) * x1574)));
21319 evalcond[3]
21320 = ((0.55) + x1574 + x1573 + pz);
21321 if (IKabs(evalcond[0])
21322 < 0.0000010000000000
21323 && IKabs(evalcond[1])
21324 < 0.0000010000000000
21325 && IKabs(evalcond[2])
21326 < 0.0000010000000000
21327 && IKabs(evalcond[3])
21328 < 0.0000010000000000)
21329 {
21330 bgotonextstatement = false;
21331 {
21332 IkReal j4eval[3];
21333 sj6 = 0;
21334 cj6 = -1.0;
21335 j6 = 3.14159265358979;
21336 IkReal x1575
21337 = (pp
21338 + (((-1.0) * (1.0)
21339 * (pz * pz))));
21340 IkReal x1576
21341 = ((1.51009803921569)
21342 * cj8);
21343 IkReal x1577
21344 = ((1.51009803921569)
21345 * sj8);
21346 IkReal x1578
21347 = ((1.32323529411765) * cj8
21348 * cj9);
21349 IkReal x1579
21350 = ((3.92156862745098) * cj8
21351 * pp);
21352 IkReal x1580
21353 = ((1.32323529411765) * cj9
21354 * sj8);
21355 IkReal x1581
21356 = ((3.92156862745098) * pp
21357 * sj8);
21358 j4eval[0] = x1575;
21359 j4eval[1]
21360 = ((IKabs(
21361 ((((-1.0) * py
21362 * x1579))
21363 + ((px * x1577))
21364 + ((py * x1578))
21365 + (((-1.0) * px
21366 * x1581))
21367 + ((py * x1576))
21368 + ((px * x1580)))))
21369 + (IKabs(
21370 (((px * x1578))
21371 + (((-1.0) * py
21372 * x1580))
21373 + ((px * x1576))
21374 + (((-1.0) * px
21375 * x1579))
21376 + ((py * x1581))
21377 + (((-1.0) * py
21378 * x1577))))));
21379 j4eval[2] = IKsign(x1575);
21380 if (IKabs(j4eval[0])
21381 < 0.0000010000000000
21382 || IKabs(j4eval[1])
21383 < 0.0000010000000000
21384 || IKabs(j4eval[2])
21385 < 0.0000010000000000)
21386 {
21387 {
21388 IkReal j4eval[2];
21389 sj6 = 0;
21390 cj6 = -1.0;
21391 j6 = 3.14159265358979;
21392 IkReal x1582
21393 = (((cj8 * (pz * pz)))
21394 + (((-1.0) * (1.0)
21395 * cj8 * pp)));
21396 j4eval[0] = x1582;
21397 j4eval[1] = IKsign(x1582);
21398 if (IKabs(j4eval[0])
21399 < 0.0000010000000000
21400 || IKabs(j4eval[1])
21401 < 0.0000010000000000)
21402 {
21403 {
21404 IkReal j4eval[2];
21405 sj6 = 0;
21406 cj6 = -1.0;
21407 j6 = 3.14159265358979;
21408 IkReal x1583
21409 = ((((-1.0) * (1.0)
21410 * cj8
21411 * (pz * pz)))
21412 + ((cj8 * pp)));
21413 j4eval[0] = x1583;
21414 j4eval[1]
21415 = IKsign(x1583);
21416 if (IKabs(j4eval[0])
21417 < 0.0000010000000000
21418 || IKabs(j4eval[1])
21419 < 0.0000010000000000)
21420 {
21421 {
21422 IkReal evalcond[6];
21423 bool
21424 bgotonextstatement
21425 = true;
21426 do
21427 {
21428 evalcond[0]
21429 = ((-3.14159265358979)
21430 + (IKfmod(
21431 ((3.14159265358979)
21432 + (IKabs((
21433 (-1.5707963267949)
21434 + j8)))),
21435 6.28318530717959)));
21436 if (IKabs(evalcond
21437 [0])
21438 < 0.0000010000000000)
21439 {
21440 bgotonextstatement
21441 = false;
21442 {
21443 IkReal
21444 j4eval[3];
21445 sj6 = 0;
21446 cj6 = -1.0;
21447 j6 = 3.14159265358979;
21448 sj8 = 1.0;
21449 cj8 = 0;
21450 j8 = 1.5707963267949;
21451 IkReal x1584
21452 = (pp
21453 + (((-1.0)
21454 * (1.0)
21455 * (pz
21456 * pz))));
21457 IkReal x1585
21458 = ((13497.0)
21459 * cj9);
21460 IkReal x1586
21461 = ((40000.0)
21462 * pp);
21463 j4eval[0]
21464 = x1584;
21465 j4eval[1]
21466 = ((IKabs((
21467 (((15403.0)
21468 * px))
21469 + ((px
21470 * x1585))
21471 + (((-1.0)
21472 * px
21473 * x1586)))))
21474 + (IKabs((
21475 (((-1.0)
21476 * py
21477 * x1585))
21478 + ((py
21479 * x1586))
21480 + (((-1.0)
21481 * (15403.0)
21482 * py))))));
21483 j4eval[2]
21484 = IKsign(
21485 x1584);
21486 if (IKabs(
21487 j4eval
21488 [0])
21489 < 0.0000010000000000
21490 || IKabs(
21491 j4eval
21492 [1])
21493 < 0.0000010000000000
21494 || IKabs(
21495 j4eval
21496 [2])
21497 < 0.0000010000000000)
21498 {
21499 {
21500 IkReal j4eval
21501 [3];
21502 sj6 = 0;
21503 cj6 = -1.0;
21504 j6 = 3.14159265358979;
21505 sj8 = 1.0;
21506 cj8 = 0;
21507 j8 = 1.5707963267949;
21508 IkReal
21509 x1587
21510 = pz
21511 * pz;
21512 IkReal
21513 x1588
21514 = ((80.0)
21515 * pp);
21516 IkReal
21517 x1589
21518 = ((88.0)
21519 * pz);
21520 j4eval[0]
21521 = (pp
21522 + (((-1.0)
21523 * x1587)));
21524 j4eval[1]
21525 = ((IKabs((
21526 ((py
21527 * x1589))
21528 + (((17.0)
21529 * py))
21530 + ((py
21531 * x1588)))))
21532 + (IKabs((
21533 ((px
21534 * x1588))
21535 + (((17.0)
21536 * px))
21537 + ((px
21538 * x1589))))));
21539 j4eval[2] = IKsign((
21540 (((9.0)
21541 * pp))
21542 + (((-9.0)
21543 * x1587))));
21544 if (IKabs(
21545 j4eval
21546 [0])
21547 < 0.0000010000000000
21548 || IKabs(
21549 j4eval
21550 [1])
21551 < 0.0000010000000000
21552 || IKabs(
21553 j4eval
21554 [2])
21555 < 0.0000010000000000)
21556 {
21557 {
21558 IkReal evalcond
21559 [5];
21560 bool
21561 bgotonextstatement
21562 = true;
21563 do
21564 {
21565 IkReal
21566 x1590
21567 = ((-1.51009803921569)
21568 + (((-1.0)
21569 * (1.32323529411765)
21570 * cj9))
21571 + (((3.92156862745098)
21572 * pp)));
21573 evalcond
21574 [0]
21575 = ((IKabs(
21576 py))
21577 + (IKabs(
21578 px)));
21579 evalcond
21580 [1]
21581 = x1590;
21582 evalcond
21583 [2]
21584 = 0;
21585 evalcond
21586 [3]
21587 = x1590;
21588 evalcond
21589 [4]
21590 = ((-0.2125)
21591 + (((-1.0)
21592 * (1.1)
21593 * pz))
21594 + (((-1.0)
21595 * (1.0)
21596 * pp)));
21597 if (IKabs(
21598 evalcond
21599 [0])
21600 < 0.0000010000000000
21601 && IKabs(
21602 evalcond
21603 [1])
21604 < 0.0000010000000000
21605 && IKabs(
21606 evalcond
21607 [2])
21608 < 0.0000010000000000
21609 && IKabs(
21610 evalcond
21611 [3])
21612 < 0.0000010000000000
21613 && IKabs(
21614 evalcond
21615 [4])
21616 < 0.0000010000000000)
21617 {
21618 bgotonextstatement
21619 = false;
21620 {
21621 IkReal j4array
21622 [4],
21623 cj4array
21624 [4],
21625 sj4array
21626 [4];
21627 bool j4valid
21628 [4]
21629 = {false};
21630 _nj4
21631 = 4;
21632 j4array
21633 [0]
21634 = 0;
21635 sj4array
21636 [0]
21637 = IKsin(
21638 j4array
21639 [0]);
21640 cj4array
21641 [0]
21642 = IKcos(
21643 j4array
21644 [0]);
21645 j4array
21646 [1]
21647 = 1.5707963267949;
21648 sj4array
21649 [1]
21650 = IKsin(
21651 j4array
21652 [1]);
21653 cj4array
21654 [1]
21655 = IKcos(
21656 j4array
21657 [1]);
21658 j4array
21659 [2]
21660 = 3.14159265358979;
21661 sj4array
21662 [2]
21663 = IKsin(
21664 j4array
21665 [2]);
21666 cj4array
21667 [2]
21668 = IKcos(
21669 j4array
21670 [2]);
21671 j4array
21672 [3]
21673 = -1.5707963267949;
21674 sj4array
21675 [3]
21676 = IKsin(
21677 j4array
21678 [3]);
21679 cj4array
21680 [3]
21681 = IKcos(
21682 j4array
21683 [3]);
21684 if (j4array
21685 [0]
21686 > IKPI)
21687 {
21688 j4array
21689 [0]
21690 -= IK2PI;
21691 }
21692 else if (
21693 j4array
21694 [0]
21695 < -IKPI)
21696 {
21697 j4array
21698 [0]
21699 += IK2PI;
21700 }
21701 j4valid
21702 [0]
21703 = true;
21704 if (j4array
21705 [1]
21706 > IKPI)
21707 {
21708 j4array
21709 [1]
21710 -= IK2PI;
21711 }
21712 else if (
21713 j4array
21714 [1]
21715 < -IKPI)
21716 {
21717 j4array
21718 [1]
21719 += IK2PI;
21720 }
21721 j4valid
21722 [1]
21723 = true;
21724 if (j4array
21725 [2]
21726 > IKPI)
21727 {
21728 j4array
21729 [2]
21730 -= IK2PI;
21731 }
21732 else if (
21733 j4array
21734 [2]
21735 < -IKPI)
21736 {
21737 j4array
21738 [2]
21739 += IK2PI;
21740 }
21741 j4valid
21742 [2]
21743 = true;
21744 if (j4array
21745 [3]
21746 > IKPI)
21747 {
21748 j4array
21749 [3]
21750 -= IK2PI;
21751 }
21752 else if (
21753 j4array
21754 [3]
21755 < -IKPI)
21756 {
21757 j4array
21758 [3]
21759 += IK2PI;
21760 }
21761 j4valid
21762 [3]
21763 = true;
21764 for (
21765 int ij4
21766 = 0;
21767 ij4
21768 < 4;
21769 ++ij4)
21770 {
21771 if (!j4valid
21772 [ij4])
21773 {
21774 continue;
21775 }
21776 _ij4[0]
21777 = ij4;
21778 _ij4[1]
21779 = -1;
21780 for (
21781 int iij4
21782 = ij4
21783 + 1;
21784 iij4
21785 < 4;
21786 ++iij4)
21787 {
21788 if (j4valid
21789 [iij4]
21790 && IKabs(
21791 cj4array
21792 [ij4]
21793 - cj4array
21794 [iij4])
21795 < IKFAST_SOLUTION_THRESH
21796 && IKabs(
21797 sj4array
21798 [ij4]
21799 - sj4array
21800 [iij4])
21801 < IKFAST_SOLUTION_THRESH)
21802 {
21803 j4valid
21804 [iij4]
21805 = false;
21806 _ij4[1]
21807 = iij4;
21808 break;
21809 }
21810 }
21811 j4 = j4array
21812 [ij4];
21813 cj4 = cj4array
21814 [ij4];
21815 sj4 = sj4array
21816 [ij4];
21817
21818 rotationfunction0(
21819 solutions);
21820 }
21821 }
21822 }
21823 } while (
21824 0);
21825 if (bgotonextstatement)
21826 {
21827 bool
21828 bgotonextstatement
21829 = true;
21830 do
21831 {
21832 if (1)
21833 {
21834 bgotonextstatement
21835 = false;
21836 continue; // branch miss [j4]
21837 }
21838 } while (
21839 0);
21840 if (bgotonextstatement)
21841 {
21842 }
21843 }
21844 }
21845 }
21846 else
21847 {
21848 {
21849 IkReal j4array
21850 [1],
21851 cj4array
21852 [1],
21853 sj4array
21854 [1];
21855 bool j4valid
21856 [1]
21857 = {false};
21858 _nj4
21859 = 1;
21860 IkReal
21861 x1591
21862 = ((100.0)
21863 * pp);
21864 IkReal
21865 x1592
21866 = ((110.0)
21867 * pz);
21868 CheckValue<IkReal> x1593 = IKatan2WithCheck(
21869 IkReal((
21870 (((-1.0)
21871 * px
21872 * x1591))
21873 + (((-1.0)
21874 * (21.25)
21875 * px))
21876 + (((-1.0)
21877 * px
21878 * x1592)))),
21879 (((py
21880 * x1591))
21881 + ((py
21882 * x1592))
21883 + (((21.25)
21884 * py))),
21885 IKFAST_ATAN2_MAGTHRESH);
21886 if (!x1593
21887 .valid)
21888 {
21889 continue;
21890 }
21891 CheckValue<IkReal> x1594 = IKPowWithIntegerCheck(
21892 IKsign((
21893 (((-1.0)
21894 * (9.0)
21895 * (pz
21896 * pz)))
21897 + (((9.0)
21898 * pp)))),
21899 -1);
21900 if (!x1594
21901 .valid)
21902 {
21903 continue;
21904 }
21905 j4array
21906 [0]
21907 = ((-1.5707963267949)
21908 + (x1593
21909 .value)
21910 + (((1.5707963267949)
21911 * (x1594
21912 .value))));
21913 sj4array
21914 [0]
21915 = IKsin(
21916 j4array
21917 [0]);
21918 cj4array
21919 [0]
21920 = IKcos(
21921 j4array
21922 [0]);
21923 if (j4array
21924 [0]
21925 > IKPI)
21926 {
21927 j4array
21928 [0]
21929 -= IK2PI;
21930 }
21931 else if (
21932 j4array
21933 [0]
21934 < -IKPI)
21935 {
21936 j4array
21937 [0]
21938 += IK2PI;
21939 }
21940 j4valid
21941 [0]
21942 = true;
21943 for (
21944 int ij4
21945 = 0;
21946 ij4
21947 < 1;
21948 ++ij4)
21949 {
21950 if (!j4valid
21951 [ij4])
21952 {
21953 continue;
21954 }
21955 _ij4[0]
21956 = ij4;
21957 _ij4[1]
21958 = -1;
21959 for (
21960 int iij4
21961 = ij4
21962 + 1;
21963 iij4
21964 < 1;
21965 ++iij4)
21966 {
21967 if (j4valid
21968 [iij4]
21969 && IKabs(
21970 cj4array
21971 [ij4]
21972 - cj4array
21973 [iij4])
21974 < IKFAST_SOLUTION_THRESH
21975 && IKabs(
21976 sj4array
21977 [ij4]
21978 - sj4array
21979 [iij4])
21980 < IKFAST_SOLUTION_THRESH)
21981 {
21982 j4valid
21983 [iij4]
21984 = false;
21985 _ij4[1]
21986 = iij4;
21987 break;
21988 }
21989 }
21990 j4 = j4array
21991 [ij4];
21992 cj4 = cj4array
21993 [ij4];
21994 sj4 = sj4array
21995 [ij4];
21996 {
21997 IkReal evalcond
21998 [3];
21999 IkReal
22000 x1595
22001 = IKcos(
22002 j4);
22003 IkReal
22004 x1596
22005 = ((1.0)
22006 * x1595);
22007 IkReal
22008 x1597
22009 = IKsin(
22010 j4);
22011 IkReal
22012 x1598
22013 = (px
22014 * x1597);
22015 evalcond
22016 [0]
22017 = ((((-1.0)
22018 * px
22019 * x1596))
22020 + (((-1.0)
22021 * py
22022 * x1597)));
22023 evalcond
22024 [1]
22025 = ((-1.51009803921569)
22026 + (((-1.0)
22027 * py
22028 * x1596))
22029 + (((-1.0)
22030 * (1.32323529411765)
22031 * cj9))
22032 + (((3.92156862745098)
22033 * pp))
22034 + x1598);
22035 evalcond
22036 [2]
22037 = ((-0.2125)
22038 + (((-1.0)
22039 * (1.1)
22040 * pz))
22041 + (((-0.09)
22042 * x1598))
22043 + (((0.09)
22044 * py
22045 * x1595))
22046 + (((-1.0)
22047 * (1.0)
22048 * pp)));
22049 if (IKabs(
22050 evalcond
22051 [0])
22052 > IKFAST_EVALCOND_THRESH
22053 || IKabs(
22054 evalcond
22055 [1])
22056 > IKFAST_EVALCOND_THRESH
22057 || IKabs(
22058 evalcond
22059 [2])
22060 > IKFAST_EVALCOND_THRESH)
22061 {
22062 continue;
22063 }
22064 }
22065
22066 rotationfunction0(
22067 solutions);
22068 }
22069 }
22070 }
22071 }
22072 }
22073 else
22074 {
22075 {
22076 IkReal j4array
22077 [1],
22078 cj4array
22079 [1],
22080 sj4array
22081 [1];
22082 bool j4valid
22083 [1]
22084 = {false};
22085 _nj4 = 1;
22086 IkReal
22087 x1599
22088 = ((1.32323529411765)
22089 * cj9);
22090 IkReal
22091 x1600
22092 = ((3.92156862745098)
22093 * pp);
22094 CheckValue<IkReal> x1601 = IKPowWithIntegerCheck(
22095 IKsign((
22096 pp
22097 + (((-1.0)
22098 * (1.0)
22099 * (pz
22100 * pz))))),
22101 -1);
22102 if (!x1601
22103 .valid)
22104 {
22105 continue;
22106 }
22107 CheckValue<IkReal> x1602 = IKatan2WithCheck(
22108 IkReal((
22109 ((px
22110 * x1599))
22111 + (((1.51009803921569)
22112 * px))
22113 + (((-1.0)
22114 * px
22115 * x1600)))),
22116 ((((-1.0)
22117 * (1.51009803921569)
22118 * py))
22119 + (((-1.0)
22120 * py
22121 * x1599))
22122 + ((py
22123 * x1600))),
22124 IKFAST_ATAN2_MAGTHRESH);
22125 if (!x1602
22126 .valid)
22127 {
22128 continue;
22129 }
22130 j4array[0]
22131 = ((-1.5707963267949)
22132 + (((1.5707963267949)
22133 * (x1601
22134 .value)))
22135 + (x1602
22136 .value));
22137 sj4array[0] = IKsin(
22138 j4array
22139 [0]);
22140 cj4array[0] = IKcos(
22141 j4array
22142 [0]);
22143 if (j4array
22144 [0]
22145 > IKPI)
22146 {
22147 j4array
22148 [0]
22149 -= IK2PI;
22150 }
22151 else if (
22152 j4array
22153 [0]
22154 < -IKPI)
22155 {
22156 j4array
22157 [0]
22158 += IK2PI;
22159 }
22160 j4valid[0]
22161 = true;
22162 for (
22163 int ij4
22164 = 0;
22165 ij4
22166 < 1;
22167 ++ij4)
22168 {
22169 if (!j4valid
22170 [ij4])
22171 {
22172 continue;
22173 }
22174 _ij4[0]
22175 = ij4;
22176 _ij4[1]
22177 = -1;
22178 for (
22179 int iij4
22180 = ij4
22181 + 1;
22182 iij4
22183 < 1;
22184 ++iij4)
22185 {
22186 if (j4valid
22187 [iij4]
22188 && IKabs(
22189 cj4array
22190 [ij4]
22191 - cj4array
22192 [iij4])
22193 < IKFAST_SOLUTION_THRESH
22194 && IKabs(
22195 sj4array
22196 [ij4]
22197 - sj4array
22198 [iij4])
22199 < IKFAST_SOLUTION_THRESH)
22200 {
22201 j4valid
22202 [iij4]
22203 = false;
22204 _ij4[1]
22205 = iij4;
22206 break;
22207 }
22208 }
22209 j4 = j4array
22210 [ij4];
22211 cj4 = cj4array
22212 [ij4];
22213 sj4 = sj4array
22214 [ij4];
22215 {
22216 IkReal evalcond
22217 [3];
22218 IkReal
22219 x1603
22220 = IKcos(
22221 j4);
22222 IkReal
22223 x1604
22224 = ((1.0)
22225 * x1603);
22226 IkReal
22227 x1605
22228 = IKsin(
22229 j4);
22230 IkReal
22231 x1606
22232 = (px
22233 * x1605);
22234 evalcond
22235 [0]
22236 = ((((-1.0)
22237 * px
22238 * x1604))
22239 + (((-1.0)
22240 * py
22241 * x1605)));
22242 evalcond
22243 [1]
22244 = ((-1.51009803921569)
22245 + (((-1.0)
22246 * (1.32323529411765)
22247 * cj9))
22248 + (((3.92156862745098)
22249 * pp))
22250 + x1606
22251 + (((-1.0)
22252 * py
22253 * x1604)));
22254 evalcond
22255 [2]
22256 = ((-0.2125)
22257 + (((-1.0)
22258 * (1.1)
22259 * pz))
22260 + (((-0.09)
22261 * x1606))
22262 + (((-1.0)
22263 * (1.0)
22264 * pp))
22265 + (((0.09)
22266 * py
22267 * x1603)));
22268 if (IKabs(
22269 evalcond
22270 [0])
22271 > IKFAST_EVALCOND_THRESH
22272 || IKabs(
22273 evalcond
22274 [1])
22275 > IKFAST_EVALCOND_THRESH
22276 || IKabs(
22277 evalcond
22278 [2])
22279 > IKFAST_EVALCOND_THRESH)
22280 {
22281 continue;
22282 }
22283 }
22284
22285 rotationfunction0(
22286 solutions);
22287 }
22288 }
22289 }
22290 }
22291 }
22292 } while (0);
22293 if (bgotonextstatement)
22294 {
22295 bool
22296 bgotonextstatement
22297 = true;
22298 do
22299 {
22300 evalcond[0]
22301 = ((-3.14159265358979)
22302 + (IKfmod(
22303 ((3.14159265358979)
22304 + (IKabs((
22305 (1.5707963267949)
22306 + j8)))),
22307 6.28318530717959)));
22308 if (IKabs(
22309 evalcond
22310 [0])
22311 < 0.0000010000000000)
22312 {
22313 bgotonextstatement
22314 = false;
22315 {
22316 IkReal
22317 j4eval
22318 [3];
22319 sj6 = 0;
22320 cj6 = -1.0;
22321 j6 = 3.14159265358979;
22322 sj8 = -1.0;
22323 cj8 = 0;
22324 j8 = -1.5707963267949;
22325 IkReal x1607
22326 = (pp
22327 + (((-1.0)
22328 * (1.0)
22329 * (pz
22330 * pz))));
22331 IkReal x1608
22332 = ((13497.0)
22333 * cj9);
22334 IkReal x1609
22335 = ((40000.0)
22336 * pp);
22337 j4eval[0]
22338 = x1607;
22339 j4eval[1]
22340 = ((IKabs((
22341 (((-1.0)
22342 * py
22343 * x1609))
22344 + (((15403.0)
22345 * py))
22346 + ((py
22347 * x1608)))))
22348 + (IKabs((
22349 (((-1.0)
22350 * px
22351 * x1608))
22352 + (((-1.0)
22353 * (15403.0)
22354 * px))
22355 + ((px
22356 * x1609))))));
22357 j4eval[2]
22358 = IKsign(
22359 x1607);
22360 if (IKabs(
22361 j4eval
22362 [0])
22363 < 0.0000010000000000
22364 || IKabs(
22365 j4eval
22366 [1])
22367 < 0.0000010000000000
22368 || IKabs(
22369 j4eval
22370 [2])
22371 < 0.0000010000000000)
22372 {
22373 {
22374 IkReal j4eval
22375 [3];
22376 sj6 = 0;
22377 cj6 = -1.0;
22378 j6 = 3.14159265358979;
22379 sj8 = -1.0;
22380 cj8 = 0;
22381 j8 = -1.5707963267949;
22382 IkReal
22383 x1610
22384 = pz
22385 * pz;
22386 IkReal
22387 x1611
22388 = ((80.0)
22389 * pp);
22390 IkReal
22391 x1612
22392 = ((88.0)
22393 * pz);
22394 j4eval
22395 [0]
22396 = (pp
22397 + (((-1.0)
22398 * x1610)));
22399 j4eval
22400 [1]
22401 = ((IKabs((
22402 (((17.0)
22403 * py))
22404 + ((py
22405 * x1612))
22406 + ((py
22407 * x1611)))))
22408 + (IKabs((
22409 (((17.0)
22410 * px))
22411 + ((px
22412 * x1612))
22413 + ((px
22414 * x1611))))));
22415 j4eval[2] = IKsign((
22416 (((-9.0)
22417 * x1610))
22418 + (((9.0)
22419 * pp))));
22420 if (IKabs(
22421 j4eval
22422 [0])
22423 < 0.0000010000000000
22424 || IKabs(
22425 j4eval
22426 [1])
22427 < 0.0000010000000000
22428 || IKabs(
22429 j4eval
22430 [2])
22431 < 0.0000010000000000)
22432 {
22433 {
22434 IkReal evalcond
22435 [5];
22436 bool
22437 bgotonextstatement
22438 = true;
22439 do
22440 {
22441 IkReal
22442 x1613
22443 = ((1.32323529411765)
22444 * cj9);
22445 IkReal
22446 x1614
22447 = ((3.92156862745098)
22448 * pp);
22449 evalcond
22450 [0]
22451 = ((IKabs(
22452 py))
22453 + (IKabs(
22454 px)));
22455 evalcond
22456 [1]
22457 = ((1.51009803921569)
22458 + (((-1.0)
22459 * x1614))
22460 + x1613);
22461 evalcond
22462 [2]
22463 = 0;
22464 evalcond
22465 [3]
22466 = ((-1.51009803921569)
22467 + (((-1.0)
22468 * x1613))
22469 + x1614);
22470 evalcond
22471 [4]
22472 = ((-0.2125)
22473 + (((-1.0)
22474 * (1.1)
22475 * pz))
22476 + (((-1.0)
22477 * (1.0)
22478 * pp)));
22479 if (IKabs(
22480 evalcond
22481 [0])
22482 < 0.0000010000000000
22483 && IKabs(
22484 evalcond
22485 [1])
22486 < 0.0000010000000000
22487 && IKabs(
22488 evalcond
22489 [2])
22490 < 0.0000010000000000
22491 && IKabs(
22492 evalcond
22493 [3])
22494 < 0.0000010000000000
22495 && IKabs(
22496 evalcond
22497 [4])
22498 < 0.0000010000000000)
22499 {
22500 bgotonextstatement
22501 = false;
22502 {
22503 IkReal j4array
22504 [4],
22505 cj4array
22506 [4],
22507 sj4array
22508 [4];
22509 bool j4valid
22510 [4]
22511 = {false};
22512 _nj4
22513 = 4;
22514 j4array
22515 [0]
22516 = 0;
22517 sj4array
22518 [0]
22519 = IKsin(
22520 j4array
22521 [0]);
22522 cj4array
22523 [0]
22524 = IKcos(
22525 j4array
22526 [0]);
22527 j4array
22528 [1]
22529 = 1.5707963267949;
22530 sj4array
22531 [1]
22532 = IKsin(
22533 j4array
22534 [1]);
22535 cj4array
22536 [1]
22537 = IKcos(
22538 j4array
22539 [1]);
22540 j4array
22541 [2]
22542 = 3.14159265358979;
22543 sj4array
22544 [2]
22545 = IKsin(
22546 j4array
22547 [2]);
22548 cj4array
22549 [2]
22550 = IKcos(
22551 j4array
22552 [2]);
22553 j4array
22554 [3]
22555 = -1.5707963267949;
22556 sj4array
22557 [3]
22558 = IKsin(
22559 j4array
22560 [3]);
22561 cj4array
22562 [3]
22563 = IKcos(
22564 j4array
22565 [3]);
22566 if (j4array
22567 [0]
22568 > IKPI)
22569 {
22570 j4array
22571 [0]
22572 -= IK2PI;
22573 }
22574 else if (
22575 j4array
22576 [0]
22577 < -IKPI)
22578 {
22579 j4array
22580 [0]
22581 += IK2PI;
22582 }
22583 j4valid
22584 [0]
22585 = true;
22586 if (j4array
22587 [1]
22588 > IKPI)
22589 {
22590 j4array
22591 [1]
22592 -= IK2PI;
22593 }
22594 else if (
22595 j4array
22596 [1]
22597 < -IKPI)
22598 {
22599 j4array
22600 [1]
22601 += IK2PI;
22602 }
22603 j4valid
22604 [1]
22605 = true;
22606 if (j4array
22607 [2]
22608 > IKPI)
22609 {
22610 j4array
22611 [2]
22612 -= IK2PI;
22613 }
22614 else if (
22615 j4array
22616 [2]
22617 < -IKPI)
22618 {
22619 j4array
22620 [2]
22621 += IK2PI;
22622 }
22623 j4valid
22624 [2]
22625 = true;
22626 if (j4array
22627 [3]
22628 > IKPI)
22629 {
22630 j4array
22631 [3]
22632 -= IK2PI;
22633 }
22634 else if (
22635 j4array
22636 [3]
22637 < -IKPI)
22638 {
22639 j4array
22640 [3]
22641 += IK2PI;
22642 }
22643 j4valid
22644 [3]
22645 = true;
22646 for (
22647 int ij4
22648 = 0;
22649 ij4
22650 < 4;
22651 ++ij4)
22652 {
22653 if (!j4valid
22654 [ij4])
22655 {
22656 continue;
22657 }
22658 _ij4[0]
22659 = ij4;
22660 _ij4[1]
22661 = -1;
22662 for (
22663 int iij4
22664 = ij4
22665 + 1;
22666 iij4
22667 < 4;
22668 ++iij4)
22669 {
22670 if (j4valid
22671 [iij4]
22672 && IKabs(
22673 cj4array
22674 [ij4]
22675 - cj4array
22676 [iij4])
22677 < IKFAST_SOLUTION_THRESH
22678 && IKabs(
22679 sj4array
22680 [ij4]
22681 - sj4array
22682 [iij4])
22683 < IKFAST_SOLUTION_THRESH)
22684 {
22685 j4valid
22686 [iij4]
22687 = false;
22688 _ij4[1]
22689 = iij4;
22690 break;
22691 }
22692 }
22693 j4 = j4array
22694 [ij4];
22695 cj4 = cj4array
22696 [ij4];
22697 sj4 = sj4array
22698 [ij4];
22699
22700 rotationfunction0(
22701 solutions);
22702 }
22703 }
22704 }
22705 } while (
22706 0);
22707 if (bgotonextstatement)
22708 {
22709 bool
22710 bgotonextstatement
22711 = true;
22712 do
22713 {
22714 if (1)
22715 {
22716 bgotonextstatement
22717 = false;
22718 continue; // branch miss [j4]
22719 }
22720 } while (
22721 0);
22722 if (bgotonextstatement)
22723 {
22724 }
22725 }
22726 }
22727 }
22728 else
22729 {
22730 {
22731 IkReal j4array
22732 [1],
22733 cj4array
22734 [1],
22735 sj4array
22736 [1];
22737 bool j4valid
22738 [1]
22739 = {false};
22740 _nj4
22741 = 1;
22742 IkReal
22743 x1615
22744 = ((100.0)
22745 * pp);
22746 IkReal
22747 x1616
22748 = ((110.0)
22749 * pz);
22750 CheckValue<IkReal> x1617 = IKatan2WithCheck(
22751 IkReal((
22752 ((px
22753 * x1616))
22754 + (((21.25)
22755 * px))
22756 + ((px
22757 * x1615)))),
22758 ((((-1.0)
22759 * py
22760 * x1616))
22761 + (((-1.0)
22762 * (21.25)
22763 * py))
22764 + (((-1.0)
22765 * py
22766 * x1615))),
22767 IKFAST_ATAN2_MAGTHRESH);
22768 if (!x1617
22769 .valid)
22770 {
22771 continue;
22772 }
22773 CheckValue<IkReal> x1618 = IKPowWithIntegerCheck(
22774 IKsign((
22775 (((-1.0)
22776 * (9.0)
22777 * (pz
22778 * pz)))
22779 + (((9.0)
22780 * pp)))),
22781 -1);
22782 if (!x1618
22783 .valid)
22784 {
22785 continue;
22786 }
22787 j4array
22788 [0]
22789 = ((-1.5707963267949)
22790 + (x1617
22791 .value)
22792 + (((1.5707963267949)
22793 * (x1618
22794 .value))));
22795 sj4array
22796 [0]
22797 = IKsin(
22798 j4array
22799 [0]);
22800 cj4array
22801 [0]
22802 = IKcos(
22803 j4array
22804 [0]);
22805 if (j4array
22806 [0]
22807 > IKPI)
22808 {
22809 j4array
22810 [0]
22811 -= IK2PI;
22812 }
22813 else if (
22814 j4array
22815 [0]
22816 < -IKPI)
22817 {
22818 j4array
22819 [0]
22820 += IK2PI;
22821 }
22822 j4valid
22823 [0]
22824 = true;
22825 for (
22826 int ij4
22827 = 0;
22828 ij4
22829 < 1;
22830 ++ij4)
22831 {
22832 if (!j4valid
22833 [ij4])
22834 {
22835 continue;
22836 }
22837 _ij4[0]
22838 = ij4;
22839 _ij4[1]
22840 = -1;
22841 for (
22842 int iij4
22843 = ij4
22844 + 1;
22845 iij4
22846 < 1;
22847 ++iij4)
22848 {
22849 if (j4valid
22850 [iij4]
22851 && IKabs(
22852 cj4array
22853 [ij4]
22854 - cj4array
22855 [iij4])
22856 < IKFAST_SOLUTION_THRESH
22857 && IKabs(
22858 sj4array
22859 [ij4]
22860 - sj4array
22861 [iij4])
22862 < IKFAST_SOLUTION_THRESH)
22863 {
22864 j4valid
22865 [iij4]
22866 = false;
22867 _ij4[1]
22868 = iij4;
22869 break;
22870 }
22871 }
22872 j4 = j4array
22873 [ij4];
22874 cj4 = cj4array
22875 [ij4];
22876 sj4 = sj4array
22877 [ij4];
22878 {
22879 IkReal evalcond
22880 [3];
22881 IkReal
22882 x1619
22883 = IKcos(
22884 j4);
22885 IkReal
22886 x1620
22887 = IKsin(
22888 j4);
22889 IkReal
22890 x1621
22891 = (px
22892 * x1620);
22893 IkReal
22894 x1622
22895 = (py
22896 * x1619);
22897 evalcond
22898 [0]
22899 = (((px
22900 * x1619))
22901 + ((py
22902 * x1620)));
22903 evalcond
22904 [1]
22905 = ((1.51009803921569)
22906 + (((-1.0)
22907 * x1622))
22908 + (((-1.0)
22909 * (3.92156862745098)
22910 * pp))
22911 + x1621
22912 + (((1.32323529411765)
22913 * cj9)));
22914 evalcond
22915 [2]
22916 = ((-0.2125)
22917 + (((-1.0)
22918 * (1.1)
22919 * pz))
22920 + (((0.09)
22921 * x1621))
22922 + (((-0.09)
22923 * x1622))
22924 + (((-1.0)
22925 * (1.0)
22926 * pp)));
22927 if (IKabs(
22928 evalcond
22929 [0])
22930 > IKFAST_EVALCOND_THRESH
22931 || IKabs(
22932 evalcond
22933 [1])
22934 > IKFAST_EVALCOND_THRESH
22935 || IKabs(
22936 evalcond
22937 [2])
22938 > IKFAST_EVALCOND_THRESH)
22939 {
22940 continue;
22941 }
22942 }
22943
22944 rotationfunction0(
22945 solutions);
22946 }
22947 }
22948 }
22949 }
22950 }
22951 else
22952 {
22953 {
22954 IkReal j4array
22955 [1],
22956 cj4array
22957 [1],
22958 sj4array
22959 [1];
22960 bool j4valid
22961 [1]
22962 = {false};
22963 _nj4
22964 = 1;
22965 IkReal
22966 x1623
22967 = ((1.32323529411765)
22968 * cj9);
22969 IkReal
22970 x1624
22971 = ((3.92156862745098)
22972 * pp);
22973 CheckValue<IkReal> x1625 = IKatan2WithCheck(
22974 IkReal((
22975 (((-1.0)
22976 * (1.51009803921569)
22977 * px))
22978 + (((-1.0)
22979 * px
22980 * x1623))
22981 + ((px
22982 * x1624)))),
22983 (((py
22984 * x1623))
22985 + (((1.51009803921569)
22986 * py))
22987 + (((-1.0)
22988 * py
22989 * x1624))),
22990 IKFAST_ATAN2_MAGTHRESH);
22991 if (!x1625
22992 .valid)
22993 {
22994 continue;
22995 }
22996 CheckValue<IkReal> x1626 = IKPowWithIntegerCheck(
22997 IKsign((
22998 pp
22999 + (((-1.0)
23000 * (1.0)
23001 * (pz
23002 * pz))))),
23003 -1);
23004 if (!x1626
23005 .valid)
23006 {
23007 continue;
23008 }
23009 j4array
23010 [0]
23011 = ((-1.5707963267949)
23012 + (x1625
23013 .value)
23014 + (((1.5707963267949)
23015 * (x1626
23016 .value))));
23017 sj4array
23018 [0]
23019 = IKsin(
23020 j4array
23021 [0]);
23022 cj4array
23023 [0]
23024 = IKcos(
23025 j4array
23026 [0]);
23027 if (j4array
23028 [0]
23029 > IKPI)
23030 {
23031 j4array
23032 [0]
23033 -= IK2PI;
23034 }
23035 else if (
23036 j4array
23037 [0]
23038 < -IKPI)
23039 {
23040 j4array
23041 [0]
23042 += IK2PI;
23043 }
23044 j4valid
23045 [0]
23046 = true;
23047 for (
23048 int ij4
23049 = 0;
23050 ij4
23051 < 1;
23052 ++ij4)
23053 {
23054 if (!j4valid
23055 [ij4])
23056 {
23057 continue;
23058 }
23059 _ij4[0]
23060 = ij4;
23061 _ij4[1]
23062 = -1;
23063 for (
23064 int iij4
23065 = ij4
23066 + 1;
23067 iij4
23068 < 1;
23069 ++iij4)
23070 {
23071 if (j4valid
23072 [iij4]
23073 && IKabs(
23074 cj4array
23075 [ij4]
23076 - cj4array
23077 [iij4])
23078 < IKFAST_SOLUTION_THRESH
23079 && IKabs(
23080 sj4array
23081 [ij4]
23082 - sj4array
23083 [iij4])
23084 < IKFAST_SOLUTION_THRESH)
23085 {
23086 j4valid
23087 [iij4]
23088 = false;
23089 _ij4[1]
23090 = iij4;
23091 break;
23092 }
23093 }
23094 j4 = j4array
23095 [ij4];
23096 cj4 = cj4array
23097 [ij4];
23098 sj4 = sj4array
23099 [ij4];
23100 {
23101 IkReal evalcond
23102 [3];
23103 IkReal
23104 x1627
23105 = IKcos(
23106 j4);
23107 IkReal
23108 x1628
23109 = IKsin(
23110 j4);
23111 IkReal
23112 x1629
23113 = (px
23114 * x1628);
23115 IkReal
23116 x1630
23117 = (py
23118 * x1627);
23119 evalcond
23120 [0]
23121 = (((py
23122 * x1628))
23123 + ((px
23124 * x1627)));
23125 evalcond
23126 [1]
23127 = ((1.51009803921569)
23128 + (((-1.0)
23129 * (3.92156862745098)
23130 * pp))
23131 + (((-1.0)
23132 * x1630))
23133 + x1629
23134 + (((1.32323529411765)
23135 * cj9)));
23136 evalcond
23137 [2]
23138 = ((-0.2125)
23139 + (((-1.0)
23140 * (1.1)
23141 * pz))
23142 + (((0.09)
23143 * x1629))
23144 + (((-0.09)
23145 * x1630))
23146 + (((-1.0)
23147 * (1.0)
23148 * pp)));
23149 if (IKabs(
23150 evalcond
23151 [0])
23152 > IKFAST_EVALCOND_THRESH
23153 || IKabs(
23154 evalcond
23155 [1])
23156 > IKFAST_EVALCOND_THRESH
23157 || IKabs(
23158 evalcond
23159 [2])
23160 > IKFAST_EVALCOND_THRESH)
23161 {
23162 continue;
23163 }
23164 }
23165
23166 rotationfunction0(
23167 solutions);
23168 }
23169 }
23170 }
23171 }
23172 }
23173 } while (0);
23174 if (bgotonextstatement)
23175 {
23176 bool
23177 bgotonextstatement
23178 = true;
23179 do
23180 {
23181 IkReal x1631
23182 = ((1.32323529411765)
23183 * cj9);
23184 IkReal x1632
23185 = ((3.92156862745098)
23186 * pp);
23187 evalcond[0]
23188 = ((IKabs(
23189 py))
23190 + (IKabs(
23191 px)));
23192 evalcond[1]
23193 = ((((-1.0)
23194 * sj8
23195 * x1631))
23196 + ((sj8
23197 * x1632))
23198 + (((-1.0)
23199 * (1.51009803921569)
23200 * sj8)));
23201 evalcond[2]
23202 = 0;
23203 evalcond[3]
23204 = ((-1.51009803921569)
23205 + (((-1.0)
23206 * x1631))
23207 + x1632);
23208 evalcond[4]
23209 = ((((1.51009803921569)
23210 * cj8))
23211 + (((-1.0)
23212 * cj8
23213 * x1632))
23214 + ((cj8
23215 * x1631)));
23216 evalcond[5]
23217 = ((-0.2125)
23218 + (((-1.0)
23219 * (1.1)
23220 * pz))
23221 + (((-1.0)
23222 * (1.0)
23223 * pp)));
23224 if (IKabs(
23225 evalcond
23226 [0])
23227 < 0.0000010000000000
23228 && IKabs(
23229 evalcond
23230 [1])
23231 < 0.0000010000000000
23232 && IKabs(
23233 evalcond
23234 [2])
23235 < 0.0000010000000000
23236 && IKabs(
23237 evalcond
23238 [3])
23239 < 0.0000010000000000
23240 && IKabs(
23241 evalcond
23242 [4])
23243 < 0.0000010000000000
23244 && IKabs(
23245 evalcond
23246 [5])
23247 < 0.0000010000000000)
23248 {
23249 bgotonextstatement
23250 = false;
23251 {
23252 IkReal j4array
23253 [4],
23254 cj4array
23255 [4],
23256 sj4array
23257 [4];
23258 bool j4valid
23259 [4]
23260 = {false};
23261 _nj4 = 4;
23262 j4array[0]
23263 = 0;
23264 sj4array[0] = IKsin(
23265 j4array
23266 [0]);
23267 cj4array[0] = IKcos(
23268 j4array
23269 [0]);
23270 j4array[1]
23271 = 1.5707963267949;
23272 sj4array[1] = IKsin(
23273 j4array
23274 [1]);
23275 cj4array[1] = IKcos(
23276 j4array
23277 [1]);
23278 j4array[2]
23279 = 3.14159265358979;
23280 sj4array[2] = IKsin(
23281 j4array
23282 [2]);
23283 cj4array[2] = IKcos(
23284 j4array
23285 [2]);
23286 j4array[3]
23287 = -1.5707963267949;
23288 sj4array[3] = IKsin(
23289 j4array
23290 [3]);
23291 cj4array[3] = IKcos(
23292 j4array
23293 [3]);
23294 if (j4array
23295 [0]
23296 > IKPI)
23297 {
23298 j4array
23299 [0]
23300 -= IK2PI;
23301 }
23302 else if (
23303 j4array
23304 [0]
23305 < -IKPI)
23306 {
23307 j4array
23308 [0]
23309 += IK2PI;
23310 }
23311 j4valid[0]
23312 = true;
23313 if (j4array
23314 [1]
23315 > IKPI)
23316 {
23317 j4array
23318 [1]
23319 -= IK2PI;
23320 }
23321 else if (
23322 j4array
23323 [1]
23324 < -IKPI)
23325 {
23326 j4array
23327 [1]
23328 += IK2PI;
23329 }
23330 j4valid[1]
23331 = true;
23332 if (j4array
23333 [2]
23334 > IKPI)
23335 {
23336 j4array
23337 [2]
23338 -= IK2PI;
23339 }
23340 else if (
23341 j4array
23342 [2]
23343 < -IKPI)
23344 {
23345 j4array
23346 [2]
23347 += IK2PI;
23348 }
23349 j4valid[2]
23350 = true;
23351 if (j4array
23352 [3]
23353 > IKPI)
23354 {
23355 j4array
23356 [3]
23357 -= IK2PI;
23358 }
23359 else if (
23360 j4array
23361 [3]
23362 < -IKPI)
23363 {
23364 j4array
23365 [3]
23366 += IK2PI;
23367 }
23368 j4valid[3]
23369 = true;
23370 for (
23371 int ij4
23372 = 0;
23373 ij4
23374 < 4;
23375 ++ij4)
23376 {
23377 if (!j4valid
23378 [ij4])
23379 {
23380 continue;
23381 }
23382 _ij4[0]
23383 = ij4;
23384 _ij4[1]
23385 = -1;
23386 for (
23387 int iij4
23388 = ij4
23389 + 1;
23390 iij4
23391 < 4;
23392 ++iij4)
23393 {
23394 if (j4valid
23395 [iij4]
23396 && IKabs(
23397 cj4array
23398 [ij4]
23399 - cj4array
23400 [iij4])
23401 < IKFAST_SOLUTION_THRESH
23402 && IKabs(
23403 sj4array
23404 [ij4]
23405 - sj4array
23406 [iij4])
23407 < IKFAST_SOLUTION_THRESH)
23408 {
23409 j4valid
23410 [iij4]
23411 = false;
23412 _ij4[1]
23413 = iij4;
23414 break;
23415 }
23416 }
23417 j4 = j4array
23418 [ij4];
23419 cj4 = cj4array
23420 [ij4];
23421 sj4 = sj4array
23422 [ij4];
23423
23424 rotationfunction0(
23425 solutions);
23426 }
23427 }
23428 }
23429 } while (0);
23430 if (bgotonextstatement)
23431 {
23432 bool
23433 bgotonextstatement
23434 = true;
23435 do
23436 {
23437 if (1)
23438 {
23439 bgotonextstatement
23440 = false;
23441 continue; // branch miss [j4]
23442 }
23443 } while (0);
23444 if (bgotonextstatement)
23445 {
23446 }
23447 }
23448 }
23449 }
23450 }
23451 }
23452 else
23453 {
23454 {
23455 IkReal j4array[1],
23456 cj4array[1],
23457 sj4array[1];
23458 bool j4valid[1]
23459 = {false};
23460 _nj4 = 1;
23461 IkReal x1633
23462 = ((1.51009803921569)
23463 * cj8 * sj8);
23464 IkReal x1634
23465 = cj8 * cj8;
23466 IkReal x1635
23467 = ((1.51009803921569)
23468 * x1634);
23469 IkReal x1636
23470 = ((1.32323529411765)
23471 * cj8 * cj9
23472 * sj8);
23473 IkReal x1637
23474 = ((3.92156862745098)
23475 * cj8 * pp
23476 * sj8);
23477 IkReal x1638
23478 = ((1.32323529411765)
23479 * cj9
23480 * x1634);
23481 IkReal x1639
23482 = ((3.92156862745098)
23483 * pp
23484 * x1634);
23485 CheckValue<IkReal> x1640 = IKatan2WithCheck(
23486 IkReal((
23487 (((-1.0)
23488 * px
23489 * x1637))
23490 + ((py
23491 * x1635))
23492 + ((py
23493 * x1638))
23494 + ((px
23495 * x1633))
23496 + ((px
23497 * x1636))
23498 + (((-1.0)
23499 * py
23500 * x1639)))),
23501 (((px * x1638))
23502 + ((py
23503 * x1637))
23504 + (((-1.0) * px
23505 * x1639))
23506 + (((-1.0) * py
23507 * x1633))
23508 + ((px
23509 * x1635))
23510 + (((-1.0) * py
23511 * x1636))),
23512 IKFAST_ATAN2_MAGTHRESH);
23513 if (!x1640.valid)
23514 {
23515 continue;
23516 }
23517 CheckValue<IkReal>
23518 x1641
23519 = IKPowWithIntegerCheck(
23520 IKsign((
23521 (((-1.0)
23522 * (1.0)
23523 * cj8
23524 * (pz
23525 * pz)))
23526 + ((cj8
23527 * pp)))),
23528 -1);
23529 if (!x1641.valid)
23530 {
23531 continue;
23532 }
23533 j4array[0]
23534 = ((-1.5707963267949)
23535 + (x1640
23536 .value)
23537 + (((1.5707963267949)
23538 * (x1641
23539 .value))));
23540 sj4array[0] = IKsin(
23541 j4array[0]);
23542 cj4array[0] = IKcos(
23543 j4array[0]);
23544 if (j4array[0]
23545 > IKPI)
23546 {
23547 j4array[0]
23548 -= IK2PI;
23549 }
23550 else if (
23551 j4array[0]
23552 < -IKPI)
23553 {
23554 j4array[0]
23555 += IK2PI;
23556 }
23557 j4valid[0] = true;
23558 for (int ij4 = 0;
23559 ij4 < 1;
23560 ++ij4)
23561 {
23562 if (!j4valid[ij4])
23563 {
23564 continue;
23565 }
23566 _ij4[0] = ij4;
23567 _ij4[1] = -1;
23568 for (int iij4
23569 = ij4 + 1;
23570 iij4 < 1;
23571 ++iij4)
23572 {
23573 if (j4valid
23574 [iij4]
23575 && IKabs(
23576 cj4array
23577 [ij4]
23578 - cj4array
23579 [iij4])
23580 < IKFAST_SOLUTION_THRESH
23581 && IKabs(
23582 sj4array
23583 [ij4]
23584 - sj4array
23585 [iij4])
23586 < IKFAST_SOLUTION_THRESH)
23587 {
23588 j4valid[iij4]
23589 = false;
23590 _ij4[1]
23591 = iij4;
23592 break;
23593 }
23594 }
23595 j4 = j4array[ij4];
23596 cj4 = cj4array
23597 [ij4];
23598 sj4 = sj4array
23599 [ij4];
23600 {
23601 IkReal
23602 evalcond[5];
23603 IkReal x1642
23604 = ((1.32323529411765)
23605 * cj9);
23606 IkReal x1643
23607 = ((3.92156862745098)
23608 * pp);
23609 IkReal x1644
23610 = IKsin(j4);
23611 IkReal x1645
23612 = (px
23613 * x1644);
23614 IkReal x1646
23615 = IKcos(j4);
23616 IkReal x1647
23617 = ((1.0)
23618 * x1646);
23619 IkReal x1648
23620 = (py
23621 * x1647);
23622 IkReal x1649
23623 = (px
23624 * x1647);
23625 IkReal x1650
23626 = (py
23627 * x1644);
23628 IkReal x1651
23629 = ((1.0)
23630 * x1650);
23631 IkReal x1652
23632 = (cj8 * px
23633 * x1646);
23634 IkReal x1653
23635 = (cj8
23636 * x1650);
23637 IkReal x1654
23638 = (sj8
23639 * x1645);
23640 evalcond[0]
23641 = (((sj8
23642 * x1643))
23643 + (((-1.0)
23644 * sj8
23645 * x1642))
23646 + (((-1.0)
23647 * x1648))
23648 + (((-1.0)
23649 * (1.51009803921569)
23650 * sj8))
23651 + x1645);
23652 evalcond[1]
23653 = (((cj8
23654 * x1642))
23655 + (((1.51009803921569)
23656 * cj8))
23657 + (((-1.0)
23658 * cj8
23659 * x1643))
23660 + (((-1.0)
23661 * x1651))
23662 + (((-1.0)
23663 * x1649)));
23664 evalcond[2]
23665 = ((((-1.0)
23666 * sj8
23667 * x1649))
23668 + (((-1.0)
23669 * sj8
23670 * x1651))
23671 + ((cj8
23672 * x1645))
23673 + (((-1.0)
23674 * cj8
23675 * x1648)));
23676 evalcond[3]
23677 = ((-1.51009803921569)
23678 + (((-1.0)
23679 * sj8
23680 * x1648))
23681 + x1654
23682 + x1652
23683 + x1653
23684 + (((-1.0)
23685 * x1642))
23686 + x1643);
23687 evalcond[4]
23688 = ((-0.2125)
23689 + (((-1.0)
23690 * (1.1)
23691 * pz))
23692 + (((-0.09)
23693 * x1654))
23694 + (((0.09)
23695 * py
23696 * sj8
23697 * x1646))
23698 + (((-1.0)
23699 * (1.0)
23700 * pp))
23701 + (((-0.09)
23702 * x1652))
23703 + (((-0.09)
23704 * x1653)));
23705 if (IKabs(
23706 evalcond
23707 [0])
23708 > IKFAST_EVALCOND_THRESH
23709 || IKabs(
23710 evalcond
23711 [1])
23712 > IKFAST_EVALCOND_THRESH
23713 || IKabs(
23714 evalcond
23715 [2])
23716 > IKFAST_EVALCOND_THRESH
23717 || IKabs(
23718 evalcond
23719 [3])
23720 > IKFAST_EVALCOND_THRESH
23721 || IKabs(
23722 evalcond
23723 [4])
23724 > IKFAST_EVALCOND_THRESH)
23725 {
23726 continue;
23727 }
23728 }
23729
23730 rotationfunction0(
23731 solutions);
23732 }
23733 }
23734 }
23735 }
23736 }
23737 else
23738 {
23739 {
23740 IkReal j4array[1],
23741 cj4array[1],
23742 sj4array[1];
23743 bool j4valid[1]
23744 = {false};
23745 _nj4 = 1;
23746 IkReal x1655
23747 = ((1.51009803921569)
23748 * cj8 * sj8);
23749 IkReal x1656
23750 = cj8 * cj8;
23751 IkReal x1657
23752 = ((1.51009803921569)
23753 * x1656);
23754 IkReal x1658
23755 = ((1.32323529411765)
23756 * cj8 * cj9
23757 * sj8);
23758 IkReal x1659
23759 = ((3.92156862745098)
23760 * cj8 * pp
23761 * sj8);
23762 IkReal x1660
23763 = ((1.32323529411765)
23764 * cj9 * x1656);
23765 IkReal x1661
23766 = ((3.92156862745098)
23767 * pp * x1656);
23768 CheckValue<IkReal> x1662
23769 = IKPowWithIntegerCheck(
23770 IKsign((
23771 ((cj8
23772 * (pz
23773 * pz)))
23774 + (((-1.0)
23775 * cj8
23776 * pp)))),
23777 -1);
23778 if (!x1662.valid)
23779 {
23780 continue;
23781 }
23782 CheckValue<IkReal> x1663
23783 = IKatan2WithCheck(
23784 IkReal((
23785 (((-1.0)
23786 * px
23787 * x1658))
23788 + ((py
23789 * x1661))
23790 + ((px
23791 * x1659))
23792 + (((-1.0)
23793 * py
23794 * x1660))
23795 + (((-1.0)
23796 * px
23797 * x1655))
23798 + (((-1.0)
23799 * py
23800 * x1657)))),
23801 ((((-1.0) * px
23802 * x1660))
23803 + ((py
23804 * x1658))
23805 + (((-1.0) * py
23806 * x1659))
23807 + ((px
23808 * x1661))
23809 + (((-1.0) * px
23810 * x1657))
23811 + ((py
23812 * x1655))),
23813 IKFAST_ATAN2_MAGTHRESH);
23814 if (!x1663.valid)
23815 {
23816 continue;
23817 }
23818 j4array[0]
23819 = ((-1.5707963267949)
23820 + (((1.5707963267949)
23821 * (x1662
23822 .value)))
23823 + (x1663.value));
23824 sj4array[0]
23825 = IKsin(j4array[0]);
23826 cj4array[0]
23827 = IKcos(j4array[0]);
23828 if (j4array[0] > IKPI)
23829 {
23830 j4array[0] -= IK2PI;
23831 }
23832 else if (
23833 j4array[0] < -IKPI)
23834 {
23835 j4array[0] += IK2PI;
23836 }
23837 j4valid[0] = true;
23838 for (int ij4 = 0;
23839 ij4 < 1;
23840 ++ij4)
23841 {
23842 if (!j4valid[ij4])
23843 {
23844 continue;
23845 }
23846 _ij4[0] = ij4;
23847 _ij4[1] = -1;
23848 for (int iij4
23849 = ij4 + 1;
23850 iij4 < 1;
23851 ++iij4)
23852 {
23853 if (j4valid[iij4]
23854 && IKabs(
23855 cj4array
23856 [ij4]
23857 - cj4array
23858 [iij4])
23859 < IKFAST_SOLUTION_THRESH
23860 && IKabs(
23861 sj4array
23862 [ij4]
23863 - sj4array
23864 [iij4])
23865 < IKFAST_SOLUTION_THRESH)
23866 {
23867 j4valid[iij4]
23868 = false;
23869 _ij4[1] = iij4;
23870 break;
23871 }
23872 }
23873 j4 = j4array[ij4];
23874 cj4 = cj4array[ij4];
23875 sj4 = sj4array[ij4];
23876 {
23877 IkReal evalcond[5];
23878 IkReal x1664
23879 = ((1.32323529411765)
23880 * cj9);
23881 IkReal x1665
23882 = ((3.92156862745098)
23883 * pp);
23884 IkReal x1666
23885 = IKsin(j4);
23886 IkReal x1667
23887 = (px * x1666);
23888 IkReal x1668
23889 = IKcos(j4);
23890 IkReal x1669
23891 = ((1.0)
23892 * x1668);
23893 IkReal x1670
23894 = (py * x1669);
23895 IkReal x1671
23896 = (px * x1669);
23897 IkReal x1672
23898 = (py * x1666);
23899 IkReal x1673
23900 = ((1.0)
23901 * x1672);
23902 IkReal x1674
23903 = (cj8 * px
23904 * x1668);
23905 IkReal x1675
23906 = (cj8 * x1672);
23907 IkReal x1676
23908 = (sj8 * x1667);
23909 evalcond[0]
23910 = (((sj8
23911 * x1665))
23912 + (((-1.0)
23913 * sj8
23914 * x1664))
23915 + (((-1.0)
23916 * x1670))
23917 + (((-1.0)
23918 * (1.51009803921569)
23919 * sj8))
23920 + x1667);
23921 evalcond[1]
23922 = ((((-1.0)
23923 * cj8
23924 * x1665))
23925 + (((1.51009803921569)
23926 * cj8))
23927 + (((-1.0)
23928 * x1673))
23929 + ((cj8
23930 * x1664))
23931 + (((-1.0)
23932 * x1671)));
23933 evalcond[2]
23934 = (((cj8
23935 * x1667))
23936 + (((-1.0)
23937 * sj8
23938 * x1673))
23939 + (((-1.0)
23940 * cj8
23941 * x1670))
23942 + (((-1.0)
23943 * sj8
23944 * x1671)));
23945 evalcond[3]
23946 = ((-1.51009803921569)
23947 + x1676
23948 + x1674
23949 + x1675
23950 + (((-1.0)
23951 * x1664))
23952 + (((-1.0)
23953 * sj8
23954 * x1670))
23955 + x1665);
23956 evalcond[4]
23957 = ((-0.2125)
23958 + (((-1.0)
23959 * (1.1)
23960 * pz))
23961 + (((0.09)
23962 * py
23963 * sj8
23964 * x1668))
23965 + (((-0.09)
23966 * x1674))
23967 + (((-0.09)
23968 * x1676))
23969 + (((-1.0)
23970 * (1.0)
23971 * pp))
23972 + (((-0.09)
23973 * x1675)));
23974 if (IKabs(
23975 evalcond[0])
23976 > IKFAST_EVALCOND_THRESH
23977 || IKabs(
23978 evalcond
23979 [1])
23980 > IKFAST_EVALCOND_THRESH
23981 || IKabs(
23982 evalcond
23983 [2])
23984 > IKFAST_EVALCOND_THRESH
23985 || IKabs(
23986 evalcond
23987 [3])
23988 > IKFAST_EVALCOND_THRESH
23989 || IKabs(
23990 evalcond
23991 [4])
23992 > IKFAST_EVALCOND_THRESH)
23993 {
23994 continue;
23995 }
23996 }
23997
23998 rotationfunction0(
23999 solutions);
24000 }
24001 }
24002 }
24003 }
24004 }
24005 else
24006 {
24007 {
24008 IkReal j4array[1],
24009 cj4array[1],
24010 sj4array[1];
24011 bool j4valid[1] = {false};
24012 _nj4 = 1;
24013 IkReal x1677
24014 = ((1.51009803921569)
24015 * cj8);
24016 IkReal x1678
24017 = ((1.51009803921569)
24018 * sj8);
24019 IkReal x1679
24020 = ((1.32323529411765)
24021 * cj8 * cj9);
24022 IkReal x1680
24023 = ((3.92156862745098)
24024 * cj8 * pp);
24025 IkReal x1681
24026 = ((1.32323529411765)
24027 * cj9 * sj8);
24028 IkReal x1682
24029 = ((3.92156862745098)
24030 * pp * sj8);
24031 CheckValue<IkReal> x1683
24032 = IKPowWithIntegerCheck(
24033 IKsign((
24034 pp
24035 + (((-1.0)
24036 * (1.0)
24037 * (pz
24038 * pz))))),
24039 -1);
24040 if (!x1683.valid)
24041 {
24042 continue;
24043 }
24044 CheckValue<IkReal> x1684
24045 = IKatan2WithCheck(
24046 IkReal((
24047 ((py * x1677))
24048 + (((-1.0) * px
24049 * x1682))
24050 + ((px * x1678))
24051 + ((py * x1679))
24052 + ((px * x1681))
24053 + (((-1.0) * py
24054 * x1680)))),
24055 ((((-1.0) * py
24056 * x1678))
24057 + ((py * x1682))
24058 + (((-1.0) * px
24059 * x1680))
24060 + ((px * x1679))
24061 + ((px * x1677))
24062 + (((-1.0) * py
24063 * x1681))),
24064 IKFAST_ATAN2_MAGTHRESH);
24065 if (!x1684.valid)
24066 {
24067 continue;
24068 }
24069 j4array[0]
24070 = ((-1.5707963267949)
24071 + (((1.5707963267949)
24072 * (x1683.value)))
24073 + (x1684.value));
24074 sj4array[0]
24075 = IKsin(j4array[0]);
24076 cj4array[0]
24077 = IKcos(j4array[0]);
24078 if (j4array[0] > IKPI)
24079 {
24080 j4array[0] -= IK2PI;
24081 }
24082 else if (j4array[0] < -IKPI)
24083 {
24084 j4array[0] += IK2PI;
24085 }
24086 j4valid[0] = true;
24087 for (int ij4 = 0; ij4 < 1;
24088 ++ij4)
24089 {
24090 if (!j4valid[ij4])
24091 {
24092 continue;
24093 }
24094 _ij4[0] = ij4;
24095 _ij4[1] = -1;
24096 for (int iij4 = ij4 + 1;
24097 iij4 < 1;
24098 ++iij4)
24099 {
24100 if (j4valid[iij4]
24101 && IKabs(
24102 cj4array[ij4]
24103 - cj4array
24104 [iij4])
24105 < IKFAST_SOLUTION_THRESH
24106 && IKabs(
24107 sj4array[ij4]
24108 - sj4array
24109 [iij4])
24110 < IKFAST_SOLUTION_THRESH)
24111 {
24112 j4valid[iij4] = false;
24113 _ij4[1] = iij4;
24114 break;
24115 }
24116 }
24117 j4 = j4array[ij4];
24118 cj4 = cj4array[ij4];
24119 sj4 = sj4array[ij4];
24120 {
24121 IkReal evalcond[5];
24122 IkReal x1685
24123 = ((1.32323529411765)
24124 * cj9);
24125 IkReal x1686
24126 = ((3.92156862745098)
24127 * pp);
24128 IkReal x1687
24129 = IKsin(j4);
24130 IkReal x1688
24131 = (px * x1687);
24132 IkReal x1689
24133 = IKcos(j4);
24134 IkReal x1690
24135 = ((1.0) * x1689);
24136 IkReal x1691
24137 = (py * x1690);
24138 IkReal x1692
24139 = (px * x1690);
24140 IkReal x1693
24141 = (py * x1687);
24142 IkReal x1694
24143 = ((1.0) * x1693);
24144 IkReal x1695
24145 = (cj8 * px
24146 * x1689);
24147 IkReal x1696
24148 = (cj8 * x1693);
24149 IkReal x1697
24150 = (sj8 * x1688);
24151 evalcond[0]
24152 = (x1688
24153 + (((-1.0)
24154 * x1691))
24155 + (((-1.0) * sj8
24156 * x1685))
24157 + ((sj8 * x1686))
24158 + (((-1.0)
24159 * (1.51009803921569)
24160 * sj8)));
24161 evalcond[1]
24162 = (((cj8 * x1685))
24163 + (((-1.0) * cj8
24164 * x1686))
24165 + (((-1.0)
24166 * x1692))
24167 + (((1.51009803921569)
24168 * cj8))
24169 + (((-1.0)
24170 * x1694)));
24171 evalcond[2]
24172 = ((((-1.0) * cj8
24173 * x1691))
24174 + (((-1.0) * sj8
24175 * x1692))
24176 + ((cj8 * x1688))
24177 + (((-1.0) * sj8
24178 * x1694)));
24179 evalcond[3]
24180 = ((-1.51009803921569)
24181 + x1695 + x1696
24182 + x1697 + x1686
24183 + (((-1.0) * sj8
24184 * x1691))
24185 + (((-1.0)
24186 * x1685)));
24187 evalcond[4]
24188 = ((-0.2125)
24189 + (((-1.0)
24190 * (1.1)
24191 * pz))
24192 + (((-0.09)
24193 * x1697))
24194 + (((-0.09)
24195 * x1696))
24196 + (((-0.09)
24197 * x1695))
24198 + (((0.09) * py
24199 * sj8
24200 * x1689))
24201 + (((-1.0)
24202 * (1.0)
24203 * pp)));
24204 if (IKabs(evalcond[0])
24205 > IKFAST_EVALCOND_THRESH
24206 || IKabs(
24207 evalcond[1])
24208 > IKFAST_EVALCOND_THRESH
24209 || IKabs(
24210 evalcond[2])
24211 > IKFAST_EVALCOND_THRESH
24212 || IKabs(
24213 evalcond[3])
24214 > IKFAST_EVALCOND_THRESH
24215 || IKabs(
24216 evalcond[4])
24217 > IKFAST_EVALCOND_THRESH)
24218 {
24219 continue;
24220 }
24221 }
24222
24223 rotationfunction0(
24224 solutions);
24225 }
24226 }
24227 }
24228 }
24229 }
24230 } while (0);
24231 if (bgotonextstatement)
24232 {
24233 bool bgotonextstatement = true;
24234 do
24235 {
24236 if (1)
24237 {
24238 bgotonextstatement = false;
24239 continue; // branch miss [j4]
24240 }
24241 } while (0);
24242 if (bgotonextstatement)
24243 {
24244 }
24245 }
24246 }
24247 }
24248 }
24249 }
24250 }
24251 else
24252 {
24253 {
24254 IkReal j4array[1], cj4array[1], sj4array[1];
24255 bool j4valid[1] = {false};
24256 _nj4 = 1;
24257 IkReal x1698 = ((0.045) * sj8);
24258 IkReal x1699 = (px * x1698);
24259 IkReal x1700 = ((0.55) * sj6);
24260 IkReal x1701 = ((0.045) * cj6 * cj8);
24261 IkReal x1702 = (py * x1701);
24262 IkReal x1703 = ((0.3) * cj9 * sj6);
24263 IkReal x1704 = ((0.3) * sj8);
24264 IkReal x1705 = (px * sj9);
24265 IkReal x1706 = ((0.045) * sj6);
24266 IkReal x1707 = (py * sj9);
24267 IkReal x1708 = ((0.3) * cj6 * cj8);
24268 IkReal x1709 = (py * x1698);
24269 IkReal x1710 = (px * x1701);
24270 CheckValue<IkReal> x1711 = IKatan2WithCheck(
24271 IkReal(
24272 ((((-1.0) * x1699))
24273 + (((-1.0) * x1704 * x1705))
24274 + (((-1.0) * cj9 * x1702))
24275 + ((py * x1700)) + x1702
24276 + ((x1706 * x1707))
24277 + ((py * x1703)) + ((cj9 * x1699))
24278 + ((x1707 * x1708)))),
24279 (x1709 + (((-1.0) * cj9 * x1709))
24280 + x1710 + ((px * x1700))
24281 + ((px * x1703)) + ((x1705 * x1708))
24282 + ((x1704 * x1707))
24283 + (((-1.0) * cj9 * x1710))
24284 + ((x1705 * x1706))),
24285 IKFAST_ATAN2_MAGTHRESH);
24286 if (!x1711.valid)
24287 {
24288 continue;
24289 }
24290 CheckValue<IkReal> x1712
24291 = IKPowWithIntegerCheck(
24292 IKsign(
24293 (pp
24294 + (((-1.0) * (1.0)
24295 * (pz * pz))))),
24296 -1);
24297 if (!x1712.valid)
24298 {
24299 continue;
24300 }
24301 j4array[0]
24302 = ((-1.5707963267949) + (x1711.value)
24303 + (((1.5707963267949)
24304 * (x1712.value))));
24305 sj4array[0] = IKsin(j4array[0]);
24306 cj4array[0] = IKcos(j4array[0]);
24307 if (j4array[0] > IKPI)
24308 {
24309 j4array[0] -= IK2PI;
24310 }
24311 else if (j4array[0] < -IKPI)
24312 {
24313 j4array[0] += IK2PI;
24314 }
24315 j4valid[0] = true;
24316 for (int ij4 = 0; ij4 < 1; ++ij4)
24317 {
24318 if (!j4valid[ij4])
24319 {
24320 continue;
24321 }
24322 _ij4[0] = ij4;
24323 _ij4[1] = -1;
24324 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24325 {
24326 if (j4valid[iij4]
24327 && IKabs(
24328 cj4array[ij4]
24329 - cj4array[iij4])
24330 < IKFAST_SOLUTION_THRESH
24331 && IKabs(
24332 sj4array[ij4]
24333 - sj4array[iij4])
24334 < IKFAST_SOLUTION_THRESH)
24335 {
24336 j4valid[iij4] = false;
24337 _ij4[1] = iij4;
24338 break;
24339 }
24340 }
24341 j4 = j4array[ij4];
24342 cj4 = cj4array[ij4];
24343 sj4 = sj4array[ij4];
24344 {
24345 IkReal evalcond[6];
24346 IkReal x1713 = ((0.3) * cj9);
24347 IkReal x1714 = ((0.045) * sj9);
24348 IkReal x1715 = (cj6 * pz);
24349 IkReal x1716 = IKcos(j4);
24350 IkReal x1717 = (px * sj6 * x1716);
24351 IkReal x1718 = IKsin(j4);
24352 IkReal x1719 = (py * x1718);
24353 IkReal x1720 = (sj6 * x1719);
24354 IkReal x1721 = ((0.045) * cj9);
24355 IkReal x1722 = (px * x1718);
24356 IkReal x1723 = ((0.3) * sj9);
24357 IkReal x1724 = ((1.0) * x1716);
24358 IkReal x1725 = (py * x1724);
24359 IkReal x1726 = (sj8 * x1716);
24360 IkReal x1727 = (cj6 * cj8);
24361 IkReal x1728 = (px * x1724);
24362 IkReal x1729 = ((1.0) * x1719);
24363 IkReal x1730 = (cj8 * pz * sj6);
24364 IkReal x1731 = (sj8 * x1722);
24365 IkReal x1732 = ((0.09) * cj6 * cj8);
24366 evalcond[0]
24367 = ((-0.55) + (((-1.0) * x1714))
24368 + x1715 + x1717
24369 + (((-1.0) * x1713)) + x1720);
24370 evalcond[1]
24371 = ((((-1.0) * sj8 * x1721))
24372 + (((-1.0) * x1725))
24373 + (((0.045) * sj8)) + x1722
24374 + ((sj8 * x1723)));
24375 evalcond[2]
24376 = (((cj6 * sj8 * x1719))
24377 + ((cj6 * px * x1726))
24378 + (((-1.0) * (1.0) * pz * sj6
24379 * sj8))
24380 + (((-1.0) * cj8 * x1725))
24381 + ((cj8 * x1722)));
24382 evalcond[3]
24383 = ((((0.045) * x1727))
24384 + ((x1723 * x1727))
24385 + ((sj6 * x1714))
24386 + (((0.55) * sj6))
24387 + (((-1.0) * x1721 * x1727))
24388 + (((-1.0) * x1729))
24389 + (((-1.0) * x1728))
24390 + ((sj6 * x1713)));
24391 evalcond[4]
24392 = ((0.045) + (((-1.0) * x1721))
24393 + (((-1.0) * x1727 * x1729))
24394 + (((-1.0) * x1727 * x1728))
24395 + (((-1.0) * sj8 * x1725))
24396 + x1723 + x1731 + x1730);
24397 evalcond[5]
24398 = ((-0.2125)
24399 + (((0.09) * py * x1726))
24400 + (((-0.09) * x1731))
24401 + ((x1719 * x1732))
24402 + (((1.1) * x1717))
24403 + ((px * x1716 * x1732))
24404 + (((1.1) * x1720))
24405 + (((-1.0) * (1.0) * pp))
24406 + (((1.1) * x1715))
24407 + (((-0.09) * x1730)));
24408 if (IKabs(evalcond[0])
24409 > IKFAST_EVALCOND_THRESH
24410 || IKabs(evalcond[1])
24411 > IKFAST_EVALCOND_THRESH
24412 || IKabs(evalcond[2])
24413 > IKFAST_EVALCOND_THRESH
24414 || IKabs(evalcond[3])
24415 > IKFAST_EVALCOND_THRESH
24416 || IKabs(evalcond[4])
24417 > IKFAST_EVALCOND_THRESH
24418 || IKabs(evalcond[5])
24419 > IKFAST_EVALCOND_THRESH)
24420 {
24421 continue;
24422 }
24423 }
24424
24425 rotationfunction0(solutions);
24426 }
24427 }
24428 }
24429 }
24430 }
24431 else
24432 {
24433 {
24434 IkReal j4array[1], cj4array[1], sj4array[1];
24435 bool j4valid[1] = {false};
24436 _nj4 = 1;
24437 IkReal x1733 = (cj8 * sj6);
24438 IkReal x1734 = ((0.55) * cj8);
24439 IkReal x1735 = ((0.55) * cj6);
24440 IkReal x1736 = (px * sj8);
24441 IkReal x1737 = ((0.3) * cj8 * cj9);
24442 IkReal x1738 = ((0.045) * cj8 * sj9);
24443 IkReal x1739 = (cj6 * cj8 * pz);
24444 IkReal x1740 = ((0.3) * cj6 * cj9);
24445 IkReal x1741 = ((0.045) * cj6 * sj9);
24446 IkReal x1742 = (py * sj8);
24447 CheckValue<IkReal> x1743 = IKatan2WithCheck(
24448 IkReal(
24449 ((((-1.0) * pz * x1736))
24450 + ((x1735 * x1736)) + ((py * x1739))
24451 + (((-1.0) * py * x1738))
24452 + ((x1736 * x1740))
24453 + (((-1.0) * py * x1737))
24454 + (((-1.0) * py * x1734))
24455 + ((x1736 * x1741)))),
24456 ((((-1.0) * px * x1738)) + ((px * x1739))
24457 + (((-1.0) * px * x1737))
24458 + (((-1.0) * px * x1734))
24459 + (((-1.0) * x1740 * x1742))
24460 + (((-1.0) * x1735 * x1742))
24461 + (((-1.0) * x1741 * x1742))
24462 + ((pz * x1742))),
24463 IKFAST_ATAN2_MAGTHRESH);
24464 if (!x1743.valid)
24465 {
24466 continue;
24467 }
24468 CheckValue<IkReal> x1744
24469 = IKPowWithIntegerCheck(
24470 IKsign(
24471 (((x1733 * (pz * pz)))
24472 + (((-1.0) * pp * x1733)))),
24473 -1);
24474 if (!x1744.valid)
24475 {
24476 continue;
24477 }
24478 j4array[0]
24479 = ((-1.5707963267949) + (x1743.value)
24480 + (((1.5707963267949) * (x1744.value))));
24481 sj4array[0] = IKsin(j4array[0]);
24482 cj4array[0] = IKcos(j4array[0]);
24483 if (j4array[0] > IKPI)
24484 {
24485 j4array[0] -= IK2PI;
24486 }
24487 else if (j4array[0] < -IKPI)
24488 {
24489 j4array[0] += IK2PI;
24490 }
24491 j4valid[0] = true;
24492 for (int ij4 = 0; ij4 < 1; ++ij4)
24493 {
24494 if (!j4valid[ij4])
24495 {
24496 continue;
24497 }
24498 _ij4[0] = ij4;
24499 _ij4[1] = -1;
24500 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24501 {
24502 if (j4valid[iij4]
24503 && IKabs(cj4array[ij4] - cj4array[iij4])
24504 < IKFAST_SOLUTION_THRESH
24505 && IKabs(sj4array[ij4] - sj4array[iij4])
24506 < IKFAST_SOLUTION_THRESH)
24507 {
24508 j4valid[iij4] = false;
24509 _ij4[1] = iij4;
24510 break;
24511 }
24512 }
24513 j4 = j4array[ij4];
24514 cj4 = cj4array[ij4];
24515 sj4 = sj4array[ij4];
24516 {
24517 IkReal evalcond[6];
24518 IkReal x1745 = ((0.3) * cj9);
24519 IkReal x1746 = ((0.045) * sj9);
24520 IkReal x1747 = (cj6 * pz);
24521 IkReal x1748 = IKcos(j4);
24522 IkReal x1749 = (px * sj6 * x1748);
24523 IkReal x1750 = IKsin(j4);
24524 IkReal x1751 = (py * x1750);
24525 IkReal x1752 = (sj6 * x1751);
24526 IkReal x1753 = ((0.045) * cj9);
24527 IkReal x1754 = (px * x1750);
24528 IkReal x1755 = ((0.3) * sj9);
24529 IkReal x1756 = ((1.0) * x1748);
24530 IkReal x1757 = (py * x1756);
24531 IkReal x1758 = (sj8 * x1748);
24532 IkReal x1759 = (cj6 * cj8);
24533 IkReal x1760 = (px * x1756);
24534 IkReal x1761 = ((1.0) * x1751);
24535 IkReal x1762 = (cj8 * pz * sj6);
24536 IkReal x1763 = (sj8 * x1754);
24537 IkReal x1764 = ((0.09) * cj6 * cj8);
24538 evalcond[0]
24539 = ((-0.55) + (((-1.0) * x1746))
24540 + (((-1.0) * x1745)) + x1747 + x1749
24541 + x1752);
24542 evalcond[1]
24543 = ((((0.045) * sj8))
24544 + (((-1.0) * sj8 * x1753))
24545 + ((sj8 * x1755))
24546 + (((-1.0) * x1757)) + x1754);
24547 evalcond[2]
24548 = (((cj6 * sj8 * x1751))
24549 + (((-1.0) * cj8 * x1757))
24550 + ((cj8 * x1754))
24551 + (((-1.0) * (1.0) * pz * sj6 * sj8))
24552 + ((cj6 * px * x1758)));
24553 evalcond[3]
24554 = ((((-1.0) * x1761))
24555 + (((-1.0) * x1753 * x1759))
24556 + (((0.55) * sj6))
24557 + ((x1755 * x1759)) + ((sj6 * x1746))
24558 + (((0.045) * x1759))
24559 + ((sj6 * x1745))
24560 + (((-1.0) * x1760)));
24561 evalcond[4]
24562 = ((0.045) + (((-1.0) * sj8 * x1757))
24563 + (((-1.0) * x1759 * x1760))
24564 + (((-1.0) * x1759 * x1761)) + x1762
24565 + x1763 + (((-1.0) * x1753))
24566 + x1755);
24567 evalcond[5]
24568 = ((-0.2125) + (((-0.09) * x1762))
24569 + ((px * x1748 * x1764))
24570 + (((1.1) * x1749))
24571 + ((x1751 * x1764))
24572 + (((0.09) * py * x1758))
24573 + (((-1.0) * (1.0) * pp))
24574 + (((1.1) * x1747))
24575 + (((1.1) * x1752))
24576 + (((-0.09) * x1763)));
24577 if (IKabs(evalcond[0])
24578 > IKFAST_EVALCOND_THRESH
24579 || IKabs(evalcond[1])
24580 > IKFAST_EVALCOND_THRESH
24581 || IKabs(evalcond[2])
24582 > IKFAST_EVALCOND_THRESH
24583 || IKabs(evalcond[3])
24584 > IKFAST_EVALCOND_THRESH
24585 || IKabs(evalcond[4])
24586 > IKFAST_EVALCOND_THRESH
24587 || IKabs(evalcond[5])
24588 > IKFAST_EVALCOND_THRESH)
24589 {
24590 continue;
24591 }
24592 }
24593
24594 rotationfunction0(solutions);
24595 }
24596 }
24597 }
24598 }
24599 }
24600 else
24601 {
24602 {
24603 IkReal j4array[1], cj4array[1], sj4array[1];
24604 bool j4valid[1] = {false};
24605 _nj4 = 1;
24606 IkReal x1765 = ((0.3) * cj9);
24607 IkReal x1766 = ((0.045) * sj9);
24608 IkReal x1767 = ((1.0) * cj6 * pz);
24609 IkReal x1768 = ((0.045) * sj6 * sj8);
24610 IkReal x1769 = (px * x1768);
24611 IkReal x1770 = ((0.3) * sj6 * sj8 * sj9);
24612 IkReal x1771 = (py * x1768);
24613 CheckValue<IkReal> x1772 = IKatan2WithCheck(
24614 IkReal(
24615 (((py * x1765)) + ((py * x1766))
24616 + ((cj9 * x1769)) + (((0.55) * py))
24617 + (((-1.0) * x1769))
24618 + (((-1.0) * py * x1767))
24619 + (((-1.0) * px * x1770)))),
24620 (((px * x1766)) + ((py * x1770))
24621 + (((-1.0) * cj9 * x1771)) + (((0.55) * px))
24622 + (((-1.0) * px * x1767)) + ((px * x1765))
24623 + x1771),
24624 IKFAST_ATAN2_MAGTHRESH);
24625 if (!x1772.valid)
24626 {
24627 continue;
24628 }
24629 CheckValue<IkReal> x1773 = IKPowWithIntegerCheck(
24630 IKsign(
24631 ((((-1.0) * (1.0) * sj6 * (pz * pz)))
24632 + ((pp * sj6)))),
24633 -1);
24634 if (!x1773.valid)
24635 {
24636 continue;
24637 }
24638 j4array[0]
24639 = ((-1.5707963267949) + (x1772.value)
24640 + (((1.5707963267949) * (x1773.value))));
24641 sj4array[0] = IKsin(j4array[0]);
24642 cj4array[0] = IKcos(j4array[0]);
24643 if (j4array[0] > IKPI)
24644 {
24645 j4array[0] -= IK2PI;
24646 }
24647 else if (j4array[0] < -IKPI)
24648 {
24649 j4array[0] += IK2PI;
24650 }
24651 j4valid[0] = true;
24652 for (int ij4 = 0; ij4 < 1; ++ij4)
24653 {
24654 if (!j4valid[ij4])
24655 {
24656 continue;
24657 }
24658 _ij4[0] = ij4;
24659 _ij4[1] = -1;
24660 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24661 {
24662 if (j4valid[iij4]
24663 && IKabs(cj4array[ij4] - cj4array[iij4])
24664 < IKFAST_SOLUTION_THRESH
24665 && IKabs(sj4array[ij4] - sj4array[iij4])
24666 < IKFAST_SOLUTION_THRESH)
24667 {
24668 j4valid[iij4] = false;
24669 _ij4[1] = iij4;
24670 break;
24671 }
24672 }
24673 j4 = j4array[ij4];
24674 cj4 = cj4array[ij4];
24675 sj4 = sj4array[ij4];
24676 {
24677 IkReal evalcond[6];
24678 IkReal x1774 = ((0.3) * cj9);
24679 IkReal x1775 = ((0.045) * sj9);
24680 IkReal x1776 = (cj6 * pz);
24681 IkReal x1777 = IKcos(j4);
24682 IkReal x1778 = (px * sj6 * x1777);
24683 IkReal x1779 = IKsin(j4);
24684 IkReal x1780 = (py * x1779);
24685 IkReal x1781 = (sj6 * x1780);
24686 IkReal x1782 = ((0.045) * cj9);
24687 IkReal x1783 = (px * x1779);
24688 IkReal x1784 = ((0.3) * sj9);
24689 IkReal x1785 = ((1.0) * x1777);
24690 IkReal x1786 = (py * x1785);
24691 IkReal x1787 = (sj8 * x1777);
24692 IkReal x1788 = (cj6 * cj8);
24693 IkReal x1789 = (px * x1785);
24694 IkReal x1790 = ((1.0) * x1780);
24695 IkReal x1791 = (cj8 * pz * sj6);
24696 IkReal x1792 = (sj8 * x1783);
24697 IkReal x1793 = ((0.09) * cj6 * cj8);
24698 evalcond[0]
24699 = ((-0.55) + x1781 + (((-1.0) * x1775))
24700 + (((-1.0) * x1774)) + x1778 + x1776);
24701 evalcond[1]
24702 = ((((0.045) * sj8)) + (((-1.0) * x1786))
24703 + x1783 + (((-1.0) * sj8 * x1782))
24704 + ((sj8 * x1784)));
24705 evalcond[2]
24706 = ((((-1.0) * cj8 * x1786))
24707 + ((cj8 * x1783)) + ((cj6 * px * x1787))
24708 + (((-1.0) * (1.0) * pz * sj6 * sj8))
24709 + ((cj6 * sj8 * x1780)));
24710 evalcond[3]
24711 = ((((-1.0) * x1790)) + ((x1784 * x1788))
24712 + (((-1.0) * x1782 * x1788))
24713 + (((0.55) * sj6)) + ((sj6 * x1775))
24714 + (((-1.0) * x1789))
24715 + (((0.045) * x1788)) + ((sj6 * x1774)));
24716 evalcond[4]
24717 = ((0.045) + (((-1.0) * x1782))
24718 + (((-1.0) * x1788 * x1790)) + x1784
24719 + (((-1.0) * x1788 * x1789))
24720 + (((-1.0) * sj8 * x1786)) + x1792
24721 + x1791);
24722 evalcond[5]
24723 = ((-0.2125) + (((1.1) * x1776))
24724 + (((1.1) * x1778)) + ((x1780 * x1793))
24725 + (((1.1) * x1781))
24726 + (((-1.0) * (1.0) * pp))
24727 + (((0.09) * py * x1787))
24728 + (((-0.09) * x1792))
24729 + ((px * x1777 * x1793))
24730 + (((-0.09) * x1791)));
24731 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
24732 || IKabs(evalcond[1])
24733 > IKFAST_EVALCOND_THRESH
24734 || IKabs(evalcond[2])
24735 > IKFAST_EVALCOND_THRESH
24736 || IKabs(evalcond[3])
24737 > IKFAST_EVALCOND_THRESH
24738 || IKabs(evalcond[4])
24739 > IKFAST_EVALCOND_THRESH
24740 || IKabs(evalcond[5])
24741 > IKFAST_EVALCOND_THRESH)
24742 {
24743 continue;
24744 }
24745 }
24746
24747 rotationfunction0(solutions);
24748 }
24749 }
24750 }
24751 }
24752 }
24753 }
24754 }
24755 }
24756 }
24757 else
24758 {
24759 {
24760 IkReal j4array[2], cj4array[2], sj4array[2];
24761 bool j4valid[2] = {false};
24762 _nj4 = 2;
24763 CheckValue<IkReal> x1797 = IKatan2WithCheck(
24764 IkReal(((-1.0) * (((1.0) * py)))),
24765 px,
24766 IKFAST_ATAN2_MAGTHRESH);
24767 if (!x1797.valid)
24768 {
24769 continue;
24770 }
24771 IkReal x1794 = ((-1.0) * (x1797.value));
24772 IkReal x1795 = ((0.045) * sj8);
24773 if ((((py * py) + (px * px))) < -0.00001)
24774 continue;
24775 CheckValue<IkReal> x1798 = IKPowWithIntegerCheck(
24776 IKabs(IKsqrt(((py * py) + (px * px)))), -1);
24777 if (!x1798.valid)
24778 {
24779 continue;
24780 }
24781 if ((((x1798.value)
24782 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24783 + x1795))))
24784 < -1 - IKFAST_SINCOS_THRESH
24785 || (((x1798.value)
24786 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24787 + x1795))))
24788 > 1 + IKFAST_SINCOS_THRESH)
24789 continue;
24790 IkReal x1796 = IKasin(
24791 ((x1798.value)
24792 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24793 + x1795))));
24794 j4array[0] = ((((-1.0) * x1796)) + x1794);
24795 sj4array[0] = IKsin(j4array[0]);
24796 cj4array[0] = IKcos(j4array[0]);
24797 j4array[1] = ((3.14159265358979) + x1794 + x1796);
24798 sj4array[1] = IKsin(j4array[1]);
24799 cj4array[1] = IKcos(j4array[1]);
24800 if (j4array[0] > IKPI)
24801 {
24802 j4array[0] -= IK2PI;
24803 }
24804 else if (j4array[0] < -IKPI)
24805 {
24806 j4array[0] += IK2PI;
24807 }
24808 j4valid[0] = true;
24809 if (j4array[1] > IKPI)
24810 {
24811 j4array[1] -= IK2PI;
24812 }
24813 else if (j4array[1] < -IKPI)
24814 {
24815 j4array[1] += IK2PI;
24816 }
24817 j4valid[1] = true;
24818 for (int ij4 = 0; ij4 < 2; ++ij4)
24819 {
24820 if (!j4valid[ij4])
24821 {
24822 continue;
24823 }
24824 _ij4[0] = ij4;
24825 _ij4[1] = -1;
24826 for (int iij4 = ij4 + 1; iij4 < 2; ++iij4)
24827 {
24828 if (j4valid[iij4]
24829 && IKabs(cj4array[ij4] - cj4array[iij4])
24830 < IKFAST_SOLUTION_THRESH
24831 && IKabs(sj4array[ij4] - sj4array[iij4])
24832 < IKFAST_SOLUTION_THRESH)
24833 {
24834 j4valid[iij4] = false;
24835 _ij4[1] = iij4;
24836 break;
24837 }
24838 }
24839 j4 = j4array[ij4];
24840 cj4 = cj4array[ij4];
24841 sj4 = sj4array[ij4];
24842
24843 {
24844 IkReal j6eval[2];
24845 IkReal x1799 = (cj4 * px);
24846 IkReal x1800 = (cj8 * pz);
24847 IkReal x1801 = (py * sj4);
24848 IkReal x1802 = (cj4 * cj9 * px);
24849 IkReal x1803 = (cj4 * px * sj9);
24850 IkReal x1804 = (cj8 * pz * sj9);
24851 IkReal x1805 = (cj9 * py * sj4);
24852 IkReal x1806 = (py * sj4 * sj9);
24853 IkReal x1807 = ((0.045) * x1800);
24854 j6eval[0]
24855 = ((((-6.66666666666667) * x1802)) + (((-1.0) * x1806))
24856 + (((-12.2222222222222) * x1799))
24857 + (((-1.0) * x1803))
24858 + (((-6.66666666666667) * x1804))
24859 + (((-1.0) * x1800))
24860 + (((-6.66666666666667) * x1805))
24861 + (((-12.2222222222222) * x1801)) + ((cj9 * x1800)));
24862 j6eval[1] = IKsign(
24863 ((((-0.3) * x1804)) + (((-0.3) * x1802))
24864 + (((-0.045) * x1806)) + (((-0.55) * x1799))
24865 + ((cj9 * x1807)) + (((-0.3) * x1805))
24866 + (((-1.0) * x1807)) + (((-0.55) * x1801))
24867 + (((-0.045) * x1803))));
24868 if (IKabs(j6eval[0]) < 0.0000010000000000
24869 || IKabs(j6eval[1]) < 0.0000010000000000)
24870 {
24871 {
24872 IkReal j6eval[2];
24873 IkReal x1808 = (sj8 * (py * py));
24874 IkReal x1809 = cj4 * cj4;
24875 IkReal x1810
24876 = ((((-1.0) * x1808 * x1809)) + ((sj8 * (pz * pz)))
24877 + x1808 + ((sj8 * x1809 * (px * px)))
24878 + (((2.0) * cj4 * px * py * sj4 * sj8)));
24879 j6eval[0] = x1810;
24880 j6eval[1] = IKsign(x1810);
24881 if (IKabs(j6eval[0]) < 0.0000010000000000
24882 || IKabs(j6eval[1]) < 0.0000010000000000)
24883 {
24884 {
24885 IkReal j6eval[2];
24886 IkReal x1811 = (pz * sj8);
24887 IkReal x1812 = (cj9 * pz * sj8);
24888 IkReal x1813 = (pz * sj8 * sj9);
24889 IkReal x1814 = (cj8 * sj8);
24890 IkReal x1815 = (cj4 * px * x1814);
24891 IkReal x1816 = (py * sj4 * x1814);
24892 IkReal x1817 = ((1.0) * cj9);
24893 IkReal x1818 = (cj4 * cj8 * px * sj8 * sj9);
24894 IkReal x1819 = (cj8 * py * sj4 * sj8 * sj9);
24895 IkReal x1820 = ((0.045) * x1815);
24896 IkReal x1821 = ((0.045) * x1816);
24897 j6eval[0]
24898 = ((((-1.0) * x1816 * x1817))
24899 + (((6.66666666666667) * x1818))
24900 + (((-1.0) * x1813)) + x1815 + x1816
24901 + (((-1.0) * x1815 * x1817))
24902 + (((-6.66666666666667) * x1812))
24903 + (((-12.2222222222222) * x1811))
24904 + (((6.66666666666667) * x1819)));
24905 j6eval[1] = IKsign(
24906 ((((-1.0) * cj9 * x1821)) + x1821 + x1820
24907 + (((0.3) * x1819)) + (((-1.0) * cj9 * x1820))
24908 + (((-0.045) * x1813)) + (((0.3) * x1818))
24909 + (((-0.55) * x1811)) + (((-0.3) * x1812))));
24910 if (IKabs(j6eval[0]) < 0.0000010000000000
24911 || IKabs(j6eval[1]) < 0.0000010000000000)
24912 {
24913 {
24914 IkReal evalcond[4];
24915 bool bgotonextstatement = true;
24916 do
24917 {
24918 IkReal x1822
24919 = (((px * sj4))
24920 + (((-1.0) * (1.0) * cj4 * py)));
24921 evalcond[0]
24922 = ((-3.14159265358979)
24923 + (IKfmod(
24924 ((3.14159265358979)
24925 + (IKabs(j8))),
24926 6.28318530717959)));
24927 evalcond[1]
24928 = ((0.39655) + (((0.0765) * sj9))
24929 + (((0.32595) * cj9))
24930 + (((-1.0) * (1.0) * pp)));
24931 evalcond[2] = x1822;
24932 evalcond[3] = x1822;
24933 if (IKabs(evalcond[0]) < 0.0000010000000000
24934 && IKabs(evalcond[1]) < 0.0000010000000000
24935 && IKabs(evalcond[2]) < 0.0000010000000000
24936 && IKabs(evalcond[3])
24937 < 0.0000010000000000)
24938 {
24939 bgotonextstatement = false;
24940 {
24941 IkReal j6eval[2];
24942 sj8 = 0;
24943 cj8 = 1.0;
24944 j8 = 0;
24945 IkReal x1823 = (cj9 * pz);
24946 IkReal x1824 = (cj4 * px);
24947 IkReal x1825 = (pp * pz);
24948 IkReal x1826 = (py * sj4);
24949 IkReal x1827 = (cj4 * cj9 * px);
24950 IkReal x1828 = (cj4 * pp * px);
24951 IkReal x1829 = (cj9 * py * sj4);
24952 IkReal x1830 = (pp * py * sj4);
24953 j6eval[0]
24954 = ((((-36.2220411120167) * x1830))
24955 + (((13.9482024812098) * x1826))
24956 + (((5.4333061668025) * x1825))
24957 + x1823
24958 + (((13.9482024812098) * x1824))
24959 + (((-36.2220411120167) * x1828))
24960 + (((12.2222222222222) * x1827))
24961 + (((12.2222222222222) * x1829))
24962 + (((2.92556370551481) * pz)));
24963 j6eval[1] = IKsign(
24964 ((((-3.92156862745098) * x1830))
24965 + (((0.316735294117647) * pz))
24966 + (((-3.92156862745098) * x1828))
24967 + (((1.32323529411765) * x1827))
24968 + (((0.588235294117647) * x1825))
24969 + (((1.51009803921569) * x1824))
24970 + (((1.32323529411765) * x1829))
24971 + (((1.51009803921569) * x1826))
24972 + (((0.108264705882353) * x1823))));
24973 if (IKabs(j6eval[0]) < 0.0000010000000000
24974 || IKabs(j6eval[1])
24975 < 0.0000010000000000)
24976 {
24977 {
24978 IkReal j6eval[2];
24979 sj8 = 0;
24980 cj8 = 1.0;
24981 j8 = 0;
24982 IkReal x1831 = (cj4 * px);
24983 IkReal x1832 = (py * sj4);
24984 IkReal x1833 = (cj9 * pz);
24985 IkReal x1834 = (pz * sj9);
24986 IkReal x1835 = ((1.0) * cj9);
24987 IkReal x1836 = (cj4 * px * sj9);
24988 IkReal x1837 = (py * sj4 * sj9);
24989 IkReal x1838 = ((0.045) * x1831);
24990 IkReal x1839 = ((0.045) * x1832);
24991 j6eval[0]
24992 = ((((-1.0) * (12.2222222222222)
24993 * pz))
24994 + (((-1.0) * x1831 * x1835))
24995 + (((-1.0) * x1834))
24996 + (((-6.66666666666667)
24997 * x1833))
24998 + x1832 + x1831
24999 + (((6.66666666666667)
25000 * x1837))
25001 + (((-1.0) * x1832 * x1835))
25002 + (((6.66666666666667)
25003 * x1836)));
25004 j6eval[1] = IKsign(
25005 ((((-1.0) * (0.55) * pz))
25006 + (((0.3) * x1836))
25007 + (((-1.0) * cj9 * x1839))
25008 + (((-1.0) * cj9 * x1838))
25009 + (((0.3) * x1837))
25010 + (((-0.045) * x1834))
25011 + (((-0.3) * x1833)) + x1839
25012 + x1838));
25013 if (IKabs(j6eval[0])
25014 < 0.0000010000000000
25015 || IKabs(j6eval[1])
25016 < 0.0000010000000000)
25017 {
25018 {
25019 IkReal j6eval[2];
25020 sj8 = 0;
25021 cj8 = 1.0;
25022 j8 = 0;
25023 IkReal x1840 = (cj4 * px);
25024 IkReal x1841 = (cj9 * pz);
25025 IkReal x1842 = (pp * pz);
25026 IkReal x1843 = (py * sj4);
25027 IkReal x1844 = (cj4 * cj9 * px);
25028 IkReal x1845 = (cj4 * pp * px);
25029 IkReal x1846 = (cj9 * py * sj4);
25030 IkReal x1847 = (pp * py * sj4);
25031 j6eval[0]
25032 = ((((-5.4333061668025)
25033 * x1847))
25034 + (((-5.4333061668025)
25035 * x1845))
25036 + (((-36.2220411120167)
25037 * x1842))
25038 + (((-2.92556370551481)
25039 * x1843))
25040 + (((-1.0) * x1846))
25041 + (((-1.0) * x1844))
25042 + (((13.9482024812098)
25043 * pz))
25044 + (((-2.92556370551481)
25045 * x1840))
25046 + (((12.2222222222222)
25047 * x1841)));
25048 j6eval[1] = IKsign(
25049 ((((-0.108264705882353)
25050 * x1844))
25051 + (((1.51009803921569) * pz))
25052 + (((1.32323529411765)
25053 * x1841))
25054 + (((-0.588235294117647)
25055 * x1845))
25056 + (((-0.108264705882353)
25057 * x1846))
25058 + (((-0.316735294117647)
25059 * x1843))
25060 + (((-0.588235294117647)
25061 * x1847))
25062 + (((-0.316735294117647)
25063 * x1840))
25064 + (((-3.92156862745098)
25065 * x1842))));
25066 if (IKabs(j6eval[0])
25067 < 0.0000010000000000
25068 || IKabs(j6eval[1])
25069 < 0.0000010000000000)
25070 {
25071 {
25072 IkReal evalcond[1];
25073 bool bgotonextstatement
25074 = true;
25075 do
25076 {
25077 evalcond[0]
25078 = ((IKabs((
25079 (-3.14159265358979)
25080 + (IKfmod(
25081 ((3.14159265358979)
25082 + j9),
25083 6.28318530717959)))))
25084 + (IKabs(pz)));
25085 if (IKabs(evalcond[0])
25086 < 0.0000010000000000)
25087 {
25088 bgotonextstatement
25089 = false;
25090 {
25091 IkReal j6eval[1];
25092 IkReal x1848
25093 = ((1.0) * py);
25094 sj8 = 0;
25095 cj8 = 1.0;
25096 j8 = 0;
25097 pz = 0;
25098 j9 = 0;
25099 sj9 = 0;
25100 cj9 = 1.0;
25101 pp
25102 = ((py * py)
25103 + (px * px));
25104 npx
25105 = (((py * r10))
25106 + ((px * r00)));
25107 npy
25108 = (((py * r11))
25109 + ((px * r01)));
25110 npz
25111 = (((px * r02))
25112 + ((py * r12)));
25113 rxp0_0
25114 = ((-1.0) * r20
25115 * x1848);
25116 rxp0_1 = (px * r20);
25117 rxp1_0
25118 = ((-1.0) * r21
25119 * x1848);
25120 rxp1_1 = (px * r21);
25121 rxp2_0
25122 = ((-1.0) * r22
25123 * x1848);
25124 rxp2_1 = (px * r22);
25125 j6eval[0]
25126 = (((py * sj4))
25127 + ((cj4 * px)));
25128 if (IKabs(j6eval[0])
25129 < 0.0000010000000000)
25130 {
25131 {
25132 IkReal j6eval[1];
25133 IkReal x1849
25134 = ((1.0) * py);
25135 sj8 = 0;
25136 cj8 = 1.0;
25137 j8 = 0;
25138 pz = 0;
25139 j9 = 0;
25140 sj9 = 0;
25141 cj9 = 1.0;
25142 pp
25143 = ((py * py)
25144 + (px * px));
25145 npx
25146 = (((py * r10))
25147 + ((px
25148 * r00)));
25149 npy
25150 = (((py * r11))
25151 + ((px
25152 * r01)));
25153 npz
25154 = (((px * r02))
25155 + ((py
25156 * r12)));
25157 rxp0_0
25158 = ((-1.0) * r20
25159 * x1849);
25160 rxp0_1 = (px * r20);
25161 rxp1_0
25162 = ((-1.0) * r21
25163 * x1849);
25164 rxp1_1 = (px * r21);
25165 rxp2_0
25166 = ((-1.0) * r22
25167 * x1849);
25168 rxp2_1 = (px * r22);
25169 j6eval[0]
25170 = ((-1.0)
25171 + (((-1.0)
25172 * (1.3840830449827)
25173 * (px
25174 * px)))
25175 + (((-1.0)
25176 * (1.3840830449827)
25177 * (py
25178 * py))));
25179 if (IKabs(j6eval[0])
25180 < 0.0000010000000000)
25181 {
25182 {
25183 IkReal
25184 j6eval[2];
25185 IkReal x1850
25186 = ((1.0)
25187 * py);
25188 sj8 = 0;
25189 cj8 = 1.0;
25190 j8 = 0;
25191 pz = 0;
25192 j9 = 0;
25193 sj9 = 0;
25194 cj9 = 1.0;
25195 pp
25196 = ((py * py)
25197 + (px
25198 * px));
25199 npx
25200 = (((py
25201 * r10))
25202 + ((px
25203 * r00)));
25204 npy
25205 = (((py
25206 * r11))
25207 + ((px
25208 * r01)));
25209 npz
25210 = (((px
25211 * r02))
25212 + ((py
25213 * r12)));
25214 rxp0_0
25215 = ((-1.0)
25216 * r20
25217 * x1850);
25218 rxp0_1
25219 = (px
25220 * r20);
25221 rxp1_0
25222 = ((-1.0)
25223 * r21
25224 * x1850);
25225 rxp1_1
25226 = (px
25227 * r21);
25228 rxp2_0
25229 = ((-1.0)
25230 * r22
25231 * x1850);
25232 rxp2_1
25233 = (px
25234 * r22);
25235 IkReal x1851
25236 = (cj4
25237 * px);
25238 IkReal x1852
25239 = (py
25240 * sj4);
25241 j6eval[0]
25242 = (x1852
25243 + x1851);
25244 j6eval[1]
25245 = ((((-1.0)
25246 * (1.3840830449827)
25247 * cj4
25248 * (px
25249 * px
25250 * px)))
25251 + (((-1.0)
25252 * x1852))
25253 + (((-1.0)
25254 * x1851))
25255 + (((-1.3840830449827)
25256 * x1852
25257 * (px
25258 * px)))
25259 + (((-1.0)
25260 * (1.3840830449827)
25261 * sj4
25262 * (py
25263 * py
25264 * py)))
25265 + (((-1.3840830449827)
25266 * x1851
25267 * (py
25268 * py))));
25269 if (IKabs(
25270 j6eval
25271 [0])
25272 < 0.0000010000000000
25273 || IKabs(
25274 j6eval
25275 [1])
25276 < 0.0000010000000000)
25277 {
25278 {
25279 IkReal
25280 evalcond
25281 [4];
25282 bool
25283 bgotonextstatement
25284 = true;
25285 do
25286 {
25287 evalcond
25288 [0]
25289 = ((IKabs(
25290 py))
25291 + (IKabs(
25292 px)));
25293 evalcond
25294 [1]
25295 = -0.85;
25296 evalcond
25297 [2]
25298 = 0;
25299 evalcond
25300 [3]
25301 = -0.2125;
25302 if (IKabs(
25303 evalcond
25304 [0])
25305 < 0.0000010000000000
25306 && IKabs(
25307 evalcond
25308 [1])
25309 < 0.0000010000000000
25310 && IKabs(
25311 evalcond
25312 [2])
25313 < 0.0000010000000000
25314 && IKabs(
25315 evalcond
25316 [3])
25317 < 0.0000010000000000)
25318 {
25319 bgotonextstatement
25320 = false;
25321 {
25322 IkReal j6array
25323 [2],
25324 cj6array
25325 [2],
25326 sj6array
25327 [2];
25328 bool j6valid
25329 [2]
25330 = {false};
25331 _nj6
25332 = 2;
25333 j6array
25334 [0]
25335 = 2.9927027059803;
25336 sj6array
25337 [0]
25338 = IKsin(
25339 j6array
25340 [0]);
25341 cj6array
25342 [0]
25343 = IKcos(
25344 j6array
25345 [0]);
25346 j6array
25347 [1]
25348 = 6.13429535957009;
25349 sj6array
25350 [1]
25351 = IKsin(
25352 j6array
25353 [1]);
25354 cj6array
25355 [1]
25356 = IKcos(
25357 j6array
25358 [1]);
25359 if (j6array
25360 [0]
25361 > IKPI)
25362 {
25363 j6array
25364 [0]
25365 -= IK2PI;
25366 }
25367 else if (
25368 j6array
25369 [0]
25370 < -IKPI)
25371 {
25372 j6array
25373 [0]
25374 += IK2PI;
25375 }
25376 j6valid
25377 [0]
25378 = true;
25379 if (j6array
25380 [1]
25381 > IKPI)
25382 {
25383 j6array
25384 [1]
25385 -= IK2PI;
25386 }
25387 else if (
25388 j6array
25389 [1]
25390 < -IKPI)
25391 {
25392 j6array
25393 [1]
25394 += IK2PI;
25395 }
25396 j6valid
25397 [1]
25398 = true;
25399 for (
25400 int ij6
25401 = 0;
25402 ij6
25403 < 2;
25404 ++ij6)
25405 {
25406 if (!j6valid
25407 [ij6])
25408 {
25409 continue;
25410 }
25411 _ij6[0]
25412 = ij6;
25413 _ij6[1]
25414 = -1;
25415 for (
25416 int iij6
25417 = ij6
25418 + 1;
25419 iij6
25420 < 2;
25421 ++iij6)
25422 {
25423 if (j6valid
25424 [iij6]
25425 && IKabs(
25426 cj6array
25427 [ij6]
25428 - cj6array
25429 [iij6])
25430 < IKFAST_SOLUTION_THRESH
25431 && IKabs(
25432 sj6array
25433 [ij6]
25434 - sj6array
25435 [iij6])
25436 < IKFAST_SOLUTION_THRESH)
25437 {
25438 j6valid
25439 [iij6]
25440 = false;
25441 _ij6[1]
25442 = iij6;
25443 break;
25444 }
25445 }
25446 j6 = j6array
25447 [ij6];
25448 cj6 = cj6array
25449 [ij6];
25450 sj6 = sj6array
25451 [ij6];
25452 {
25453 IkReal evalcond
25454 [1];
25455 evalcond
25456 [0]
25457 = ((0.85)
25458 * (IKsin(
25459 j6)));
25460 if (IKabs(
25461 evalcond
25462 [0])
25463 > IKFAST_EVALCOND_THRESH)
25464 {
25465 continue;
25466 }
25467 }
25468
25469 rotationfunction0(
25470 solutions);
25471 }
25472 }
25473 }
25474 } while (0);
25475 if (bgotonextstatement)
25476 {
25477 bool
25478 bgotonextstatement
25479 = true;
25480 do
25481 {
25482 evalcond
25483 [0]
25484 = ((IKabs((
25485 (-3.14159265358979)
25486 + (IKfmod(
25487 ((3.14159265358979)
25488 + j4),
25489 6.28318530717959)))))
25490 + (IKabs(
25491 px)));
25492 evalcond
25493 [1]
25494 = -0.85;
25495 evalcond
25496 [2]
25497 = 0;
25498 evalcond
25499 [3]
25500 = ((-0.2125)
25501 + (((-1.0)
25502 * (1.0)
25503 * (py
25504 * py))));
25505 if (IKabs(
25506 evalcond
25507 [0])
25508 < 0.0000010000000000
25509 && IKabs(
25510 evalcond
25511 [1])
25512 < 0.0000010000000000
25513 && IKabs(
25514 evalcond
25515 [2])
25516 < 0.0000010000000000
25517 && IKabs(
25518 evalcond
25519 [3])
25520 < 0.0000010000000000)
25521 {
25522 bgotonextstatement
25523 = false;
25524 {
25525 IkReal j6eval
25526 [1];
25527 IkReal
25528 x1853
25529 = ((1.0)
25530 * py);
25531 sj8 = 0;
25532 cj8 = 1.0;
25533 j8 = 0;
25534 pz = 0;
25535 j9 = 0;
25536 sj9 = 0;
25537 cj9 = 1.0;
25538 pp = py
25539 * py;
25540 npx
25541 = (py
25542 * r10);
25543 npy
25544 = (py
25545 * r11);
25546 npz
25547 = (py
25548 * r12);
25549 rxp0_0
25550 = ((-1.0)
25551 * r20
25552 * x1853);
25553 rxp0_1
25554 = 0;
25555 rxp1_0
25556 = ((-1.0)
25557 * r21
25558 * x1853);
25559 rxp1_1
25560 = 0;
25561 rxp2_0
25562 = ((-1.0)
25563 * r22
25564 * x1853);
25565 rxp2_1
25566 = 0;
25567 px = 0;
25568 j4 = 0;
25569 sj4 = 0;
25570 cj4 = 1.0;
25571 rxp0_2
25572 = (py
25573 * r00);
25574 rxp1_2
25575 = (py
25576 * r01);
25577 rxp2_2
25578 = (py
25579 * r02);
25580 j6eval
25581 [0]
25582 = ((1.0)
25583 + (((1.91568587540858)
25584 * (py
25585 * py
25586 * py
25587 * py)))
25588 + (((-1.0)
25589 * (2.64633970947792)
25590 * (py
25591 * py))));
25592 if (IKabs(
25593 j6eval
25594 [0])
25595 < 0.0000010000000000)
25596 {
25597 continue; // no branches [j6]
25598 }
25599 else
25600 {
25601 {
25602 IkReal j6array
25603 [2],
25604 cj6array
25605 [2],
25606 sj6array
25607 [2];
25608 bool j6valid
25609 [2]
25610 = {false};
25611 _nj6
25612 = 2;
25613 IkReal
25614 x1854
25615 = py
25616 * py;
25617 CheckValue<IkReal> x1856 = IKatan2WithCheck(
25618 IkReal((
25619 (-0.425)
25620 + (((-0.588235294117647)
25621 * x1854)))),
25622 ((-2.83333333333333)
25623 + (((3.92156862745098)
25624 * x1854))),
25625 IKFAST_ATAN2_MAGTHRESH);
25626 if (!x1856
25627 .valid)
25628 {
25629 continue;
25630 }
25631 IkReal
25632 x1855
25633 = ((-1.0)
25634 * (x1856
25635 .value));
25636 j6array
25637 [0]
25638 = x1855;
25639 sj6array
25640 [0]
25641 = IKsin(
25642 j6array
25643 [0]);
25644 cj6array
25645 [0]
25646 = IKcos(
25647 j6array
25648 [0]);
25649 j6array
25650 [1]
25651 = ((3.14159265358979)
25652 + x1855);
25653 sj6array
25654 [1]
25655 = IKsin(
25656 j6array
25657 [1]);
25658 cj6array
25659 [1]
25660 = IKcos(
25661 j6array
25662 [1]);
25663 if (j6array
25664 [0]
25665 > IKPI)
25666 {
25667 j6array
25668 [0]
25669 -= IK2PI;
25670 }
25671 else if (
25672 j6array
25673 [0]
25674 < -IKPI)
25675 {
25676 j6array
25677 [0]
25678 += IK2PI;
25679 }
25680 j6valid
25681 [0]
25682 = true;
25683 if (j6array
25684 [1]
25685 > IKPI)
25686 {
25687 j6array
25688 [1]
25689 -= IK2PI;
25690 }
25691 else if (
25692 j6array
25693 [1]
25694 < -IKPI)
25695 {
25696 j6array
25697 [1]
25698 += IK2PI;
25699 }
25700 j6valid
25701 [1]
25702 = true;
25703 for (
25704 int ij6
25705 = 0;
25706 ij6
25707 < 2;
25708 ++ij6)
25709 {
25710 if (!j6valid
25711 [ij6])
25712 {
25713 continue;
25714 }
25715 _ij6[0]
25716 = ij6;
25717 _ij6[1]
25718 = -1;
25719 for (
25720 int iij6
25721 = ij6
25722 + 1;
25723 iij6
25724 < 2;
25725 ++iij6)
25726 {
25727 if (j6valid
25728 [iij6]
25729 && IKabs(
25730 cj6array
25731 [ij6]
25732 - cj6array
25733 [iij6])
25734 < IKFAST_SOLUTION_THRESH
25735 && IKabs(
25736 sj6array
25737 [ij6]
25738 - sj6array
25739 [iij6])
25740 < IKFAST_SOLUTION_THRESH)
25741 {
25742 j6valid
25743 [iij6]
25744 = false;
25745 _ij6[1]
25746 = iij6;
25747 break;
25748 }
25749 }
25750 j6 = j6array
25751 [ij6];
25752 cj6 = cj6array
25753 [ij6];
25754 sj6 = sj6array
25755 [ij6];
25756 {
25757 IkReal evalcond
25758 [1];
25759 evalcond
25760 [0]
25761 = ((0.85)
25762 * (IKsin(
25763 j6)));
25764 if (IKabs(
25765 evalcond
25766 [0])
25767 > IKFAST_EVALCOND_THRESH)
25768 {
25769 continue;
25770 }
25771 }
25772
25773 rotationfunction0(
25774 solutions);
25775 }
25776 }
25777 }
25778 }
25779 }
25780 } while (
25781 0);
25782 if (bgotonextstatement)
25783 {
25784 bool
25785 bgotonextstatement
25786 = true;
25787 do
25788 {
25789 evalcond
25790 [0]
25791 = ((IKabs((
25792 (-3.14159265358979)
25793 + (IKfmod(
25794 j4,
25795 6.28318530717959)))))
25796 + (IKabs(
25797 px)));
25798 evalcond
25799 [1]
25800 = -0.85;
25801 evalcond
25802 [2]
25803 = 0;
25804 evalcond
25805 [3]
25806 = ((-0.2125)
25807 + (((-1.0)
25808 * (1.0)
25809 * (py
25810 * py))));
25811 if (IKabs(
25812 evalcond
25813 [0])
25814 < 0.0000010000000000
25815 && IKabs(
25816 evalcond
25817 [1])
25818 < 0.0000010000000000
25819 && IKabs(
25820 evalcond
25821 [2])
25822 < 0.0000010000000000
25823 && IKabs(
25824 evalcond
25825 [3])
25826 < 0.0000010000000000)
25827 {
25828 bgotonextstatement
25829 = false;
25830 {
25831 IkReal j6eval
25832 [1];
25833 IkReal
25834 x1857
25835 = ((1.0)
25836 * py);
25837 sj8 = 0;
25838 cj8 = 1.0;
25839 j8 = 0;
25840 pz = 0;
25841 j9 = 0;
25842 sj9 = 0;
25843 cj9 = 1.0;
25844 pp = py
25845 * py;
25846 npx
25847 = (py
25848 * r10);
25849 npy
25850 = (py
25851 * r11);
25852 npz
25853 = (py
25854 * r12);
25855 rxp0_0
25856 = ((-1.0)
25857 * r20
25858 * x1857);
25859 rxp0_1
25860 = 0;
25861 rxp1_0
25862 = ((-1.0)
25863 * r21
25864 * x1857);
25865 rxp1_1
25866 = 0;
25867 rxp2_0
25868 = ((-1.0)
25869 * r22
25870 * x1857);
25871 rxp2_1
25872 = 0;
25873 px = 0;
25874 j4 = 3.14159265358979;
25875 sj4 = 0;
25876 cj4 = -1.0;
25877 rxp0_2
25878 = (py
25879 * r00);
25880 rxp1_2
25881 = (py
25882 * r01);
25883 rxp2_2
25884 = (py
25885 * r02);
25886 j6eval
25887 [0]
25888 = ((1.0)
25889 + (((1.91568587540858)
25890 * (py
25891 * py
25892 * py
25893 * py)))
25894 + (((-1.0)
25895 * (2.64633970947792)
25896 * (py
25897 * py))));
25898 if (IKabs(
25899 j6eval
25900 [0])
25901 < 0.0000010000000000)
25902 {
25903 continue; // no branches [j6]
25904 }
25905 else
25906 {
25907 {
25908 IkReal j6array
25909 [2],
25910 cj6array
25911 [2],
25912 sj6array
25913 [2];
25914 bool j6valid
25915 [2]
25916 = {false};
25917 _nj6
25918 = 2;
25919 IkReal
25920 x1858
25921 = py
25922 * py;
25923 CheckValue<IkReal> x1860 = IKatan2WithCheck(
25924 IkReal((
25925 (-0.425)
25926 + (((-0.588235294117647)
25927 * x1858)))),
25928 ((-2.83333333333333)
25929 + (((3.92156862745098)
25930 * x1858))),
25931 IKFAST_ATAN2_MAGTHRESH);
25932 if (!x1860
25933 .valid)
25934 {
25935 continue;
25936 }
25937 IkReal
25938 x1859
25939 = ((-1.0)
25940 * (x1860
25941 .value));
25942 j6array
25943 [0]
25944 = x1859;
25945 sj6array
25946 [0]
25947 = IKsin(
25948 j6array
25949 [0]);
25950 cj6array
25951 [0]
25952 = IKcos(
25953 j6array
25954 [0]);
25955 j6array
25956 [1]
25957 = ((3.14159265358979)
25958 + x1859);
25959 sj6array
25960 [1]
25961 = IKsin(
25962 j6array
25963 [1]);
25964 cj6array
25965 [1]
25966 = IKcos(
25967 j6array
25968 [1]);
25969 if (j6array
25970 [0]
25971 > IKPI)
25972 {
25973 j6array
25974 [0]
25975 -= IK2PI;
25976 }
25977 else if (
25978 j6array
25979 [0]
25980 < -IKPI)
25981 {
25982 j6array
25983 [0]
25984 += IK2PI;
25985 }
25986 j6valid
25987 [0]
25988 = true;
25989 if (j6array
25990 [1]
25991 > IKPI)
25992 {
25993 j6array
25994 [1]
25995 -= IK2PI;
25996 }
25997 else if (
25998 j6array
25999 [1]
26000 < -IKPI)
26001 {
26002 j6array
26003 [1]
26004 += IK2PI;
26005 }
26006 j6valid
26007 [1]
26008 = true;
26009 for (
26010 int ij6
26011 = 0;
26012 ij6
26013 < 2;
26014 ++ij6)
26015 {
26016 if (!j6valid
26017 [ij6])
26018 {
26019 continue;
26020 }
26021 _ij6[0]
26022 = ij6;
26023 _ij6[1]
26024 = -1;
26025 for (
26026 int iij6
26027 = ij6
26028 + 1;
26029 iij6
26030 < 2;
26031 ++iij6)
26032 {
26033 if (j6valid
26034 [iij6]
26035 && IKabs(
26036 cj6array
26037 [ij6]
26038 - cj6array
26039 [iij6])
26040 < IKFAST_SOLUTION_THRESH
26041 && IKabs(
26042 sj6array
26043 [ij6]
26044 - sj6array
26045 [iij6])
26046 < IKFAST_SOLUTION_THRESH)
26047 {
26048 j6valid
26049 [iij6]
26050 = false;
26051 _ij6[1]
26052 = iij6;
26053 break;
26054 }
26055 }
26056 j6 = j6array
26057 [ij6];
26058 cj6 = cj6array
26059 [ij6];
26060 sj6 = sj6array
26061 [ij6];
26062 {
26063 IkReal evalcond
26064 [1];
26065 evalcond
26066 [0]
26067 = ((0.85)
26068 * (IKsin(
26069 j6)));
26070 if (IKabs(
26071 evalcond
26072 [0])
26073 > IKFAST_EVALCOND_THRESH)
26074 {
26075 continue;
26076 }
26077 }
26078
26079 rotationfunction0(
26080 solutions);
26081 }
26082 }
26083 }
26084 }
26085 }
26086 } while (
26087 0);
26088 if (bgotonextstatement)
26089 {
26090 bool
26091 bgotonextstatement
26092 = true;
26093 do
26094 {
26095 evalcond
26096 [0]
26097 = ((IKabs((
26098 (-3.14159265358979)
26099 + (IKfmod(
26100 ((1.5707963267949)
26101 + j4),
26102 6.28318530717959)))))
26103 + (IKabs(
26104 py)));
26105 evalcond
26106 [1]
26107 = -0.85;
26108 evalcond
26109 [2]
26110 = 0;
26111 evalcond
26112 [3]
26113 = ((-0.2125)
26114 + (((-1.0)
26115 * (1.0)
26116 * (px
26117 * px))));
26118 if (IKabs(
26119 evalcond
26120 [0])
26121 < 0.0000010000000000
26122 && IKabs(
26123 evalcond
26124 [1])
26125 < 0.0000010000000000
26126 && IKabs(
26127 evalcond
26128 [2])
26129 < 0.0000010000000000
26130 && IKabs(
26131 evalcond
26132 [3])
26133 < 0.0000010000000000)
26134 {
26135 bgotonextstatement
26136 = false;
26137 {
26138 IkReal j6eval
26139 [1];
26140 IkReal
26141 x1861
26142 = ((1.0)
26143 * px);
26144 sj8 = 0;
26145 cj8 = 1.0;
26146 j8 = 0;
26147 pz = 0;
26148 j9 = 0;
26149 sj9 = 0;
26150 cj9 = 1.0;
26151 pp = px
26152 * px;
26153 npx
26154 = (px
26155 * r00);
26156 npy
26157 = (px
26158 * r01);
26159 npz
26160 = (px
26161 * r02);
26162 rxp0_0
26163 = 0;
26164 rxp0_1
26165 = (px
26166 * r20);
26167 rxp1_0
26168 = 0;
26169 rxp1_1
26170 = (px
26171 * r21);
26172 rxp2_0
26173 = 0;
26174 rxp2_1
26175 = (px
26176 * r22);
26177 py = 0;
26178 j4 = 1.5707963267949;
26179 sj4 = 1.0;
26180 cj4 = 0;
26181 rxp0_2
26182 = ((-1.0)
26183 * r10
26184 * x1861);
26185 rxp1_2
26186 = ((-1.0)
26187 * r11
26188 * x1861);
26189 rxp2_2
26190 = ((-1.0)
26191 * r12
26192 * x1861);
26193 j6eval
26194 [0]
26195 = ((1.0)
26196 + (((1.91568587540858)
26197 * (px
26198 * px
26199 * px
26200 * px)))
26201 + (((-1.0)
26202 * (2.64633970947792)
26203 * (px
26204 * px))));
26205 if (IKabs(
26206 j6eval
26207 [0])
26208 < 0.0000010000000000)
26209 {
26210 continue; // no branches [j6]
26211 }
26212 else
26213 {
26214 {
26215 IkReal j6array
26216 [2],
26217 cj6array
26218 [2],
26219 sj6array
26220 [2];
26221 bool j6valid
26222 [2]
26223 = {false};
26224 _nj6
26225 = 2;
26226 IkReal
26227 x1862
26228 = px
26229 * px;
26230 CheckValue<IkReal> x1864 = IKatan2WithCheck(
26231 IkReal((
26232 (-0.425)
26233 + (((-0.588235294117647)
26234 * x1862)))),
26235 ((-2.83333333333333)
26236 + (((3.92156862745098)
26237 * x1862))),
26238 IKFAST_ATAN2_MAGTHRESH);
26239 if (!x1864
26240 .valid)
26241 {
26242 continue;
26243 }
26244 IkReal
26245 x1863
26246 = ((-1.0)
26247 * (x1864
26248 .value));
26249 j6array
26250 [0]
26251 = x1863;
26252 sj6array
26253 [0]
26254 = IKsin(
26255 j6array
26256 [0]);
26257 cj6array
26258 [0]
26259 = IKcos(
26260 j6array
26261 [0]);
26262 j6array
26263 [1]
26264 = ((3.14159265358979)
26265 + x1863);
26266 sj6array
26267 [1]
26268 = IKsin(
26269 j6array
26270 [1]);
26271 cj6array
26272 [1]
26273 = IKcos(
26274 j6array
26275 [1]);
26276 if (j6array
26277 [0]
26278 > IKPI)
26279 {
26280 j6array
26281 [0]
26282 -= IK2PI;
26283 }
26284 else if (
26285 j6array
26286 [0]
26287 < -IKPI)
26288 {
26289 j6array
26290 [0]
26291 += IK2PI;
26292 }
26293 j6valid
26294 [0]
26295 = true;
26296 if (j6array
26297 [1]
26298 > IKPI)
26299 {
26300 j6array
26301 [1]
26302 -= IK2PI;
26303 }
26304 else if (
26305 j6array
26306 [1]
26307 < -IKPI)
26308 {
26309 j6array
26310 [1]
26311 += IK2PI;
26312 }
26313 j6valid
26314 [1]
26315 = true;
26316 for (
26317 int ij6
26318 = 0;
26319 ij6
26320 < 2;
26321 ++ij6)
26322 {
26323 if (!j6valid
26324 [ij6])
26325 {
26326 continue;
26327 }
26328 _ij6[0]
26329 = ij6;
26330 _ij6[1]
26331 = -1;
26332 for (
26333 int iij6
26334 = ij6
26335 + 1;
26336 iij6
26337 < 2;
26338 ++iij6)
26339 {
26340 if (j6valid
26341 [iij6]
26342 && IKabs(
26343 cj6array
26344 [ij6]
26345 - cj6array
26346 [iij6])
26347 < IKFAST_SOLUTION_THRESH
26348 && IKabs(
26349 sj6array
26350 [ij6]
26351 - sj6array
26352 [iij6])
26353 < IKFAST_SOLUTION_THRESH)
26354 {
26355 j6valid
26356 [iij6]
26357 = false;
26358 _ij6[1]
26359 = iij6;
26360 break;
26361 }
26362 }
26363 j6 = j6array
26364 [ij6];
26365 cj6 = cj6array
26366 [ij6];
26367 sj6 = sj6array
26368 [ij6];
26369 {
26370 IkReal evalcond
26371 [1];
26372 evalcond
26373 [0]
26374 = ((0.85)
26375 * (IKsin(
26376 j6)));
26377 if (IKabs(
26378 evalcond
26379 [0])
26380 > IKFAST_EVALCOND_THRESH)
26381 {
26382 continue;
26383 }
26384 }
26385
26386 rotationfunction0(
26387 solutions);
26388 }
26389 }
26390 }
26391 }
26392 }
26393 } while (
26394 0);
26395 if (bgotonextstatement)
26396 {
26397 bool
26398 bgotonextstatement
26399 = true;
26400 do
26401 {
26402 evalcond
26403 [0]
26404 = ((IKabs(
26405 py))
26406 + (IKabs((
26407 (-3.14159265358979)
26408 + (IKfmod(
26409 ((4.71238898038469)
26410 + j4),
26411 6.28318530717959))))));
26412 evalcond
26413 [1]
26414 = -0.85;
26415 evalcond
26416 [2]
26417 = 0;
26418 evalcond
26419 [3]
26420 = ((-0.2125)
26421 + (((-1.0)
26422 * (1.0)
26423 * (px
26424 * px))));
26425 if (IKabs(
26426 evalcond
26427 [0])
26428 < 0.0000010000000000
26429 && IKabs(
26430 evalcond
26431 [1])
26432 < 0.0000010000000000
26433 && IKabs(
26434 evalcond
26435 [2])
26436 < 0.0000010000000000
26437 && IKabs(
26438 evalcond
26439 [3])
26440 < 0.0000010000000000)
26441 {
26442 bgotonextstatement
26443 = false;
26444 {
26445 IkReal j6eval
26446 [1];
26447 IkReal
26448 x1865
26449 = ((1.0)
26450 * px);
26451 sj8 = 0;
26452 cj8 = 1.0;
26453 j8 = 0;
26454 pz = 0;
26455 j9 = 0;
26456 sj9 = 0;
26457 cj9 = 1.0;
26458 pp = px
26459 * px;
26460 npx
26461 = (px
26462 * r00);
26463 npy
26464 = (px
26465 * r01);
26466 npz
26467 = (px
26468 * r02);
26469 rxp0_0
26470 = 0;
26471 rxp0_1
26472 = (px
26473 * r20);
26474 rxp1_0
26475 = 0;
26476 rxp1_1
26477 = (px
26478 * r21);
26479 rxp2_0
26480 = 0;
26481 rxp2_1
26482 = (px
26483 * r22);
26484 py = 0;
26485 j4 = -1.5707963267949;
26486 sj4 = -1.0;
26487 cj4 = 0;
26488 rxp0_2
26489 = ((-1.0)
26490 * r10
26491 * x1865);
26492 rxp1_2
26493 = ((-1.0)
26494 * r11
26495 * x1865);
26496 rxp2_2
26497 = ((-1.0)
26498 * r12
26499 * x1865);
26500 j6eval
26501 [0]
26502 = ((1.0)
26503 + (((1.91568587540858)
26504 * (px
26505 * px
26506 * px
26507 * px)))
26508 + (((-1.0)
26509 * (2.64633970947792)
26510 * (px
26511 * px))));
26512 if (IKabs(
26513 j6eval
26514 [0])
26515 < 0.0000010000000000)
26516 {
26517 continue; // no branches [j6]
26518 }
26519 else
26520 {
26521 {
26522 IkReal j6array
26523 [2],
26524 cj6array
26525 [2],
26526 sj6array
26527 [2];
26528 bool j6valid
26529 [2]
26530 = {false};
26531 _nj6
26532 = 2;
26533 IkReal
26534 x1866
26535 = px
26536 * px;
26537 CheckValue<IkReal> x1868 = IKatan2WithCheck(
26538 IkReal((
26539 (-0.425)
26540 + (((-0.588235294117647)
26541 * x1866)))),
26542 ((-2.83333333333333)
26543 + (((3.92156862745098)
26544 * x1866))),
26545 IKFAST_ATAN2_MAGTHRESH);
26546 if (!x1868
26547 .valid)
26548 {
26549 continue;
26550 }
26551 IkReal
26552 x1867
26553 = ((-1.0)
26554 * (x1868
26555 .value));
26556 j6array
26557 [0]
26558 = x1867;
26559 sj6array
26560 [0]
26561 = IKsin(
26562 j6array
26563 [0]);
26564 cj6array
26565 [0]
26566 = IKcos(
26567 j6array
26568 [0]);
26569 j6array
26570 [1]
26571 = ((3.14159265358979)
26572 + x1867);
26573 sj6array
26574 [1]
26575 = IKsin(
26576 j6array
26577 [1]);
26578 cj6array
26579 [1]
26580 = IKcos(
26581 j6array
26582 [1]);
26583 if (j6array
26584 [0]
26585 > IKPI)
26586 {
26587 j6array
26588 [0]
26589 -= IK2PI;
26590 }
26591 else if (
26592 j6array
26593 [0]
26594 < -IKPI)
26595 {
26596 j6array
26597 [0]
26598 += IK2PI;
26599 }
26600 j6valid
26601 [0]
26602 = true;
26603 if (j6array
26604 [1]
26605 > IKPI)
26606 {
26607 j6array
26608 [1]
26609 -= IK2PI;
26610 }
26611 else if (
26612 j6array
26613 [1]
26614 < -IKPI)
26615 {
26616 j6array
26617 [1]
26618 += IK2PI;
26619 }
26620 j6valid
26621 [1]
26622 = true;
26623 for (
26624 int ij6
26625 = 0;
26626 ij6
26627 < 2;
26628 ++ij6)
26629 {
26630 if (!j6valid
26631 [ij6])
26632 {
26633 continue;
26634 }
26635 _ij6[0]
26636 = ij6;
26637 _ij6[1]
26638 = -1;
26639 for (
26640 int iij6
26641 = ij6
26642 + 1;
26643 iij6
26644 < 2;
26645 ++iij6)
26646 {
26647 if (j6valid
26648 [iij6]
26649 && IKabs(
26650 cj6array
26651 [ij6]
26652 - cj6array
26653 [iij6])
26654 < IKFAST_SOLUTION_THRESH
26655 && IKabs(
26656 sj6array
26657 [ij6]
26658 - sj6array
26659 [iij6])
26660 < IKFAST_SOLUTION_THRESH)
26661 {
26662 j6valid
26663 [iij6]
26664 = false;
26665 _ij6[1]
26666 = iij6;
26667 break;
26668 }
26669 }
26670 j6 = j6array
26671 [ij6];
26672 cj6 = cj6array
26673 [ij6];
26674 sj6 = sj6array
26675 [ij6];
26676 {
26677 IkReal evalcond
26678 [1];
26679 evalcond
26680 [0]
26681 = ((0.85)
26682 * (IKsin(
26683 j6)));
26684 if (IKabs(
26685 evalcond
26686 [0])
26687 > IKFAST_EVALCOND_THRESH)
26688 {
26689 continue;
26690 }
26691 }
26692
26693 rotationfunction0(
26694 solutions);
26695 }
26696 }
26697 }
26698 }
26699 }
26700 } while (
26701 0);
26702 if (bgotonextstatement)
26703 {
26704 bool
26705 bgotonextstatement
26706 = true;
26707 do
26708 {
26709 if (1)
26710 {
26711 bgotonextstatement
26712 = false;
26713 continue; // branch miss [j6]
26714 }
26715 } while (
26716 0);
26717 if (bgotonextstatement)
26718 {
26719 }
26720 }
26721 }
26722 }
26723 }
26724 }
26725 }
26726 }
26727 else
26728 {
26729 {
26730 IkReal
26731 j6array
26732 [1],
26733 cj6array
26734 [1],
26735 sj6array
26736 [1];
26737 bool j6valid
26738 [1]
26739 = {false};
26740 _nj6 = 1;
26741 IkReal x1869
26742 = (cj4
26743 * px);
26744 IkReal x1870
26745 = (py
26746 * sj4);
26747 IkReal x1871
26748 = px
26749 * px;
26750 IkReal x1872
26751 = py
26752 * py;
26753 CheckValue<
26754 IkReal>
26755 x1873
26756 = IKPowWithIntegerCheck(
26757 ((((20.0)
26758 * x1870))
26759 + (((20.0)
26760 * x1869))),
26761 -1);
26762 if (!x1873
26763 .valid)
26764 {
26765 continue;
26766 }
26767 CheckValue<IkReal> x1874 = IKPowWithIntegerCheck(
26768 ((((-1.0)
26769 * (11.7647058823529)
26770 * cj4
26771 * (px
26772 * px
26773 * px)))
26774 + (((-11.7647058823529)
26775 * x1869
26776 * x1872))
26777 + (((-8.5)
26778 * x1869))
26779 + (((-8.5)
26780 * x1870))
26781 + (((-11.7647058823529)
26782 * x1870
26783 * x1871))
26784 + (((-1.0)
26785 * (11.7647058823529)
26786 * sj4
26787 * (py
26788 * py
26789 * py)))),
26790 -1);
26791 if (!x1874
26792 .valid)
26793 {
26794 continue;
26795 }
26796 if (IKabs((
26797 (17.0)
26798 * (x1873
26799 .value)))
26800 < IKFAST_ATAN2_MAGTHRESH
26801 && IKabs((
26802 (x1874
26803 .value)
26804 * (((48.1666666666667)
26805 + (((-66.6666666666667)
26806 * x1872))
26807 + (((-66.6666666666667)
26808 * x1871))))))
26809 < IKFAST_ATAN2_MAGTHRESH
26810 && IKabs(
26811 IKsqr((
26812 (17.0)
26813 * (x1873
26814 .value)))
26815 + IKsqr((
26816 (x1874
26817 .value)
26818 * (((48.1666666666667)
26819 + (((-66.6666666666667)
26820 * x1872))
26821 + (((-66.6666666666667)
26822 * x1871))))))
26823 - 1)
26824 <= IKFAST_SINCOS_THRESH)
26825 continue;
26826 j6array[0] = IKatan2(
26827 ((17.0)
26828 * (x1873
26829 .value)),
26830 ((x1874
26831 .value)
26832 * (((48.1666666666667)
26833 + (((-66.6666666666667)
26834 * x1872))
26835 + (((-66.6666666666667)
26836 * x1871))))));
26837 sj6array[0] = IKsin(
26838 j6array
26839 [0]);
26840 cj6array[0] = IKcos(
26841 j6array
26842 [0]);
26843 if (j6array
26844 [0]
26845 > IKPI)
26846 {
26847 j6array[0]
26848 -= IK2PI;
26849 }
26850 else if (
26851 j6array
26852 [0]
26853 < -IKPI)
26854 {
26855 j6array[0]
26856 += IK2PI;
26857 }
26858 j6valid[0]
26859 = true;
26860 for (int ij6
26861 = 0;
26862 ij6
26863 < 1;
26864 ++ij6)
26865 {
26866 if (!j6valid
26867 [ij6])
26868 {
26869 continue;
26870 }
26871 _ij6[0]
26872 = ij6;
26873 _ij6[1]
26874 = -1;
26875 for (
26876 int iij6
26877 = ij6
26878 + 1;
26879 iij6
26880 < 1;
26881 ++iij6)
26882 {
26883 if (j6valid
26884 [iij6]
26885 && IKabs(
26886 cj6array
26887 [ij6]
26888 - cj6array
26889 [iij6])
26890 < IKFAST_SOLUTION_THRESH
26891 && IKabs(
26892 sj6array
26893 [ij6]
26894 - sj6array
26895 [iij6])
26896 < IKFAST_SOLUTION_THRESH)
26897 {
26898 j6valid
26899 [iij6]
26900 = false;
26901 _ij6[1]
26902 = iij6;
26903 break;
26904 }
26905 }
26906 j6 = j6array
26907 [ij6];
26908 cj6 = cj6array
26909 [ij6];
26910 sj6 = sj6array
26911 [ij6];
26912 {
26913 IkReal evalcond
26914 [5];
26915 IkReal
26916 x1875
26917 = IKsin(
26918 j6);
26919 IkReal
26920 x1876
26921 = (cj4
26922 * px);
26923 IkReal
26924 x1877
26925 = (x1875
26926 * x1876);
26927 IkReal
26928 x1878
26929 = (py
26930 * sj4);
26931 IkReal
26932 x1879
26933 = (x1875
26934 * x1878);
26935 IkReal
26936 x1880
26937 = ((1.0)
26938 * x1876);
26939 IkReal
26940 x1881
26941 = ((1.0)
26942 * x1878);
26943 IkReal
26944 x1882
26945 = IKcos(
26946 j6);
26947 IkReal
26948 x1883
26949 = px
26950 * px;
26951 IkReal
26952 x1884
26953 = ((3.92156862745098)
26954 * x1875);
26955 IkReal
26956 x1885
26957 = ((0.588235294117647)
26958 * x1882);
26959 IkReal
26960 x1886
26961 = py
26962 * py;
26963 IkReal
26964 x1887
26965 = ((0.09)
26966 * x1882);
26967 evalcond
26968 [0]
26969 = ((-0.85)
26970 + x1879
26971 + x1877);
26972 evalcond
26973 [1]
26974 = ((((-1.0)
26975 * x1880))
26976 + (((0.85)
26977 * x1875))
26978 + (((-1.0)
26979 * x1881)));
26980 evalcond
26981 [2]
26982 = ((((-1.0)
26983 * x1881
26984 * x1882))
26985 + (((-1.0)
26986 * x1880
26987 * x1882)));
26988 evalcond
26989 [3]
26990 = ((((-0.425)
26991 * x1882))
26992 + (((-1.0)
26993 * x1885
26994 * x1886))
26995 + ((x1884
26996 * x1886))
26997 + (((-1.0)
26998 * x1883
26999 * x1885))
27000 + ((x1883
27001 * x1884))
27002 + (((-2.83333333333333)
27003 * x1875)));
27004 evalcond
27005 [4]
27006 = ((-0.2125)
27007 + (((1.1)
27008 * x1879))
27009 + (((-1.0)
27010 * x1883))
27011 + (((-1.0)
27012 * x1886))
27013 + (((1.1)
27014 * x1877))
27015 + ((x1876
27016 * x1887))
27017 + ((x1878
27018 * x1887)));
27019 if (IKabs(
27020 evalcond
27021 [0])
27022 > IKFAST_EVALCOND_THRESH
27023 || IKabs(
27024 evalcond
27025 [1])
27026 > IKFAST_EVALCOND_THRESH
27027 || IKabs(
27028 evalcond
27029 [2])
27030 > IKFAST_EVALCOND_THRESH
27031 || IKabs(
27032 evalcond
27033 [3])
27034 > IKFAST_EVALCOND_THRESH
27035 || IKabs(
27036 evalcond
27037 [4])
27038 > IKFAST_EVALCOND_THRESH)
27039 {
27040 continue;
27041 }
27042 }
27043
27044 rotationfunction0(
27045 solutions);
27046 }
27047 }
27048 }
27049 }
27050 }
27051 else
27052 {
27053 {
27054 IkReal
27055 j6array[1],
27056 cj6array[1],
27057 sj6array[1];
27058 bool j6valid[1]
27059 = {false};
27060 _nj6 = 1;
27061 IkReal x1888
27062 = (cj4
27063 * px);
27064 IkReal x1889
27065 = (py
27066 * sj4);
27067 IkReal x1890
27068 = px * px;
27069 IkReal x1891
27070 = py * py;
27071 CheckValue<
27072 IkReal>
27073 x1892
27074 = IKPowWithIntegerCheck(
27075 ((-7.225)
27076 + (((-10.0)
27077 * x1891))
27078 + (((-10.0)
27079 * x1890))),
27080 -1);
27081 if (!x1892
27082 .valid)
27083 {
27084 continue;
27085 }
27086 if (IKabs((
27087 (((1.17647058823529)
27088 * x1888))
27089 + (((1.17647058823529)
27090 * x1889))))
27091 < IKFAST_ATAN2_MAGTHRESH
27092 && IKabs((
27093 (x1892
27094 .value)
27095 * (((((56.6666666666667)
27096 * x1889))
27097 + (((56.6666666666667)
27098 * x1888))
27099 + (((-1.0)
27100 * (78.4313725490196)
27101 * sj4
27102 * (py
27103 * py
27104 * py)))
27105 + (((-78.4313725490196)
27106 * x1888
27107 * x1891))
27108 + (((-1.0)
27109 * (78.4313725490196)
27110 * cj4
27111 * (px
27112 * px
27113 * px)))
27114 + (((-78.4313725490196)
27115 * x1889
27116 * x1890))))))
27117 < IKFAST_ATAN2_MAGTHRESH
27118 && IKabs(
27119 IKsqr((
27120 (((1.17647058823529)
27121 * x1888))
27122 + (((1.17647058823529)
27123 * x1889))))
27124 + IKsqr((
27125 (x1892
27126 .value)
27127 * (((((56.6666666666667)
27128 * x1889))
27129 + (((56.6666666666667)
27130 * x1888))
27131 + (((-1.0)
27132 * (78.4313725490196)
27133 * sj4
27134 * (py
27135 * py
27136 * py)))
27137 + (((-78.4313725490196)
27138 * x1888
27139 * x1891))
27140 + (((-1.0)
27141 * (78.4313725490196)
27142 * cj4
27143 * (px
27144 * px
27145 * px)))
27146 + (((-78.4313725490196)
27147 * x1889
27148 * x1890))))))
27149 - 1)
27150 <= IKFAST_SINCOS_THRESH)
27151 continue;
27152 j6array[0] = IKatan2(
27153 ((((1.17647058823529)
27154 * x1888))
27155 + (((1.17647058823529)
27156 * x1889))),
27157 ((x1892
27158 .value)
27159 * (((((56.6666666666667)
27160 * x1889))
27161 + (((56.6666666666667)
27162 * x1888))
27163 + (((-1.0)
27164 * (78.4313725490196)
27165 * sj4
27166 * (py
27167 * py
27168 * py)))
27169 + (((-78.4313725490196)
27170 * x1888
27171 * x1891))
27172 + (((-1.0)
27173 * (78.4313725490196)
27174 * cj4
27175 * (px
27176 * px
27177 * px)))
27178 + (((-78.4313725490196)
27179 * x1889
27180 * x1890))))));
27181 sj6array[0]
27182 = IKsin(
27183 j6array
27184 [0]);
27185 cj6array[0]
27186 = IKcos(
27187 j6array
27188 [0]);
27189 if (j6array[0]
27190 > IKPI)
27191 {
27192 j6array[0]
27193 -= IK2PI;
27194 }
27195 else if (
27196 j6array[0]
27197 < -IKPI)
27198 {
27199 j6array[0]
27200 += IK2PI;
27201 }
27202 j6valid[0]
27203 = true;
27204 for (int ij6
27205 = 0;
27206 ij6 < 1;
27207 ++ij6)
27208 {
27209 if (!j6valid
27210 [ij6])
27211 {
27212 continue;
27213 }
27214 _ij6[0] = ij6;
27215 _ij6[1] = -1;
27216 for (int iij6
27217 = ij6
27218 + 1;
27219 iij6 < 1;
27220 ++iij6)
27221 {
27222 if (j6valid
27223 [iij6]
27224 && IKabs(
27225 cj6array
27226 [ij6]
27227 - cj6array
27228 [iij6])
27229 < IKFAST_SOLUTION_THRESH
27230 && IKabs(
27231 sj6array
27232 [ij6]
27233 - sj6array
27234 [iij6])
27235 < IKFAST_SOLUTION_THRESH)
27236 {
27237 j6valid
27238 [iij6]
27239 = false;
27240 _ij6[1]
27241 = iij6;
27242 break;
27243 }
27244 }
27245 j6 = j6array
27246 [ij6];
27247 cj6 = cj6array
27248 [ij6];
27249 sj6 = sj6array
27250 [ij6];
27251 {
27252 IkReal
27253 evalcond
27254 [5];
27255 IkReal x1893
27256 = IKsin(
27257 j6);
27258 IkReal x1894
27259 = (cj4
27260 * px);
27261 IkReal x1895
27262 = (x1893
27263 * x1894);
27264 IkReal x1896
27265 = (py
27266 * sj4);
27267 IkReal x1897
27268 = (x1893
27269 * x1896);
27270 IkReal x1898
27271 = ((1.0)
27272 * x1894);
27273 IkReal x1899
27274 = ((1.0)
27275 * x1896);
27276 IkReal x1900
27277 = IKcos(
27278 j6);
27279 IkReal x1901
27280 = px
27281 * px;
27282 IkReal x1902
27283 = ((3.92156862745098)
27284 * x1893);
27285 IkReal x1903
27286 = ((0.588235294117647)
27287 * x1900);
27288 IkReal x1904
27289 = py
27290 * py;
27291 IkReal x1905
27292 = ((0.09)
27293 * x1900);
27294 evalcond[0]
27295 = ((-0.85)
27296 + x1897
27297 + x1895);
27298 evalcond[1]
27299 = ((((-1.0)
27300 * x1898))
27301 + (((0.85)
27302 * x1893))
27303 + (((-1.0)
27304 * x1899)));
27305 evalcond[2]
27306 = ((((-1.0)
27307 * x1898
27308 * x1900))
27309 + (((-1.0)
27310 * x1899
27311 * x1900)));
27312 evalcond[3]
27313 = ((((-1.0)
27314 * x1901
27315 * x1903))
27316 + (((-1.0)
27317 * x1903
27318 * x1904))
27319 + ((x1902
27320 * x1904))
27321 + ((x1901
27322 * x1902))
27323 + (((-0.425)
27324 * x1900))
27325 + (((-2.83333333333333)
27326 * x1893)));
27327 evalcond[4]
27328 = ((-0.2125)
27329 + (((-1.0)
27330 * x1901))
27331 + (((1.1)
27332 * x1895))
27333 + (((1.1)
27334 * x1897))
27335 + (((-1.0)
27336 * x1904))
27337 + ((x1894
27338 * x1905))
27339 + ((x1896
27340 * x1905)));
27341 if (IKabs(
27342 evalcond
27343 [0])
27344 > IKFAST_EVALCOND_THRESH
27345 || IKabs(
27346 evalcond
27347 [1])
27348 > IKFAST_EVALCOND_THRESH
27349 || IKabs(
27350 evalcond
27351 [2])
27352 > IKFAST_EVALCOND_THRESH
27353 || IKabs(
27354 evalcond
27355 [3])
27356 > IKFAST_EVALCOND_THRESH
27357 || IKabs(
27358 evalcond
27359 [4])
27360 > IKFAST_EVALCOND_THRESH)
27361 {
27362 continue;
27363 }
27364 }
27365
27366 rotationfunction0(
27367 solutions);
27368 }
27369 }
27370 }
27371 }
27372 }
27373 else
27374 {
27375 {
27376 IkReal j6array[1],
27377 cj6array[1],
27378 sj6array[1];
27379 bool j6valid[1]
27380 = {false};
27381 _nj6 = 1;
27382 IkReal x1906
27383 = (cj4 * px);
27384 IkReal x1907
27385 = (py * sj4);
27386 IkReal x1908
27387 = px * px;
27388 IkReal x1909
27389 = py * py;
27390 IkReal x1910
27391 = ((1.29411764705882)
27392 * (cj4
27393 * cj4));
27394 CheckValue<IkReal>
27395 x1911
27396 = IKPowWithIntegerCheck(
27397 ((((0.09)
27398 * x1907))
27399 + (((0.09)
27400 * x1906))),
27401 -1);
27402 if (!x1911.valid)
27403 {
27404 continue;
27405 }
27406 if (IKabs((
27407 (((1.17647058823529)
27408 * x1907))
27409 + (((1.17647058823529)
27410 * x1906))))
27411 < IKFAST_ATAN2_MAGTHRESH
27412 && IKabs((
27413 (x1911
27414 .value)
27415 * (((0.2125)
27416 + (((-1.0)
27417 * x1908
27418 * x1910))
27419 + ((x1909
27420 * x1910))
27421 + x1908
27422 + (((-0.294117647058824)
27423 * x1909))
27424 + (((-2.58823529411765)
27425 * cj4
27426 * px
27427 * x1907))))))
27428 < IKFAST_ATAN2_MAGTHRESH
27429 && IKabs(
27430 IKsqr((
27431 (((1.17647058823529)
27432 * x1907))
27433 + (((1.17647058823529)
27434 * x1906))))
27435 + IKsqr((
27436 (x1911
27437 .value)
27438 * (((0.2125)
27439 + (((-1.0)
27440 * x1908
27441 * x1910))
27442 + ((x1909
27443 * x1910))
27444 + x1908
27445 + (((-0.294117647058824)
27446 * x1909))
27447 + (((-2.58823529411765)
27448 * cj4
27449 * px
27450 * x1907))))))
27451 - 1)
27452 <= IKFAST_SINCOS_THRESH)
27453 continue;
27454 j6array[0] = IKatan2(
27455 ((((1.17647058823529)
27456 * x1907))
27457 + (((1.17647058823529)
27458 * x1906))),
27459 ((x1911.value)
27460 * (((0.2125)
27461 + (((-1.0)
27462 * x1908
27463 * x1910))
27464 + ((x1909
27465 * x1910))
27466 + x1908
27467 + (((-0.294117647058824)
27468 * x1909))
27469 + (((-2.58823529411765)
27470 * cj4
27471 * px
27472 * x1907))))));
27473 sj6array[0] = IKsin(
27474 j6array[0]);
27475 cj6array[0] = IKcos(
27476 j6array[0]);
27477 if (j6array[0]
27478 > IKPI)
27479 {
27480 j6array[0]
27481 -= IK2PI;
27482 }
27483 else if (
27484 j6array[0]
27485 < -IKPI)
27486 {
27487 j6array[0]
27488 += IK2PI;
27489 }
27490 j6valid[0] = true;
27491 for (int ij6 = 0;
27492 ij6 < 1;
27493 ++ij6)
27494 {
27495 if (!j6valid[ij6])
27496 {
27497 continue;
27498 }
27499 _ij6[0] = ij6;
27500 _ij6[1] = -1;
27501 for (int iij6
27502 = ij6 + 1;
27503 iij6 < 1;
27504 ++iij6)
27505 {
27506 if (j6valid
27507 [iij6]
27508 && IKabs(
27509 cj6array
27510 [ij6]
27511 - cj6array
27512 [iij6])
27513 < IKFAST_SOLUTION_THRESH
27514 && IKabs(
27515 sj6array
27516 [ij6]
27517 - sj6array
27518 [iij6])
27519 < IKFAST_SOLUTION_THRESH)
27520 {
27521 j6valid[iij6]
27522 = false;
27523 _ij6[1]
27524 = iij6;
27525 break;
27526 }
27527 }
27528 j6 = j6array[ij6];
27529 cj6 = cj6array
27530 [ij6];
27531 sj6 = sj6array
27532 [ij6];
27533 {
27534 IkReal
27535 evalcond[5];
27536 IkReal x1912
27537 = IKsin(j6);
27538 IkReal x1913
27539 = (cj4
27540 * px);
27541 IkReal x1914
27542 = (x1912
27543 * x1913);
27544 IkReal x1915
27545 = (py
27546 * sj4);
27547 IkReal x1916
27548 = (x1912
27549 * x1915);
27550 IkReal x1917
27551 = ((1.0)
27552 * x1913);
27553 IkReal x1918
27554 = ((1.0)
27555 * x1915);
27556 IkReal x1919
27557 = IKcos(j6);
27558 IkReal x1920
27559 = px * px;
27560 IkReal x1921
27561 = ((3.92156862745098)
27562 * x1912);
27563 IkReal x1922
27564 = ((0.588235294117647)
27565 * x1919);
27566 IkReal x1923
27567 = py * py;
27568 IkReal x1924
27569 = ((0.09)
27570 * x1919);
27571 evalcond[0]
27572 = ((-0.85)
27573 + x1914
27574 + x1916);
27575 evalcond[1]
27576 = ((((0.85)
27577 * x1912))
27578 + (((-1.0)
27579 * x1918))
27580 + (((-1.0)
27581 * x1917)));
27582 evalcond[2]
27583 = ((((-1.0)
27584 * x1917
27585 * x1919))
27586 + (((-1.0)
27587 * x1918
27588 * x1919)));
27589 evalcond[3]
27590 = ((((-1.0)
27591 * x1920
27592 * x1922))
27593 + (((-1.0)
27594 * x1922
27595 * x1923))
27596 + ((x1921
27597 * x1923))
27598 + ((x1920
27599 * x1921))
27600 + (((-0.425)
27601 * x1919))
27602 + (((-2.83333333333333)
27603 * x1912)));
27604 evalcond[4]
27605 = ((-0.2125)
27606 + ((x1913
27607 * x1924))
27608 + (((-1.0)
27609 * x1923))
27610 + (((1.1)
27611 * x1916))
27612 + ((x1915
27613 * x1924))
27614 + (((1.1)
27615 * x1914))
27616 + (((-1.0)
27617 * x1920)));
27618 if (IKabs(
27619 evalcond
27620 [0])
27621 > IKFAST_EVALCOND_THRESH
27622 || IKabs(
27623 evalcond
27624 [1])
27625 > IKFAST_EVALCOND_THRESH
27626 || IKabs(
27627 evalcond
27628 [2])
27629 > IKFAST_EVALCOND_THRESH
27630 || IKabs(
27631 evalcond
27632 [3])
27633 > IKFAST_EVALCOND_THRESH
27634 || IKabs(
27635 evalcond
27636 [4])
27637 > IKFAST_EVALCOND_THRESH)
27638 {
27639 continue;
27640 }
27641 }
27642
27643 rotationfunction0(
27644 solutions);
27645 }
27646 }
27647 }
27648 }
27649 }
27650 } while (0);
27651 if (bgotonextstatement)
27652 {
27653 bool bgotonextstatement
27654 = true;
27655 do
27656 {
27657 if (1)
27658 {
27659 bgotonextstatement
27660 = false;
27661 continue; // branch miss
27662 // [j6]
27663 }
27664 } while (0);
27665 if (bgotonextstatement)
27666 {
27667 }
27668 }
27669 }
27670 }
27671 else
27672 {
27673 {
27674 IkReal j6array[1],
27675 cj6array[1], sj6array[1];
27676 bool j6valid[1] = {false};
27677 _nj6 = 1;
27678 IkReal x1925 = (cj4 * px);
27679 IkReal x1926 = (py * sj4);
27680 IkReal x1927
27681 = ((0.108264705882353)
27682 * cj9);
27683 IkReal x1928
27684 = ((0.588235294117647)
27685 * pp);
27686 IkReal x1929 = (cj9 * pp);
27687 IkReal x1930 = (cj9 * sj9);
27688 IkReal x1931 = (pp * sj9);
27689 IkReal x1932 = cj9 * cj9;
27690 IkReal x1933 = ((1.0) * pz);
27691 CheckValue<IkReal> x1934
27692 = IKPowWithIntegerCheck(
27693 IKsign((
27694 (((1.51009803921569)
27695 * pz))
27696 + (((1.32323529411765)
27697 * cj9 * pz))
27698 + (((-1.0)
27699 * (3.92156862745098)
27700 * pp * pz))
27701 + (((-1.0) * x1926
27702 * x1928))
27703 + (((-1.0) * x1926
27704 * x1927))
27705 + (((-1.0) * x1925
27706 * x1927))
27707 + (((-0.316735294117647)
27708 * x1925))
27709 + (((-0.316735294117647)
27710 * x1926))
27711 + (((-1.0) * x1925
27712 * x1928)))),
27713 -1);
27714 if (!x1934.valid)
27715 {
27716 continue;
27717 }
27718 CheckValue<IkReal> x1935 = IKatan2WithCheck(
27719 IkReal((
27720 (-0.174204411764706)
27721 + (((-0.00487191176470588)
27722 * x1930))
27723 + (((-0.0264705882352941)
27724 * x1931))
27725 + (pz * pz)
27726 + (((-0.176470588235294)
27727 * x1929))
27728 + (((-1.0)
27729 * (0.154566176470588)
27730 * cj9))
27731 + (((-0.0324794117647059)
27732 * x1932))
27733 + (((-1.0)
27734 * (0.323529411764706)
27735 * pp))
27736 + (((-1.0)
27737 * (0.0142530882352941)
27738 * sj9)))),
27739 ((0.830553921568627)
27740 + (((-1.17647058823529)
27741 * x1929))
27742 + (((-0.176470588235294)
27743 * x1931))
27744 + (((0.396970588235294)
27745 * x1932))
27746 + (((-1.0) * x1926
27747 * x1933))
27748 + (((1.18080882352941)
27749 * cj9))
27750 + (((-1.0)
27751 * (2.15686274509804)
27752 * pp))
27753 + (((-1.0) * x1925
27754 * x1933))
27755 + (((0.0679544117647059)
27756 * sj9))
27757 + (((0.0595455882352941)
27758 * x1930))),
27759 IKFAST_ATAN2_MAGTHRESH);
27760 if (!x1935.valid)
27761 {
27762 continue;
27763 }
27764 j6array[0]
27765 = ((-1.5707963267949)
27766 + (((1.5707963267949)
27767 * (x1934.value)))
27768 + (x1935.value));
27769 sj6array[0]
27770 = IKsin(j6array[0]);
27771 cj6array[0]
27772 = IKcos(j6array[0]);
27773 if (j6array[0] > IKPI)
27774 {
27775 j6array[0] -= IK2PI;
27776 }
27777 else if (j6array[0] < -IKPI)
27778 {
27779 j6array[0] += IK2PI;
27780 }
27781 j6valid[0] = true;
27782 for (int ij6 = 0; ij6 < 1;
27783 ++ij6)
27784 {
27785 if (!j6valid[ij6])
27786 {
27787 continue;
27788 }
27789 _ij6[0] = ij6;
27790 _ij6[1] = -1;
27791 for (int iij6 = ij6 + 1;
27792 iij6 < 1;
27793 ++iij6)
27794 {
27795 if (j6valid[iij6]
27796 && IKabs(
27797 cj6array[ij6]
27798 - cj6array
27799 [iij6])
27800 < IKFAST_SOLUTION_THRESH
27801 && IKabs(
27802 sj6array[ij6]
27803 - sj6array
27804 [iij6])
27805 < IKFAST_SOLUTION_THRESH)
27806 {
27807 j6valid[iij6] = false;
27808 _ij6[1] = iij6;
27809 break;
27810 }
27811 }
27812 j6 = j6array[ij6];
27813 cj6 = cj6array[ij6];
27814 sj6 = sj6array[ij6];
27815 {
27816 IkReal evalcond[5];
27817 IkReal x1936
27818 = ((0.3) * cj9);
27819 IkReal x1937
27820 = ((0.045) * sj9);
27821 IkReal x1938 = IKcos(j6);
27822 IkReal x1939
27823 = (pz * x1938);
27824 IkReal x1940 = IKsin(j6);
27825 IkReal x1941 = (cj4 * px);
27826 IkReal x1942
27827 = (x1940 * x1941);
27828 IkReal x1943 = (py * sj4);
27829 IkReal x1944
27830 = (x1940 * x1943);
27831 IkReal x1945
27832 = ((0.045) * cj9);
27833 IkReal x1946
27834 = ((0.3) * sj9);
27835 IkReal x1947
27836 = (pz * x1940);
27837 IkReal x1948
27838 = ((1.0) * x1941);
27839 IkReal x1949
27840 = ((1.0) * x1943);
27841 IkReal x1950
27842 = ((0.09) * x1938);
27843 evalcond[0]
27844 = ((-0.55) + x1944
27845 + x1942 + x1939
27846 + (((-1.0)
27847 * x1937))
27848 + (((-1.0)
27849 * x1936)));
27850 evalcond[1]
27851 = ((0.045) + x1946
27852 + x1947
27853 + (((-1.0)
27854 * x1945))
27855 + (((-1.0) * x1938
27856 * x1948))
27857 + (((-1.0) * x1938
27858 * x1949)));
27859 evalcond[2]
27860 = ((((-1.51009803921569)
27861 * x1940))
27862 + pz
27863 + (((3.92156862745098)
27864 * pp * x1940))
27865 + (((-0.588235294117647)
27866 * pp * x1938))
27867 + (((-0.108264705882353)
27868 * cj9 * x1938))
27869 + (((-1.32323529411765)
27870 * cj9 * x1940))
27871 + (((-0.316735294117647)
27872 * x1938)));
27873 evalcond[3]
27874 = ((((-1.0) * x1948))
27875 + ((x1936 * x1940))
27876 + (((0.55)
27877 * x1940))
27878 + (((-1.0) * x1938
27879 * x1945))
27880 + (((-1.0)
27881 * x1949))
27882 + (((0.045)
27883 * x1938))
27884 + ((x1937 * x1940))
27885 + ((x1938
27886 * x1946)));
27887 evalcond[4]
27888 = ((-0.2125)
27889 + (((1.1) * x1944))
27890 + ((x1943 * x1950))
27891 + ((x1941 * x1950))
27892 + (((1.1) * x1942))
27893 + (((1.1) * x1939))
27894 + (((-0.09)
27895 * x1947))
27896 + (((-1.0) * (1.0)
27897 * pp)));
27898 if (IKabs(evalcond[0])
27899 > IKFAST_EVALCOND_THRESH
27900 || IKabs(evalcond[1])
27901 > IKFAST_EVALCOND_THRESH
27902 || IKabs(evalcond[2])
27903 > IKFAST_EVALCOND_THRESH
27904 || IKabs(evalcond[3])
27905 > IKFAST_EVALCOND_THRESH
27906 || IKabs(evalcond[4])
27907 > IKFAST_EVALCOND_THRESH)
27908 {
27909 continue;
27910 }
27911 }
27912
27913 rotationfunction0(
27914 solutions);
27915 }
27916 }
27917 }
27918 }
27919 }
27920 else
27921 {
27922 {
27923 IkReal j6array[1], cj6array[1],
27924 sj6array[1];
27925 bool j6valid[1] = {false};
27926 _nj6 = 1;
27927 IkReal x1951
27928 = ((0.045) * cj4 * px);
27929 IkReal x1952
27930 = ((0.045) * py * sj4);
27931 IkReal x1953 = ((0.3) * sj9);
27932 IkReal x1954 = (cj4 * px);
27933 IkReal x1955 = (py * sj4);
27934 IkReal x1956 = (cj9 * sj9);
27935 IkReal x1957 = cj9 * cj9;
27936 IkReal x1958 = ((1.0) * pz);
27937 IkReal x1959 = py * py;
27938 IkReal x1960 = cj4 * cj4;
27939 CheckValue<IkReal> x1961
27940 = IKatan2WithCheck(
27941 IkReal((
27942 (0.03825)
27943 + (((-1.0) * x1955
27944 * x1958))
27945 + (((0.087975)
27946 * x1956))
27947 + (((-0.027) * x1957))
27948 + (((0.167025) * sj9))
27949 + (((-1.0) * (0.01125)
27950 * cj9))
27951 + (((-1.0) * x1954
27952 * x1958)))),
27953 ((-0.304525)
27954 + (((-1.0) * x1959
27955 * x1960))
27956 + ((x1960 * (px * px)))
27957 + (((-1.0) * (0.0495)
27958 * sj9))
27959 + (((-0.087975) * x1957))
27960 + (((-1.0) * (0.33)
27961 * cj9))
27962 + (((2.0) * cj4 * px
27963 * x1955))
27964 + (((-0.027) * x1956))
27965 + x1959),
27966 IKFAST_ATAN2_MAGTHRESH);
27967 if (!x1961.valid)
27968 {
27969 continue;
27970 }
27971 CheckValue<IkReal> x1962
27972 = IKPowWithIntegerCheck(
27973 IKsign(
27974 ((((-1.0) * (0.55)
27975 * pz))
27976 + ((x1953 * x1954))
27977 + (((-1.0) * cj9
27978 * x1952))
27979 + ((x1953 * x1955))
27980 + (((-1.0) * cj9
27981 * x1951))
27982 + x1951 + x1952
27983 + (((-1.0) * (0.045)
27984 * pz * sj9))
27985 + (((-1.0) * (0.3)
27986 * cj9 * pz)))),
27987 -1);
27988 if (!x1962.valid)
27989 {
27990 continue;
27991 }
27992 j6array[0]
27993 = ((-1.5707963267949)
27994 + (x1961.value)
27995 + (((1.5707963267949)
27996 * (x1962.value))));
27997 sj6array[0] = IKsin(j6array[0]);
27998 cj6array[0] = IKcos(j6array[0]);
27999 if (j6array[0] > IKPI)
28000 {
28001 j6array[0] -= IK2PI;
28002 }
28003 else if (j6array[0] < -IKPI)
28004 {
28005 j6array[0] += IK2PI;
28006 }
28007 j6valid[0] = true;
28008 for (int ij6 = 0; ij6 < 1; ++ij6)
28009 {
28010 if (!j6valid[ij6])
28011 {
28012 continue;
28013 }
28014 _ij6[0] = ij6;
28015 _ij6[1] = -1;
28016 for (int iij6 = ij6 + 1;
28017 iij6 < 1;
28018 ++iij6)
28019 {
28020 if (j6valid[iij6]
28021 && IKabs(
28022 cj6array[ij6]
28023 - cj6array[iij6])
28024 < IKFAST_SOLUTION_THRESH
28025 && IKabs(
28026 sj6array[ij6]
28027 - sj6array[iij6])
28028 < IKFAST_SOLUTION_THRESH)
28029 {
28030 j6valid[iij6] = false;
28031 _ij6[1] = iij6;
28032 break;
28033 }
28034 }
28035 j6 = j6array[ij6];
28036 cj6 = cj6array[ij6];
28037 sj6 = sj6array[ij6];
28038 {
28039 IkReal evalcond[5];
28040 IkReal x1963 = ((0.3) * cj9);
28041 IkReal x1964
28042 = ((0.045) * sj9);
28043 IkReal x1965 = IKcos(j6);
28044 IkReal x1966 = (pz * x1965);
28045 IkReal x1967 = IKsin(j6);
28046 IkReal x1968 = (cj4 * px);
28047 IkReal x1969
28048 = (x1967 * x1968);
28049 IkReal x1970 = (py * sj4);
28050 IkReal x1971
28051 = (x1967 * x1970);
28052 IkReal x1972
28053 = ((0.045) * cj9);
28054 IkReal x1973 = ((0.3) * sj9);
28055 IkReal x1974 = (pz * x1967);
28056 IkReal x1975
28057 = ((1.0) * x1968);
28058 IkReal x1976
28059 = ((1.0) * x1970);
28060 IkReal x1977
28061 = ((0.09) * x1965);
28062 evalcond[0]
28063 = ((-0.55) + x1966 + x1969
28064 + (((-1.0) * x1964))
28065 + x1971
28066 + (((-1.0) * x1963)));
28067 evalcond[1]
28068 = ((0.045)
28069 + (((-1.0) * x1965
28070 * x1976))
28071 + (((-1.0) * x1965
28072 * x1975))
28073 + (((-1.0) * x1972))
28074 + x1973 + x1974);
28075 evalcond[2]
28076 = ((((-0.316735294117647)
28077 * x1965))
28078 + (((-0.588235294117647)
28079 * pp * x1965))
28080 + pz
28081 + (((-1.51009803921569)
28082 * x1967))
28083 + (((-0.108264705882353)
28084 * cj9 * x1965))
28085 + (((3.92156862745098)
28086 * pp * x1967))
28087 + (((-1.32323529411765)
28088 * cj9 * x1967)));
28089 evalcond[3]
28090 = ((((0.045) * x1965))
28091 + ((x1964 * x1967))
28092 + ((x1963 * x1967))
28093 + (((-1.0) * x1976))
28094 + (((-1.0) * x1965
28095 * x1972))
28096 + (((-1.0) * x1975))
28097 + (((0.55) * x1967))
28098 + ((x1965 * x1973)));
28099 evalcond[4]
28100 = ((-0.2125)
28101 + (((1.1) * x1966))
28102 + (((1.1) * x1969))
28103 + (((1.1) * x1971))
28104 + (((-0.09) * x1974))
28105 + ((x1970 * x1977))
28106 + ((x1968 * x1977))
28107 + (((-1.0) * (1.0)
28108 * pp)));
28109 if (IKabs(evalcond[0])
28110 > IKFAST_EVALCOND_THRESH
28111 || IKabs(evalcond[1])
28112 > IKFAST_EVALCOND_THRESH
28113 || IKabs(evalcond[2])
28114 > IKFAST_EVALCOND_THRESH
28115 || IKabs(evalcond[3])
28116 > IKFAST_EVALCOND_THRESH
28117 || IKabs(evalcond[4])
28118 > IKFAST_EVALCOND_THRESH)
28119 {
28120 continue;
28121 }
28122 }
28123
28124 rotationfunction0(solutions);
28125 }
28126 }
28127 }
28128 }
28129 }
28130 else
28131 {
28132 {
28133 IkReal j6array[1], cj6array[1],
28134 sj6array[1];
28135 bool j6valid[1] = {false};
28136 _nj6 = 1;
28137 IkReal x1978 = (cj4 * px);
28138 IkReal x1979 = (py * sj4);
28139 IkReal x1980
28140 = ((1.32323529411765) * cj9);
28141 IkReal x1981
28142 = ((3.92156862745098) * pp);
28143 IkReal x1982
28144 = ((0.0264705882352941) * pp);
28145 IkReal x1983 = (cj9 * sj9);
28146 IkReal x1984
28147 = ((0.176470588235294) * pp);
28148 IkReal x1985 = cj9 * cj9;
28149 CheckValue<IkReal> x1986
28150 = IKatan2WithCheck(
28151 IkReal((
28152 (-0.0142530882352941)
28153 + ((pz * x1979))
28154 + (((0.00938117647058823)
28155 * cj9))
28156 + ((cj9 * x1982))
28157 + (((-0.0324794117647059)
28158 * x1983))
28159 + (((0.00487191176470588)
28160 * x1985))
28161 + (((-1.0) * x1982))
28162 + ((pz * x1978))
28163 + (((-1.0) * sj9 * x1984))
28164 + (((-1.0)
28165 * (0.0950205882352941)
28166 * sj9)))),
28167 ((0.0679544117647059)
28168 + (((0.396970588235294)
28169 * x1983))
28170 + (((-0.0595455882352941)
28171 * x1985))
28172 + (pz * pz) + ((cj9 * x1984))
28173 + (((-1.0)
28174 * (0.00840882352941177)
28175 * cj9))
28176 + (((0.453029411764706)
28177 * sj9))
28178 + (((-1.0) * x1984))
28179 + (((-1.0)
28180 * (1.17647058823529) * pp
28181 * sj9))),
28182 IKFAST_ATAN2_MAGTHRESH);
28183 if (!x1986.valid)
28184 {
28185 continue;
28186 }
28187 CheckValue<IkReal> x1987
28188 = IKPowWithIntegerCheck(
28189 IKsign((
28190 (((-1.0) * x1978 * x1981))
28191 + (((1.51009803921569)
28192 * x1978))
28193 + (((1.51009803921569)
28194 * x1979))
28195 + (((0.316735294117647)
28196 * pz))
28197 + (((0.108264705882353)
28198 * cj9 * pz))
28199 + (((0.588235294117647)
28200 * pp * pz))
28201 + ((x1978 * x1980))
28202 + (((-1.0) * x1979
28203 * x1981))
28204 + ((x1979 * x1980)))),
28205 -1);
28206 if (!x1987.valid)
28207 {
28208 continue;
28209 }
28210 j6array[0]
28211 = ((-1.5707963267949)
28212 + (x1986.value)
28213 + (((1.5707963267949)
28214 * (x1987.value))));
28215 sj6array[0] = IKsin(j6array[0]);
28216 cj6array[0] = IKcos(j6array[0]);
28217 if (j6array[0] > IKPI)
28218 {
28219 j6array[0] -= IK2PI;
28220 }
28221 else if (j6array[0] < -IKPI)
28222 {
28223 j6array[0] += IK2PI;
28224 }
28225 j6valid[0] = true;
28226 for (int ij6 = 0; ij6 < 1; ++ij6)
28227 {
28228 if (!j6valid[ij6])
28229 {
28230 continue;
28231 }
28232 _ij6[0] = ij6;
28233 _ij6[1] = -1;
28234 for (int iij6 = ij6 + 1; iij6 < 1;
28235 ++iij6)
28236 {
28237 if (j6valid[iij6]
28238 && IKabs(
28239 cj6array[ij6]
28240 - cj6array[iij6])
28241 < IKFAST_SOLUTION_THRESH
28242 && IKabs(
28243 sj6array[ij6]
28244 - sj6array[iij6])
28245 < IKFAST_SOLUTION_THRESH)
28246 {
28247 j6valid[iij6] = false;
28248 _ij6[1] = iij6;
28249 break;
28250 }
28251 }
28252 j6 = j6array[ij6];
28253 cj6 = cj6array[ij6];
28254 sj6 = sj6array[ij6];
28255 {
28256 IkReal evalcond[5];
28257 IkReal x1988 = ((0.3) * cj9);
28258 IkReal x1989 = ((0.045) * sj9);
28259 IkReal x1990 = IKcos(j6);
28260 IkReal x1991 = (pz * x1990);
28261 IkReal x1992 = IKsin(j6);
28262 IkReal x1993 = (cj4 * px);
28263 IkReal x1994 = (x1992 * x1993);
28264 IkReal x1995 = (py * sj4);
28265 IkReal x1996 = (x1992 * x1995);
28266 IkReal x1997 = ((0.045) * cj9);
28267 IkReal x1998 = ((0.3) * sj9);
28268 IkReal x1999 = (pz * x1992);
28269 IkReal x2000 = ((1.0) * x1993);
28270 IkReal x2001 = ((1.0) * x1995);
28271 IkReal x2002 = ((0.09) * x1990);
28272 evalcond[0]
28273 = ((-0.55)
28274 + (((-1.0) * x1989))
28275 + x1996 + x1994 + x1991
28276 + (((-1.0) * x1988)));
28277 evalcond[1]
28278 = ((0.045)
28279 + (((-1.0) * x1990
28280 * x2000))
28281 + (((-1.0) * x1997))
28282 + x1998 + x1999
28283 + (((-1.0) * x1990
28284 * x2001)));
28285 evalcond[2]
28286 = ((((-0.108264705882353)
28287 * cj9 * x1990))
28288 + (((-0.588235294117647)
28289 * pp * x1990))
28290 + pz
28291 + (((-1.51009803921569)
28292 * x1992))
28293 + (((-1.32323529411765)
28294 * cj9 * x1992))
28295 + (((3.92156862745098) * pp
28296 * x1992))
28297 + (((-0.316735294117647)
28298 * x1990)));
28299 evalcond[3]
28300 = ((((0.045) * x1990))
28301 + (((-1.0) * x2001))
28302 + (((0.55) * x1992))
28303 + (((-1.0) * x2000))
28304 + (((-1.0) * x1990
28305 * x1997))
28306 + ((x1990 * x1998))
28307 + ((x1989 * x1992))
28308 + ((x1988 * x1992)));
28309 evalcond[4]
28310 = ((-0.2125)
28311 + (((1.1) * x1996))
28312 + (((1.1) * x1991))
28313 + ((x1995 * x2002))
28314 + (((-0.09) * x1999))
28315 + ((x1993 * x2002))
28316 + (((-1.0) * (1.0) * pp))
28317 + (((1.1) * x1994)));
28318 if (IKabs(evalcond[0])
28319 > IKFAST_EVALCOND_THRESH
28320 || IKabs(evalcond[1])
28321 > IKFAST_EVALCOND_THRESH
28322 || IKabs(evalcond[2])
28323 > IKFAST_EVALCOND_THRESH
28324 || IKabs(evalcond[3])
28325 > IKFAST_EVALCOND_THRESH
28326 || IKabs(evalcond[4])
28327 > IKFAST_EVALCOND_THRESH)
28328 {
28329 continue;
28330 }
28331 }
28332
28333 rotationfunction0(solutions);
28334 }
28335 }
28336 }
28337 }
28338 }
28339 } while (0);
28340 if (bgotonextstatement)
28341 {
28342 bool bgotonextstatement = true;
28343 do
28344 {
28345 IkReal x2003 = (px * sj4);
28346 IkReal x2004 = (cj4 * py);
28347 evalcond[0]
28348 = ((-3.14159265358979)
28349 + (IKfmod(
28350 ((3.14159265358979)
28351 + (IKabs(
28352 ((-3.14159265358979)
28353 + j8)))),
28354 6.28318530717959)));
28355 evalcond[1]
28356 = ((0.39655) + (((0.0765) * sj9))
28357 + (((0.32595) * cj9))
28358 + (((-1.0) * (1.0) * pp)));
28359 evalcond[2] = (x2003 + (((-1.0) * x2004)));
28360 evalcond[3] = ((((-1.0) * x2003)) + x2004);
28361 if (IKabs(evalcond[0]) < 0.0000010000000000
28362 && IKabs(evalcond[1])
28363 < 0.0000010000000000
28364 && IKabs(evalcond[2])
28365 < 0.0000010000000000
28366 && IKabs(evalcond[3])
28367 < 0.0000010000000000)
28368 {
28369 bgotonextstatement = false;
28370 {
28371 IkReal j6eval[2];
28372 sj8 = 0;
28373 cj8 = -1.0;
28374 j8 = 3.14159265358979;
28375 IkReal x2005 = py * py;
28376 IkReal x2006 = cj4 * cj4;
28377 IkReal x2007
28378 = (x2005
28379 + (((-1.0) * x2005 * x2006))
28380 + (pz * pz)
28381 + (((2.0) * cj4 * px * py * sj4))
28382 + ((x2006 * (px * px))));
28383 j6eval[0] = x2007;
28384 j6eval[1] = IKsign(x2007);
28385 if (IKabs(j6eval[0])
28386 < 0.0000010000000000
28387 || IKabs(j6eval[1])
28388 < 0.0000010000000000)
28389 {
28390 {
28391 IkReal j6eval[2];
28392 sj8 = 0;
28393 cj8 = -1.0;
28394 j8 = 3.14159265358979;
28395 IkReal x2008 = (cj4 * px);
28396 IkReal x2009 = (cj9 * pz);
28397 IkReal x2010 = (py * sj4);
28398 IkReal x2011 = (pz * sj9);
28399 IkReal x2012 = (cj4 * px * sj9);
28400 IkReal x2013 = (py * sj4 * sj9);
28401 IkReal x2014 = ((0.045) * x2008);
28402 IkReal x2015 = ((0.045) * x2010);
28403 j6eval[0]
28404 = (((cj9 * x2010))
28405 + (((-1.0) * x2010))
28406 + ((cj9 * x2008))
28407 + (((-1.0)
28408 * (12.2222222222222)
28409 * pz))
28410 + (((-1.0) * x2011))
28411 + (((-6.66666666666667)
28412 * x2009))
28413 + (((-6.66666666666667)
28414 * x2013))
28415 + (((-1.0) * x2008))
28416 + (((-6.66666666666667)
28417 * x2012)));
28418 j6eval[1] = IKsign(
28419 (((cj9 * x2014))
28420 + (((-0.3) * x2012))
28421 + (((-0.3) * x2013))
28422 + (((-1.0) * (0.55) * pz))
28423 + (((-0.045) * x2011))
28424 + ((cj9 * x2015))
28425 + (((-1.0) * x2014))
28426 + (((-1.0) * x2015))
28427 + (((-0.3) * x2009))));
28428 if (IKabs(j6eval[0])
28429 < 0.0000010000000000
28430 || IKabs(j6eval[1])
28431 < 0.0000010000000000)
28432 {
28433 {
28434 IkReal j6eval[2];
28435 sj8 = 0;
28436 cj8 = -1.0;
28437 j8 = 3.14159265358979;
28438 IkReal x2016 = (cj4 * px);
28439 IkReal x2017 = (cj9 * pz);
28440 IkReal x2018 = (pp * pz);
28441 IkReal x2019 = (py * sj4);
28442 IkReal x2020 = (cj4 * cj9 * px);
28443 IkReal x2021 = (cj4 * pp * px);
28444 IkReal x2022 = (cj9 * py * sj4);
28445 IkReal x2023 = (pp * py * sj4);
28446 j6eval[0]
28447 = ((((-2.92556370551481)
28448 * x2016))
28449 + (((-1.0)
28450 * (13.9482024812098)
28451 * pz))
28452 + (((-5.4333061668025)
28453 * x2021))
28454 + (((-1.0) * x2022))
28455 + (((-12.2222222222222)
28456 * x2017))
28457 + (((-5.4333061668025)
28458 * x2023))
28459 + (((-2.92556370551481)
28460 * x2019))
28461 + (((36.2220411120167)
28462 * x2018))
28463 + (((-1.0) * x2020)));
28464 j6eval[1] = IKsign(
28465 ((((-0.588235294117647)
28466 * x2021))
28467 + (((-1.32323529411765)
28468 * x2017))
28469 + (((3.92156862745098)
28470 * x2018))
28471 + (((-0.316735294117647)
28472 * x2019))
28473 + (((-1.0)
28474 * (1.51009803921569)
28475 * pz))
28476 + (((-0.108264705882353)
28477 * x2020))
28478 + (((-0.108264705882353)
28479 * x2022))
28480 + (((-0.588235294117647)
28481 * x2023))
28482 + (((-0.316735294117647)
28483 * x2016))));
28484 if (IKabs(j6eval[0])
28485 < 0.0000010000000000
28486 || IKabs(j6eval[1])
28487 < 0.0000010000000000)
28488 {
28489 {
28490 IkReal evalcond[1];
28491 bool bgotonextstatement
28492 = true;
28493 do
28494 {
28495 evalcond[0]
28496 = ((IKabs((
28497 (-3.14159265358979)
28498 + (IKfmod(
28499 ((3.14159265358979)
28500 + j9),
28501 6.28318530717959)))))
28502 + (IKabs(pz)));
28503 if (IKabs(evalcond[0])
28504 < 0.0000010000000000)
28505 {
28506 bgotonextstatement
28507 = false;
28508 {
28509 IkReal j6eval[1];
28510 IkReal x2024
28511 = ((1.0) * py);
28512 sj8 = 0;
28513 cj8 = -1.0;
28514 j8 = 3.14159265358979;
28515 pz = 0;
28516 j9 = 0;
28517 sj9 = 0;
28518 cj9 = 1.0;
28519 pp
28520 = ((py * py)
28521 + (px * px));
28522 npx
28523 = (((py * r10))
28524 + ((px
28525 * r00)));
28526 npy
28527 = (((py * r11))
28528 + ((px
28529 * r01)));
28530 npz
28531 = (((px * r02))
28532 + ((py
28533 * r12)));
28534 rxp0_0
28535 = ((-1.0) * r20
28536 * x2024);
28537 rxp0_1 = (px * r20);
28538 rxp1_0
28539 = ((-1.0) * r21
28540 * x2024);
28541 rxp1_1 = (px * r21);
28542 rxp2_0
28543 = ((-1.0) * r22
28544 * x2024);
28545 rxp2_1 = (px * r22);
28546 j6eval[0]
28547 = ((((-1.0)
28548 * (1.0) * cj4
28549 * px))
28550 + (((-1.0)
28551 * (1.0)
28552 * py
28553 * sj4)));
28554 if (IKabs(j6eval[0])
28555 < 0.0000010000000000)
28556 {
28557 {
28558 IkReal j6eval[1];
28559 IkReal x2025
28560 = ((1.0)
28561 * py);
28562 sj8 = 0;
28563 cj8 = -1.0;
28564 j8 = 3.14159265358979;
28565 pz = 0;
28566 j9 = 0;
28567 sj9 = 0;
28568 cj9 = 1.0;
28569 pp
28570 = ((py * py)
28571 + (px
28572 * px));
28573 npx
28574 = (((py
28575 * r10))
28576 + ((px
28577 * r00)));
28578 npy
28579 = (((py
28580 * r11))
28581 + ((px
28582 * r01)));
28583 npz
28584 = (((px
28585 * r02))
28586 + ((py
28587 * r12)));
28588 rxp0_0
28589 = ((-1.0)
28590 * r20
28591 * x2025);
28592 rxp0_1
28593 = (px * r20);
28594 rxp1_0
28595 = ((-1.0)
28596 * r21
28597 * x2025);
28598 rxp1_1
28599 = (px * r21);
28600 rxp2_0
28601 = ((-1.0)
28602 * r22
28603 * x2025);
28604 rxp2_1
28605 = (px * r22);
28606 j6eval[0]
28607 = ((-1.0)
28608 + (((-1.0)
28609 * (1.3840830449827)
28610 * (px
28611 * px)))
28612 + (((-1.0)
28613 * (1.3840830449827)
28614 * (py
28615 * py))));
28616 if (IKabs(
28617 j6eval[0])
28618 < 0.0000010000000000)
28619 {
28620 {
28621 IkReal
28622 j6eval[2];
28623 IkReal x2026
28624 = ((1.0)
28625 * py);
28626 sj8 = 0;
28627 cj8 = -1.0;
28628 j8 = 3.14159265358979;
28629 pz = 0;
28630 j9 = 0;
28631 sj9 = 0;
28632 cj9 = 1.0;
28633 pp
28634 = ((py
28635 * py)
28636 + (px
28637 * px));
28638 npx
28639 = (((py
28640 * r10))
28641 + ((px
28642 * r00)));
28643 npy
28644 = (((py
28645 * r11))
28646 + ((px
28647 * r01)));
28648 npz
28649 = (((px
28650 * r02))
28651 + ((py
28652 * r12)));
28653 rxp0_0
28654 = ((-1.0)
28655 * r20
28656 * x2026);
28657 rxp0_1
28658 = (px
28659 * r20);
28660 rxp1_0
28661 = ((-1.0)
28662 * r21
28663 * x2026);
28664 rxp1_1
28665 = (px
28666 * r21);
28667 rxp2_0
28668 = ((-1.0)
28669 * r22
28670 * x2026);
28671 rxp2_1
28672 = (px
28673 * r22);
28674 IkReal x2027
28675 = (cj4
28676 * px);
28677 IkReal x2028
28678 = (py
28679 * sj4);
28680 j6eval[0]
28681 = (x2027
28682 + x2028);
28683 j6eval[1]
28684 = ((((-1.0)
28685 * (1.3840830449827)
28686 * cj4
28687 * (px
28688 * px
28689 * px)))
28690 + (((-1.0)
28691 * x2028))
28692 + (((-1.3840830449827)
28693 * x2027
28694 * (py
28695 * py)))
28696 + (((-1.0)
28697 * (1.3840830449827)
28698 * sj4
28699 * (py
28700 * py
28701 * py)))
28702 + (((-1.0)
28703 * x2027))
28704 + (((-1.3840830449827)
28705 * x2028
28706 * (px
28707 * px))));
28708 if (IKabs(
28709 j6eval
28710 [0])
28711 < 0.0000010000000000
28712 || IKabs(
28713 j6eval
28714 [1])
28715 < 0.0000010000000000)
28716 {
28717 {
28718 IkReal evalcond
28719 [4];
28720 bool
28721 bgotonextstatement
28722 = true;
28723 do
28724 {
28725 evalcond
28726 [0]
28727 = ((IKabs(
28728 py))
28729 + (IKabs(
28730 px)));
28731 evalcond
28732 [1]
28733 = -0.85;
28734 evalcond
28735 [2]
28736 = 0;
28737 evalcond
28738 [3]
28739 = -0.2125;
28740 if (IKabs(
28741 evalcond
28742 [0])
28743 < 0.0000010000000000
28744 && IKabs(
28745 evalcond
28746 [1])
28747 < 0.0000010000000000
28748 && IKabs(
28749 evalcond
28750 [2])
28751 < 0.0000010000000000
28752 && IKabs(
28753 evalcond
28754 [3])
28755 < 0.0000010000000000)
28756 {
28757 bgotonextstatement
28758 = false;
28759 {
28760 IkReal j6array
28761 [2],
28762 cj6array
28763 [2],
28764 sj6array
28765 [2];
28766 bool j6valid
28767 [2]
28768 = {false};
28769 _nj6
28770 = 2;
28771 j6array
28772 [0]
28773 = 0.148889947609497;
28774 sj6array
28775 [0]
28776 = IKsin(
28777 j6array
28778 [0]);
28779 cj6array
28780 [0]
28781 = IKcos(
28782 j6array
28783 [0]);
28784 j6array
28785 [1]
28786 = 3.29048260119929;
28787 sj6array
28788 [1]
28789 = IKsin(
28790 j6array
28791 [1]);
28792 cj6array
28793 [1]
28794 = IKcos(
28795 j6array
28796 [1]);
28797 if (j6array
28798 [0]
28799 > IKPI)
28800 {
28801 j6array
28802 [0]
28803 -= IK2PI;
28804 }
28805 else if (
28806 j6array
28807 [0]
28808 < -IKPI)
28809 {
28810 j6array
28811 [0]
28812 += IK2PI;
28813 }
28814 j6valid
28815 [0]
28816 = true;
28817 if (j6array
28818 [1]
28819 > IKPI)
28820 {
28821 j6array
28822 [1]
28823 -= IK2PI;
28824 }
28825 else if (
28826 j6array
28827 [1]
28828 < -IKPI)
28829 {
28830 j6array
28831 [1]
28832 += IK2PI;
28833 }
28834 j6valid
28835 [1]
28836 = true;
28837 for (
28838 int ij6
28839 = 0;
28840 ij6
28841 < 2;
28842 ++ij6)
28843 {
28844 if (!j6valid
28845 [ij6])
28846 {
28847 continue;
28848 }
28849 _ij6[0]
28850 = ij6;
28851 _ij6[1]
28852 = -1;
28853 for (
28854 int iij6
28855 = ij6
28856 + 1;
28857 iij6
28858 < 2;
28859 ++iij6)
28860 {
28861 if (j6valid
28862 [iij6]
28863 && IKabs(
28864 cj6array
28865 [ij6]
28866 - cj6array
28867 [iij6])
28868 < IKFAST_SOLUTION_THRESH
28869 && IKabs(
28870 sj6array
28871 [ij6]
28872 - sj6array
28873 [iij6])
28874 < IKFAST_SOLUTION_THRESH)
28875 {
28876 j6valid
28877 [iij6]
28878 = false;
28879 _ij6[1]
28880 = iij6;
28881 break;
28882 }
28883 }
28884 j6 = j6array
28885 [ij6];
28886 cj6 = cj6array
28887 [ij6];
28888 sj6 = sj6array
28889 [ij6];
28890 {
28891 IkReal evalcond
28892 [1];
28893 evalcond
28894 [0]
28895 = ((0.85)
28896 * (IKsin(
28897 j6)));
28898 if (IKabs(
28899 evalcond
28900 [0])
28901 > IKFAST_EVALCOND_THRESH)
28902 {
28903 continue;
28904 }
28905 }
28906
28907 rotationfunction0(
28908 solutions);
28909 }
28910 }
28911 }
28912 } while (
28913 0);
28914 if (bgotonextstatement)
28915 {
28916 bool
28917 bgotonextstatement
28918 = true;
28919 do
28920 {
28921 evalcond
28922 [0]
28923 = ((IKabs((
28924 (-3.14159265358979)
28925 + (IKfmod(
28926 ((3.14159265358979)
28927 + j4),
28928 6.28318530717959)))))
28929 + (IKabs(
28930 px)));
28931 evalcond
28932 [1]
28933 = -0.85;
28934 evalcond
28935 [2]
28936 = 0;
28937 evalcond
28938 [3]
28939 = ((-0.2125)
28940 + (((-1.0)
28941 * (1.0)
28942 * (py
28943 * py))));
28944 if (IKabs(
28945 evalcond
28946 [0])
28947 < 0.0000010000000000
28948 && IKabs(
28949 evalcond
28950 [1])
28951 < 0.0000010000000000
28952 && IKabs(
28953 evalcond
28954 [2])
28955 < 0.0000010000000000
28956 && IKabs(
28957 evalcond
28958 [3])
28959 < 0.0000010000000000)
28960 {
28961 bgotonextstatement
28962 = false;
28963 {
28964 IkReal j6eval
28965 [1];
28966 IkReal
28967 x2029
28968 = ((1.0)
28969 * py);
28970 sj8 = 0;
28971 cj8 = -1.0;
28972 j8 = 3.14159265358979;
28973 pz = 0;
28974 j9 = 0;
28975 sj9 = 0;
28976 cj9 = 1.0;
28977 pp = py
28978 * py;
28979 npx
28980 = (py
28981 * r10);
28982 npy
28983 = (py
28984 * r11);
28985 npz
28986 = (py
28987 * r12);
28988 rxp0_0
28989 = ((-1.0)
28990 * r20
28991 * x2029);
28992 rxp0_1
28993 = 0;
28994 rxp1_0
28995 = ((-1.0)
28996 * r21
28997 * x2029);
28998 rxp1_1
28999 = 0;
29000 rxp2_0
29001 = ((-1.0)
29002 * r22
29003 * x2029);
29004 rxp2_1
29005 = 0;
29006 px = 0;
29007 j4 = 0;
29008 sj4 = 0;
29009 cj4 = 1.0;
29010 rxp0_2
29011 = (py
29012 * r00);
29013 rxp1_2
29014 = (py
29015 * r01);
29016 rxp2_2
29017 = (py
29018 * r02);
29019 j6eval
29020 [0]
29021 = ((1.0)
29022 + (((1.91568587540858)
29023 * (py
29024 * py
29025 * py
29026 * py)))
29027 + (((-1.0)
29028 * (2.64633970947792)
29029 * (py
29030 * py))));
29031 if (IKabs(
29032 j6eval
29033 [0])
29034 < 0.0000010000000000)
29035 {
29036 continue; // no branches [j6]
29037 }
29038 else
29039 {
29040 {
29041 IkReal j6array
29042 [2],
29043 cj6array
29044 [2],
29045 sj6array
29046 [2];
29047 bool j6valid
29048 [2]
29049 = {false};
29050 _nj6
29051 = 2;
29052 IkReal
29053 x2030
29054 = py
29055 * py;
29056 CheckValue<IkReal> x2032 = IKatan2WithCheck(
29057 IkReal((
29058 (-0.425)
29059 + (((-0.588235294117647)
29060 * x2030)))),
29061 ((2.83333333333333)
29062 + (((-3.92156862745098)
29063 * x2030))),
29064 IKFAST_ATAN2_MAGTHRESH);
29065 if (!x2032
29066 .valid)
29067 {
29068 continue;
29069 }
29070 IkReal
29071 x2031
29072 = ((-1.0)
29073 * (x2032
29074 .value));
29075 j6array
29076 [0]
29077 = x2031;
29078 sj6array
29079 [0]
29080 = IKsin(
29081 j6array
29082 [0]);
29083 cj6array
29084 [0]
29085 = IKcos(
29086 j6array
29087 [0]);
29088 j6array
29089 [1]
29090 = ((3.14159265358979)
29091 + x2031);
29092 sj6array
29093 [1]
29094 = IKsin(
29095 j6array
29096 [1]);
29097 cj6array
29098 [1]
29099 = IKcos(
29100 j6array
29101 [1]);
29102 if (j6array
29103 [0]
29104 > IKPI)
29105 {
29106 j6array
29107 [0]
29108 -= IK2PI;
29109 }
29110 else if (
29111 j6array
29112 [0]
29113 < -IKPI)
29114 {
29115 j6array
29116 [0]
29117 += IK2PI;
29118 }
29119 j6valid
29120 [0]
29121 = true;
29122 if (j6array
29123 [1]
29124 > IKPI)
29125 {
29126 j6array
29127 [1]
29128 -= IK2PI;
29129 }
29130 else if (
29131 j6array
29132 [1]
29133 < -IKPI)
29134 {
29135 j6array
29136 [1]
29137 += IK2PI;
29138 }
29139 j6valid
29140 [1]
29141 = true;
29142 for (
29143 int ij6
29144 = 0;
29145 ij6
29146 < 2;
29147 ++ij6)
29148 {
29149 if (!j6valid
29150 [ij6])
29151 {
29152 continue;
29153 }
29154 _ij6[0]
29155 = ij6;
29156 _ij6[1]
29157 = -1;
29158 for (
29159 int iij6
29160 = ij6
29161 + 1;
29162 iij6
29163 < 2;
29164 ++iij6)
29165 {
29166 if (j6valid
29167 [iij6]
29168 && IKabs(
29169 cj6array
29170 [ij6]
29171 - cj6array
29172 [iij6])
29173 < IKFAST_SOLUTION_THRESH
29174 && IKabs(
29175 sj6array
29176 [ij6]
29177 - sj6array
29178 [iij6])
29179 < IKFAST_SOLUTION_THRESH)
29180 {
29181 j6valid
29182 [iij6]
29183 = false;
29184 _ij6[1]
29185 = iij6;
29186 break;
29187 }
29188 }
29189 j6 = j6array
29190 [ij6];
29191 cj6 = cj6array
29192 [ij6];
29193 sj6 = sj6array
29194 [ij6];
29195 {
29196 IkReal evalcond
29197 [1];
29198 evalcond
29199 [0]
29200 = ((0.85)
29201 * (IKsin(
29202 j6)));
29203 if (IKabs(
29204 evalcond
29205 [0])
29206 > IKFAST_EVALCOND_THRESH)
29207 {
29208 continue;
29209 }
29210 }
29211
29212 rotationfunction0(
29213 solutions);
29214 }
29215 }
29216 }
29217 }
29218 }
29219 } while (
29220 0);
29221 if (bgotonextstatement)
29222 {
29223 bool
29224 bgotonextstatement
29225 = true;
29226 do
29227 {
29228 evalcond
29229 [0]
29230 = ((IKabs((
29231 (-3.14159265358979)
29232 + (IKfmod(
29233 j4,
29234 6.28318530717959)))))
29235 + (IKabs(
29236 px)));
29237 evalcond
29238 [1]
29239 = -0.85;
29240 evalcond
29241 [2]
29242 = 0;
29243 evalcond
29244 [3]
29245 = ((-0.2125)
29246 + (((-1.0)
29247 * (1.0)
29248 * (py
29249 * py))));
29250 if (IKabs(
29251 evalcond
29252 [0])
29253 < 0.0000010000000000
29254 && IKabs(
29255 evalcond
29256 [1])
29257 < 0.0000010000000000
29258 && IKabs(
29259 evalcond
29260 [2])
29261 < 0.0000010000000000
29262 && IKabs(
29263 evalcond
29264 [3])
29265 < 0.0000010000000000)
29266 {
29267 bgotonextstatement
29268 = false;
29269 {
29270 IkReal j6eval
29271 [1];
29272 IkReal
29273 x2033
29274 = ((1.0)
29275 * py);
29276 sj8 = 0;
29277 cj8 = -1.0;
29278 j8 = 3.14159265358979;
29279 pz = 0;
29280 j9 = 0;
29281 sj9 = 0;
29282 cj9 = 1.0;
29283 pp = py
29284 * py;
29285 npx
29286 = (py
29287 * r10);
29288 npy
29289 = (py
29290 * r11);
29291 npz
29292 = (py
29293 * r12);
29294 rxp0_0
29295 = ((-1.0)
29296 * r20
29297 * x2033);
29298 rxp0_1
29299 = 0;
29300 rxp1_0
29301 = ((-1.0)
29302 * r21
29303 * x2033);
29304 rxp1_1
29305 = 0;
29306 rxp2_0
29307 = ((-1.0)
29308 * r22
29309 * x2033);
29310 rxp2_1
29311 = 0;
29312 px = 0;
29313 j4 = 3.14159265358979;
29314 sj4 = 0;
29315 cj4 = -1.0;
29316 rxp0_2
29317 = (py
29318 * r00);
29319 rxp1_2
29320 = (py
29321 * r01);
29322 rxp2_2
29323 = (py
29324 * r02);
29325 j6eval
29326 [0]
29327 = ((1.0)
29328 + (((1.91568587540858)
29329 * (py
29330 * py
29331 * py
29332 * py)))
29333 + (((-1.0)
29334 * (2.64633970947792)
29335 * (py
29336 * py))));
29337 if (IKabs(
29338 j6eval
29339 [0])
29340 < 0.0000010000000000)
29341 {
29342 continue; // no branches [j6]
29343 }
29344 else
29345 {
29346 {
29347 IkReal j6array
29348 [2],
29349 cj6array
29350 [2],
29351 sj6array
29352 [2];
29353 bool j6valid
29354 [2]
29355 = {false};
29356 _nj6
29357 = 2;
29358 IkReal
29359 x2034
29360 = py
29361 * py;
29362 CheckValue<IkReal> x2036 = IKatan2WithCheck(
29363 IkReal((
29364 (-0.425)
29365 + (((-0.588235294117647)
29366 * x2034)))),
29367 ((2.83333333333333)
29368 + (((-3.92156862745098)
29369 * x2034))),
29370 IKFAST_ATAN2_MAGTHRESH);
29371 if (!x2036
29372 .valid)
29373 {
29374 continue;
29375 }
29376 IkReal
29377 x2035
29378 = ((-1.0)
29379 * (x2036
29380 .value));
29381 j6array
29382 [0]
29383 = x2035;
29384 sj6array
29385 [0]
29386 = IKsin(
29387 j6array
29388 [0]);
29389 cj6array
29390 [0]
29391 = IKcos(
29392 j6array
29393 [0]);
29394 j6array
29395 [1]
29396 = ((3.14159265358979)
29397 + x2035);
29398 sj6array
29399 [1]
29400 = IKsin(
29401 j6array
29402 [1]);
29403 cj6array
29404 [1]
29405 = IKcos(
29406 j6array
29407 [1]);
29408 if (j6array
29409 [0]
29410 > IKPI)
29411 {
29412 j6array
29413 [0]
29414 -= IK2PI;
29415 }
29416 else if (
29417 j6array
29418 [0]
29419 < -IKPI)
29420 {
29421 j6array
29422 [0]
29423 += IK2PI;
29424 }
29425 j6valid
29426 [0]
29427 = true;
29428 if (j6array
29429 [1]
29430 > IKPI)
29431 {
29432 j6array
29433 [1]
29434 -= IK2PI;
29435 }
29436 else if (
29437 j6array
29438 [1]
29439 < -IKPI)
29440 {
29441 j6array
29442 [1]
29443 += IK2PI;
29444 }
29445 j6valid
29446 [1]
29447 = true;
29448 for (
29449 int ij6
29450 = 0;
29451 ij6
29452 < 2;
29453 ++ij6)
29454 {
29455 if (!j6valid
29456 [ij6])
29457 {
29458 continue;
29459 }
29460 _ij6[0]
29461 = ij6;
29462 _ij6[1]
29463 = -1;
29464 for (
29465 int iij6
29466 = ij6
29467 + 1;
29468 iij6
29469 < 2;
29470 ++iij6)
29471 {
29472 if (j6valid
29473 [iij6]
29474 && IKabs(
29475 cj6array
29476 [ij6]
29477 - cj6array
29478 [iij6])
29479 < IKFAST_SOLUTION_THRESH
29480 && IKabs(
29481 sj6array
29482 [ij6]
29483 - sj6array
29484 [iij6])
29485 < IKFAST_SOLUTION_THRESH)
29486 {
29487 j6valid
29488 [iij6]
29489 = false;
29490 _ij6[1]
29491 = iij6;
29492 break;
29493 }
29494 }
29495 j6 = j6array
29496 [ij6];
29497 cj6 = cj6array
29498 [ij6];
29499 sj6 = sj6array
29500 [ij6];
29501 {
29502 IkReal evalcond
29503 [1];
29504 evalcond
29505 [0]
29506 = ((0.85)
29507 * (IKsin(
29508 j6)));
29509 if (IKabs(
29510 evalcond
29511 [0])
29512 > IKFAST_EVALCOND_THRESH)
29513 {
29514 continue;
29515 }
29516 }
29517
29518 rotationfunction0(
29519 solutions);
29520 }
29521 }
29522 }
29523 }
29524 }
29525 } while (
29526 0);
29527 if (bgotonextstatement)
29528 {
29529 bool
29530 bgotonextstatement
29531 = true;
29532 do
29533 {
29534 evalcond
29535 [0]
29536 = ((IKabs((
29537 (-3.14159265358979)
29538 + (IKfmod(
29539 ((1.5707963267949)
29540 + j4),
29541 6.28318530717959)))))
29542 + (IKabs(
29543 py)));
29544 evalcond
29545 [1]
29546 = -0.85;
29547 evalcond
29548 [2]
29549 = 0;
29550 evalcond
29551 [3]
29552 = ((-0.2125)
29553 + (((-1.0)
29554 * (1.0)
29555 * (px
29556 * px))));
29557 if (IKabs(
29558 evalcond
29559 [0])
29560 < 0.0000010000000000
29561 && IKabs(
29562 evalcond
29563 [1])
29564 < 0.0000010000000000
29565 && IKabs(
29566 evalcond
29567 [2])
29568 < 0.0000010000000000
29569 && IKabs(
29570 evalcond
29571 [3])
29572 < 0.0000010000000000)
29573 {
29574 bgotonextstatement
29575 = false;
29576 {
29577 IkReal j6eval
29578 [1];
29579 IkReal
29580 x2037
29581 = ((1.0)
29582 * px);
29583 sj8 = 0;
29584 cj8 = -1.0;
29585 j8 = 3.14159265358979;
29586 pz = 0;
29587 j9 = 0;
29588 sj9 = 0;
29589 cj9 = 1.0;
29590 pp = px
29591 * px;
29592 npx
29593 = (px
29594 * r00);
29595 npy
29596 = (px
29597 * r01);
29598 npz
29599 = (px
29600 * r02);
29601 rxp0_0
29602 = 0;
29603 rxp0_1
29604 = (px
29605 * r20);
29606 rxp1_0
29607 = 0;
29608 rxp1_1
29609 = (px
29610 * r21);
29611 rxp2_0
29612 = 0;
29613 rxp2_1
29614 = (px
29615 * r22);
29616 py = 0;
29617 j4 = 1.5707963267949;
29618 sj4 = 1.0;
29619 cj4 = 0;
29620 rxp0_2
29621 = ((-1.0)
29622 * r10
29623 * x2037);
29624 rxp1_2
29625 = ((-1.0)
29626 * r11
29627 * x2037);
29628 rxp2_2
29629 = ((-1.0)
29630 * r12
29631 * x2037);
29632 j6eval
29633 [0]
29634 = ((1.0)
29635 + (((1.91568587540858)
29636 * (px
29637 * px
29638 * px
29639 * px)))
29640 + (((-1.0)
29641 * (2.64633970947792)
29642 * (px
29643 * px))));
29644 if (IKabs(
29645 j6eval
29646 [0])
29647 < 0.0000010000000000)
29648 {
29649 continue; // no branches [j6]
29650 }
29651 else
29652 {
29653 {
29654 IkReal j6array
29655 [2],
29656 cj6array
29657 [2],
29658 sj6array
29659 [2];
29660 bool j6valid
29661 [2]
29662 = {false};
29663 _nj6
29664 = 2;
29665 IkReal
29666 x2038
29667 = px
29668 * px;
29669 CheckValue<IkReal> x2040 = IKatan2WithCheck(
29670 IkReal((
29671 (-0.425)
29672 + (((-0.588235294117647)
29673 * x2038)))),
29674 ((2.83333333333333)
29675 + (((-3.92156862745098)
29676 * x2038))),
29677 IKFAST_ATAN2_MAGTHRESH);
29678 if (!x2040
29679 .valid)
29680 {
29681 continue;
29682 }
29683 IkReal
29684 x2039
29685 = ((-1.0)
29686 * (x2040
29687 .value));
29688 j6array
29689 [0]
29690 = x2039;
29691 sj6array
29692 [0]
29693 = IKsin(
29694 j6array
29695 [0]);
29696 cj6array
29697 [0]
29698 = IKcos(
29699 j6array
29700 [0]);
29701 j6array
29702 [1]
29703 = ((3.14159265358979)
29704 + x2039);
29705 sj6array
29706 [1]
29707 = IKsin(
29708 j6array
29709 [1]);
29710 cj6array
29711 [1]
29712 = IKcos(
29713 j6array
29714 [1]);
29715 if (j6array
29716 [0]
29717 > IKPI)
29718 {
29719 j6array
29720 [0]
29721 -= IK2PI;
29722 }
29723 else if (
29724 j6array
29725 [0]
29726 < -IKPI)
29727 {
29728 j6array
29729 [0]
29730 += IK2PI;
29731 }
29732 j6valid
29733 [0]
29734 = true;
29735 if (j6array
29736 [1]
29737 > IKPI)
29738 {
29739 j6array
29740 [1]
29741 -= IK2PI;
29742 }
29743 else if (
29744 j6array
29745 [1]
29746 < -IKPI)
29747 {
29748 j6array
29749 [1]
29750 += IK2PI;
29751 }
29752 j6valid
29753 [1]
29754 = true;
29755 for (
29756 int ij6
29757 = 0;
29758 ij6
29759 < 2;
29760 ++ij6)
29761 {
29762 if (!j6valid
29763 [ij6])
29764 {
29765 continue;
29766 }
29767 _ij6[0]
29768 = ij6;
29769 _ij6[1]
29770 = -1;
29771 for (
29772 int iij6
29773 = ij6
29774 + 1;
29775 iij6
29776 < 2;
29777 ++iij6)
29778 {
29779 if (j6valid
29780 [iij6]
29781 && IKabs(
29782 cj6array
29783 [ij6]
29784 - cj6array
29785 [iij6])
29786 < IKFAST_SOLUTION_THRESH
29787 && IKabs(
29788 sj6array
29789 [ij6]
29790 - sj6array
29791 [iij6])
29792 < IKFAST_SOLUTION_THRESH)
29793 {
29794 j6valid
29795 [iij6]
29796 = false;
29797 _ij6[1]
29798 = iij6;
29799 break;
29800 }
29801 }
29802 j6 = j6array
29803 [ij6];
29804 cj6 = cj6array
29805 [ij6];
29806 sj6 = sj6array
29807 [ij6];
29808 {
29809 IkReal evalcond
29810 [1];
29811 evalcond
29812 [0]
29813 = ((0.85)
29814 * (IKsin(
29815 j6)));
29816 if (IKabs(
29817 evalcond
29818 [0])
29819 > IKFAST_EVALCOND_THRESH)
29820 {
29821 continue;
29822 }
29823 }
29824
29825 rotationfunction0(
29826 solutions);
29827 }
29828 }
29829 }
29830 }
29831 }
29832 } while (
29833 0);
29834 if (bgotonextstatement)
29835 {
29836 bool
29837 bgotonextstatement
29838 = true;
29839 do
29840 {
29841 evalcond
29842 [0]
29843 = ((IKabs(
29844 py))
29845 + (IKabs((
29846 (-3.14159265358979)
29847 + (IKfmod(
29848 ((4.71238898038469)
29849 + j4),
29850 6.28318530717959))))));
29851 evalcond
29852 [1]
29853 = -0.85;
29854 evalcond
29855 [2]
29856 = 0;
29857 evalcond
29858 [3]
29859 = ((-0.2125)
29860 + (((-1.0)
29861 * (1.0)
29862 * (px
29863 * px))));
29864 if (IKabs(
29865 evalcond
29866 [0])
29867 < 0.0000010000000000
29868 && IKabs(
29869 evalcond
29870 [1])
29871 < 0.0000010000000000
29872 && IKabs(
29873 evalcond
29874 [2])
29875 < 0.0000010000000000
29876 && IKabs(
29877 evalcond
29878 [3])
29879 < 0.0000010000000000)
29880 {
29881 bgotonextstatement
29882 = false;
29883 {
29884 IkReal j6eval
29885 [1];
29886 IkReal
29887 x2041
29888 = ((1.0)
29889 * px);
29890 sj8 = 0;
29891 cj8 = -1.0;
29892 j8 = 3.14159265358979;
29893 pz = 0;
29894 j9 = 0;
29895 sj9 = 0;
29896 cj9 = 1.0;
29897 pp = px
29898 * px;
29899 npx
29900 = (px
29901 * r00);
29902 npy
29903 = (px
29904 * r01);
29905 npz
29906 = (px
29907 * r02);
29908 rxp0_0
29909 = 0;
29910 rxp0_1
29911 = (px
29912 * r20);
29913 rxp1_0
29914 = 0;
29915 rxp1_1
29916 = (px
29917 * r21);
29918 rxp2_0
29919 = 0;
29920 rxp2_1
29921 = (px
29922 * r22);
29923 py = 0;
29924 j4 = -1.5707963267949;
29925 sj4 = -1.0;
29926 cj4 = 0;
29927 rxp0_2
29928 = ((-1.0)
29929 * r10
29930 * x2041);
29931 rxp1_2
29932 = ((-1.0)
29933 * r11
29934 * x2041);
29935 rxp2_2
29936 = ((-1.0)
29937 * r12
29938 * x2041);
29939 j6eval
29940 [0]
29941 = ((1.0)
29942 + (((1.91568587540858)
29943 * (px
29944 * px
29945 * px
29946 * px)))
29947 + (((-1.0)
29948 * (2.64633970947792)
29949 * (px
29950 * px))));
29951 if (IKabs(
29952 j6eval
29953 [0])
29954 < 0.0000010000000000)
29955 {
29956 continue; // no branches [j6]
29957 }
29958 else
29959 {
29960 {
29961 IkReal j6array
29962 [2],
29963 cj6array
29964 [2],
29965 sj6array
29966 [2];
29967 bool j6valid
29968 [2]
29969 = {false};
29970 _nj6
29971 = 2;
29972 IkReal
29973 x2042
29974 = px
29975 * px;
29976 CheckValue<IkReal> x2044 = IKatan2WithCheck(
29977 IkReal((
29978 (-0.425)
29979 + (((-0.588235294117647)
29980 * x2042)))),
29981 ((2.83333333333333)
29982 + (((-3.92156862745098)
29983 * x2042))),
29984 IKFAST_ATAN2_MAGTHRESH);
29985 if (!x2044
29986 .valid)
29987 {
29988 continue;
29989 }
29990 IkReal
29991 x2043
29992 = ((-1.0)
29993 * (x2044
29994 .value));
29995 j6array
29996 [0]
29997 = x2043;
29998 sj6array
29999 [0]
30000 = IKsin(
30001 j6array
30002 [0]);
30003 cj6array
30004 [0]
30005 = IKcos(
30006 j6array
30007 [0]);
30008 j6array
30009 [1]
30010 = ((3.14159265358979)
30011 + x2043);
30012 sj6array
30013 [1]
30014 = IKsin(
30015 j6array
30016 [1]);
30017 cj6array
30018 [1]
30019 = IKcos(
30020 j6array
30021 [1]);
30022 if (j6array
30023 [0]
30024 > IKPI)
30025 {
30026 j6array
30027 [0]
30028 -= IK2PI;
30029 }
30030 else if (
30031 j6array
30032 [0]
30033 < -IKPI)
30034 {
30035 j6array
30036 [0]
30037 += IK2PI;
30038 }
30039 j6valid
30040 [0]
30041 = true;
30042 if (j6array
30043 [1]
30044 > IKPI)
30045 {
30046 j6array
30047 [1]
30048 -= IK2PI;
30049 }
30050 else if (
30051 j6array
30052 [1]
30053 < -IKPI)
30054 {
30055 j6array
30056 [1]
30057 += IK2PI;
30058 }
30059 j6valid
30060 [1]
30061 = true;
30062 for (
30063 int ij6
30064 = 0;
30065 ij6
30066 < 2;
30067 ++ij6)
30068 {
30069 if (!j6valid
30070 [ij6])
30071 {
30072 continue;
30073 }
30074 _ij6[0]
30075 = ij6;
30076 _ij6[1]
30077 = -1;
30078 for (
30079 int iij6
30080 = ij6
30081 + 1;
30082 iij6
30083 < 2;
30084 ++iij6)
30085 {
30086 if (j6valid
30087 [iij6]
30088 && IKabs(
30089 cj6array
30090 [ij6]
30091 - cj6array
30092 [iij6])
30093 < IKFAST_SOLUTION_THRESH
30094 && IKabs(
30095 sj6array
30096 [ij6]
30097 - sj6array
30098 [iij6])
30099 < IKFAST_SOLUTION_THRESH)
30100 {
30101 j6valid
30102 [iij6]
30103 = false;
30104 _ij6[1]
30105 = iij6;
30106 break;
30107 }
30108 }
30109 j6 = j6array
30110 [ij6];
30111 cj6 = cj6array
30112 [ij6];
30113 sj6 = sj6array
30114 [ij6];
30115 {
30116 IkReal evalcond
30117 [1];
30118 evalcond
30119 [0]
30120 = ((0.85)
30121 * (IKsin(
30122 j6)));
30123 if (IKabs(
30124 evalcond
30125 [0])
30126 > IKFAST_EVALCOND_THRESH)
30127 {
30128 continue;
30129 }
30130 }
30131
30132 rotationfunction0(
30133 solutions);
30134 }
30135 }
30136 }
30137 }
30138 }
30139 } while (
30140 0);
30141 if (bgotonextstatement)
30142 {
30143 bool
30144 bgotonextstatement
30145 = true;
30146 do
30147 {
30148 if (1)
30149 {
30150 bgotonextstatement
30151 = false;
30152 continue; // branch miss [j6]
30153 }
30154 } while (
30155 0);
30156 if (bgotonextstatement)
30157 {
30158 }
30159 }
30160 }
30161 }
30162 }
30163 }
30164 }
30165 }
30166 else
30167 {
30168 {
30169 IkReal j6array
30170 [1],
30171 cj6array
30172 [1],
30173 sj6array
30174 [1];
30175 bool j6valid
30176 [1]
30177 = {false};
30178 _nj6 = 1;
30179 IkReal
30180 x2045
30181 = (cj4
30182 * px);
30183 IkReal
30184 x2046
30185 = (py
30186 * sj4);
30187 IkReal
30188 x2047
30189 = px
30190 * px;
30191 IkReal
30192 x2048
30193 = py
30194 * py;
30195 CheckValue<
30196 IkReal>
30197 x2049
30198 = IKPowWithIntegerCheck(
30199 ((((20.0)
30200 * x2046))
30201 + (((20.0)
30202 * x2045))),
30203 -1);
30204 if (!x2049
30205 .valid)
30206 {
30207 continue;
30208 }
30209 CheckValue<IkReal> x2050 = IKPowWithIntegerCheck(
30210 ((((-8.5)
30211 * x2046))
30212 + (((-1.0)
30213 * (11.7647058823529)
30214 * cj4
30215 * (px
30216 * px
30217 * px)))
30218 + (((-8.5)
30219 * x2045))
30220 + (((-11.7647058823529)
30221 * x2045
30222 * x2048))
30223 + (((-11.7647058823529)
30224 * x2046
30225 * x2047))
30226 + (((-1.0)
30227 * (11.7647058823529)
30228 * sj4
30229 * (py
30230 * py
30231 * py)))),
30232 -1);
30233 if (!x2050
30234 .valid)
30235 {
30236 continue;
30237 }
30238 if (IKabs((
30239 (17.0)
30240 * (x2049
30241 .value)))
30242 < IKFAST_ATAN2_MAGTHRESH
30243 && IKabs((
30244 (x2050
30245 .value)
30246 * (((-48.1666666666667)
30247 + (((66.6666666666667)
30248 * x2047))
30249 + (((66.6666666666667)
30250 * x2048))))))
30251 < IKFAST_ATAN2_MAGTHRESH
30252 && IKabs(
30253 IKsqr((
30254 (17.0)
30255 * (x2049
30256 .value)))
30257 + IKsqr((
30258 (x2050
30259 .value)
30260 * (((-48.1666666666667)
30261 + (((66.6666666666667)
30262 * x2047))
30263 + (((66.6666666666667)
30264 * x2048))))))
30265 - 1)
30266 <= IKFAST_SINCOS_THRESH)
30267 continue;
30268 j6array[0] = IKatan2(
30269 ((17.0)
30270 * (x2049
30271 .value)),
30272 ((x2050
30273 .value)
30274 * (((-48.1666666666667)
30275 + (((66.6666666666667)
30276 * x2047))
30277 + (((66.6666666666667)
30278 * x2048))))));
30279 sj6array[0] = IKsin(
30280 j6array
30281 [0]);
30282 cj6array[0] = IKcos(
30283 j6array
30284 [0]);
30285 if (j6array
30286 [0]
30287 > IKPI)
30288 {
30289 j6array
30290 [0]
30291 -= IK2PI;
30292 }
30293 else if (
30294 j6array
30295 [0]
30296 < -IKPI)
30297 {
30298 j6array
30299 [0]
30300 += IK2PI;
30301 }
30302 j6valid[0]
30303 = true;
30304 for (
30305 int ij6
30306 = 0;
30307 ij6
30308 < 1;
30309 ++ij6)
30310 {
30311 if (!j6valid
30312 [ij6])
30313 {
30314 continue;
30315 }
30316 _ij6[0]
30317 = ij6;
30318 _ij6[1]
30319 = -1;
30320 for (
30321 int iij6
30322 = ij6
30323 + 1;
30324 iij6
30325 < 1;
30326 ++iij6)
30327 {
30328 if (j6valid
30329 [iij6]
30330 && IKabs(
30331 cj6array
30332 [ij6]
30333 - cj6array
30334 [iij6])
30335 < IKFAST_SOLUTION_THRESH
30336 && IKabs(
30337 sj6array
30338 [ij6]
30339 - sj6array
30340 [iij6])
30341 < IKFAST_SOLUTION_THRESH)
30342 {
30343 j6valid
30344 [iij6]
30345 = false;
30346 _ij6[1]
30347 = iij6;
30348 break;
30349 }
30350 }
30351 j6 = j6array
30352 [ij6];
30353 cj6 = cj6array
30354 [ij6];
30355 sj6 = sj6array
30356 [ij6];
30357 {
30358 IkReal evalcond
30359 [5];
30360 IkReal
30361 x2051
30362 = IKcos(
30363 j6);
30364 IkReal
30365 x2052
30366 = (cj4
30367 * px);
30368 IkReal
30369 x2053
30370 = (x2051
30371 * x2052);
30372 IkReal
30373 x2054
30374 = (py
30375 * sj4);
30376 IkReal
30377 x2055
30378 = (x2051
30379 * x2054);
30380 IkReal
30381 x2056
30382 = IKsin(
30383 j6);
30384 IkReal
30385 x2057
30386 = (x2052
30387 * x2056);
30388 IkReal
30389 x2058
30390 = (x2054
30391 * x2056);
30392 IkReal
30393 x2059
30394 = px
30395 * px;
30396 IkReal
30397 x2060
30398 = ((3.92156862745098)
30399 * x2056);
30400 IkReal
30401 x2061
30402 = ((0.588235294117647)
30403 * x2051);
30404 IkReal
30405 x2062
30406 = py
30407 * py;
30408 evalcond
30409 [0]
30410 = (x2055
30411 + x2053);
30412 evalcond
30413 [1]
30414 = ((-0.85)
30415 + x2057
30416 + x2058);
30417 evalcond
30418 [2]
30419 = ((((0.85)
30420 * x2056))
30421 + (((-1.0)
30422 * x2052))
30423 + (((-1.0)
30424 * x2054)));
30425 evalcond
30426 [3]
30427 = ((((-1.0)
30428 * x2061
30429 * x2062))
30430 + (((-1.0)
30431 * x2059
30432 * x2061))
30433 + (((-0.425)
30434 * x2051))
30435 + (((-1.0)
30436 * x2059
30437 * x2060))
30438 + (((2.83333333333333)
30439 * x2056))
30440 + (((-1.0)
30441 * x2060
30442 * x2062)));
30443 evalcond
30444 [4]
30445 = ((-0.2125)
30446 + (((1.1)
30447 * x2057))
30448 + (((-1.0)
30449 * x2059))
30450 + (((-0.09)
30451 * x2055))
30452 + (((1.1)
30453 * x2058))
30454 + (((-1.0)
30455 * x2062))
30456 + (((-0.09)
30457 * x2053)));
30458 if (IKabs(
30459 evalcond
30460 [0])
30461 > IKFAST_EVALCOND_THRESH
30462 || IKabs(
30463 evalcond
30464 [1])
30465 > IKFAST_EVALCOND_THRESH
30466 || IKabs(
30467 evalcond
30468 [2])
30469 > IKFAST_EVALCOND_THRESH
30470 || IKabs(
30471 evalcond
30472 [3])
30473 > IKFAST_EVALCOND_THRESH
30474 || IKabs(
30475 evalcond
30476 [4])
30477 > IKFAST_EVALCOND_THRESH)
30478 {
30479 continue;
30480 }
30481 }
30482
30483 rotationfunction0(
30484 solutions);
30485 }
30486 }
30487 }
30488 }
30489 }
30490 else
30491 {
30492 {
30493 IkReal j6array
30494 [1],
30495 cj6array
30496 [1],
30497 sj6array
30498 [1];
30499 bool
30500 j6valid[1]
30501 = {false};
30502 _nj6 = 1;
30503 IkReal x2063
30504 = (cj4
30505 * px);
30506 IkReal x2064
30507 = (py
30508 * sj4);
30509 IkReal x2065
30510 = px * px;
30511 IkReal x2066
30512 = py * py;
30513 CheckValue<
30514 IkReal>
30515 x2067
30516 = IKPowWithIntegerCheck(
30517 ((-7.225)
30518 + (((-10.0)
30519 * x2066))
30520 + (((-10.0)
30521 * x2065))),
30522 -1);
30523 if (!x2067
30524 .valid)
30525 {
30526 continue;
30527 }
30528 if (IKabs((
30529 (((1.17647058823529)
30530 * x2063))
30531 + (((1.17647058823529)
30532 * x2064))))
30533 < IKFAST_ATAN2_MAGTHRESH
30534 && IKabs((
30535 (x2067
30536 .value)
30537 * (((((-56.6666666666667)
30538 * x2064))
30539 + (((78.4313725490196)
30540 * sj4
30541 * (py
30542 * py
30543 * py)))
30544 + (((78.4313725490196)
30545 * cj4
30546 * (px
30547 * px
30548 * px)))
30549 + (((78.4313725490196)
30550 * x2063
30551 * x2066))
30552 + (((78.4313725490196)
30553 * x2064
30554 * x2065))
30555 + (((-56.6666666666667)
30556 * x2063))))))
30557 < IKFAST_ATAN2_MAGTHRESH
30558 && IKabs(
30559 IKsqr((
30560 (((1.17647058823529)
30561 * x2063))
30562 + (((1.17647058823529)
30563 * x2064))))
30564 + IKsqr((
30565 (x2067
30566 .value)
30567 * (((((-56.6666666666667)
30568 * x2064))
30569 + (((78.4313725490196)
30570 * sj4
30571 * (py
30572 * py
30573 * py)))
30574 + (((78.4313725490196)
30575 * cj4
30576 * (px
30577 * px
30578 * px)))
30579 + (((78.4313725490196)
30580 * x2063
30581 * x2066))
30582 + (((78.4313725490196)
30583 * x2064
30584 * x2065))
30585 + (((-56.6666666666667)
30586 * x2063))))))
30587 - 1)
30588 <= IKFAST_SINCOS_THRESH)
30589 continue;
30590 j6array[0] = IKatan2(
30591 ((((1.17647058823529)
30592 * x2063))
30593 + (((1.17647058823529)
30594 * x2064))),
30595 ((x2067
30596 .value)
30597 * (((((-56.6666666666667)
30598 * x2064))
30599 + (((78.4313725490196)
30600 * sj4
30601 * (py
30602 * py
30603 * py)))
30604 + (((78.4313725490196)
30605 * cj4
30606 * (px
30607 * px
30608 * px)))
30609 + (((78.4313725490196)
30610 * x2063
30611 * x2066))
30612 + (((78.4313725490196)
30613 * x2064
30614 * x2065))
30615 + (((-56.6666666666667)
30616 * x2063))))));
30617 sj6array[0]
30618 = IKsin(
30619 j6array
30620 [0]);
30621 cj6array[0]
30622 = IKcos(
30623 j6array
30624 [0]);
30625 if (j6array[0]
30626 > IKPI)
30627 {
30628 j6array[0]
30629 -= IK2PI;
30630 }
30631 else if (
30632 j6array[0]
30633 < -IKPI)
30634 {
30635 j6array[0]
30636 += IK2PI;
30637 }
30638 j6valid[0]
30639 = true;
30640 for (int ij6
30641 = 0;
30642 ij6 < 1;
30643 ++ij6)
30644 {
30645 if (!j6valid
30646 [ij6])
30647 {
30648 continue;
30649 }
30650 _ij6[0]
30651 = ij6;
30652 _ij6[1]
30653 = -1;
30654 for (
30655 int iij6
30656 = ij6
30657 + 1;
30658 iij6
30659 < 1;
30660 ++iij6)
30661 {
30662 if (j6valid
30663 [iij6]
30664 && IKabs(
30665 cj6array
30666 [ij6]
30667 - cj6array
30668 [iij6])
30669 < IKFAST_SOLUTION_THRESH
30670 && IKabs(
30671 sj6array
30672 [ij6]
30673 - sj6array
30674 [iij6])
30675 < IKFAST_SOLUTION_THRESH)
30676 {
30677 j6valid
30678 [iij6]
30679 = false;
30680 _ij6[1]
30681 = iij6;
30682 break;
30683 }
30684 }
30685 j6 = j6array
30686 [ij6];
30687 cj6 = cj6array
30688 [ij6];
30689 sj6 = sj6array
30690 [ij6];
30691 {
30692 IkReal evalcond
30693 [5];
30694 IkReal
30695 x2068
30696 = IKcos(
30697 j6);
30698 IkReal
30699 x2069
30700 = (cj4
30701 * px);
30702 IkReal
30703 x2070
30704 = (x2068
30705 * x2069);
30706 IkReal
30707 x2071
30708 = (py
30709 * sj4);
30710 IkReal
30711 x2072
30712 = (x2068
30713 * x2071);
30714 IkReal
30715 x2073
30716 = IKsin(
30717 j6);
30718 IkReal
30719 x2074
30720 = (x2069
30721 * x2073);
30722 IkReal
30723 x2075
30724 = (x2071
30725 * x2073);
30726 IkReal
30727 x2076
30728 = px
30729 * px;
30730 IkReal
30731 x2077
30732 = ((3.92156862745098)
30733 * x2073);
30734 IkReal
30735 x2078
30736 = ((0.588235294117647)
30737 * x2068);
30738 IkReal
30739 x2079
30740 = py
30741 * py;
30742 evalcond
30743 [0]
30744 = (x2070
30745 + x2072);
30746 evalcond
30747 [1]
30748 = ((-0.85)
30749 + x2075
30750 + x2074);
30751 evalcond
30752 [2]
30753 = ((((-1.0)
30754 * x2071))
30755 + (((0.85)
30756 * x2073))
30757 + (((-1.0)
30758 * x2069)));
30759 evalcond
30760 [3]
30761 = ((((-1.0)
30762 * x2076
30763 * x2077))
30764 + (((-1.0)
30765 * x2076
30766 * x2078))
30767 + (((-0.425)
30768 * x2068))
30769 + (((-1.0)
30770 * x2078
30771 * x2079))
30772 + (((2.83333333333333)
30773 * x2073))
30774 + (((-1.0)
30775 * x2077
30776 * x2079)));
30777 evalcond
30778 [4]
30779 = ((-0.2125)
30780 + (((-1.0)
30781 * x2079))
30782 + (((-1.0)
30783 * x2076))
30784 + (((1.1)
30785 * x2075))
30786 + (((-0.09)
30787 * x2070))
30788 + (((1.1)
30789 * x2074))
30790 + (((-0.09)
30791 * x2072)));
30792 if (IKabs(
30793 evalcond
30794 [0])
30795 > IKFAST_EVALCOND_THRESH
30796 || IKabs(
30797 evalcond
30798 [1])
30799 > IKFAST_EVALCOND_THRESH
30800 || IKabs(
30801 evalcond
30802 [2])
30803 > IKFAST_EVALCOND_THRESH
30804 || IKabs(
30805 evalcond
30806 [3])
30807 > IKFAST_EVALCOND_THRESH
30808 || IKabs(
30809 evalcond
30810 [4])
30811 > IKFAST_EVALCOND_THRESH)
30812 {
30813 continue;
30814 }
30815 }
30816
30817 rotationfunction0(
30818 solutions);
30819 }
30820 }
30821 }
30822 }
30823 }
30824 else
30825 {
30826 {
30827 IkReal j6array[1],
30828 cj6array[1],
30829 sj6array[1];
30830 bool j6valid[1]
30831 = {false};
30832 _nj6 = 1;
30833 IkReal x2080
30834 = (cj4 * px);
30835 IkReal x2081
30836 = (py * sj4);
30837 IkReal x2082
30838 = px * px;
30839 IkReal x2083
30840 = py * py;
30841 IkReal x2084
30842 = ((1.29411764705882)
30843 * (cj4
30844 * cj4));
30845 CheckValue<IkReal>
30846 x2085
30847 = IKPowWithIntegerCheck(
30848 ((((-0.09)
30849 * x2080))
30850 + (((-0.09)
30851 * x2081))),
30852 -1);
30853 if (!x2085.valid)
30854 {
30855 continue;
30856 }
30857 if (IKabs((
30858 (((1.17647058823529)
30859 * x2080))
30860 + (((1.17647058823529)
30861 * x2081))))
30862 < IKFAST_ATAN2_MAGTHRESH
30863 && IKabs((
30864 (x2085
30865 .value)
30866 * (((0.2125)
30867 + (((-2.58823529411765)
30868 * cj4
30869 * px
30870 * x2081))
30871 + (((-0.294117647058824)
30872 * x2083))
30873 + ((x2083
30874 * x2084))
30875 + (((-1.0)
30876 * x2082
30877 * x2084))
30878 + x2082))))
30879 < IKFAST_ATAN2_MAGTHRESH
30880 && IKabs(
30881 IKsqr((
30882 (((1.17647058823529)
30883 * x2080))
30884 + (((1.17647058823529)
30885 * x2081))))
30886 + IKsqr((
30887 (x2085
30888 .value)
30889 * (((0.2125)
30890 + (((-2.58823529411765)
30891 * cj4
30892 * px
30893 * x2081))
30894 + (((-0.294117647058824)
30895 * x2083))
30896 + ((x2083
30897 * x2084))
30898 + (((-1.0)
30899 * x2082
30900 * x2084))
30901 + x2082))))
30902 - 1)
30903 <= IKFAST_SINCOS_THRESH)
30904 continue;
30905 j6array[0] = IKatan2(
30906 ((((1.17647058823529)
30907 * x2080))
30908 + (((1.17647058823529)
30909 * x2081))),
30910 ((x2085.value)
30911 * (((0.2125)
30912 + (((-2.58823529411765)
30913 * cj4
30914 * px
30915 * x2081))
30916 + (((-0.294117647058824)
30917 * x2083))
30918 + ((x2083
30919 * x2084))
30920 + (((-1.0)
30921 * x2082
30922 * x2084))
30923 + x2082))));
30924 sj6array[0]
30925 = IKsin(
30926 j6array
30927 [0]);
30928 cj6array[0]
30929 = IKcos(
30930 j6array
30931 [0]);
30932 if (j6array[0]
30933 > IKPI)
30934 {
30935 j6array[0]
30936 -= IK2PI;
30937 }
30938 else if (
30939 j6array[0]
30940 < -IKPI)
30941 {
30942 j6array[0]
30943 += IK2PI;
30944 }
30945 j6valid[0] = true;
30946 for (int ij6 = 0;
30947 ij6 < 1;
30948 ++ij6)
30949 {
30950 if (!j6valid
30951 [ij6])
30952 {
30953 continue;
30954 }
30955 _ij6[0] = ij6;
30956 _ij6[1] = -1;
30957 for (int iij6
30958 = ij6 + 1;
30959 iij6 < 1;
30960 ++iij6)
30961 {
30962 if (j6valid
30963 [iij6]
30964 && IKabs(
30965 cj6array
30966 [ij6]
30967 - cj6array
30968 [iij6])
30969 < IKFAST_SOLUTION_THRESH
30970 && IKabs(
30971 sj6array
30972 [ij6]
30973 - sj6array
30974 [iij6])
30975 < IKFAST_SOLUTION_THRESH)
30976 {
30977 j6valid
30978 [iij6]
30979 = false;
30980 _ij6[1]
30981 = iij6;
30982 break;
30983 }
30984 }
30985 j6 = j6array
30986 [ij6];
30987 cj6 = cj6array
30988 [ij6];
30989 sj6 = sj6array
30990 [ij6];
30991 {
30992 IkReal
30993 evalcond
30994 [5];
30995 IkReal x2086
30996 = IKcos(
30997 j6);
30998 IkReal x2087
30999 = (cj4
31000 * px);
31001 IkReal x2088
31002 = (x2086
31003 * x2087);
31004 IkReal x2089
31005 = (py
31006 * sj4);
31007 IkReal x2090
31008 = (x2086
31009 * x2089);
31010 IkReal x2091
31011 = IKsin(
31012 j6);
31013 IkReal x2092
31014 = (x2087
31015 * x2091);
31016 IkReal x2093
31017 = (x2089
31018 * x2091);
31019 IkReal x2094
31020 = px * px;
31021 IkReal x2095
31022 = ((3.92156862745098)
31023 * x2091);
31024 IkReal x2096
31025 = ((0.588235294117647)
31026 * x2086);
31027 IkReal x2097
31028 = py * py;
31029 evalcond[0]
31030 = (x2090
31031 + x2088);
31032 evalcond[1]
31033 = ((-0.85)
31034 + x2093
31035 + x2092);
31036 evalcond[2]
31037 = ((((-1.0)
31038 * x2089))
31039 + (((-1.0)
31040 * x2087))
31041 + (((0.85)
31042 * x2091)));
31043 evalcond[3]
31044 = ((((-1.0)
31045 * x2095
31046 * x2097))
31047 + (((-1.0)
31048 * x2094
31049 * x2096))
31050 + (((-1.0)
31051 * x2096
31052 * x2097))
31053 + (((2.83333333333333)
31054 * x2091))
31055 + (((-1.0)
31056 * x2094
31057 * x2095))
31058 + (((-0.425)
31059 * x2086)));
31060 evalcond[4]
31061 = ((-0.2125)
31062 + (((-1.0)
31063 * x2094))
31064 + (((1.1)
31065 * x2093))
31066 + (((1.1)
31067 * x2092))
31068 + (((-1.0)
31069 * x2097))
31070 + (((-0.09)
31071 * x2090))
31072 + (((-0.09)
31073 * x2088)));
31074 if (IKabs(
31075 evalcond
31076 [0])
31077 > IKFAST_EVALCOND_THRESH
31078 || IKabs(
31079 evalcond
31080 [1])
31081 > IKFAST_EVALCOND_THRESH
31082 || IKabs(
31083 evalcond
31084 [2])
31085 > IKFAST_EVALCOND_THRESH
31086 || IKabs(
31087 evalcond
31088 [3])
31089 > IKFAST_EVALCOND_THRESH
31090 || IKabs(
31091 evalcond
31092 [4])
31093 > IKFAST_EVALCOND_THRESH)
31094 {
31095 continue;
31096 }
31097 }
31098
31099 rotationfunction0(
31100 solutions);
31101 }
31102 }
31103 }
31104 }
31105 }
31106 } while (0);
31107 if (bgotonextstatement)
31108 {
31109 bool bgotonextstatement
31110 = true;
31111 do
31112 {
31113 if (1)
31114 {
31115 bgotonextstatement
31116 = false;
31117 continue; // branch
31118 // miss [j6]
31119 }
31120 } while (0);
31121 if (bgotonextstatement)
31122 {
31123 }
31124 }
31125 }
31126 }
31127 else
31128 {
31129 {
31130 IkReal j6array[1],
31131 cj6array[1],
31132 sj6array[1];
31133 bool j6valid[1] = {false};
31134 _nj6 = 1;
31135 IkReal x2098 = (cj4 * px);
31136 IkReal x2099 = (py * sj4);
31137 IkReal x2100
31138 = ((0.108264705882353)
31139 * cj9);
31140 IkReal x2101
31141 = ((0.588235294117647)
31142 * pp);
31143 IkReal x2102 = (cj9 * pp);
31144 IkReal x2103 = (cj9 * sj9);
31145 IkReal x2104 = (pp * sj9);
31146 IkReal x2105 = cj9 * cj9;
31147 IkReal x2106 = ((1.0) * pz);
31148 CheckValue<IkReal> x2107
31149 = IKPowWithIntegerCheck(
31150 IKsign((
31151 (((-1.0) * x2099
31152 * x2100))
31153 + (((-1.0)
31154 * x2098
31155 * x2101))
31156 + (((-1.0)
31157 * (1.32323529411765)
31158 * cj9 * pz))
31159 + (((3.92156862745098)
31160 * pp * pz))
31161 + (((-1.0)
31162 * x2098
31163 * x2100))
31164 + (((-1.0)
31165 * (1.51009803921569)
31166 * pz))
31167 + (((-0.316735294117647)
31168 * x2098))
31169 + (((-1.0)
31170 * x2099
31171 * x2101))
31172 + (((-0.316735294117647)
31173 * x2099)))),
31174 -1);
31175 if (!x2107.valid)
31176 {
31177 continue;
31178 }
31179 CheckValue<IkReal> x2108 = IKatan2WithCheck(
31180 IkReal((
31181 (-0.174204411764706)
31182 + (((-0.0324794117647059)
31183 * x2105))
31184 + (pz * pz)
31185 + (((-0.0264705882352941)
31186 * x2104))
31187 + (((-1.0)
31188 * (0.154566176470588)
31189 * cj9))
31190 + (((-1.0)
31191 * (0.323529411764706)
31192 * pp))
31193 + (((-0.00487191176470588)
31194 * x2103))
31195 + (((-1.0)
31196 * (0.0142530882352941)
31197 * sj9))
31198 + (((-0.176470588235294)
31199 * x2102)))),
31200 ((-0.830553921568627)
31201 + (((-1.0)
31202 * (0.0679544117647059)
31203 * sj9))
31204 + (((-1.0)
31205 * (1.18080882352941)
31206 * cj9))
31207 + (((-0.396970588235294)
31208 * x2105))
31209 + (((-1.0) * x2099
31210 * x2106))
31211 + (((-0.0595455882352941)
31212 * x2103))
31213 + (((2.15686274509804)
31214 * pp))
31215 + (((-1.0) * x2098
31216 * x2106))
31217 + (((0.176470588235294)
31218 * x2104))
31219 + (((1.17647058823529)
31220 * x2102))),
31221 IKFAST_ATAN2_MAGTHRESH);
31222 if (!x2108.valid)
31223 {
31224 continue;
31225 }
31226 j6array[0]
31227 = ((-1.5707963267949)
31228 + (((1.5707963267949)
31229 * (x2107.value)))
31230 + (x2108.value));
31231 sj6array[0]
31232 = IKsin(j6array[0]);
31233 cj6array[0]
31234 = IKcos(j6array[0]);
31235 if (j6array[0] > IKPI)
31236 {
31237 j6array[0] -= IK2PI;
31238 }
31239 else if (j6array[0] < -IKPI)
31240 {
31241 j6array[0] += IK2PI;
31242 }
31243 j6valid[0] = true;
31244 for (int ij6 = 0; ij6 < 1;
31245 ++ij6)
31246 {
31247 if (!j6valid[ij6])
31248 {
31249 continue;
31250 }
31251 _ij6[0] = ij6;
31252 _ij6[1] = -1;
31253 for (int iij6 = ij6 + 1;
31254 iij6 < 1;
31255 ++iij6)
31256 {
31257 if (j6valid[iij6]
31258 && IKabs(
31259 cj6array[ij6]
31260 - cj6array
31261 [iij6])
31262 < IKFAST_SOLUTION_THRESH
31263 && IKabs(
31264 sj6array[ij6]
31265 - sj6array
31266 [iij6])
31267 < IKFAST_SOLUTION_THRESH)
31268 {
31269 j6valid[iij6] = false;
31270 _ij6[1] = iij6;
31271 break;
31272 }
31273 }
31274 j6 = j6array[ij6];
31275 cj6 = cj6array[ij6];
31276 sj6 = sj6array[ij6];
31277 {
31278 IkReal evalcond[5];
31279 IkReal x2109
31280 = ((0.3) * cj9);
31281 IkReal x2110
31282 = ((0.045) * sj9);
31283 IkReal x2111
31284 = IKcos(j6);
31285 IkReal x2112
31286 = (pz * x2111);
31287 IkReal x2113
31288 = IKsin(j6);
31289 IkReal x2114
31290 = (cj4 * px);
31291 IkReal x2115
31292 = (x2113 * x2114);
31293 IkReal x2116
31294 = (py * sj4);
31295 IkReal x2117
31296 = (x2113 * x2116);
31297 IkReal x2118
31298 = ((0.045) * cj9);
31299 IkReal x2119
31300 = ((0.3) * sj9);
31301 IkReal x2120
31302 = (pz * x2113);
31303 IkReal x2121
31304 = (x2111 * x2114);
31305 IkReal x2122
31306 = (x2111 * x2116);
31307 evalcond[0]
31308 = ((-0.55) + x2115
31309 + x2117 + x2112
31310 + (((-1.0)
31311 * x2110))
31312 + (((-1.0)
31313 * x2109)));
31314 evalcond[1]
31315 = ((0.045) + x2119
31316 + x2121 + x2122
31317 + (((-1.0)
31318 * x2118))
31319 + (((-1.0)
31320 * x2120)));
31321 evalcond[2]
31322 = ((((-3.92156862745098)
31323 * pp * x2113))
31324 + pz
31325 + (((-0.316735294117647)
31326 * x2111))
31327 + (((-0.588235294117647)
31328 * pp
31329 * x2111))
31330 + (((1.32323529411765)
31331 * cj9
31332 * x2113))
31333 + (((-0.108264705882353)
31334 * cj9
31335 * x2111))
31336 + (((1.51009803921569)
31337 * x2113)));
31338 evalcond[3]
31339 = ((((-1.0)
31340 * x2116))
31341 + (((-1.0)
31342 * x2114))
31343 + ((x2111
31344 * x2118))
31345 + (((-1.0)
31346 * x2111
31347 * x2119))
31348 + ((x2110
31349 * x2113))
31350 + (((0.55)
31351 * x2113))
31352 + (((-0.045)
31353 * x2111))
31354 + ((x2109
31355 * x2113)));
31356 evalcond[4]
31357 = ((-0.2125)
31358 + (((1.1)
31359 * x2115))
31360 + (((1.1)
31361 * x2112))
31362 + (((1.1)
31363 * x2117))
31364 + (((-0.09)
31365 * x2122))
31366 + (((-1.0)
31367 * (1.0)
31368 * pp))
31369 + (((0.09)
31370 * x2120))
31371 + (((-0.09)
31372 * x2121)));
31373 if (IKabs(evalcond[0])
31374 > IKFAST_EVALCOND_THRESH
31375 || IKabs(
31376 evalcond[1])
31377 > IKFAST_EVALCOND_THRESH
31378 || IKabs(
31379 evalcond[2])
31380 > IKFAST_EVALCOND_THRESH
31381 || IKabs(
31382 evalcond[3])
31383 > IKFAST_EVALCOND_THRESH
31384 || IKabs(
31385 evalcond[4])
31386 > IKFAST_EVALCOND_THRESH)
31387 {
31388 continue;
31389 }
31390 }
31391
31392 rotationfunction0(
31393 solutions);
31394 }
31395 }
31396 }
31397 }
31398 }
31399 else
31400 {
31401 {
31402 IkReal j6array[1], cj6array[1],
31403 sj6array[1];
31404 bool j6valid[1] = {false};
31405 _nj6 = 1;
31406 IkReal x2123
31407 = ((0.045) * cj4 * px);
31408 IkReal x2124
31409 = ((0.045) * py * sj4);
31410 IkReal x2125 = ((0.3) * sj9);
31411 IkReal x2126 = (cj4 * px);
31412 IkReal x2127 = (py * sj4);
31413 IkReal x2128 = (cj9 * sj9);
31414 IkReal x2129 = cj9 * cj9;
31415 IkReal x2130 = ((1.0) * pz);
31416 IkReal x2131 = py * py;
31417 IkReal x2132 = cj4 * cj4;
31418 CheckValue<IkReal> x2133
31419 = IKPowWithIntegerCheck(
31420 IKsign((
31421 (((-1.0) * (0.55)
31422 * pz))
31423 + (((-1.0) * x2125
31424 * x2127))
31425 + (((-1.0) * x2125
31426 * x2126))
31427 + ((cj9 * x2123))
31428 + (((-1.0) * x2123))
31429 + ((cj9 * x2124))
31430 + (((-1.0) * x2124))
31431 + (((-1.0) * (0.045)
31432 * pz * sj9))
31433 + (((-1.0) * (0.3)
31434 * cj9 * pz)))),
31435 -1);
31436 if (!x2133.valid)
31437 {
31438 continue;
31439 }
31440 CheckValue<IkReal> x2134
31441 = IKatan2WithCheck(
31442 IkReal(
31443 ((-0.03825)
31444 + (((-0.087975)
31445 * x2128))
31446 + (((0.027)
31447 * x2129))
31448 + (((-1.0)
31449 * (0.167025)
31450 * sj9))
31451 + (((-1.0) * x2126
31452 * x2130))
31453 + (((-1.0) * x2127
31454 * x2130))
31455 + (((0.01125)
31456 * cj9)))),
31457 ((-0.304525)
31458 + ((x2132 * (px * px)))
31459 + (((-1.0) * (0.0495)
31460 * sj9))
31461 + (((-0.027) * x2128))
31462 + (((-1.0) * x2131
31463 * x2132))
31464 + (((-1.0) * (0.33)
31465 * cj9))
31466 + (((-0.087975)
31467 * x2129))
31468 + (((2.0) * cj4 * px
31469 * x2127))
31470 + x2131),
31471 IKFAST_ATAN2_MAGTHRESH);
31472 if (!x2134.valid)
31473 {
31474 continue;
31475 }
31476 j6array[0]
31477 = ((-1.5707963267949)
31478 + (((1.5707963267949)
31479 * (x2133.value)))
31480 + (x2134.value));
31481 sj6array[0] = IKsin(j6array[0]);
31482 cj6array[0] = IKcos(j6array[0]);
31483 if (j6array[0] > IKPI)
31484 {
31485 j6array[0] -= IK2PI;
31486 }
31487 else if (j6array[0] < -IKPI)
31488 {
31489 j6array[0] += IK2PI;
31490 }
31491 j6valid[0] = true;
31492 for (int ij6 = 0; ij6 < 1;
31493 ++ij6)
31494 {
31495 if (!j6valid[ij6])
31496 {
31497 continue;
31498 }
31499 _ij6[0] = ij6;
31500 _ij6[1] = -1;
31501 for (int iij6 = ij6 + 1;
31502 iij6 < 1;
31503 ++iij6)
31504 {
31505 if (j6valid[iij6]
31506 && IKabs(
31507 cj6array[ij6]
31508 - cj6array[iij6])
31509 < IKFAST_SOLUTION_THRESH
31510 && IKabs(
31511 sj6array[ij6]
31512 - sj6array[iij6])
31513 < IKFAST_SOLUTION_THRESH)
31514 {
31515 j6valid[iij6] = false;
31516 _ij6[1] = iij6;
31517 break;
31518 }
31519 }
31520 j6 = j6array[ij6];
31521 cj6 = cj6array[ij6];
31522 sj6 = sj6array[ij6];
31523 {
31524 IkReal evalcond[5];
31525 IkReal x2135
31526 = ((0.3) * cj9);
31527 IkReal x2136
31528 = ((0.045) * sj9);
31529 IkReal x2137 = IKcos(j6);
31530 IkReal x2138 = (pz * x2137);
31531 IkReal x2139 = IKsin(j6);
31532 IkReal x2140 = (cj4 * px);
31533 IkReal x2141
31534 = (x2139 * x2140);
31535 IkReal x2142 = (py * sj4);
31536 IkReal x2143
31537 = (x2139 * x2142);
31538 IkReal x2144
31539 = ((0.045) * cj9);
31540 IkReal x2145
31541 = ((0.3) * sj9);
31542 IkReal x2146 = (pz * x2139);
31543 IkReal x2147
31544 = (x2137 * x2140);
31545 IkReal x2148
31546 = (x2137 * x2142);
31547 evalcond[0]
31548 = ((-0.55)
31549 + (((-1.0) * x2136))
31550 + x2141 + x2143
31551 + (((-1.0) * x2135))
31552 + x2138);
31553 evalcond[1]
31554 = ((0.045) + x2148
31555 + x2145 + x2147
31556 + (((-1.0) * x2144))
31557 + (((-1.0)
31558 * x2146)));
31559 evalcond[2]
31560 = ((((-0.588235294117647)
31561 * pp * x2137))
31562 + (((1.32323529411765)
31563 * cj9 * x2139))
31564 + (((1.51009803921569)
31565 * x2139))
31566 + pz
31567 + (((-3.92156862745098)
31568 * pp * x2139))
31569 + (((-0.316735294117647)
31570 * x2137))
31571 + (((-0.108264705882353)
31572 * cj9 * x2137)));
31573 evalcond[3]
31574 = ((((0.55) * x2139))
31575 + (((-0.045)
31576 * x2137))
31577 + (((-1.0) * x2140))
31578 + ((x2135 * x2139))
31579 + (((-1.0) * x2142))
31580 + ((x2136 * x2139))
31581 + ((x2137 * x2144))
31582 + (((-1.0) * x2137
31583 * x2145)));
31584 evalcond[4]
31585 = ((-0.2125)
31586 + (((-0.09) * x2148))
31587 + (((1.1) * x2143))
31588 + (((-0.09) * x2147))
31589 + (((1.1) * x2141))
31590 + (((0.09) * x2146))
31591 + (((1.1) * x2138))
31592 + (((-1.0) * (1.0)
31593 * pp)));
31594 if (IKabs(evalcond[0])
31595 > IKFAST_EVALCOND_THRESH
31596 || IKabs(evalcond[1])
31597 > IKFAST_EVALCOND_THRESH
31598 || IKabs(evalcond[2])
31599 > IKFAST_EVALCOND_THRESH
31600 || IKabs(evalcond[3])
31601 > IKFAST_EVALCOND_THRESH
31602 || IKabs(evalcond[4])
31603 > IKFAST_EVALCOND_THRESH)
31604 {
31605 continue;
31606 }
31607 }
31608
31609 rotationfunction0(solutions);
31610 }
31611 }
31612 }
31613 }
31614 }
31615 else
31616 {
31617 {
31618 IkReal j6array[1], cj6array[1],
31619 sj6array[1];
31620 bool j6valid[1] = {false};
31621 _nj6 = 1;
31622 IkReal x2149 = py * py;
31623 IkReal x2150 = (py * sj4);
31624 IkReal x2151 = cj4 * cj4;
31625 IkReal x2152 = ((0.045) * pz);
31626 IkReal x2153 = (cj4 * px);
31627 IkReal x2154 = ((0.3) * pz);
31628 IkReal x2155 = ((0.3) * cj4 * px);
31629 IkReal x2156 = ((0.045) * x2153);
31630 IkReal x2157 = ((0.3) * py * sj4);
31631 IkReal x2158 = ((0.045) * x2150);
31632 CheckValue<IkReal> x2159
31633 = IKPowWithIntegerCheck(
31634 IKsign(
31635 ((((-1.0) * x2149
31636 * x2151))
31637 + (pz * pz) + x2149
31638 + (((2.0) * cj4 * px
31639 * x2150))
31640 + ((x2151
31641 * (px * px))))),
31642 -1);
31643 if (!x2159.valid)
31644 {
31645 continue;
31646 }
31647 CheckValue<IkReal> x2160
31648 = IKatan2WithCheck(
31649 IkReal(
31650 (((sj9 * x2158)) + x2152
31651 + (((0.55) * x2150))
31652 + ((cj9 * x2155))
31653 + (((0.55) * x2153))
31654 + ((sj9 * x2156))
31655 + ((sj9 * x2154))
31656 + (((-1.0) * cj9
31657 * x2152))
31658 + ((cj9 * x2157)))),
31659 ((((-1.0) * sj9 * x2157))
31660 + (((-1.0) * x2156))
31661 + ((cj9 * x2154))
31662 + ((sj9 * x2152))
31663 + (((-1.0) * sj9 * x2155))
31664 + ((cj9 * x2156))
31665 + (((-1.0) * x2158))
31666 + (((0.55) * pz))
31667 + ((cj9 * x2158))),
31668 IKFAST_ATAN2_MAGTHRESH);
31669 if (!x2160.valid)
31670 {
31671 continue;
31672 }
31673 j6array[0]
31674 = ((-1.5707963267949)
31675 + (((1.5707963267949)
31676 * (x2159.value)))
31677 + (x2160.value));
31678 sj6array[0] = IKsin(j6array[0]);
31679 cj6array[0] = IKcos(j6array[0]);
31680 if (j6array[0] > IKPI)
31681 {
31682 j6array[0] -= IK2PI;
31683 }
31684 else if (j6array[0] < -IKPI)
31685 {
31686 j6array[0] += IK2PI;
31687 }
31688 j6valid[0] = true;
31689 for (int ij6 = 0; ij6 < 1; ++ij6)
31690 {
31691 if (!j6valid[ij6])
31692 {
31693 continue;
31694 }
31695 _ij6[0] = ij6;
31696 _ij6[1] = -1;
31697 for (int iij6 = ij6 + 1; iij6 < 1;
31698 ++iij6)
31699 {
31700 if (j6valid[iij6]
31701 && IKabs(
31702 cj6array[ij6]
31703 - cj6array[iij6])
31704 < IKFAST_SOLUTION_THRESH
31705 && IKabs(
31706 sj6array[ij6]
31707 - sj6array[iij6])
31708 < IKFAST_SOLUTION_THRESH)
31709 {
31710 j6valid[iij6] = false;
31711 _ij6[1] = iij6;
31712 break;
31713 }
31714 }
31715 j6 = j6array[ij6];
31716 cj6 = cj6array[ij6];
31717 sj6 = sj6array[ij6];
31718 {
31719 IkReal evalcond[5];
31720 IkReal x2161 = ((0.3) * cj9);
31721 IkReal x2162 = ((0.045) * sj9);
31722 IkReal x2163 = IKcos(j6);
31723 IkReal x2164 = (pz * x2163);
31724 IkReal x2165 = IKsin(j6);
31725 IkReal x2166 = (cj4 * px);
31726 IkReal x2167 = (x2165 * x2166);
31727 IkReal x2168 = (py * sj4);
31728 IkReal x2169 = (x2165 * x2168);
31729 IkReal x2170 = ((0.045) * cj9);
31730 IkReal x2171 = ((0.3) * sj9);
31731 IkReal x2172 = (pz * x2165);
31732 IkReal x2173 = (x2163 * x2166);
31733 IkReal x2174 = (x2163 * x2168);
31734 evalcond[0]
31735 = ((-0.55)
31736 + (((-1.0) * x2162))
31737 + x2169 + x2164 + x2167
31738 + (((-1.0) * x2161)));
31739 evalcond[1]
31740 = ((0.045) + x2173 + x2174
31741 + x2171
31742 + (((-1.0) * x2172))
31743 + (((-1.0) * x2170)));
31744 evalcond[2]
31745 = ((((1.32323529411765)
31746 * cj9 * x2165))
31747 + (((-0.108264705882353)
31748 * cj9 * x2163))
31749 + (((-3.92156862745098)
31750 * pp * x2165))
31751 + (((-0.588235294117647)
31752 * pp * x2163))
31753 + pz
31754 + (((1.51009803921569)
31755 * x2165))
31756 + (((-0.316735294117647)
31757 * x2163)));
31758 evalcond[3]
31759 = (((x2163 * x2170))
31760 + (((-1.0) * x2168))
31761 + ((x2161 * x2165))
31762 + (((-1.0) * x2163
31763 * x2171))
31764 + (((-0.045) * x2163))
31765 + ((x2162 * x2165))
31766 + (((-1.0) * x2166))
31767 + (((0.55) * x2165)));
31768 evalcond[4]
31769 = ((-0.2125)
31770 + (((-0.09) * x2174))
31771 + (((0.09) * x2172))
31772 + (((1.1) * x2164))
31773 + (((1.1) * x2167))
31774 + (((1.1) * x2169))
31775 + (((-1.0) * (1.0) * pp))
31776 + (((-0.09) * x2173)));
31777 if (IKabs(evalcond[0])
31778 > IKFAST_EVALCOND_THRESH
31779 || IKabs(evalcond[1])
31780 > IKFAST_EVALCOND_THRESH
31781 || IKabs(evalcond[2])
31782 > IKFAST_EVALCOND_THRESH
31783 || IKabs(evalcond[3])
31784 > IKFAST_EVALCOND_THRESH
31785 || IKabs(evalcond[4])
31786 > IKFAST_EVALCOND_THRESH)
31787 {
31788 continue;
31789 }
31790 }
31791
31792 rotationfunction0(solutions);
31793 }
31794 }
31795 }
31796 }
31797 }
31798 } while (0);
31799 if (bgotonextstatement)
31800 {
31801 bool bgotonextstatement = true;
31802 do
31803 {
31804 if (1)
31805 {
31806 bgotonextstatement = false;
31807 continue; // branch miss [j6]
31808 }
31809 } while (0);
31810 if (bgotonextstatement)
31811 {
31812 }
31813 }
31814 }
31815 }
31816 }
31817 else
31818 {
31819 {
31820 IkReal j6array[1], cj6array[1], sj6array[1];
31821 bool j6valid[1] = {false};
31822 _nj6 = 1;
31823 IkReal x2175 = (pz * sj8);
31824 IkReal x2176 = ((0.3) * cj9);
31825 IkReal x2177 = ((0.045) * sj9);
31826 IkReal x2178 = (cj4 * px);
31827 IkReal x2179 = ((0.045) * cj8 * sj8);
31828 IkReal x2180 = (x2178 * x2179);
31829 IkReal x2181 = (py * sj4);
31830 IkReal x2182 = (x2179 * x2181);
31831 IkReal x2183 = ((0.3) * cj8 * sj8 * sj9);
31832 IkReal x2184 = ((0.55) * cj8);
31833 IkReal x2185 = (cj4 * py);
31834 IkReal x2186 = (px * sj4);
31835 IkReal x2187 = (cj4 * cj8 * py);
31836 IkReal x2188 = ((1.0) * pz * sj8);
31837 IkReal x2189 = (cj8 * px * sj4);
31838 IkReal x2190 = cj8 * cj8;
31839 IkReal x2191 = ((0.045) * x2190);
31840 IkReal x2192 = (x2185 * x2191);
31841 IkReal x2193 = (x2186 * x2191);
31842 IkReal x2194 = ((0.3) * sj9 * x2190);
31843 CheckValue<IkReal> x2195
31844 = IKPowWithIntegerCheck(
31845 IKsign(
31846 ((((-1.0) * cj9 * x2182))
31847 + ((x2181 * x2183))
31848 + (((-0.55) * x2175))
31849 + (((-1.0) * x2175 * x2177))
31850 + (((-1.0) * x2175 * x2176))
31851 + (((-1.0) * cj9 * x2180))
31852 + ((x2178 * x2183)) + x2182
31853 + x2180)),
31854 -1);
31855 if (!x2195.valid)
31856 {
31857 continue;
31858 }
31859 CheckValue<IkReal> x2196 = IKatan2WithCheck(
31860 IkReal(
31861 ((((-1.0) * x2176 * x2189))
31862 + ((x2184 * x2185)) + ((x2176 * x2187))
31863 + (((-1.0) * x2178 * x2188))
31864 + ((x2177 * x2187))
31865 + (((-1.0) * x2181 * x2188))
31866 + (((-1.0) * x2184 * x2186))
31867 + (((-1.0) * x2177 * x2189)))),
31868 ((((-1.0) * (1.0) * sj8 * (pz * pz)))
31869 + (((-1.0) * x2193)) + x2192
31870 + (((-1.0) * x2186 * x2194))
31871 + (((-1.0) * cj9 * x2192))
31872 + ((x2185 * x2194)) + ((cj9 * x2193))),
31873 IKFAST_ATAN2_MAGTHRESH);
31874 if (!x2196.valid)
31875 {
31876 continue;
31877 }
31878 j6array[0]
31879 = ((-1.5707963267949)
31880 + (((1.5707963267949) * (x2195.value)))
31881 + (x2196.value));
31882 sj6array[0] = IKsin(j6array[0]);
31883 cj6array[0] = IKcos(j6array[0]);
31884 if (j6array[0] > IKPI)
31885 {
31886 j6array[0] -= IK2PI;
31887 }
31888 else if (j6array[0] < -IKPI)
31889 {
31890 j6array[0] += IK2PI;
31891 }
31892 j6valid[0] = true;
31893 for (int ij6 = 0; ij6 < 1; ++ij6)
31894 {
31895 if (!j6valid[ij6])
31896 {
31897 continue;
31898 }
31899 _ij6[0] = ij6;
31900 _ij6[1] = -1;
31901 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
31902 {
31903 if (j6valid[iij6]
31904 && IKabs(cj6array[ij6] - cj6array[iij6])
31905 < IKFAST_SOLUTION_THRESH
31906 && IKabs(sj6array[ij6] - sj6array[iij6])
31907 < IKFAST_SOLUTION_THRESH)
31908 {
31909 j6valid[iij6] = false;
31910 _ij6[1] = iij6;
31911 break;
31912 }
31913 }
31914 j6 = j6array[ij6];
31915 cj6 = cj6array[ij6];
31916 sj6 = sj6array[ij6];
31917 {
31918 IkReal evalcond[6];
31919 IkReal x2197 = ((0.3) * cj9);
31920 IkReal x2198 = ((0.045) * sj9);
31921 IkReal x2199 = IKcos(j6);
31922 IkReal x2200 = (pz * x2199);
31923 IkReal x2201 = IKsin(j6);
31924 IkReal x2202 = (cj4 * px * x2201);
31925 IkReal x2203 = (py * sj4);
31926 IkReal x2204 = (x2201 * x2203);
31927 IkReal x2205 = (px * sj4);
31928 IkReal x2206 = ((1.0) * cj4 * py);
31929 IkReal x2207 = (cj4 * sj8);
31930 IkReal x2208 = ((0.045) * cj8);
31931 IkReal x2209 = ((0.045) * cj9);
31932 IkReal x2210 = (cj8 * x2201);
31933 IkReal x2211 = ((0.3) * sj9);
31934 IkReal x2212 = (sj8 * x2205);
31935 IkReal x2213 = (pz * x2210);
31936 IkReal x2214 = (px * (((1.0) * cj4)));
31937 IkReal x2215 = (cj8 * x2199);
31938 IkReal x2216 = ((1.0) * x2203);
31939 IkReal x2217 = ((0.09) * cj8 * x2199);
31940 evalcond[0]
31941 = ((-0.55) + x2204 + x2202 + x2200
31942 + (((-1.0) * x2198))
31943 + (((-1.0) * x2197)));
31944 evalcond[1]
31945 = (((cj8 * x2205))
31946 + (((-1.0) * pz * sj8 * x2201))
31947 + (((-1.0) * cj8 * x2206))
31948 + ((sj8 * x2199 * x2203))
31949 + ((px * x2199 * x2207)));
31950 evalcond[2]
31951 = ((((-0.55) * x2199))
31952 + (((-1.0) * x2197 * x2199))
31953 + ((x2201 * x2208)) + pz
31954 + (((-1.0) * x2209 * x2210))
31955 + (((-1.0) * x2198 * x2199))
31956 + ((x2210 * x2211)));
31957 evalcond[3]
31958 = ((0.045) + (((-1.0) * x2215 * x2216))
31959 + (((-1.0) * x2209)) + x2211 + x2213
31960 + x2212 + (((-1.0) * x2214 * x2215))
31961 + (((-1.0) * sj8 * x2206)));
31962 evalcond[4]
31963 = (((x2211 * x2215)) + ((x2198 * x2201))
31964 + (((-1.0) * x2214))
31965 + (((-1.0) * x2209 * x2215))
31966 + (((0.55) * x2201))
31967 + ((x2199 * x2208))
31968 + (((-1.0) * x2216))
31969 + ((x2197 * x2201)));
31970 evalcond[5]
31971 = ((-0.2125) + (((1.1) * x2204))
31972 + (((1.1) * x2202))
31973 + (((0.09) * py * x2207))
31974 + (((-0.09) * x2213))
31975 + (((-0.09) * x2212))
31976 + ((x2203 * x2217))
31977 + (((-1.0) * (1.0) * pp))
31978 + ((cj4 * px * x2217))
31979 + (((1.1) * x2200)));
31980 if (IKabs(evalcond[0])
31981 > IKFAST_EVALCOND_THRESH
31982 || IKabs(evalcond[1])
31983 > IKFAST_EVALCOND_THRESH
31984 || IKabs(evalcond[2])
31985 > IKFAST_EVALCOND_THRESH
31986 || IKabs(evalcond[3])
31987 > IKFAST_EVALCOND_THRESH
31988 || IKabs(evalcond[4])
31989 > IKFAST_EVALCOND_THRESH
31990 || IKabs(evalcond[5])
31991 > IKFAST_EVALCOND_THRESH)
31992 {
31993 continue;
31994 }
31995 }
31996
31997 rotationfunction0(solutions);
31998 }
31999 }
32000 }
32001 }
32002 }
32003 else
32004 {
32005 {
32006 IkReal j6array[1], cj6array[1], sj6array[1];
32007 bool j6valid[1] = {false};
32008 _nj6 = 1;
32009 IkReal x2218 = py * py;
32010 IkReal x2219 = (sj8 * x2218);
32011 IkReal x2220 = (cj4 * px * sj8);
32012 IkReal x2221 = cj4 * cj4;
32013 IkReal x2222 = px * px;
32014 IkReal x2223 = ((0.55) * sj8);
32015 IkReal x2224 = (cj8 * px);
32016 IkReal x2225 = ((0.3) * cj9);
32017 IkReal x2226 = ((0.045) * sj9);
32018 IkReal x2227 = (py * sj4 * sj8);
32019 IkReal x2228 = (pz * sj8);
32020 IkReal x2229 = (cj4 * cj8 * sj4);
32021 CheckValue<IkReal> x2230 = IKatan2WithCheck(
32022 IkReal(
32023 (((py * sj4 * x2223)) + ((x2220 * x2225))
32024 + ((pz * sj4 * x2224))
32025 + (((-1.0) * cj4 * cj8 * py * pz))
32026 + ((cj4 * px * x2223)) + ((x2226 * x2227))
32027 + ((x2225 * x2227)) + ((x2220 * x2226)))),
32028 ((((2.0) * py * x2221 * x2224))
32029 + ((x2225 * x2228)) + ((x2218 * x2229))
32030 + ((x2226 * x2228)) + ((pz * x2223))
32031 + (((-1.0) * x2222 * x2229))
32032 + (((-1.0) * py * x2224))),
32033 IKFAST_ATAN2_MAGTHRESH);
32034 if (!x2230.valid)
32035 {
32036 continue;
32037 }
32038 CheckValue<IkReal> x2231 = IKPowWithIntegerCheck(
32039 IKsign(
32040 ((((-1.0) * x2219 * x2221))
32041 + (((2.0) * py * sj4 * x2220)) + x2219
32042 + ((sj8 * (pz * pz)))
32043 + ((sj8 * x2221 * x2222)))),
32044 -1);
32045 if (!x2231.valid)
32046 {
32047 continue;
32048 }
32049 j6array[0]
32050 = ((-1.5707963267949) + (x2230.value)
32051 + (((1.5707963267949) * (x2231.value))));
32052 sj6array[0] = IKsin(j6array[0]);
32053 cj6array[0] = IKcos(j6array[0]);
32054 if (j6array[0] > IKPI)
32055 {
32056 j6array[0] -= IK2PI;
32057 }
32058 else if (j6array[0] < -IKPI)
32059 {
32060 j6array[0] += IK2PI;
32061 }
32062 j6valid[0] = true;
32063 for (int ij6 = 0; ij6 < 1; ++ij6)
32064 {
32065 if (!j6valid[ij6])
32066 {
32067 continue;
32068 }
32069 _ij6[0] = ij6;
32070 _ij6[1] = -1;
32071 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
32072 {
32073 if (j6valid[iij6]
32074 && IKabs(cj6array[ij6] - cj6array[iij6])
32075 < IKFAST_SOLUTION_THRESH
32076 && IKabs(sj6array[ij6] - sj6array[iij6])
32077 < IKFAST_SOLUTION_THRESH)
32078 {
32079 j6valid[iij6] = false;
32080 _ij6[1] = iij6;
32081 break;
32082 }
32083 }
32084 j6 = j6array[ij6];
32085 cj6 = cj6array[ij6];
32086 sj6 = sj6array[ij6];
32087 {
32088 IkReal evalcond[6];
32089 IkReal x2232 = ((0.3) * cj9);
32090 IkReal x2233 = ((0.045) * sj9);
32091 IkReal x2234 = IKcos(j6);
32092 IkReal x2235 = (pz * x2234);
32093 IkReal x2236 = IKsin(j6);
32094 IkReal x2237 = (cj4 * px * x2236);
32095 IkReal x2238 = (py * sj4);
32096 IkReal x2239 = (x2236 * x2238);
32097 IkReal x2240 = (px * sj4);
32098 IkReal x2241 = ((1.0) * cj4 * py);
32099 IkReal x2242 = (cj4 * sj8);
32100 IkReal x2243 = ((0.045) * cj8);
32101 IkReal x2244 = ((0.045) * cj9);
32102 IkReal x2245 = (cj8 * x2236);
32103 IkReal x2246 = ((0.3) * sj9);
32104 IkReal x2247 = (sj8 * x2240);
32105 IkReal x2248 = (pz * x2245);
32106 IkReal x2249 = (px * (((1.0) * cj4)));
32107 IkReal x2250 = (cj8 * x2234);
32108 IkReal x2251 = ((1.0) * x2238);
32109 IkReal x2252 = ((0.09) * cj8 * x2234);
32110 evalcond[0]
32111 = ((-0.55) + x2237 + x2235 + x2239
32112 + (((-1.0) * x2232))
32113 + (((-1.0) * x2233)));
32114 evalcond[1]
32115 = (((px * x2234 * x2242))
32116 + ((sj8 * x2234 * x2238))
32117 + (((-1.0) * cj8 * x2241))
32118 + ((cj8 * x2240))
32119 + (((-1.0) * pz * sj8 * x2236)));
32120 evalcond[2]
32121 = ((((-1.0) * x2244 * x2245))
32122 + ((x2236 * x2243)) + pz
32123 + (((-1.0) * x2233 * x2234))
32124 + (((-1.0) * x2232 * x2234))
32125 + (((-0.55) * x2234))
32126 + ((x2245 * x2246)));
32127 evalcond[3]
32128 = ((0.045) + (((-1.0) * x2244))
32129 + (((-1.0) * x2249 * x2250))
32130 + (((-1.0) * sj8 * x2241)) + x2247
32131 + x2246 + x2248
32132 + (((-1.0) * x2250 * x2251)));
32133 evalcond[4]
32134 = ((((0.55) * x2236)) + (((-1.0) * x2249))
32135 + ((x2233 * x2236))
32136 + (((-1.0) * x2244 * x2250))
32137 + ((x2246 * x2250)) + (((-1.0) * x2251))
32138 + ((x2232 * x2236)) + ((x2234 * x2243)));
32139 evalcond[5]
32140 = ((-0.2125) + (((-0.09) * x2247))
32141 + (((-0.09) * x2248)) + ((x2238 * x2252))
32142 + (((1.1) * x2239)) + (((1.1) * x2237))
32143 + (((1.1) * x2235))
32144 + ((cj4 * px * x2252))
32145 + (((-1.0) * (1.0) * pp))
32146 + (((0.09) * py * x2242)));
32147 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
32148 || IKabs(evalcond[1])
32149 > IKFAST_EVALCOND_THRESH
32150 || IKabs(evalcond[2])
32151 > IKFAST_EVALCOND_THRESH
32152 || IKabs(evalcond[3])
32153 > IKFAST_EVALCOND_THRESH
32154 || IKabs(evalcond[4])
32155 > IKFAST_EVALCOND_THRESH
32156 || IKabs(evalcond[5])
32157 > IKFAST_EVALCOND_THRESH)
32158 {
32159 continue;
32160 }
32161 }
32162
32163 rotationfunction0(solutions);
32164 }
32165 }
32166 }
32167 }
32168 }
32169 else
32170 {
32171 {
32172 IkReal j6array[1], cj6array[1], sj6array[1];
32173 bool j6valid[1] = {false};
32174 _nj6 = 1;
32175 IkReal x2253 = (cj4 * px);
32176 IkReal x2254 = ((0.045) * pz);
32177 IkReal x2255 = (py * sj4);
32178 IkReal x2256 = ((0.3) * cj9);
32179 IkReal x2257 = ((0.045) * sj9);
32180 IkReal x2258 = (cj8 * cj9);
32181 IkReal x2259 = (cj8 * sj9);
32182 IkReal x2260 = cj9 * cj9;
32183 IkReal x2261 = ((1.0) * pz);
32184 CheckValue<IkReal> x2262 = IKatan2WithCheck(
32185 IkReal(
32186 ((-0.304525) + (((-1.0) * (0.027) * cj9 * sj9))
32187 + (((-1.0) * (0.0495) * sj9)) + (pz * pz)
32188 + (((-0.087975) * x2260))
32189 + (((-1.0) * (0.33) * cj9)))),
32190 ((((-1.0) * x2255 * x2261))
32191 + (((0.027) * cj8 * x2260)) + (((0.01125) * x2258))
32192 + (((-1.0) * x2253 * x2261))
32193 + (((-1.0) * (0.03825) * cj8))
32194 + (((-0.087975) * sj9 * x2258))
32195 + (((-0.167025) * x2259))),
32196 IKFAST_ATAN2_MAGTHRESH);
32197 if (!x2262.valid)
32198 {
32199 continue;
32200 }
32201 CheckValue<IkReal> x2263 = IKPowWithIntegerCheck(
32202 IKsign((
32203 (((-1.0) * x2255 * x2257))
32204 + (((-1.0) * x2255 * x2256)) + ((x2254 * x2258))
32205 + (((-1.0) * x2253 * x2257))
32206 + (((-0.55) * x2255)) + (((-0.3) * pz * x2259))
32207 + (((-1.0) * x2253 * x2256))
32208 + (((-0.55) * x2253))
32209 + (((-1.0) * cj8 * x2254)))),
32210 -1);
32211 if (!x2263.valid)
32212 {
32213 continue;
32214 }
32215 j6array[0]
32216 = ((-1.5707963267949) + (x2262.value)
32217 + (((1.5707963267949) * (x2263.value))));
32218 sj6array[0] = IKsin(j6array[0]);
32219 cj6array[0] = IKcos(j6array[0]);
32220 if (j6array[0] > IKPI)
32221 {
32222 j6array[0] -= IK2PI;
32223 }
32224 else if (j6array[0] < -IKPI)
32225 {
32226 j6array[0] += IK2PI;
32227 }
32228 j6valid[0] = true;
32229 for (int ij6 = 0; ij6 < 1; ++ij6)
32230 {
32231 if (!j6valid[ij6])
32232 {
32233 continue;
32234 }
32235 _ij6[0] = ij6;
32236 _ij6[1] = -1;
32237 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
32238 {
32239 if (j6valid[iij6]
32240 && IKabs(cj6array[ij6] - cj6array[iij6])
32241 < IKFAST_SOLUTION_THRESH
32242 && IKabs(sj6array[ij6] - sj6array[iij6])
32243 < IKFAST_SOLUTION_THRESH)
32244 {
32245 j6valid[iij6] = false;
32246 _ij6[1] = iij6;
32247 break;
32248 }
32249 }
32250 j6 = j6array[ij6];
32251 cj6 = cj6array[ij6];
32252 sj6 = sj6array[ij6];
32253 {
32254 IkReal evalcond[6];
32255 IkReal x2264 = ((0.3) * cj9);
32256 IkReal x2265 = ((0.045) * sj9);
32257 IkReal x2266 = IKcos(j6);
32258 IkReal x2267 = (pz * x2266);
32259 IkReal x2268 = IKsin(j6);
32260 IkReal x2269 = (cj4 * px * x2268);
32261 IkReal x2270 = (py * sj4);
32262 IkReal x2271 = (x2268 * x2270);
32263 IkReal x2272 = (px * sj4);
32264 IkReal x2273 = ((1.0) * cj4 * py);
32265 IkReal x2274 = (cj4 * sj8);
32266 IkReal x2275 = ((0.045) * cj8);
32267 IkReal x2276 = ((0.045) * cj9);
32268 IkReal x2277 = (cj8 * x2268);
32269 IkReal x2278 = ((0.3) * sj9);
32270 IkReal x2279 = (sj8 * x2272);
32271 IkReal x2280 = (pz * x2277);
32272 IkReal x2281 = (px * (((1.0) * cj4)));
32273 IkReal x2282 = (cj8 * x2266);
32274 IkReal x2283 = ((1.0) * x2270);
32275 IkReal x2284 = ((0.09) * cj8 * x2266);
32276 evalcond[0]
32277 = ((-0.55) + x2267 + x2269 + (((-1.0) * x2265))
32278 + x2271 + (((-1.0) * x2264)));
32279 evalcond[1]
32280 = (((cj8 * x2272)) + (((-1.0) * cj8 * x2273))
32281 + (((-1.0) * pz * sj8 * x2268))
32282 + ((sj8 * x2266 * x2270))
32283 + ((px * x2266 * x2274)));
32284 evalcond[2]
32285 = ((((-1.0) * x2264 * x2266))
32286 + ((x2268 * x2275)) + pz
32287 + (((-1.0) * x2265 * x2266))
32288 + ((x2277 * x2278)) + (((-0.55) * x2266))
32289 + (((-1.0) * x2276 * x2277)));
32290 evalcond[3]
32291 = ((0.045) + (((-1.0) * x2282 * x2283)) + x2280
32292 + (((-1.0) * sj8 * x2273))
32293 + (((-1.0) * x2281 * x2282))
32294 + (((-1.0) * x2276)) + x2278 + x2279);
32295 evalcond[4]
32296 = ((((-1.0) * x2281)) + ((x2266 * x2275))
32297 + (((-1.0) * x2276 * x2282))
32298 + ((x2264 * x2268)) + ((x2278 * x2282))
32299 + ((x2265 * x2268)) + (((-1.0) * x2283))
32300 + (((0.55) * x2268)));
32301 evalcond[5]
32302 = ((-0.2125) + (((-0.09) * x2279))
32303 + (((-0.09) * x2280)) + ((cj4 * px * x2284))
32304 + (((0.09) * py * x2274)) + (((1.1) * x2267))
32305 + ((x2270 * x2284)) + (((-1.0) * (1.0) * pp))
32306 + (((1.1) * x2269)) + (((1.1) * x2271)));
32307 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
32308 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
32309 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
32310 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
32311 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
32312 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH)
32313 {
32314 continue;
32315 }
32316 }
32317
32318 rotationfunction0(solutions);
32319 }
32320 }
32321 }
32322 }
32323 }
32324 }
32325 }
32326 }
32327 }
32328 }
32329 }
32330 return solutions.GetNumSolutions() > 0;
32331 }
rotationfunction0(IkSolutionListBase<IkReal> & solutions)32332 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions)
32333 {
32334 for (int rotationiter = 0; rotationiter < 1; ++rotationiter)
32335 {
32336 IkReal x158 = ((1.0) * sj9);
32337 IkReal x159 = (cj9 * sj6);
32338 IkReal x160 = ((((-1.0) * cj6 * x158)) + (((-1.0) * cj8 * x159)));
32339 IkReal x161 = (cj4 * sj8);
32340 IkReal x162 = (sj6 * x158);
32341 IkReal x163 = (cj6 * cj9);
32342 IkReal x164 = (((cj8 * x163)) + (((-1.0) * x162)));
32343 IkReal x165 = (((cj9 * x161)) + ((sj4 * x164)));
32344 IkReal x166 = ((1.0) * sj4 * sj8);
32345 IkReal x167 = ((((-1.0) * cj9 * x166)) + ((cj4 * x164)));
32346 IkReal x168 = (sj6 * sj8);
32347 IkReal x169 = (((cj4 * cj8)) + (((-1.0) * cj6 * x166)));
32348 IkReal x170 = ((((-1.0) * cj6 * x161)) + (((-1.0) * cj8 * sj4)));
32349 IkReal x171 = ((((-1.0) * cj8 * x162)) + x163);
32350 IkReal x172 = (x159 + ((cj6 * cj8 * sj9)));
32351 IkReal x173 = (((sj4 * x172)) + ((sj9 * x161)));
32352 IkReal x174 = (((cj4 * x172)) + (((-1.0) * sj4 * sj8 * x158)));
32353 new_r00 = (((r00 * x167)) + ((r20 * x160)) + ((r10 * x165)));
32354 new_r01 = (((r11 * x165)) + ((r21 * x160)) + ((r01 * x167)));
32355 new_r02 = (((r22 * x160)) + ((r12 * x165)) + ((r02 * x167)));
32356 new_r10 = (((r20 * x168)) + ((r10 * x169)) + ((r00 * x170)));
32357 new_r11 = (((r01 * x170)) + ((r21 * x168)) + ((r11 * x169)));
32358 new_r12 = (((r22 * x168)) + ((r02 * x170)) + ((r12 * x169)));
32359 new_r20 = (((r20 * x171)) + ((r10 * x173)) + ((r00 * x174)));
32360 new_r21 = (((r01 * x174)) + ((r11 * x173)) + ((r21 * x171)));
32361 new_r22 = (((r12 * x173)) + ((r22 * x171)) + ((r02 * x174)));
32362 {
32363 IkReal j11array[2], cj11array[2], sj11array[2];
32364 bool j11valid[2] = {false};
32365 _nj11 = 2;
32366 cj11array[0] = new_r22;
32367 if (cj11array[0] >= -1 - IKFAST_SINCOS_THRESH
32368 && cj11array[0] <= 1 + IKFAST_SINCOS_THRESH)
32369 {
32370 j11valid[0] = j11valid[1] = true;
32371 j11array[0] = IKacos(cj11array[0]);
32372 sj11array[0] = IKsin(j11array[0]);
32373 cj11array[1] = cj11array[0];
32374 j11array[1] = -j11array[0];
32375 sj11array[1] = -sj11array[0];
32376 }
32377 else if (std::isnan(cj11array[0]))
32378 {
32379 // probably any value will work
32380 j11valid[0] = true;
32381 cj11array[0] = 1;
32382 sj11array[0] = 0;
32383 j11array[0] = 0;
32384 }
32385 for (int ij11 = 0; ij11 < 2; ++ij11)
32386 {
32387 if (!j11valid[ij11])
32388 {
32389 continue;
32390 }
32391 _ij11[0] = ij11;
32392 _ij11[1] = -1;
32393 for (int iij11 = ij11 + 1; iij11 < 2; ++iij11)
32394 {
32395 if (j11valid[iij11]
32396 && IKabs(cj11array[ij11] - cj11array[iij11])
32397 < IKFAST_SOLUTION_THRESH
32398 && IKabs(sj11array[ij11] - sj11array[iij11])
32399 < IKFAST_SOLUTION_THRESH)
32400 {
32401 j11valid[iij11] = false;
32402 _ij11[1] = iij11;
32403 break;
32404 }
32405 }
32406 j11 = j11array[ij11];
32407 cj11 = cj11array[ij11];
32408 sj11 = sj11array[ij11];
32409
32410 {
32411 IkReal j10eval[2];
32412 IkReal x175 = ((1.0) * sj9);
32413 IkReal x176 = (cj9 * sj6);
32414 IkReal x177 = x160;
32415 IkReal x178 = (cj4 * sj8);
32416 IkReal x179 = (sj6 * x175);
32417 IkReal x180 = (cj6 * cj9);
32418 IkReal x181 = (((cj8 * x180)) + (((-1.0) * x179)));
32419 IkReal x182 = (((sj4 * x181)) + ((cj9 * x178)));
32420 IkReal x183 = ((1.0) * sj4 * sj8);
32421 IkReal x184 = (((cj4 * x181)) + (((-1.0) * cj9 * x183)));
32422 IkReal x185 = (sj6 * sj8);
32423 IkReal x186 = x169;
32424 IkReal x187 = x170;
32425 IkReal x188 = ((((-1.0) * cj8 * x179)) + x180);
32426 IkReal x189 = x172;
32427 IkReal x190 = (((sj9 * x178)) + ((sj4 * x189)));
32428 IkReal x191 = ((((-1.0) * sj4 * sj8 * x175)) + ((cj4 * x189)));
32429 new_r00 = (((r20 * x177)) + ((r00 * x184)) + ((r10 * x182)));
32430 new_r01 = (((r21 * x177)) + ((r11 * x182)) + ((r01 * x184)));
32431 new_r02 = (((r22 * x177)) + ((r12 * x182)) + ((r02 * x184)));
32432 new_r10 = (((r10 * x186)) + ((r20 * x185)) + ((r00 * x187)));
32433 new_r11 = (((r21 * x185)) + ((r11 * x186)) + ((r01 * x187)));
32434 new_r12 = (((r12 * x186)) + ((r22 * x185)) + ((r02 * x187)));
32435 new_r20 = (((r00 * x191)) + ((r20 * x188)) + ((r10 * x190)));
32436 new_r21 = (((r21 * x188)) + ((r11 * x190)) + ((r01 * x191)));
32437 new_r22 = (((r12 * x190)) + ((r22 * x188)) + ((r02 * x191)));
32438 j10eval[0] = sj11;
32439 j10eval[1] = IKsign(sj11);
32440 if (IKabs(j10eval[0]) < 0.0000010000000000
32441 || IKabs(j10eval[1]) < 0.0000010000000000)
32442 {
32443 {
32444 IkReal j10eval[1];
32445 IkReal x192 = ((1.0) * sj9);
32446 IkReal x193 = (cj9 * sj6);
32447 IkReal x194 = x160;
32448 IkReal x195 = (cj4 * sj8);
32449 IkReal x196 = (sj6 * x192);
32450 IkReal x197 = (cj6 * cj9);
32451 IkReal x198 = (((cj8 * x197)) + (((-1.0) * x196)));
32452 IkReal x199 = (((cj9 * x195)) + ((sj4 * x198)));
32453 IkReal x200 = ((1.0) * sj4 * sj8);
32454 IkReal x201 = ((((-1.0) * cj9 * x200)) + ((cj4 * x198)));
32455 IkReal x202 = (sj6 * sj8);
32456 IkReal x203 = x169;
32457 IkReal x204 = x170;
32458 IkReal x205 = ((((-1.0) * cj8 * x196)) + x197);
32459 IkReal x206 = x172;
32460 IkReal x207 = (((sj9 * x195)) + ((sj4 * x206)));
32461 IkReal x208 = (((cj4 * x206)) + (((-1.0) * sj4 * sj8 * x192)));
32462 new_r00 = (((r20 * x194)) + ((r00 * x201)) + ((r10 * x199)));
32463 new_r01 = (((r11 * x199)) + ((r01 * x201)) + ((r21 * x194)));
32464 new_r02 = (((r02 * x201)) + ((r22 * x194)) + ((r12 * x199)));
32465 new_r10 = (((r00 * x204)) + ((r10 * x203)) + ((r20 * x202)));
32466 new_r11 = (((r21 * x202)) + ((r11 * x203)) + ((r01 * x204)));
32467 new_r12 = (((r12 * x203)) + ((r22 * x202)) + ((r02 * x204)));
32468 new_r20 = (((r20 * x205)) + ((r10 * x207)) + ((r00 * x208)));
32469 new_r21 = (((r11 * x207)) + ((r01 * x208)) + ((r21 * x205)));
32470 new_r22 = (((r12 * x207)) + ((r02 * x208)) + ((r22 * x205)));
32471 j10eval[0] = sj11;
32472 if (IKabs(j10eval[0]) < 0.0000010000000000)
32473 {
32474 {
32475 IkReal evalcond[6];
32476 bool bgotonextstatement = true;
32477 do
32478 {
32479 evalcond[0]
32480 = ((-3.14159265358979)
32481 + (IKfmod(
32482 ((3.14159265358979) + (IKabs(j11))),
32483 6.28318530717959)));
32484 evalcond[1] = ((-1.0) + new_r22);
32485 evalcond[2] = new_r20;
32486 evalcond[3] = new_r02;
32487 evalcond[4] = new_r12;
32488 evalcond[5] = new_r21;
32489 if (IKabs(evalcond[0]) < 0.0000010000000000
32490 && IKabs(evalcond[1]) < 0.0000010000000000
32491 && IKabs(evalcond[2]) < 0.0000010000000000
32492 && IKabs(evalcond[3]) < 0.0000010000000000
32493 && IKabs(evalcond[4]) < 0.0000010000000000
32494 && IKabs(evalcond[5]) < 0.0000010000000000)
32495 {
32496 bgotonextstatement = false;
32497 {
32498 IkReal j10array[2], cj10array[2], sj10array[2];
32499 bool j10valid[2] = {false};
32500 _nj10 = 2;
32501 CheckValue<IkReal> x210 = IKatan2WithCheck(
32502 IkReal(new_r02), new_r12, IKFAST_ATAN2_MAGTHRESH);
32503 if (!x210.valid)
32504 {
32505 continue;
32506 }
32507 IkReal x209 = ((-1.0) * (((1.0) * (x210.value))));
32508 j10array[0] = x209;
32509 sj10array[0] = IKsin(j10array[0]);
32510 cj10array[0] = IKcos(j10array[0]);
32511 j10array[1] = ((3.14159265358979) + x209);
32512 sj10array[1] = IKsin(j10array[1]);
32513 cj10array[1] = IKcos(j10array[1]);
32514 if (j10array[0] > IKPI)
32515 {
32516 j10array[0] -= IK2PI;
32517 }
32518 else if (j10array[0] < -IKPI)
32519 {
32520 j10array[0] += IK2PI;
32521 }
32522 j10valid[0] = true;
32523 if (j10array[1] > IKPI)
32524 {
32525 j10array[1] -= IK2PI;
32526 }
32527 else if (j10array[1] < -IKPI)
32528 {
32529 j10array[1] += IK2PI;
32530 }
32531 j10valid[1] = true;
32532 for (int ij10 = 0; ij10 < 2; ++ij10)
32533 {
32534 if (!j10valid[ij10])
32535 {
32536 continue;
32537 }
32538 _ij10[0] = ij10;
32539 _ij10[1] = -1;
32540 for (int iij10 = ij10 + 1; iij10 < 2; ++iij10)
32541 {
32542 if (j10valid[iij10]
32543 && IKabs(cj10array[ij10] - cj10array[iij10])
32544 < IKFAST_SOLUTION_THRESH
32545 && IKabs(sj10array[ij10] - sj10array[iij10])
32546 < IKFAST_SOLUTION_THRESH)
32547 {
32548 j10valid[iij10] = false;
32549 _ij10[1] = iij10;
32550 break;
32551 }
32552 }
32553 j10 = j10array[ij10];
32554 cj10 = cj10array[ij10];
32555 sj10 = sj10array[ij10];
32556 {
32557 IkReal evalcond[1];
32558 evalcond[0]
32559 = (((new_r12 * (IKcos(j10))))
32560 + (((-1.0) * (1.0) * new_r02
32561 * (IKsin(j10)))));
32562 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
32563 {
32564 continue;
32565 }
32566 }
32567
32568 {
32569 IkReal j12array[1], cj12array[1], sj12array[1];
32570 bool j12valid[1] = {false};
32571 _nj12 = 1;
32572 IkReal x211 = ((1.0) * new_r01);
32573 if (IKabs(
32574 ((((-1.0) * cj10 * x211))
32575 + (((-1.0) * (1.0) * new_r00 * sj10))))
32576 < IKFAST_ATAN2_MAGTHRESH
32577 && IKabs(
32578 ((((-1.0) * sj10 * x211))
32579 + ((cj10 * new_r00))))
32580 < IKFAST_ATAN2_MAGTHRESH
32581 && IKabs(
32582 IKsqr(
32583 ((((-1.0) * cj10 * x211))
32584 + (((-1.0) * (1.0) * new_r00
32585 * sj10))))
32586 + IKsqr(
32587 ((((-1.0) * sj10 * x211))
32588 + ((cj10 * new_r00))))
32589 - 1)
32590 <= IKFAST_SINCOS_THRESH)
32591 continue;
32592 j12array[0] = IKatan2(
32593 ((((-1.0) * cj10 * x211))
32594 + (((-1.0) * (1.0) * new_r00 * sj10))),
32595 ((((-1.0) * sj10 * x211))
32596 + ((cj10 * new_r00))));
32597 sj12array[0] = IKsin(j12array[0]);
32598 cj12array[0] = IKcos(j12array[0]);
32599 if (j12array[0] > IKPI)
32600 {
32601 j12array[0] -= IK2PI;
32602 }
32603 else if (j12array[0] < -IKPI)
32604 {
32605 j12array[0] += IK2PI;
32606 }
32607 j12valid[0] = true;
32608 for (int ij12 = 0; ij12 < 1; ++ij12)
32609 {
32610 if (!j12valid[ij12])
32611 {
32612 continue;
32613 }
32614 _ij12[0] = ij12;
32615 _ij12[1] = -1;
32616 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
32617 {
32618 if (j12valid[iij12]
32619 && IKabs(
32620 cj12array[ij12] - cj12array[iij12])
32621 < IKFAST_SOLUTION_THRESH
32622 && IKabs(
32623 sj12array[ij12] - sj12array[iij12])
32624 < IKFAST_SOLUTION_THRESH)
32625 {
32626 j12valid[iij12] = false;
32627 _ij12[1] = iij12;
32628 break;
32629 }
32630 }
32631 j12 = j12array[ij12];
32632 cj12 = cj12array[ij12];
32633 sj12 = sj12array[ij12];
32634 {
32635 IkReal evalcond[8];
32636 IkReal x212 = IKsin(j12);
32637 IkReal x213 = (cj10 * x212);
32638 IkReal x214 = IKcos(j12);
32639 IkReal x215 = ((1.0) * x214);
32640 IkReal x216 = ((-1.0) * x215);
32641 IkReal x217 = ((1.0) * sj10);
32642 IkReal x218
32643 = ((((-1.0) * cj10 * x215))
32644 + ((sj10 * x212)));
32645 evalcond[0]
32646 = (((cj10 * new_r01)) + x212
32647 + ((new_r11 * sj10)));
32648 evalcond[1]
32649 = (((sj10 * x214)) + x213 + new_r01);
32650 evalcond[2]
32651 = (((cj10 * new_r00)) + ((new_r10 * sj10))
32652 + x216);
32653 evalcond[3]
32654 = ((((-1.0) * new_r00 * x217))
32655 + (((-1.0) * x212))
32656 + ((cj10 * new_r10)));
32657 evalcond[4]
32658 = (((cj10 * new_r11)) + x216
32659 + (((-1.0) * new_r01 * x217)));
32660 evalcond[5] = (x218 + new_r00);
32661 evalcond[6] = (x218 + new_r11);
32662 evalcond[7]
32663 = ((((-1.0) * x213))
32664 + (((-1.0) * x214 * x217)) + new_r10);
32665 if (IKabs(evalcond[0])
32666 > IKFAST_EVALCOND_THRESH
32667 || IKabs(evalcond[1])
32668 > IKFAST_EVALCOND_THRESH
32669 || IKabs(evalcond[2])
32670 > IKFAST_EVALCOND_THRESH
32671 || IKabs(evalcond[3])
32672 > IKFAST_EVALCOND_THRESH
32673 || IKabs(evalcond[4])
32674 > IKFAST_EVALCOND_THRESH
32675 || IKabs(evalcond[5])
32676 > IKFAST_EVALCOND_THRESH
32677 || IKabs(evalcond[6])
32678 > IKFAST_EVALCOND_THRESH
32679 || IKabs(evalcond[7])
32680 > IKFAST_EVALCOND_THRESH)
32681 {
32682 continue;
32683 }
32684 }
32685
32686 {
32687 std::vector<IkSingleDOFSolutionBase<IkReal> >
32688 vinfos(7);
32689 vinfos[0].jointtype = 1;
32690 vinfos[0].foffset = j4;
32691 vinfos[0].indices[0] = _ij4[0];
32692 vinfos[0].indices[1] = _ij4[1];
32693 vinfos[0].maxsolutions = _nj4;
32694 vinfos[1].jointtype = 1;
32695 vinfos[1].foffset = j6;
32696 vinfos[1].indices[0] = _ij6[0];
32697 vinfos[1].indices[1] = _ij6[1];
32698 vinfos[1].maxsolutions = _nj6;
32699 vinfos[2].jointtype = 1;
32700 vinfos[2].foffset = j8;
32701 vinfos[2].indices[0] = _ij8[0];
32702 vinfos[2].indices[1] = _ij8[1];
32703 vinfos[2].maxsolutions = _nj8;
32704 vinfos[3].jointtype = 1;
32705 vinfos[3].foffset = j9;
32706 vinfos[3].indices[0] = _ij9[0];
32707 vinfos[3].indices[1] = _ij9[1];
32708 vinfos[3].maxsolutions = _nj9;
32709 vinfos[4].jointtype = 1;
32710 vinfos[4].foffset = j10;
32711 vinfos[4].indices[0] = _ij10[0];
32712 vinfos[4].indices[1] = _ij10[1];
32713 vinfos[4].maxsolutions = _nj10;
32714 vinfos[5].jointtype = 1;
32715 vinfos[5].foffset = j11;
32716 vinfos[5].indices[0] = _ij11[0];
32717 vinfos[5].indices[1] = _ij11[1];
32718 vinfos[5].maxsolutions = _nj11;
32719 vinfos[6].jointtype = 1;
32720 vinfos[6].foffset = j12;
32721 vinfos[6].indices[0] = _ij12[0];
32722 vinfos[6].indices[1] = _ij12[1];
32723 vinfos[6].maxsolutions = _nj12;
32724 std::vector<int> vfree(0);
32725 solutions.AddSolution(vinfos, vfree);
32726 }
32727 }
32728 }
32729 }
32730 }
32731 }
32732 } while (0);
32733 if (bgotonextstatement)
32734 {
32735 bool bgotonextstatement = true;
32736 do
32737 {
32738 evalcond[0]
32739 = ((-3.14159265358979)
32740 + (IKfmod(
32741 ((3.14159265358979)
32742 + (IKabs(((-3.14159265358979) + j11)))),
32743 6.28318530717959)));
32744 evalcond[1] = ((1.0) + new_r22);
32745 evalcond[2] = new_r20;
32746 evalcond[3] = new_r02;
32747 evalcond[4] = new_r12;
32748 evalcond[5] = new_r21;
32749 if (IKabs(evalcond[0]) < 0.0000010000000000
32750 && IKabs(evalcond[1]) < 0.0000010000000000
32751 && IKabs(evalcond[2]) < 0.0000010000000000
32752 && IKabs(evalcond[3]) < 0.0000010000000000
32753 && IKabs(evalcond[4]) < 0.0000010000000000
32754 && IKabs(evalcond[5]) < 0.0000010000000000)
32755 {
32756 bgotonextstatement = false;
32757 {
32758 IkReal j10array[2], cj10array[2], sj10array[2];
32759 bool j10valid[2] = {false};
32760 _nj10 = 2;
32761 CheckValue<IkReal> x220 = IKatan2WithCheck(
32762 IkReal(new_r02),
32763 new_r12,
32764 IKFAST_ATAN2_MAGTHRESH);
32765 if (!x220.valid)
32766 {
32767 continue;
32768 }
32769 IkReal x219 = ((-1.0) * (((1.0) * (x220.value))));
32770 j10array[0] = x219;
32771 sj10array[0] = IKsin(j10array[0]);
32772 cj10array[0] = IKcos(j10array[0]);
32773 j10array[1] = ((3.14159265358979) + x219);
32774 sj10array[1] = IKsin(j10array[1]);
32775 cj10array[1] = IKcos(j10array[1]);
32776 if (j10array[0] > IKPI)
32777 {
32778 j10array[0] -= IK2PI;
32779 }
32780 else if (j10array[0] < -IKPI)
32781 {
32782 j10array[0] += IK2PI;
32783 }
32784 j10valid[0] = true;
32785 if (j10array[1] > IKPI)
32786 {
32787 j10array[1] -= IK2PI;
32788 }
32789 else if (j10array[1] < -IKPI)
32790 {
32791 j10array[1] += IK2PI;
32792 }
32793 j10valid[1] = true;
32794 for (int ij10 = 0; ij10 < 2; ++ij10)
32795 {
32796 if (!j10valid[ij10])
32797 {
32798 continue;
32799 }
32800 _ij10[0] = ij10;
32801 _ij10[1] = -1;
32802 for (int iij10 = ij10 + 1; iij10 < 2; ++iij10)
32803 {
32804 if (j10valid[iij10]
32805 && IKabs(cj10array[ij10] - cj10array[iij10])
32806 < IKFAST_SOLUTION_THRESH
32807 && IKabs(sj10array[ij10] - sj10array[iij10])
32808 < IKFAST_SOLUTION_THRESH)
32809 {
32810 j10valid[iij10] = false;
32811 _ij10[1] = iij10;
32812 break;
32813 }
32814 }
32815 j10 = j10array[ij10];
32816 cj10 = cj10array[ij10];
32817 sj10 = sj10array[ij10];
32818 {
32819 IkReal evalcond[1];
32820 evalcond[0]
32821 = (((new_r12 * (IKcos(j10))))
32822 + (((-1.0) * (1.0) * new_r02
32823 * (IKsin(j10)))));
32824 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
32825 {
32826 continue;
32827 }
32828 }
32829
32830 {
32831 IkReal j12array[1], cj12array[1], sj12array[1];
32832 bool j12valid[1] = {false};
32833 _nj12 = 1;
32834 IkReal x221 = ((1.0) * new_r00);
32835 if (IKabs(
32836 ((((-1.0) * sj10 * x221))
32837 + ((cj10 * new_r01))))
32838 < IKFAST_ATAN2_MAGTHRESH
32839 && IKabs(
32840 ((((-1.0) * cj10 * x221))
32841 + (((-1.0) * (1.0) * new_r01
32842 * sj10))))
32843 < IKFAST_ATAN2_MAGTHRESH
32844 && IKabs(
32845 IKsqr(
32846 ((((-1.0) * sj10 * x221))
32847 + ((cj10 * new_r01))))
32848 + IKsqr(
32849 ((((-1.0) * cj10 * x221))
32850 + (((-1.0) * (1.0) * new_r01
32851 * sj10))))
32852 - 1)
32853 <= IKFAST_SINCOS_THRESH)
32854 continue;
32855 j12array[0] = IKatan2(
32856 ((((-1.0) * sj10 * x221))
32857 + ((cj10 * new_r01))),
32858 ((((-1.0) * cj10 * x221))
32859 + (((-1.0) * (1.0) * new_r01 * sj10))));
32860 sj12array[0] = IKsin(j12array[0]);
32861 cj12array[0] = IKcos(j12array[0]);
32862 if (j12array[0] > IKPI)
32863 {
32864 j12array[0] -= IK2PI;
32865 }
32866 else if (j12array[0] < -IKPI)
32867 {
32868 j12array[0] += IK2PI;
32869 }
32870 j12valid[0] = true;
32871 for (int ij12 = 0; ij12 < 1; ++ij12)
32872 {
32873 if (!j12valid[ij12])
32874 {
32875 continue;
32876 }
32877 _ij12[0] = ij12;
32878 _ij12[1] = -1;
32879 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
32880 {
32881 if (j12valid[iij12]
32882 && IKabs(
32883 cj12array[ij12]
32884 - cj12array[iij12])
32885 < IKFAST_SOLUTION_THRESH
32886 && IKabs(
32887 sj12array[ij12]
32888 - sj12array[iij12])
32889 < IKFAST_SOLUTION_THRESH)
32890 {
32891 j12valid[iij12] = false;
32892 _ij12[1] = iij12;
32893 break;
32894 }
32895 }
32896 j12 = j12array[ij12];
32897 cj12 = cj12array[ij12];
32898 sj12 = sj12array[ij12];
32899 {
32900 IkReal evalcond[8];
32901 IkReal x222 = IKcos(j12);
32902 IkReal x223 = IKsin(j12);
32903 IkReal x224 = ((1.0) * x223);
32904 IkReal x225 = ((-1.0) * x224);
32905 IkReal x226 = (cj10 * x222);
32906 IkReal x227 = ((1.0) * sj10);
32907 IkReal x228
32908 = (((sj10 * x222))
32909 + (((-1.0) * cj10 * x224)));
32910 evalcond[0]
32911 = (((cj10 * new_r00))
32912 + ((new_r10 * sj10)) + x222);
32913 evalcond[1]
32914 = (((cj10 * new_r01))
32915 + ((new_r11 * sj10)) + x225);
32916 evalcond[2]
32917 = (((sj10 * x223)) + new_r00 + x226);
32918 evalcond[3]
32919 = (((cj10 * new_r10))
32920 + (((-1.0) * new_r00 * x227))
32921 + x225);
32922 evalcond[4]
32923 = (((cj10 * new_r11))
32924 + (((-1.0) * x222))
32925 + (((-1.0) * new_r01 * x227)));
32926 evalcond[5] = (new_r01 + x228);
32927 evalcond[6] = (new_r10 + x228);
32928 evalcond[7]
32929 = ((((-1.0) * x226)) + new_r11
32930 + (((-1.0) * x223 * x227)));
32931 if (IKabs(evalcond[0])
32932 > IKFAST_EVALCOND_THRESH
32933 || IKabs(evalcond[1])
32934 > IKFAST_EVALCOND_THRESH
32935 || IKabs(evalcond[2])
32936 > IKFAST_EVALCOND_THRESH
32937 || IKabs(evalcond[3])
32938 > IKFAST_EVALCOND_THRESH
32939 || IKabs(evalcond[4])
32940 > IKFAST_EVALCOND_THRESH
32941 || IKabs(evalcond[5])
32942 > IKFAST_EVALCOND_THRESH
32943 || IKabs(evalcond[6])
32944 > IKFAST_EVALCOND_THRESH
32945 || IKabs(evalcond[7])
32946 > IKFAST_EVALCOND_THRESH)
32947 {
32948 continue;
32949 }
32950 }
32951
32952 {
32953 std::vector<
32954 IkSingleDOFSolutionBase<IkReal> >
32955 vinfos(7);
32956 vinfos[0].jointtype = 1;
32957 vinfos[0].foffset = j4;
32958 vinfos[0].indices[0] = _ij4[0];
32959 vinfos[0].indices[1] = _ij4[1];
32960 vinfos[0].maxsolutions = _nj4;
32961 vinfos[1].jointtype = 1;
32962 vinfos[1].foffset = j6;
32963 vinfos[1].indices[0] = _ij6[0];
32964 vinfos[1].indices[1] = _ij6[1];
32965 vinfos[1].maxsolutions = _nj6;
32966 vinfos[2].jointtype = 1;
32967 vinfos[2].foffset = j8;
32968 vinfos[2].indices[0] = _ij8[0];
32969 vinfos[2].indices[1] = _ij8[1];
32970 vinfos[2].maxsolutions = _nj8;
32971 vinfos[3].jointtype = 1;
32972 vinfos[3].foffset = j9;
32973 vinfos[3].indices[0] = _ij9[0];
32974 vinfos[3].indices[1] = _ij9[1];
32975 vinfos[3].maxsolutions = _nj9;
32976 vinfos[4].jointtype = 1;
32977 vinfos[4].foffset = j10;
32978 vinfos[4].indices[0] = _ij10[0];
32979 vinfos[4].indices[1] = _ij10[1];
32980 vinfos[4].maxsolutions = _nj10;
32981 vinfos[5].jointtype = 1;
32982 vinfos[5].foffset = j11;
32983 vinfos[5].indices[0] = _ij11[0];
32984 vinfos[5].indices[1] = _ij11[1];
32985 vinfos[5].maxsolutions = _nj11;
32986 vinfos[6].jointtype = 1;
32987 vinfos[6].foffset = j12;
32988 vinfos[6].indices[0] = _ij12[0];
32989 vinfos[6].indices[1] = _ij12[1];
32990 vinfos[6].maxsolutions = _nj12;
32991 std::vector<int> vfree(0);
32992 solutions.AddSolution(vinfos, vfree);
32993 }
32994 }
32995 }
32996 }
32997 }
32998 }
32999 } while (0);
33000 if (bgotonextstatement)
33001 {
33002 bool bgotonextstatement = true;
33003 do
33004 {
33005 if (1)
33006 {
33007 bgotonextstatement = false;
33008 continue; // branch miss [j10, j12]
33009 }
33010 } while (0);
33011 if (bgotonextstatement)
33012 {
33013 }
33014 }
33015 }
33016 }
33017 }
33018 else
33019 {
33020 {
33021 IkReal j10array[1], cj10array[1], sj10array[1];
33022 bool j10valid[1] = {false};
33023 _nj10 = 1;
33024 CheckValue<IkReal> x230 = IKPowWithIntegerCheck(sj11, -1);
33025 if (!x230.valid)
33026 {
33027 continue;
33028 }
33029 IkReal x229 = x230.value;
33030 CheckValue<IkReal> x231
33031 = IKPowWithIntegerCheck(new_r12, -1);
33032 if (!x231.valid)
33033 {
33034 continue;
33035 }
33036 if (IKabs(
33037 (x229 * (x231.value)
33038 * (((1.0) + (((-1.0) * (1.0) * (cj11 * cj11)))
33039 + (((-1.0) * (1.0) * (new_r02 * new_r02)))))))
33040 < IKFAST_ATAN2_MAGTHRESH
33041 && IKabs((new_r02 * x229)) < IKFAST_ATAN2_MAGTHRESH
33042 && IKabs(
33043 IKsqr(
33044 (x229 * (x231.value)
33045 * (((1.0)
33046 + (((-1.0) * (1.0) * (cj11 * cj11)))
33047 + (((-1.0) * (1.0)
33048 * (new_r02 * new_r02)))))))
33049 + IKsqr((new_r02 * x229)) - 1)
33050 <= IKFAST_SINCOS_THRESH)
33051 continue;
33052 j10array[0] = IKatan2(
33053 (x229 * (x231.value)
33054 * (((1.0) + (((-1.0) * (1.0) * (cj11 * cj11)))
33055 + (((-1.0) * (1.0) * (new_r02 * new_r02)))))),
33056 (new_r02 * x229));
33057 sj10array[0] = IKsin(j10array[0]);
33058 cj10array[0] = IKcos(j10array[0]);
33059 if (j10array[0] > IKPI)
33060 {
33061 j10array[0] -= IK2PI;
33062 }
33063 else if (j10array[0] < -IKPI)
33064 {
33065 j10array[0] += IK2PI;
33066 }
33067 j10valid[0] = true;
33068 for (int ij10 = 0; ij10 < 1; ++ij10)
33069 {
33070 if (!j10valid[ij10])
33071 {
33072 continue;
33073 }
33074 _ij10[0] = ij10;
33075 _ij10[1] = -1;
33076 for (int iij10 = ij10 + 1; iij10 < 1; ++iij10)
33077 {
33078 if (j10valid[iij10]
33079 && IKabs(cj10array[ij10] - cj10array[iij10])
33080 < IKFAST_SOLUTION_THRESH
33081 && IKabs(sj10array[ij10] - sj10array[iij10])
33082 < IKFAST_SOLUTION_THRESH)
33083 {
33084 j10valid[iij10] = false;
33085 _ij10[1] = iij10;
33086 break;
33087 }
33088 }
33089 j10 = j10array[ij10];
33090 cj10 = cj10array[ij10];
33091 sj10 = sj10array[ij10];
33092 {
33093 IkReal evalcond[8];
33094 IkReal x232 = IKcos(j10);
33095 IkReal x233 = ((1.0) * sj11);
33096 IkReal x234 = (x232 * x233);
33097 IkReal x235 = IKsin(j10);
33098 IkReal x236 = (x233 * x235);
33099 IkReal x237 = (new_r02 * x232);
33100 IkReal x238 = (new_r12 * x235);
33101 IkReal x239 = ((1.0) * cj11);
33102 evalcond[0] = ((((-1.0) * x234)) + new_r02);
33103 evalcond[1] = (new_r12 + (((-1.0) * x236)));
33104 evalcond[2]
33105 = (((new_r12 * x232))
33106 + (((-1.0) * new_r02 * x235)));
33107 evalcond[3] = ((((-1.0) * x233)) + x237 + x238);
33108 evalcond[4]
33109 = (((cj11 * x238)) + (((-1.0) * new_r22 * x233))
33110 + ((cj11 * x237)));
33111 evalcond[5]
33112 = ((((-1.0) * new_r00 * x234))
33113 + (((-1.0) * new_r10 * x236))
33114 + (((-1.0) * new_r20 * x239)));
33115 evalcond[6]
33116 = ((((-1.0) * new_r01 * x234))
33117 + (((-1.0) * new_r21 * x239))
33118 + (((-1.0) * new_r11 * x236)));
33119 evalcond[7]
33120 = ((1.0) + (((-1.0) * new_r22 * x239))
33121 + (((-1.0) * x233 * x238))
33122 + (((-1.0) * x233 * x237)));
33123 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
33124 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
33125 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
33126 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
33127 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
33128 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
33129 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
33130 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
33131 {
33132 continue;
33133 }
33134 }
33135
33136 {
33137 IkReal j12eval[2];
33138 IkReal x240 = ((1.0) * sj9);
33139 IkReal x241 = (cj9 * sj6);
33140 IkReal x242 = x160;
33141 IkReal x243 = (cj4 * sj8);
33142 IkReal x244 = (sj6 * x240);
33143 IkReal x245 = (cj6 * cj9);
33144 IkReal x246 = (((cj8 * x245)) + (((-1.0) * x244)));
33145 IkReal x247 = (((cj9 * x243)) + ((sj4 * x246)));
33146 IkReal x248 = ((1.0) * sj4 * sj8);
33147 IkReal x249
33148 = ((((-1.0) * cj9 * x248)) + ((cj4 * x246)));
33149 IkReal x250 = (sj6 * sj8);
33150 IkReal x251 = x169;
33151 IkReal x252 = x170;
33152 IkReal x253 = (x245 + (((-1.0) * cj8 * x244)));
33153 IkReal x254 = x172;
33154 IkReal x255 = (((sj9 * x243)) + ((sj4 * x254)));
33155 IkReal x256
33156 = (((cj4 * x254)) + (((-1.0) * sj4 * sj8 * x240)));
33157 new_r00
33158 = (((r20 * x242)) + ((r00 * x249))
33159 + ((r10 * x247)));
33160 new_r01
33161 = (((r21 * x242)) + ((r01 * x249))
33162 + ((r11 * x247)));
33163 new_r02
33164 = (((r22 * x242)) + ((r12 * x247))
33165 + ((r02 * x249)));
33166 new_r10
33167 = (((r10 * x251)) + ((r20 * x250))
33168 + ((r00 * x252)));
33169 new_r11
33170 = (((r11 * x251)) + ((r21 * x250))
33171 + ((r01 * x252)));
33172 new_r12
33173 = (((r22 * x250)) + ((r12 * x251))
33174 + ((r02 * x252)));
33175 new_r20
33176 = (((r20 * x253)) + ((r00 * x256))
33177 + ((r10 * x255)));
33178 new_r21
33179 = (((r21 * x253)) + ((r11 * x255))
33180 + ((r01 * x256)));
33181 new_r22
33182 = (((r02 * x256)) + ((r12 * x255))
33183 + ((r22 * x253)));
33184 j12eval[0] = sj11;
33185 j12eval[1] = IKsign(sj11);
33186 if (IKabs(j12eval[0]) < 0.0000010000000000
33187 || IKabs(j12eval[1]) < 0.0000010000000000)
33188 {
33189 {
33190 IkReal j12eval[2];
33191 IkReal x257 = ((1.0) * sj9);
33192 IkReal x258 = (cj9 * sj6);
33193 IkReal x259 = x160;
33194 IkReal x260 = (cj4 * sj8);
33195 IkReal x261 = (sj6 * x257);
33196 IkReal x262 = (cj6 * cj9);
33197 IkReal x263 = (((cj8 * x262)) + (((-1.0) * x261)));
33198 IkReal x264 = (((sj4 * x263)) + ((cj9 * x260)));
33199 IkReal x265 = ((1.0) * sj4 * sj8);
33200 IkReal x266
33201 = ((((-1.0) * cj9 * x265)) + ((cj4 * x263)));
33202 IkReal x267 = (sj6 * sj8);
33203 IkReal x268 = x169;
33204 IkReal x269 = x170;
33205 IkReal x270 = (x262 + (((-1.0) * cj8 * x261)));
33206 IkReal x271 = x172;
33207 IkReal x272 = (((sj4 * x271)) + ((sj9 * x260)));
33208 IkReal x273
33209 = (((cj4 * x271))
33210 + (((-1.0) * sj4 * sj8 * x257)));
33211 new_r00
33212 = (((r00 * x266)) + ((r20 * x259))
33213 + ((r10 * x264)));
33214 new_r01
33215 = (((r21 * x259)) + ((r01 * x266))
33216 + ((r11 * x264)));
33217 new_r02
33218 = (((r02 * x266)) + ((r12 * x264))
33219 + ((r22 * x259)));
33220 new_r10
33221 = (((r20 * x267)) + ((r00 * x269))
33222 + ((r10 * x268)));
33223 new_r11
33224 = (((r01 * x269)) + ((r21 * x267))
33225 + ((r11 * x268)));
33226 new_r12
33227 = (((r22 * x267)) + ((r12 * x268))
33228 + ((r02 * x269)));
33229 new_r20
33230 = (((r00 * x273)) + ((r10 * x272))
33231 + ((r20 * x270)));
33232 new_r21
33233 = (((r11 * x272)) + ((r01 * x273))
33234 + ((r21 * x270)));
33235 new_r22
33236 = (((r12 * x272)) + ((r02 * x273))
33237 + ((r22 * x270)));
33238 j12eval[0] = sj10;
33239 j12eval[1] = sj11;
33240 if (IKabs(j12eval[0]) < 0.0000010000000000
33241 || IKabs(j12eval[1]) < 0.0000010000000000)
33242 {
33243 {
33244 IkReal j12eval[3];
33245 IkReal x274 = ((1.0) * sj9);
33246 IkReal x275 = (cj9 * sj6);
33247 IkReal x276 = x160;
33248 IkReal x277 = (cj4 * sj8);
33249 IkReal x278 = (sj6 * x274);
33250 IkReal x279 = (cj6 * cj9);
33251 IkReal x280
33252 = (((cj8 * x279)) + (((-1.0) * x278)));
33253 IkReal x281 = (((cj9 * x277)) + ((sj4 * x280)));
33254 IkReal x282 = ((1.0) * sj4 * sj8);
33255 IkReal x283
33256 = (((cj4 * x280))
33257 + (((-1.0) * cj9 * x282)));
33258 IkReal x284 = (sj6 * sj8);
33259 IkReal x285 = x169;
33260 IkReal x286 = x170;
33261 IkReal x287 = ((((-1.0) * cj8 * x278)) + x279);
33262 IkReal x288 = x172;
33263 IkReal x289 = (((sj9 * x277)) + ((sj4 * x288)));
33264 IkReal x290
33265 = (((cj4 * x288))
33266 + (((-1.0) * sj4 * sj8 * x274)));
33267 new_r00
33268 = (((r00 * x283)) + ((r10 * x281))
33269 + ((r20 * x276)));
33270 new_r01
33271 = (((r01 * x283)) + ((r11 * x281))
33272 + ((r21 * x276)));
33273 new_r02
33274 = (((r22 * x276)) + ((r02 * x283))
33275 + ((r12 * x281)));
33276 new_r10
33277 = (((r10 * x285)) + ((r00 * x286))
33278 + ((r20 * x284)));
33279 new_r11
33280 = (((r01 * x286)) + ((r21 * x284))
33281 + ((r11 * x285)));
33282 new_r12
33283 = (((r22 * x284)) + ((r02 * x286))
33284 + ((r12 * x285)));
33285 new_r20
33286 = (((r00 * x290)) + ((r20 * x287))
33287 + ((r10 * x289)));
33288 new_r21
33289 = (((r21 * x287)) + ((r11 * x289))
33290 + ((r01 * x290)));
33291 new_r22
33292 = (((r22 * x287)) + ((r12 * x289))
33293 + ((r02 * x290)));
33294 j12eval[0] = cj10;
33295 j12eval[1] = cj11;
33296 j12eval[2] = sj11;
33297 if (IKabs(j12eval[0]) < 0.0000010000000000
33298 || IKabs(j12eval[1]) < 0.0000010000000000
33299 || IKabs(j12eval[2]) < 0.0000010000000000)
33300 {
33301 {
33302 IkReal evalcond[12];
33303 bool bgotonextstatement = true;
33304 do
33305 {
33306 IkReal x291 = ((1.0) * cj11);
33307 IkReal x292
33308 = ((((-1.0) * x291)) + new_r22);
33309 IkReal x293 = ((1.0) * sj11);
33310 IkReal x294
33311 = ((((-1.0) * x293)) + new_r12);
33312 evalcond[0]
33313 = ((-3.14159265358979)
33314 + (IKfmod(
33315 ((3.14159265358979)
33316 + (IKabs(
33317 ((-1.5707963267949)
33318 + j10)))),
33319 6.28318530717959)));
33320 evalcond[1] = x292;
33321 evalcond[2] = x292;
33322 evalcond[3] = new_r02;
33323 evalcond[4] = x294;
33324 evalcond[5] = x294;
33325 evalcond[6]
33326 = ((((-1.0) * new_r22 * x293))
33327 + ((cj11 * new_r12)));
33328 evalcond[7]
33329 = ((((-1.0) * new_r10 * x293))
33330 + (((-1.0) * new_r20 * x291)));
33331 evalcond[8]
33332 = ((((-1.0) * new_r11 * x293))
33333 + (((-1.0) * new_r21 * x291)));
33334 evalcond[9]
33335 = ((1.0) + (((-1.0) * new_r12 * x293))
33336 + (((-1.0) * new_r22 * x291)));
33337 if (IKabs(evalcond[0])
33338 < 0.0000010000000000
33339 && IKabs(evalcond[1])
33340 < 0.0000010000000000
33341 && IKabs(evalcond[2])
33342 < 0.0000010000000000
33343 && IKabs(evalcond[3])
33344 < 0.0000010000000000
33345 && IKabs(evalcond[4])
33346 < 0.0000010000000000
33347 && IKabs(evalcond[5])
33348 < 0.0000010000000000
33349 && IKabs(evalcond[6])
33350 < 0.0000010000000000
33351 && IKabs(evalcond[7])
33352 < 0.0000010000000000
33353 && IKabs(evalcond[8])
33354 < 0.0000010000000000
33355 && IKabs(evalcond[9])
33356 < 0.0000010000000000)
33357 {
33358 bgotonextstatement = false;
33359 {
33360 IkReal j12array[1], cj12array[1],
33361 sj12array[1];
33362 bool j12valid[1] = {false};
33363 _nj12 = 1;
33364 CheckValue<IkReal> x295
33365 = IKPowWithIntegerCheck(
33366 IKsign(new_r12), -1);
33367 if (!x295.valid)
33368 {
33369 continue;
33370 }
33371 CheckValue<IkReal> x296
33372 = IKatan2WithCheck(
33373 IkReal(new_r21),
33374 ((-1.0)
33375 * (((1.0) * new_r20))),
33376 IKFAST_ATAN2_MAGTHRESH);
33377 if (!x296.valid)
33378 {
33379 continue;
33380 }
33381 j12array[0]
33382 = ((-1.5707963267949)
33383 + (((1.5707963267949)
33384 * (x295.value)))
33385 + (x296.value));
33386 sj12array[0] = IKsin(j12array[0]);
33387 cj12array[0] = IKcos(j12array[0]);
33388 if (j12array[0] > IKPI)
33389 {
33390 j12array[0] -= IK2PI;
33391 }
33392 else if (j12array[0] < -IKPI)
33393 {
33394 j12array[0] += IK2PI;
33395 }
33396 j12valid[0] = true;
33397 for (int ij12 = 0; ij12 < 1; ++ij12)
33398 {
33399 if (!j12valid[ij12])
33400 {
33401 continue;
33402 }
33403 _ij12[0] = ij12;
33404 _ij12[1] = -1;
33405 for (int iij12 = ij12 + 1;
33406 iij12 < 1;
33407 ++iij12)
33408 {
33409 if (j12valid[iij12]
33410 && IKabs(
33411 cj12array[ij12]
33412 - cj12array[iij12])
33413 < IKFAST_SOLUTION_THRESH
33414 && IKabs(
33415 sj12array[ij12]
33416 - sj12array[iij12])
33417 < IKFAST_SOLUTION_THRESH)
33418 {
33419 j12valid[iij12] = false;
33420 _ij12[1] = iij12;
33421 break;
33422 }
33423 }
33424 j12 = j12array[ij12];
33425 cj12 = cj12array[ij12];
33426 sj12 = sj12array[ij12];
33427 {
33428 IkReal evalcond[8];
33429 IkReal x297 = IKcos(j12);
33430 IkReal x298 = IKsin(j12);
33431 IkReal x299 = ((1.0) * new_r12);
33432 IkReal x300 = ((1.0) * x297);
33433 IkReal x301 = ((-1.0) * x300);
33434 evalcond[0]
33435 = (((new_r12 * x297))
33436 + new_r20);
33437 evalcond[1]
33438 = (((new_r22 * x298))
33439 + new_r11);
33440 evalcond[2]
33441 = ((((-1.0) * x298 * x299))
33442 + new_r21);
33443 evalcond[3]
33444 = ((((-1.0) * new_r22 * x300))
33445 + new_r10);
33446 evalcond[4]
33447 = ((((-1.0) * (1.0)
33448 * new_r00))
33449 + (((-1.0) * x298)));
33450 evalcond[5]
33451 = (x301
33452 + (((-1.0) * (1.0)
33453 * new_r01)));
33454 evalcond[6]
33455 = (((new_r11 * new_r22))
33456 + x298
33457 + (((-1.0) * new_r21
33458 * x299)));
33459 evalcond[7]
33460 = ((((-1.0) * new_r20 * x299))
33461 + x301
33462 + ((new_r10 * new_r22)));
33463 if (IKabs(evalcond[0])
33464 > IKFAST_EVALCOND_THRESH
33465 || IKabs(evalcond[1])
33466 > IKFAST_EVALCOND_THRESH
33467 || IKabs(evalcond[2])
33468 > IKFAST_EVALCOND_THRESH
33469 || IKabs(evalcond[3])
33470 > IKFAST_EVALCOND_THRESH
33471 || IKabs(evalcond[4])
33472 > IKFAST_EVALCOND_THRESH
33473 || IKabs(evalcond[5])
33474 > IKFAST_EVALCOND_THRESH
33475 || IKabs(evalcond[6])
33476 > IKFAST_EVALCOND_THRESH
33477 || IKabs(evalcond[7])
33478 > IKFAST_EVALCOND_THRESH)
33479 {
33480 continue;
33481 }
33482 }
33483
33484 {
33485 std::vector<
33486 IkSingleDOFSolutionBase<
33487 IkReal> >
33488 vinfos(7);
33489 vinfos[0].jointtype = 1;
33490 vinfos[0].foffset = j4;
33491 vinfos[0].indices[0] = _ij4[0];
33492 vinfos[0].indices[1] = _ij4[1];
33493 vinfos[0].maxsolutions = _nj4;
33494 vinfos[1].jointtype = 1;
33495 vinfos[1].foffset = j6;
33496 vinfos[1].indices[0] = _ij6[0];
33497 vinfos[1].indices[1] = _ij6[1];
33498 vinfos[1].maxsolutions = _nj6;
33499 vinfos[2].jointtype = 1;
33500 vinfos[2].foffset = j8;
33501 vinfos[2].indices[0] = _ij8[0];
33502 vinfos[2].indices[1] = _ij8[1];
33503 vinfos[2].maxsolutions = _nj8;
33504 vinfos[3].jointtype = 1;
33505 vinfos[3].foffset = j9;
33506 vinfos[3].indices[0] = _ij9[0];
33507 vinfos[3].indices[1] = _ij9[1];
33508 vinfos[3].maxsolutions = _nj9;
33509 vinfos[4].jointtype = 1;
33510 vinfos[4].foffset = j10;
33511 vinfos[4].indices[0] = _ij10[0];
33512 vinfos[4].indices[1] = _ij10[1];
33513 vinfos[4].maxsolutions = _nj10;
33514 vinfos[5].jointtype = 1;
33515 vinfos[5].foffset = j11;
33516 vinfos[5].indices[0] = _ij11[0];
33517 vinfos[5].indices[1] = _ij11[1];
33518 vinfos[5].maxsolutions = _nj11;
33519 vinfos[6].jointtype = 1;
33520 vinfos[6].foffset = j12;
33521 vinfos[6].indices[0] = _ij12[0];
33522 vinfos[6].indices[1] = _ij12[1];
33523 vinfos[6].maxsolutions = _nj12;
33524 std::vector<int> vfree(0);
33525 solutions.AddSolution(
33526 vinfos, vfree);
33527 }
33528 }
33529 }
33530 }
33531 } while (0);
33532 if (bgotonextstatement)
33533 {
33534 bool bgotonextstatement = true;
33535 do
33536 {
33537 IkReal x302 = ((1.0) * cj11);
33538 IkReal x303
33539 = ((((-1.0) * x302)) + new_r22);
33540 IkReal x304 = ((1.0) * sj11);
33541 evalcond[0]
33542 = ((-3.14159265358979)
33543 + (IKfmod(
33544 ((3.14159265358979)
33545 + (IKabs(
33546 ((1.5707963267949)
33547 + j10)))),
33548 6.28318530717959)));
33549 evalcond[1] = x303;
33550 evalcond[2] = x303;
33551 evalcond[3] = new_r02;
33552 evalcond[4] = (sj11 + new_r12);
33553 evalcond[5]
33554 = ((((-1.0) * (1.0) * new_r12))
33555 + (((-1.0) * x304)));
33556 evalcond[6]
33557 = ((((-1.0) * new_r22 * x304))
33558 + (((-1.0) * new_r12 * x302)));
33559 evalcond[7]
33560 = (((new_r10 * sj11))
33561 + (((-1.0) * new_r20 * x302)));
33562 evalcond[8]
33563 = ((((-1.0) * new_r21 * x302))
33564 + ((new_r11 * sj11)));
33565 evalcond[9]
33566 = ((1.0)
33567 + (((-1.0) * new_r22 * x302))
33568 + ((new_r12 * sj11)));
33569 if (IKabs(evalcond[0])
33570 < 0.0000010000000000
33571 && IKabs(evalcond[1])
33572 < 0.0000010000000000
33573 && IKabs(evalcond[2])
33574 < 0.0000010000000000
33575 && IKabs(evalcond[3])
33576 < 0.0000010000000000
33577 && IKabs(evalcond[4])
33578 < 0.0000010000000000
33579 && IKabs(evalcond[5])
33580 < 0.0000010000000000
33581 && IKabs(evalcond[6])
33582 < 0.0000010000000000
33583 && IKabs(evalcond[7])
33584 < 0.0000010000000000
33585 && IKabs(evalcond[8])
33586 < 0.0000010000000000
33587 && IKabs(evalcond[9])
33588 < 0.0000010000000000)
33589 {
33590 bgotonextstatement = false;
33591 {
33592 IkReal j12array[1], cj12array[1],
33593 sj12array[1];
33594 bool j12valid[1] = {false};
33595 _nj12 = 1;
33596 if (IKabs(new_r00)
33597 < IKFAST_ATAN2_MAGTHRESH
33598 && IKabs(new_r01)
33599 < IKFAST_ATAN2_MAGTHRESH
33600 && IKabs(
33601 IKsqr(new_r00)
33602 + IKsqr(new_r01) - 1)
33603 <= IKFAST_SINCOS_THRESH)
33604 continue;
33605 j12array[0]
33606 = IKatan2(new_r00, new_r01);
33607 sj12array[0] = IKsin(j12array[0]);
33608 cj12array[0] = IKcos(j12array[0]);
33609 if (j12array[0] > IKPI)
33610 {
33611 j12array[0] -= IK2PI;
33612 }
33613 else if (j12array[0] < -IKPI)
33614 {
33615 j12array[0] += IK2PI;
33616 }
33617 j12valid[0] = true;
33618 for (int ij12 = 0; ij12 < 1; ++ij12)
33619 {
33620 if (!j12valid[ij12])
33621 {
33622 continue;
33623 }
33624 _ij12[0] = ij12;
33625 _ij12[1] = -1;
33626 for (int iij12 = ij12 + 1;
33627 iij12 < 1;
33628 ++iij12)
33629 {
33630 if (j12valid[iij12]
33631 && IKabs(
33632 cj12array[ij12]
33633 - cj12array[iij12])
33634 < IKFAST_SOLUTION_THRESH
33635 && IKabs(
33636 sj12array[ij12]
33637 - sj12array[iij12])
33638 < IKFAST_SOLUTION_THRESH)
33639 {
33640 j12valid[iij12] = false;
33641 _ij12[1] = iij12;
33642 break;
33643 }
33644 }
33645 j12 = j12array[ij12];
33646 cj12 = cj12array[ij12];
33647 sj12 = sj12array[ij12];
33648 {
33649 IkReal evalcond[8];
33650 IkReal x305 = IKsin(j12);
33651 IkReal x306
33652 = ((1.0) * (IKcos(j12)));
33653 IkReal x307 = ((-1.0) * x306);
33654 IkReal x308 = ((1.0) * new_r11);
33655 IkReal x309 = ((1.0) * new_r10);
33656 evalcond[0]
33657 = (((new_r12 * x305))
33658 + new_r21);
33659 evalcond[1]
33660 = ((((-1.0) * x305))
33661 + new_r00);
33662 evalcond[2] = (x307 + new_r01);
33663 evalcond[3]
33664 = ((((-1.0) * new_r12
33665 * x306))
33666 + new_r20);
33667 evalcond[4]
33668 = ((((-1.0) * x308))
33669 + ((new_r22 * x305)));
33670 evalcond[5]
33671 = ((((-1.0) * x309))
33672 + (((-1.0) * new_r22
33673 * x306)));
33674 evalcond[6]
33675 = (x305
33676 + ((new_r12 * new_r21))
33677 + (((-1.0) * new_r22
33678 * x308)));
33679 evalcond[7]
33680 = (x307
33681 + ((new_r12 * new_r20))
33682 + (((-1.0) * new_r22
33683 * x309)));
33684 if (IKabs(evalcond[0])
33685 > IKFAST_EVALCOND_THRESH
33686 || IKabs(evalcond[1])
33687 > IKFAST_EVALCOND_THRESH
33688 || IKabs(evalcond[2])
33689 > IKFAST_EVALCOND_THRESH
33690 || IKabs(evalcond[3])
33691 > IKFAST_EVALCOND_THRESH
33692 || IKabs(evalcond[4])
33693 > IKFAST_EVALCOND_THRESH
33694 || IKabs(evalcond[5])
33695 > IKFAST_EVALCOND_THRESH
33696 || IKabs(evalcond[6])
33697 > IKFAST_EVALCOND_THRESH
33698 || IKabs(evalcond[7])
33699 > IKFAST_EVALCOND_THRESH)
33700 {
33701 continue;
33702 }
33703 }
33704
33705 {
33706 std::vector<
33707 IkSingleDOFSolutionBase<
33708 IkReal> >
33709 vinfos(7);
33710 vinfos[0].jointtype = 1;
33711 vinfos[0].foffset = j4;
33712 vinfos[0].indices[0] = _ij4[0];
33713 vinfos[0].indices[1] = _ij4[1];
33714 vinfos[0].maxsolutions = _nj4;
33715 vinfos[1].jointtype = 1;
33716 vinfos[1].foffset = j6;
33717 vinfos[1].indices[0] = _ij6[0];
33718 vinfos[1].indices[1] = _ij6[1];
33719 vinfos[1].maxsolutions = _nj6;
33720 vinfos[2].jointtype = 1;
33721 vinfos[2].foffset = j8;
33722 vinfos[2].indices[0] = _ij8[0];
33723 vinfos[2].indices[1] = _ij8[1];
33724 vinfos[2].maxsolutions = _nj8;
33725 vinfos[3].jointtype = 1;
33726 vinfos[3].foffset = j9;
33727 vinfos[3].indices[0] = _ij9[0];
33728 vinfos[3].indices[1] = _ij9[1];
33729 vinfos[3].maxsolutions = _nj9;
33730 vinfos[4].jointtype = 1;
33731 vinfos[4].foffset = j10;
33732 vinfos[4].indices[0] = _ij10[0];
33733 vinfos[4].indices[1] = _ij10[1];
33734 vinfos[4].maxsolutions = _nj10;
33735 vinfos[5].jointtype = 1;
33736 vinfos[5].foffset = j11;
33737 vinfos[5].indices[0] = _ij11[0];
33738 vinfos[5].indices[1] = _ij11[1];
33739 vinfos[5].maxsolutions = _nj11;
33740 vinfos[6].jointtype = 1;
33741 vinfos[6].foffset = j12;
33742 vinfos[6].indices[0] = _ij12[0];
33743 vinfos[6].indices[1] = _ij12[1];
33744 vinfos[6].maxsolutions = _nj12;
33745 std::vector<int> vfree(0);
33746 solutions.AddSolution(
33747 vinfos, vfree);
33748 }
33749 }
33750 }
33751 }
33752 } while (0);
33753 if (bgotonextstatement)
33754 {
33755 bool bgotonextstatement = true;
33756 do
33757 {
33758 IkReal x310 = ((1.0) * cj10);
33759 IkReal x311 = ((1.0) * sj10);
33760 IkReal x312
33761 = ((((-1.0) * new_r02 * x311))
33762 + ((cj10 * new_r12)));
33763 evalcond[0]
33764 = ((-3.14159265358979)
33765 + (IKfmod(
33766 ((3.14159265358979)
33767 + (IKabs((
33768 (-1.5707963267949)
33769 + j11)))),
33770 6.28318530717959)));
33771 evalcond[1] = new_r22;
33772 evalcond[2]
33773 = ((((-1.0) * x310)) + new_r02);
33774 evalcond[3]
33775 = (new_r12 + (((-1.0) * x311)));
33776 evalcond[4] = x312;
33777 evalcond[5] = x312;
33778 evalcond[6]
33779 = ((-1.0) + ((cj10 * new_r02))
33780 + ((new_r12 * sj10)));
33781 evalcond[7]
33782 = (((cj10 * new_r01))
33783 + ((new_r11 * sj10)));
33784 evalcond[8]
33785 = (((cj10 * new_r00))
33786 + ((new_r10 * sj10)));
33787 evalcond[9]
33788 = ((((-1.0) * new_r10 * x311))
33789 + (((-1.0) * new_r00 * x310)));
33790 evalcond[10]
33791 = ((((-1.0) * new_r01 * x310))
33792 + (((-1.0) * new_r11 * x311)));
33793 evalcond[11]
33794 = ((1.0)
33795 + (((-1.0) * new_r12 * x311))
33796 + (((-1.0) * new_r02 * x310)));
33797 if (IKabs(evalcond[0])
33798 < 0.0000010000000000
33799 && IKabs(evalcond[1])
33800 < 0.0000010000000000
33801 && IKabs(evalcond[2])
33802 < 0.0000010000000000
33803 && IKabs(evalcond[3])
33804 < 0.0000010000000000
33805 && IKabs(evalcond[4])
33806 < 0.0000010000000000
33807 && IKabs(evalcond[5])
33808 < 0.0000010000000000
33809 && IKabs(evalcond[6])
33810 < 0.0000010000000000
33811 && IKabs(evalcond[7])
33812 < 0.0000010000000000
33813 && IKabs(evalcond[8])
33814 < 0.0000010000000000
33815 && IKabs(evalcond[9])
33816 < 0.0000010000000000
33817 && IKabs(evalcond[10])
33818 < 0.0000010000000000
33819 && IKabs(evalcond[11])
33820 < 0.0000010000000000)
33821 {
33822 bgotonextstatement = false;
33823 {
33824 IkReal j12array[1], cj12array[1],
33825 sj12array[1];
33826 bool j12valid[1] = {false};
33827 _nj12 = 1;
33828 if (IKabs(new_r21)
33829 < IKFAST_ATAN2_MAGTHRESH
33830 && IKabs((
33831 (-1.0)
33832 * (((1.0) * new_r20))))
33833 < IKFAST_ATAN2_MAGTHRESH
33834 && IKabs(
33835 IKsqr(new_r21)
33836 + IKsqr((
33837 (-1.0)
33838 * (((1.0)
33839 * new_r20))))
33840 - 1)
33841 <= IKFAST_SINCOS_THRESH)
33842 continue;
33843 j12array[0] = IKatan2(
33844 new_r21,
33845 ((-1.0)
33846 * (((1.0) * new_r20))));
33847 sj12array[0] = IKsin(j12array[0]);
33848 cj12array[0] = IKcos(j12array[0]);
33849 if (j12array[0] > IKPI)
33850 {
33851 j12array[0] -= IK2PI;
33852 }
33853 else if (j12array[0] < -IKPI)
33854 {
33855 j12array[0] += IK2PI;
33856 }
33857 j12valid[0] = true;
33858 for (int ij12 = 0; ij12 < 1;
33859 ++ij12)
33860 {
33861 if (!j12valid[ij12])
33862 {
33863 continue;
33864 }
33865 _ij12[0] = ij12;
33866 _ij12[1] = -1;
33867 for (int iij12 = ij12 + 1;
33868 iij12 < 1;
33869 ++iij12)
33870 {
33871 if (j12valid[iij12]
33872 && IKabs(
33873 cj12array[ij12]
33874 - cj12array[iij12])
33875 < IKFAST_SOLUTION_THRESH
33876 && IKabs(
33877 sj12array[ij12]
33878 - sj12array[iij12])
33879 < IKFAST_SOLUTION_THRESH)
33880 {
33881 j12valid[iij12] = false;
33882 _ij12[1] = iij12;
33883 break;
33884 }
33885 }
33886 j12 = j12array[ij12];
33887 cj12 = cj12array[ij12];
33888 sj12 = sj12array[ij12];
33889 {
33890 IkReal evalcond[8];
33891 IkReal x313 = IKcos(j12);
33892 IkReal x314 = IKsin(j12);
33893 IkReal x315 = ((1.0) * x314);
33894 IkReal x316 = ((-1.0) * x315);
33895 IkReal x317 = ((1.0) * x313);
33896 IkReal x318
33897 = ((1.0) * new_r12);
33898 evalcond[0]
33899 = (x313 + new_r20);
33900 evalcond[1]
33901 = (x316 + new_r21);
33902 evalcond[2]
33903 = (((new_r12 * x313))
33904 + new_r01);
33905 evalcond[3]
33906 = (((new_r12 * x314))
33907 + new_r00);
33908 evalcond[4]
33909 = ((((-1.0) * new_r02
33910 * x317))
33911 + new_r11);
33912 evalcond[5]
33913 = (new_r10
33914 + (((-1.0) * new_r02
33915 * x315)));
33916 evalcond[6]
33917 = ((((-1.0) * new_r00
33918 * x318))
33919 + x316
33920 + ((new_r02
33921 * new_r10)));
33922 evalcond[7]
33923 = (((new_r02 * new_r11))
33924 + (((-1.0) * new_r01
33925 * x318))
33926 + (((-1.0) * x317)));
33927 if (IKabs(evalcond[0])
33928 > IKFAST_EVALCOND_THRESH
33929 || IKabs(evalcond[1])
33930 > IKFAST_EVALCOND_THRESH
33931 || IKabs(evalcond[2])
33932 > IKFAST_EVALCOND_THRESH
33933 || IKabs(evalcond[3])
33934 > IKFAST_EVALCOND_THRESH
33935 || IKabs(evalcond[4])
33936 > IKFAST_EVALCOND_THRESH
33937 || IKabs(evalcond[5])
33938 > IKFAST_EVALCOND_THRESH
33939 || IKabs(evalcond[6])
33940 > IKFAST_EVALCOND_THRESH
33941 || IKabs(evalcond[7])
33942 > IKFAST_EVALCOND_THRESH)
33943 {
33944 continue;
33945 }
33946 }
33947
33948 {
33949 std::vector<
33950 IkSingleDOFSolutionBase<
33951 IkReal> >
33952 vinfos(7);
33953 vinfos[0].jointtype = 1;
33954 vinfos[0].foffset = j4;
33955 vinfos[0].indices[0]
33956 = _ij4[0];
33957 vinfos[0].indices[1]
33958 = _ij4[1];
33959 vinfos[0].maxsolutions = _nj4;
33960 vinfos[1].jointtype = 1;
33961 vinfos[1].foffset = j6;
33962 vinfos[1].indices[0]
33963 = _ij6[0];
33964 vinfos[1].indices[1]
33965 = _ij6[1];
33966 vinfos[1].maxsolutions = _nj6;
33967 vinfos[2].jointtype = 1;
33968 vinfos[2].foffset = j8;
33969 vinfos[2].indices[0]
33970 = _ij8[0];
33971 vinfos[2].indices[1]
33972 = _ij8[1];
33973 vinfos[2].maxsolutions = _nj8;
33974 vinfos[3].jointtype = 1;
33975 vinfos[3].foffset = j9;
33976 vinfos[3].indices[0]
33977 = _ij9[0];
33978 vinfos[3].indices[1]
33979 = _ij9[1];
33980 vinfos[3].maxsolutions = _nj9;
33981 vinfos[4].jointtype = 1;
33982 vinfos[4].foffset = j10;
33983 vinfos[4].indices[0]
33984 = _ij10[0];
33985 vinfos[4].indices[1]
33986 = _ij10[1];
33987 vinfos[4].maxsolutions
33988 = _nj10;
33989 vinfos[5].jointtype = 1;
33990 vinfos[5].foffset = j11;
33991 vinfos[5].indices[0]
33992 = _ij11[0];
33993 vinfos[5].indices[1]
33994 = _ij11[1];
33995 vinfos[5].maxsolutions
33996 = _nj11;
33997 vinfos[6].jointtype = 1;
33998 vinfos[6].foffset = j12;
33999 vinfos[6].indices[0]
34000 = _ij12[0];
34001 vinfos[6].indices[1]
34002 = _ij12[1];
34003 vinfos[6].maxsolutions
34004 = _nj12;
34005 std::vector<int> vfree(0);
34006 solutions.AddSolution(
34007 vinfos, vfree);
34008 }
34009 }
34010 }
34011 }
34012 } while (0);
34013 if (bgotonextstatement)
34014 {
34015 bool bgotonextstatement = true;
34016 do
34017 {
34018 IkReal x319
34019 = ((((-1.0) * (1.0) * new_r02
34020 * sj10))
34021 + ((cj10 * new_r12)));
34022 IkReal x320
34023 = ((1.0) + ((cj10 * new_r02))
34024 + ((new_r12 * sj10)));
34025 IkReal x321
34026 = (((cj10 * new_r01))
34027 + ((new_r11 * sj10)));
34028 IkReal x322
34029 = (((cj10 * new_r00))
34030 + ((new_r10 * sj10)));
34031 evalcond[0]
34032 = ((-3.14159265358979)
34033 + (IKfmod(
34034 ((3.14159265358979)
34035 + (IKabs((
34036 (1.5707963267949)
34037 + j11)))),
34038 6.28318530717959)));
34039 evalcond[1] = new_r22;
34040 evalcond[2] = (cj10 + new_r02);
34041 evalcond[3] = (sj10 + new_r12);
34042 evalcond[4] = x319;
34043 evalcond[5] = x319;
34044 evalcond[6] = x320;
34045 evalcond[7] = x321;
34046 evalcond[8] = x322;
34047 evalcond[9] = x322;
34048 evalcond[10] = x321;
34049 evalcond[11] = x320;
34050 if (IKabs(evalcond[0])
34051 < 0.0000010000000000
34052 && IKabs(evalcond[1])
34053 < 0.0000010000000000
34054 && IKabs(evalcond[2])
34055 < 0.0000010000000000
34056 && IKabs(evalcond[3])
34057 < 0.0000010000000000
34058 && IKabs(evalcond[4])
34059 < 0.0000010000000000
34060 && IKabs(evalcond[5])
34061 < 0.0000010000000000
34062 && IKabs(evalcond[6])
34063 < 0.0000010000000000
34064 && IKabs(evalcond[7])
34065 < 0.0000010000000000
34066 && IKabs(evalcond[8])
34067 < 0.0000010000000000
34068 && IKabs(evalcond[9])
34069 < 0.0000010000000000
34070 && IKabs(evalcond[10])
34071 < 0.0000010000000000
34072 && IKabs(evalcond[11])
34073 < 0.0000010000000000)
34074 {
34075 bgotonextstatement = false;
34076 {
34077 IkReal j12array[1],
34078 cj12array[1], sj12array[1];
34079 bool j12valid[1] = {false};
34080 _nj12 = 1;
34081 if (IKabs(
34082 ((-1.0)
34083 * (((1.0) * new_r21))))
34084 < IKFAST_ATAN2_MAGTHRESH
34085 && IKabs(new_r20)
34086 < IKFAST_ATAN2_MAGTHRESH
34087 && IKabs(
34088 IKsqr((
34089 (-1.0)
34090 * (((1.0)
34091 * new_r21))))
34092 + IKsqr(new_r20) - 1)
34093 <= IKFAST_SINCOS_THRESH)
34094 continue;
34095 j12array[0] = IKatan2(
34096 ((-1.0)
34097 * (((1.0) * new_r21))),
34098 new_r20);
34099 sj12array[0]
34100 = IKsin(j12array[0]);
34101 cj12array[0]
34102 = IKcos(j12array[0]);
34103 if (j12array[0] > IKPI)
34104 {
34105 j12array[0] -= IK2PI;
34106 }
34107 else if (j12array[0] < -IKPI)
34108 {
34109 j12array[0] += IK2PI;
34110 }
34111 j12valid[0] = true;
34112 for (int ij12 = 0; ij12 < 1;
34113 ++ij12)
34114 {
34115 if (!j12valid[ij12])
34116 {
34117 continue;
34118 }
34119 _ij12[0] = ij12;
34120 _ij12[1] = -1;
34121 for (int iij12 = ij12 + 1;
34122 iij12 < 1;
34123 ++iij12)
34124 {
34125 if (j12valid[iij12]
34126 && IKabs(
34127 cj12array[ij12]
34128 - cj12array
34129 [iij12])
34130 < IKFAST_SOLUTION_THRESH
34131 && IKabs(
34132 sj12array[ij12]
34133 - sj12array
34134 [iij12])
34135 < IKFAST_SOLUTION_THRESH)
34136 {
34137 j12valid[iij12] = false;
34138 _ij12[1] = iij12;
34139 break;
34140 }
34141 }
34142 j12 = j12array[ij12];
34143 cj12 = cj12array[ij12];
34144 sj12 = sj12array[ij12];
34145 {
34146 IkReal evalcond[8];
34147 IkReal x323 = IKsin(j12);
34148 IkReal x324 = IKcos(j12);
34149 IkReal x325
34150 = ((1.0) * x324);
34151 IkReal x326
34152 = ((-1.0) * x325);
34153 IkReal x327
34154 = ((1.0) * x323);
34155 IkReal x328
34156 = ((1.0) * new_r02);
34157 evalcond[0]
34158 = (x323 + new_r21);
34159 evalcond[1]
34160 = (x326 + new_r20);
34161 evalcond[2]
34162 = (((new_r02 * x324))
34163 + new_r11);
34164 evalcond[3]
34165 = (((new_r02 * x323))
34166 + new_r10);
34167 evalcond[4]
34168 = ((((-1.0) * new_r12
34169 * x325))
34170 + new_r01);
34171 evalcond[5]
34172 = ((((-1.0) * new_r12
34173 * x327))
34174 + new_r00);
34175 evalcond[6]
34176 = (((new_r00 * new_r12))
34177 + (((-1.0) * x327))
34178 + (((-1.0) * new_r10
34179 * x328)));
34180 evalcond[7]
34181 = (((new_r01 * new_r12))
34182 + x326
34183 + (((-1.0) * new_r11
34184 * x328)));
34185 if (IKabs(evalcond[0])
34186 > IKFAST_EVALCOND_THRESH
34187 || IKabs(evalcond[1])
34188 > IKFAST_EVALCOND_THRESH
34189 || IKabs(evalcond[2])
34190 > IKFAST_EVALCOND_THRESH
34191 || IKabs(evalcond[3])
34192 > IKFAST_EVALCOND_THRESH
34193 || IKabs(evalcond[4])
34194 > IKFAST_EVALCOND_THRESH
34195 || IKabs(evalcond[5])
34196 > IKFAST_EVALCOND_THRESH
34197 || IKabs(evalcond[6])
34198 > IKFAST_EVALCOND_THRESH
34199 || IKabs(evalcond[7])
34200 > IKFAST_EVALCOND_THRESH)
34201 {
34202 continue;
34203 }
34204 }
34205
34206 {
34207 std::vector<
34208 IkSingleDOFSolutionBase<
34209 IkReal> >
34210 vinfos(7);
34211 vinfos[0].jointtype = 1;
34212 vinfos[0].foffset = j4;
34213 vinfos[0].indices[0]
34214 = _ij4[0];
34215 vinfos[0].indices[1]
34216 = _ij4[1];
34217 vinfos[0].maxsolutions
34218 = _nj4;
34219 vinfos[1].jointtype = 1;
34220 vinfos[1].foffset = j6;
34221 vinfos[1].indices[0]
34222 = _ij6[0];
34223 vinfos[1].indices[1]
34224 = _ij6[1];
34225 vinfos[1].maxsolutions
34226 = _nj6;
34227 vinfos[2].jointtype = 1;
34228 vinfos[2].foffset = j8;
34229 vinfos[2].indices[0]
34230 = _ij8[0];
34231 vinfos[2].indices[1]
34232 = _ij8[1];
34233 vinfos[2].maxsolutions
34234 = _nj8;
34235 vinfos[3].jointtype = 1;
34236 vinfos[3].foffset = j9;
34237 vinfos[3].indices[0]
34238 = _ij9[0];
34239 vinfos[3].indices[1]
34240 = _ij9[1];
34241 vinfos[3].maxsolutions
34242 = _nj9;
34243 vinfos[4].jointtype = 1;
34244 vinfos[4].foffset = j10;
34245 vinfos[4].indices[0]
34246 = _ij10[0];
34247 vinfos[4].indices[1]
34248 = _ij10[1];
34249 vinfos[4].maxsolutions
34250 = _nj10;
34251 vinfos[5].jointtype = 1;
34252 vinfos[5].foffset = j11;
34253 vinfos[5].indices[0]
34254 = _ij11[0];
34255 vinfos[5].indices[1]
34256 = _ij11[1];
34257 vinfos[5].maxsolutions
34258 = _nj11;
34259 vinfos[6].jointtype = 1;
34260 vinfos[6].foffset = j12;
34261 vinfos[6].indices[0]
34262 = _ij12[0];
34263 vinfos[6].indices[1]
34264 = _ij12[1];
34265 vinfos[6].maxsolutions
34266 = _nj12;
34267 std::vector<int> vfree(0);
34268 solutions.AddSolution(
34269 vinfos, vfree);
34270 }
34271 }
34272 }
34273 }
34274 } while (0);
34275 if (bgotonextstatement)
34276 {
34277 bool bgotonextstatement = true;
34278 do
34279 {
34280 IkReal x329
34281 = ((((-1.0) * (1.0) * new_r02
34282 * sj10))
34283 + ((cj10 * new_r12)));
34284 IkReal x330
34285 = (((cj10 * new_r02))
34286 + ((new_r12 * sj10)));
34287 evalcond[0]
34288 = ((-3.14159265358979)
34289 + (IKfmod(
34290 ((3.14159265358979)
34291 + (IKabs(j11))),
34292 6.28318530717959)));
34293 evalcond[1] = ((-1.0) + new_r22);
34294 evalcond[2] = new_r20;
34295 evalcond[3] = new_r02;
34296 evalcond[4] = new_r12;
34297 evalcond[5] = new_r21;
34298 evalcond[6] = x329;
34299 evalcond[7] = x329;
34300 evalcond[8] = x330;
34301 evalcond[9] = x330;
34302 if (IKabs(evalcond[0])
34303 < 0.0000010000000000
34304 && IKabs(evalcond[1])
34305 < 0.0000010000000000
34306 && IKabs(evalcond[2])
34307 < 0.0000010000000000
34308 && IKabs(evalcond[3])
34309 < 0.0000010000000000
34310 && IKabs(evalcond[4])
34311 < 0.0000010000000000
34312 && IKabs(evalcond[5])
34313 < 0.0000010000000000
34314 && IKabs(evalcond[6])
34315 < 0.0000010000000000
34316 && IKabs(evalcond[7])
34317 < 0.0000010000000000
34318 && IKabs(evalcond[8])
34319 < 0.0000010000000000
34320 && IKabs(evalcond[9])
34321 < 0.0000010000000000)
34322 {
34323 bgotonextstatement = false;
34324 {
34325 IkReal j12array[1],
34326 cj12array[1],
34327 sj12array[1];
34328 bool j12valid[1] = {false};
34329 _nj12 = 1;
34330 IkReal x331
34331 = ((1.0) * new_r01);
34332 if (IKabs(
34333 ((((-1.0) * cj10
34334 * x331))
34335 + (((-1.0) * (1.0)
34336 * new_r00
34337 * sj10))))
34338 < IKFAST_ATAN2_MAGTHRESH
34339 && IKabs(
34340 (((cj10 * new_r00))
34341 + (((-1.0) * sj10
34342 * x331))))
34343 < IKFAST_ATAN2_MAGTHRESH
34344 && IKabs(
34345 IKsqr((
34346 (((-1.0) * cj10
34347 * x331))
34348 + (((-1.0)
34349 * (1.0)
34350 * new_r00
34351 * sj10))))
34352 + IKsqr((
34353 ((cj10
34354 * new_r00))
34355 + (((-1.0)
34356 * sj10
34357 * x331))))
34358 - 1)
34359 <= IKFAST_SINCOS_THRESH)
34360 continue;
34361 j12array[0] = IKatan2(
34362 ((((-1.0) * cj10 * x331))
34363 + (((-1.0) * (1.0)
34364 * new_r00 * sj10))),
34365 (((cj10 * new_r00))
34366 + (((-1.0) * sj10
34367 * x331))));
34368 sj12array[0]
34369 = IKsin(j12array[0]);
34370 cj12array[0]
34371 = IKcos(j12array[0]);
34372 if (j12array[0] > IKPI)
34373 {
34374 j12array[0] -= IK2PI;
34375 }
34376 else if (j12array[0] < -IKPI)
34377 {
34378 j12array[0] += IK2PI;
34379 }
34380 j12valid[0] = true;
34381 for (int ij12 = 0; ij12 < 1;
34382 ++ij12)
34383 {
34384 if (!j12valid[ij12])
34385 {
34386 continue;
34387 }
34388 _ij12[0] = ij12;
34389 _ij12[1] = -1;
34390 for (int iij12 = ij12 + 1;
34391 iij12 < 1;
34392 ++iij12)
34393 {
34394 if (j12valid[iij12]
34395 && IKabs(
34396 cj12array[ij12]
34397 - cj12array
34398 [iij12])
34399 < IKFAST_SOLUTION_THRESH
34400 && IKabs(
34401 sj12array[ij12]
34402 - sj12array
34403 [iij12])
34404 < IKFAST_SOLUTION_THRESH)
34405 {
34406 j12valid[iij12] = false;
34407 _ij12[1] = iij12;
34408 break;
34409 }
34410 }
34411 j12 = j12array[ij12];
34412 cj12 = cj12array[ij12];
34413 sj12 = sj12array[ij12];
34414 {
34415 IkReal evalcond[8];
34416 IkReal x332 = IKsin(j12);
34417 IkReal x333
34418 = (cj10 * x332);
34419 IkReal x334 = IKcos(j12);
34420 IkReal x335
34421 = ((1.0) * x334);
34422 IkReal x336
34423 = ((-1.0) * x335);
34424 IkReal x337
34425 = ((1.0) * sj10);
34426 IkReal x338
34427 = (((sj10 * x332))
34428 + (((-1.0) * cj10
34429 * x335)));
34430 evalcond[0]
34431 = (((cj10 * new_r01))
34432 + x332
34433 + ((new_r11
34434 * sj10)));
34435 evalcond[1]
34436 = (((sj10 * x334))
34437 + x333 + new_r01);
34438 evalcond[2]
34439 = (((cj10 * new_r00))
34440 + ((new_r10
34441 * sj10))
34442 + x336);
34443 evalcond[3]
34444 = (((cj10 * new_r10))
34445 + (((-1.0) * x332))
34446 + (((-1.0)
34447 * new_r00
34448 * x337)));
34449 evalcond[4]
34450 = (((cj10 * new_r11))
34451 + x336
34452 + (((-1.0)
34453 * new_r01
34454 * x337)));
34455 evalcond[5]
34456 = (x338 + new_r00);
34457 evalcond[6]
34458 = (x338 + new_r11);
34459 evalcond[7]
34460 = ((((-1.0) * x334
34461 * x337))
34462 + (((-1.0) * x333))
34463 + new_r10);
34464 if (IKabs(evalcond[0])
34465 > IKFAST_EVALCOND_THRESH
34466 || IKabs(evalcond[1])
34467 > IKFAST_EVALCOND_THRESH
34468 || IKabs(evalcond[2])
34469 > IKFAST_EVALCOND_THRESH
34470 || IKabs(evalcond[3])
34471 > IKFAST_EVALCOND_THRESH
34472 || IKabs(evalcond[4])
34473 > IKFAST_EVALCOND_THRESH
34474 || IKabs(evalcond[5])
34475 > IKFAST_EVALCOND_THRESH
34476 || IKabs(evalcond[6])
34477 > IKFAST_EVALCOND_THRESH
34478 || IKabs(evalcond[7])
34479 > IKFAST_EVALCOND_THRESH)
34480 {
34481 continue;
34482 }
34483 }
34484
34485 {
34486 std::vector<
34487 IkSingleDOFSolutionBase<
34488 IkReal> >
34489 vinfos(7);
34490 vinfos[0].jointtype = 1;
34491 vinfos[0].foffset = j4;
34492 vinfos[0].indices[0]
34493 = _ij4[0];
34494 vinfos[0].indices[1]
34495 = _ij4[1];
34496 vinfos[0].maxsolutions
34497 = _nj4;
34498 vinfos[1].jointtype = 1;
34499 vinfos[1].foffset = j6;
34500 vinfos[1].indices[0]
34501 = _ij6[0];
34502 vinfos[1].indices[1]
34503 = _ij6[1];
34504 vinfos[1].maxsolutions
34505 = _nj6;
34506 vinfos[2].jointtype = 1;
34507 vinfos[2].foffset = j8;
34508 vinfos[2].indices[0]
34509 = _ij8[0];
34510 vinfos[2].indices[1]
34511 = _ij8[1];
34512 vinfos[2].maxsolutions
34513 = _nj8;
34514 vinfos[3].jointtype = 1;
34515 vinfos[3].foffset = j9;
34516 vinfos[3].indices[0]
34517 = _ij9[0];
34518 vinfos[3].indices[1]
34519 = _ij9[1];
34520 vinfos[3].maxsolutions
34521 = _nj9;
34522 vinfos[4].jointtype = 1;
34523 vinfos[4].foffset = j10;
34524 vinfos[4].indices[0]
34525 = _ij10[0];
34526 vinfos[4].indices[1]
34527 = _ij10[1];
34528 vinfos[4].maxsolutions
34529 = _nj10;
34530 vinfos[5].jointtype = 1;
34531 vinfos[5].foffset = j11;
34532 vinfos[5].indices[0]
34533 = _ij11[0];
34534 vinfos[5].indices[1]
34535 = _ij11[1];
34536 vinfos[5].maxsolutions
34537 = _nj11;
34538 vinfos[6].jointtype = 1;
34539 vinfos[6].foffset = j12;
34540 vinfos[6].indices[0]
34541 = _ij12[0];
34542 vinfos[6].indices[1]
34543 = _ij12[1];
34544 vinfos[6].maxsolutions
34545 = _nj12;
34546 std::vector<int> vfree(0);
34547 solutions.AddSolution(
34548 vinfos, vfree);
34549 }
34550 }
34551 }
34552 }
34553 } while (0);
34554 if (bgotonextstatement)
34555 {
34556 bool bgotonextstatement = true;
34557 do
34558 {
34559 IkReal x339
34560 = ((((-1.0) * (1.0)
34561 * new_r02 * sj10))
34562 + ((cj10 * new_r12)));
34563 IkReal x340 = (cj10 * new_r02);
34564 IkReal x341 = (new_r12 * sj10);
34565 evalcond[0]
34566 = ((-3.14159265358979)
34567 + (IKfmod(
34568 ((3.14159265358979)
34569 + (IKabs((
34570 (-3.14159265358979)
34571 + j11)))),
34572 6.28318530717959)));
34573 evalcond[1] = ((1.0) + new_r22);
34574 evalcond[2] = new_r20;
34575 evalcond[3] = new_r02;
34576 evalcond[4] = new_r12;
34577 evalcond[5] = new_r21;
34578 evalcond[6] = x339;
34579 evalcond[7] = x339;
34580 evalcond[8] = (x340 + x341);
34581 evalcond[9]
34582 = ((((-1.0) * x341))
34583 + (((-1.0) * x340)));
34584 if (IKabs(evalcond[0])
34585 < 0.0000010000000000
34586 && IKabs(evalcond[1])
34587 < 0.0000010000000000
34588 && IKabs(evalcond[2])
34589 < 0.0000010000000000
34590 && IKabs(evalcond[3])
34591 < 0.0000010000000000
34592 && IKabs(evalcond[4])
34593 < 0.0000010000000000
34594 && IKabs(evalcond[5])
34595 < 0.0000010000000000
34596 && IKabs(evalcond[6])
34597 < 0.0000010000000000
34598 && IKabs(evalcond[7])
34599 < 0.0000010000000000
34600 && IKabs(evalcond[8])
34601 < 0.0000010000000000
34602 && IKabs(evalcond[9])
34603 < 0.0000010000000000)
34604 {
34605 bgotonextstatement = false;
34606 {
34607 IkReal j12array[1],
34608 cj12array[1],
34609 sj12array[1];
34610 bool j12valid[1] = {false};
34611 _nj12 = 1;
34612 IkReal x342
34613 = ((1.0) * new_r00);
34614 if (IKabs(
34615 (((cj10 * new_r01))
34616 + (((-1.0) * sj10
34617 * x342))))
34618 < IKFAST_ATAN2_MAGTHRESH
34619 && IKabs(
34620 ((((-1.0) * cj10
34621 * x342))
34622 + (((-1.0)
34623 * (1.0)
34624 * new_r01
34625 * sj10))))
34626 < IKFAST_ATAN2_MAGTHRESH
34627 && IKabs(
34628 IKsqr((
34629 ((cj10
34630 * new_r01))
34631 + (((-1.0)
34632 * sj10
34633 * x342))))
34634 + IKsqr((
34635 (((-1.0)
34636 * cj10
34637 * x342))
34638 + (((-1.0)
34639 * (1.0)
34640 * new_r01
34641 * sj10))))
34642 - 1)
34643 <= IKFAST_SINCOS_THRESH)
34644 continue;
34645 j12array[0] = IKatan2(
34646 (((cj10 * new_r01))
34647 + (((-1.0) * sj10
34648 * x342))),
34649 ((((-1.0) * cj10
34650 * x342))
34651 + (((-1.0) * (1.0)
34652 * new_r01
34653 * sj10))));
34654 sj12array[0]
34655 = IKsin(j12array[0]);
34656 cj12array[0]
34657 = IKcos(j12array[0]);
34658 if (j12array[0] > IKPI)
34659 {
34660 j12array[0] -= IK2PI;
34661 }
34662 else if (
34663 j12array[0] < -IKPI)
34664 {
34665 j12array[0] += IK2PI;
34666 }
34667 j12valid[0] = true;
34668 for (int ij12 = 0; ij12 < 1;
34669 ++ij12)
34670 {
34671 if (!j12valid[ij12])
34672 {
34673 continue;
34674 }
34675 _ij12[0] = ij12;
34676 _ij12[1] = -1;
34677 for (int iij12 = ij12 + 1;
34678 iij12 < 1;
34679 ++iij12)
34680 {
34681 if (j12valid[iij12]
34682 && IKabs(
34683 cj12array
34684 [ij12]
34685 - cj12array
34686 [iij12])
34687 < IKFAST_SOLUTION_THRESH
34688 && IKabs(
34689 sj12array
34690 [ij12]
34691 - sj12array
34692 [iij12])
34693 < IKFAST_SOLUTION_THRESH)
34694 {
34695 j12valid[iij12]
34696 = false;
34697 _ij12[1] = iij12;
34698 break;
34699 }
34700 }
34701 j12 = j12array[ij12];
34702 cj12 = cj12array[ij12];
34703 sj12 = sj12array[ij12];
34704 {
34705 IkReal evalcond[8];
34706 IkReal x343
34707 = IKcos(j12);
34708 IkReal x344
34709 = IKsin(j12);
34710 IkReal x345
34711 = ((1.0) * x344);
34712 IkReal x346
34713 = ((-1.0) * x345);
34714 IkReal x347
34715 = (cj10 * x343);
34716 IkReal x348
34717 = ((1.0) * sj10);
34718 IkReal x349
34719 = (((sj10 * x343))
34720 + (((-1.0) * cj10
34721 * x345)));
34722 evalcond[0]
34723 = (((cj10
34724 * new_r00))
34725 + ((new_r10
34726 * sj10))
34727 + x343);
34728 evalcond[1]
34729 = (((cj10
34730 * new_r01))
34731 + ((new_r11
34732 * sj10))
34733 + x346);
34734 evalcond[2]
34735 = (((sj10 * x344))
34736 + x347
34737 + new_r00);
34738 evalcond[3]
34739 = (((cj10
34740 * new_r10))
34741 + (((-1.0)
34742 * new_r00
34743 * x348))
34744 + x346);
34745 evalcond[4]
34746 = (((cj10
34747 * new_r11))
34748 + (((-1.0)
34749 * new_r01
34750 * x348))
34751 + (((-1.0)
34752 * x343)));
34753 evalcond[5]
34754 = (x349 + new_r01);
34755 evalcond[6]
34756 = (new_r10 + x349);
34757 evalcond[7]
34758 = ((((-1.0) * x347))
34759 + (((-1.0) * x344
34760 * x348))
34761 + new_r11);
34762 if (IKabs(evalcond[0])
34763 > IKFAST_EVALCOND_THRESH
34764 || IKabs(
34765 evalcond[1])
34766 > IKFAST_EVALCOND_THRESH
34767 || IKabs(
34768 evalcond[2])
34769 > IKFAST_EVALCOND_THRESH
34770 || IKabs(
34771 evalcond[3])
34772 > IKFAST_EVALCOND_THRESH
34773 || IKabs(
34774 evalcond[4])
34775 > IKFAST_EVALCOND_THRESH
34776 || IKabs(
34777 evalcond[5])
34778 > IKFAST_EVALCOND_THRESH
34779 || IKabs(
34780 evalcond[6])
34781 > IKFAST_EVALCOND_THRESH
34782 || IKabs(
34783 evalcond[7])
34784 > IKFAST_EVALCOND_THRESH)
34785 {
34786 continue;
34787 }
34788 }
34789
34790 {
34791 std::vector<
34792 IkSingleDOFSolutionBase<
34793 IkReal> >
34794 vinfos(7);
34795 vinfos[0].jointtype = 1;
34796 vinfos[0].foffset = j4;
34797 vinfos[0].indices[0]
34798 = _ij4[0];
34799 vinfos[0].indices[1]
34800 = _ij4[1];
34801 vinfos[0].maxsolutions
34802 = _nj4;
34803 vinfos[1].jointtype = 1;
34804 vinfos[1].foffset = j6;
34805 vinfos[1].indices[0]
34806 = _ij6[0];
34807 vinfos[1].indices[1]
34808 = _ij6[1];
34809 vinfos[1].maxsolutions
34810 = _nj6;
34811 vinfos[2].jointtype = 1;
34812 vinfos[2].foffset = j8;
34813 vinfos[2].indices[0]
34814 = _ij8[0];
34815 vinfos[2].indices[1]
34816 = _ij8[1];
34817 vinfos[2].maxsolutions
34818 = _nj8;
34819 vinfos[3].jointtype = 1;
34820 vinfos[3].foffset = j9;
34821 vinfos[3].indices[0]
34822 = _ij9[0];
34823 vinfos[3].indices[1]
34824 = _ij9[1];
34825 vinfos[3].maxsolutions
34826 = _nj9;
34827 vinfos[4].jointtype = 1;
34828 vinfos[4].foffset = j10;
34829 vinfos[4].indices[0]
34830 = _ij10[0];
34831 vinfos[4].indices[1]
34832 = _ij10[1];
34833 vinfos[4].maxsolutions
34834 = _nj10;
34835 vinfos[5].jointtype = 1;
34836 vinfos[5].foffset = j11;
34837 vinfos[5].indices[0]
34838 = _ij11[0];
34839 vinfos[5].indices[1]
34840 = _ij11[1];
34841 vinfos[5].maxsolutions
34842 = _nj11;
34843 vinfos[6].jointtype = 1;
34844 vinfos[6].foffset = j12;
34845 vinfos[6].indices[0]
34846 = _ij12[0];
34847 vinfos[6].indices[1]
34848 = _ij12[1];
34849 vinfos[6].maxsolutions
34850 = _nj12;
34851 std::vector<int> vfree(
34852 0);
34853 solutions.AddSolution(
34854 vinfos, vfree);
34855 }
34856 }
34857 }
34858 }
34859 } while (0);
34860 if (bgotonextstatement)
34861 {
34862 bool bgotonextstatement = true;
34863 do
34864 {
34865 IkReal x350 = ((1.0) * cj11);
34866 IkReal x351
34867 = ((((-1.0) * x350))
34868 + new_r22);
34869 IkReal x352 = ((1.0) * sj11);
34870 IkReal x353
34871 = ((((-1.0) * x352))
34872 + new_r02);
34873 evalcond[0]
34874 = ((-3.14159265358979)
34875 + (IKfmod(
34876 ((3.14159265358979)
34877 + (IKabs(j10))),
34878 6.28318530717959)));
34879 evalcond[1] = x351;
34880 evalcond[2] = x351;
34881 evalcond[3] = x353;
34882 evalcond[4] = new_r12;
34883 evalcond[5] = x353;
34884 evalcond[6]
34885 = ((((-1.0) * new_r22
34886 * x352))
34887 + ((cj11 * new_r02)));
34888 evalcond[7]
34889 = ((((-1.0) * new_r00
34890 * x352))
34891 + (((-1.0) * new_r20
34892 * x350)));
34893 evalcond[8]
34894 = ((((-1.0) * new_r01
34895 * x352))
34896 + (((-1.0) * new_r21
34897 * x350)));
34898 evalcond[9]
34899 = ((1.0)
34900 + (((-1.0) * new_r02
34901 * x352))
34902 + (((-1.0) * new_r22
34903 * x350)));
34904 if (IKabs(evalcond[0])
34905 < 0.0000010000000000
34906 && IKabs(evalcond[1])
34907 < 0.0000010000000000
34908 && IKabs(evalcond[2])
34909 < 0.0000010000000000
34910 && IKabs(evalcond[3])
34911 < 0.0000010000000000
34912 && IKabs(evalcond[4])
34913 < 0.0000010000000000
34914 && IKabs(evalcond[5])
34915 < 0.0000010000000000
34916 && IKabs(evalcond[6])
34917 < 0.0000010000000000
34918 && IKabs(evalcond[7])
34919 < 0.0000010000000000
34920 && IKabs(evalcond[8])
34921 < 0.0000010000000000
34922 && IKabs(evalcond[9])
34923 < 0.0000010000000000)
34924 {
34925 bgotonextstatement = false;
34926 {
34927 IkReal j12array[1],
34928 cj12array[1],
34929 sj12array[1];
34930 bool j12valid[1]
34931 = {false};
34932 _nj12 = 1;
34933 if (IKabs(new_r10)
34934 < IKFAST_ATAN2_MAGTHRESH
34935 && IKabs(new_r11)
34936 < IKFAST_ATAN2_MAGTHRESH
34937 && IKabs(
34938 IKsqr(new_r10)
34939 + IKsqr(
34940 new_r11)
34941 - 1)
34942 <= IKFAST_SINCOS_THRESH)
34943 continue;
34944 j12array[0] = IKatan2(
34945 new_r10, new_r11);
34946 sj12array[0]
34947 = IKsin(j12array[0]);
34948 cj12array[0]
34949 = IKcos(j12array[0]);
34950 if (j12array[0] > IKPI)
34951 {
34952 j12array[0] -= IK2PI;
34953 }
34954 else if (
34955 j12array[0] < -IKPI)
34956 {
34957 j12array[0] += IK2PI;
34958 }
34959 j12valid[0] = true;
34960 for (int ij12 = 0;
34961 ij12 < 1;
34962 ++ij12)
34963 {
34964 if (!j12valid[ij12])
34965 {
34966 continue;
34967 }
34968 _ij12[0] = ij12;
34969 _ij12[1] = -1;
34970 for (int iij12
34971 = ij12 + 1;
34972 iij12 < 1;
34973 ++iij12)
34974 {
34975 if (j12valid[iij12]
34976 && IKabs(
34977 cj12array
34978 [ij12]
34979 - cj12array
34980 [iij12])
34981 < IKFAST_SOLUTION_THRESH
34982 && IKabs(
34983 sj12array
34984 [ij12]
34985 - sj12array
34986 [iij12])
34987 < IKFAST_SOLUTION_THRESH)
34988 {
34989 j12valid[iij12]
34990 = false;
34991 _ij12[1] = iij12;
34992 break;
34993 }
34994 }
34995 j12 = j12array[ij12];
34996 cj12 = cj12array[ij12];
34997 sj12 = sj12array[ij12];
34998 {
34999 IkReal evalcond[8];
35000 IkReal x354
35001 = IKcos(j12);
35002 IkReal x355
35003 = IKsin(j12);
35004 IkReal x356
35005 = ((1.0) * x354);
35006 IkReal x357
35007 = ((-1.0) * x356);
35008 IkReal x358
35009 = ((1.0)
35010 * new_r02);
35011 evalcond[0]
35012 = (((new_r02
35013 * x354))
35014 + new_r20);
35015 evalcond[1]
35016 = ((((-1.0)
35017 * x355))
35018 + new_r10);
35019 evalcond[2]
35020 = (x357
35021 + new_r11);
35022 evalcond[3]
35023 = (((new_r22
35024 * x355))
35025 + new_r01);
35026 evalcond[4]
35027 = ((((-1.0) * x355
35028 * x358))
35029 + new_r21);
35030 evalcond[5]
35031 = ((((-1.0)
35032 * new_r22
35033 * x356))
35034 + new_r00);
35035 evalcond[6]
35036 = (x355
35037 + ((new_r01
35038 * new_r22))
35039 + (((-1.0)
35040 * new_r21
35041 * x358)));
35042 evalcond[7]
35043 = (x357
35044 + ((new_r00
35045 * new_r22))
35046 + (((-1.0)
35047 * new_r20
35048 * x358)));
35049 if (IKabs(evalcond[0])
35050 > IKFAST_EVALCOND_THRESH
35051 || IKabs(evalcond
35052 [1])
35053 > IKFAST_EVALCOND_THRESH
35054 || IKabs(evalcond
35055 [2])
35056 > IKFAST_EVALCOND_THRESH
35057 || IKabs(evalcond
35058 [3])
35059 > IKFAST_EVALCOND_THRESH
35060 || IKabs(evalcond
35061 [4])
35062 > IKFAST_EVALCOND_THRESH
35063 || IKabs(evalcond
35064 [5])
35065 > IKFAST_EVALCOND_THRESH
35066 || IKabs(evalcond
35067 [6])
35068 > IKFAST_EVALCOND_THRESH
35069 || IKabs(evalcond
35070 [7])
35071 > IKFAST_EVALCOND_THRESH)
35072 {
35073 continue;
35074 }
35075 }
35076
35077 {
35078 std::vector<
35079 IkSingleDOFSolutionBase<
35080 IkReal> >
35081 vinfos(7);
35082 vinfos[0].jointtype
35083 = 1;
35084 vinfos[0].foffset
35085 = j4;
35086 vinfos[0].indices[0]
35087 = _ij4[0];
35088 vinfos[0].indices[1]
35089 = _ij4[1];
35090 vinfos[0].maxsolutions
35091 = _nj4;
35092 vinfos[1].jointtype
35093 = 1;
35094 vinfos[1].foffset
35095 = j6;
35096 vinfos[1].indices[0]
35097 = _ij6[0];
35098 vinfos[1].indices[1]
35099 = _ij6[1];
35100 vinfos[1].maxsolutions
35101 = _nj6;
35102 vinfos[2].jointtype
35103 = 1;
35104 vinfos[2].foffset
35105 = j8;
35106 vinfos[2].indices[0]
35107 = _ij8[0];
35108 vinfos[2].indices[1]
35109 = _ij8[1];
35110 vinfos[2].maxsolutions
35111 = _nj8;
35112 vinfos[3].jointtype
35113 = 1;
35114 vinfos[3].foffset
35115 = j9;
35116 vinfos[3].indices[0]
35117 = _ij9[0];
35118 vinfos[3].indices[1]
35119 = _ij9[1];
35120 vinfos[3].maxsolutions
35121 = _nj9;
35122 vinfos[4].jointtype
35123 = 1;
35124 vinfos[4].foffset
35125 = j10;
35126 vinfos[4].indices[0]
35127 = _ij10[0];
35128 vinfos[4].indices[1]
35129 = _ij10[1];
35130 vinfos[4].maxsolutions
35131 = _nj10;
35132 vinfos[5].jointtype
35133 = 1;
35134 vinfos[5].foffset
35135 = j11;
35136 vinfos[5].indices[0]
35137 = _ij11[0];
35138 vinfos[5].indices[1]
35139 = _ij11[1];
35140 vinfos[5].maxsolutions
35141 = _nj11;
35142 vinfos[6].jointtype
35143 = 1;
35144 vinfos[6].foffset
35145 = j12;
35146 vinfos[6].indices[0]
35147 = _ij12[0];
35148 vinfos[6].indices[1]
35149 = _ij12[1];
35150 vinfos[6].maxsolutions
35151 = _nj12;
35152 std::vector<int>
35153 vfree(0);
35154 solutions.AddSolution(
35155 vinfos, vfree);
35156 }
35157 }
35158 }
35159 }
35160 } while (0);
35161 if (bgotonextstatement)
35162 {
35163 bool bgotonextstatement
35164 = true;
35165 do
35166 {
35167 IkReal x359
35168 = ((1.0) * cj11);
35169 IkReal x360
35170 = ((((-1.0) * x359))
35171 + new_r22);
35172 IkReal x361
35173 = ((1.0) * sj11);
35174 evalcond[0]
35175 = ((-3.14159265358979)
35176 + (IKfmod(
35177 ((3.14159265358979)
35178 + (IKabs((
35179 (-3.14159265358979)
35180 + j10)))),
35181 6.28318530717959)));
35182 evalcond[1] = x360;
35183 evalcond[2] = x360;
35184 evalcond[3]
35185 = (sj11 + new_r02);
35186 evalcond[4] = new_r12;
35187 evalcond[5]
35188 = ((((-1.0) * (1.0)
35189 * new_r02))
35190 + (((-1.0) * x361)));
35191 evalcond[6]
35192 = ((((-1.0) * new_r02
35193 * x359))
35194 + (((-1.0) * new_r22
35195 * x361)));
35196 evalcond[7]
35197 = ((((-1.0) * new_r20
35198 * x359))
35199 + ((new_r00
35200 * sj11)));
35201 evalcond[8]
35202 = (((new_r01 * sj11))
35203 + (((-1.0) * new_r21
35204 * x359)));
35205 evalcond[9]
35206 = ((1.0)
35207 + (((-1.0) * new_r22
35208 * x359))
35209 + ((new_r02
35210 * sj11)));
35211 if (IKabs(evalcond[0])
35212 < 0.0000010000000000
35213 && IKabs(evalcond[1])
35214 < 0.0000010000000000
35215 && IKabs(evalcond[2])
35216 < 0.0000010000000000
35217 && IKabs(evalcond[3])
35218 < 0.0000010000000000
35219 && IKabs(evalcond[4])
35220 < 0.0000010000000000
35221 && IKabs(evalcond[5])
35222 < 0.0000010000000000
35223 && IKabs(evalcond[6])
35224 < 0.0000010000000000
35225 && IKabs(evalcond[7])
35226 < 0.0000010000000000
35227 && IKabs(evalcond[8])
35228 < 0.0000010000000000
35229 && IKabs(evalcond[9])
35230 < 0.0000010000000000)
35231 {
35232 bgotonextstatement
35233 = false;
35234 {
35235 IkReal j12array[1],
35236 cj12array[1],
35237 sj12array[1];
35238 bool j12valid[1]
35239 = {false};
35240 _nj12 = 1;
35241 CheckValue<IkReal> x362
35242 = IKatan2WithCheck(
35243 IkReal((
35244 (-1.0)
35245 * (((1.0)
35246 * new_r21)))),
35247 new_r20,
35248 IKFAST_ATAN2_MAGTHRESH);
35249 if (!x362.valid)
35250 {
35251 continue;
35252 }
35253 CheckValue<IkReal> x363
35254 = IKPowWithIntegerCheck(
35255 IKsign(new_r02),
35256 -1);
35257 if (!x363.valid)
35258 {
35259 continue;
35260 }
35261 j12array[0]
35262 = ((-1.5707963267949)
35263 + (x362.value)
35264 + (((1.5707963267949)
35265 * (x363.value))));
35266 sj12array[0] = IKsin(
35267 j12array[0]);
35268 cj12array[0] = IKcos(
35269 j12array[0]);
35270 if (j12array[0] > IKPI)
35271 {
35272 j12array[0] -= IK2PI;
35273 }
35274 else if (
35275 j12array[0] < -IKPI)
35276 {
35277 j12array[0] += IK2PI;
35278 }
35279 j12valid[0] = true;
35280 for (int ij12 = 0;
35281 ij12 < 1;
35282 ++ij12)
35283 {
35284 if (!j12valid[ij12])
35285 {
35286 continue;
35287 }
35288 _ij12[0] = ij12;
35289 _ij12[1] = -1;
35290 for (int iij12
35291 = ij12 + 1;
35292 iij12 < 1;
35293 ++iij12)
35294 {
35295 if (j12valid[iij12]
35296 && IKabs(
35297 cj12array
35298 [ij12]
35299 - cj12array
35300 [iij12])
35301 < IKFAST_SOLUTION_THRESH
35302 && IKabs(
35303 sj12array
35304 [ij12]
35305 - sj12array
35306 [iij12])
35307 < IKFAST_SOLUTION_THRESH)
35308 {
35309 j12valid[iij12]
35310 = false;
35311 _ij12[1] = iij12;
35312 break;
35313 }
35314 }
35315 j12 = j12array[ij12];
35316 cj12
35317 = cj12array[ij12];
35318 sj12
35319 = sj12array[ij12];
35320 {
35321 IkReal evalcond[8];
35322 IkReal x364
35323 = IKsin(j12);
35324 IkReal x365
35325 = ((1.0)
35326 * (IKcos(
35327 j12)));
35328 IkReal x366
35329 = ((-1.0)
35330 * x365);
35331 IkReal x367
35332 = ((1.0)
35333 * new_r01);
35334 IkReal x368
35335 = ((1.0)
35336 * new_r00);
35337 evalcond[0]
35338 = (((new_r02
35339 * x364))
35340 + new_r21);
35341 evalcond[1]
35342 = ((((-1.0)
35343 * new_r02
35344 * x365))
35345 + new_r20);
35346 evalcond[2]
35347 = ((((-1.0)
35348 * x364))
35349 + (((-1.0)
35350 * (1.0)
35351 * new_r10)));
35352 evalcond[3]
35353 = (x366
35354 + (((-1.0)
35355 * (1.0)
35356 * new_r11)));
35357 evalcond[4]
35358 = ((((-1.0)
35359 * x367))
35360 + ((new_r22
35361 * x364)));
35362 evalcond[5]
35363 = ((((-1.0)
35364 * new_r22
35365 * x365))
35366 + (((-1.0)
35367 * x368)));
35368 evalcond[6]
35369 = (((new_r02
35370 * new_r21))
35371 + x364
35372 + (((-1.0)
35373 * new_r22
35374 * x367)));
35375 evalcond[7]
35376 = (((new_r02
35377 * new_r20))
35378 + x366
35379 + (((-1.0)
35380 * new_r22
35381 * x368)));
35382 if (IKabs(
35383 evalcond[0])
35384 > IKFAST_EVALCOND_THRESH
35385 || IKabs(
35386 evalcond
35387 [1])
35388 > IKFAST_EVALCOND_THRESH
35389 || IKabs(
35390 evalcond
35391 [2])
35392 > IKFAST_EVALCOND_THRESH
35393 || IKabs(
35394 evalcond
35395 [3])
35396 > IKFAST_EVALCOND_THRESH
35397 || IKabs(
35398 evalcond
35399 [4])
35400 > IKFAST_EVALCOND_THRESH
35401 || IKabs(
35402 evalcond
35403 [5])
35404 > IKFAST_EVALCOND_THRESH
35405 || IKabs(
35406 evalcond
35407 [6])
35408 > IKFAST_EVALCOND_THRESH
35409 || IKabs(
35410 evalcond
35411 [7])
35412 > IKFAST_EVALCOND_THRESH)
35413 {
35414 continue;
35415 }
35416 }
35417
35418 {
35419 std::vector<
35420 IkSingleDOFSolutionBase<
35421 IkReal> >
35422 vinfos(7);
35423 vinfos[0].jointtype
35424 = 1;
35425 vinfos[0].foffset
35426 = j4;
35427 vinfos[0].indices[0]
35428 = _ij4[0];
35429 vinfos[0].indices[1]
35430 = _ij4[1];
35431 vinfos[0]
35432 .maxsolutions
35433 = _nj4;
35434 vinfos[1].jointtype
35435 = 1;
35436 vinfos[1].foffset
35437 = j6;
35438 vinfos[1].indices[0]
35439 = _ij6[0];
35440 vinfos[1].indices[1]
35441 = _ij6[1];
35442 vinfos[1]
35443 .maxsolutions
35444 = _nj6;
35445 vinfos[2].jointtype
35446 = 1;
35447 vinfos[2].foffset
35448 = j8;
35449 vinfos[2].indices[0]
35450 = _ij8[0];
35451 vinfos[2].indices[1]
35452 = _ij8[1];
35453 vinfos[2]
35454 .maxsolutions
35455 = _nj8;
35456 vinfos[3].jointtype
35457 = 1;
35458 vinfos[3].foffset
35459 = j9;
35460 vinfos[3].indices[0]
35461 = _ij9[0];
35462 vinfos[3].indices[1]
35463 = _ij9[1];
35464 vinfos[3]
35465 .maxsolutions
35466 = _nj9;
35467 vinfos[4].jointtype
35468 = 1;
35469 vinfos[4].foffset
35470 = j10;
35471 vinfos[4].indices[0]
35472 = _ij10[0];
35473 vinfos[4].indices[1]
35474 = _ij10[1];
35475 vinfos[4]
35476 .maxsolutions
35477 = _nj10;
35478 vinfos[5].jointtype
35479 = 1;
35480 vinfos[5].foffset
35481 = j11;
35482 vinfos[5].indices[0]
35483 = _ij11[0];
35484 vinfos[5].indices[1]
35485 = _ij11[1];
35486 vinfos[5]
35487 .maxsolutions
35488 = _nj11;
35489 vinfos[6].jointtype
35490 = 1;
35491 vinfos[6].foffset
35492 = j12;
35493 vinfos[6].indices[0]
35494 = _ij12[0];
35495 vinfos[6].indices[1]
35496 = _ij12[1];
35497 vinfos[6]
35498 .maxsolutions
35499 = _nj12;
35500 std::vector<int>
35501 vfree(0);
35502 solutions
35503 .AddSolution(
35504 vinfos,
35505 vfree);
35506 }
35507 }
35508 }
35509 }
35510 } while (0);
35511 if (bgotonextstatement)
35512 {
35513 bool bgotonextstatement
35514 = true;
35515 do
35516 {
35517 if (1)
35518 {
35519 bgotonextstatement
35520 = false;
35521 continue; // branch miss
35522 // [j12]
35523 }
35524 } while (0);
35525 if (bgotonextstatement)
35526 {
35527 }
35528 }
35529 }
35530 }
35531 }
35532 }
35533 }
35534 }
35535 }
35536 }
35537 }
35538 else
35539 {
35540 {
35541 IkReal j12array[1], cj12array[1],
35542 sj12array[1];
35543 bool j12valid[1] = {false};
35544 _nj12 = 1;
35545 CheckValue<IkReal> x370
35546 = IKPowWithIntegerCheck(sj11, -1);
35547 if (!x370.valid)
35548 {
35549 continue;
35550 }
35551 IkReal x369 = x370.value;
35552 CheckValue<IkReal> x371
35553 = IKPowWithIntegerCheck(cj10, -1);
35554 if (!x371.valid)
35555 {
35556 continue;
35557 }
35558 CheckValue<IkReal> x372
35559 = IKPowWithIntegerCheck(cj11, -1);
35560 if (!x372.valid)
35561 {
35562 continue;
35563 }
35564 if (IKabs(
35565 (x369 * (x371.value) * (x372.value)
35566 * (((((-1.0) * (1.0) * new_r01
35567 * sj11))
35568 + ((new_r20 * sj10))))))
35569 < IKFAST_ATAN2_MAGTHRESH
35570 && IKabs(((-1.0) * new_r20 * x369))
35571 < IKFAST_ATAN2_MAGTHRESH
35572 && IKabs(
35573 IKsqr(
35574 (x369 * (x371.value)
35575 * (x372.value)
35576 * (((((-1.0) * (1.0)
35577 * new_r01 * sj11))
35578 + ((new_r20 * sj10))))))
35579 + IKsqr(
35580 ((-1.0) * new_r20 * x369))
35581 - 1)
35582 <= IKFAST_SINCOS_THRESH)
35583 continue;
35584 j12array[0] = IKatan2(
35585 (x369 * (x371.value) * (x372.value)
35586 * (((((-1.0) * (1.0) * new_r01 * sj11))
35587 + ((new_r20 * sj10))))),
35588 ((-1.0) * new_r20 * x369));
35589 sj12array[0] = IKsin(j12array[0]);
35590 cj12array[0] = IKcos(j12array[0]);
35591 if (j12array[0] > IKPI)
35592 {
35593 j12array[0] -= IK2PI;
35594 }
35595 else if (j12array[0] < -IKPI)
35596 {
35597 j12array[0] += IK2PI;
35598 }
35599 j12valid[0] = true;
35600 for (int ij12 = 0; ij12 < 1; ++ij12)
35601 {
35602 if (!j12valid[ij12])
35603 {
35604 continue;
35605 }
35606 _ij12[0] = ij12;
35607 _ij12[1] = -1;
35608 for (int iij12 = ij12 + 1; iij12 < 1;
35609 ++iij12)
35610 {
35611 if (j12valid[iij12]
35612 && IKabs(
35613 cj12array[ij12]
35614 - cj12array[iij12])
35615 < IKFAST_SOLUTION_THRESH
35616 && IKabs(
35617 sj12array[ij12]
35618 - sj12array[iij12])
35619 < IKFAST_SOLUTION_THRESH)
35620 {
35621 j12valid[iij12] = false;
35622 _ij12[1] = iij12;
35623 break;
35624 }
35625 }
35626 j12 = j12array[ij12];
35627 cj12 = cj12array[ij12];
35628 sj12 = sj12array[ij12];
35629 {
35630 IkReal evalcond[12];
35631 IkReal x373 = IKcos(j12);
35632 IkReal x374 = IKsin(j12);
35633 IkReal x375 = ((1.0) * sj11);
35634 IkReal x376 = (cj10 * new_r01);
35635 IkReal x377 = (new_r11 * sj10);
35636 IkReal x378 = (cj11 * x374);
35637 IkReal x379 = ((1.0) * sj10);
35638 IkReal x380 = ((1.0) * x374);
35639 IkReal x381 = ((1.0) * x373);
35640 IkReal x382 = ((-1.0) * x381);
35641 IkReal x383 = (cj10 * new_r00);
35642 IkReal x384 = (new_r10 * sj10);
35643 IkReal x385 = (cj10 * x381);
35644 evalcond[0]
35645 = (((sj11 * x373)) + new_r20);
35646 evalcond[1]
35647 = ((((-1.0) * x374 * x375))
35648 + new_r21);
35649 evalcond[2] = (x378 + x377 + x376);
35650 evalcond[3]
35651 = (((cj10 * new_r10))
35652 + (((-1.0) * x380))
35653 + (((-1.0) * new_r00 * x379)));
35654 evalcond[4]
35655 = (((cj10 * new_r11)) + x382
35656 + (((-1.0) * new_r01 * x379)));
35657 evalcond[5]
35658 = (((sj10 * x373)) + ((cj10 * x378))
35659 + new_r01);
35660 evalcond[6]
35661 = ((((-1.0) * cj11 * x381)) + x384
35662 + x383);
35663 evalcond[7]
35664 = (((sj10 * x374))
35665 + (((-1.0) * cj11 * x385))
35666 + new_r00);
35667 evalcond[8]
35668 = ((((-1.0) * x385)) + new_r11
35669 + ((sj10 * x378)));
35670 evalcond[9]
35671 = ((((-1.0) * cj10 * x380))
35672 + (((-1.0) * cj11 * x373 * x379))
35673 + new_r10);
35674 evalcond[10]
35675 = (((cj11 * x376)) + ((cj11 * x377))
35676 + (((-1.0) * new_r21 * x375))
35677 + x374);
35678 evalcond[11]
35679 = ((((-1.0) * new_r20 * x375))
35680 + x382 + ((cj11 * x384))
35681 + ((cj11 * x383)));
35682 if (IKabs(evalcond[0])
35683 > IKFAST_EVALCOND_THRESH
35684 || IKabs(evalcond[1])
35685 > IKFAST_EVALCOND_THRESH
35686 || IKabs(evalcond[2])
35687 > IKFAST_EVALCOND_THRESH
35688 || IKabs(evalcond[3])
35689 > IKFAST_EVALCOND_THRESH
35690 || IKabs(evalcond[4])
35691 > IKFAST_EVALCOND_THRESH
35692 || IKabs(evalcond[5])
35693 > IKFAST_EVALCOND_THRESH
35694 || IKabs(evalcond[6])
35695 > IKFAST_EVALCOND_THRESH
35696 || IKabs(evalcond[7])
35697 > IKFAST_EVALCOND_THRESH
35698 || IKabs(evalcond[8])
35699 > IKFAST_EVALCOND_THRESH
35700 || IKabs(evalcond[9])
35701 > IKFAST_EVALCOND_THRESH
35702 || IKabs(evalcond[10])
35703 > IKFAST_EVALCOND_THRESH
35704 || IKabs(evalcond[11])
35705 > IKFAST_EVALCOND_THRESH)
35706 {
35707 continue;
35708 }
35709 }
35710
35711 {
35712 std::vector<
35713 IkSingleDOFSolutionBase<IkReal> >
35714 vinfos(7);
35715 vinfos[0].jointtype = 1;
35716 vinfos[0].foffset = j4;
35717 vinfos[0].indices[0] = _ij4[0];
35718 vinfos[0].indices[1] = _ij4[1];
35719 vinfos[0].maxsolutions = _nj4;
35720 vinfos[1].jointtype = 1;
35721 vinfos[1].foffset = j6;
35722 vinfos[1].indices[0] = _ij6[0];
35723 vinfos[1].indices[1] = _ij6[1];
35724 vinfos[1].maxsolutions = _nj6;
35725 vinfos[2].jointtype = 1;
35726 vinfos[2].foffset = j8;
35727 vinfos[2].indices[0] = _ij8[0];
35728 vinfos[2].indices[1] = _ij8[1];
35729 vinfos[2].maxsolutions = _nj8;
35730 vinfos[3].jointtype = 1;
35731 vinfos[3].foffset = j9;
35732 vinfos[3].indices[0] = _ij9[0];
35733 vinfos[3].indices[1] = _ij9[1];
35734 vinfos[3].maxsolutions = _nj9;
35735 vinfos[4].jointtype = 1;
35736 vinfos[4].foffset = j10;
35737 vinfos[4].indices[0] = _ij10[0];
35738 vinfos[4].indices[1] = _ij10[1];
35739 vinfos[4].maxsolutions = _nj10;
35740 vinfos[5].jointtype = 1;
35741 vinfos[5].foffset = j11;
35742 vinfos[5].indices[0] = _ij11[0];
35743 vinfos[5].indices[1] = _ij11[1];
35744 vinfos[5].maxsolutions = _nj11;
35745 vinfos[6].jointtype = 1;
35746 vinfos[6].foffset = j12;
35747 vinfos[6].indices[0] = _ij12[0];
35748 vinfos[6].indices[1] = _ij12[1];
35749 vinfos[6].maxsolutions = _nj12;
35750 std::vector<int> vfree(0);
35751 solutions.AddSolution(vinfos, vfree);
35752 }
35753 }
35754 }
35755 }
35756 }
35757 }
35758 else
35759 {
35760 {
35761 IkReal j12array[1], cj12array[1], sj12array[1];
35762 bool j12valid[1] = {false};
35763 _nj12 = 1;
35764 CheckValue<IkReal> x388
35765 = IKPowWithIntegerCheck(sj11, -1);
35766 if (!x388.valid)
35767 {
35768 continue;
35769 }
35770 IkReal x386 = x388.value;
35771 IkReal x387 = ((1.0) * new_r20);
35772 CheckValue<IkReal> x389
35773 = IKPowWithIntegerCheck(sj10, -1);
35774 if (!x389.valid)
35775 {
35776 continue;
35777 }
35778 if (IKabs(
35779 (x386 * (x389.value)
35780 * (((((-1.0) * cj10 * cj11 * x387))
35781 + (((-1.0) * (1.0) * new_r00
35782 * sj11))))))
35783 < IKFAST_ATAN2_MAGTHRESH
35784 && IKabs(((-1.0) * x386 * x387))
35785 < IKFAST_ATAN2_MAGTHRESH
35786 && IKabs(
35787 IKsqr(
35788 (x386 * (x389.value)
35789 * (((((-1.0) * cj10 * cj11
35790 * x387))
35791 + (((-1.0) * (1.0) * new_r00
35792 * sj11))))))
35793 + IKsqr(((-1.0) * x386 * x387)) - 1)
35794 <= IKFAST_SINCOS_THRESH)
35795 continue;
35796 j12array[0] = IKatan2(
35797 (x386 * (x389.value)
35798 * (((((-1.0) * cj10 * cj11 * x387))
35799 + (((-1.0) * (1.0) * new_r00
35800 * sj11))))),
35801 ((-1.0) * x386 * x387));
35802 sj12array[0] = IKsin(j12array[0]);
35803 cj12array[0] = IKcos(j12array[0]);
35804 if (j12array[0] > IKPI)
35805 {
35806 j12array[0] -= IK2PI;
35807 }
35808 else if (j12array[0] < -IKPI)
35809 {
35810 j12array[0] += IK2PI;
35811 }
35812 j12valid[0] = true;
35813 for (int ij12 = 0; ij12 < 1; ++ij12)
35814 {
35815 if (!j12valid[ij12])
35816 {
35817 continue;
35818 }
35819 _ij12[0] = ij12;
35820 _ij12[1] = -1;
35821 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
35822 {
35823 if (j12valid[iij12]
35824 && IKabs(
35825 cj12array[ij12]
35826 - cj12array[iij12])
35827 < IKFAST_SOLUTION_THRESH
35828 && IKabs(
35829 sj12array[ij12]
35830 - sj12array[iij12])
35831 < IKFAST_SOLUTION_THRESH)
35832 {
35833 j12valid[iij12] = false;
35834 _ij12[1] = iij12;
35835 break;
35836 }
35837 }
35838 j12 = j12array[ij12];
35839 cj12 = cj12array[ij12];
35840 sj12 = sj12array[ij12];
35841 {
35842 IkReal evalcond[12];
35843 IkReal x390 = IKcos(j12);
35844 IkReal x391 = IKsin(j12);
35845 IkReal x392 = ((1.0) * sj11);
35846 IkReal x393 = (cj10 * new_r01);
35847 IkReal x394 = (new_r11 * sj10);
35848 IkReal x395 = (cj11 * x391);
35849 IkReal x396 = ((1.0) * sj10);
35850 IkReal x397 = ((1.0) * x391);
35851 IkReal x398 = ((1.0) * x390);
35852 IkReal x399 = ((-1.0) * x398);
35853 IkReal x400 = (cj10 * new_r00);
35854 IkReal x401 = (new_r10 * sj10);
35855 IkReal x402 = (cj10 * x398);
35856 evalcond[0] = (((sj11 * x390)) + new_r20);
35857 evalcond[1]
35858 = ((((-1.0) * x391 * x392)) + new_r21);
35859 evalcond[2] = (x394 + x395 + x393);
35860 evalcond[3]
35861 = (((cj10 * new_r10))
35862 + (((-1.0) * x397))
35863 + (((-1.0) * new_r00 * x396)));
35864 evalcond[4]
35865 = (((cj10 * new_r11)) + x399
35866 + (((-1.0) * new_r01 * x396)));
35867 evalcond[5]
35868 = (((cj10 * x395)) + ((sj10 * x390))
35869 + new_r01);
35870 evalcond[6]
35871 = (x401 + x400
35872 + (((-1.0) * cj11 * x398)));
35873 evalcond[7]
35874 = (new_r00 + (((-1.0) * cj11 * x402))
35875 + ((sj10 * x391)));
35876 evalcond[8]
35877 = ((((-1.0) * x402)) + ((sj10 * x395))
35878 + new_r11);
35879 evalcond[9]
35880 = ((((-1.0) * cj10 * x397)) + new_r10
35881 + (((-1.0) * cj11 * x390 * x396)));
35882 evalcond[10]
35883 = (x391 + ((cj11 * x393))
35884 + ((cj11 * x394))
35885 + (((-1.0) * new_r21 * x392)));
35886 evalcond[11]
35887 = (x399 + ((cj11 * x400))
35888 + (((-1.0) * new_r20 * x392))
35889 + ((cj11 * x401)));
35890 if (IKabs(evalcond[0])
35891 > IKFAST_EVALCOND_THRESH
35892 || IKabs(evalcond[1])
35893 > IKFAST_EVALCOND_THRESH
35894 || IKabs(evalcond[2])
35895 > IKFAST_EVALCOND_THRESH
35896 || IKabs(evalcond[3])
35897 > IKFAST_EVALCOND_THRESH
35898 || IKabs(evalcond[4])
35899 > IKFAST_EVALCOND_THRESH
35900 || IKabs(evalcond[5])
35901 > IKFAST_EVALCOND_THRESH
35902 || IKabs(evalcond[6])
35903 > IKFAST_EVALCOND_THRESH
35904 || IKabs(evalcond[7])
35905 > IKFAST_EVALCOND_THRESH
35906 || IKabs(evalcond[8])
35907 > IKFAST_EVALCOND_THRESH
35908 || IKabs(evalcond[9])
35909 > IKFAST_EVALCOND_THRESH
35910 || IKabs(evalcond[10])
35911 > IKFAST_EVALCOND_THRESH
35912 || IKabs(evalcond[11])
35913 > IKFAST_EVALCOND_THRESH)
35914 {
35915 continue;
35916 }
35917 }
35918
35919 {
35920 std::vector<
35921 IkSingleDOFSolutionBase<IkReal> >
35922 vinfos(7);
35923 vinfos[0].jointtype = 1;
35924 vinfos[0].foffset = j4;
35925 vinfos[0].indices[0] = _ij4[0];
35926 vinfos[0].indices[1] = _ij4[1];
35927 vinfos[0].maxsolutions = _nj4;
35928 vinfos[1].jointtype = 1;
35929 vinfos[1].foffset = j6;
35930 vinfos[1].indices[0] = _ij6[0];
35931 vinfos[1].indices[1] = _ij6[1];
35932 vinfos[1].maxsolutions = _nj6;
35933 vinfos[2].jointtype = 1;
35934 vinfos[2].foffset = j8;
35935 vinfos[2].indices[0] = _ij8[0];
35936 vinfos[2].indices[1] = _ij8[1];
35937 vinfos[2].maxsolutions = _nj8;
35938 vinfos[3].jointtype = 1;
35939 vinfos[3].foffset = j9;
35940 vinfos[3].indices[0] = _ij9[0];
35941 vinfos[3].indices[1] = _ij9[1];
35942 vinfos[3].maxsolutions = _nj9;
35943 vinfos[4].jointtype = 1;
35944 vinfos[4].foffset = j10;
35945 vinfos[4].indices[0] = _ij10[0];
35946 vinfos[4].indices[1] = _ij10[1];
35947 vinfos[4].maxsolutions = _nj10;
35948 vinfos[5].jointtype = 1;
35949 vinfos[5].foffset = j11;
35950 vinfos[5].indices[0] = _ij11[0];
35951 vinfos[5].indices[1] = _ij11[1];
35952 vinfos[5].maxsolutions = _nj11;
35953 vinfos[6].jointtype = 1;
35954 vinfos[6].foffset = j12;
35955 vinfos[6].indices[0] = _ij12[0];
35956 vinfos[6].indices[1] = _ij12[1];
35957 vinfos[6].maxsolutions = _nj12;
35958 std::vector<int> vfree(0);
35959 solutions.AddSolution(vinfos, vfree);
35960 }
35961 }
35962 }
35963 }
35964 }
35965 }
35966 else
35967 {
35968 {
35969 IkReal j12array[1], cj12array[1], sj12array[1];
35970 bool j12valid[1] = {false};
35971 _nj12 = 1;
35972 CheckValue<IkReal> x403
35973 = IKPowWithIntegerCheck(IKsign(sj11), -1);
35974 if (!x403.valid)
35975 {
35976 continue;
35977 }
35978 CheckValue<IkReal> x404 = IKatan2WithCheck(
35979 IkReal(new_r21),
35980 ((-1.0) * (((1.0) * new_r20))),
35981 IKFAST_ATAN2_MAGTHRESH);
35982 if (!x404.valid)
35983 {
35984 continue;
35985 }
35986 j12array[0]
35987 = ((-1.5707963267949)
35988 + (((1.5707963267949) * (x403.value)))
35989 + (x404.value));
35990 sj12array[0] = IKsin(j12array[0]);
35991 cj12array[0] = IKcos(j12array[0]);
35992 if (j12array[0] > IKPI)
35993 {
35994 j12array[0] -= IK2PI;
35995 }
35996 else if (j12array[0] < -IKPI)
35997 {
35998 j12array[0] += IK2PI;
35999 }
36000 j12valid[0] = true;
36001 for (int ij12 = 0; ij12 < 1; ++ij12)
36002 {
36003 if (!j12valid[ij12])
36004 {
36005 continue;
36006 }
36007 _ij12[0] = ij12;
36008 _ij12[1] = -1;
36009 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
36010 {
36011 if (j12valid[iij12]
36012 && IKabs(cj12array[ij12] - cj12array[iij12])
36013 < IKFAST_SOLUTION_THRESH
36014 && IKabs(sj12array[ij12] - sj12array[iij12])
36015 < IKFAST_SOLUTION_THRESH)
36016 {
36017 j12valid[iij12] = false;
36018 _ij12[1] = iij12;
36019 break;
36020 }
36021 }
36022 j12 = j12array[ij12];
36023 cj12 = cj12array[ij12];
36024 sj12 = sj12array[ij12];
36025 {
36026 IkReal evalcond[12];
36027 IkReal x405 = IKcos(j12);
36028 IkReal x406 = IKsin(j12);
36029 IkReal x407 = ((1.0) * sj11);
36030 IkReal x408 = (cj10 * new_r01);
36031 IkReal x409 = (new_r11 * sj10);
36032 IkReal x410 = (cj11 * x406);
36033 IkReal x411 = ((1.0) * sj10);
36034 IkReal x412 = ((1.0) * x406);
36035 IkReal x413 = ((1.0) * x405);
36036 IkReal x414 = ((-1.0) * x413);
36037 IkReal x415 = (cj10 * new_r00);
36038 IkReal x416 = (new_r10 * sj10);
36039 IkReal x417 = (cj10 * x413);
36040 evalcond[0] = (((sj11 * x405)) + new_r20);
36041 evalcond[1]
36042 = ((((-1.0) * x406 * x407)) + new_r21);
36043 evalcond[2] = (x410 + x409 + x408);
36044 evalcond[3]
36045 = ((((-1.0) * new_r00 * x411))
36046 + ((cj10 * new_r10))
36047 + (((-1.0) * x412)));
36048 evalcond[4]
36049 = (((cj10 * new_r11)) + x414
36050 + (((-1.0) * new_r01 * x411)));
36051 evalcond[5]
36052 = (((cj10 * x410)) + ((sj10 * x405))
36053 + new_r01);
36054 evalcond[6]
36055 = ((((-1.0) * cj11 * x413)) + x415 + x416);
36056 evalcond[7]
36057 = ((((-1.0) * cj11 * x417))
36058 + ((sj10 * x406)) + new_r00);
36059 evalcond[8]
36060 = (((sj10 * x410)) + (((-1.0) * x417))
36061 + new_r11);
36062 evalcond[9]
36063 = ((((-1.0) * cj10 * x412))
36064 + (((-1.0) * cj11 * x405 * x411))
36065 + new_r10);
36066 evalcond[10]
36067 = (((cj11 * x409))
36068 + (((-1.0) * new_r21 * x407)) + x406
36069 + ((cj11 * x408)));
36070 evalcond[11]
36071 = (x414 + ((cj11 * x416)) + ((cj11 * x415))
36072 + (((-1.0) * new_r20 * x407)));
36073 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
36074 || IKabs(evalcond[1])
36075 > IKFAST_EVALCOND_THRESH
36076 || IKabs(evalcond[2])
36077 > IKFAST_EVALCOND_THRESH
36078 || IKabs(evalcond[3])
36079 > IKFAST_EVALCOND_THRESH
36080 || IKabs(evalcond[4])
36081 > IKFAST_EVALCOND_THRESH
36082 || IKabs(evalcond[5])
36083 > IKFAST_EVALCOND_THRESH
36084 || IKabs(evalcond[6])
36085 > IKFAST_EVALCOND_THRESH
36086 || IKabs(evalcond[7])
36087 > IKFAST_EVALCOND_THRESH
36088 || IKabs(evalcond[8])
36089 > IKFAST_EVALCOND_THRESH
36090 || IKabs(evalcond[9])
36091 > IKFAST_EVALCOND_THRESH
36092 || IKabs(evalcond[10])
36093 > IKFAST_EVALCOND_THRESH
36094 || IKabs(evalcond[11])
36095 > IKFAST_EVALCOND_THRESH)
36096 {
36097 continue;
36098 }
36099 }
36100
36101 {
36102 std::vector<IkSingleDOFSolutionBase<IkReal> >
36103 vinfos(7);
36104 vinfos[0].jointtype = 1;
36105 vinfos[0].foffset = j4;
36106 vinfos[0].indices[0] = _ij4[0];
36107 vinfos[0].indices[1] = _ij4[1];
36108 vinfos[0].maxsolutions = _nj4;
36109 vinfos[1].jointtype = 1;
36110 vinfos[1].foffset = j6;
36111 vinfos[1].indices[0] = _ij6[0];
36112 vinfos[1].indices[1] = _ij6[1];
36113 vinfos[1].maxsolutions = _nj6;
36114 vinfos[2].jointtype = 1;
36115 vinfos[2].foffset = j8;
36116 vinfos[2].indices[0] = _ij8[0];
36117 vinfos[2].indices[1] = _ij8[1];
36118 vinfos[2].maxsolutions = _nj8;
36119 vinfos[3].jointtype = 1;
36120 vinfos[3].foffset = j9;
36121 vinfos[3].indices[0] = _ij9[0];
36122 vinfos[3].indices[1] = _ij9[1];
36123 vinfos[3].maxsolutions = _nj9;
36124 vinfos[4].jointtype = 1;
36125 vinfos[4].foffset = j10;
36126 vinfos[4].indices[0] = _ij10[0];
36127 vinfos[4].indices[1] = _ij10[1];
36128 vinfos[4].maxsolutions = _nj10;
36129 vinfos[5].jointtype = 1;
36130 vinfos[5].foffset = j11;
36131 vinfos[5].indices[0] = _ij11[0];
36132 vinfos[5].indices[1] = _ij11[1];
36133 vinfos[5].maxsolutions = _nj11;
36134 vinfos[6].jointtype = 1;
36135 vinfos[6].foffset = j12;
36136 vinfos[6].indices[0] = _ij12[0];
36137 vinfos[6].indices[1] = _ij12[1];
36138 vinfos[6].maxsolutions = _nj12;
36139 std::vector<int> vfree(0);
36140 solutions.AddSolution(vinfos, vfree);
36141 }
36142 }
36143 }
36144 }
36145 }
36146 }
36147 }
36148 }
36149 }
36150 }
36151 else
36152 {
36153 {
36154 IkReal j10array[1], cj10array[1], sj10array[1];
36155 bool j10valid[1] = {false};
36156 _nj10 = 1;
36157 CheckValue<IkReal> x418 = IKatan2WithCheck(
36158 IkReal(new_r12), new_r02, IKFAST_ATAN2_MAGTHRESH);
36159 if (!x418.valid)
36160 {
36161 continue;
36162 }
36163 CheckValue<IkReal> x419
36164 = IKPowWithIntegerCheck(IKsign(sj11), -1);
36165 if (!x419.valid)
36166 {
36167 continue;
36168 }
36169 j10array[0]
36170 = ((-1.5707963267949) + (x418.value)
36171 + (((1.5707963267949) * (x419.value))));
36172 sj10array[0] = IKsin(j10array[0]);
36173 cj10array[0] = IKcos(j10array[0]);
36174 if (j10array[0] > IKPI)
36175 {
36176 j10array[0] -= IK2PI;
36177 }
36178 else if (j10array[0] < -IKPI)
36179 {
36180 j10array[0] += IK2PI;
36181 }
36182 j10valid[0] = true;
36183 for (int ij10 = 0; ij10 < 1; ++ij10)
36184 {
36185 if (!j10valid[ij10])
36186 {
36187 continue;
36188 }
36189 _ij10[0] = ij10;
36190 _ij10[1] = -1;
36191 for (int iij10 = ij10 + 1; iij10 < 1; ++iij10)
36192 {
36193 if (j10valid[iij10]
36194 && IKabs(cj10array[ij10] - cj10array[iij10])
36195 < IKFAST_SOLUTION_THRESH
36196 && IKabs(sj10array[ij10] - sj10array[iij10])
36197 < IKFAST_SOLUTION_THRESH)
36198 {
36199 j10valid[iij10] = false;
36200 _ij10[1] = iij10;
36201 break;
36202 }
36203 }
36204 j10 = j10array[ij10];
36205 cj10 = cj10array[ij10];
36206 sj10 = sj10array[ij10];
36207 {
36208 IkReal evalcond[8];
36209 IkReal x420 = IKcos(j10);
36210 IkReal x421 = ((1.0) * sj11);
36211 IkReal x422 = (x420 * x421);
36212 IkReal x423 = IKsin(j10);
36213 IkReal x424 = (x421 * x423);
36214 IkReal x425 = (new_r02 * x420);
36215 IkReal x426 = (new_r12 * x423);
36216 IkReal x427 = ((1.0) * cj11);
36217 evalcond[0] = ((((-1.0) * x422)) + new_r02);
36218 evalcond[1] = ((((-1.0) * x424)) + new_r12);
36219 evalcond[2]
36220 = ((((-1.0) * new_r02 * x423)) + ((new_r12 * x420)));
36221 evalcond[3] = ((((-1.0) * x421)) + x426 + x425);
36222 evalcond[4]
36223 = (((cj11 * x426)) + ((cj11 * x425))
36224 + (((-1.0) * new_r22 * x421)));
36225 evalcond[5]
36226 = ((((-1.0) * new_r10 * x424))
36227 + (((-1.0) * new_r20 * x427))
36228 + (((-1.0) * new_r00 * x422)));
36229 evalcond[6]
36230 = ((((-1.0) * new_r21 * x427))
36231 + (((-1.0) * new_r01 * x422))
36232 + (((-1.0) * new_r11 * x424)));
36233 evalcond[7]
36234 = ((1.0) + (((-1.0) * new_r22 * x427))
36235 + (((-1.0) * x421 * x425))
36236 + (((-1.0) * x421 * x426)));
36237 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
36238 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
36239 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
36240 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
36241 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
36242 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
36243 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
36244 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
36245 {
36246 continue;
36247 }
36248 }
36249
36250 {
36251 IkReal j12eval[2];
36252 IkReal x428 = ((1.0) * sj9);
36253 IkReal x429 = (cj9 * sj6);
36254 IkReal x430 = x160;
36255 IkReal x431 = (cj4 * sj8);
36256 IkReal x432 = (sj6 * x428);
36257 IkReal x433 = (cj6 * cj9);
36258 IkReal x434 = ((((-1.0) * x432)) + ((cj8 * x433)));
36259 IkReal x435 = (((cj9 * x431)) + ((sj4 * x434)));
36260 IkReal x436 = ((1.0) * sj4 * sj8);
36261 IkReal x437 = ((((-1.0) * cj9 * x436)) + ((cj4 * x434)));
36262 IkReal x438 = (sj6 * sj8);
36263 IkReal x439 = x169;
36264 IkReal x440 = x170;
36265 IkReal x441 = ((((-1.0) * cj8 * x432)) + x433);
36266 IkReal x442 = x172;
36267 IkReal x443 = (((sj9 * x431)) + ((sj4 * x442)));
36268 IkReal x444
36269 = (((cj4 * x442)) + (((-1.0) * sj4 * sj8 * x428)));
36270 new_r00
36271 = (((r20 * x430)) + ((r10 * x435)) + ((r00 * x437)));
36272 new_r01
36273 = (((r11 * x435)) + ((r01 * x437)) + ((r21 * x430)));
36274 new_r02
36275 = (((r12 * x435)) + ((r02 * x437)) + ((r22 * x430)));
36276 new_r10
36277 = (((r20 * x438)) + ((r00 * x440)) + ((r10 * x439)));
36278 new_r11
36279 = (((r11 * x439)) + ((r21 * x438)) + ((r01 * x440)));
36280 new_r12
36281 = (((r22 * x438)) + ((r12 * x439)) + ((r02 * x440)));
36282 new_r20
36283 = (((r10 * x443)) + ((r20 * x441)) + ((r00 * x444)));
36284 new_r21
36285 = (((r01 * x444)) + ((r21 * x441)) + ((r11 * x443)));
36286 new_r22
36287 = (((r22 * x441)) + ((r12 * x443)) + ((r02 * x444)));
36288 j12eval[0] = sj11;
36289 j12eval[1] = IKsign(sj11);
36290 if (IKabs(j12eval[0]) < 0.0000010000000000
36291 || IKabs(j12eval[1]) < 0.0000010000000000)
36292 {
36293 {
36294 IkReal j12eval[2];
36295 IkReal x445 = ((1.0) * sj9);
36296 IkReal x446 = (cj9 * sj6);
36297 IkReal x447 = x160;
36298 IkReal x448 = (cj4 * sj8);
36299 IkReal x449 = (sj6 * x445);
36300 IkReal x450 = (cj6 * cj9);
36301 IkReal x451 = (((cj8 * x450)) + (((-1.0) * x449)));
36302 IkReal x452 = (((cj9 * x448)) + ((sj4 * x451)));
36303 IkReal x453 = ((1.0) * sj4 * sj8);
36304 IkReal x454
36305 = (((cj4 * x451)) + (((-1.0) * cj9 * x453)));
36306 IkReal x455 = (sj6 * sj8);
36307 IkReal x456 = x169;
36308 IkReal x457 = x170;
36309 IkReal x458 = (x450 + (((-1.0) * cj8 * x449)));
36310 IkReal x459 = x172;
36311 IkReal x460 = (((sj4 * x459)) + ((sj9 * x448)));
36312 IkReal x461
36313 = (((cj4 * x459)) + (((-1.0) * sj4 * sj8 * x445)));
36314 new_r00
36315 = (((r00 * x454)) + ((r20 * x447))
36316 + ((r10 * x452)));
36317 new_r01
36318 = (((r01 * x454)) + ((r21 * x447))
36319 + ((r11 * x452)));
36320 new_r02
36321 = (((r02 * x454)) + ((r12 * x452))
36322 + ((r22 * x447)));
36323 new_r10
36324 = (((r20 * x455)) + ((r00 * x457))
36325 + ((r10 * x456)));
36326 new_r11
36327 = (((r21 * x455)) + ((r11 * x456))
36328 + ((r01 * x457)));
36329 new_r12
36330 = (((r12 * x456)) + ((r22 * x455))
36331 + ((r02 * x457)));
36332 new_r20
36333 = (((r10 * x460)) + ((r20 * x458))
36334 + ((r00 * x461)));
36335 new_r21
36336 = (((r11 * x460)) + ((r21 * x458))
36337 + ((r01 * x461)));
36338 new_r22
36339 = (((r12 * x460)) + ((r22 * x458))
36340 + ((r02 * x461)));
36341 j12eval[0] = sj10;
36342 j12eval[1] = sj11;
36343 if (IKabs(j12eval[0]) < 0.0000010000000000
36344 || IKabs(j12eval[1]) < 0.0000010000000000)
36345 {
36346 {
36347 IkReal j12eval[3];
36348 IkReal x462 = ((1.0) * sj9);
36349 IkReal x463 = (cj9 * sj6);
36350 IkReal x464 = x160;
36351 IkReal x465 = (cj4 * sj8);
36352 IkReal x466 = (sj6 * x462);
36353 IkReal x467 = (cj6 * cj9);
36354 IkReal x468 = ((((-1.0) * x466)) + ((cj8 * x467)));
36355 IkReal x469 = (((cj9 * x465)) + ((sj4 * x468)));
36356 IkReal x470 = ((1.0) * sj4 * sj8);
36357 IkReal x471
36358 = ((((-1.0) * cj9 * x470)) + ((cj4 * x468)));
36359 IkReal x472 = (sj6 * sj8);
36360 IkReal x473 = x169;
36361 IkReal x474 = x170;
36362 IkReal x475 = ((((-1.0) * cj8 * x466)) + x467);
36363 IkReal x476 = x172;
36364 IkReal x477 = (((sj9 * x465)) + ((sj4 * x476)));
36365 IkReal x478
36366 = ((((-1.0) * sj4 * sj8 * x462))
36367 + ((cj4 * x476)));
36368 new_r00
36369 = (((r10 * x469)) + ((r00 * x471))
36370 + ((r20 * x464)));
36371 new_r01
36372 = (((r11 * x469)) + ((r01 * x471))
36373 + ((r21 * x464)));
36374 new_r02
36375 = (((r22 * x464)) + ((r02 * x471))
36376 + ((r12 * x469)));
36377 new_r10
36378 = (((r00 * x474)) + ((r10 * x473))
36379 + ((r20 * x472)));
36380 new_r11
36381 = (((r01 * x474)) + ((r11 * x473))
36382 + ((r21 * x472)));
36383 new_r12
36384 = (((r02 * x474)) + ((r12 * x473))
36385 + ((r22 * x472)));
36386 new_r20
36387 = (((r20 * x475)) + ((r10 * x477))
36388 + ((r00 * x478)));
36389 new_r21
36390 = (((r01 * x478)) + ((r21 * x475))
36391 + ((r11 * x477)));
36392 new_r22
36393 = (((r22 * x475)) + ((r02 * x478))
36394 + ((r12 * x477)));
36395 j12eval[0] = cj10;
36396 j12eval[1] = cj11;
36397 j12eval[2] = sj11;
36398 if (IKabs(j12eval[0]) < 0.0000010000000000
36399 || IKabs(j12eval[1]) < 0.0000010000000000
36400 || IKabs(j12eval[2]) < 0.0000010000000000)
36401 {
36402 {
36403 IkReal evalcond[12];
36404 bool bgotonextstatement = true;
36405 do
36406 {
36407 IkReal x479 = ((1.0) * cj11);
36408 IkReal x480 = ((((-1.0) * x479)) + new_r22);
36409 IkReal x481 = ((1.0) * sj11);
36410 IkReal x482 = ((((-1.0) * x481)) + new_r12);
36411 evalcond[0]
36412 = ((-3.14159265358979)
36413 + (IKfmod(
36414 ((3.14159265358979)
36415 + (IKabs(
36416 ((-1.5707963267949)
36417 + j10)))),
36418 6.28318530717959)));
36419 evalcond[1] = x480;
36420 evalcond[2] = x480;
36421 evalcond[3] = new_r02;
36422 evalcond[4] = x482;
36423 evalcond[5] = x482;
36424 evalcond[6]
36425 = ((((-1.0) * new_r22 * x481))
36426 + ((cj11 * new_r12)));
36427 evalcond[7]
36428 = ((((-1.0) * new_r10 * x481))
36429 + (((-1.0) * new_r20 * x479)));
36430 evalcond[8]
36431 = ((((-1.0) * new_r11 * x481))
36432 + (((-1.0) * new_r21 * x479)));
36433 evalcond[9]
36434 = ((1.0) + (((-1.0) * new_r12 * x481))
36435 + (((-1.0) * new_r22 * x479)));
36436 if (IKabs(evalcond[0]) < 0.0000010000000000
36437 && IKabs(evalcond[1]) < 0.0000010000000000
36438 && IKabs(evalcond[2]) < 0.0000010000000000
36439 && IKabs(evalcond[3]) < 0.0000010000000000
36440 && IKabs(evalcond[4]) < 0.0000010000000000
36441 && IKabs(evalcond[5]) < 0.0000010000000000
36442 && IKabs(evalcond[6]) < 0.0000010000000000
36443 && IKabs(evalcond[7]) < 0.0000010000000000
36444 && IKabs(evalcond[8]) < 0.0000010000000000
36445 && IKabs(evalcond[9])
36446 < 0.0000010000000000)
36447 {
36448 bgotonextstatement = false;
36449 {
36450 IkReal j12array[1], cj12array[1],
36451 sj12array[1];
36452 bool j12valid[1] = {false};
36453 _nj12 = 1;
36454 CheckValue<IkReal> x483
36455 = IKPowWithIntegerCheck(
36456 IKsign(new_r12), -1);
36457 if (!x483.valid)
36458 {
36459 continue;
36460 }
36461 CheckValue<IkReal> x484
36462 = IKatan2WithCheck(
36463 IkReal(new_r21),
36464 ((-1.0) * (((1.0) * new_r20))),
36465 IKFAST_ATAN2_MAGTHRESH);
36466 if (!x484.valid)
36467 {
36468 continue;
36469 }
36470 j12array[0]
36471 = ((-1.5707963267949)
36472 + (((1.5707963267949)
36473 * (x483.value)))
36474 + (x484.value));
36475 sj12array[0] = IKsin(j12array[0]);
36476 cj12array[0] = IKcos(j12array[0]);
36477 if (j12array[0] > IKPI)
36478 {
36479 j12array[0] -= IK2PI;
36480 }
36481 else if (j12array[0] < -IKPI)
36482 {
36483 j12array[0] += IK2PI;
36484 }
36485 j12valid[0] = true;
36486 for (int ij12 = 0; ij12 < 1; ++ij12)
36487 {
36488 if (!j12valid[ij12])
36489 {
36490 continue;
36491 }
36492 _ij12[0] = ij12;
36493 _ij12[1] = -1;
36494 for (int iij12 = ij12 + 1; iij12 < 1;
36495 ++iij12)
36496 {
36497 if (j12valid[iij12]
36498 && IKabs(
36499 cj12array[ij12]
36500 - cj12array[iij12])
36501 < IKFAST_SOLUTION_THRESH
36502 && IKabs(
36503 sj12array[ij12]
36504 - sj12array[iij12])
36505 < IKFAST_SOLUTION_THRESH)
36506 {
36507 j12valid[iij12] = false;
36508 _ij12[1] = iij12;
36509 break;
36510 }
36511 }
36512 j12 = j12array[ij12];
36513 cj12 = cj12array[ij12];
36514 sj12 = sj12array[ij12];
36515 {
36516 IkReal evalcond[8];
36517 IkReal x485 = IKcos(j12);
36518 IkReal x486 = IKsin(j12);
36519 IkReal x487 = ((1.0) * new_r12);
36520 IkReal x488 = ((1.0) * x485);
36521 IkReal x489 = ((-1.0) * x488);
36522 evalcond[0]
36523 = (((new_r12 * x485)) + new_r20);
36524 evalcond[1]
36525 = (((new_r22 * x486)) + new_r11);
36526 evalcond[2]
36527 = ((((-1.0) * x486 * x487))
36528 + new_r21);
36529 evalcond[3]
36530 = ((((-1.0) * new_r22 * x488))
36531 + new_r10);
36532 evalcond[4]
36533 = ((((-1.0) * (1.0) * new_r00))
36534 + (((-1.0) * x486)));
36535 evalcond[5]
36536 = ((((-1.0) * (1.0) * new_r01))
36537 + x489);
36538 evalcond[6]
36539 = (((new_r11 * new_r22))
36540 + (((-1.0) * new_r21 * x487))
36541 + x486);
36542 evalcond[7]
36543 = ((((-1.0) * new_r20 * x487))
36544 + ((new_r10 * new_r22))
36545 + x489);
36546 if (IKabs(evalcond[0])
36547 > IKFAST_EVALCOND_THRESH
36548 || IKabs(evalcond[1])
36549 > IKFAST_EVALCOND_THRESH
36550 || IKabs(evalcond[2])
36551 > IKFAST_EVALCOND_THRESH
36552 || IKabs(evalcond[3])
36553 > IKFAST_EVALCOND_THRESH
36554 || IKabs(evalcond[4])
36555 > IKFAST_EVALCOND_THRESH
36556 || IKabs(evalcond[5])
36557 > IKFAST_EVALCOND_THRESH
36558 || IKabs(evalcond[6])
36559 > IKFAST_EVALCOND_THRESH
36560 || IKabs(evalcond[7])
36561 > IKFAST_EVALCOND_THRESH)
36562 {
36563 continue;
36564 }
36565 }
36566
36567 {
36568 std::vector<
36569 IkSingleDOFSolutionBase<IkReal> >
36570 vinfos(7);
36571 vinfos[0].jointtype = 1;
36572 vinfos[0].foffset = j4;
36573 vinfos[0].indices[0] = _ij4[0];
36574 vinfos[0].indices[1] = _ij4[1];
36575 vinfos[0].maxsolutions = _nj4;
36576 vinfos[1].jointtype = 1;
36577 vinfos[1].foffset = j6;
36578 vinfos[1].indices[0] = _ij6[0];
36579 vinfos[1].indices[1] = _ij6[1];
36580 vinfos[1].maxsolutions = _nj6;
36581 vinfos[2].jointtype = 1;
36582 vinfos[2].foffset = j8;
36583 vinfos[2].indices[0] = _ij8[0];
36584 vinfos[2].indices[1] = _ij8[1];
36585 vinfos[2].maxsolutions = _nj8;
36586 vinfos[3].jointtype = 1;
36587 vinfos[3].foffset = j9;
36588 vinfos[3].indices[0] = _ij9[0];
36589 vinfos[3].indices[1] = _ij9[1];
36590 vinfos[3].maxsolutions = _nj9;
36591 vinfos[4].jointtype = 1;
36592 vinfos[4].foffset = j10;
36593 vinfos[4].indices[0] = _ij10[0];
36594 vinfos[4].indices[1] = _ij10[1];
36595 vinfos[4].maxsolutions = _nj10;
36596 vinfos[5].jointtype = 1;
36597 vinfos[5].foffset = j11;
36598 vinfos[5].indices[0] = _ij11[0];
36599 vinfos[5].indices[1] = _ij11[1];
36600 vinfos[5].maxsolutions = _nj11;
36601 vinfos[6].jointtype = 1;
36602 vinfos[6].foffset = j12;
36603 vinfos[6].indices[0] = _ij12[0];
36604 vinfos[6].indices[1] = _ij12[1];
36605 vinfos[6].maxsolutions = _nj12;
36606 std::vector<int> vfree(0);
36607 solutions.AddSolution(vinfos, vfree);
36608 }
36609 }
36610 }
36611 }
36612 } while (0);
36613 if (bgotonextstatement)
36614 {
36615 bool bgotonextstatement = true;
36616 do
36617 {
36618 IkReal x490 = ((1.0) * cj11);
36619 IkReal x491 = ((((-1.0) * x490)) + new_r22);
36620 IkReal x492 = ((1.0) * sj11);
36621 evalcond[0]
36622 = ((-3.14159265358979)
36623 + (IKfmod(
36624 ((3.14159265358979)
36625 + (IKabs(
36626 ((1.5707963267949)
36627 + j10)))),
36628 6.28318530717959)));
36629 evalcond[1] = x491;
36630 evalcond[2] = x491;
36631 evalcond[3] = new_r02;
36632 evalcond[4] = (sj11 + new_r12);
36633 evalcond[5]
36634 = ((((-1.0) * (1.0) * new_r12))
36635 + (((-1.0) * x492)));
36636 evalcond[6]
36637 = ((((-1.0) * new_r22 * x492))
36638 + (((-1.0) * new_r12 * x490)));
36639 evalcond[7]
36640 = (((new_r10 * sj11))
36641 + (((-1.0) * new_r20 * x490)));
36642 evalcond[8]
36643 = ((((-1.0) * new_r21 * x490))
36644 + ((new_r11 * sj11)));
36645 evalcond[9]
36646 = ((1.0) + (((-1.0) * new_r22 * x490))
36647 + ((new_r12 * sj11)));
36648 if (IKabs(evalcond[0]) < 0.0000010000000000
36649 && IKabs(evalcond[1])
36650 < 0.0000010000000000
36651 && IKabs(evalcond[2])
36652 < 0.0000010000000000
36653 && IKabs(evalcond[3])
36654 < 0.0000010000000000
36655 && IKabs(evalcond[4])
36656 < 0.0000010000000000
36657 && IKabs(evalcond[5])
36658 < 0.0000010000000000
36659 && IKabs(evalcond[6])
36660 < 0.0000010000000000
36661 && IKabs(evalcond[7])
36662 < 0.0000010000000000
36663 && IKabs(evalcond[8])
36664 < 0.0000010000000000
36665 && IKabs(evalcond[9])
36666 < 0.0000010000000000)
36667 {
36668 bgotonextstatement = false;
36669 {
36670 IkReal j12array[1], cj12array[1],
36671 sj12array[1];
36672 bool j12valid[1] = {false};
36673 _nj12 = 1;
36674 if (IKabs(new_r00)
36675 < IKFAST_ATAN2_MAGTHRESH
36676 && IKabs(new_r01)
36677 < IKFAST_ATAN2_MAGTHRESH
36678 && IKabs(
36679 IKsqr(new_r00)
36680 + IKsqr(new_r01) - 1)
36681 <= IKFAST_SINCOS_THRESH)
36682 continue;
36683 j12array[0] = IKatan2(new_r00, new_r01);
36684 sj12array[0] = IKsin(j12array[0]);
36685 cj12array[0] = IKcos(j12array[0]);
36686 if (j12array[0] > IKPI)
36687 {
36688 j12array[0] -= IK2PI;
36689 }
36690 else if (j12array[0] < -IKPI)
36691 {
36692 j12array[0] += IK2PI;
36693 }
36694 j12valid[0] = true;
36695 for (int ij12 = 0; ij12 < 1; ++ij12)
36696 {
36697 if (!j12valid[ij12])
36698 {
36699 continue;
36700 }
36701 _ij12[0] = ij12;
36702 _ij12[1] = -1;
36703 for (int iij12 = ij12 + 1; iij12 < 1;
36704 ++iij12)
36705 {
36706 if (j12valid[iij12]
36707 && IKabs(
36708 cj12array[ij12]
36709 - cj12array[iij12])
36710 < IKFAST_SOLUTION_THRESH
36711 && IKabs(
36712 sj12array[ij12]
36713 - sj12array[iij12])
36714 < IKFAST_SOLUTION_THRESH)
36715 {
36716 j12valid[iij12] = false;
36717 _ij12[1] = iij12;
36718 break;
36719 }
36720 }
36721 j12 = j12array[ij12];
36722 cj12 = cj12array[ij12];
36723 sj12 = sj12array[ij12];
36724 {
36725 IkReal evalcond[8];
36726 IkReal x493 = IKsin(j12);
36727 IkReal x494
36728 = ((1.0) * (IKcos(j12)));
36729 IkReal x495 = ((-1.0) * x494);
36730 IkReal x496 = ((1.0) * new_r11);
36731 IkReal x497 = ((1.0) * new_r10);
36732 evalcond[0]
36733 = (((new_r12 * x493))
36734 + new_r21);
36735 evalcond[1]
36736 = (new_r00 + (((-1.0) * x493)));
36737 evalcond[2] = (new_r01 + x495);
36738 evalcond[3]
36739 = ((((-1.0) * new_r12 * x494))
36740 + new_r20);
36741 evalcond[4]
36742 = (((new_r22 * x493))
36743 + (((-1.0) * x496)));
36744 evalcond[5]
36745 = ((((-1.0) * new_r22 * x494))
36746 + (((-1.0) * x497)));
36747 evalcond[6]
36748 = (((new_r12 * new_r21))
36749 + (((-1.0) * new_r22 * x496))
36750 + x493);
36751 evalcond[7]
36752 = (((new_r12 * new_r20))
36753 + (((-1.0) * new_r22 * x497))
36754 + x495);
36755 if (IKabs(evalcond[0])
36756 > IKFAST_EVALCOND_THRESH
36757 || IKabs(evalcond[1])
36758 > IKFAST_EVALCOND_THRESH
36759 || IKabs(evalcond[2])
36760 > IKFAST_EVALCOND_THRESH
36761 || IKabs(evalcond[3])
36762 > IKFAST_EVALCOND_THRESH
36763 || IKabs(evalcond[4])
36764 > IKFAST_EVALCOND_THRESH
36765 || IKabs(evalcond[5])
36766 > IKFAST_EVALCOND_THRESH
36767 || IKabs(evalcond[6])
36768 > IKFAST_EVALCOND_THRESH
36769 || IKabs(evalcond[7])
36770 > IKFAST_EVALCOND_THRESH)
36771 {
36772 continue;
36773 }
36774 }
36775
36776 {
36777 std::vector<IkSingleDOFSolutionBase<
36778 IkReal> >
36779 vinfos(7);
36780 vinfos[0].jointtype = 1;
36781 vinfos[0].foffset = j4;
36782 vinfos[0].indices[0] = _ij4[0];
36783 vinfos[0].indices[1] = _ij4[1];
36784 vinfos[0].maxsolutions = _nj4;
36785 vinfos[1].jointtype = 1;
36786 vinfos[1].foffset = j6;
36787 vinfos[1].indices[0] = _ij6[0];
36788 vinfos[1].indices[1] = _ij6[1];
36789 vinfos[1].maxsolutions = _nj6;
36790 vinfos[2].jointtype = 1;
36791 vinfos[2].foffset = j8;
36792 vinfos[2].indices[0] = _ij8[0];
36793 vinfos[2].indices[1] = _ij8[1];
36794 vinfos[2].maxsolutions = _nj8;
36795 vinfos[3].jointtype = 1;
36796 vinfos[3].foffset = j9;
36797 vinfos[3].indices[0] = _ij9[0];
36798 vinfos[3].indices[1] = _ij9[1];
36799 vinfos[3].maxsolutions = _nj9;
36800 vinfos[4].jointtype = 1;
36801 vinfos[4].foffset = j10;
36802 vinfos[4].indices[0] = _ij10[0];
36803 vinfos[4].indices[1] = _ij10[1];
36804 vinfos[4].maxsolutions = _nj10;
36805 vinfos[5].jointtype = 1;
36806 vinfos[5].foffset = j11;
36807 vinfos[5].indices[0] = _ij11[0];
36808 vinfos[5].indices[1] = _ij11[1];
36809 vinfos[5].maxsolutions = _nj11;
36810 vinfos[6].jointtype = 1;
36811 vinfos[6].foffset = j12;
36812 vinfos[6].indices[0] = _ij12[0];
36813 vinfos[6].indices[1] = _ij12[1];
36814 vinfos[6].maxsolutions = _nj12;
36815 std::vector<int> vfree(0);
36816 solutions.AddSolution(
36817 vinfos, vfree);
36818 }
36819 }
36820 }
36821 }
36822 } while (0);
36823 if (bgotonextstatement)
36824 {
36825 bool bgotonextstatement = true;
36826 do
36827 {
36828 IkReal x498 = ((1.0) * cj10);
36829 IkReal x499 = ((1.0) * sj10);
36830 IkReal x500
36831 = ((((-1.0) * new_r02 * x499))
36832 + ((cj10 * new_r12)));
36833 evalcond[0]
36834 = ((-3.14159265358979)
36835 + (IKfmod(
36836 ((3.14159265358979)
36837 + (IKabs(
36838 ((-1.5707963267949)
36839 + j11)))),
36840 6.28318530717959)));
36841 evalcond[1] = new_r22;
36842 evalcond[2]
36843 = ((((-1.0) * x498)) + new_r02);
36844 evalcond[3]
36845 = ((((-1.0) * x499)) + new_r12);
36846 evalcond[4] = x500;
36847 evalcond[5] = x500;
36848 evalcond[6]
36849 = ((-1.0) + ((cj10 * new_r02))
36850 + ((new_r12 * sj10)));
36851 evalcond[7]
36852 = (((cj10 * new_r01))
36853 + ((new_r11 * sj10)));
36854 evalcond[8]
36855 = (((cj10 * new_r00))
36856 + ((new_r10 * sj10)));
36857 evalcond[9]
36858 = ((((-1.0) * new_r10 * x499))
36859 + (((-1.0) * new_r00 * x498)));
36860 evalcond[10]
36861 = ((((-1.0) * new_r11 * x499))
36862 + (((-1.0) * new_r01 * x498)));
36863 evalcond[11]
36864 = ((1.0) + (((-1.0) * new_r02 * x498))
36865 + (((-1.0) * new_r12 * x499)));
36866 if (IKabs(evalcond[0])
36867 < 0.0000010000000000
36868 && IKabs(evalcond[1])
36869 < 0.0000010000000000
36870 && IKabs(evalcond[2])
36871 < 0.0000010000000000
36872 && IKabs(evalcond[3])
36873 < 0.0000010000000000
36874 && IKabs(evalcond[4])
36875 < 0.0000010000000000
36876 && IKabs(evalcond[5])
36877 < 0.0000010000000000
36878 && IKabs(evalcond[6])
36879 < 0.0000010000000000
36880 && IKabs(evalcond[7])
36881 < 0.0000010000000000
36882 && IKabs(evalcond[8])
36883 < 0.0000010000000000
36884 && IKabs(evalcond[9])
36885 < 0.0000010000000000
36886 && IKabs(evalcond[10])
36887 < 0.0000010000000000
36888 && IKabs(evalcond[11])
36889 < 0.0000010000000000)
36890 {
36891 bgotonextstatement = false;
36892 {
36893 IkReal j12array[1], cj12array[1],
36894 sj12array[1];
36895 bool j12valid[1] = {false};
36896 _nj12 = 1;
36897 if (IKabs(new_r21)
36898 < IKFAST_ATAN2_MAGTHRESH
36899 && IKabs(
36900 ((-1.0)
36901 * (((1.0) * new_r20))))
36902 < IKFAST_ATAN2_MAGTHRESH
36903 && IKabs(
36904 IKsqr(new_r21)
36905 + IKsqr(
36906 ((-1.0)
36907 * (((1.0)
36908 * new_r20))))
36909 - 1)
36910 <= IKFAST_SINCOS_THRESH)
36911 continue;
36912 j12array[0] = IKatan2(
36913 new_r21,
36914 ((-1.0) * (((1.0) * new_r20))));
36915 sj12array[0] = IKsin(j12array[0]);
36916 cj12array[0] = IKcos(j12array[0]);
36917 if (j12array[0] > IKPI)
36918 {
36919 j12array[0] -= IK2PI;
36920 }
36921 else if (j12array[0] < -IKPI)
36922 {
36923 j12array[0] += IK2PI;
36924 }
36925 j12valid[0] = true;
36926 for (int ij12 = 0; ij12 < 1; ++ij12)
36927 {
36928 if (!j12valid[ij12])
36929 {
36930 continue;
36931 }
36932 _ij12[0] = ij12;
36933 _ij12[1] = -1;
36934 for (int iij12 = ij12 + 1;
36935 iij12 < 1;
36936 ++iij12)
36937 {
36938 if (j12valid[iij12]
36939 && IKabs(
36940 cj12array[ij12]
36941 - cj12array[iij12])
36942 < IKFAST_SOLUTION_THRESH
36943 && IKabs(
36944 sj12array[ij12]
36945 - sj12array[iij12])
36946 < IKFAST_SOLUTION_THRESH)
36947 {
36948 j12valid[iij12] = false;
36949 _ij12[1] = iij12;
36950 break;
36951 }
36952 }
36953 j12 = j12array[ij12];
36954 cj12 = cj12array[ij12];
36955 sj12 = sj12array[ij12];
36956 {
36957 IkReal evalcond[8];
36958 IkReal x501 = IKcos(j12);
36959 IkReal x502 = IKsin(j12);
36960 IkReal x503 = ((1.0) * x502);
36961 IkReal x504 = ((-1.0) * x503);
36962 IkReal x505 = ((1.0) * x501);
36963 IkReal x506 = ((1.0) * new_r12);
36964 evalcond[0] = (x501 + new_r20);
36965 evalcond[1] = (x504 + new_r21);
36966 evalcond[2]
36967 = (((new_r12 * x501))
36968 + new_r01);
36969 evalcond[3]
36970 = (((new_r12 * x502))
36971 + new_r00);
36972 evalcond[4]
36973 = ((((-1.0) * new_r02 * x505))
36974 + new_r11);
36975 evalcond[5]
36976 = ((((-1.0) * new_r02 * x503))
36977 + new_r10);
36978 evalcond[6]
36979 = ((((-1.0) * new_r00 * x506))
36980 + x504
36981 + ((new_r02 * new_r10)));
36982 evalcond[7]
36983 = ((((-1.0) * new_r01 * x506))
36984 + ((new_r02 * new_r11))
36985 + (((-1.0) * x505)));
36986 if (IKabs(evalcond[0])
36987 > IKFAST_EVALCOND_THRESH
36988 || IKabs(evalcond[1])
36989 > IKFAST_EVALCOND_THRESH
36990 || IKabs(evalcond[2])
36991 > IKFAST_EVALCOND_THRESH
36992 || IKabs(evalcond[3])
36993 > IKFAST_EVALCOND_THRESH
36994 || IKabs(evalcond[4])
36995 > IKFAST_EVALCOND_THRESH
36996 || IKabs(evalcond[5])
36997 > IKFAST_EVALCOND_THRESH
36998 || IKabs(evalcond[6])
36999 > IKFAST_EVALCOND_THRESH
37000 || IKabs(evalcond[7])
37001 > IKFAST_EVALCOND_THRESH)
37002 {
37003 continue;
37004 }
37005 }
37006
37007 {
37008 std::vector<
37009 IkSingleDOFSolutionBase<
37010 IkReal> >
37011 vinfos(7);
37012 vinfos[0].jointtype = 1;
37013 vinfos[0].foffset = j4;
37014 vinfos[0].indices[0] = _ij4[0];
37015 vinfos[0].indices[1] = _ij4[1];
37016 vinfos[0].maxsolutions = _nj4;
37017 vinfos[1].jointtype = 1;
37018 vinfos[1].foffset = j6;
37019 vinfos[1].indices[0] = _ij6[0];
37020 vinfos[1].indices[1] = _ij6[1];
37021 vinfos[1].maxsolutions = _nj6;
37022 vinfos[2].jointtype = 1;
37023 vinfos[2].foffset = j8;
37024 vinfos[2].indices[0] = _ij8[0];
37025 vinfos[2].indices[1] = _ij8[1];
37026 vinfos[2].maxsolutions = _nj8;
37027 vinfos[3].jointtype = 1;
37028 vinfos[3].foffset = j9;
37029 vinfos[3].indices[0] = _ij9[0];
37030 vinfos[3].indices[1] = _ij9[1];
37031 vinfos[3].maxsolutions = _nj9;
37032 vinfos[4].jointtype = 1;
37033 vinfos[4].foffset = j10;
37034 vinfos[4].indices[0] = _ij10[0];
37035 vinfos[4].indices[1] = _ij10[1];
37036 vinfos[4].maxsolutions = _nj10;
37037 vinfos[5].jointtype = 1;
37038 vinfos[5].foffset = j11;
37039 vinfos[5].indices[0] = _ij11[0];
37040 vinfos[5].indices[1] = _ij11[1];
37041 vinfos[5].maxsolutions = _nj11;
37042 vinfos[6].jointtype = 1;
37043 vinfos[6].foffset = j12;
37044 vinfos[6].indices[0] = _ij12[0];
37045 vinfos[6].indices[1] = _ij12[1];
37046 vinfos[6].maxsolutions = _nj12;
37047 std::vector<int> vfree(0);
37048 solutions.AddSolution(
37049 vinfos, vfree);
37050 }
37051 }
37052 }
37053 }
37054 } while (0);
37055 if (bgotonextstatement)
37056 {
37057 bool bgotonextstatement = true;
37058 do
37059 {
37060 IkReal x507
37061 = ((((-1.0) * (1.0) * new_r02
37062 * sj10))
37063 + ((cj10 * new_r12)));
37064 IkReal x508
37065 = ((1.0) + ((cj10 * new_r02))
37066 + ((new_r12 * sj10)));
37067 IkReal x509
37068 = (((cj10 * new_r01))
37069 + ((new_r11 * sj10)));
37070 IkReal x510
37071 = (((cj10 * new_r00))
37072 + ((new_r10 * sj10)));
37073 evalcond[0]
37074 = ((-3.14159265358979)
37075 + (IKfmod(
37076 ((3.14159265358979)
37077 + (IKabs(
37078 ((1.5707963267949)
37079 + j11)))),
37080 6.28318530717959)));
37081 evalcond[1] = new_r22;
37082 evalcond[2] = (cj10 + new_r02);
37083 evalcond[3] = (sj10 + new_r12);
37084 evalcond[4] = x507;
37085 evalcond[5] = x507;
37086 evalcond[6] = x508;
37087 evalcond[7] = x509;
37088 evalcond[8] = x510;
37089 evalcond[9] = x510;
37090 evalcond[10] = x509;
37091 evalcond[11] = x508;
37092 if (IKabs(evalcond[0])
37093 < 0.0000010000000000
37094 && IKabs(evalcond[1])
37095 < 0.0000010000000000
37096 && IKabs(evalcond[2])
37097 < 0.0000010000000000
37098 && IKabs(evalcond[3])
37099 < 0.0000010000000000
37100 && IKabs(evalcond[4])
37101 < 0.0000010000000000
37102 && IKabs(evalcond[5])
37103 < 0.0000010000000000
37104 && IKabs(evalcond[6])
37105 < 0.0000010000000000
37106 && IKabs(evalcond[7])
37107 < 0.0000010000000000
37108 && IKabs(evalcond[8])
37109 < 0.0000010000000000
37110 && IKabs(evalcond[9])
37111 < 0.0000010000000000
37112 && IKabs(evalcond[10])
37113 < 0.0000010000000000
37114 && IKabs(evalcond[11])
37115 < 0.0000010000000000)
37116 {
37117 bgotonextstatement = false;
37118 {
37119 IkReal j12array[1], cj12array[1],
37120 sj12array[1];
37121 bool j12valid[1] = {false};
37122 _nj12 = 1;
37123 if (IKabs(
37124 ((-1.0)
37125 * (((1.0) * new_r21))))
37126 < IKFAST_ATAN2_MAGTHRESH
37127 && IKabs(new_r20)
37128 < IKFAST_ATAN2_MAGTHRESH
37129 && IKabs(
37130 IKsqr(
37131 ((-1.0)
37132 * (((1.0)
37133 * new_r21))))
37134 + IKsqr(new_r20) - 1)
37135 <= IKFAST_SINCOS_THRESH)
37136 continue;
37137 j12array[0] = IKatan2(
37138 ((-1.0) * (((1.0) * new_r21))),
37139 new_r20);
37140 sj12array[0] = IKsin(j12array[0]);
37141 cj12array[0] = IKcos(j12array[0]);
37142 if (j12array[0] > IKPI)
37143 {
37144 j12array[0] -= IK2PI;
37145 }
37146 else if (j12array[0] < -IKPI)
37147 {
37148 j12array[0] += IK2PI;
37149 }
37150 j12valid[0] = true;
37151 for (int ij12 = 0; ij12 < 1; ++ij12)
37152 {
37153 if (!j12valid[ij12])
37154 {
37155 continue;
37156 }
37157 _ij12[0] = ij12;
37158 _ij12[1] = -1;
37159 for (int iij12 = ij12 + 1;
37160 iij12 < 1;
37161 ++iij12)
37162 {
37163 if (j12valid[iij12]
37164 && IKabs(
37165 cj12array[ij12]
37166 - cj12array[iij12])
37167 < IKFAST_SOLUTION_THRESH
37168 && IKabs(
37169 sj12array[ij12]
37170 - sj12array[iij12])
37171 < IKFAST_SOLUTION_THRESH)
37172 {
37173 j12valid[iij12] = false;
37174 _ij12[1] = iij12;
37175 break;
37176 }
37177 }
37178 j12 = j12array[ij12];
37179 cj12 = cj12array[ij12];
37180 sj12 = sj12array[ij12];
37181 {
37182 IkReal evalcond[8];
37183 IkReal x511 = IKsin(j12);
37184 IkReal x512 = IKcos(j12);
37185 IkReal x513 = ((1.0) * x512);
37186 IkReal x514 = ((-1.0) * x513);
37187 IkReal x515 = ((1.0) * x511);
37188 IkReal x516 = ((1.0) * new_r02);
37189 evalcond[0] = (x511 + new_r21);
37190 evalcond[1] = (x514 + new_r20);
37191 evalcond[2]
37192 = (((new_r02 * x512))
37193 + new_r11);
37194 evalcond[3]
37195 = (((new_r02 * x511))
37196 + new_r10);
37197 evalcond[4]
37198 = ((((-1.0) * new_r12
37199 * x513))
37200 + new_r01);
37201 evalcond[5]
37202 = ((((-1.0) * new_r12
37203 * x515))
37204 + new_r00);
37205 evalcond[6]
37206 = ((((-1.0) * new_r10
37207 * x516))
37208 + ((new_r00 * new_r12))
37209 + (((-1.0) * x515)));
37210 evalcond[7]
37211 = ((((-1.0) * new_r11
37212 * x516))
37213 + ((new_r01 * new_r12))
37214 + x514);
37215 if (IKabs(evalcond[0])
37216 > IKFAST_EVALCOND_THRESH
37217 || IKabs(evalcond[1])
37218 > IKFAST_EVALCOND_THRESH
37219 || IKabs(evalcond[2])
37220 > IKFAST_EVALCOND_THRESH
37221 || IKabs(evalcond[3])
37222 > IKFAST_EVALCOND_THRESH
37223 || IKabs(evalcond[4])
37224 > IKFAST_EVALCOND_THRESH
37225 || IKabs(evalcond[5])
37226 > IKFAST_EVALCOND_THRESH
37227 || IKabs(evalcond[6])
37228 > IKFAST_EVALCOND_THRESH
37229 || IKabs(evalcond[7])
37230 > IKFAST_EVALCOND_THRESH)
37231 {
37232 continue;
37233 }
37234 }
37235
37236 {
37237 std::vector<
37238 IkSingleDOFSolutionBase<
37239 IkReal> >
37240 vinfos(7);
37241 vinfos[0].jointtype = 1;
37242 vinfos[0].foffset = j4;
37243 vinfos[0].indices[0] = _ij4[0];
37244 vinfos[0].indices[1] = _ij4[1];
37245 vinfos[0].maxsolutions = _nj4;
37246 vinfos[1].jointtype = 1;
37247 vinfos[1].foffset = j6;
37248 vinfos[1].indices[0] = _ij6[0];
37249 vinfos[1].indices[1] = _ij6[1];
37250 vinfos[1].maxsolutions = _nj6;
37251 vinfos[2].jointtype = 1;
37252 vinfos[2].foffset = j8;
37253 vinfos[2].indices[0] = _ij8[0];
37254 vinfos[2].indices[1] = _ij8[1];
37255 vinfos[2].maxsolutions = _nj8;
37256 vinfos[3].jointtype = 1;
37257 vinfos[3].foffset = j9;
37258 vinfos[3].indices[0] = _ij9[0];
37259 vinfos[3].indices[1] = _ij9[1];
37260 vinfos[3].maxsolutions = _nj9;
37261 vinfos[4].jointtype = 1;
37262 vinfos[4].foffset = j10;
37263 vinfos[4].indices[0] = _ij10[0];
37264 vinfos[4].indices[1] = _ij10[1];
37265 vinfos[4].maxsolutions = _nj10;
37266 vinfos[5].jointtype = 1;
37267 vinfos[5].foffset = j11;
37268 vinfos[5].indices[0] = _ij11[0];
37269 vinfos[5].indices[1] = _ij11[1];
37270 vinfos[5].maxsolutions = _nj11;
37271 vinfos[6].jointtype = 1;
37272 vinfos[6].foffset = j12;
37273 vinfos[6].indices[0] = _ij12[0];
37274 vinfos[6].indices[1] = _ij12[1];
37275 vinfos[6].maxsolutions = _nj12;
37276 std::vector<int> vfree(0);
37277 solutions.AddSolution(
37278 vinfos, vfree);
37279 }
37280 }
37281 }
37282 }
37283 } while (0);
37284 if (bgotonextstatement)
37285 {
37286 bool bgotonextstatement = true;
37287 do
37288 {
37289 IkReal x517
37290 = ((((-1.0) * (1.0) * new_r02
37291 * sj10))
37292 + ((cj10 * new_r12)));
37293 IkReal x518
37294 = (((cj10 * new_r02))
37295 + ((new_r12 * sj10)));
37296 evalcond[0]
37297 = ((-3.14159265358979)
37298 + (IKfmod(
37299 ((3.14159265358979)
37300 + (IKabs(j11))),
37301 6.28318530717959)));
37302 evalcond[1] = ((-1.0) + new_r22);
37303 evalcond[2] = new_r20;
37304 evalcond[3] = new_r02;
37305 evalcond[4] = new_r12;
37306 evalcond[5] = new_r21;
37307 evalcond[6] = x517;
37308 evalcond[7] = x517;
37309 evalcond[8] = x518;
37310 evalcond[9] = x518;
37311 if (IKabs(evalcond[0])
37312 < 0.0000010000000000
37313 && IKabs(evalcond[1])
37314 < 0.0000010000000000
37315 && IKabs(evalcond[2])
37316 < 0.0000010000000000
37317 && IKabs(evalcond[3])
37318 < 0.0000010000000000
37319 && IKabs(evalcond[4])
37320 < 0.0000010000000000
37321 && IKabs(evalcond[5])
37322 < 0.0000010000000000
37323 && IKabs(evalcond[6])
37324 < 0.0000010000000000
37325 && IKabs(evalcond[7])
37326 < 0.0000010000000000
37327 && IKabs(evalcond[8])
37328 < 0.0000010000000000
37329 && IKabs(evalcond[9])
37330 < 0.0000010000000000)
37331 {
37332 bgotonextstatement = false;
37333 {
37334 IkReal j12array[1], cj12array[1],
37335 sj12array[1];
37336 bool j12valid[1] = {false};
37337 _nj12 = 1;
37338 IkReal x519 = ((1.0) * new_r01);
37339 if (IKabs(
37340 ((((-1.0) * cj10 * x519))
37341 + (((-1.0) * (1.0)
37342 * new_r00 * sj10))))
37343 < IKFAST_ATAN2_MAGTHRESH
37344 && IKabs(
37345 (((cj10 * new_r00))
37346 + (((-1.0) * sj10
37347 * x519))))
37348 < IKFAST_ATAN2_MAGTHRESH
37349 && IKabs(
37350 IKsqr(
37351 ((((-1.0) * cj10
37352 * x519))
37353 + (((-1.0) * (1.0)
37354 * new_r00
37355 * sj10))))
37356 + IKsqr((
37357 ((cj10
37358 * new_r00))
37359 + (((-1.0) * sj10
37360 * x519))))
37361 - 1)
37362 <= IKFAST_SINCOS_THRESH)
37363 continue;
37364 j12array[0] = IKatan2(
37365 ((((-1.0) * cj10 * x519))
37366 + (((-1.0) * (1.0) * new_r00
37367 * sj10))),
37368 (((cj10 * new_r00))
37369 + (((-1.0) * sj10 * x519))));
37370 sj12array[0] = IKsin(j12array[0]);
37371 cj12array[0] = IKcos(j12array[0]);
37372 if (j12array[0] > IKPI)
37373 {
37374 j12array[0] -= IK2PI;
37375 }
37376 else if (j12array[0] < -IKPI)
37377 {
37378 j12array[0] += IK2PI;
37379 }
37380 j12valid[0] = true;
37381 for (int ij12 = 0; ij12 < 1;
37382 ++ij12)
37383 {
37384 if (!j12valid[ij12])
37385 {
37386 continue;
37387 }
37388 _ij12[0] = ij12;
37389 _ij12[1] = -1;
37390 for (int iij12 = ij12 + 1;
37391 iij12 < 1;
37392 ++iij12)
37393 {
37394 if (j12valid[iij12]
37395 && IKabs(
37396 cj12array[ij12]
37397 - cj12array[iij12])
37398 < IKFAST_SOLUTION_THRESH
37399 && IKabs(
37400 sj12array[ij12]
37401 - sj12array[iij12])
37402 < IKFAST_SOLUTION_THRESH)
37403 {
37404 j12valid[iij12] = false;
37405 _ij12[1] = iij12;
37406 break;
37407 }
37408 }
37409 j12 = j12array[ij12];
37410 cj12 = cj12array[ij12];
37411 sj12 = sj12array[ij12];
37412 {
37413 IkReal evalcond[8];
37414 IkReal x520 = IKsin(j12);
37415 IkReal x521 = (cj10 * x520);
37416 IkReal x522 = IKcos(j12);
37417 IkReal x523 = ((1.0) * x522);
37418 IkReal x524 = ((-1.0) * x523);
37419 IkReal x525 = ((1.0) * sj10);
37420 IkReal x526
37421 = ((((-1.0) * cj10
37422 * x523))
37423 + ((sj10 * x520)));
37424 evalcond[0]
37425 = (((cj10 * new_r01))
37426 + ((new_r11 * sj10))
37427 + x520);
37428 evalcond[1]
37429 = (((sj10 * x522))
37430 + new_r01 + x521);
37431 evalcond[2]
37432 = (((cj10 * new_r00))
37433 + ((new_r10 * sj10))
37434 + x524);
37435 evalcond[3]
37436 = (((cj10 * new_r10))
37437 + (((-1.0) * new_r00
37438 * x525))
37439 + (((-1.0) * x520)));
37440 evalcond[4]
37441 = (((cj10 * new_r11))
37442 + (((-1.0) * new_r01
37443 * x525))
37444 + x524);
37445 evalcond[5]
37446 = (new_r00 + x526);
37447 evalcond[6]
37448 = (new_r11 + x526);
37449 evalcond[7]
37450 = ((((-1.0) * x521))
37451 + (((-1.0) * x522
37452 * x525))
37453 + new_r10);
37454 if (IKabs(evalcond[0])
37455 > IKFAST_EVALCOND_THRESH
37456 || IKabs(evalcond[1])
37457 > IKFAST_EVALCOND_THRESH
37458 || IKabs(evalcond[2])
37459 > IKFAST_EVALCOND_THRESH
37460 || IKabs(evalcond[3])
37461 > IKFAST_EVALCOND_THRESH
37462 || IKabs(evalcond[4])
37463 > IKFAST_EVALCOND_THRESH
37464 || IKabs(evalcond[5])
37465 > IKFAST_EVALCOND_THRESH
37466 || IKabs(evalcond[6])
37467 > IKFAST_EVALCOND_THRESH
37468 || IKabs(evalcond[7])
37469 > IKFAST_EVALCOND_THRESH)
37470 {
37471 continue;
37472 }
37473 }
37474
37475 {
37476 std::vector<
37477 IkSingleDOFSolutionBase<
37478 IkReal> >
37479 vinfos(7);
37480 vinfos[0].jointtype = 1;
37481 vinfos[0].foffset = j4;
37482 vinfos[0].indices[0]
37483 = _ij4[0];
37484 vinfos[0].indices[1]
37485 = _ij4[1];
37486 vinfos[0].maxsolutions = _nj4;
37487 vinfos[1].jointtype = 1;
37488 vinfos[1].foffset = j6;
37489 vinfos[1].indices[0]
37490 = _ij6[0];
37491 vinfos[1].indices[1]
37492 = _ij6[1];
37493 vinfos[1].maxsolutions = _nj6;
37494 vinfos[2].jointtype = 1;
37495 vinfos[2].foffset = j8;
37496 vinfos[2].indices[0]
37497 = _ij8[0];
37498 vinfos[2].indices[1]
37499 = _ij8[1];
37500 vinfos[2].maxsolutions = _nj8;
37501 vinfos[3].jointtype = 1;
37502 vinfos[3].foffset = j9;
37503 vinfos[3].indices[0]
37504 = _ij9[0];
37505 vinfos[3].indices[1]
37506 = _ij9[1];
37507 vinfos[3].maxsolutions = _nj9;
37508 vinfos[4].jointtype = 1;
37509 vinfos[4].foffset = j10;
37510 vinfos[4].indices[0]
37511 = _ij10[0];
37512 vinfos[4].indices[1]
37513 = _ij10[1];
37514 vinfos[4].maxsolutions
37515 = _nj10;
37516 vinfos[5].jointtype = 1;
37517 vinfos[5].foffset = j11;
37518 vinfos[5].indices[0]
37519 = _ij11[0];
37520 vinfos[5].indices[1]
37521 = _ij11[1];
37522 vinfos[5].maxsolutions
37523 = _nj11;
37524 vinfos[6].jointtype = 1;
37525 vinfos[6].foffset = j12;
37526 vinfos[6].indices[0]
37527 = _ij12[0];
37528 vinfos[6].indices[1]
37529 = _ij12[1];
37530 vinfos[6].maxsolutions
37531 = _nj12;
37532 std::vector<int> vfree(0);
37533 solutions.AddSolution(
37534 vinfos, vfree);
37535 }
37536 }
37537 }
37538 }
37539 } while (0);
37540 if (bgotonextstatement)
37541 {
37542 bool bgotonextstatement = true;
37543 do
37544 {
37545 IkReal x527
37546 = ((((-1.0) * (1.0) * new_r02
37547 * sj10))
37548 + ((cj10 * new_r12)));
37549 IkReal x528 = (cj10 * new_r02);
37550 IkReal x529 = (new_r12 * sj10);
37551 evalcond[0]
37552 = ((-3.14159265358979)
37553 + (IKfmod(
37554 ((3.14159265358979)
37555 + (IKabs((
37556 (-3.14159265358979)
37557 + j11)))),
37558 6.28318530717959)));
37559 evalcond[1] = ((1.0) + new_r22);
37560 evalcond[2] = new_r20;
37561 evalcond[3] = new_r02;
37562 evalcond[4] = new_r12;
37563 evalcond[5] = new_r21;
37564 evalcond[6] = x527;
37565 evalcond[7] = x527;
37566 evalcond[8] = (x529 + x528);
37567 evalcond[9]
37568 = ((((-1.0) * x528))
37569 + (((-1.0) * x529)));
37570 if (IKabs(evalcond[0])
37571 < 0.0000010000000000
37572 && IKabs(evalcond[1])
37573 < 0.0000010000000000
37574 && IKabs(evalcond[2])
37575 < 0.0000010000000000
37576 && IKabs(evalcond[3])
37577 < 0.0000010000000000
37578 && IKabs(evalcond[4])
37579 < 0.0000010000000000
37580 && IKabs(evalcond[5])
37581 < 0.0000010000000000
37582 && IKabs(evalcond[6])
37583 < 0.0000010000000000
37584 && IKabs(evalcond[7])
37585 < 0.0000010000000000
37586 && IKabs(evalcond[8])
37587 < 0.0000010000000000
37588 && IKabs(evalcond[9])
37589 < 0.0000010000000000)
37590 {
37591 bgotonextstatement = false;
37592 {
37593 IkReal j12array[1],
37594 cj12array[1], sj12array[1];
37595 bool j12valid[1] = {false};
37596 _nj12 = 1;
37597 IkReal x530 = ((1.0) * new_r00);
37598 if (IKabs((
37599 (((-1.0) * sj10 * x530))
37600 + ((cj10 * new_r01))))
37601 < IKFAST_ATAN2_MAGTHRESH
37602 && IKabs(
37603 ((((-1.0) * cj10
37604 * x530))
37605 + (((-1.0) * (1.0)
37606 * new_r01
37607 * sj10))))
37608 < IKFAST_ATAN2_MAGTHRESH
37609 && IKabs(
37610 IKsqr((
37611 (((-1.0) * sj10
37612 * x530))
37613 + ((cj10
37614 * new_r01))))
37615 + IKsqr((
37616 (((-1.0) * cj10
37617 * x530))
37618 + (((-1.0)
37619 * (1.0)
37620 * new_r01
37621 * sj10))))
37622 - 1)
37623 <= IKFAST_SINCOS_THRESH)
37624 continue;
37625 j12array[0] = IKatan2(
37626 ((((-1.0) * sj10 * x530))
37627 + ((cj10 * new_r01))),
37628 ((((-1.0) * cj10 * x530))
37629 + (((-1.0) * (1.0)
37630 * new_r01 * sj10))));
37631 sj12array[0]
37632 = IKsin(j12array[0]);
37633 cj12array[0]
37634 = IKcos(j12array[0]);
37635 if (j12array[0] > IKPI)
37636 {
37637 j12array[0] -= IK2PI;
37638 }
37639 else if (j12array[0] < -IKPI)
37640 {
37641 j12array[0] += IK2PI;
37642 }
37643 j12valid[0] = true;
37644 for (int ij12 = 0; ij12 < 1;
37645 ++ij12)
37646 {
37647 if (!j12valid[ij12])
37648 {
37649 continue;
37650 }
37651 _ij12[0] = ij12;
37652 _ij12[1] = -1;
37653 for (int iij12 = ij12 + 1;
37654 iij12 < 1;
37655 ++iij12)
37656 {
37657 if (j12valid[iij12]
37658 && IKabs(
37659 cj12array[ij12]
37660 - cj12array
37661 [iij12])
37662 < IKFAST_SOLUTION_THRESH
37663 && IKabs(
37664 sj12array[ij12]
37665 - sj12array
37666 [iij12])
37667 < IKFAST_SOLUTION_THRESH)
37668 {
37669 j12valid[iij12] = false;
37670 _ij12[1] = iij12;
37671 break;
37672 }
37673 }
37674 j12 = j12array[ij12];
37675 cj12 = cj12array[ij12];
37676 sj12 = sj12array[ij12];
37677 {
37678 IkReal evalcond[8];
37679 IkReal x531 = IKcos(j12);
37680 IkReal x532 = IKsin(j12);
37681 IkReal x533
37682 = ((1.0) * x532);
37683 IkReal x534
37684 = ((-1.0) * x533);
37685 IkReal x535 = (cj10 * x531);
37686 IkReal x536
37687 = ((1.0) * sj10);
37688 IkReal x537
37689 = ((((-1.0) * cj10
37690 * x533))
37691 + ((sj10 * x531)));
37692 evalcond[0]
37693 = (x531
37694 + ((cj10 * new_r00))
37695 + ((new_r10
37696 * sj10)));
37697 evalcond[1]
37698 = (x534
37699 + ((cj10 * new_r01))
37700 + ((new_r11
37701 * sj10)));
37702 evalcond[2]
37703 = (x535
37704 + ((sj10 * x532))
37705 + new_r00);
37706 evalcond[3]
37707 = (x534
37708 + ((cj10 * new_r10))
37709 + (((-1.0) * new_r00
37710 * x536)));
37711 evalcond[4]
37712 = (((cj10 * new_r11))
37713 + (((-1.0) * x531))
37714 + (((-1.0) * new_r01
37715 * x536)));
37716 evalcond[5]
37717 = (x537 + new_r01);
37718 evalcond[6]
37719 = (x537 + new_r10);
37720 evalcond[7]
37721 = ((((-1.0) * x532
37722 * x536))
37723 + new_r11
37724 + (((-1.0) * x535)));
37725 if (IKabs(evalcond[0])
37726 > IKFAST_EVALCOND_THRESH
37727 || IKabs(evalcond[1])
37728 > IKFAST_EVALCOND_THRESH
37729 || IKabs(evalcond[2])
37730 > IKFAST_EVALCOND_THRESH
37731 || IKabs(evalcond[3])
37732 > IKFAST_EVALCOND_THRESH
37733 || IKabs(evalcond[4])
37734 > IKFAST_EVALCOND_THRESH
37735 || IKabs(evalcond[5])
37736 > IKFAST_EVALCOND_THRESH
37737 || IKabs(evalcond[6])
37738 > IKFAST_EVALCOND_THRESH
37739 || IKabs(evalcond[7])
37740 > IKFAST_EVALCOND_THRESH)
37741 {
37742 continue;
37743 }
37744 }
37745
37746 {
37747 std::vector<
37748 IkSingleDOFSolutionBase<
37749 IkReal> >
37750 vinfos(7);
37751 vinfos[0].jointtype = 1;
37752 vinfos[0].foffset = j4;
37753 vinfos[0].indices[0]
37754 = _ij4[0];
37755 vinfos[0].indices[1]
37756 = _ij4[1];
37757 vinfos[0].maxsolutions
37758 = _nj4;
37759 vinfos[1].jointtype = 1;
37760 vinfos[1].foffset = j6;
37761 vinfos[1].indices[0]
37762 = _ij6[0];
37763 vinfos[1].indices[1]
37764 = _ij6[1];
37765 vinfos[1].maxsolutions
37766 = _nj6;
37767 vinfos[2].jointtype = 1;
37768 vinfos[2].foffset = j8;
37769 vinfos[2].indices[0]
37770 = _ij8[0];
37771 vinfos[2].indices[1]
37772 = _ij8[1];
37773 vinfos[2].maxsolutions
37774 = _nj8;
37775 vinfos[3].jointtype = 1;
37776 vinfos[3].foffset = j9;
37777 vinfos[3].indices[0]
37778 = _ij9[0];
37779 vinfos[3].indices[1]
37780 = _ij9[1];
37781 vinfos[3].maxsolutions
37782 = _nj9;
37783 vinfos[4].jointtype = 1;
37784 vinfos[4].foffset = j10;
37785 vinfos[4].indices[0]
37786 = _ij10[0];
37787 vinfos[4].indices[1]
37788 = _ij10[1];
37789 vinfos[4].maxsolutions
37790 = _nj10;
37791 vinfos[5].jointtype = 1;
37792 vinfos[5].foffset = j11;
37793 vinfos[5].indices[0]
37794 = _ij11[0];
37795 vinfos[5].indices[1]
37796 = _ij11[1];
37797 vinfos[5].maxsolutions
37798 = _nj11;
37799 vinfos[6].jointtype = 1;
37800 vinfos[6].foffset = j12;
37801 vinfos[6].indices[0]
37802 = _ij12[0];
37803 vinfos[6].indices[1]
37804 = _ij12[1];
37805 vinfos[6].maxsolutions
37806 = _nj12;
37807 std::vector<int> vfree(0);
37808 solutions.AddSolution(
37809 vinfos, vfree);
37810 }
37811 }
37812 }
37813 }
37814 } while (0);
37815 if (bgotonextstatement)
37816 {
37817 bool bgotonextstatement = true;
37818 do
37819 {
37820 IkReal x538 = ((1.0) * cj11);
37821 IkReal x539
37822 = ((((-1.0) * x538))
37823 + new_r22);
37824 IkReal x540 = ((1.0) * sj11);
37825 IkReal x541
37826 = ((((-1.0) * x540))
37827 + new_r02);
37828 evalcond[0]
37829 = ((-3.14159265358979)
37830 + (IKfmod(
37831 ((3.14159265358979)
37832 + (IKabs(j10))),
37833 6.28318530717959)));
37834 evalcond[1] = x539;
37835 evalcond[2] = x539;
37836 evalcond[3] = x541;
37837 evalcond[4] = new_r12;
37838 evalcond[5] = x541;
37839 evalcond[6]
37840 = ((((-1.0) * new_r22 * x540))
37841 + ((cj11 * new_r02)));
37842 evalcond[7]
37843 = ((((-1.0) * new_r20 * x538))
37844 + (((-1.0) * new_r00
37845 * x540)));
37846 evalcond[8]
37847 = ((((-1.0) * new_r21 * x538))
37848 + (((-1.0) * new_r01
37849 * x540)));
37850 evalcond[9]
37851 = ((1.0)
37852 + (((-1.0) * new_r02
37853 * x540))
37854 + (((-1.0) * new_r22
37855 * x538)));
37856 if (IKabs(evalcond[0])
37857 < 0.0000010000000000
37858 && IKabs(evalcond[1])
37859 < 0.0000010000000000
37860 && IKabs(evalcond[2])
37861 < 0.0000010000000000
37862 && IKabs(evalcond[3])
37863 < 0.0000010000000000
37864 && IKabs(evalcond[4])
37865 < 0.0000010000000000
37866 && IKabs(evalcond[5])
37867 < 0.0000010000000000
37868 && IKabs(evalcond[6])
37869 < 0.0000010000000000
37870 && IKabs(evalcond[7])
37871 < 0.0000010000000000
37872 && IKabs(evalcond[8])
37873 < 0.0000010000000000
37874 && IKabs(evalcond[9])
37875 < 0.0000010000000000)
37876 {
37877 bgotonextstatement = false;
37878 {
37879 IkReal j12array[1],
37880 cj12array[1],
37881 sj12array[1];
37882 bool j12valid[1] = {false};
37883 _nj12 = 1;
37884 if (IKabs(new_r10)
37885 < IKFAST_ATAN2_MAGTHRESH
37886 && IKabs(new_r11)
37887 < IKFAST_ATAN2_MAGTHRESH
37888 && IKabs(
37889 IKsqr(new_r10)
37890 + IKsqr(new_r11)
37891 - 1)
37892 <= IKFAST_SINCOS_THRESH)
37893 continue;
37894 j12array[0] = IKatan2(
37895 new_r10, new_r11);
37896 sj12array[0]
37897 = IKsin(j12array[0]);
37898 cj12array[0]
37899 = IKcos(j12array[0]);
37900 if (j12array[0] > IKPI)
37901 {
37902 j12array[0] -= IK2PI;
37903 }
37904 else if (j12array[0] < -IKPI)
37905 {
37906 j12array[0] += IK2PI;
37907 }
37908 j12valid[0] = true;
37909 for (int ij12 = 0; ij12 < 1;
37910 ++ij12)
37911 {
37912 if (!j12valid[ij12])
37913 {
37914 continue;
37915 }
37916 _ij12[0] = ij12;
37917 _ij12[1] = -1;
37918 for (int iij12 = ij12 + 1;
37919 iij12 < 1;
37920 ++iij12)
37921 {
37922 if (j12valid[iij12]
37923 && IKabs(
37924 cj12array[ij12]
37925 - cj12array
37926 [iij12])
37927 < IKFAST_SOLUTION_THRESH
37928 && IKabs(
37929 sj12array[ij12]
37930 - sj12array
37931 [iij12])
37932 < IKFAST_SOLUTION_THRESH)
37933 {
37934 j12valid[iij12] = false;
37935 _ij12[1] = iij12;
37936 break;
37937 }
37938 }
37939 j12 = j12array[ij12];
37940 cj12 = cj12array[ij12];
37941 sj12 = sj12array[ij12];
37942 {
37943 IkReal evalcond[8];
37944 IkReal x542 = IKcos(j12);
37945 IkReal x543 = IKsin(j12);
37946 IkReal x544
37947 = ((1.0) * x542);
37948 IkReal x545
37949 = ((-1.0) * x544);
37950 IkReal x546
37951 = ((1.0) * new_r02);
37952 evalcond[0]
37953 = (((new_r02 * x542))
37954 + new_r20);
37955 evalcond[1]
37956 = ((((-1.0) * x543))
37957 + new_r10);
37958 evalcond[2]
37959 = (new_r11 + x545);
37960 evalcond[3]
37961 = (((new_r22 * x543))
37962 + new_r01);
37963 evalcond[4]
37964 = ((((-1.0) * x543
37965 * x546))
37966 + new_r21);
37967 evalcond[5]
37968 = ((((-1.0) * new_r22
37969 * x544))
37970 + new_r00);
37971 evalcond[6]
37972 = (((new_r01
37973 * new_r22))
37974 + (((-1.0)
37975 * new_r21
37976 * x546))
37977 + x543);
37978 evalcond[7]
37979 = ((((-1.0) * new_r20
37980 * x546))
37981 + ((new_r00
37982 * new_r22))
37983 + x545);
37984 if (IKabs(evalcond[0])
37985 > IKFAST_EVALCOND_THRESH
37986 || IKabs(evalcond[1])
37987 > IKFAST_EVALCOND_THRESH
37988 || IKabs(evalcond[2])
37989 > IKFAST_EVALCOND_THRESH
37990 || IKabs(evalcond[3])
37991 > IKFAST_EVALCOND_THRESH
37992 || IKabs(evalcond[4])
37993 > IKFAST_EVALCOND_THRESH
37994 || IKabs(evalcond[5])
37995 > IKFAST_EVALCOND_THRESH
37996 || IKabs(evalcond[6])
37997 > IKFAST_EVALCOND_THRESH
37998 || IKabs(evalcond[7])
37999 > IKFAST_EVALCOND_THRESH)
38000 {
38001 continue;
38002 }
38003 }
38004
38005 {
38006 std::vector<
38007 IkSingleDOFSolutionBase<
38008 IkReal> >
38009 vinfos(7);
38010 vinfos[0].jointtype = 1;
38011 vinfos[0].foffset = j4;
38012 vinfos[0].indices[0]
38013 = _ij4[0];
38014 vinfos[0].indices[1]
38015 = _ij4[1];
38016 vinfos[0].maxsolutions
38017 = _nj4;
38018 vinfos[1].jointtype = 1;
38019 vinfos[1].foffset = j6;
38020 vinfos[1].indices[0]
38021 = _ij6[0];
38022 vinfos[1].indices[1]
38023 = _ij6[1];
38024 vinfos[1].maxsolutions
38025 = _nj6;
38026 vinfos[2].jointtype = 1;
38027 vinfos[2].foffset = j8;
38028 vinfos[2].indices[0]
38029 = _ij8[0];
38030 vinfos[2].indices[1]
38031 = _ij8[1];
38032 vinfos[2].maxsolutions
38033 = _nj8;
38034 vinfos[3].jointtype = 1;
38035 vinfos[3].foffset = j9;
38036 vinfos[3].indices[0]
38037 = _ij9[0];
38038 vinfos[3].indices[1]
38039 = _ij9[1];
38040 vinfos[3].maxsolutions
38041 = _nj9;
38042 vinfos[4].jointtype = 1;
38043 vinfos[4].foffset = j10;
38044 vinfos[4].indices[0]
38045 = _ij10[0];
38046 vinfos[4].indices[1]
38047 = _ij10[1];
38048 vinfos[4].maxsolutions
38049 = _nj10;
38050 vinfos[5].jointtype = 1;
38051 vinfos[5].foffset = j11;
38052 vinfos[5].indices[0]
38053 = _ij11[0];
38054 vinfos[5].indices[1]
38055 = _ij11[1];
38056 vinfos[5].maxsolutions
38057 = _nj11;
38058 vinfos[6].jointtype = 1;
38059 vinfos[6].foffset = j12;
38060 vinfos[6].indices[0]
38061 = _ij12[0];
38062 vinfos[6].indices[1]
38063 = _ij12[1];
38064 vinfos[6].maxsolutions
38065 = _nj12;
38066 std::vector<int> vfree(0);
38067 solutions.AddSolution(
38068 vinfos, vfree);
38069 }
38070 }
38071 }
38072 }
38073 } while (0);
38074 if (bgotonextstatement)
38075 {
38076 bool bgotonextstatement = true;
38077 do
38078 {
38079 IkReal x547 = ((1.0) * cj11);
38080 IkReal x548
38081 = ((((-1.0) * x547))
38082 + new_r22);
38083 IkReal x549 = ((1.0) * sj11);
38084 evalcond[0]
38085 = ((-3.14159265358979)
38086 + (IKfmod(
38087 ((3.14159265358979)
38088 + (IKabs((
38089 (-3.14159265358979)
38090 + j10)))),
38091 6.28318530717959)));
38092 evalcond[1] = x548;
38093 evalcond[2] = x548;
38094 evalcond[3] = (sj11 + new_r02);
38095 evalcond[4] = new_r12;
38096 evalcond[5]
38097 = ((((-1.0) * x549))
38098 + (((-1.0) * (1.0)
38099 * new_r02)));
38100 evalcond[6]
38101 = ((((-1.0) * new_r22
38102 * x549))
38103 + (((-1.0) * new_r02
38104 * x547)));
38105 evalcond[7]
38106 = ((((-1.0) * new_r20
38107 * x547))
38108 + ((new_r00 * sj11)));
38109 evalcond[8]
38110 = ((((-1.0) * new_r21
38111 * x547))
38112 + ((new_r01 * sj11)));
38113 evalcond[9]
38114 = ((1.0)
38115 + ((new_r02 * sj11))
38116 + (((-1.0) * new_r22
38117 * x547)));
38118 if (IKabs(evalcond[0])
38119 < 0.0000010000000000
38120 && IKabs(evalcond[1])
38121 < 0.0000010000000000
38122 && IKabs(evalcond[2])
38123 < 0.0000010000000000
38124 && IKabs(evalcond[3])
38125 < 0.0000010000000000
38126 && IKabs(evalcond[4])
38127 < 0.0000010000000000
38128 && IKabs(evalcond[5])
38129 < 0.0000010000000000
38130 && IKabs(evalcond[6])
38131 < 0.0000010000000000
38132 && IKabs(evalcond[7])
38133 < 0.0000010000000000
38134 && IKabs(evalcond[8])
38135 < 0.0000010000000000
38136 && IKabs(evalcond[9])
38137 < 0.0000010000000000)
38138 {
38139 bgotonextstatement = false;
38140 {
38141 IkReal j12array[1],
38142 cj12array[1],
38143 sj12array[1];
38144 bool j12valid[1] = {false};
38145 _nj12 = 1;
38146 CheckValue<IkReal> x550
38147 = IKatan2WithCheck(
38148 IkReal((
38149 (-1.0)
38150 * (((1.0)
38151 * new_r21)))),
38152 new_r20,
38153 IKFAST_ATAN2_MAGTHRESH);
38154 if (!x550.valid)
38155 {
38156 continue;
38157 }
38158 CheckValue<IkReal> x551
38159 = IKPowWithIntegerCheck(
38160 IKsign(new_r02),
38161 -1);
38162 if (!x551.valid)
38163 {
38164 continue;
38165 }
38166 j12array[0]
38167 = ((-1.5707963267949)
38168 + (x550.value)
38169 + (((1.5707963267949)
38170 * (x551.value))));
38171 sj12array[0]
38172 = IKsin(j12array[0]);
38173 cj12array[0]
38174 = IKcos(j12array[0]);
38175 if (j12array[0] > IKPI)
38176 {
38177 j12array[0] -= IK2PI;
38178 }
38179 else if (
38180 j12array[0] < -IKPI)
38181 {
38182 j12array[0] += IK2PI;
38183 }
38184 j12valid[0] = true;
38185 for (int ij12 = 0; ij12 < 1;
38186 ++ij12)
38187 {
38188 if (!j12valid[ij12])
38189 {
38190 continue;
38191 }
38192 _ij12[0] = ij12;
38193 _ij12[1] = -1;
38194 for (int iij12 = ij12 + 1;
38195 iij12 < 1;
38196 ++iij12)
38197 {
38198 if (j12valid[iij12]
38199 && IKabs(
38200 cj12array
38201 [ij12]
38202 - cj12array
38203 [iij12])
38204 < IKFAST_SOLUTION_THRESH
38205 && IKabs(
38206 sj12array
38207 [ij12]
38208 - sj12array
38209 [iij12])
38210 < IKFAST_SOLUTION_THRESH)
38211 {
38212 j12valid[iij12]
38213 = false;
38214 _ij12[1] = iij12;
38215 break;
38216 }
38217 }
38218 j12 = j12array[ij12];
38219 cj12 = cj12array[ij12];
38220 sj12 = sj12array[ij12];
38221 {
38222 IkReal evalcond[8];
38223 IkReal x552
38224 = IKsin(j12);
38225 IkReal x553
38226 = ((1.0)
38227 * (IKcos(j12)));
38228 IkReal x554
38229 = ((-1.0) * x553);
38230 IkReal x555
38231 = ((1.0) * new_r01);
38232 IkReal x556
38233 = ((1.0) * new_r00);
38234 evalcond[0]
38235 = (((new_r02
38236 * x552))
38237 + new_r21);
38238 evalcond[1]
38239 = ((((-1.0)
38240 * new_r02
38241 * x553))
38242 + new_r20);
38243 evalcond[2]
38244 = ((((-1.0) * (1.0)
38245 * new_r10))
38246 + (((-1.0)
38247 * x552)));
38248 evalcond[3]
38249 = (x554
38250 + (((-1.0)
38251 * (1.0)
38252 * new_r11)));
38253 evalcond[4]
38254 = (((new_r22
38255 * x552))
38256 + (((-1.0)
38257 * x555)));
38258 evalcond[5]
38259 = ((((-1.0) * x556))
38260 + (((-1.0)
38261 * new_r22
38262 * x553)));
38263 evalcond[6]
38264 = (((new_r02
38265 * new_r21))
38266 + (((-1.0)
38267 * new_r22
38268 * x555))
38269 + x552);
38270 evalcond[7]
38271 = (((new_r02
38272 * new_r20))
38273 + x554
38274 + (((-1.0)
38275 * new_r22
38276 * x556)));
38277 if (IKabs(evalcond[0])
38278 > IKFAST_EVALCOND_THRESH
38279 || IKabs(
38280 evalcond[1])
38281 > IKFAST_EVALCOND_THRESH
38282 || IKabs(
38283 evalcond[2])
38284 > IKFAST_EVALCOND_THRESH
38285 || IKabs(
38286 evalcond[3])
38287 > IKFAST_EVALCOND_THRESH
38288 || IKabs(
38289 evalcond[4])
38290 > IKFAST_EVALCOND_THRESH
38291 || IKabs(
38292 evalcond[5])
38293 > IKFAST_EVALCOND_THRESH
38294 || IKabs(
38295 evalcond[6])
38296 > IKFAST_EVALCOND_THRESH
38297 || IKabs(
38298 evalcond[7])
38299 > IKFAST_EVALCOND_THRESH)
38300 {
38301 continue;
38302 }
38303 }
38304
38305 {
38306 std::vector<
38307 IkSingleDOFSolutionBase<
38308 IkReal> >
38309 vinfos(7);
38310 vinfos[0].jointtype = 1;
38311 vinfos[0].foffset = j4;
38312 vinfos[0].indices[0]
38313 = _ij4[0];
38314 vinfos[0].indices[1]
38315 = _ij4[1];
38316 vinfos[0].maxsolutions
38317 = _nj4;
38318 vinfos[1].jointtype = 1;
38319 vinfos[1].foffset = j6;
38320 vinfos[1].indices[0]
38321 = _ij6[0];
38322 vinfos[1].indices[1]
38323 = _ij6[1];
38324 vinfos[1].maxsolutions
38325 = _nj6;
38326 vinfos[2].jointtype = 1;
38327 vinfos[2].foffset = j8;
38328 vinfos[2].indices[0]
38329 = _ij8[0];
38330 vinfos[2].indices[1]
38331 = _ij8[1];
38332 vinfos[2].maxsolutions
38333 = _nj8;
38334 vinfos[3].jointtype = 1;
38335 vinfos[3].foffset = j9;
38336 vinfos[3].indices[0]
38337 = _ij9[0];
38338 vinfos[3].indices[1]
38339 = _ij9[1];
38340 vinfos[3].maxsolutions
38341 = _nj9;
38342 vinfos[4].jointtype = 1;
38343 vinfos[4].foffset = j10;
38344 vinfos[4].indices[0]
38345 = _ij10[0];
38346 vinfos[4].indices[1]
38347 = _ij10[1];
38348 vinfos[4].maxsolutions
38349 = _nj10;
38350 vinfos[5].jointtype = 1;
38351 vinfos[5].foffset = j11;
38352 vinfos[5].indices[0]
38353 = _ij11[0];
38354 vinfos[5].indices[1]
38355 = _ij11[1];
38356 vinfos[5].maxsolutions
38357 = _nj11;
38358 vinfos[6].jointtype = 1;
38359 vinfos[6].foffset = j12;
38360 vinfos[6].indices[0]
38361 = _ij12[0];
38362 vinfos[6].indices[1]
38363 = _ij12[1];
38364 vinfos[6].maxsolutions
38365 = _nj12;
38366 std::vector<int> vfree(
38367 0);
38368 solutions.AddSolution(
38369 vinfos, vfree);
38370 }
38371 }
38372 }
38373 }
38374 } while (0);
38375 if (bgotonextstatement)
38376 {
38377 bool bgotonextstatement = true;
38378 do
38379 {
38380 if (1)
38381 {
38382 bgotonextstatement = false;
38383 continue; // branch miss
38384 // [j12]
38385 }
38386 } while (0);
38387 if (bgotonextstatement)
38388 {
38389 }
38390 }
38391 }
38392 }
38393 }
38394 }
38395 }
38396 }
38397 }
38398 }
38399 }
38400 else
38401 {
38402 {
38403 IkReal j12array[1], cj12array[1], sj12array[1];
38404 bool j12valid[1] = {false};
38405 _nj12 = 1;
38406 CheckValue<IkReal> x558
38407 = IKPowWithIntegerCheck(sj11, -1);
38408 if (!x558.valid)
38409 {
38410 continue;
38411 }
38412 IkReal x557 = x558.value;
38413 CheckValue<IkReal> x559
38414 = IKPowWithIntegerCheck(cj10, -1);
38415 if (!x559.valid)
38416 {
38417 continue;
38418 }
38419 CheckValue<IkReal> x560
38420 = IKPowWithIntegerCheck(cj11, -1);
38421 if (!x560.valid)
38422 {
38423 continue;
38424 }
38425 if (IKabs(
38426 (x557 * (x559.value) * (x560.value)
38427 * (((((-1.0) * (1.0) * new_r01 * sj11))
38428 + ((new_r20 * sj10))))))
38429 < IKFAST_ATAN2_MAGTHRESH
38430 && IKabs(((-1.0) * new_r20 * x557))
38431 < IKFAST_ATAN2_MAGTHRESH
38432 && IKabs(
38433 IKsqr(
38434 (x557 * (x559.value)
38435 * (x560.value)
38436 * (((((-1.0) * (1.0) * new_r01
38437 * sj11))
38438 + ((new_r20 * sj10))))))
38439 + IKsqr(((-1.0) * new_r20 * x557))
38440 - 1)
38441 <= IKFAST_SINCOS_THRESH)
38442 continue;
38443 j12array[0] = IKatan2(
38444 (x557 * (x559.value) * (x560.value)
38445 * (((((-1.0) * (1.0) * new_r01 * sj11))
38446 + ((new_r20 * sj10))))),
38447 ((-1.0) * new_r20 * x557));
38448 sj12array[0] = IKsin(j12array[0]);
38449 cj12array[0] = IKcos(j12array[0]);
38450 if (j12array[0] > IKPI)
38451 {
38452 j12array[0] -= IK2PI;
38453 }
38454 else if (j12array[0] < -IKPI)
38455 {
38456 j12array[0] += IK2PI;
38457 }
38458 j12valid[0] = true;
38459 for (int ij12 = 0; ij12 < 1; ++ij12)
38460 {
38461 if (!j12valid[ij12])
38462 {
38463 continue;
38464 }
38465 _ij12[0] = ij12;
38466 _ij12[1] = -1;
38467 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38468 {
38469 if (j12valid[iij12]
38470 && IKabs(
38471 cj12array[ij12]
38472 - cj12array[iij12])
38473 < IKFAST_SOLUTION_THRESH
38474 && IKabs(
38475 sj12array[ij12]
38476 - sj12array[iij12])
38477 < IKFAST_SOLUTION_THRESH)
38478 {
38479 j12valid[iij12] = false;
38480 _ij12[1] = iij12;
38481 break;
38482 }
38483 }
38484 j12 = j12array[ij12];
38485 cj12 = cj12array[ij12];
38486 sj12 = sj12array[ij12];
38487 {
38488 IkReal evalcond[12];
38489 IkReal x561 = IKcos(j12);
38490 IkReal x562 = IKsin(j12);
38491 IkReal x563 = ((1.0) * sj11);
38492 IkReal x564 = (cj10 * new_r01);
38493 IkReal x565 = (new_r11 * sj10);
38494 IkReal x566 = (cj11 * x562);
38495 IkReal x567 = ((1.0) * sj10);
38496 IkReal x568 = ((1.0) * x562);
38497 IkReal x569 = ((1.0) * x561);
38498 IkReal x570 = ((-1.0) * x569);
38499 IkReal x571 = (cj10 * new_r00);
38500 IkReal x572 = (new_r10 * sj10);
38501 IkReal x573 = (cj10 * x569);
38502 evalcond[0] = (((sj11 * x561)) + new_r20);
38503 evalcond[1]
38504 = ((((-1.0) * x562 * x563)) + new_r21);
38505 evalcond[2] = (x564 + x565 + x566);
38506 evalcond[3]
38507 = (((cj10 * new_r10))
38508 + (((-1.0) * x568))
38509 + (((-1.0) * new_r00 * x567)));
38510 evalcond[4]
38511 = (((cj10 * new_r11)) + x570
38512 + (((-1.0) * new_r01 * x567)));
38513 evalcond[5]
38514 = (((cj10 * x566)) + new_r01
38515 + ((sj10 * x561)));
38516 evalcond[6]
38517 = (x572 + x571
38518 + (((-1.0) * cj11 * x569)));
38519 evalcond[7]
38520 = (((sj10 * x562))
38521 + (((-1.0) * cj11 * x573))
38522 + new_r00);
38523 evalcond[8]
38524 = ((((-1.0) * x573)) + new_r11
38525 + ((sj10 * x566)));
38526 evalcond[9]
38527 = ((((-1.0) * cj10 * x568))
38528 + (((-1.0) * cj11 * x561 * x567))
38529 + new_r10);
38530 evalcond[10]
38531 = ((((-1.0) * new_r21 * x563))
38532 + ((cj11 * x565)) + ((cj11 * x564))
38533 + x562);
38534 evalcond[11]
38535 = (x570 + ((cj11 * x572))
38536 + ((cj11 * x571))
38537 + (((-1.0) * new_r20 * x563)));
38538 if (IKabs(evalcond[0])
38539 > IKFAST_EVALCOND_THRESH
38540 || IKabs(evalcond[1])
38541 > IKFAST_EVALCOND_THRESH
38542 || IKabs(evalcond[2])
38543 > IKFAST_EVALCOND_THRESH
38544 || IKabs(evalcond[3])
38545 > IKFAST_EVALCOND_THRESH
38546 || IKabs(evalcond[4])
38547 > IKFAST_EVALCOND_THRESH
38548 || IKabs(evalcond[5])
38549 > IKFAST_EVALCOND_THRESH
38550 || IKabs(evalcond[6])
38551 > IKFAST_EVALCOND_THRESH
38552 || IKabs(evalcond[7])
38553 > IKFAST_EVALCOND_THRESH
38554 || IKabs(evalcond[8])
38555 > IKFAST_EVALCOND_THRESH
38556 || IKabs(evalcond[9])
38557 > IKFAST_EVALCOND_THRESH
38558 || IKabs(evalcond[10])
38559 > IKFAST_EVALCOND_THRESH
38560 || IKabs(evalcond[11])
38561 > IKFAST_EVALCOND_THRESH)
38562 {
38563 continue;
38564 }
38565 }
38566
38567 {
38568 std::vector<
38569 IkSingleDOFSolutionBase<IkReal> >
38570 vinfos(7);
38571 vinfos[0].jointtype = 1;
38572 vinfos[0].foffset = j4;
38573 vinfos[0].indices[0] = _ij4[0];
38574 vinfos[0].indices[1] = _ij4[1];
38575 vinfos[0].maxsolutions = _nj4;
38576 vinfos[1].jointtype = 1;
38577 vinfos[1].foffset = j6;
38578 vinfos[1].indices[0] = _ij6[0];
38579 vinfos[1].indices[1] = _ij6[1];
38580 vinfos[1].maxsolutions = _nj6;
38581 vinfos[2].jointtype = 1;
38582 vinfos[2].foffset = j8;
38583 vinfos[2].indices[0] = _ij8[0];
38584 vinfos[2].indices[1] = _ij8[1];
38585 vinfos[2].maxsolutions = _nj8;
38586 vinfos[3].jointtype = 1;
38587 vinfos[3].foffset = j9;
38588 vinfos[3].indices[0] = _ij9[0];
38589 vinfos[3].indices[1] = _ij9[1];
38590 vinfos[3].maxsolutions = _nj9;
38591 vinfos[4].jointtype = 1;
38592 vinfos[4].foffset = j10;
38593 vinfos[4].indices[0] = _ij10[0];
38594 vinfos[4].indices[1] = _ij10[1];
38595 vinfos[4].maxsolutions = _nj10;
38596 vinfos[5].jointtype = 1;
38597 vinfos[5].foffset = j11;
38598 vinfos[5].indices[0] = _ij11[0];
38599 vinfos[5].indices[1] = _ij11[1];
38600 vinfos[5].maxsolutions = _nj11;
38601 vinfos[6].jointtype = 1;
38602 vinfos[6].foffset = j12;
38603 vinfos[6].indices[0] = _ij12[0];
38604 vinfos[6].indices[1] = _ij12[1];
38605 vinfos[6].maxsolutions = _nj12;
38606 std::vector<int> vfree(0);
38607 solutions.AddSolution(vinfos, vfree);
38608 }
38609 }
38610 }
38611 }
38612 }
38613 }
38614 else
38615 {
38616 {
38617 IkReal j12array[1], cj12array[1], sj12array[1];
38618 bool j12valid[1] = {false};
38619 _nj12 = 1;
38620 CheckValue<IkReal> x576
38621 = IKPowWithIntegerCheck(sj11, -1);
38622 if (!x576.valid)
38623 {
38624 continue;
38625 }
38626 IkReal x574 = x576.value;
38627 IkReal x575 = ((1.0) * new_r20);
38628 CheckValue<IkReal> x577
38629 = IKPowWithIntegerCheck(sj10, -1);
38630 if (!x577.valid)
38631 {
38632 continue;
38633 }
38634 if (IKabs(
38635 (x574 * (x577.value)
38636 * (((((-1.0) * cj10 * cj11 * x575))
38637 + (((-1.0) * (1.0) * new_r00
38638 * sj11))))))
38639 < IKFAST_ATAN2_MAGTHRESH
38640 && IKabs(((-1.0) * x574 * x575))
38641 < IKFAST_ATAN2_MAGTHRESH
38642 && IKabs(
38643 IKsqr(
38644 (x574 * (x577.value)
38645 * (((((-1.0) * cj10 * cj11 * x575))
38646 + (((-1.0) * (1.0) * new_r00
38647 * sj11))))))
38648 + IKsqr(((-1.0) * x574 * x575)) - 1)
38649 <= IKFAST_SINCOS_THRESH)
38650 continue;
38651 j12array[0] = IKatan2(
38652 (x574 * (x577.value)
38653 * (((((-1.0) * cj10 * cj11 * x575))
38654 + (((-1.0) * (1.0) * new_r00 * sj11))))),
38655 ((-1.0) * x574 * x575));
38656 sj12array[0] = IKsin(j12array[0]);
38657 cj12array[0] = IKcos(j12array[0]);
38658 if (j12array[0] > IKPI)
38659 {
38660 j12array[0] -= IK2PI;
38661 }
38662 else if (j12array[0] < -IKPI)
38663 {
38664 j12array[0] += IK2PI;
38665 }
38666 j12valid[0] = true;
38667 for (int ij12 = 0; ij12 < 1; ++ij12)
38668 {
38669 if (!j12valid[ij12])
38670 {
38671 continue;
38672 }
38673 _ij12[0] = ij12;
38674 _ij12[1] = -1;
38675 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38676 {
38677 if (j12valid[iij12]
38678 && IKabs(cj12array[ij12] - cj12array[iij12])
38679 < IKFAST_SOLUTION_THRESH
38680 && IKabs(sj12array[ij12] - sj12array[iij12])
38681 < IKFAST_SOLUTION_THRESH)
38682 {
38683 j12valid[iij12] = false;
38684 _ij12[1] = iij12;
38685 break;
38686 }
38687 }
38688 j12 = j12array[ij12];
38689 cj12 = cj12array[ij12];
38690 sj12 = sj12array[ij12];
38691 {
38692 IkReal evalcond[12];
38693 IkReal x578 = IKcos(j12);
38694 IkReal x579 = IKsin(j12);
38695 IkReal x580 = ((1.0) * sj11);
38696 IkReal x581 = (cj10 * new_r01);
38697 IkReal x582 = (new_r11 * sj10);
38698 IkReal x583 = (cj11 * x579);
38699 IkReal x584 = ((1.0) * sj10);
38700 IkReal x585 = ((1.0) * x579);
38701 IkReal x586 = ((1.0) * x578);
38702 IkReal x587 = ((-1.0) * x586);
38703 IkReal x588 = (cj10 * new_r00);
38704 IkReal x589 = (new_r10 * sj10);
38705 IkReal x590 = (cj10 * x586);
38706 evalcond[0] = (((sj11 * x578)) + new_r20);
38707 evalcond[1]
38708 = ((((-1.0) * x579 * x580)) + new_r21);
38709 evalcond[2] = (x581 + x582 + x583);
38710 evalcond[3]
38711 = (((cj10 * new_r10)) + (((-1.0) * x585))
38712 + (((-1.0) * new_r00 * x584)));
38713 evalcond[4]
38714 = (((cj10 * new_r11))
38715 + (((-1.0) * new_r01 * x584)) + x587);
38716 evalcond[5]
38717 = (((sj10 * x578)) + ((cj10 * x583))
38718 + new_r01);
38719 evalcond[6]
38720 = ((((-1.0) * cj11 * x586)) + x589 + x588);
38721 evalcond[7]
38722 = (new_r00 + ((sj10 * x579))
38723 + (((-1.0) * cj11 * x590)));
38724 evalcond[8]
38725 = ((((-1.0) * x590)) + ((sj10 * x583))
38726 + new_r11);
38727 evalcond[9]
38728 = ((((-1.0) * cj10 * x585))
38729 + (((-1.0) * cj11 * x578 * x584))
38730 + new_r10);
38731 evalcond[10]
38732 = (x579 + ((cj11 * x581)) + ((cj11 * x582))
38733 + (((-1.0) * new_r21 * x580)));
38734 evalcond[11]
38735 = (((cj11 * x589))
38736 + (((-1.0) * new_r20 * x580))
38737 + ((cj11 * x588)) + x587);
38738 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
38739 || IKabs(evalcond[1])
38740 > IKFAST_EVALCOND_THRESH
38741 || IKabs(evalcond[2])
38742 > IKFAST_EVALCOND_THRESH
38743 || IKabs(evalcond[3])
38744 > IKFAST_EVALCOND_THRESH
38745 || IKabs(evalcond[4])
38746 > IKFAST_EVALCOND_THRESH
38747 || IKabs(evalcond[5])
38748 > IKFAST_EVALCOND_THRESH
38749 || IKabs(evalcond[6])
38750 > IKFAST_EVALCOND_THRESH
38751 || IKabs(evalcond[7])
38752 > IKFAST_EVALCOND_THRESH
38753 || IKabs(evalcond[8])
38754 > IKFAST_EVALCOND_THRESH
38755 || IKabs(evalcond[9])
38756 > IKFAST_EVALCOND_THRESH
38757 || IKabs(evalcond[10])
38758 > IKFAST_EVALCOND_THRESH
38759 || IKabs(evalcond[11])
38760 > IKFAST_EVALCOND_THRESH)
38761 {
38762 continue;
38763 }
38764 }
38765
38766 {
38767 std::vector<IkSingleDOFSolutionBase<IkReal> >
38768 vinfos(7);
38769 vinfos[0].jointtype = 1;
38770 vinfos[0].foffset = j4;
38771 vinfos[0].indices[0] = _ij4[0];
38772 vinfos[0].indices[1] = _ij4[1];
38773 vinfos[0].maxsolutions = _nj4;
38774 vinfos[1].jointtype = 1;
38775 vinfos[1].foffset = j6;
38776 vinfos[1].indices[0] = _ij6[0];
38777 vinfos[1].indices[1] = _ij6[1];
38778 vinfos[1].maxsolutions = _nj6;
38779 vinfos[2].jointtype = 1;
38780 vinfos[2].foffset = j8;
38781 vinfos[2].indices[0] = _ij8[0];
38782 vinfos[2].indices[1] = _ij8[1];
38783 vinfos[2].maxsolutions = _nj8;
38784 vinfos[3].jointtype = 1;
38785 vinfos[3].foffset = j9;
38786 vinfos[3].indices[0] = _ij9[0];
38787 vinfos[3].indices[1] = _ij9[1];
38788 vinfos[3].maxsolutions = _nj9;
38789 vinfos[4].jointtype = 1;
38790 vinfos[4].foffset = j10;
38791 vinfos[4].indices[0] = _ij10[0];
38792 vinfos[4].indices[1] = _ij10[1];
38793 vinfos[4].maxsolutions = _nj10;
38794 vinfos[5].jointtype = 1;
38795 vinfos[5].foffset = j11;
38796 vinfos[5].indices[0] = _ij11[0];
38797 vinfos[5].indices[1] = _ij11[1];
38798 vinfos[5].maxsolutions = _nj11;
38799 vinfos[6].jointtype = 1;
38800 vinfos[6].foffset = j12;
38801 vinfos[6].indices[0] = _ij12[0];
38802 vinfos[6].indices[1] = _ij12[1];
38803 vinfos[6].maxsolutions = _nj12;
38804 std::vector<int> vfree(0);
38805 solutions.AddSolution(vinfos, vfree);
38806 }
38807 }
38808 }
38809 }
38810 }
38811 }
38812 else
38813 {
38814 {
38815 IkReal j12array[1], cj12array[1], sj12array[1];
38816 bool j12valid[1] = {false};
38817 _nj12 = 1;
38818 CheckValue<IkReal> x591
38819 = IKPowWithIntegerCheck(IKsign(sj11), -1);
38820 if (!x591.valid)
38821 {
38822 continue;
38823 }
38824 CheckValue<IkReal> x592 = IKatan2WithCheck(
38825 IkReal(new_r21),
38826 ((-1.0) * (((1.0) * new_r20))),
38827 IKFAST_ATAN2_MAGTHRESH);
38828 if (!x592.valid)
38829 {
38830 continue;
38831 }
38832 j12array[0]
38833 = ((-1.5707963267949)
38834 + (((1.5707963267949) * (x591.value)))
38835 + (x592.value));
38836 sj12array[0] = IKsin(j12array[0]);
38837 cj12array[0] = IKcos(j12array[0]);
38838 if (j12array[0] > IKPI)
38839 {
38840 j12array[0] -= IK2PI;
38841 }
38842 else if (j12array[0] < -IKPI)
38843 {
38844 j12array[0] += IK2PI;
38845 }
38846 j12valid[0] = true;
38847 for (int ij12 = 0; ij12 < 1; ++ij12)
38848 {
38849 if (!j12valid[ij12])
38850 {
38851 continue;
38852 }
38853 _ij12[0] = ij12;
38854 _ij12[1] = -1;
38855 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38856 {
38857 if (j12valid[iij12]
38858 && IKabs(cj12array[ij12] - cj12array[iij12])
38859 < IKFAST_SOLUTION_THRESH
38860 && IKabs(sj12array[ij12] - sj12array[iij12])
38861 < IKFAST_SOLUTION_THRESH)
38862 {
38863 j12valid[iij12] = false;
38864 _ij12[1] = iij12;
38865 break;
38866 }
38867 }
38868 j12 = j12array[ij12];
38869 cj12 = cj12array[ij12];
38870 sj12 = sj12array[ij12];
38871 {
38872 IkReal evalcond[12];
38873 IkReal x593 = IKcos(j12);
38874 IkReal x594 = IKsin(j12);
38875 IkReal x595 = ((1.0) * sj11);
38876 IkReal x596 = (cj10 * new_r01);
38877 IkReal x597 = (new_r11 * sj10);
38878 IkReal x598 = (cj11 * x594);
38879 IkReal x599 = ((1.0) * sj10);
38880 IkReal x600 = ((1.0) * x594);
38881 IkReal x601 = ((1.0) * x593);
38882 IkReal x602 = ((-1.0) * x601);
38883 IkReal x603 = (cj10 * new_r00);
38884 IkReal x604 = (new_r10 * sj10);
38885 IkReal x605 = (cj10 * x601);
38886 evalcond[0] = (new_r20 + ((sj11 * x593)));
38887 evalcond[1] = ((((-1.0) * x594 * x595)) + new_r21);
38888 evalcond[2] = (x598 + x596 + x597);
38889 evalcond[3]
38890 = (((cj10 * new_r10)) + (((-1.0) * x600))
38891 + (((-1.0) * new_r00 * x599)));
38892 evalcond[4]
38893 = (((cj10 * new_r11))
38894 + (((-1.0) * new_r01 * x599)) + x602);
38895 evalcond[5]
38896 = (((cj10 * x598)) + ((sj10 * x593)) + new_r01);
38897 evalcond[6]
38898 = ((((-1.0) * cj11 * x601)) + x603 + x604);
38899 evalcond[7]
38900 = ((((-1.0) * cj11 * x605)) + ((sj10 * x594))
38901 + new_r00);
38902 evalcond[8]
38903 = (((sj10 * x598)) + new_r11
38904 + (((-1.0) * x605)));
38905 evalcond[9]
38906 = ((((-1.0) * cj10 * x600))
38907 + (((-1.0) * cj11 * x593 * x599)) + new_r10);
38908 evalcond[10]
38909 = (((cj11 * x597)) + x594
38910 + (((-1.0) * new_r21 * x595))
38911 + ((cj11 * x596)));
38912 evalcond[11]
38913 = (((cj11 * x603)) + x602 + ((cj11 * x604))
38914 + (((-1.0) * new_r20 * x595)));
38915 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
38916 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
38917 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
38918 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
38919 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
38920 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
38921 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
38922 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH
38923 || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH
38924 || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH
38925 || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH
38926 || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
38927 {
38928 continue;
38929 }
38930 }
38931
38932 {
38933 std::vector<IkSingleDOFSolutionBase<IkReal> >
38934 vinfos(7);
38935 vinfos[0].jointtype = 1;
38936 vinfos[0].foffset = j4;
38937 vinfos[0].indices[0] = _ij4[0];
38938 vinfos[0].indices[1] = _ij4[1];
38939 vinfos[0].maxsolutions = _nj4;
38940 vinfos[1].jointtype = 1;
38941 vinfos[1].foffset = j6;
38942 vinfos[1].indices[0] = _ij6[0];
38943 vinfos[1].indices[1] = _ij6[1];
38944 vinfos[1].maxsolutions = _nj6;
38945 vinfos[2].jointtype = 1;
38946 vinfos[2].foffset = j8;
38947 vinfos[2].indices[0] = _ij8[0];
38948 vinfos[2].indices[1] = _ij8[1];
38949 vinfos[2].maxsolutions = _nj8;
38950 vinfos[3].jointtype = 1;
38951 vinfos[3].foffset = j9;
38952 vinfos[3].indices[0] = _ij9[0];
38953 vinfos[3].indices[1] = _ij9[1];
38954 vinfos[3].maxsolutions = _nj9;
38955 vinfos[4].jointtype = 1;
38956 vinfos[4].foffset = j10;
38957 vinfos[4].indices[0] = _ij10[0];
38958 vinfos[4].indices[1] = _ij10[1];
38959 vinfos[4].maxsolutions = _nj10;
38960 vinfos[5].jointtype = 1;
38961 vinfos[5].foffset = j11;
38962 vinfos[5].indices[0] = _ij11[0];
38963 vinfos[5].indices[1] = _ij11[1];
38964 vinfos[5].maxsolutions = _nj11;
38965 vinfos[6].jointtype = 1;
38966 vinfos[6].foffset = j12;
38967 vinfos[6].indices[0] = _ij12[0];
38968 vinfos[6].indices[1] = _ij12[1];
38969 vinfos[6].maxsolutions = _nj12;
38970 std::vector<int> vfree(0);
38971 solutions.AddSolution(vinfos, vfree);
38972 }
38973 }
38974 }
38975 }
38976 }
38977 }
38978 }
38979 }
38980 }
38981 }
38982 }
38983 }
38984 }
38985 };
38986
38987 /// solves the inverse kinematics equations.
38988 /// \param pfree is an array specifying the free joints of the chain.
ComputeIk(const IkReal * eetrans,const IkReal * eerot,const IkReal * pfree,IkSolutionListBase<IkReal> & solutions)38989 IKFAST_API bool ComputeIk(
38990 const IkReal* eetrans,
38991 const IkReal* eerot,
38992 const IkReal* pfree,
38993 IkSolutionListBase<IkReal>& solutions)
38994 {
38995 IKSolver solver;
38996 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
38997 }
38998
ComputeIk2(const IkReal * eetrans,const IkReal * eerot,const IkReal * pfree,IkSolutionListBase<IkReal> & solutions,void * pOpenRAVEManip)38999 IKFAST_API bool ComputeIk2(
39000 const IkReal* eetrans,
39001 const IkReal* eerot,
39002 const IkReal* pfree,
39003 IkSolutionListBase<IkReal>& solutions,
39004 void* pOpenRAVEManip)
39005 {
39006 IKSolver solver;
39007 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
39008 }
39009
GetKinematicsHash()39010 IKFAST_API const char* GetKinematicsHash()
39011 {
39012 return "268c2c509bc1bb657da055f0ef2eb7e1";
39013 }
39014
GetIkFastVersion()39015 IKFAST_API const char* GetIkFastVersion()
39016 {
39017 return IKFAST_STRINGIZE(IKFAST_VERSION);
39018 }
39019
39020 #ifdef IKFAST_NAMESPACE
39021 } // end namespace
39022 #endif
39023
39024 //==============================================================================
SharedLibraryWamIkFast(dart::dynamics::InverseKinematics * ik,const std::vector<std::size_t> & dofMap,const std::vector<std::size_t> & freeDofMap,const std::string & methodName,const dart::dynamics::InverseKinematics::Analytical::Properties & properties)39025 SharedLibraryWamIkFast::SharedLibraryWamIkFast(
39026 dart::dynamics::InverseKinematics* ik,
39027 const std::vector<std::size_t>& dofMap,
39028 const std::vector<std::size_t>& freeDofMap,
39029 const std::string& methodName,
39030 const dart::dynamics::InverseKinematics::Analytical::Properties& properties)
39031 : IkFast{ik, dofMap, freeDofMap, methodName, properties}
39032 {
39033 // Do nothing
39034 }
39035
39036 //==============================================================================
39037 std::unique_ptr<dart::dynamics::InverseKinematics::GradientMethod>
clone(dart::dynamics::InverseKinematics * newIK) const39038 SharedLibraryWamIkFast::clone(dart::dynamics::InverseKinematics* newIK) const
39039 {
39040 return std::make_unique<SharedLibraryWamIkFast>(
39041 newIK, mDofs, mFreeDofs, getMethodName(), getAnalyticalProperties());
39042 }
39043
39044 //==============================================================================
getNumFreeParameters() const39045 int SharedLibraryWamIkFast::getNumFreeParameters() const
39046 {
39047 return GetNumFreeParameters();
39048 }
39049
39050 //==============================================================================
getFreeParameters() const39051 int* SharedLibraryWamIkFast::getFreeParameters() const
39052 {
39053 return GetFreeParameters();
39054 }
39055
39056 //==============================================================================
getNumJoints() const39057 int SharedLibraryWamIkFast::getNumJoints() const
39058 {
39059 return GetNumJoints();
39060 }
39061
39062 //==============================================================================
getIkRealSize() const39063 int SharedLibraryWamIkFast::getIkRealSize() const
39064 {
39065 return GetIkRealSize();
39066 }
39067
39068 //==============================================================================
getIkType() const39069 int SharedLibraryWamIkFast::getIkType() const
39070 {
39071 return GetIkType();
39072 }
39073
39074 //==============================================================================
computeIk(const IkReal * targetTranspose,const IkReal * targetRotation,const IkReal * freeParams,ikfast::IkSolutionListBase<IkReal> & solutions)39075 bool SharedLibraryWamIkFast::computeIk(
39076 const IkReal* targetTranspose,
39077 const IkReal* targetRotation,
39078 const IkReal* freeParams,
39079 ikfast::IkSolutionListBase<IkReal>& solutions)
39080 {
39081 return ComputeIk(targetTranspose, targetRotation, freeParams, solutions);
39082 }
39083
39084 //==============================================================================
computeFk(const IkReal * parameters,IkReal * targetTranspose,IkReal * targetRotation)39085 void SharedLibraryWamIkFast::computeFk(
39086 const IkReal* parameters, IkReal* targetTranspose, IkReal* targetRotation)
39087 {
39088 ComputeFk(parameters, targetTranspose, targetRotation);
39089 }
39090
39091 //==============================================================================
getKinematicsHash()39092 const char* SharedLibraryWamIkFast::getKinematicsHash()
39093 {
39094 return GetKinematicsHash();
39095 }
39096
39097 //==============================================================================
getIkFastVersion()39098 const char* SharedLibraryWamIkFast::getIkFastVersion()
39099 {
39100 return GetIkFastVersion();
39101 }
39102