Debian Codegraph @

unittests/integration/GeneratedWamIkFast.cpp

raw
1 /// autogenerated analytical inverse kinematics code from ikfast program part of
2 /// OpenRAVE \author Rosen Diankov
3 ///
4 /// Licensed under the Apache License, Version 2.0 (the "License");
5 /// you may not use this file except in compliance with the License.
6 /// You may obtain a copy of the License at
7 /// http://www.apache.org/licenses/LICENSE-2.0
8 ///
9 /// Unless required by applicable law or agreed to in writing, software
10 /// distributed under the License is distributed on an "AS IS" BASIS,
11 /// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 /// See the License for the specific language governing permissions and
13 /// limitations under the License.
14 ///
15 /// ikfast version 71 generated on 2016-04-05 16:09:02.982560
16 /// To compile with gcc:
17 /// gcc -lstdc++ ik.cpp
18 /// To compile without any main function as a shared object (might need
19 /// -llapack):
20 /// gcc -fPIC -lstdc++ -DIKFAST_NO_MAIN -DIKFAST_CLIBRARY -shared
21 /// -Wl,-soname,libik.so -o libik.so ik.cpp
22 #define IKFAST_HAS_LIBRARY
23 #include "dart/external/ikfast/ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
24 using namespace ikfast;
25
26 // check if the included ikfast version matches what this file was compiled with
27 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
28 IKFAST_COMPILE_ASSERT(IKFAST_VERSION == 71);
29
30 #include <algorithm>
31 #include <cmath>
32 #include <complex>
33 #include <limits>
34 #include <vector>
35
36 #define IKFAST_STRINGIZE2(s) #s
37 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
38
39 #ifndef IKFAST_ASSERT
40 #include <iostream>
41 #include <sstream>
42 #include <stdexcept>
43
44 #ifdef _MSC_VER
45 #ifndef __PRETTY_FUNCTION__
46 #define __PRETTY_FUNCTION__ __FUNCDNAME__
47 #endif
48 #endif
49
50 #ifndef __PRETTY_FUNCTION__
51 #define __PRETTY_FUNCTION__ __func__
52 #endif
53
54 #define IKFAST_ASSERT(b) \
55 { \
56 if (!(b)) \
57 { \
58 std::stringstream ss; \
59 ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " \
60 << __PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; \
61 throw std::runtime_error(ss.str()); \
62 } \
63 }
64
65 #endif
66
67 #if defined(_MSC_VER)
68 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
69 #else
70 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
71 #endif
72
73 #define IK2PI ((IkReal)6.28318530717959)
74 #define IKPI ((IkReal)3.14159265358979)
75 #define IKPI_2 ((IkReal)1.57079632679490)
76
77 // lapack routines
78 extern "C" {
79 void dgetrf_(
80 const int* m,
81 const int* n,
82 double* a,
83 const int* lda,
84 int* ipiv,
85 int* info);
86 void zgetrf_(
87 const int* m,
88 const int* n,
89 std::complex<double>* a,
90 const int* lda,
91 int* ipiv,
92 int* info);
93 void dgetri_(
94 const int* n,
95 const double* a,
96 const int* lda,
97 int* ipiv,
98 double* work,
99 const int* lwork,
100 int* info);
101 void dgesv_(
102 const int* n,
103 const int* nrhs,
104 double* a,
105 const int* lda,
106 int* ipiv,
107 double* b,
108 const int* ldb,
109 int* info);
110 void dgetrs_(
111 const char* trans,
112 const int* n,
113 const int* nrhs,
114 double* a,
115 const int* lda,
116 int* ipiv,
117 double* b,
118 const int* ldb,
119 int* info);
120 void dgeev_(
121 const char* jobvl,
122 const char* jobvr,
123 const int* n,
124 double* a,
125 const int* lda,
126 double* wr,
127 double* wi,
128 double* vl,
129 const int* ldvl,
130 double* vr,
131 const int* ldvr,
132 double* work,
133 const int* lwork,
134 int* info);
135 }
136
137 using namespace std; // necessary to get std math routines
138
139 #ifdef IKFAST_NAMESPACE
140 namespace IKFAST_NAMESPACE {
141 #endif
142
143 inline float IKabs(float f)
144 {
145 return fabsf(f);
146 }
147 inline double IKabs(double f)
148 {
149 return fabs(f);
150 }
151
152 inline float IKsqr(float f)
153 {
154 return f * f;
155 }
156 inline double IKsqr(double f)
157 {
158 return f * f;
159 }
160
161 inline float IKlog(float f)
162 {
163 return logf(f);
164 }
165 inline double IKlog(double f)
166 {
167 return log(f);
168 }
169
170 // allows asin and acos to exceed 1
171 #ifndef IKFAST_SINCOS_THRESH
172 #define IKFAST_SINCOS_THRESH ((IkReal)2e-6)
173 #endif
174
175 // used to check input to atan2 for degenerate cases
176 #ifndef IKFAST_ATAN2_MAGTHRESH
177 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
178 #endif
179
180 // minimum distance of separate solutions
181 #ifndef IKFAST_SOLUTION_THRESH
182 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
183 #endif
184
185 // there are checkpoints in ikfast that are evaluated to make sure they are 0.
186 // This threshold speicfies by how much they can deviate
187 #ifndef IKFAST_EVALCOND_THRESH
188 #define IKFAST_EVALCOND_THRESH ((IkReal)0.000005)
189 #endif
190
191 inline float IKasin(float f)
192 {
193 IKFAST_ASSERT(
194 f > -1 - IKFAST_SINCOS_THRESH
195 && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
196 // wrong with the solver
197 if (f <= -1)
198 return float(-IKPI_2);
199 else if (f >= 1)
200 return float(IKPI_2);
201 return asinf(f);
202 }
203 inline double IKasin(double 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 -IKPI_2;
211 else if (f >= 1)
212 return IKPI_2;
213 return asin(f);
214 }
215
216 // return positive value in [0,y)
217 inline float IKfmod(float x, float y)
218 {
219 while (x < 0)
220 {
221 x += y;
222 }
223 return fmodf(x, y);
224 }
225
226 // return positive value in [0,y)
227 inline double IKfmod(double x, double y)
228 {
229 while (x < 0)
230 {
231 x += y;
232 }
233 return fmod(x, y);
234 }
235
236 inline float IKacos(float f)
237 {
238 IKFAST_ASSERT(
239 f > -1 - IKFAST_SINCOS_THRESH
240 && f < 1 + IKFAST_SINCOS_THRESH); // any more error implies something is
241 // wrong with the solver
242 if (f <= -1)
243 return float(IKPI);
244 else if (f >= 1)
245 return float(0);
246 return acosf(f);
247 }
248 inline double IKacos(double 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 IKPI;
256 else if (f >= 1)
257 return 0;
258 return acos(f);
259 }
260 inline float IKsin(float f)
261 {
262 return sinf(f);
263 }
264 inline double IKsin(double f)
265 {
266 return sin(f);
267 }
268 inline float IKcos(float f)
269 {
270 return cosf(f);
271 }
272 inline double IKcos(double f)
273 {
274 return cos(f);
275 }
276 inline float IKtan(float f)
277 {
278 return tanf(f);
279 }
280 inline double IKtan(double f)
281 {
282 return tan(f);
283 }
284 inline float IKsqrt(float f)
285 {
286 if (f <= 0.0f)
287 return 0.0f;
288 return sqrtf(f);
289 }
290 inline double IKsqrt(double f)
291 {
292 if (f <= 0.0)
293 return 0.0;
294 return sqrt(f);
295 }
296 inline float IKatan2Simple(float fy, float fx)
297 {
298 return atan2f(fy, fx);
299 }
300 inline float IKatan2(float fy, float fx)
301 {
302 if (isnan(fy))
303 {
304 IKFAST_ASSERT(
305 !isnan(fx)); // if both are nan, probably wrong value will be returned
306 return float(IKPI_2);
307 }
308 else if (isnan(fx))
309 {
310 return 0;
311 }
312 return atan2f(fy, fx);
313 }
314 inline double IKatan2Simple(double fy, double fx)
315 {
316 return atan2(fy, fx);
317 }
318 inline double IKatan2(double fy, double fx)
319 {
320 if (std::isnan(fy))
321 {
322 IKFAST_ASSERT(!std::isnan(
323 fx)); // if both are nan, probably wrong value will be returned
324 return IKPI_2;
325 }
326 else if (std::isnan(fx))
327 {
328 return 0;
329 }
330 return atan2(fy, fx);
331 }
332
333 template <typename T>
334 struct CheckValue
335 {
336 T value;
337 bool valid;
338 };
339
340 template <typename T>
341 inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T epsilon)
342 {
343 CheckValue<T> ret;
344 ret.valid = false;
345 ret.value = 0;
346 if (!std::isnan(fy) && !std::isnan(fx))
347 {
348 if (IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH
349 || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH)
350 {
351 ret.value = IKatan2Simple(fy, fx);
352 ret.valid = true;
353 }
354 }
355 return ret;
356 }
357
358 inline float IKsign(float f)
359 {
360 if (f > 0)
361 {
362 return float(1);
363 }
364 else if (f < 0)
365 {
366 return float(-1);
367 }
368 return 0;
369 }
370
371 inline double IKsign(double f)
372 {
373 if (f > 0)
374 {
375 return 1.0;
376 }
377 else if (f < 0)
378 {
379 return -1.0;
380 }
381 return 0;
382 }
383
384 template <typename T>
385 inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
386 {
387 CheckValue<T> ret;
388 ret.valid = true;
389 if (n == 0)
390 {
391 ret.value = 1.0;
392 return ret;
393 }
394 else if (n == 1)
395 {
396 ret.value = f;
397 return ret;
398 }
399 else if (n < 0)
400 {
401 if (f == 0)
402 {
403 ret.valid = false;
404 ret.value = (T)1.0e30;
405 return ret;
406 }
407 if (n == -1)
408 {
409 ret.value = T(1.0) / f;
410 return ret;
411 }
412 }
413
414 int num = n > 0 ? n : -n;
415 if (num == 2)
416 {
417 ret.value = f * f;
418 }
419 else if (num == 3)
420 {
421 ret.value = f * f * f;
422 }
423 else
424 {
425 ret.value = 1.0;
426 while (num > 0)
427 {
428 if (num & 1)
429 {
430 ret.value *= f;
431 }
432 num >>= 1;
433 f *= f;
434 }
435 }
436
437 if (n < 0)
438 {
439 ret.value = T(1.0) / ret.value;
440 }
441 return ret;
442 }
443
444 /// solves the forward kinematics equations.
445 /// \param pfree is an array specifying the free joints of the chain.
446 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot)
447 {
448 IkReal x0, x1, x2, x3, x4, x5, x6, x7, x8, x9, x10, x11, x12, x13, x14, x15,
449 x16, x17, x18, x19, x20, x21, x22, x23, x24, x25, x26, x27, x28, x29, x30,
450 x31, x32, x33, x34, x35, x36, x37, x38, x39, x40, x41, x42, x43, x44, x45,
451 x46, x47, x48, x49, x50, x51, x52, x53, x54, x55, x56, x57, x58, x59, x60,
452 x61, x62, x63;
453 x0 = IKsin(j[6]);
454 x1 = IKcos(j[4]);
455 x2 = IKsin(j[0]);
456 x3 = IKcos(j[2]);
457 x4 = (x2 * x3);
458 x5 = ((1.0) * x4);
459 x6 = IKcos(j[1]);
460 x7 = IKsin(j[2]);
461 x8 = IKcos(j[0]);
462 x9 = (x7 * x8);
463 x10 = ((1.0) * x9);
464 x11 = ((((-1.0) * (1.0) * x5)) + (((-1.0) * (1.0) * x10 * x6)));
465 x12 = IKsin(j[4]);
466 x13 = IKsin(j[1]);
467 x14 = IKsin(j[3]);
468 x15 = (x13 * x14 * x8);
469 x16 = ((1.0) * x15);
470 x17 = IKcos(j[3]);
471 x18 = ((1.0) * x17);
472 x19 = (x2 * x7);
473 x20 = ((1.0) * x19);
474 x21 = (x3 * x8);
475 x22 = (x21 * x6);
476 x23 = (x22 + (((-1.0) * (1.0) * x20)));
477 x24 = (((x12 * (((((-1.0) * (1.0) * x18 * x23)) + x16)))) + ((x1 * x11)));
478 x25 = IKcos(j[6]);
479 x26 = IKsin(j[5]);
480 x27 = (x13 * x17 * x8);
481 x28 = ((1.0) * x27);
482 x29 = (x14 * ((x20 + (((-1.0) * (1.0) * x22)))));
483 x30 = (x26 * ((x29 + (((-1.0) * (1.0) * x28)))));
484 x31 = IKcos(j[5]);
485 x32 = (((x11 * x12)) + ((x1 * (((((-1.0) * (1.0) * x16)) + ((x17 * x23)))))));
486 x33 = (x31 * x32);
487 x34 = ((0.045) * x19);
488 x35 = ((0.55) * x13);
489 x36 = ((0.045) * x22);
490 x37 = ((((-1.0) * (1.0) * x20 * x6)) + x21);
491 x38 = (x13 * x14 * x2);
492 x39 = ((1.0) * x38);
493 x40 = (x4 * x6);
494 x41 = (x9 + x40);
495 x42 = (((x12 * ((x39 + (((-1.0) * (1.0) * x18 * x41)))))) + ((x1 * x37)));
496 x43 = (x13 * x17 * x2);
497 x44 = ((1.0) * x43);
498 x45 = (x14 * (((((-1.0) * (1.0) * x10)) + (((-1.0) * (1.0) * x5 * x6)))));
499 x46 = (x26 * (((((-1.0) * (1.0) * x44)) + x45)));
500 x47 = (((x1 * (((((-1.0) * (1.0) * x39)) + ((x17 * x41)))))) + ((x12 * x37)));
501 x48 = (x31 * x47);
502 x49 = ((0.045) * x9);
503 x50 = ((0.045) * x40);
504 x51 = (x13 * x7);
505 x52 = (x14 * x6);
506 x53 = ((1.0) * x52);
507 x54 = (x13 * x3);
508 x55 = (x18 * x54);
509 x56 = (((x1 * x51)) + ((x12 * ((x55 + x53)))));
510 x57 = (x17 * x6);
511 x58 = ((1.0) * x57);
512 x59 = (x14 * x54);
513 x60 = (x26 * (((((-1.0) * (1.0) * x58)) + x59)));
514 x61
515 = (((x1 * (((((-1.0) * (1.0) * x55)) + (((-1.0) * (1.0) * x53))))))
516 + ((x12 * x51)));
517 x62 = (x31 * x61);
518 x63 = ((0.045) * x54);
519 eerot[0] = (((x0 * x24)) + ((x25 * ((x30 + x33)))));
520 eerot[1]
521 = (((x0 * (((((-1.0) * (1.0) * x30)) + (((-1.0) * (1.0) * x33))))))
522 + ((x24 * x25)));
523 eerot[2] = (((x26 * x32)) + ((x31 * ((x28 + (((-1.0) * (1.0) * x29)))))));
524 eetrans[0]
525 = ((0.22) + ((x17 * ((x34 + (((-1.0) * (1.0) * x36)))))) + x36
526 + (((0.045) * x15))
527 + ((x14 * (((((0.3) * x22)) + (((-1.0) * (0.3) * x19))))))
528 + (((0.3) * x27)) + (((-1.0) * (1.0) * x34)) + ((x35 * x8)));
529 eerot[3] = (((x25 * ((x48 + x46)))) + ((x0 * x42)));
530 eerot[4]
531 = (((x0 * (((((-1.0) * (1.0) * x48)) + (((-1.0) * (1.0) * x46))))))
532 + ((x25 * x42)));
533 eerot[5] = (((x26 * x47)) + ((x31 * (((((-1.0) * (1.0) * x45)) + x44)))));
534 eetrans[1]
535 = ((0.14) + ((x2 * x35)) + (((0.3) * x43)) + (((0.045) * x38))
536 + ((x17 * (((((-1.0) * (1.0) * x49)) + (((-1.0) * (1.0) * x50))))))
537 + x49 + x50 + ((x14 * (((((0.3) * x40)) + (((0.3) * x9)))))));
538 eerot[6] = (((x0 * x56)) + ((x25 * ((x60 + x62)))));
539 eerot[7]
540 = (((x0 * (((((-1.0) * (1.0) * x60)) + (((-1.0) * (1.0) * x62))))))
541 + ((x25 * x56)));
542 eerot[8] = (((x26 * x61)) + ((x31 * (((((-1.0) * (1.0) * x59)) + x58)))));
543 eetrans[2]
544 = ((0.346) + (((0.045) * x52)) + (((-1.0) * (0.3) * x59))
545 + (((0.55) * x6)) + (((0.3) * x57)) + (((-1.0) * (1.0) * x63))
546 + ((x17 * x63)));
547 }
548
549 IKFAST_API int GetNumFreeParameters()
550 {
551 return 1;
552 }
553 IKFAST_API int* GetFreeParameters()
554 {
555 static int freeparams[] = {2};
556 return freeparams;
557 }
558 IKFAST_API int GetNumJoints()
559 {
560 return 7;
561 }
562
563 IKFAST_API int GetIkRealSize()
564 {
565 return sizeof(IkReal);
566 }
567
568 IKFAST_API int GetIkType()
569 {
570 return 0x67000001;
571 }
572
573 class IKSolver
574 {
575 public:
576 IkReal j4, cj4, sj4, htj4, j4mul, j6, cj6, sj6, htj6, j6mul, j9, cj9, sj9,
577 htj9, j9mul, j10, cj10, sj10, htj10, j10mul, j11, cj11, sj11, htj11,
578 j11mul, j12, cj12, sj12, htj12, j12mul, j8, cj8, sj8, htj8, new_r00, r00,
579 rxp0_0, new_r01, r01, rxp0_1, new_r02, r02, rxp0_2, new_r10, r10, rxp1_0,
580 new_r11, r11, rxp1_1, new_r12, r12, rxp1_2, new_r20, r20, rxp2_0, new_r21,
581 r21, rxp2_1, new_r22, r22, rxp2_2, new_px, px, npx, new_py, py, npy,
582 new_pz, pz, npz, pp;
583 unsigned char _ij4[2], _nj4, _ij6[2], _nj6, _ij9[2], _nj9, _ij10[2], _nj10,
584 _ij11[2], _nj11, _ij12[2], _nj12, _ij8[2], _nj8;
585
586 IkReal j100, cj100, sj100;
587 unsigned char _ij100[2], _nj100;
588 bool ComputeIk(
589 const IkReal* eetrans,
590 const IkReal* eerot,
591 const IkReal* pfree,
592 IkSolutionListBase<IkReal>& solutions)
593 {
594 j4 = numeric_limits<IkReal>::quiet_NaN();
595 _ij4[0] = -1;
596 _ij4[1] = -1;
597 _nj4 = -1;
598 j6 = numeric_limits<IkReal>::quiet_NaN();
599 _ij6[0] = -1;
600 _ij6[1] = -1;
601 _nj6 = -1;
602 j9 = numeric_limits<IkReal>::quiet_NaN();
603 _ij9[0] = -1;
604 _ij9[1] = -1;
605 _nj9 = -1;
606 j10 = numeric_limits<IkReal>::quiet_NaN();
607 _ij10[0] = -1;
608 _ij10[1] = -1;
609 _nj10 = -1;
610 j11 = numeric_limits<IkReal>::quiet_NaN();
611 _ij11[0] = -1;
612 _ij11[1] = -1;
613 _nj11 = -1;
614 j12 = numeric_limits<IkReal>::quiet_NaN();
615 _ij12[0] = -1;
616 _ij12[1] = -1;
617 _nj12 = -1;
618 _ij8[0] = -1;
619 _ij8[1] = -1;
620 _nj8 = 0;
621 for (int dummyiter = 0; dummyiter < 1; ++dummyiter)
622 {
623 solutions.Clear();
624 j8 = pfree[0];
625 cj8 = cos(pfree[0]);
626 sj8 = sin(pfree[0]), htj8 = tan(pfree[0] * 0.5);
627 r00 = eerot[0 * 3 + 0];
628 r01 = eerot[0 * 3 + 1];
629 r02 = eerot[0 * 3 + 2];
630 r10 = eerot[1 * 3 + 0];
631 r11 = eerot[1 * 3 + 1];
632 r12 = eerot[1 * 3 + 2];
633 r20 = eerot[2 * 3 + 0];
634 r21 = eerot[2 * 3 + 1];
635 r22 = eerot[2 * 3 + 2];
636 px = eetrans[0];
637 py = eetrans[1];
638 pz = eetrans[2];
639
640 new_r00 = r00;
641 new_r01 = r01;
642 new_r02 = r02;
643 new_px = ((-0.22) + px);
644 new_r10 = r10;
645 new_r11 = r11;
646 new_r12 = r12;
647 new_py = ((-0.14) + py);
648 new_r20 = r20;
649 new_r21 = r21;
650 new_r22 = r22;
651 new_pz = ((-0.346) + pz);
652 r00 = new_r00;
653 r01 = new_r01;
654 r02 = new_r02;
655 r10 = new_r10;
656 r11 = new_r11;
657 r12 = new_r12;
658 r20 = new_r20;
659 r21 = new_r21;
660 r22 = new_r22;
661 px = new_px;
662 py = new_py;
663 pz = new_pz;
664 IkReal x64 = ((1.0) * py);
665 IkReal x65 = ((1.0) * pz);
666 IkReal x66 = ((1.0) * px);
667 pp = ((pz * pz) + (py * py) + (px * px));
668 npx = (((pz * r20)) + ((py * r10)) + ((px * r00)));
669 npy = (((pz * r21)) + ((py * r11)) + ((px * r01)));
670 npz = (((px * r02)) + ((pz * r22)) + ((py * r12)));
671 rxp0_0 = (((pz * r10)) + (((-1.0) * r20 * x64)));
672 rxp0_1 = (((px * r20)) + (((-1.0) * r00 * x65)));
673 rxp0_2 = (((py * r00)) + (((-1.0) * r10 * x66)));
674 rxp1_0 = ((((-1.0) * r21 * x64)) + ((pz * r11)));
675 rxp1_1 = (((px * r21)) + (((-1.0) * r01 * x65)));
676 rxp1_2 = ((((-1.0) * r11 * x66)) + ((py * r01)));
677 rxp2_0 = ((((-1.0) * r22 * x64)) + ((pz * r12)));
678 rxp2_1 = ((((-1.0) * r02 * x65)) + ((px * r22)));
679 rxp2_2 = (((py * r02)) + (((-1.0) * r12 * x66)));
680 {
681 IkReal j9array[2], cj9array[2], sj9array[2];
682 bool j9valid[2] = {false};
683 _nj9 = 2;
684 if ((((-1.18441410190393) + (((2.9867963734811) * pp))))
685 < -1 - IKFAST_SINCOS_THRESH
686 || (((-1.18441410190393) + (((2.9867963734811) * pp))))
687 > 1 + IKFAST_SINCOS_THRESH)
688 continue;
689 IkReal x67 = IKasin(((-1.18441410190393) + (((2.9867963734811) * pp))));
690 j9array[0] = ((-1.34027003705633) + (((1.0) * x67)));
691 sj9array[0] = IKsin(j9array[0]);
692 cj9array[0] = IKcos(j9array[0]);
693 j9array[1] = ((1.80132261653346) + (((-1.0) * x67)));
694 sj9array[1] = IKsin(j9array[1]);
695 cj9array[1] = IKcos(j9array[1]);
696 if (j9array[0] > IKPI)
697 {
698 j9array[0] -= IK2PI;
699 }
700 else if (j9array[0] < -IKPI)
701 {
702 j9array[0] += IK2PI;
703 }
704 j9valid[0] = true;
705 if (j9array[1] > IKPI)
706 {
707 j9array[1] -= IK2PI;
708 }
709 else if (j9array[1] < -IKPI)
710 {
711 j9array[1] += IK2PI;
712 }
713 j9valid[1] = true;
714 for (int ij9 = 0; ij9 < 2; ++ij9)
715 {
716 if (!j9valid[ij9])
717 {
718 continue;
719 }
720 _ij9[0] = ij9;
721 _ij9[1] = -1;
722 for (int iij9 = ij9 + 1; iij9 < 2; ++iij9)
723 {
724 if (j9valid[iij9]
725 && IKabs(cj9array[ij9] - cj9array[iij9])
726 < IKFAST_SOLUTION_THRESH
727 && IKabs(sj9array[ij9] - sj9array[iij9])
728 < IKFAST_SOLUTION_THRESH)
729 {
730 j9valid[iij9] = false;
731 _ij9[1] = iij9;
732 break;
733 }
734 }
735 j9 = j9array[ij9];
736 cj9 = cj9array[ij9];
737 sj9 = sj9array[ij9];
738
739 {
740 IkReal j4eval[2];
741 j4eval[0] = ((IKabs(py)) + (IKabs(px)));
742 j4eval[1] = ((py * py) + (px * px));
743 if (IKabs(j4eval[0]) < 0.0000010000000000
744 || IKabs(j4eval[1]) < 0.0000010000000000)
745 {
746 {
747 IkReal j6eval[2];
748 IkReal x68 = cj8 * cj8;
749 IkReal x69 = sj9 * sj9;
750 IkReal x70 = ((13.3333333333333) * sj9);
751 IkReal x71 = (cj9 * x70);
752 IkReal x72 = cj9 * cj9;
753 IkReal x73 = ((3.0) * cj8);
754 j6eval[0]
755 = ((149.382716049383) + (((-2.0) * cj9 * x68))
756 + ((x68 * x72)) + (((44.4444444444444) * x68 * x69))
757 + (((44.4444444444444) * x72)) + x68 + x69
758 + (((24.4444444444444) * sj9))
759 + (((162.962962962963) * cj9)) + x71
760 + (((-1.0) * x68 * x71)) + ((x68 * x70)));
761 j6eval[1]
762 = ((((66.6666666666667)
763 * (IKabs(
764 ((-0.55) + (((-1.0) * (0.3) * cj9))
765 + (((-1.0) * (0.045) * sj9)))))))
766 + (IKabs(
767 ((((-1.0) * cj9 * x73)) + x73
768 + (((20.0) * cj8 * sj9))))));
769 if (IKabs(j6eval[0]) < 0.0000010000000000
770 || IKabs(j6eval[1]) < 0.0000010000000000)
771 {
772 {
773 IkReal j4eval[2];
774 IkReal x74 = cj8 * cj8 * cj8 * cj8;
775 IkReal x75 = py * py * py * py;
776 IkReal x76 = sj8 * sj8 * sj8 * sj8;
777 IkReal x77 = px * px;
778 IkReal x78 = py * py;
779 IkReal x79 = (x77 * x78);
780 IkReal x80 = sj8 * sj8;
781 IkReal x81 = ((2.0) * x80);
782 IkReal x82 = cj8 * cj8;
783 IkReal x83 = (px * py);
784 j4eval[0]
785 = (((x75 * x76)) + ((x74 * x75)) + ((x76 * x79))
786 + ((x74 * x79)) + ((x75 * x81 * x82))
787 + ((x77 * x78 * x81 * x82)));
788 j4eval[1]
789 = ((IKabs((((x80 * x83)) + ((x82 * x83)))))
790 + (IKabs((((x78 * x82)) + ((x78 * x80))))));
791 if (IKabs(j4eval[0]) < 0.0000010000000000
792 || IKabs(j4eval[1]) < 0.0000010000000000)
793 {
794 continue; // no branches [j4, j6]
795 }
796 else
797 {
798 {
799 IkReal j4array[2], cj4array[2], sj4array[2];
800 bool j4valid[2] = {false};
801 _nj4 = 2;
802 IkReal x84 = cj8 * cj8;
803 IkReal x85 = py * py;
804 IkReal x86 = sj8 * sj8;
805 IkReal x87 = (((x85 * x86)) + ((x84 * x85)));
806 IkReal x88 = ((1.0) * px * py);
807 IkReal x89
808 = ((((-1.0) * x86 * x88)) + (((-1.0) * x84 * x88)));
809 CheckValue<IkReal> x93 = IKatan2WithCheck(
810 IkReal(x87), x89, IKFAST_ATAN2_MAGTHRESH);
811 if (!x93.valid)
812 {
813 continue;
814 }
815 IkReal x90 = ((-1.0) * (x93.value));
816 IkReal x91 = ((0.045) * py * sj8);
817 if ((((x87 * x87) + (x89 * x89))) < -0.00001)
818 continue;
819 CheckValue<IkReal> x94 = IKPowWithIntegerCheck(
820 IKabs(IKsqrt(((x87 * x87) + (x89 * x89)))), -1);
821 if (!x94.valid)
822 {
823 continue;
824 }
825 if ((((x94.value)
826 * ((((cj9 * x91)) + (((-1.0) * x91))
827 + (((-1.0) * (0.3) * py * sj8 * sj9))))))
828 < -1 - IKFAST_SINCOS_THRESH
829 || (((x94.value)
830 * ((((cj9 * x91)) + (((-1.0) * x91))
831 + (((-1.0) * (0.3) * py * sj8 * sj9))))))
832 > 1 + IKFAST_SINCOS_THRESH)
833 continue;
834 IkReal x92 = IKasin(
835 ((x94.value)
836 * ((((cj9 * x91)) + (((-1.0) * x91))
837 + (((-1.0) * (0.3) * py * sj8 * sj9))))));
838 j4array[0] = (x90 + (((-1.0) * x92)));
839 sj4array[0] = IKsin(j4array[0]);
840 cj4array[0] = IKcos(j4array[0]);
841 j4array[1] = ((3.14159265358979) + x92 + x90);
842 sj4array[1] = IKsin(j4array[1]);
843 cj4array[1] = IKcos(j4array[1]);
844 if (j4array[0] > IKPI)
845 {
846 j4array[0] -= IK2PI;
847 }
848 else if (j4array[0] < -IKPI)
849 {
850 j4array[0] += IK2PI;
851 }
852 j4valid[0] = true;
853 if (j4array[1] > IKPI)
854 {
855 j4array[1] -= IK2PI;
856 }
857 else if (j4array[1] < -IKPI)
858 {
859 j4array[1] += IK2PI;
860 }
861 j4valid[1] = true;
862 for (int ij4 = 0; ij4 < 2; ++ij4)
863 {
864 if (!j4valid[ij4])
865 {
866 continue;
867 }
868 _ij4[0] = ij4;
869 _ij4[1] = -1;
870 for (int iij4 = ij4 + 1; iij4 < 2; ++iij4)
871 {
872 if (j4valid[iij4]
873 && IKabs(cj4array[ij4] - cj4array[iij4])
874 < IKFAST_SOLUTION_THRESH
875 && IKabs(sj4array[ij4] - sj4array[iij4])
876 < IKFAST_SOLUTION_THRESH)
877 {
878 j4valid[iij4] = false;
879 _ij4[1] = iij4;
880 break;
881 }
882 }
883 j4 = j4array[ij4];
884 cj4 = cj4array[ij4];
885 sj4 = sj4array[ij4];
886 {
887 IkReal evalcond[2];
888 IkReal x95 = ((0.045) * sj8);
889 IkReal x96 = (cj9 * x95);
890 IkReal x97 = ((0.3) * sj8 * sj9);
891 IkReal x98 = IKcos(j4);
892 IkReal x99 = cj8 * cj8;
893 IkReal x100 = (px * py);
894 IkReal x101 = sj8 * sj8;
895 IkReal x102 = IKsin(j4);
896 IkReal x103 = ((1.0) * (px * px));
897 evalcond[0]
898 = ((((-1.0) * px * x95)) + ((px * x96))
899 + ((x98
900 * ((((x100 * x101)) + ((x100 * x99))))))
901 + (((-1.0) * px * x97))
902 + ((x102
903 * (((((-1.0) * x101 * x103))
904 + (((-1.0) * x103 * x99)))))));
905 evalcond[1]
906 = (x95 + x97 + ((px * x102)) + (((-1.0) * x96))
907 + (((-1.0) * py * x98)));
908 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
909 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH)
910 {
911 continue;
912 }
913 }
914
915 {
916 IkReal j6eval[2];
917 IkReal x104 = (cj4 * px);
918 IkReal x105 = (cj8 * pz);
919 IkReal x106 = (py * sj4);
920 IkReal x107 = (cj4 * cj9 * px);
921 IkReal x108 = (cj4 * px * sj9);
922 IkReal x109 = (cj8 * pz * sj9);
923 IkReal x110 = (cj9 * py * sj4);
924 IkReal x111 = (py * sj4 * sj9);
925 IkReal x112 = ((0.045) * x105);
926 j6eval[0]
927 = ((((-1.0) * x111)) + (((-1.0) * x108))
928 + (((-12.2222222222222) * x106))
929 + (((-6.66666666666667) * x109))
930 + ((cj9 * x105))
931 + (((-12.2222222222222) * x104))
932 + (((-1.0) * x105))
933 + (((-6.66666666666667) * x110))
934 + (((-6.66666666666667) * x107)));
935 j6eval[1] = IKsign(
936 ((((-1.0) * x112)) + (((-0.045) * x108))
937 + (((-0.55) * x106)) + (((-0.55) * x104))
938 + ((cj9 * x112)) + (((-0.3) * x107))
939 + (((-0.045) * x111)) + (((-0.3) * x110))
940 + (((-0.3) * x109))));
941 if (IKabs(j6eval[0]) < 0.0000010000000000
942 || IKabs(j6eval[1]) < 0.0000010000000000)
943 {
944 {
945 IkReal j6eval[2];
946 IkReal x113 = (sj8 * (py * py));
947 IkReal x114 = cj4 * cj4;
948 IkReal x115
949 = (((sj8 * x114 * (px * px)))
950 + ((sj8 * (pz * pz)))
951 + (((-1.0) * x113 * x114))
952 + (((2.0) * cj4 * px * py * sj4 * sj8))
953 + x113);
954 j6eval[0] = x115;
955 j6eval[1] = IKsign(x115);
956 if (IKabs(j6eval[0]) < 0.0000010000000000
957 || IKabs(j6eval[1]) < 0.0000010000000000)
958 {
959 {
960 IkReal j6eval[2];
961 IkReal x116 = (pz * sj8);
962 IkReal x117 = (cj9 * pz * sj8);
963 IkReal x118 = (pz * sj8 * sj9);
964 IkReal x119 = (cj8 * sj8);
965 IkReal x120 = (cj4 * px * x119);
966 IkReal x121 = (py * sj4 * x119);
967 IkReal x122 = ((1.0) * cj9);
968 IkReal x123 = (cj4 * cj8 * px * sj8 * sj9);
969 IkReal x124 = (cj8 * py * sj4 * sj8 * sj9);
970 IkReal x125 = ((0.045) * x120);
971 IkReal x126 = ((0.045) * x121);
972 j6eval[0]
973 = ((((6.66666666666667) * x123))
974 + (((-1.0) * x121 * x122)) + x120
975 + x121
976 + (((-12.2222222222222) * x116))
977 + (((-1.0) * x120 * x122))
978 + (((6.66666666666667) * x124))
979 + (((-6.66666666666667) * x117))
980 + (((-1.0) * x118)));
981 j6eval[1] = IKsign((
982 (((-1.0) * cj9 * x126)) + x125 + x126
983 + (((-0.55) * x116)) + (((0.3) * x123))
984 + (((0.3) * x124)) + (((-0.045) * x118))
985 + (((-0.3) * x117))
986 + (((-1.0) * cj9 * x125))));
987 if (IKabs(j6eval[0]) < 0.0000010000000000
988 || IKabs(j6eval[1])
989 < 0.0000010000000000)
990 {
991 {
992 IkReal evalcond[4];
993 bool bgotonextstatement = true;
994 do
995 {
996 IkReal x127
997 = (((px * sj4))
998 + (((-1.0) * (1.0) * cj4
999 * py)));
1000 evalcond[0]
1001 = ((-3.14159265358979)
1002 + (IKfmod(
1003 ((3.14159265358979)
1004 + (IKabs(j8))),
1005 6.28318530717959)));
1006 evalcond[1]
1007 = ((0.39655) + (((0.0765) * sj9))
1008 + (((0.32595) * cj9))
1009 + (((-1.0) * (1.0) * pp)));
1010 evalcond[2] = x127;
1011 evalcond[3] = x127;
1012 if (IKabs(evalcond[0])
1013 < 0.0000010000000000
1014 && IKabs(evalcond[1])
1015 < 0.0000010000000000
1016 && IKabs(evalcond[2])
1017 < 0.0000010000000000
1018 && IKabs(evalcond[3])
1019 < 0.0000010000000000)
1020 {
1021 bgotonextstatement = false;
1022 {
1023 IkReal j6eval[2];
1024 sj8 = 0;
1025 cj8 = 1.0;
1026 j8 = 0;
1027 IkReal x128 = (cj9 * pz);
1028 IkReal x129 = (cj4 * px);
1029 IkReal x130 = (pp * pz);
1030 IkReal x131 = (py * sj4);
1031 IkReal x132 = (cj4 * cj9 * px);
1032 IkReal x133 = (cj4 * pp * px);
1033 IkReal x134 = (cj9 * py * sj4);
1034 IkReal x135 = (pp * py * sj4);
1035 j6eval[0]
1036 = (x128
1037 + (((5.4333061668025)
1038 * x130))
1039 + (((13.9482024812098)
1040 * x129))
1041 + (((12.2222222222222)
1042 * x134))
1043 + (((2.92556370551481)
1044 * pz))
1045 + (((12.2222222222222)
1046 * x132))
1047 + (((-36.2220411120167)
1048 * x135))
1049 + (((13.9482024812098)
1050 * x131))
1051 + (((-36.2220411120167)
1052 * x133)));
1053 j6eval[1] = IKsign((
1054 (((1.32323529411765) * x132))
1055 + (((0.316735294117647) * pz))
1056 + (((0.108264705882353)
1057 * x128))
1058 + (((-3.92156862745098)
1059 * x135))
1060 + (((1.32323529411765)
1061 * x134))
1062 + (((1.51009803921569)
1063 * x131))
1064 + (((1.51009803921569)
1065 * x129))
1066 + (((0.588235294117647)
1067 * x130))
1068 + (((-3.92156862745098)
1069 * x133))));
1070 if (IKabs(j6eval[0])
1071 < 0.0000010000000000
1072 || IKabs(j6eval[1])
1073 < 0.0000010000000000)
1074 {
1075 {
1076 IkReal j6eval[2];
1077 sj8 = 0;
1078 cj8 = 1.0;
1079 j8 = 0;
1080 IkReal x136 = (cj4 * px);
1081 IkReal x137 = (py * sj4);
1082 IkReal x138 = (cj9 * pz);
1083 IkReal x139 = (pz * sj9);
1084 IkReal x140 = ((1.0) * cj9);
1085 IkReal x141
1086 = (cj4 * px * sj9);
1087 IkReal x142
1088 = (py * sj4 * sj9);
1089 IkReal x143
1090 = ((0.045) * x136);
1091 IkReal x144
1092 = ((0.045) * x137);
1093 j6eval[0]
1094 = (x137 + x136
1095 + (((-1.0)
1096 * (12.2222222222222)
1097 * pz))
1098 + (((6.66666666666667)
1099 * x141))
1100 + (((-1.0) * x136
1101 * x140))
1102 + (((6.66666666666667)
1103 * x142))
1104 + (((-6.66666666666667)
1105 * x138))
1106 + (((-1.0) * x139))
1107 + (((-1.0) * x137
1108 * x140)));
1109 j6eval[1] = IKsign((
1110 (((-0.3) * x138))
1111 + (((-1.0) * (0.55) * pz))
1112 + (((0.3) * x142)) + x143
1113 + x144 + (((0.3) * x141))
1114 + (((-0.045) * x139))
1115 + (((-1.0) * cj9 * x143))
1116 + (((-1.0) * cj9
1117 * x144))));
1118 if (IKabs(j6eval[0])
1119 < 0.0000010000000000
1120 || IKabs(j6eval[1])
1121 < 0.0000010000000000)
1122 {
1123 {
1124 IkReal j6eval[2];
1125 sj8 = 0;
1126 cj8 = 1.0;
1127 j8 = 0;
1128 IkReal x145 = (cj4 * px);
1129 IkReal x146 = (cj9 * pz);
1130 IkReal x147 = (pp * pz);
1131 IkReal x148 = (py * sj4);
1132 IkReal x149
1133 = (cj4 * cj9 * px);
1134 IkReal x150
1135 = (cj4 * pp * px);
1136 IkReal x151
1137 = (cj9 * py * sj4);
1138 IkReal x152
1139 = (pp * py * sj4);
1140 j6eval[0]
1141 = ((((-1.0) * x151))
1142 + (((-5.4333061668025)
1143 * x150))
1144 + (((-1.0) * x149))
1145 + (((-5.4333061668025)
1146 * x152))
1147 + (((12.2222222222222)
1148 * x146))
1149 + (((13.9482024812098)
1150 * pz))
1151 + (((-2.92556370551481)
1152 * x148))
1153 + (((-2.92556370551481)
1154 * x145))
1155 + (((-36.2220411120167)
1156 * x147)));
1157 j6eval[1] = IKsign((
1158 (((-0.108264705882353)
1159 * x151))
1160 + (((1.51009803921569)
1161 * pz))
1162 + (((-0.588235294117647)
1163 * x152))
1164 + (((-0.316735294117647)
1165 * x148))
1166 + (((-0.588235294117647)
1167 * x150))
1168 + (((-0.108264705882353)
1169 * x149))
1170 + (((1.32323529411765)
1171 * x146))
1172 + (((-3.92156862745098)
1173 * x147))
1174 + (((-0.316735294117647)
1175 * x145))));
1176 if (IKabs(j6eval[0])
1177 < 0.0000010000000000
1178 || IKabs(j6eval[1])
1179 < 0.0000010000000000)
1180 {
1181 {
1182 IkReal evalcond[1];
1183 bool
1184 bgotonextstatement
1185 = true;
1186 do
1187 {
1188 evalcond[0]
1189 = ((IKabs((
1190 (-3.14159265358979)
1191 + (IKfmod(
1192 ((3.14159265358979)
1193 + j9),
1194 6.28318530717959)))))
1195 + (IKabs(
1196 pz)));
1197 if (IKabs(
1198 evalcond[0])
1199 < 0.0000010000000000)
1200 {
1201 bgotonextstatement
1202 = false;
1203 {
1204 IkReal
1205 j6eval[1];
1206 IkReal x153
1207 = ((1.0)
1208 * py);
1209 sj8 = 0;
1210 cj8 = 1.0;
1211 j8 = 0;
1212 pz = 0;
1213 j9 = 0;
1214 sj9 = 0;
1215 cj9 = 1.0;
1216 pp
1217 = ((py * py)
1218 + (px
1219 * px));
1220 npx
1221 = (((py
1222 * r10))
1223 + ((px
1224 * r00)));
1225 npy
1226 = (((py
1227 * r11))
1228 + ((px
1229 * r01)));
1230 npz
1231 = (((px
1232 * r02))
1233 + ((py
1234 * r12)));
1235 rxp0_0
1236 = ((-1.0)
1237 * r20
1238 * x153);
1239 rxp0_1
1240 = (px
1241 * r20);
1242 rxp1_0
1243 = ((-1.0)
1244 * r21
1245 * x153);
1246 rxp1_1
1247 = (px
1248 * r21);
1249 rxp2_0
1250 = ((-1.0)
1251 * r22
1252 * x153);
1253 rxp2_1
1254 = (px
1255 * r22);
1256 j6eval[0]
1257 = (((py
1258 * sj4))
1259 + ((cj4
1260 * px)));
1261 if (IKabs(
1262 j6eval
1263 [0])
1264 < 0.0000010000000000)
1265 {
1266 {
1267 IkReal
1268 j6eval
1269 [1];
1270 IkReal x154
1271 = ((1.0)
1272 * py);
1273 sj8 = 0;
1274 cj8 = 1.0;
1275 j8 = 0;
1276 pz = 0;
1277 j9 = 0;
1278 sj9 = 0;
1279 cj9 = 1.0;
1280 pp
1281 = ((py
1282 * py)
1283 + (px
1284 * px));
1285 npx
1286 = (((py
1287 * r10))
1288 + ((px
1289 * r00)));
1290 npy
1291 = (((py
1292 * r11))
1293 + ((px
1294 * r01)));
1295 npz
1296 = (((px
1297 * r02))
1298 + ((py
1299 * r12)));
1300 rxp0_0
1301 = ((-1.0)
1302 * r20
1303 * x154);
1304 rxp0_1
1305 = (px
1306 * r20);
1307 rxp1_0
1308 = ((-1.0)
1309 * r21
1310 * x154);
1311 rxp1_1
1312 = (px
1313 * r21);
1314 rxp2_0
1315 = ((-1.0)
1316 * r22
1317 * x154);
1318 rxp2_1
1319 = (px
1320 * r22);
1321 j6eval[0]
1322 = ((-1.0)
1323 + (((-1.0)
1324 * (1.3840830449827)
1325 * (px
1326 * px)))
1327 + (((-1.0)
1328 * (1.3840830449827)
1329 * (py
1330 * py))));
1331 if (IKabs(
1332 j6eval
1333 [0])
1334 < 0.0000010000000000)
1335 {
1336 {
1337 IkReal j6eval
1338 [2];
1339 IkReal
1340 x155
1341 = ((1.0)
1342 * py);
1343 sj8 = 0;
1344 cj8 = 1.0;
1345 j8 = 0;
1346 pz = 0;
1347 j9 = 0;
1348 sj9 = 0;
1349 cj9 = 1.0;
1350 pp
1351 = ((py
1352 * py)
1353 + (px
1354 * px));
1355 npx
1356 = (((py
1357 * r10))
1358 + ((px
1359 * r00)));
1360 npy
1361 = (((py
1362 * r11))
1363 + ((px
1364 * r01)));
1365 npz
1366 = (((px
1367 * r02))
1368 + ((py
1369 * r12)));
1370 rxp0_0
1371 = ((-1.0)
1372 * r20
1373 * x155);
1374 rxp0_1
1375 = (px
1376 * r20);
1377 rxp1_0
1378 = ((-1.0)
1379 * r21
1380 * x155);
1381 rxp1_1
1382 = (px
1383 * r21);
1384 rxp2_0
1385 = ((-1.0)
1386 * r22
1387 * x155);
1388 rxp2_1
1389 = (px
1390 * r22);
1391 IkReal
1392 x156
1393 = (cj4
1394 * px);
1395 IkReal
1396 x157
1397 = (py
1398 * sj4);
1399 j6eval
1400 [0]
1401 = (x156
1402 + x157);
1403 j6eval
1404 [1]
1405 = ((((-1.0)
1406 * (1.3840830449827)
1407 * cj4
1408 * (px
1409 * px
1410 * px)))
1411 + (((-1.0)
1412 * x157))
1413 + (((-1.3840830449827)
1414 * x157
1415 * (px
1416 * px)))
1417 + (((-1.3840830449827)
1418 * x156
1419 * (py
1420 * py)))
1421 + (((-1.0)
1422 * x156))
1423 + (((-1.0)
1424 * (1.3840830449827)
1425 * sj4
1426 * (py
1427 * py
1428 * py))));
1429 if (IKabs(
1430 j6eval
1431 [0])
1432 < 0.0000010000000000
1433 || IKabs(
1434 j6eval
1435 [1])
1436 < 0.0000010000000000)
1437 {
1438 {
1439 IkReal evalcond
1440 [4];
1441 bool
1442 bgotonextstatement
1443 = true;
1444 do
1445 {
1446 evalcond
1447 [0]
1448 = ((IKabs(
1449 py))
1450 + (IKabs(
1451 px)));
1452 evalcond
1453 [1]
1454 = -0.85;
1455 evalcond
1456 [2]
1457 = 0;
1458 evalcond
1459 [3]
1460 = -0.2125;
1461 if (IKabs(
1462 evalcond
1463 [0])
1464 < 0.0000010000000000
1465 && IKabs(
1466 evalcond
1467 [1])
1468 < 0.0000010000000000
1469 && IKabs(
1470 evalcond
1471 [2])
1472 < 0.0000010000000000
1473 && IKabs(
1474 evalcond
1475 [3])
1476 < 0.0000010000000000)
1477 {
1478 bgotonextstatement
1479 = false;
1480 {
1481 IkReal j6array
1482 [2],
1483 cj6array
1484 [2],
1485 sj6array
1486 [2];
1487 bool j6valid
1488 [2]
1489 = {false};
1490 _nj6
1491 = 2;
1492 j6array
1493 [0]
1494 = 2.9927027059803;
1495 sj6array
1496 [0]
1497 = IKsin(
1498 j6array
1499 [0]);
1500 cj6array
1501 [0]
1502 = IKcos(
1503 j6array
1504 [0]);
1505 j6array
1506 [1]
1507 = 6.13429535957009;
1508 sj6array
1509 [1]
1510 = IKsin(
1511 j6array
1512 [1]);
1513 cj6array
1514 [1]
1515 = IKcos(
1516 j6array
1517 [1]);
1518 if (j6array
1519 [0]
1520 > IKPI)
1521 {
1522 j6array
1523 [0]
1524 -= IK2PI;
1525 }
1526 else if (
1527 j6array
1528 [0]
1529 < -IKPI)
1530 {
1531 j6array
1532 [0]
1533 += IK2PI;
1534 }
1535 j6valid
1536 [0]
1537 = true;
1538 if (j6array
1539 [1]
1540 > IKPI)
1541 {
1542 j6array
1543 [1]
1544 -= IK2PI;
1545 }
1546 else if (
1547 j6array
1548 [1]
1549 < -IKPI)
1550 {
1551 j6array
1552 [1]
1553 += IK2PI;
1554 }
1555 j6valid
1556 [1]
1557 = true;
1558 for (
1559 int ij6
1560 = 0;
1561 ij6
1562 < 2;
1563 ++ij6)
1564 {
1565 if (!j6valid
1566 [ij6])
1567 {
1568 continue;
1569 }
1570 _ij6[0]
1571 = ij6;
1572 _ij6[1]
1573 = -1;
1574 for (
1575 int iij6
1576 = ij6
1577 + 1;
1578 iij6
1579 < 2;
1580 ++iij6)
1581 {
1582 if (j6valid
1583 [iij6]
1584 && IKabs(
1585 cj6array
1586 [ij6]
1587 - cj6array
1588 [iij6])
1589 < IKFAST_SOLUTION_THRESH
1590 && IKabs(
1591 sj6array
1592 [ij6]
1593 - sj6array
1594 [iij6])
1595 < IKFAST_SOLUTION_THRESH)
1596 {
1597 j6valid
1598 [iij6]
1599 = false;
1600 _ij6[1]
1601 = iij6;
1602 break;
1603 }
1604 }
1605 j6 = j6array
1606 [ij6];
1607 cj6 = cj6array
1608 [ij6];
1609 sj6 = sj6array
1610 [ij6];
1611 {
1612 IkReal evalcond
1613 [1];
1614 evalcond
1615 [0]
1616 = ((0.85)
1617 * (IKsin(
1618 j6)));
1619 if (IKabs(
1620 evalcond
1621 [0])
1622 > IKFAST_EVALCOND_THRESH)
1623 {
1624 continue;
1625 }
1626 }
1627
1628 rotationfunction0(
1629 solutions);
1630 }
1631 }
1632 }
1633 } while (
1634 0);
1635 if (bgotonextstatement)
1636 {
1637 bool
1638 bgotonextstatement
1639 = true;
1640 do
1641 {
1642 evalcond
1643 [0]
1644 = ((IKabs((
1645 (-3.14159265358979)
1646 + (IKfmod(
1647 ((3.14159265358979)
1648 + j4),
1649 6.28318530717959)))))
1650 + (IKabs(
1651 px)));
1652 evalcond
1653 [1]
1654 = -0.85;
1655 evalcond
1656 [2]
1657 = 0;
1658 evalcond
1659 [3]
1660 = ((-0.2125)
1661 + (((-1.0)
1662 * (1.0)
1663 * (py
1664 * py))));
1665 if (IKabs(
1666 evalcond
1667 [0])
1668 < 0.0000010000000000
1669 && IKabs(
1670 evalcond
1671 [1])
1672 < 0.0000010000000000
1673 && IKabs(
1674 evalcond
1675 [2])
1676 < 0.0000010000000000
1677 && IKabs(
1678 evalcond
1679 [3])
1680 < 0.0000010000000000)
1681 {
1682 bgotonextstatement
1683 = false;
1684 {
1685 IkReal j6eval
1686 [1];
1687 IkReal
1688 x606
1689 = ((1.0)
1690 * py);
1691 sj8 = 0;
1692 cj8 = 1.0;
1693 j8 = 0;
1694 pz = 0;
1695 j9 = 0;
1696 sj9 = 0;
1697 cj9 = 1.0;
1698 pp = py
1699 * py;
1700 npx
1701 = (py
1702 * r10);
1703 npy
1704 = (py
1705 * r11);
1706 npz
1707 = (py
1708 * r12);
1709 rxp0_0
1710 = ((-1.0)
1711 * r20
1712 * x606);
1713 rxp0_1
1714 = 0;
1715 rxp1_0
1716 = ((-1.0)
1717 * r21
1718 * x606);
1719 rxp1_1
1720 = 0;
1721 rxp2_0
1722 = ((-1.0)
1723 * r22
1724 * x606);
1725 rxp2_1
1726 = 0;
1727 px = 0;
1728 j4 = 0;
1729 sj4 = 0;
1730 cj4 = 1.0;
1731 rxp0_2
1732 = (py
1733 * r00);
1734 rxp1_2
1735 = (py
1736 * r01);
1737 rxp2_2
1738 = (py
1739 * r02);
1740 j6eval
1741 [0]
1742 = ((1.0)
1743 + (((1.91568587540858)
1744 * (py
1745 * py
1746 * py
1747 * py)))
1748 + (((-1.0)
1749 * (2.64633970947792)
1750 * (py
1751 * py))));
1752 if (IKabs(
1753 j6eval
1754 [0])
1755 < 0.0000010000000000)
1756 {
1757 continue; // no branches [j6]
1758 }
1759 else
1760 {
1761 {
1762 IkReal j6array
1763 [2],
1764 cj6array
1765 [2],
1766 sj6array
1767 [2];
1768 bool j6valid
1769 [2]
1770 = {false};
1771 _nj6
1772 = 2;
1773 IkReal
1774 x607
1775 = py
1776 * py;
1777 CheckValue<IkReal> x609 = IKatan2WithCheck(
1778 IkReal((
1779 (-0.425)
1780 + (((-0.588235294117647)
1781 * x607)))),
1782 ((-2.83333333333333)
1783 + (((3.92156862745098)
1784 * x607))),
1785 IKFAST_ATAN2_MAGTHRESH);
1786 if (!x609.valid)
1787 {
1788 continue;
1789 }
1790 IkReal
1791 x608
1792 = ((-1.0)
1793 * (x609.value));
1794 j6array
1795 [0]
1796 = x608;
1797 sj6array
1798 [0]
1799 = IKsin(
1800 j6array
1801 [0]);
1802 cj6array
1803 [0]
1804 = IKcos(
1805 j6array
1806 [0]);
1807 j6array
1808 [1]
1809 = ((3.14159265358979)
1810 + x608);
1811 sj6array
1812 [1]
1813 = IKsin(
1814 j6array
1815 [1]);
1816 cj6array
1817 [1]
1818 = IKcos(
1819 j6array
1820 [1]);
1821 if (j6array
1822 [0]
1823 > IKPI)
1824 {
1825 j6array
1826 [0]
1827 -= IK2PI;
1828 }
1829 else if (
1830 j6array
1831 [0]
1832 < -IKPI)
1833 {
1834 j6array
1835 [0]
1836 += IK2PI;
1837 }
1838 j6valid
1839 [0]
1840 = true;
1841 if (j6array
1842 [1]
1843 > IKPI)
1844 {
1845 j6array
1846 [1]
1847 -= IK2PI;
1848 }
1849 else if (
1850 j6array
1851 [1]
1852 < -IKPI)
1853 {
1854 j6array
1855 [1]
1856 += IK2PI;
1857 }
1858 j6valid
1859 [1]
1860 = true;
1861 for (
1862 int ij6
1863 = 0;
1864 ij6
1865 < 2;
1866 ++ij6)
1867 {
1868 if (!j6valid
1869 [ij6])
1870 {
1871 continue;
1872 }
1873 _ij6[0]
1874 = ij6;
1875 _ij6[1]
1876 = -1;
1877 for (
1878 int iij6
1879 = ij6
1880 + 1;
1881 iij6
1882 < 2;
1883 ++iij6)
1884 {
1885 if (j6valid
1886 [iij6]
1887 && IKabs(
1888 cj6array
1889 [ij6]
1890 - cj6array
1891 [iij6])
1892 < IKFAST_SOLUTION_THRESH
1893 && IKabs(
1894 sj6array
1895 [ij6]
1896 - sj6array
1897 [iij6])
1898 < IKFAST_SOLUTION_THRESH)
1899 {
1900 j6valid
1901 [iij6]
1902 = false;
1903 _ij6[1]
1904 = iij6;
1905 break;
1906 }
1907 }
1908 j6 = j6array
1909 [ij6];
1910 cj6 = cj6array
1911 [ij6];
1912 sj6 = sj6array
1913 [ij6];
1914 {
1915 IkReal evalcond
1916 [1];
1917 evalcond
1918 [0]
1919 = ((0.85)
1920 * (IKsin(
1921 j6)));
1922 if (IKabs(
1923 evalcond
1924 [0])
1925 > IKFAST_EVALCOND_THRESH)
1926 {
1927 continue;
1928 }
1929 }
1930
1931 rotationfunction0(
1932 solutions);
1933 }
1934 }
1935 }
1936 }
1937 }
1938 } while (
1939 0);
1940 if (bgotonextstatement)
1941 {
1942 bool
1943 bgotonextstatement
1944 = true;
1945 do
1946 {
1947 evalcond
1948 [0]
1949 = ((IKabs((
1950 (-3.14159265358979)
1951 + (IKfmod(
1952 j4,
1953 6.28318530717959)))))
1954 + (IKabs(
1955 px)));
1956 evalcond
1957 [1]
1958 = -0.85;
1959 evalcond
1960 [2]
1961 = 0;
1962 evalcond
1963 [3]
1964 = ((-0.2125)
1965 + (((-1.0)
1966 * (1.0)
1967 * (py
1968 * py))));
1969 if (IKabs(
1970 evalcond
1971 [0])
1972 < 0.0000010000000000
1973 && IKabs(
1974 evalcond
1975 [1])
1976 < 0.0000010000000000
1977 && IKabs(
1978 evalcond
1979 [2])
1980 < 0.0000010000000000
1981 && IKabs(
1982 evalcond
1983 [3])
1984 < 0.0000010000000000)
1985 {
1986 bgotonextstatement
1987 = false;
1988 {
1989 IkReal j6eval
1990 [1];
1991 IkReal
1992 x610
1993 = ((1.0)
1994 * py);
1995 sj8 = 0;
1996 cj8 = 1.0;
1997 j8 = 0;
1998 pz = 0;
1999 j9 = 0;
2000 sj9 = 0;
2001 cj9 = 1.0;
2002 pp = py
2003 * py;
2004 npx
2005 = (py
2006 * r10);
2007 npy
2008 = (py
2009 * r11);
2010 npz
2011 = (py
2012 * r12);
2013 rxp0_0
2014 = ((-1.0)
2015 * r20
2016 * x610);
2017 rxp0_1
2018 = 0;
2019 rxp1_0
2020 = ((-1.0)
2021 * r21
2022 * x610);
2023 rxp1_1
2024 = 0;
2025 rxp2_0
2026 = ((-1.0)
2027 * r22
2028 * x610);
2029 rxp2_1
2030 = 0;
2031 px = 0;
2032 j4 = 3.14159265358979;
2033 sj4 = 0;
2034 cj4 = -1.0;
2035 rxp0_2
2036 = (py
2037 * r00);
2038 rxp1_2
2039 = (py
2040 * r01);
2041 rxp2_2
2042 = (py
2043 * r02);
2044 j6eval
2045 [0]
2046 = ((1.0)
2047 + (((1.91568587540858)
2048 * (py
2049 * py
2050 * py
2051 * py)))
2052 + (((-1.0)
2053 * (2.64633970947792)
2054 * (py
2055 * py))));
2056 if (IKabs(
2057 j6eval
2058 [0])
2059 < 0.0000010000000000)
2060 {
2061 continue; // no branches [j6]
2062 }
2063 else
2064 {
2065 {
2066 IkReal j6array
2067 [2],
2068 cj6array
2069 [2],
2070 sj6array
2071 [2];
2072 bool j6valid
2073 [2]
2074 = {false};
2075 _nj6
2076 = 2;
2077 IkReal
2078 x611
2079 = py
2080 * py;
2081 CheckValue<IkReal> x613 = IKatan2WithCheck(
2082 IkReal((
2083 (-0.425)
2084 + (((-0.588235294117647)
2085 * x611)))),
2086 ((-2.83333333333333)
2087 + (((3.92156862745098)
2088 * x611))),
2089 IKFAST_ATAN2_MAGTHRESH);
2090 if (!x613.valid)
2091 {
2092 continue;
2093 }
2094 IkReal
2095 x612
2096 = ((-1.0)
2097 * (x613.value));
2098 j6array
2099 [0]
2100 = x612;
2101 sj6array
2102 [0]
2103 = IKsin(
2104 j6array
2105 [0]);
2106 cj6array
2107 [0]
2108 = IKcos(
2109 j6array
2110 [0]);
2111 j6array
2112 [1]
2113 = ((3.14159265358979)
2114 + x612);
2115 sj6array
2116 [1]
2117 = IKsin(
2118 j6array
2119 [1]);
2120 cj6array
2121 [1]
2122 = IKcos(
2123 j6array
2124 [1]);
2125 if (j6array
2126 [0]
2127 > IKPI)
2128 {
2129 j6array
2130 [0]
2131 -= IK2PI;
2132 }
2133 else if (
2134 j6array
2135 [0]
2136 < -IKPI)
2137 {
2138 j6array
2139 [0]
2140 += IK2PI;
2141 }
2142 j6valid
2143 [0]
2144 = true;
2145 if (j6array
2146 [1]
2147 > IKPI)
2148 {
2149 j6array
2150 [1]
2151 -= IK2PI;
2152 }
2153 else if (
2154 j6array
2155 [1]
2156 < -IKPI)
2157 {
2158 j6array
2159 [1]
2160 += IK2PI;
2161 }
2162 j6valid
2163 [1]
2164 = true;
2165 for (
2166 int ij6
2167 = 0;
2168 ij6
2169 < 2;
2170 ++ij6)
2171 {
2172 if (!j6valid
2173 [ij6])
2174 {
2175 continue;
2176 }
2177 _ij6[0]
2178 = ij6;
2179 _ij6[1]
2180 = -1;
2181 for (
2182 int iij6
2183 = ij6
2184 + 1;
2185 iij6
2186 < 2;
2187 ++iij6)
2188 {
2189 if (j6valid
2190 [iij6]
2191 && IKabs(
2192 cj6array
2193 [ij6]
2194 - cj6array
2195 [iij6])
2196 < IKFAST_SOLUTION_THRESH
2197 && IKabs(
2198 sj6array
2199 [ij6]
2200 - sj6array
2201 [iij6])
2202 < IKFAST_SOLUTION_THRESH)
2203 {
2204 j6valid
2205 [iij6]
2206 = false;
2207 _ij6[1]
2208 = iij6;
2209 break;
2210 }
2211 }
2212 j6 = j6array
2213 [ij6];
2214 cj6 = cj6array
2215 [ij6];
2216 sj6 = sj6array
2217 [ij6];
2218 {
2219 IkReal evalcond
2220 [1];
2221 evalcond
2222 [0]
2223 = ((0.85)
2224 * (IKsin(
2225 j6)));
2226 if (IKabs(
2227 evalcond
2228 [0])
2229 > IKFAST_EVALCOND_THRESH)
2230 {
2231 continue;
2232 }
2233 }
2234
2235 rotationfunction0(
2236 solutions);
2237 }
2238 }
2239 }
2240 }
2241 }
2242 } while (
2243 0);
2244 if (bgotonextstatement)
2245 {
2246 bool
2247 bgotonextstatement
2248 = true;
2249 do
2250 {
2251 evalcond
2252 [0]
2253 = ((IKabs((
2254 (-3.14159265358979)
2255 + (IKfmod(
2256 ((1.5707963267949)
2257 + j4),
2258 6.28318530717959)))))
2259 + (IKabs(
2260 py)));
2261 evalcond
2262 [1]
2263 = -0.85;
2264 evalcond
2265 [2]
2266 = 0;
2267 evalcond
2268 [3]
2269 = ((-0.2125)
2270 + (((-1.0)
2271 * (1.0)
2272 * (px
2273 * px))));
2274 if (IKabs(
2275 evalcond
2276 [0])
2277 < 0.0000010000000000
2278 && IKabs(
2279 evalcond
2280 [1])
2281 < 0.0000010000000000
2282 && IKabs(
2283 evalcond
2284 [2])
2285 < 0.0000010000000000
2286 && IKabs(
2287 evalcond
2288 [3])
2289 < 0.0000010000000000)
2290 {
2291 bgotonextstatement
2292 = false;
2293 {
2294 IkReal j6eval
2295 [1];
2296 IkReal
2297 x614
2298 = ((1.0)
2299 * px);
2300 sj8 = 0;
2301 cj8 = 1.0;
2302 j8 = 0;
2303 pz = 0;
2304 j9 = 0;
2305 sj9 = 0;
2306 cj9 = 1.0;
2307 pp = px
2308 * px;
2309 npx
2310 = (px
2311 * r00);
2312 npy
2313 = (px
2314 * r01);
2315 npz
2316 = (px
2317 * r02);
2318 rxp0_0
2319 = 0;
2320 rxp0_1
2321 = (px
2322 * r20);
2323 rxp1_0
2324 = 0;
2325 rxp1_1
2326 = (px
2327 * r21);
2328 rxp2_0
2329 = 0;
2330 rxp2_1
2331 = (px
2332 * r22);
2333 py = 0;
2334 j4 = 1.5707963267949;
2335 sj4 = 1.0;
2336 cj4 = 0;
2337 rxp0_2
2338 = ((-1.0)
2339 * r10
2340 * x614);
2341 rxp1_2
2342 = ((-1.0)
2343 * r11
2344 * x614);
2345 rxp2_2
2346 = ((-1.0)
2347 * r12
2348 * x614);
2349 j6eval
2350 [0]
2351 = ((1.0)
2352 + (((1.91568587540858)
2353 * (px
2354 * px
2355 * px
2356 * px)))
2357 + (((-1.0)
2358 * (2.64633970947792)
2359 * (px
2360 * px))));
2361 if (IKabs(
2362 j6eval
2363 [0])
2364 < 0.0000010000000000)
2365 {
2366 continue; // no branches [j6]
2367 }
2368 else
2369 {
2370 {
2371 IkReal j6array
2372 [2],
2373 cj6array
2374 [2],
2375 sj6array
2376 [2];
2377 bool j6valid
2378 [2]
2379 = {false};
2380 _nj6
2381 = 2;
2382 IkReal
2383 x615
2384 = px
2385 * px;
2386 CheckValue<IkReal> x617 = IKatan2WithCheck(
2387 IkReal((
2388 (-0.425)
2389 + (((-0.588235294117647)
2390 * x615)))),
2391 ((-2.83333333333333)
2392 + (((3.92156862745098)
2393 * x615))),
2394 IKFAST_ATAN2_MAGTHRESH);
2395 if (!x617.valid)
2396 {
2397 continue;
2398 }
2399 IkReal
2400 x616
2401 = ((-1.0)
2402 * (x617.value));
2403 j6array
2404 [0]
2405 = x616;
2406 sj6array
2407 [0]
2408 = IKsin(
2409 j6array
2410 [0]);
2411 cj6array
2412 [0]
2413 = IKcos(
2414 j6array
2415 [0]);
2416 j6array
2417 [1]
2418 = ((3.14159265358979)
2419 + x616);
2420 sj6array
2421 [1]
2422 = IKsin(
2423 j6array
2424 [1]);
2425 cj6array
2426 [1]
2427 = IKcos(
2428 j6array
2429 [1]);
2430 if (j6array
2431 [0]
2432 > IKPI)
2433 {
2434 j6array
2435 [0]
2436 -= IK2PI;
2437 }
2438 else if (
2439 j6array
2440 [0]
2441 < -IKPI)
2442 {
2443 j6array
2444 [0]
2445 += IK2PI;
2446 }
2447 j6valid
2448 [0]
2449 = true;
2450 if (j6array
2451 [1]
2452 > IKPI)
2453 {
2454 j6array
2455 [1]
2456 -= IK2PI;
2457 }
2458 else if (
2459 j6array
2460 [1]
2461 < -IKPI)
2462 {
2463 j6array
2464 [1]
2465 += IK2PI;
2466 }
2467 j6valid
2468 [1]
2469 = true;
2470 for (
2471 int ij6
2472 = 0;
2473 ij6
2474 < 2;
2475 ++ij6)
2476 {
2477 if (!j6valid
2478 [ij6])
2479 {
2480 continue;
2481 }
2482 _ij6[0]
2483 = ij6;
2484 _ij6[1]
2485 = -1;
2486 for (
2487 int iij6
2488 = ij6
2489 + 1;
2490 iij6
2491 < 2;
2492 ++iij6)
2493 {
2494 if (j6valid
2495 [iij6]
2496 && IKabs(
2497 cj6array
2498 [ij6]
2499 - cj6array
2500 [iij6])
2501 < IKFAST_SOLUTION_THRESH
2502 && IKabs(
2503 sj6array
2504 [ij6]
2505 - sj6array
2506 [iij6])
2507 < IKFAST_SOLUTION_THRESH)
2508 {
2509 j6valid
2510 [iij6]
2511 = false;
2512 _ij6[1]
2513 = iij6;
2514 break;
2515 }
2516 }
2517 j6 = j6array
2518 [ij6];
2519 cj6 = cj6array
2520 [ij6];
2521 sj6 = sj6array
2522 [ij6];
2523 {
2524 IkReal evalcond
2525 [1];
2526 evalcond
2527 [0]
2528 = ((0.85)
2529 * (IKsin(
2530 j6)));
2531 if (IKabs(
2532 evalcond
2533 [0])
2534 > IKFAST_EVALCOND_THRESH)
2535 {
2536 continue;
2537 }
2538 }
2539
2540 rotationfunction0(
2541 solutions);
2542 }
2543 }
2544 }
2545 }
2546 }
2547 } while (
2548 0);
2549 if (bgotonextstatement)
2550 {
2551 bool
2552 bgotonextstatement
2553 = true;
2554 do
2555 {
2556 evalcond
2557 [0]
2558 = ((IKabs(
2559 py))
2560 + (IKabs((
2561 (-3.14159265358979)
2562 + (IKfmod(
2563 ((4.71238898038469)
2564 + j4),
2565 6.28318530717959))))));
2566 evalcond
2567 [1]
2568 = -0.85;
2569 evalcond
2570 [2]
2571 = 0;
2572 evalcond
2573 [3]
2574 = ((-0.2125)
2575 + (((-1.0)
2576 * (1.0)
2577 * (px
2578 * px))));
2579 if (IKabs(
2580 evalcond
2581 [0])
2582 < 0.0000010000000000
2583 && IKabs(
2584 evalcond
2585 [1])
2586 < 0.0000010000000000
2587 && IKabs(
2588 evalcond
2589 [2])
2590 < 0.0000010000000000
2591 && IKabs(
2592 evalcond
2593 [3])
2594 < 0.0000010000000000)
2595 {
2596 bgotonextstatement
2597 = false;
2598 {
2599 IkReal j6eval
2600 [1];
2601 IkReal
2602 x618
2603 = ((1.0)
2604 * px);
2605 sj8 = 0;
2606 cj8 = 1.0;
2607 j8 = 0;
2608 pz = 0;
2609 j9 = 0;
2610 sj9 = 0;
2611 cj9 = 1.0;
2612 pp = px
2613 * px;
2614 npx
2615 = (px
2616 * r00);
2617 npy
2618 = (px
2619 * r01);
2620 npz
2621 = (px
2622 * r02);
2623 rxp0_0
2624 = 0;
2625 rxp0_1
2626 = (px
2627 * r20);
2628 rxp1_0
2629 = 0;
2630 rxp1_1
2631 = (px
2632 * r21);
2633 rxp2_0
2634 = 0;
2635 rxp2_1
2636 = (px
2637 * r22);
2638 py = 0;
2639 j4 = -1.5707963267949;
2640 sj4 = -1.0;
2641 cj4 = 0;
2642 rxp0_2
2643 = ((-1.0)
2644 * r10
2645 * x618);
2646 rxp1_2
2647 = ((-1.0)
2648 * r11
2649 * x618);
2650 rxp2_2
2651 = ((-1.0)
2652 * r12
2653 * x618);
2654 j6eval
2655 [0]
2656 = ((1.0)
2657 + (((1.91568587540858)
2658 * (px
2659 * px
2660 * px
2661 * px)))
2662 + (((-1.0)
2663 * (2.64633970947792)
2664 * (px
2665 * px))));
2666 if (IKabs(
2667 j6eval
2668 [0])
2669 < 0.0000010000000000)
2670 {
2671 continue; // no branches [j6]
2672 }
2673 else
2674 {
2675 {
2676 IkReal j6array
2677 [2],
2678 cj6array
2679 [2],
2680 sj6array
2681 [2];
2682 bool j6valid
2683 [2]
2684 = {false};
2685 _nj6
2686 = 2;
2687 IkReal
2688 x619
2689 = px
2690 * px;
2691 CheckValue<IkReal> x621 = IKatan2WithCheck(
2692 IkReal((
2693 (-0.425)
2694 + (((-0.588235294117647)
2695 * x619)))),
2696 ((-2.83333333333333)
2697 + (((3.92156862745098)
2698 * x619))),
2699 IKFAST_ATAN2_MAGTHRESH);
2700 if (!x621.valid)
2701 {
2702 continue;
2703 }
2704 IkReal
2705 x620
2706 = ((-1.0)
2707 * (x621.value));
2708 j6array
2709 [0]
2710 = x620;
2711 sj6array
2712 [0]
2713 = IKsin(
2714 j6array
2715 [0]);
2716 cj6array
2717 [0]
2718 = IKcos(
2719 j6array
2720 [0]);
2721 j6array
2722 [1]
2723 = ((3.14159265358979)
2724 + x620);
2725 sj6array
2726 [1]
2727 = IKsin(
2728 j6array
2729 [1]);
2730 cj6array
2731 [1]
2732 = IKcos(
2733 j6array
2734 [1]);
2735 if (j6array
2736 [0]
2737 > IKPI)
2738 {
2739 j6array
2740 [0]
2741 -= IK2PI;
2742 }
2743 else if (
2744 j6array
2745 [0]
2746 < -IKPI)
2747 {
2748 j6array
2749 [0]
2750 += IK2PI;
2751 }
2752 j6valid
2753 [0]
2754 = true;
2755 if (j6array
2756 [1]
2757 > IKPI)
2758 {
2759 j6array
2760 [1]
2761 -= IK2PI;
2762 }
2763 else if (
2764 j6array
2765 [1]
2766 < -IKPI)
2767 {
2768 j6array
2769 [1]
2770 += IK2PI;
2771 }
2772 j6valid
2773 [1]
2774 = true;
2775 for (
2776 int ij6
2777 = 0;
2778 ij6
2779 < 2;
2780 ++ij6)
2781 {
2782 if (!j6valid
2783 [ij6])
2784 {
2785 continue;
2786 }
2787 _ij6[0]
2788 = ij6;
2789 _ij6[1]
2790 = -1;
2791 for (
2792 int iij6
2793 = ij6
2794 + 1;
2795 iij6
2796 < 2;
2797 ++iij6)
2798 {
2799 if (j6valid
2800 [iij6]
2801 && IKabs(
2802 cj6array
2803 [ij6]
2804 - cj6array
2805 [iij6])
2806 < IKFAST_SOLUTION_THRESH
2807 && IKabs(
2808 sj6array
2809 [ij6]
2810 - sj6array
2811 [iij6])
2812 < IKFAST_SOLUTION_THRESH)
2813 {
2814 j6valid
2815 [iij6]
2816 = false;
2817 _ij6[1]
2818 = iij6;
2819 break;
2820 }
2821 }
2822 j6 = j6array
2823 [ij6];
2824 cj6 = cj6array
2825 [ij6];
2826 sj6 = sj6array
2827 [ij6];
2828 {
2829 IkReal evalcond
2830 [1];
2831 evalcond
2832 [0]
2833 = ((0.85)
2834 * (IKsin(
2835 j6)));
2836 if (IKabs(
2837 evalcond
2838 [0])
2839 > IKFAST_EVALCOND_THRESH)
2840 {
2841 continue;
2842 }
2843 }
2844
2845 rotationfunction0(
2846 solutions);
2847 }
2848 }
2849 }
2850 }
2851 }
2852 } while (
2853 0);
2854 if (bgotonextstatement)
2855 {
2856 bool
2857 bgotonextstatement
2858 = true;
2859 do
2860 {
2861 if (1)
2862 {
2863 bgotonextstatement
2864 = false;
2865 continue; // branch miss [j6]
2866 }
2867 } while (
2868 0);
2869 if (bgotonextstatement)
2870 {
2871 }
2872 }
2873 }
2874 }
2875 }
2876 }
2877 }
2878 }
2879 else
2880 {
2881 {
2882 IkReal j6array
2883 [1],
2884 cj6array
2885 [1],
2886 sj6array
2887 [1];
2888 bool j6valid
2889 [1]
2890 = {false};
2891 _nj6
2892 = 1;
2893 IkReal
2894 x622
2895 = (cj4
2896 * px);
2897 IkReal
2898 x623
2899 = (py
2900 * sj4);
2901 IkReal
2902 x624
2903 = px
2904 * px;
2905 IkReal
2906 x625
2907 = py
2908 * py;
2909 CheckValue<IkReal> x626 = IKPowWithIntegerCheck(
2910 ((((20.0)
2911 * x622))
2912 + (((20.0)
2913 * x623))),
2914 -1);
2915 if (!x626.valid)
2916 {
2917 continue;
2918 }
2919 CheckValue<IkReal> x627 = IKPowWithIntegerCheck(
2920 ((((-1.0)
2921 * (11.7647058823529)
2922 * cj4
2923 * (px
2924 * px
2925 * px)))
2926 + (((-8.5)
2927 * x623))
2928 + (((-11.7647058823529)
2929 * x623
2930 * x624))
2931 + (((-11.7647058823529)
2932 * x622
2933 * x625))
2934 + (((-8.5)
2935 * x622))
2936 + (((-1.0)
2937 * (11.7647058823529)
2938 * sj4
2939 * (py
2940 * py
2941 * py)))),
2942 -1);
2943 if (!x627.valid)
2944 {
2945 continue;
2946 }
2947 if (IKabs((
2948 (17.0)
2949 * (x626.value)))
2950 < IKFAST_ATAN2_MAGTHRESH
2951 && IKabs((
2952 (x627.value)
2953 * (((48.1666666666667)
2954 + (((-66.6666666666667)
2955 * x625))
2956 + (((-66.6666666666667)
2957 * x624))))))
2958 < IKFAST_ATAN2_MAGTHRESH
2959 && IKabs(
2960 IKsqr((
2961 (17.0)
2962 * (x626.value)))
2963 + IKsqr((
2964 (x627.value)
2965 * (((48.1666666666667)
2966 + (((-66.6666666666667)
2967 * x625))
2968 + (((-66.6666666666667)
2969 * x624))))))
2970 - 1)
2971 <= IKFAST_SINCOS_THRESH)
2972 continue;
2973 j6array[0] = IKatan2(
2974 ((17.0)
2975 * (x626.value)),
2976 ((x627.value)
2977 * (((48.1666666666667)
2978 + (((-66.6666666666667)
2979 * x625))
2980 + (((-66.6666666666667)
2981 * x624))))));
2982 sj6array
2983 [0]
2984 = IKsin(
2985 j6array
2986 [0]);
2987 cj6array
2988 [0]
2989 = IKcos(
2990 j6array
2991 [0]);
2992 if (j6array
2993 [0]
2994 > IKPI)
2995 {
2996 j6array
2997 [0]
2998 -= IK2PI;
2999 }
3000 else if (
3001 j6array
3002 [0]
3003 < -IKPI)
3004 {
3005 j6array
3006 [0]
3007 += IK2PI;
3008 }
3009 j6valid
3010 [0]
3011 = true;
3012 for (
3013 int ij6
3014 = 0;
3015 ij6
3016 < 1;
3017 ++ij6)
3018 {
3019 if (!j6valid
3020 [ij6])
3021 {
3022 continue;
3023 }
3024 _ij6[0]
3025 = ij6;
3026 _ij6[1]
3027 = -1;
3028 for (
3029 int iij6
3030 = ij6
3031 + 1;
3032 iij6
3033 < 1;
3034 ++iij6)
3035 {
3036 if (j6valid
3037 [iij6]
3038 && IKabs(
3039 cj6array
3040 [ij6]
3041 - cj6array
3042 [iij6])
3043 < IKFAST_SOLUTION_THRESH
3044 && IKabs(
3045 sj6array
3046 [ij6]
3047 - sj6array
3048 [iij6])
3049 < IKFAST_SOLUTION_THRESH)
3050 {
3051 j6valid
3052 [iij6]
3053 = false;
3054 _ij6[1]
3055 = iij6;
3056 break;
3057 }
3058 }
3059 j6 = j6array
3060 [ij6];
3061 cj6 = cj6array
3062 [ij6];
3063 sj6 = sj6array
3064 [ij6];
3065 {
3066 IkReal evalcond
3067 [5];
3068 IkReal
3069 x628
3070 = IKsin(
3071 j6);
3072 IkReal
3073 x629
3074 = (cj4
3075 * px);
3076 IkReal
3077 x630
3078 = (x628
3079 * x629);
3080 IkReal
3081 x631
3082 = (py
3083 * sj4);
3084 IkReal
3085 x632
3086 = (x628
3087 * x631);
3088 IkReal
3089 x633
3090 = ((1.0)
3091 * x629);
3092 IkReal
3093 x634
3094 = ((1.0)
3095 * x631);
3096 IkReal
3097 x635
3098 = IKcos(
3099 j6);
3100 IkReal
3101 x636
3102 = px
3103 * px;
3104 IkReal
3105 x637
3106 = ((3.92156862745098)
3107 * x628);
3108 IkReal
3109 x638
3110 = ((0.588235294117647)
3111 * x635);
3112 IkReal
3113 x639
3114 = py
3115 * py;
3116 IkReal
3117 x640
3118 = ((0.09)
3119 * x635);
3120 evalcond
3121 [0]
3122 = ((-0.85)
3123 + x630
3124 + x632);
3125 evalcond
3126 [1]
3127 = ((((-1.0)
3128 * x634))
3129 + (((0.85)
3130 * x628))
3131 + (((-1.0)
3132 * x633)));
3133 evalcond
3134 [2]
3135 = ((((-1.0)
3136 * x634
3137 * x635))
3138 + (((-1.0)
3139 * x633
3140 * x635)));
3141 evalcond
3142 [3]
3143 = (((x637
3144 * x639))
3145 + ((x636
3146 * x637))
3147 + (((-1.0)
3148 * x638
3149 * x639))
3150 + (((-0.425)
3151 * x635))
3152 + (((-2.83333333333333)
3153 * x628))
3154 + (((-1.0)
3155 * x636
3156 * x638)));
3157 evalcond
3158 [4]
3159 = ((-0.2125)
3160 + (((-1.0)
3161 * x639))
3162 + ((x631
3163 * x640))
3164 + (((-1.0)
3165 * x636))
3166 + ((x629
3167 * x640))
3168 + (((1.1)
3169 * x632))
3170 + (((1.1)
3171 * x630)));
3172 if (IKabs(
3173 evalcond
3174 [0])
3175 > IKFAST_EVALCOND_THRESH
3176 || IKabs(
3177 evalcond
3178 [1])
3179 > IKFAST_EVALCOND_THRESH
3180 || IKabs(
3181 evalcond
3182 [2])
3183 > IKFAST_EVALCOND_THRESH
3184 || IKabs(
3185 evalcond
3186 [3])
3187 > IKFAST_EVALCOND_THRESH
3188 || IKabs(
3189 evalcond
3190 [4])
3191 > IKFAST_EVALCOND_THRESH)
3192 {
3193 continue;
3194 }
3195 }
3196
3197 rotationfunction0(
3198 solutions);
3199 }
3200 }
3201 }
3202 }
3203 }
3204 else
3205 {
3206 {
3207 IkReal j6array
3208 [1],
3209 cj6array
3210 [1],
3211 sj6array
3212 [1];
3213 bool j6valid
3214 [1]
3215 = {false};
3216 _nj6
3217 = 1;
3218 IkReal
3219 x641
3220 = (cj4
3221 * px);
3222 IkReal
3223 x642
3224 = (py
3225 * sj4);
3226 IkReal
3227 x643
3228 = px
3229 * px;
3230 IkReal
3231 x644
3232 = py
3233 * py;
3234 CheckValue<IkReal> x645 = IKPowWithIntegerCheck(
3235 ((-7.225)
3236 + (((-10.0)
3237 * x643))
3238 + (((-10.0)
3239 * x644))),
3240 -1);
3241 if (!x645.valid)
3242 {
3243 continue;
3244 }
3245 if (IKabs((
3246 (((1.17647058823529)
3247 * x642))
3248 + (((1.17647058823529)
3249 * x641))))
3250 < IKFAST_ATAN2_MAGTHRESH
3251 && IKabs((
3252 (x645.value)
3253 * (((((-1.0)
3254 * (78.4313725490196)
3255 * sj4
3256 * (py
3257 * py
3258 * py)))
3259 + (((-78.4313725490196)
3260 * x641
3261 * x644))
3262 + (((-78.4313725490196)
3263 * x642
3264 * x643))
3265 + (((-1.0)
3266 * (78.4313725490196)
3267 * cj4
3268 * (px
3269 * px
3270 * px)))
3271 + (((56.6666666666667)
3272 * x642))
3273 + (((56.6666666666667)
3274 * x641))))))
3275 < IKFAST_ATAN2_MAGTHRESH
3276 && IKabs(
3277 IKsqr((
3278 (((1.17647058823529)
3279 * x642))
3280 + (((1.17647058823529)
3281 * x641))))
3282 + IKsqr((
3283 (x645.value)
3284 * (((((-1.0)
3285 * (78.4313725490196)
3286 * sj4
3287 * (py
3288 * py
3289 * py)))
3290 + (((-78.4313725490196)
3291 * x641
3292 * x644))
3293 + (((-78.4313725490196) * x642 * x643)) + (((-1.0) * (78.4313725490196) * cj4 * (px * px * px))) + (((56.6666666666667) * x642)) + (((56.6666666666667) * x641))))))
3294 - 1)
3295 <= IKFAST_SINCOS_THRESH)
3296 continue;
3297 j6array[0] = IKatan2(
3298 ((((1.17647058823529)
3299 * x642))
3300 + (((1.17647058823529)
3301 * x641))),
3302 ((x645.value)
3303 * (((((-1.0)
3304 * (78.4313725490196)
3305 * sj4
3306 * (py
3307 * py
3308 * py)))
3309 + (((-78.4313725490196)
3310 * x641
3311 * x644))
3312 + (((-78.4313725490196)
3313 * x642
3314 * x643))
3315 + (((-1.0)
3316 * (78.4313725490196)
3317 * cj4
3318 * (px
3319 * px
3320 * px)))
3321 + (((56.6666666666667)
3322 * x642))
3323 + (((56.6666666666667)
3324 * x641))))));
3325 sj6array
3326 [0]
3327 = IKsin(
3328 j6array
3329 [0]);
3330 cj6array
3331 [0]
3332 = IKcos(
3333 j6array
3334 [0]);
3335 if (j6array
3336 [0]
3337 > IKPI)
3338 {
3339 j6array
3340 [0]
3341 -= IK2PI;
3342 }
3343 else if (
3344 j6array
3345 [0]
3346 < -IKPI)
3347 {
3348 j6array
3349 [0]
3350 += IK2PI;
3351 }
3352 j6valid
3353 [0]
3354 = true;
3355 for (
3356 int ij6
3357 = 0;
3358 ij6
3359 < 1;
3360 ++ij6)
3361 {
3362 if (!j6valid
3363 [ij6])
3364 {
3365 continue;
3366 }
3367 _ij6[0]
3368 = ij6;
3369 _ij6[1]
3370 = -1;
3371 for (
3372 int iij6
3373 = ij6
3374 + 1;
3375 iij6
3376 < 1;
3377 ++iij6)
3378 {
3379 if (j6valid
3380 [iij6]
3381 && IKabs(
3382 cj6array
3383 [ij6]
3384 - cj6array
3385 [iij6])
3386 < IKFAST_SOLUTION_THRESH
3387 && IKabs(
3388 sj6array
3389 [ij6]
3390 - sj6array
3391 [iij6])
3392 < IKFAST_SOLUTION_THRESH)
3393 {
3394 j6valid
3395 [iij6]
3396 = false;
3397 _ij6[1]
3398 = iij6;
3399 break;
3400 }
3401 }
3402 j6 = j6array
3403 [ij6];
3404 cj6 = cj6array
3405 [ij6];
3406 sj6 = sj6array
3407 [ij6];
3408 {
3409 IkReal evalcond
3410 [5];
3411 IkReal
3412 x646
3413 = IKsin(
3414 j6);
3415 IkReal
3416 x647
3417 = (cj4
3418 * px);
3419 IkReal
3420 x648
3421 = (x646
3422 * x647);
3423 IkReal
3424 x649
3425 = (py
3426 * sj4);
3427 IkReal
3428 x650
3429 = (x646
3430 * x649);
3431 IkReal
3432 x651
3433 = ((1.0)
3434 * x647);
3435 IkReal
3436 x652
3437 = ((1.0)
3438 * x649);
3439 IkReal
3440 x653
3441 = IKcos(
3442 j6);
3443 IkReal
3444 x654
3445 = px
3446 * px;
3447 IkReal
3448 x655
3449 = ((3.92156862745098)
3450 * x646);
3451 IkReal
3452 x656
3453 = ((0.588235294117647)
3454 * x653);
3455 IkReal
3456 x657
3457 = py
3458 * py;
3459 IkReal
3460 x658
3461 = ((0.09)
3462 * x653);
3463 evalcond
3464 [0]
3465 = ((-0.85)
3466 + x650
3467 + x648);
3468 evalcond
3469 [1]
3470 = ((((-1.0)
3471 * x652))
3472 + (((-1.0)
3473 * x651))
3474 + (((0.85)
3475 * x646)));
3476 evalcond
3477 [2]
3478 = ((((-1.0)
3479 * x652
3480 * x653))
3481 + (((-1.0)
3482 * x651
3483 * x653)));
3484 evalcond
3485 [3]
3486 = (((x654
3487 * x655))
3488 + ((x655
3489 * x657))
3490 + (((-1.0)
3491 * x654
3492 * x656))
3493 + (((-0.425)
3494 * x653))
3495 + (((-1.0)
3496 * x656
3497 * x657))
3498 + (((-2.83333333333333)
3499 * x646)));
3500 evalcond
3501 [4]
3502 = ((-0.2125)
3503 + (((1.1)
3504 * x648))
3505 + (((1.1)
3506 * x650))
3507 + ((x649
3508 * x658))
3509 + (((-1.0)
3510 * x657))
3511 + ((x647
3512 * x658))
3513 + (((-1.0)
3514 * x654)));
3515 if (IKabs(
3516 evalcond
3517 [0])
3518 > IKFAST_EVALCOND_THRESH
3519 || IKabs(
3520 evalcond
3521 [1])
3522 > IKFAST_EVALCOND_THRESH
3523 || IKabs(
3524 evalcond
3525 [2])
3526 > IKFAST_EVALCOND_THRESH
3527 || IKabs(
3528 evalcond
3529 [3])
3530 > IKFAST_EVALCOND_THRESH
3531 || IKabs(
3532 evalcond
3533 [4])
3534 > IKFAST_EVALCOND_THRESH)
3535 {
3536 continue;
3537 }
3538 }
3539
3540 rotationfunction0(
3541 solutions);
3542 }
3543 }
3544 }
3545 }
3546 }
3547 else
3548 {
3549 {
3550 IkReal
3551 j6array
3552 [1],
3553 cj6array
3554 [1],
3555 sj6array
3556 [1];
3557 bool j6valid
3558 [1]
3559 = {false};
3560 _nj6 = 1;
3561 IkReal x659
3562 = (cj4
3563 * px);
3564 IkReal x660
3565 = (py
3566 * sj4);
3567 IkReal x661
3568 = px
3569 * px;
3570 IkReal x662
3571 = py
3572 * py;
3573 IkReal x663
3574 = ((1.29411764705882)
3575 * (cj4
3576 * cj4));
3577 CheckValue<
3578 IkReal>
3579 x664
3580 = IKPowWithIntegerCheck(
3581 ((((0.09)
3582 * x659))
3583 + (((0.09)
3584 * x660))),
3585 -1);
3586 if (!x664.valid)
3587 {
3588 continue;
3589 }
3590 if (IKabs((
3591 (((1.17647058823529)
3592 * x660))
3593 + (((1.17647058823529)
3594 * x659))))
3595 < IKFAST_ATAN2_MAGTHRESH
3596 && IKabs((
3597 (x664.value)
3598 * (((0.2125)
3599 + (((-2.58823529411765)
3600 * cj4
3601 * px
3602 * x660))
3603 + (((-0.294117647058824)
3604 * x662))
3605 + (((-1.0)
3606 * x661
3607 * x663))
3608 + ((x662
3609 * x663))
3610 + x661))))
3611 < IKFAST_ATAN2_MAGTHRESH
3612 && IKabs(
3613 IKsqr((
3614 (((1.17647058823529)
3615 * x660))
3616 + (((1.17647058823529)
3617 * x659))))
3618 + IKsqr((
3619 (x664.value)
3620 * (((0.2125)
3621 + (((-2.58823529411765)
3622 * cj4
3623 * px
3624 * x660))
3625 + (((-0.294117647058824)
3626 * x662))
3627 + (((-1.0)
3628 * x661
3629 * x663))
3630 + ((x662
3631 * x663))
3632 + x661))))
3633 - 1)
3634 <= IKFAST_SINCOS_THRESH)
3635 continue;
3636 j6array[0] = IKatan2(
3637 ((((1.17647058823529)
3638 * x660))
3639 + (((1.17647058823529)
3640 * x659))),
3641 ((x664.value)
3642 * (((0.2125)
3643 + (((-2.58823529411765)
3644 * cj4
3645 * px
3646 * x660))
3647 + (((-0.294117647058824)
3648 * x662))
3649 + (((-1.0)
3650 * x661
3651 * x663))
3652 + ((x662
3653 * x663))
3654 + x661))));
3655 sj6array[0] = IKsin(
3656 j6array
3657 [0]);
3658 cj6array[0] = IKcos(
3659 j6array
3660 [0]);
3661 if (j6array
3662 [0]
3663 > IKPI)
3664 {
3665 j6array[0]
3666 -= IK2PI;
3667 }
3668 else if (
3669 j6array
3670 [0]
3671 < -IKPI)
3672 {
3673 j6array[0]
3674 += IK2PI;
3675 }
3676 j6valid[0]
3677 = true;
3678 for (int ij6
3679 = 0;
3680 ij6
3681 < 1;
3682 ++ij6)
3683 {
3684 if (!j6valid
3685 [ij6])
3686 {
3687 continue;
3688 }
3689 _ij6[0]
3690 = ij6;
3691 _ij6[1]
3692 = -1;
3693 for (
3694 int iij6
3695 = ij6
3696 + 1;
3697 iij6
3698 < 1;
3699 ++iij6)
3700 {
3701 if (j6valid
3702 [iij6]
3703 && IKabs(
3704 cj6array
3705 [ij6]
3706 - cj6array
3707 [iij6])
3708 < IKFAST_SOLUTION_THRESH
3709 && IKabs(
3710 sj6array
3711 [ij6]
3712 - sj6array
3713 [iij6])
3714 < IKFAST_SOLUTION_THRESH)
3715 {
3716 j6valid
3717 [iij6]
3718 = false;
3719 _ij6[1]
3720 = iij6;
3721 break;
3722 }
3723 }
3724 j6 = j6array
3725 [ij6];
3726 cj6 = cj6array
3727 [ij6];
3728 sj6 = sj6array
3729 [ij6];
3730 {
3731 IkReal evalcond
3732 [5];
3733 IkReal
3734 x665
3735 = IKsin(
3736 j6);
3737 IkReal
3738 x666
3739 = (cj4
3740 * px);
3741 IkReal
3742 x667
3743 = (x665
3744 * x666);
3745 IkReal
3746 x668
3747 = (py
3748 * sj4);
3749 IkReal
3750 x669
3751 = (x665
3752 * x668);
3753 IkReal
3754 x670
3755 = ((1.0)
3756 * x666);
3757 IkReal
3758 x671
3759 = ((1.0)
3760 * x668);
3761 IkReal
3762 x672
3763 = IKcos(
3764 j6);
3765 IkReal
3766 x673
3767 = px
3768 * px;
3769 IkReal
3770 x674
3771 = ((3.92156862745098)
3772 * x665);
3773 IkReal
3774 x675
3775 = ((0.588235294117647)
3776 * x672);
3777 IkReal
3778 x676
3779 = py
3780 * py;
3781 IkReal
3782 x677
3783 = ((0.09)
3784 * x672);
3785 evalcond
3786 [0]
3787 = ((-0.85)
3788 + x667
3789 + x669);
3790 evalcond
3791 [1]
3792 = ((((0.85)
3793 * x665))
3794 + (((-1.0)
3795 * x670))
3796 + (((-1.0)
3797 * x671)));
3798 evalcond
3799 [2]
3800 = ((((-1.0)
3801 * x671
3802 * x672))
3803 + (((-1.0)
3804 * x670
3805 * x672)));
3806 evalcond
3807 [3]
3808 = ((((-2.83333333333333)
3809 * x665))
3810 + (((-0.425)
3811 * x672))
3812 + ((x673
3813 * x674))
3814 + (((-1.0)
3815 * x675
3816 * x676))
3817 + ((x674
3818 * x676))
3819 + (((-1.0)
3820 * x673
3821 * x675)));
3822 evalcond
3823 [4]
3824 = ((-0.2125)
3825 + ((x668
3826 * x677))
3827 + (((-1.0)
3828 * x676))
3829 + ((x666
3830 * x677))
3831 + (((1.1)
3832 * x667))
3833 + (((-1.0)
3834 * x673))
3835 + (((1.1)
3836 * x669)));
3837 if (IKabs(
3838 evalcond
3839 [0])
3840 > IKFAST_EVALCOND_THRESH
3841 || IKabs(
3842 evalcond
3843 [1])
3844 > IKFAST_EVALCOND_THRESH
3845 || IKabs(
3846 evalcond
3847 [2])
3848 > IKFAST_EVALCOND_THRESH
3849 || IKabs(
3850 evalcond
3851 [3])
3852 > IKFAST_EVALCOND_THRESH
3853 || IKabs(
3854 evalcond
3855 [4])
3856 > IKFAST_EVALCOND_THRESH)
3857 {
3858 continue;
3859 }
3860 }
3861
3862 rotationfunction0(
3863 solutions);
3864 }
3865 }
3866 }
3867 }
3868 }
3869 } while (0);
3870 if (bgotonextstatement)
3871 {
3872 bool
3873 bgotonextstatement
3874 = true;
3875 do
3876 {
3877 if (1)
3878 {
3879 bgotonextstatement
3880 = false;
3881 continue; // branch
3882 // miss
3883 // [j6]
3884 }
3885 } while (0);
3886 if (bgotonextstatement)
3887 {
3888 }
3889 }
3890 }
3891 }
3892 else
3893 {
3894 {
3895 IkReal j6array[1],
3896 cj6array[1],
3897 sj6array[1];
3898 bool j6valid[1]
3899 = {false};
3900 _nj6 = 1;
3901 IkReal x678
3902 = (cj4 * px);
3903 IkReal x679
3904 = (py * sj4);
3905 IkReal x680
3906 = ((0.108264705882353)
3907 * cj9);
3908 IkReal x681
3909 = ((0.588235294117647)
3910 * pp);
3911 IkReal x682
3912 = (cj9 * pp);
3913 IkReal x683
3914 = (cj9 * sj9);
3915 IkReal x684
3916 = (pp * sj9);
3917 IkReal x685
3918 = cj9 * cj9;
3919 IkReal x686
3920 = ((1.0) * pz);
3921 CheckValue<IkReal> x687 = IKPowWithIntegerCheck(
3922 IKsign((
3923 (((-1.0)
3924 * x678
3925 * x681))
3926 + (((-1.0)
3927 * x679
3928 * x680))
3929 + (((1.51009803921569)
3930 * pz))
3931 + (((1.32323529411765)
3932 * cj9
3933 * pz))
3934 + (((-1.0)
3935 * (3.92156862745098)
3936 * pp
3937 * pz))
3938 + (((-1.0)
3939 * x678
3940 * x680))
3941 + (((-0.316735294117647)
3942 * x679))
3943 + (((-0.316735294117647)
3944 * x678))
3945 + (((-1.0)
3946 * x679
3947 * x681)))),
3948 -1);
3949 if (!x687.valid)
3950 {
3951 continue;
3952 }
3953 CheckValue<IkReal> x688 = IKatan2WithCheck(
3954 IkReal((
3955 (-0.174204411764706)
3956 + (((-0.0264705882352941)
3957 * x684))
3958 + (((-0.00487191176470588)
3959 * x683))
3960 + (pz * pz)
3961 + (((-0.0324794117647059)
3962 * x685))
3963 + (((-1.0)
3964 * (0.154566176470588)
3965 * cj9))
3966 + (((-0.176470588235294)
3967 * x682))
3968 + (((-1.0)
3969 * (0.323529411764706)
3970 * pp))
3971 + (((-1.0)
3972 * (0.0142530882352941)
3973 * sj9)))),
3974 ((0.830553921568627)
3975 + (((-1.17647058823529)
3976 * x682))
3977 + (((0.396970588235294)
3978 * x685))
3979 + (((1.18080882352941)
3980 * cj9))
3981 + (((0.0595455882352941)
3982 * x683))
3983 + (((-1.0)
3984 * (2.15686274509804)
3985 * pp))
3986 + (((-1.0) * x678
3987 * x686))
3988 + (((-1.0) * x679
3989 * x686))
3990 + (((-0.176470588235294)
3991 * x684))
3992 + (((0.0679544117647059)
3993 * sj9))),
3994 IKFAST_ATAN2_MAGTHRESH);
3995 if (!x688.valid)
3996 {
3997 continue;
3998 }
3999 j6array[0]
4000 = ((-1.5707963267949)
4001 + (((1.5707963267949)
4002 * (x687.value)))
4003 + (x688.value));
4004 sj6array[0] = IKsin(
4005 j6array[0]);
4006 cj6array[0] = IKcos(
4007 j6array[0]);
4008 if (j6array[0] > IKPI)
4009 {
4010 j6array[0] -= IK2PI;
4011 }
4012 else if (
4013 j6array[0]
4014 < -IKPI)
4015 {
4016 j6array[0] += IK2PI;
4017 }
4018 j6valid[0] = true;
4019 for (int ij6 = 0;
4020 ij6 < 1;
4021 ++ij6)
4022 {
4023 if (!j6valid[ij6])
4024 {
4025 continue;
4026 }
4027 _ij6[0] = ij6;
4028 _ij6[1] = -1;
4029 for (int iij6
4030 = ij6 + 1;
4031 iij6 < 1;
4032 ++iij6)
4033 {
4034 if (j6valid[iij6]
4035 && IKabs(
4036 cj6array
4037 [ij6]
4038 - cj6array
4039 [iij6])
4040 < IKFAST_SOLUTION_THRESH
4041 && IKabs(
4042 sj6array
4043 [ij6]
4044 - sj6array
4045 [iij6])
4046 < IKFAST_SOLUTION_THRESH)
4047 {
4048 j6valid[iij6]
4049 = false;
4050 _ij6[1] = iij6;
4051 break;
4052 }
4053 }
4054 j6 = j6array[ij6];
4055 cj6 = cj6array[ij6];
4056 sj6 = sj6array[ij6];
4057 {
4058 IkReal
4059 evalcond[5];
4060 IkReal x689
4061 = ((0.3)
4062 * cj9);
4063 IkReal x690
4064 = ((0.045)
4065 * sj9);
4066 IkReal x691
4067 = IKcos(j6);
4068 IkReal x692
4069 = (pz * x691);
4070 IkReal x693
4071 = IKsin(j6);
4072 IkReal x694
4073 = (cj4 * px);
4074 IkReal x695
4075 = (x693
4076 * x694);
4077 IkReal x696
4078 = (py * sj4);
4079 IkReal x697
4080 = (x693
4081 * x696);
4082 IkReal x698
4083 = ((0.045)
4084 * cj9);
4085 IkReal x699
4086 = ((0.3)
4087 * sj9);
4088 IkReal x700
4089 = (pz * x693);
4090 IkReal x701
4091 = ((1.0)
4092 * x694);
4093 IkReal x702
4094 = ((1.0)
4095 * x696);
4096 IkReal x703
4097 = ((0.09)
4098 * x691);
4099 evalcond[0]
4100 = ((-0.55)
4101 + (((-1.0)
4102 * x689))
4103 + (((-1.0)
4104 * x690))
4105 + x692
4106 + x695
4107 + x697);
4108 evalcond[1]
4109 = ((0.045)
4110 + x700
4111 + (((-1.0)
4112 * x691
4113 * x701))
4114 + (((-1.0)
4115 * x698))
4116 + x699
4117 + (((-1.0)
4118 * x691
4119 * x702)));
4120 evalcond[2]
4121 = ((((-1.51009803921569)
4122 * x693))
4123 + (((-0.588235294117647)
4124 * pp
4125 * x691))
4126 + (((3.92156862745098)
4127 * pp
4128 * x693))
4129 + pz
4130 + (((-1.32323529411765)
4131 * cj9
4132 * x693))
4133 + (((-0.316735294117647)
4134 * x691))
4135 + (((-0.108264705882353)
4136 * cj9
4137 * x691)));
4138 evalcond[3]
4139 = ((((-1.0)
4140 * x691
4141 * x698))
4142 + (((-1.0)
4143 * x702))
4144 + ((x690
4145 * x693))
4146 + ((x691
4147 * x699))
4148 + (((0.55)
4149 * x693))
4150 + (((0.045)
4151 * x691))
4152 + ((x689
4153 * x693))
4154 + (((-1.0)
4155 * x701)));
4156 evalcond[4]
4157 = ((-0.2125)
4158 + ((x696
4159 * x703))
4160 + (((1.1)
4161 * x695))
4162 + (((-0.09)
4163 * x700))
4164 + (((1.1)
4165 * x692))
4166 + (((1.1)
4167 * x697))
4168 + ((x694
4169 * x703))
4170 + (((-1.0)
4171 * (1.0)
4172 * pp)));
4173 if (IKabs(evalcond
4174 [0])
4175 > IKFAST_EVALCOND_THRESH
4176 || IKabs(
4177 evalcond
4178 [1])
4179 > IKFAST_EVALCOND_THRESH
4180 || IKabs(
4181 evalcond
4182 [2])
4183 > IKFAST_EVALCOND_THRESH
4184 || IKabs(
4185 evalcond
4186 [3])
4187 > IKFAST_EVALCOND_THRESH
4188 || IKabs(
4189 evalcond
4190 [4])
4191 > IKFAST_EVALCOND_THRESH)
4192 {
4193 continue;
4194 }
4195 }
4196
4197 rotationfunction0(
4198 solutions);
4199 }
4200 }
4201 }
4202 }
4203 }
4204 else
4205 {
4206 {
4207 IkReal j6array[1],
4208 cj6array[1],
4209 sj6array[1];
4210 bool j6valid[1] = {false};
4211 _nj6 = 1;
4212 IkReal x704
4213 = ((0.045) * cj4
4214 * px);
4215 IkReal x705
4216 = ((0.045) * py
4217 * sj4);
4218 IkReal x706
4219 = ((0.3) * sj9);
4220 IkReal x707 = (cj4 * px);
4221 IkReal x708 = (py * sj4);
4222 IkReal x709 = (cj9 * sj9);
4223 IkReal x710 = cj9 * cj9;
4224 IkReal x711
4225 = ((1.0) * pz);
4226 IkReal x712 = py * py;
4227 IkReal x713 = cj4 * cj4;
4228 CheckValue<IkReal> x714
4229 = IKatan2WithCheck(
4230 IkReal((
4231 (0.03825)
4232 + (((0.087975)
4233 * x709))
4234 + (((-0.027)
4235 * x710))
4236 + (((-1.0)
4237 * x707
4238 * x711))
4239 + (((0.167025)
4240 * sj9))
4241 + (((-1.0)
4242 * (0.01125)
4243 * cj9))
4244 + (((-1.0)
4245 * x708
4246 * x711)))),
4247 ((-0.304525)
4248 + (((-1.0)
4249 * (0.0495)
4250 * sj9))
4251 + (((-0.087975)
4252 * x710))
4253 + (((2.0) * cj4
4254 * px * x708))
4255 + x712
4256 + (((-1.0) * x712
4257 * x713))
4258 + (((-1.0)
4259 * (0.33)
4260 * cj9))
4261 + ((x713
4262 * (px * px)))
4263 + (((-0.027)
4264 * x709))),
4265 IKFAST_ATAN2_MAGTHRESH);
4266 if (!x714.valid)
4267 {
4268 continue;
4269 }
4270 CheckValue<IkReal> x715
4271 = IKPowWithIntegerCheck(
4272 IKsign((
4273 (((-1.0) * cj9
4274 * x704))
4275 + (((-1.0)
4276 * (0.55)
4277 * pz))
4278 + ((x706
4279 * x708))
4280 + ((x706
4281 * x707))
4282 + x705 + x704
4283 + (((-1.0)
4284 * cj9
4285 * x705))
4286 + (((-1.0)
4287 * (0.045)
4288 * pz
4289 * sj9))
4290 + (((-1.0)
4291 * (0.3)
4292 * cj9
4293 * pz)))),
4294 -1);
4295 if (!x715.valid)
4296 {
4297 continue;
4298 }
4299 j6array[0]
4300 = ((-1.5707963267949)
4301 + (x714.value)
4302 + (((1.5707963267949)
4303 * (x715.value))));
4304 sj6array[0]
4305 = IKsin(j6array[0]);
4306 cj6array[0]
4307 = IKcos(j6array[0]);
4308 if (j6array[0] > IKPI)
4309 {
4310 j6array[0] -= IK2PI;
4311 }
4312 else if (
4313 j6array[0] < -IKPI)
4314 {
4315 j6array[0] += IK2PI;
4316 }
4317 j6valid[0] = true;
4318 for (int ij6 = 0; ij6 < 1;
4319 ++ij6)
4320 {
4321 if (!j6valid[ij6])
4322 {
4323 continue;
4324 }
4325 _ij6[0] = ij6;
4326 _ij6[1] = -1;
4327 for (int iij6 = ij6 + 1;
4328 iij6 < 1;
4329 ++iij6)
4330 {
4331 if (j6valid[iij6]
4332 && IKabs(
4333 cj6array
4334 [ij6]
4335 - cj6array
4336 [iij6])
4337 < IKFAST_SOLUTION_THRESH
4338 && IKabs(
4339 sj6array
4340 [ij6]
4341 - sj6array
4342 [iij6])
4343 < IKFAST_SOLUTION_THRESH)
4344 {
4345 j6valid[iij6]
4346 = false;
4347 _ij6[1] = iij6;
4348 break;
4349 }
4350 }
4351 j6 = j6array[ij6];
4352 cj6 = cj6array[ij6];
4353 sj6 = sj6array[ij6];
4354 {
4355 IkReal evalcond[5];
4356 IkReal x716
4357 = ((0.3) * cj9);
4358 IkReal x717
4359 = ((0.045) * sj9);
4360 IkReal x718
4361 = IKcos(j6);
4362 IkReal x719
4363 = (pz * x718);
4364 IkReal x720
4365 = IKsin(j6);
4366 IkReal x721
4367 = (cj4 * px);
4368 IkReal x722
4369 = (x720 * x721);
4370 IkReal x723
4371 = (py * sj4);
4372 IkReal x724
4373 = (x720 * x723);
4374 IkReal x725
4375 = ((0.045) * cj9);
4376 IkReal x726
4377 = ((0.3) * sj9);
4378 IkReal x727
4379 = (pz * x720);
4380 IkReal x728
4381 = ((1.0) * x721);
4382 IkReal x729
4383 = ((1.0) * x723);
4384 IkReal x730
4385 = ((0.09) * x718);
4386 evalcond[0]
4387 = ((-0.55) + x719
4388 + (((-1.0)
4389 * x716))
4390 + (((-1.0)
4391 * x717))
4392 + x724 + x722);
4393 evalcond[1]
4394 = ((0.045)
4395 + (((-1.0)
4396 * x718
4397 * x729))
4398 + (((-1.0)
4399 * x725))
4400 + (((-1.0)
4401 * x718
4402 * x728))
4403 + x727 + x726);
4404 evalcond[2]
4405 = ((((3.92156862745098)
4406 * pp * x720))
4407 + (((-1.32323529411765)
4408 * cj9
4409 * x720))
4410 + (((-0.108264705882353)
4411 * cj9
4412 * x718))
4413 + (((-1.51009803921569)
4414 * x720))
4415 + pz
4416 + (((-0.588235294117647)
4417 * pp
4418 * x718))
4419 + (((-0.316735294117647)
4420 * x718)));
4421 evalcond[3]
4422 = (((x717 * x720))
4423 + (((-1.0)
4424 * x718
4425 * x725))
4426 + (((0.045)
4427 * x718))
4428 + ((x716
4429 * x720))
4430 + (((-1.0)
4431 * x728))
4432 + (((0.55)
4433 * x720))
4434 + (((-1.0)
4435 * x729))
4436 + ((x718
4437 * x726)));
4438 evalcond[4]
4439 = ((-0.2125)
4440 + (((1.1)
4441 * x722))
4442 + (((-0.09)
4443 * x727))
4444 + (((1.1)
4445 * x719))
4446 + ((x723
4447 * x730))
4448 + (((1.1)
4449 * x724))
4450 + (((-1.0)
4451 * (1.0)
4452 * pp))
4453 + ((x721
4454 * x730)));
4455 if (IKabs(evalcond[0])
4456 > IKFAST_EVALCOND_THRESH
4457 || IKabs(evalcond
4458 [1])
4459 > IKFAST_EVALCOND_THRESH
4460 || IKabs(evalcond
4461 [2])
4462 > IKFAST_EVALCOND_THRESH
4463 || IKabs(evalcond
4464 [3])
4465 > IKFAST_EVALCOND_THRESH
4466 || IKabs(evalcond
4467 [4])
4468 > IKFAST_EVALCOND_THRESH)
4469 {
4470 continue;
4471 }
4472 }
4473
4474 rotationfunction0(
4475 solutions);
4476 }
4477 }
4478 }
4479 }
4480 }
4481 else
4482 {
4483 {
4484 IkReal j6array[1],
4485 cj6array[1], sj6array[1];
4486 bool j6valid[1] = {false};
4487 _nj6 = 1;
4488 IkReal x731 = (cj4 * px);
4489 IkReal x732 = (py * sj4);
4490 IkReal x733
4491 = ((1.32323529411765)
4492 * cj9);
4493 IkReal x734
4494 = ((3.92156862745098)
4495 * pp);
4496 IkReal x735
4497 = ((0.0264705882352941)
4498 * pp);
4499 IkReal x736 = (cj9 * sj9);
4500 IkReal x737
4501 = ((0.176470588235294)
4502 * pp);
4503 IkReal x738 = cj9 * cj9;
4504 CheckValue<IkReal> x739 = IKatan2WithCheck(
4505 IkReal((
4506 (-0.0142530882352941)
4507 + (((-0.0324794117647059)
4508 * x736))
4509 + (((0.00938117647058823)
4510 * cj9))
4511 + ((pz * x732))
4512 + ((pz * x731))
4513 + (((-1.0) * x735))
4514 + (((0.00487191176470588)
4515 * x738))
4516 + (((-1.0)
4517 * (0.0950205882352941)
4518 * sj9))
4519 + ((cj9 * x735))
4520 + (((-1.0) * sj9
4521 * x737)))),
4522 ((0.0679544117647059)
4523 + (((-1.0) * x737))
4524 + ((cj9 * x737))
4525 + (pz * pz)
4526 + (((-1.0)
4527 * (0.00840882352941177)
4528 * cj9))
4529 + (((-0.0595455882352941)
4530 * x738))
4531 + (((0.453029411764706)
4532 * sj9))
4533 + (((0.396970588235294)
4534 * x736))
4535 + (((-1.0)
4536 * (1.17647058823529)
4537 * pp * sj9))),
4538 IKFAST_ATAN2_MAGTHRESH);
4539 if (!x739.valid)
4540 {
4541 continue;
4542 }
4543 CheckValue<IkReal> x740 = IKPowWithIntegerCheck(
4544 IKsign((
4545 (((0.316735294117647)
4546 * pz))
4547 + (((0.108264705882353)
4548 * cj9 * pz))
4549 + (((0.588235294117647)
4550 * pp * pz))
4551 + ((x732 * x733))
4552 + (((-1.0) * x732
4553 * x734))
4554 + (((1.51009803921569)
4555 * x731))
4556 + (((1.51009803921569)
4557 * x732))
4558 + ((x731 * x733))
4559 + (((-1.0) * x731
4560 * x734)))),
4561 -1);
4562 if (!x740.valid)
4563 {
4564 continue;
4565 }
4566 j6array[0]
4567 = ((-1.5707963267949)
4568 + (x739.value)
4569 + (((1.5707963267949)
4570 * (x740.value))));
4571 sj6array[0]
4572 = IKsin(j6array[0]);
4573 cj6array[0]
4574 = IKcos(j6array[0]);
4575 if (j6array[0] > IKPI)
4576 {
4577 j6array[0] -= IK2PI;
4578 }
4579 else if (j6array[0] < -IKPI)
4580 {
4581 j6array[0] += IK2PI;
4582 }
4583 j6valid[0] = true;
4584 for (int ij6 = 0; ij6 < 1;
4585 ++ij6)
4586 {
4587 if (!j6valid[ij6])
4588 {
4589 continue;
4590 }
4591 _ij6[0] = ij6;
4592 _ij6[1] = -1;
4593 for (int iij6 = ij6 + 1;
4594 iij6 < 1;
4595 ++iij6)
4596 {
4597 if (j6valid[iij6]
4598 && IKabs(
4599 cj6array[ij6]
4600 - cj6array
4601 [iij6])
4602 < IKFAST_SOLUTION_THRESH
4603 && IKabs(
4604 sj6array[ij6]
4605 - sj6array
4606 [iij6])
4607 < IKFAST_SOLUTION_THRESH)
4608 {
4609 j6valid[iij6] = false;
4610 _ij6[1] = iij6;
4611 break;
4612 }
4613 }
4614 j6 = j6array[ij6];
4615 cj6 = cj6array[ij6];
4616 sj6 = sj6array[ij6];
4617 {
4618 IkReal evalcond[5];
4619 IkReal x741
4620 = ((0.3) * cj9);
4621 IkReal x742
4622 = ((0.045) * sj9);
4623 IkReal x743 = IKcos(j6);
4624 IkReal x744 = (pz * x743);
4625 IkReal x745 = IKsin(j6);
4626 IkReal x746 = (cj4 * px);
4627 IkReal x747
4628 = (x745 * x746);
4629 IkReal x748 = (py * sj4);
4630 IkReal x749
4631 = (x745 * x748);
4632 IkReal x750
4633 = ((0.045) * cj9);
4634 IkReal x751
4635 = ((0.3) * sj9);
4636 IkReal x752 = (pz * x745);
4637 IkReal x753
4638 = ((1.0) * x746);
4639 IkReal x754
4640 = ((1.0) * x748);
4641 IkReal x755
4642 = ((0.09) * x743);
4643 evalcond[0]
4644 = ((-0.55)
4645 + (((-1.0) * x741))
4646 + x749 + x744
4647 + x747
4648 + (((-1.0)
4649 * x742)));
4650 evalcond[1]
4651 = ((0.045)
4652 + (((-1.0) * x743
4653 * x753))
4654 + (((-1.0) * x743
4655 * x754))
4656 + x752 + x751
4657 + (((-1.0)
4658 * x750)));
4659 evalcond[2]
4660 = ((((-0.316735294117647)
4661 * x743))
4662 + (((-1.32323529411765)
4663 * cj9 * x745))
4664 + pz
4665 + (((-0.108264705882353)
4666 * cj9 * x743))
4667 + (((-1.51009803921569)
4668 * x745))
4669 + (((-0.588235294117647)
4670 * pp * x743))
4671 + (((3.92156862745098)
4672 * pp * x745)));
4673 evalcond[3]
4674 = ((((-1.0) * x753))
4675 + (((0.55) * x745))
4676 + (((0.045)
4677 * x743))
4678 + ((x741 * x745))
4679 + ((x743 * x751))
4680 + (((-1.0) * x743
4681 * x750))
4682 + (((-1.0) * x754))
4683 + ((x742 * x745)));
4684 evalcond[4]
4685 = ((-0.2125)
4686 + (((1.1) * x749))
4687 + (((-0.09)
4688 * x752))
4689 + ((x748 * x755))
4690 + (((1.1) * x744))
4691 + (((-1.0) * (1.0)
4692 * pp))
4693 + ((x746 * x755))
4694 + (((1.1)
4695 * x747)));
4696 if (IKabs(evalcond[0])
4697 > IKFAST_EVALCOND_THRESH
4698 || IKabs(evalcond[1])
4699 > IKFAST_EVALCOND_THRESH
4700 || IKabs(evalcond[2])
4701 > IKFAST_EVALCOND_THRESH
4702 || IKabs(evalcond[3])
4703 > IKFAST_EVALCOND_THRESH
4704 || IKabs(evalcond[4])
4705 > IKFAST_EVALCOND_THRESH)
4706 {
4707 continue;
4708 }
4709 }
4710
4711 rotationfunction0(
4712 solutions);
4713 }
4714 }
4715 }
4716 }
4717 }
4718 } while (0);
4719 if (bgotonextstatement)
4720 {
4721 bool bgotonextstatement = true;
4722 do
4723 {
4724 IkReal x756 = (px * sj4);
4725 IkReal x757 = (cj4 * py);
4726 evalcond[0]
4727 = ((-3.14159265358979)
4728 + (IKfmod(
4729 ((3.14159265358979)
4730 + (IKabs(
4731 ((-3.14159265358979)
4732 + j8)))),
4733 6.28318530717959)));
4734 evalcond[1]
4735 = ((0.39655)
4736 + (((0.0765) * sj9))
4737 + (((0.32595) * cj9))
4738 + (((-1.0) * (1.0) * pp)));
4739 evalcond[2]
4740 = ((((-1.0) * x757)) + x756);
4741 evalcond[3]
4742 = ((((-1.0) * x756)) + x757);
4743 if (IKabs(evalcond[0])
4744 < 0.0000010000000000
4745 && IKabs(evalcond[1])
4746 < 0.0000010000000000
4747 && IKabs(evalcond[2])
4748 < 0.0000010000000000
4749 && IKabs(evalcond[3])
4750 < 0.0000010000000000)
4751 {
4752 bgotonextstatement = false;
4753 {
4754 IkReal j6eval[2];
4755 sj8 = 0;
4756 cj8 = -1.0;
4757 j8 = 3.14159265358979;
4758 IkReal x758 = py * py;
4759 IkReal x759 = cj4 * cj4;
4760 IkReal x760
4761 = ((pz * pz)
4762 + ((x759 * (px * px)))
4763 + (((2.0) * cj4 * px * py
4764 * sj4))
4765 + x758
4766 + (((-1.0) * x758
4767 * x759)));
4768 j6eval[0] = x760;
4769 j6eval[1] = IKsign(x760);
4770 if (IKabs(j6eval[0])
4771 < 0.0000010000000000
4772 || IKabs(j6eval[1])
4773 < 0.0000010000000000)
4774 {
4775 {
4776 IkReal j6eval[2];
4777 sj8 = 0;
4778 cj8 = -1.0;
4779 j8 = 3.14159265358979;
4780 IkReal x761 = (cj4 * px);
4781 IkReal x762 = (cj9 * pz);
4782 IkReal x763 = (py * sj4);
4783 IkReal x764 = (pz * sj9);
4784 IkReal x765
4785 = (cj4 * px * sj9);
4786 IkReal x766
4787 = (py * sj4 * sj9);
4788 IkReal x767
4789 = ((0.045) * x761);
4790 IkReal x768
4791 = ((0.045) * x763);
4792 j6eval[0]
4793 = (((cj9 * x761))
4794 + (((-1.0)
4795 * (12.2222222222222)
4796 * pz))
4797 + (((-1.0) * x764))
4798 + (((-6.66666666666667)
4799 * x765))
4800 + (((-1.0) * x763))
4801 + (((-6.66666666666667)
4802 * x766))
4803 + ((cj9 * x763))
4804 + (((-6.66666666666667)
4805 * x762))
4806 + (((-1.0) * x761)));
4807 j6eval[1] = IKsign(
4808 ((((-0.3) * x766))
4809 + (((-1.0) * x767))
4810 + (((-1.0) * (0.55)
4811 * pz))
4812 + ((cj9 * x768))
4813 + (((-0.3) * x765))
4814 + (((-1.0) * x768))
4815 + (((-0.3) * x762))
4816 + (((-0.045) * x764))
4817 + ((cj9 * x767))));
4818 if (IKabs(j6eval[0])
4819 < 0.0000010000000000
4820 || IKabs(j6eval[1])
4821 < 0.0000010000000000)
4822 {
4823 {
4824 IkReal j6eval[2];
4825 sj8 = 0;
4826 cj8 = -1.0;
4827 j8 = 3.14159265358979;
4828 IkReal x769
4829 = (cj4 * px);
4830 IkReal x770
4831 = (cj9 * pz);
4832 IkReal x771 = (pp * pz);
4833 IkReal x772
4834 = (py * sj4);
4835 IkReal x773
4836 = (cj4 * cj9 * px);
4837 IkReal x774
4838 = (cj4 * pp * px);
4839 IkReal x775
4840 = (cj9 * py * sj4);
4841 IkReal x776
4842 = (pp * py * sj4);
4843 j6eval[0]
4844 = ((((-1.0) * x775))
4845 + (((-1.0)
4846 * (13.9482024812098)
4847 * pz))
4848 + (((-2.92556370551481)
4849 * x772))
4850 + (((-12.2222222222222)
4851 * x770))
4852 + (((-5.4333061668025)
4853 * x776))
4854 + (((-1.0)
4855 * x773))
4856 + (((-5.4333061668025)
4857 * x774))
4858 + (((-2.92556370551481)
4859 * x769))
4860 + (((36.2220411120167)
4861 * x771)));
4862 j6eval[1] = IKsign((
4863 (((-0.588235294117647)
4864 * x776))
4865 + (((-0.108264705882353)
4866 * x773))
4867 + (((3.92156862745098)
4868 * x771))
4869 + (((-0.316735294117647)
4870 * x772))
4871 + (((-1.0)
4872 * (1.51009803921569)
4873 * pz))
4874 + (((-0.316735294117647)
4875 * x769))
4876 + (((-1.32323529411765)
4877 * x770))
4878 + (((-0.588235294117647)
4879 * x774))
4880 + (((-0.108264705882353)
4881 * x775))));
4882 if (IKabs(j6eval[0])
4883 < 0.0000010000000000
4884 || IKabs(j6eval[1])
4885 < 0.0000010000000000)
4886 {
4887 {
4888 IkReal evalcond[1];
4889 bool
4890 bgotonextstatement
4891 = true;
4892 do
4893 {
4894 evalcond[0]
4895 = ((IKabs((
4896 (-3.14159265358979)
4897 + (IKfmod(
4898 ((3.14159265358979)
4899 + j9),
4900 6.28318530717959)))))
4901 + (IKabs(
4902 pz)));
4903 if (IKabs(evalcond
4904 [0])
4905 < 0.0000010000000000)
4906 {
4907 bgotonextstatement
4908 = false;
4909 {
4910 IkReal
4911 j6eval[1];
4912 IkReal x777
4913 = ((1.0)
4914 * py);
4915 sj8 = 0;
4916 cj8 = -1.0;
4917 j8 = 3.14159265358979;
4918 pz = 0;
4919 j9 = 0;
4920 sj9 = 0;
4921 cj9 = 1.0;
4922 pp
4923 = ((py
4924 * py)
4925 + (px
4926 * px));
4927 npx
4928 = (((py
4929 * r10))
4930 + ((px
4931 * r00)));
4932 npy
4933 = (((py
4934 * r11))
4935 + ((px
4936 * r01)));
4937 npz
4938 = (((px
4939 * r02))
4940 + ((py
4941 * r12)));
4942 rxp0_0
4943 = ((-1.0)
4944 * r20
4945 * x777);
4946 rxp0_1
4947 = (px
4948 * r20);
4949 rxp1_0
4950 = ((-1.0)
4951 * r21
4952 * x777);
4953 rxp1_1
4954 = (px
4955 * r21);
4956 rxp2_0
4957 = ((-1.0)
4958 * r22
4959 * x777);
4960 rxp2_1
4961 = (px
4962 * r22);
4963 j6eval[0]
4964 = ((((-1.0)
4965 * (1.0)
4966 * cj4
4967 * px))
4968 + (((-1.0)
4969 * (1.0)
4970 * py
4971 * sj4)));
4972 if (IKabs(
4973 j6eval
4974 [0])
4975 < 0.0000010000000000)
4976 {
4977 {
4978 IkReal j6eval
4979 [1];
4980 IkReal
4981 x778
4982 = ((1.0)
4983 * py);
4984 sj8 = 0;
4985 cj8 = -1.0;
4986 j8 = 3.14159265358979;
4987 pz = 0;
4988 j9 = 0;
4989 sj9 = 0;
4990 cj9 = 1.0;
4991 pp
4992 = ((py
4993 * py)
4994 + (px
4995 * px));
4996 npx
4997 = (((py
4998 * r10))
4999 + ((px
5000 * r00)));
5001 npy
5002 = (((py
5003 * r11))
5004 + ((px
5005 * r01)));
5006 npz
5007 = (((px
5008 * r02))
5009 + ((py
5010 * r12)));
5011 rxp0_0
5012 = ((-1.0)
5013 * r20
5014 * x778);
5015 rxp0_1
5016 = (px
5017 * r20);
5018 rxp1_0
5019 = ((-1.0)
5020 * r21
5021 * x778);
5022 rxp1_1
5023 = (px
5024 * r21);
5025 rxp2_0
5026 = ((-1.0)
5027 * r22
5028 * x778);
5029 rxp2_1
5030 = (px
5031 * r22);
5032 j6eval[0]
5033 = ((-1.0)
5034 + (((-1.0)
5035 * (1.3840830449827)
5036 * (px
5037 * px)))
5038 + (((-1.0)
5039 * (1.3840830449827)
5040 * (py
5041 * py))));
5042 if (IKabs(
5043 j6eval
5044 [0])
5045 < 0.0000010000000000)
5046 {
5047 {
5048 IkReal j6eval
5049 [2];
5050 IkReal
5051 x779
5052 = ((1.0)
5053 * py);
5054 sj8 = 0;
5055 cj8 = -1.0;
5056 j8 = 3.14159265358979;
5057 pz = 0;
5058 j9 = 0;
5059 sj9 = 0;
5060 cj9 = 1.0;
5061 pp
5062 = ((py
5063 * py)
5064 + (px
5065 * px));
5066 npx
5067 = (((py
5068 * r10))
5069 + ((px
5070 * r00)));
5071 npy
5072 = (((py
5073 * r11))
5074 + ((px
5075 * r01)));
5076 npz
5077 = (((px
5078 * r02))
5079 + ((py
5080 * r12)));
5081 rxp0_0
5082 = ((-1.0)
5083 * r20
5084 * x779);
5085 rxp0_1
5086 = (px
5087 * r20);
5088 rxp1_0
5089 = ((-1.0)
5090 * r21
5091 * x779);
5092 rxp1_1
5093 = (px
5094 * r21);
5095 rxp2_0
5096 = ((-1.0)
5097 * r22
5098 * x779);
5099 rxp2_1
5100 = (px
5101 * r22);
5102 IkReal
5103 x780
5104 = (cj4
5105 * px);
5106 IkReal
5107 x781
5108 = (py
5109 * sj4);
5110 j6eval
5111 [0]
5112 = (x780
5113 + x781);
5114 j6eval
5115 [1]
5116 = ((((-1.0)
5117 * (1.3840830449827)
5118 * cj4
5119 * (px
5120 * px
5121 * px)))
5122 + (((-1.0)
5123 * x780))
5124 + (((-1.3840830449827)
5125 * x780
5126 * (py
5127 * py)))
5128 + (((-1.0)
5129 * x781))
5130 + (((-1.0)
5131 * (1.3840830449827)
5132 * sj4
5133 * (py
5134 * py
5135 * py)))
5136 + (((-1.3840830449827)
5137 * x781
5138 * (px
5139 * px))));
5140 if (IKabs(
5141 j6eval
5142 [0])
5143 < 0.0000010000000000
5144 || IKabs(
5145 j6eval
5146 [1])
5147 < 0.0000010000000000)
5148 {
5149 {
5150 IkReal evalcond
5151 [4];
5152 bool
5153 bgotonextstatement
5154 = true;
5155 do
5156 {
5157 evalcond
5158 [0]
5159 = ((IKabs(
5160 py))
5161 + (IKabs(
5162 px)));
5163 evalcond
5164 [1]
5165 = -0.85;
5166 evalcond
5167 [2]
5168 = 0;
5169 evalcond
5170 [3]
5171 = -0.2125;
5172 if (IKabs(
5173 evalcond
5174 [0])
5175 < 0.0000010000000000
5176 && IKabs(
5177 evalcond
5178 [1])
5179 < 0.0000010000000000
5180 && IKabs(
5181 evalcond
5182 [2])
5183 < 0.0000010000000000
5184 && IKabs(
5185 evalcond
5186 [3])
5187 < 0.0000010000000000)
5188 {
5189 bgotonextstatement
5190 = false;
5191 {
5192 IkReal j6array
5193 [2],
5194 cj6array
5195 [2],
5196 sj6array
5197 [2];
5198 bool j6valid
5199 [2]
5200 = {false};
5201 _nj6
5202 = 2;
5203 j6array
5204 [0]
5205 = 0.148889947609497;
5206 sj6array
5207 [0]
5208 = IKsin(
5209 j6array
5210 [0]);
5211 cj6array
5212 [0]
5213 = IKcos(
5214 j6array
5215 [0]);
5216 j6array
5217 [1]
5218 = 3.29048260119929;
5219 sj6array
5220 [1]
5221 = IKsin(
5222 j6array
5223 [1]);
5224 cj6array
5225 [1]
5226 = IKcos(
5227 j6array
5228 [1]);
5229 if (j6array
5230 [0]
5231 > IKPI)
5232 {
5233 j6array
5234 [0]
5235 -= IK2PI;
5236 }
5237 else if (
5238 j6array
5239 [0]
5240 < -IKPI)
5241 {
5242 j6array
5243 [0]
5244 += IK2PI;
5245 }
5246 j6valid
5247 [0]
5248 = true;
5249 if (j6array
5250 [1]
5251 > IKPI)
5252 {
5253 j6array
5254 [1]
5255 -= IK2PI;
5256 }
5257 else if (
5258 j6array
5259 [1]
5260 < -IKPI)
5261 {
5262 j6array
5263 [1]
5264 += IK2PI;
5265 }
5266 j6valid
5267 [1]
5268 = true;
5269 for (
5270 int ij6
5271 = 0;
5272 ij6
5273 < 2;
5274 ++ij6)
5275 {
5276 if (!j6valid
5277 [ij6])
5278 {
5279 continue;
5280 }
5281 _ij6[0]
5282 = ij6;
5283 _ij6[1]
5284 = -1;
5285 for (
5286 int iij6
5287 = ij6
5288 + 1;
5289 iij6
5290 < 2;
5291 ++iij6)
5292 {
5293 if (j6valid
5294 [iij6]
5295 && IKabs(
5296 cj6array
5297 [ij6]
5298 - cj6array
5299 [iij6])
5300 < IKFAST_SOLUTION_THRESH
5301 && IKabs(
5302 sj6array
5303 [ij6]
5304 - sj6array
5305 [iij6])
5306 < IKFAST_SOLUTION_THRESH)
5307 {
5308 j6valid
5309 [iij6]
5310 = false;
5311 _ij6[1]
5312 = iij6;
5313 break;
5314 }
5315 }
5316 j6 = j6array
5317 [ij6];
5318 cj6 = cj6array
5319 [ij6];
5320 sj6 = sj6array
5321 [ij6];
5322 {
5323 IkReal evalcond
5324 [1];
5325 evalcond
5326 [0]
5327 = ((0.85)
5328 * (IKsin(
5329 j6)));
5330 if (IKabs(
5331 evalcond
5332 [0])
5333 > IKFAST_EVALCOND_THRESH)
5334 {
5335 continue;
5336 }
5337 }
5338
5339 rotationfunction0(
5340 solutions);
5341 }
5342 }
5343 }
5344 } while (
5345 0);
5346 if (bgotonextstatement)
5347 {
5348 bool
5349 bgotonextstatement
5350 = true;
5351 do
5352 {
5353 evalcond
5354 [0]
5355 = ((IKabs((
5356 (-3.14159265358979)
5357 + (IKfmod(
5358 ((3.14159265358979)
5359 + j4),
5360 6.28318530717959)))))
5361 + (IKabs(
5362 px)));
5363 evalcond
5364 [1]
5365 = -0.85;
5366 evalcond
5367 [2]
5368 = 0;
5369 evalcond
5370 [3]
5371 = ((-0.2125)
5372 + (((-1.0)
5373 * (1.0)
5374 * (py
5375 * py))));
5376 if (IKabs(
5377 evalcond
5378 [0])
5379 < 0.0000010000000000
5380 && IKabs(
5381 evalcond
5382 [1])
5383 < 0.0000010000000000
5384 && IKabs(
5385 evalcond
5386 [2])
5387 < 0.0000010000000000
5388 && IKabs(
5389 evalcond
5390 [3])
5391 < 0.0000010000000000)
5392 {
5393 bgotonextstatement
5394 = false;
5395 {
5396 IkReal j6eval
5397 [1];
5398 IkReal
5399 x782
5400 = ((1.0)
5401 * py);
5402 sj8 = 0;
5403 cj8 = -1.0;
5404 j8 = 3.14159265358979;
5405 pz = 0;
5406 j9 = 0;
5407 sj9 = 0;
5408 cj9 = 1.0;
5409 pp = py
5410 * py;
5411 npx
5412 = (py
5413 * r10);
5414 npy
5415 = (py
5416 * r11);
5417 npz
5418 = (py
5419 * r12);
5420 rxp0_0
5421 = ((-1.0)
5422 * r20
5423 * x782);
5424 rxp0_1
5425 = 0;
5426 rxp1_0
5427 = ((-1.0)
5428 * r21
5429 * x782);
5430 rxp1_1
5431 = 0;
5432 rxp2_0
5433 = ((-1.0)
5434 * r22
5435 * x782);
5436 rxp2_1
5437 = 0;
5438 px = 0;
5439 j4 = 0;
5440 sj4 = 0;
5441 cj4 = 1.0;
5442 rxp0_2
5443 = (py
5444 * r00);
5445 rxp1_2
5446 = (py
5447 * r01);
5448 rxp2_2
5449 = (py
5450 * r02);
5451 j6eval
5452 [0]
5453 = ((1.0)
5454 + (((1.91568587540858)
5455 * (py
5456 * py
5457 * py
5458 * py)))
5459 + (((-1.0)
5460 * (2.64633970947792)
5461 * (py
5462 * py))));
5463 if (IKabs(
5464 j6eval
5465 [0])
5466 < 0.0000010000000000)
5467 {
5468 continue; // no branches [j6]
5469 }
5470 else
5471 {
5472 {
5473 IkReal j6array
5474 [2],
5475 cj6array
5476 [2],
5477 sj6array
5478 [2];
5479 bool j6valid
5480 [2]
5481 = {false};
5482 _nj6
5483 = 2;
5484 IkReal
5485 x783
5486 = py
5487 * py;
5488 CheckValue<IkReal> x785 = IKatan2WithCheck(
5489 IkReal((
5490 (-0.425)
5491 + (((-0.588235294117647)
5492 * x783)))),
5493 ((2.83333333333333)
5494 + (((-3.92156862745098)
5495 * x783))),
5496 IKFAST_ATAN2_MAGTHRESH);
5497 if (!x785.valid)
5498 {
5499 continue;
5500 }
5501 IkReal
5502 x784
5503 = ((-1.0)
5504 * (x785.value));
5505 j6array
5506 [0]
5507 = x784;
5508 sj6array
5509 [0]
5510 = IKsin(
5511 j6array
5512 [0]);
5513 cj6array
5514 [0]
5515 = IKcos(
5516 j6array
5517 [0]);
5518 j6array
5519 [1]
5520 = ((3.14159265358979)
5521 + x784);
5522 sj6array
5523 [1]
5524 = IKsin(
5525 j6array
5526 [1]);
5527 cj6array
5528 [1]
5529 = IKcos(
5530 j6array
5531 [1]);
5532 if (j6array
5533 [0]
5534 > IKPI)
5535 {
5536 j6array
5537 [0]
5538 -= IK2PI;
5539 }
5540 else if (
5541 j6array
5542 [0]
5543 < -IKPI)
5544 {
5545 j6array
5546 [0]
5547 += IK2PI;
5548 }
5549 j6valid
5550 [0]
5551 = true;
5552 if (j6array
5553 [1]
5554 > IKPI)
5555 {
5556 j6array
5557 [1]
5558 -= IK2PI;
5559 }
5560 else if (
5561 j6array
5562 [1]
5563 < -IKPI)
5564 {
5565 j6array
5566 [1]
5567 += IK2PI;
5568 }
5569 j6valid
5570 [1]
5571 = true;
5572 for (
5573 int ij6
5574 = 0;
5575 ij6
5576 < 2;
5577 ++ij6)
5578 {
5579 if (!j6valid
5580 [ij6])
5581 {
5582 continue;
5583 }
5584 _ij6[0]
5585 = ij6;
5586 _ij6[1]
5587 = -1;
5588 for (
5589 int iij6
5590 = ij6
5591 + 1;
5592 iij6
5593 < 2;
5594 ++iij6)
5595 {
5596 if (j6valid
5597 [iij6]
5598 && IKabs(
5599 cj6array
5600 [ij6]
5601 - cj6array
5602 [iij6])
5603 < IKFAST_SOLUTION_THRESH
5604 && IKabs(
5605 sj6array
5606 [ij6]
5607 - sj6array
5608 [iij6])
5609 < IKFAST_SOLUTION_THRESH)
5610 {
5611 j6valid
5612 [iij6]
5613 = false;
5614 _ij6[1]
5615 = iij6;
5616 break;
5617 }
5618 }
5619 j6 = j6array
5620 [ij6];
5621 cj6 = cj6array
5622 [ij6];
5623 sj6 = sj6array
5624 [ij6];
5625 {
5626 IkReal evalcond
5627 [1];
5628 evalcond
5629 [0]
5630 = ((0.85)
5631 * (IKsin(
5632 j6)));
5633 if (IKabs(
5634 evalcond
5635 [0])
5636 > IKFAST_EVALCOND_THRESH)
5637 {
5638 continue;
5639 }
5640 }
5641
5642 rotationfunction0(
5643 solutions);
5644 }
5645 }
5646 }
5647 }
5648 }
5649 } while (
5650 0);
5651 if (bgotonextstatement)
5652 {
5653 bool
5654 bgotonextstatement
5655 = true;
5656 do
5657 {
5658 evalcond
5659 [0]
5660 = ((IKabs((
5661 (-3.14159265358979)
5662 + (IKfmod(
5663 j4,
5664 6.28318530717959)))))
5665 + (IKabs(
5666 px)));
5667 evalcond
5668 [1]
5669 = -0.85;
5670 evalcond
5671 [2]
5672 = 0;
5673 evalcond
5674 [3]
5675 = ((-0.2125)
5676 + (((-1.0)
5677 * (1.0)
5678 * (py
5679 * py))));
5680 if (IKabs(
5681 evalcond
5682 [0])
5683 < 0.0000010000000000
5684 && IKabs(
5685 evalcond
5686 [1])
5687 < 0.0000010000000000
5688 && IKabs(
5689 evalcond
5690 [2])
5691 < 0.0000010000000000
5692 && IKabs(
5693 evalcond
5694 [3])
5695 < 0.0000010000000000)
5696 {
5697 bgotonextstatement
5698 = false;
5699 {
5700 IkReal j6eval
5701 [1];
5702 IkReal
5703 x786
5704 = ((1.0)
5705 * py);
5706 sj8 = 0;
5707 cj8 = -1.0;
5708 j8 = 3.14159265358979;
5709 pz = 0;
5710 j9 = 0;
5711 sj9 = 0;
5712 cj9 = 1.0;
5713 pp = py
5714 * py;
5715 npx
5716 = (py
5717 * r10);
5718 npy
5719 = (py
5720 * r11);
5721 npz
5722 = (py
5723 * r12);
5724 rxp0_0
5725 = ((-1.0)
5726 * r20
5727 * x786);
5728 rxp0_1
5729 = 0;
5730 rxp1_0
5731 = ((-1.0)
5732 * r21
5733 * x786);
5734 rxp1_1
5735 = 0;
5736 rxp2_0
5737 = ((-1.0)
5738 * r22
5739 * x786);
5740 rxp2_1
5741 = 0;
5742 px = 0;
5743 j4 = 3.14159265358979;
5744 sj4 = 0;
5745 cj4 = -1.0;
5746 rxp0_2
5747 = (py
5748 * r00);
5749 rxp1_2
5750 = (py
5751 * r01);
5752 rxp2_2
5753 = (py
5754 * r02);
5755 j6eval
5756 [0]
5757 = ((1.0)
5758 + (((1.91568587540858)
5759 * (py
5760 * py
5761 * py
5762 * py)))
5763 + (((-1.0)
5764 * (2.64633970947792)
5765 * (py
5766 * py))));
5767 if (IKabs(
5768 j6eval
5769 [0])
5770 < 0.0000010000000000)
5771 {
5772 continue; // no branches [j6]
5773 }
5774 else
5775 {
5776 {
5777 IkReal j6array
5778 [2],
5779 cj6array
5780 [2],
5781 sj6array
5782 [2];
5783 bool j6valid
5784 [2]
5785 = {false};
5786 _nj6
5787 = 2;
5788 IkReal
5789 x787
5790 = py
5791 * py;
5792 CheckValue<IkReal> x789 = IKatan2WithCheck(
5793 IkReal((
5794 (-0.425)
5795 + (((-0.588235294117647)
5796 * x787)))),
5797 ((2.83333333333333)
5798 + (((-3.92156862745098)
5799 * x787))),
5800 IKFAST_ATAN2_MAGTHRESH);
5801 if (!x789.valid)
5802 {
5803 continue;
5804 }
5805 IkReal
5806 x788
5807 = ((-1.0)
5808 * (x789.value));
5809 j6array
5810 [0]
5811 = x788;
5812 sj6array
5813 [0]
5814 = IKsin(
5815 j6array
5816 [0]);
5817 cj6array
5818 [0]
5819 = IKcos(
5820 j6array
5821 [0]);
5822 j6array
5823 [1]
5824 = ((3.14159265358979)
5825 + x788);
5826 sj6array
5827 [1]
5828 = IKsin(
5829 j6array
5830 [1]);
5831 cj6array
5832 [1]
5833 = IKcos(
5834 j6array
5835 [1]);
5836 if (j6array
5837 [0]
5838 > IKPI)
5839 {
5840 j6array
5841 [0]
5842 -= IK2PI;
5843 }
5844 else if (
5845 j6array
5846 [0]
5847 < -IKPI)
5848 {
5849 j6array
5850 [0]
5851 += IK2PI;
5852 }
5853 j6valid
5854 [0]
5855 = true;
5856 if (j6array
5857 [1]
5858 > IKPI)
5859 {
5860 j6array
5861 [1]
5862 -= IK2PI;
5863 }
5864 else if (
5865 j6array
5866 [1]
5867 < -IKPI)
5868 {
5869 j6array
5870 [1]
5871 += IK2PI;
5872 }
5873 j6valid
5874 [1]
5875 = true;
5876 for (
5877 int ij6
5878 = 0;
5879 ij6
5880 < 2;
5881 ++ij6)
5882 {
5883 if (!j6valid
5884 [ij6])
5885 {
5886 continue;
5887 }
5888 _ij6[0]
5889 = ij6;
5890 _ij6[1]
5891 = -1;
5892 for (
5893 int iij6
5894 = ij6
5895 + 1;
5896 iij6
5897 < 2;
5898 ++iij6)
5899 {
5900 if (j6valid
5901 [iij6]
5902 && IKabs(
5903 cj6array
5904 [ij6]
5905 - cj6array
5906 [iij6])
5907 < IKFAST_SOLUTION_THRESH
5908 && IKabs(
5909 sj6array
5910 [ij6]
5911 - sj6array
5912 [iij6])
5913 < IKFAST_SOLUTION_THRESH)
5914 {
5915 j6valid
5916 [iij6]
5917 = false;
5918 _ij6[1]
5919 = iij6;
5920 break;
5921 }
5922 }
5923 j6 = j6array
5924 [ij6];
5925 cj6 = cj6array
5926 [ij6];
5927 sj6 = sj6array
5928 [ij6];
5929 {
5930 IkReal evalcond
5931 [1];
5932 evalcond
5933 [0]
5934 = ((0.85)
5935 * (IKsin(
5936 j6)));
5937 if (IKabs(
5938 evalcond
5939 [0])
5940 > IKFAST_EVALCOND_THRESH)
5941 {
5942 continue;
5943 }
5944 }
5945
5946 rotationfunction0(
5947 solutions);
5948 }
5949 }
5950 }
5951 }
5952 }
5953 } while (
5954 0);
5955 if (bgotonextstatement)
5956 {
5957 bool
5958 bgotonextstatement
5959 = true;
5960 do
5961 {
5962 evalcond
5963 [0]
5964 = ((IKabs((
5965 (-3.14159265358979)
5966 + (IKfmod(
5967 ((1.5707963267949)
5968 + j4),
5969 6.28318530717959)))))
5970 + (IKabs(
5971 py)));
5972 evalcond
5973 [1]
5974 = -0.85;
5975 evalcond
5976 [2]
5977 = 0;
5978 evalcond
5979 [3]
5980 = ((-0.2125)
5981 + (((-1.0)
5982 * (1.0)
5983 * (px
5984 * px))));
5985 if (IKabs(
5986 evalcond
5987 [0])
5988 < 0.0000010000000000
5989 && IKabs(
5990 evalcond
5991 [1])
5992 < 0.0000010000000000
5993 && IKabs(
5994 evalcond
5995 [2])
5996 < 0.0000010000000000
5997 && IKabs(
5998 evalcond
5999 [3])
6000 < 0.0000010000000000)
6001 {
6002 bgotonextstatement
6003 = false;
6004 {
6005 IkReal j6eval
6006 [1];
6007 IkReal
6008 x790
6009 = ((1.0)
6010 * px);
6011 sj8 = 0;
6012 cj8 = -1.0;
6013 j8 = 3.14159265358979;
6014 pz = 0;
6015 j9 = 0;
6016 sj9 = 0;
6017 cj9 = 1.0;
6018 pp = px
6019 * px;
6020 npx
6021 = (px
6022 * r00);
6023 npy
6024 = (px
6025 * r01);
6026 npz
6027 = (px
6028 * r02);
6029 rxp0_0
6030 = 0;
6031 rxp0_1
6032 = (px
6033 * r20);
6034 rxp1_0
6035 = 0;
6036 rxp1_1
6037 = (px
6038 * r21);
6039 rxp2_0
6040 = 0;
6041 rxp2_1
6042 = (px
6043 * r22);
6044 py = 0;
6045 j4 = 1.5707963267949;
6046 sj4 = 1.0;
6047 cj4 = 0;
6048 rxp0_2
6049 = ((-1.0)
6050 * r10
6051 * x790);
6052 rxp1_2
6053 = ((-1.0)
6054 * r11
6055 * x790);
6056 rxp2_2
6057 = ((-1.0)
6058 * r12
6059 * x790);
6060 j6eval
6061 [0]
6062 = ((1.0)
6063 + (((1.91568587540858)
6064 * (px
6065 * px
6066 * px
6067 * px)))
6068 + (((-1.0)
6069 * (2.64633970947792)
6070 * (px
6071 * px))));
6072 if (IKabs(
6073 j6eval
6074 [0])
6075 < 0.0000010000000000)
6076 {
6077 continue; // no branches [j6]
6078 }
6079 else
6080 {
6081 {
6082 IkReal j6array
6083 [2],
6084 cj6array
6085 [2],
6086 sj6array
6087 [2];
6088 bool j6valid
6089 [2]
6090 = {false};
6091 _nj6
6092 = 2;
6093 IkReal
6094 x791
6095 = px
6096 * px;
6097 CheckValue<IkReal> x793 = IKatan2WithCheck(
6098 IkReal((
6099 (-0.425)
6100 + (((-0.588235294117647)
6101 * x791)))),
6102 ((2.83333333333333)
6103 + (((-3.92156862745098)
6104 * x791))),
6105 IKFAST_ATAN2_MAGTHRESH);
6106 if (!x793.valid)
6107 {
6108 continue;
6109 }
6110 IkReal
6111 x792
6112 = ((-1.0)
6113 * (x793.value));
6114 j6array
6115 [0]
6116 = x792;
6117 sj6array
6118 [0]
6119 = IKsin(
6120 j6array
6121 [0]);
6122 cj6array
6123 [0]
6124 = IKcos(
6125 j6array
6126 [0]);
6127 j6array
6128 [1]
6129 = ((3.14159265358979)
6130 + x792);
6131 sj6array
6132 [1]
6133 = IKsin(
6134 j6array
6135 [1]);
6136 cj6array
6137 [1]
6138 = IKcos(
6139 j6array
6140 [1]);
6141 if (j6array
6142 [0]
6143 > IKPI)
6144 {
6145 j6array
6146 [0]
6147 -= IK2PI;
6148 }
6149 else if (
6150 j6array
6151 [0]
6152 < -IKPI)
6153 {
6154 j6array
6155 [0]
6156 += IK2PI;
6157 }
6158 j6valid
6159 [0]
6160 = true;
6161 if (j6array
6162 [1]
6163 > IKPI)
6164 {
6165 j6array
6166 [1]
6167 -= IK2PI;
6168 }
6169 else if (
6170 j6array
6171 [1]
6172 < -IKPI)
6173 {
6174 j6array
6175 [1]
6176 += IK2PI;
6177 }
6178 j6valid
6179 [1]
6180 = true;
6181 for (
6182 int ij6
6183 = 0;
6184 ij6
6185 < 2;
6186 ++ij6)
6187 {
6188 if (!j6valid
6189 [ij6])
6190 {
6191 continue;
6192 }
6193 _ij6[0]
6194 = ij6;
6195 _ij6[1]
6196 = -1;
6197 for (
6198 int iij6
6199 = ij6
6200 + 1;
6201 iij6
6202 < 2;
6203 ++iij6)
6204 {
6205 if (j6valid
6206 [iij6]
6207 && IKabs(
6208 cj6array
6209 [ij6]
6210 - cj6array
6211 [iij6])
6212 < IKFAST_SOLUTION_THRESH
6213 && IKabs(
6214 sj6array
6215 [ij6]
6216 - sj6array
6217 [iij6])
6218 < IKFAST_SOLUTION_THRESH)
6219 {
6220 j6valid
6221 [iij6]
6222 = false;
6223 _ij6[1]
6224 = iij6;
6225 break;
6226 }
6227 }
6228 j6 = j6array
6229 [ij6];
6230 cj6 = cj6array
6231 [ij6];
6232 sj6 = sj6array
6233 [ij6];
6234 {
6235 IkReal evalcond
6236 [1];
6237 evalcond
6238 [0]
6239 = ((0.85)
6240 * (IKsin(
6241 j6)));
6242 if (IKabs(
6243 evalcond
6244 [0])
6245 > IKFAST_EVALCOND_THRESH)
6246 {
6247 continue;
6248 }
6249 }
6250
6251 rotationfunction0(
6252 solutions);
6253 }
6254 }
6255 }
6256 }
6257 }
6258 } while (
6259 0);
6260 if (bgotonextstatement)
6261 {
6262 bool
6263 bgotonextstatement
6264 = true;
6265 do
6266 {
6267 evalcond
6268 [0]
6269 = ((IKabs(
6270 py))
6271 + (IKabs((
6272 (-3.14159265358979)
6273 + (IKfmod(
6274 ((4.71238898038469)
6275 + j4),
6276 6.28318530717959))))));
6277 evalcond
6278 [1]
6279 = -0.85;
6280 evalcond
6281 [2]
6282 = 0;
6283 evalcond
6284 [3]
6285 = ((-0.2125)
6286 + (((-1.0)
6287 * (1.0)
6288 * (px
6289 * px))));
6290 if (IKabs(
6291 evalcond
6292 [0])
6293 < 0.0000010000000000
6294 && IKabs(
6295 evalcond
6296 [1])
6297 < 0.0000010000000000
6298 && IKabs(
6299 evalcond
6300 [2])
6301 < 0.0000010000000000
6302 && IKabs(
6303 evalcond
6304 [3])
6305 < 0.0000010000000000)
6306 {
6307 bgotonextstatement
6308 = false;
6309 {
6310 IkReal j6eval
6311 [1];
6312 IkReal
6313 x794
6314 = ((1.0)
6315 * px);
6316 sj8 = 0;
6317 cj8 = -1.0;
6318 j8 = 3.14159265358979;
6319 pz = 0;
6320 j9 = 0;
6321 sj9 = 0;
6322 cj9 = 1.0;
6323 pp = px
6324 * px;
6325 npx
6326 = (px
6327 * r00);
6328 npy
6329 = (px
6330 * r01);
6331 npz
6332 = (px
6333 * r02);
6334 rxp0_0
6335 = 0;
6336 rxp0_1
6337 = (px
6338 * r20);
6339 rxp1_0
6340 = 0;
6341 rxp1_1
6342 = (px
6343 * r21);
6344 rxp2_0
6345 = 0;
6346 rxp2_1
6347 = (px
6348 * r22);
6349 py = 0;
6350 j4 = -1.5707963267949;
6351 sj4 = -1.0;
6352 cj4 = 0;
6353 rxp0_2
6354 = ((-1.0)
6355 * r10
6356 * x794);
6357 rxp1_2
6358 = ((-1.0)
6359 * r11
6360 * x794);
6361 rxp2_2
6362 = ((-1.0)
6363 * r12
6364 * x794);
6365 j6eval
6366 [0]
6367 = ((1.0)
6368 + (((1.91568587540858)
6369 * (px
6370 * px
6371 * px
6372 * px)))
6373 + (((-1.0)
6374 * (2.64633970947792)
6375 * (px
6376 * px))));
6377 if (IKabs(
6378 j6eval
6379 [0])
6380 < 0.0000010000000000)
6381 {
6382 continue; // no branches [j6]
6383 }
6384 else
6385 {
6386 {
6387 IkReal j6array
6388 [2],
6389 cj6array
6390 [2],
6391 sj6array
6392 [2];
6393 bool j6valid
6394 [2]
6395 = {false};
6396 _nj6
6397 = 2;
6398 IkReal
6399 x795
6400 = px
6401 * px;
6402 CheckValue<IkReal> x797 = IKatan2WithCheck(
6403 IkReal((
6404 (-0.425)
6405 + (((-0.588235294117647)
6406 * x795)))),
6407 ((2.83333333333333)
6408 + (((-3.92156862745098)
6409 * x795))),
6410 IKFAST_ATAN2_MAGTHRESH);
6411 if (!x797.valid)
6412 {
6413 continue;
6414 }
6415 IkReal
6416 x796
6417 = ((-1.0)
6418 * (x797.value));
6419 j6array
6420 [0]
6421 = x796;
6422 sj6array
6423 [0]
6424 = IKsin(
6425 j6array
6426 [0]);
6427 cj6array
6428 [0]
6429 = IKcos(
6430 j6array
6431 [0]);
6432 j6array
6433 [1]
6434 = ((3.14159265358979)
6435 + x796);
6436 sj6array
6437 [1]
6438 = IKsin(
6439 j6array
6440 [1]);
6441 cj6array
6442 [1]
6443 = IKcos(
6444 j6array
6445 [1]);
6446 if (j6array
6447 [0]
6448 > IKPI)
6449 {
6450 j6array
6451 [0]
6452 -= IK2PI;
6453 }
6454 else if (
6455 j6array
6456 [0]
6457 < -IKPI)
6458 {
6459 j6array
6460 [0]
6461 += IK2PI;
6462 }
6463 j6valid
6464 [0]
6465 = true;
6466 if (j6array
6467 [1]
6468 > IKPI)
6469 {
6470 j6array
6471 [1]
6472 -= IK2PI;
6473 }
6474 else if (
6475 j6array
6476 [1]
6477 < -IKPI)
6478 {
6479 j6array
6480 [1]
6481 += IK2PI;
6482 }
6483 j6valid
6484 [1]
6485 = true;
6486 for (
6487 int ij6
6488 = 0;
6489 ij6
6490 < 2;
6491 ++ij6)
6492 {
6493 if (!j6valid
6494 [ij6])
6495 {
6496 continue;
6497 }
6498 _ij6[0]
6499 = ij6;
6500 _ij6[1]
6501 = -1;
6502 for (
6503 int iij6
6504 = ij6
6505 + 1;
6506 iij6
6507 < 2;
6508 ++iij6)
6509 {
6510 if (j6valid
6511 [iij6]
6512 && IKabs(
6513 cj6array
6514 [ij6]
6515 - cj6array
6516 [iij6])
6517 < IKFAST_SOLUTION_THRESH
6518 && IKabs(
6519 sj6array
6520 [ij6]
6521 - sj6array
6522 [iij6])
6523 < IKFAST_SOLUTION_THRESH)
6524 {
6525 j6valid
6526 [iij6]
6527 = false;
6528 _ij6[1]
6529 = iij6;
6530 break;
6531 }
6532 }
6533 j6 = j6array
6534 [ij6];
6535 cj6 = cj6array
6536 [ij6];
6537 sj6 = sj6array
6538 [ij6];
6539 {
6540 IkReal evalcond
6541 [1];
6542 evalcond
6543 [0]
6544 = ((0.85)
6545 * (IKsin(
6546 j6)));
6547 if (IKabs(
6548 evalcond
6549 [0])
6550 > IKFAST_EVALCOND_THRESH)
6551 {
6552 continue;
6553 }
6554 }
6555
6556 rotationfunction0(
6557 solutions);
6558 }
6559 }
6560 }
6561 }
6562 }
6563 } while (
6564 0);
6565 if (bgotonextstatement)
6566 {
6567 bool
6568 bgotonextstatement
6569 = true;
6570 do
6571 {
6572 if (1)
6573 {
6574 bgotonextstatement
6575 = false;
6576 continue; // branch miss [j6]
6577 }
6578 } while (
6579 0);
6580 if (bgotonextstatement)
6581 {
6582 }
6583 }
6584 }
6585 }
6586 }
6587 }
6588 }
6589 }
6590 else
6591 {
6592 {
6593 IkReal j6array
6594 [1],
6595 cj6array
6596 [1],
6597 sj6array
6598 [1];
6599 bool j6valid
6600 [1]
6601 = {false};
6602 _nj6
6603 = 1;
6604 IkReal
6605 x798
6606 = (cj4
6607 * px);
6608 IkReal
6609 x799
6610 = (py
6611 * sj4);
6612 IkReal
6613 x800
6614 = px
6615 * px;
6616 IkReal
6617 x801
6618 = py
6619 * py;
6620 CheckValue<IkReal> x802 = IKPowWithIntegerCheck(
6621 ((((20.0)
6622 * x799))
6623 + (((20.0)
6624 * x798))),
6625 -1);
6626 if (!x802.valid)
6627 {
6628 continue;
6629 }
6630 CheckValue<IkReal> x803 = IKPowWithIntegerCheck(
6631 ((((-8.5)
6632 * x798))
6633 + (((-1.0)
6634 * (11.7647058823529)
6635 * cj4
6636 * (px
6637 * px
6638 * px)))
6639 + (((-11.7647058823529)
6640 * x798
6641 * x801))
6642 + (((-11.7647058823529)
6643 * x799
6644 * x800))
6645 + (((-8.5)
6646 * x799))
6647 + (((-1.0)
6648 * (11.7647058823529)
6649 * sj4
6650 * (py
6651 * py
6652 * py)))),
6653 -1);
6654 if (!x803.valid)
6655 {
6656 continue;
6657 }
6658 if (IKabs((
6659 (17.0)
6660 * (x802.value)))
6661 < IKFAST_ATAN2_MAGTHRESH
6662 && IKabs((
6663 (x803.value)
6664 * (((-48.1666666666667)
6665 + (((66.6666666666667)
6666 * x800))
6667 + (((66.6666666666667)
6668 * x801))))))
6669 < IKFAST_ATAN2_MAGTHRESH
6670 && IKabs(
6671 IKsqr((
6672 (17.0)
6673 * (x802.value)))
6674 + IKsqr((
6675 (x803.value)
6676 * (((-48.1666666666667)
6677 + (((66.6666666666667)
6678 * x800))
6679 + (((66.6666666666667)
6680 * x801))))))
6681 - 1)
6682 <= IKFAST_SINCOS_THRESH)
6683 continue;
6684 j6array[0] = IKatan2(
6685 ((17.0)
6686 * (x802.value)),
6687 ((x803.value)
6688 * (((-48.1666666666667)
6689 + (((66.6666666666667)
6690 * x800))
6691 + (((66.6666666666667)
6692 * x801))))));
6693 sj6array
6694 [0]
6695 = IKsin(
6696 j6array
6697 [0]);
6698 cj6array
6699 [0]
6700 = IKcos(
6701 j6array
6702 [0]);
6703 if (j6array
6704 [0]
6705 > IKPI)
6706 {
6707 j6array
6708 [0]
6709 -= IK2PI;
6710 }
6711 else if (
6712 j6array
6713 [0]
6714 < -IKPI)
6715 {
6716 j6array
6717 [0]
6718 += IK2PI;
6719 }
6720 j6valid
6721 [0]
6722 = true;
6723 for (
6724 int ij6
6725 = 0;
6726 ij6
6727 < 1;
6728 ++ij6)
6729 {
6730 if (!j6valid
6731 [ij6])
6732 {
6733 continue;
6734 }
6735 _ij6[0]
6736 = ij6;
6737 _ij6[1]
6738 = -1;
6739 for (
6740 int iij6
6741 = ij6
6742 + 1;
6743 iij6
6744 < 1;
6745 ++iij6)
6746 {
6747 if (j6valid
6748 [iij6]
6749 && IKabs(
6750 cj6array
6751 [ij6]
6752 - cj6array
6753 [iij6])
6754 < IKFAST_SOLUTION_THRESH
6755 && IKabs(
6756 sj6array
6757 [ij6]
6758 - sj6array
6759 [iij6])
6760 < IKFAST_SOLUTION_THRESH)
6761 {
6762 j6valid
6763 [iij6]
6764 = false;
6765 _ij6[1]
6766 = iij6;
6767 break;
6768 }
6769 }
6770 j6 = j6array
6771 [ij6];
6772 cj6 = cj6array
6773 [ij6];
6774 sj6 = sj6array
6775 [ij6];
6776 {
6777 IkReal evalcond
6778 [5];
6779 IkReal
6780 x804
6781 = IKcos(
6782 j6);
6783 IkReal
6784 x805
6785 = (cj4
6786 * px);
6787 IkReal
6788 x806
6789 = (x804
6790 * x805);
6791 IkReal
6792 x807
6793 = (py
6794 * sj4);
6795 IkReal
6796 x808
6797 = (x804
6798 * x807);
6799 IkReal
6800 x809
6801 = IKsin(
6802 j6);
6803 IkReal
6804 x810
6805 = (x805
6806 * x809);
6807 IkReal
6808 x811
6809 = (x807
6810 * x809);
6811 IkReal
6812 x812
6813 = px
6814 * px;
6815 IkReal
6816 x813
6817 = ((3.92156862745098)
6818 * x809);
6819 IkReal
6820 x814
6821 = ((0.588235294117647)
6822 * x804);
6823 IkReal
6824 x815
6825 = py
6826 * py;
6827 evalcond
6828 [0]
6829 = (x806
6830 + x808);
6831 evalcond
6832 [1]
6833 = ((-0.85)
6834 + x811
6835 + x810);
6836 evalcond
6837 [2]
6838 = ((((-1.0)
6839 * x807))
6840 + (((0.85)
6841 * x809))
6842 + (((-1.0)
6843 * x805)));
6844 evalcond
6845 [3]
6846 = ((((-1.0)
6847 * x812
6848 * x814))
6849 + (((-1.0)
6850 * x814
6851 * x815))
6852 + (((-1.0)
6853 * x812
6854 * x813))
6855 + (((2.83333333333333)
6856 * x809))
6857 + (((-0.425)
6858 * x804))
6859 + (((-1.0)
6860 * x813
6861 * x815)));
6862 evalcond
6863 [4]
6864 = ((-0.2125)
6865 + (((1.1)
6866 * x810))
6867 + (((-0.09)
6868 * x808))
6869 + (((-1.0)
6870 * x815))
6871 + (((1.1)
6872 * x811))
6873 + (((-1.0)
6874 * x812))
6875 + (((-0.09)
6876 * x806)));
6877 if (IKabs(
6878 evalcond
6879 [0])
6880 > IKFAST_EVALCOND_THRESH
6881 || IKabs(
6882 evalcond
6883 [1])
6884 > IKFAST_EVALCOND_THRESH
6885 || IKabs(
6886 evalcond
6887 [2])
6888 > IKFAST_EVALCOND_THRESH
6889 || IKabs(
6890 evalcond
6891 [3])
6892 > IKFAST_EVALCOND_THRESH
6893 || IKabs(
6894 evalcond
6895 [4])
6896 > IKFAST_EVALCOND_THRESH)
6897 {
6898 continue;
6899 }
6900 }
6901
6902 rotationfunction0(
6903 solutions);
6904 }
6905 }
6906 }
6907 }
6908 }
6909 else
6910 {
6911 {
6912 IkReal j6array
6913 [1],
6914 cj6array
6915 [1],
6916 sj6array
6917 [1];
6918 bool j6valid
6919 [1]
6920 = {false};
6921 _nj6
6922 = 1;
6923 IkReal
6924 x816
6925 = (cj4
6926 * px);
6927 IkReal
6928 x817
6929 = (py
6930 * sj4);
6931 IkReal
6932 x818
6933 = px
6934 * px;
6935 IkReal
6936 x819
6937 = py
6938 * py;
6939 CheckValue<IkReal> x820 = IKPowWithIntegerCheck(
6940 ((-7.225)
6941 + (((-10.0)
6942 * x818))
6943 + (((-10.0)
6944 * x819))),
6945 -1);
6946 if (!x820.valid)
6947 {
6948 continue;
6949 }
6950 if (IKabs((
6951 (((1.17647058823529)
6952 * x816))
6953 + (((1.17647058823529)
6954 * x817))))
6955 < IKFAST_ATAN2_MAGTHRESH
6956 && IKabs((
6957 (x820.value)
6958 * (((((78.4313725490196)
6959 * x817
6960 * x818))
6961 + (((78.4313725490196)
6962 * sj4
6963 * (py
6964 * py
6965 * py)))
6966 + (((78.4313725490196)
6967 * cj4
6968 * (px
6969 * px
6970 * px)))
6971 + (((-56.6666666666667)
6972 * x816))
6973 + (((-56.6666666666667)
6974 * x817))
6975 + (((78.4313725490196)
6976 * x816
6977 * x819))))))
6978 < IKFAST_ATAN2_MAGTHRESH
6979 && IKabs(
6980 IKsqr((
6981 (((1.17647058823529)
6982 * x816))
6983 + (((1.17647058823529)
6984 * x817))))
6985 + IKsqr((
6986 (x820.value) * (((((78.4313725490196) * x817 * x818)) + (((78.4313725490196) * sj4 * (py * py * py))) + (((78.4313725490196) * cj4 * (px * px * px))) + (((-56.6666666666667) * x816)) + (((-56.6666666666667) * x817)) + (((78.4313725490196) * x816 * x819))))))
6987 - 1)
6988 <= IKFAST_SINCOS_THRESH)
6989 continue;
6990 j6array[0] = IKatan2(
6991 ((((1.17647058823529)
6992 * x816))
6993 + (((1.17647058823529)
6994 * x817))),
6995 ((x820.value)
6996 * (((((78.4313725490196)
6997 * x817
6998 * x818))
6999 + (((78.4313725490196)
7000 * sj4
7001 * (py
7002 * py
7003 * py)))
7004 + (((78.4313725490196)
7005 * cj4
7006 * (px
7007 * px
7008 * px)))
7009 + (((-56.6666666666667)
7010 * x816))
7011 + (((-56.6666666666667)
7012 * x817))
7013 + (((78.4313725490196)
7014 * x816
7015 * x819))))));
7016 sj6array
7017 [0]
7018 = IKsin(
7019 j6array
7020 [0]);
7021 cj6array
7022 [0]
7023 = IKcos(
7024 j6array
7025 [0]);
7026 if (j6array
7027 [0]
7028 > IKPI)
7029 {
7030 j6array
7031 [0]
7032 -= IK2PI;
7033 }
7034 else if (
7035 j6array
7036 [0]
7037 < -IKPI)
7038 {
7039 j6array
7040 [0]
7041 += IK2PI;
7042 }
7043 j6valid
7044 [0]
7045 = true;
7046 for (
7047 int ij6
7048 = 0;
7049 ij6
7050 < 1;
7051 ++ij6)
7052 {
7053 if (!j6valid
7054 [ij6])
7055 {
7056 continue;
7057 }
7058 _ij6[0]
7059 = ij6;
7060 _ij6[1]
7061 = -1;
7062 for (
7063 int iij6
7064 = ij6
7065 + 1;
7066 iij6
7067 < 1;
7068 ++iij6)
7069 {
7070 if (j6valid
7071 [iij6]
7072 && IKabs(
7073 cj6array
7074 [ij6]
7075 - cj6array
7076 [iij6])
7077 < IKFAST_SOLUTION_THRESH
7078 && IKabs(
7079 sj6array
7080 [ij6]
7081 - sj6array
7082 [iij6])
7083 < IKFAST_SOLUTION_THRESH)
7084 {
7085 j6valid
7086 [iij6]
7087 = false;
7088 _ij6[1]
7089 = iij6;
7090 break;
7091 }
7092 }
7093 j6 = j6array
7094 [ij6];
7095 cj6 = cj6array
7096 [ij6];
7097 sj6 = sj6array
7098 [ij6];
7099 {
7100 IkReal evalcond
7101 [5];
7102 IkReal
7103 x821
7104 = IKcos(
7105 j6);
7106 IkReal
7107 x822
7108 = (cj4
7109 * px);
7110 IkReal
7111 x823
7112 = (x821
7113 * x822);
7114 IkReal
7115 x824
7116 = (py
7117 * sj4);
7118 IkReal
7119 x825
7120 = (x821
7121 * x824);
7122 IkReal
7123 x826
7124 = IKsin(
7125 j6);
7126 IkReal
7127 x827
7128 = (x822
7129 * x826);
7130 IkReal
7131 x828
7132 = (x824
7133 * x826);
7134 IkReal
7135 x829
7136 = px
7137 * px;
7138 IkReal
7139 x830
7140 = ((3.92156862745098)
7141 * x826);
7142 IkReal
7143 x831
7144 = ((0.588235294117647)
7145 * x821);
7146 IkReal
7147 x832
7148 = py
7149 * py;
7150 evalcond
7151 [0]
7152 = (x825
7153 + x823);
7154 evalcond
7155 [1]
7156 = ((-0.85)
7157 + x827
7158 + x828);
7159 evalcond
7160 [2]
7161 = ((((0.85)
7162 * x826))
7163 + (((-1.0)
7164 * x822))
7165 + (((-1.0)
7166 * x824)));
7167 evalcond
7168 [3]
7169 = ((((-1.0)
7170 * x830
7171 * x832))
7172 + (((2.83333333333333)
7173 * x826))
7174 + (((-1.0)
7175 * x829
7176 * x831))
7177 + (((-1.0)
7178 * x831
7179 * x832))
7180 + (((-1.0)
7181 * x829
7182 * x830))
7183 + (((-0.425)
7184 * x821)));
7185 evalcond
7186 [4]
7187 = ((-0.2125)
7188 + (((1.1)
7189 * x828))
7190 + (((-0.09)
7191 * x825))
7192 + (((1.1)
7193 * x827))
7194 + (((-0.09)
7195 * x823))
7196 + (((-1.0)
7197 * x832))
7198 + (((-1.0)
7199 * x829)));
7200 if (IKabs(
7201 evalcond
7202 [0])
7203 > IKFAST_EVALCOND_THRESH
7204 || IKabs(
7205 evalcond
7206 [1])
7207 > IKFAST_EVALCOND_THRESH
7208 || IKabs(
7209 evalcond
7210 [2])
7211 > IKFAST_EVALCOND_THRESH
7212 || IKabs(
7213 evalcond
7214 [3])
7215 > IKFAST_EVALCOND_THRESH
7216 || IKabs(
7217 evalcond
7218 [4])
7219 > IKFAST_EVALCOND_THRESH)
7220 {
7221 continue;
7222 }
7223 }
7224
7225 rotationfunction0(
7226 solutions);
7227 }
7228 }
7229 }
7230 }
7231 }
7232 else
7233 {
7234 {
7235 IkReal j6array
7236 [1],
7237 cj6array
7238 [1],
7239 sj6array
7240 [1];
7241 bool j6valid
7242 [1]
7243 = {false};
7244 _nj6 = 1;
7245 IkReal
7246 x833
7247 = (cj4
7248 * px);
7249 IkReal
7250 x834
7251 = (py
7252 * sj4);
7253 IkReal
7254 x835
7255 = px
7256 * px;
7257 IkReal
7258 x836
7259 = py
7260 * py;
7261 IkReal
7262 x837
7263 = ((1.29411764705882)
7264 * (cj4
7265 * cj4));
7266 CheckValue<
7267 IkReal>
7268 x838
7269 = IKPowWithIntegerCheck(
7270 ((((-0.09)
7271 * x834))
7272 + (((-0.09)
7273 * x833))),
7274 -1);
7275 if (!x838.valid)
7276 {
7277 continue;
7278 }
7279 if (IKabs((
7280 (((1.17647058823529)
7281 * x833))
7282 + (((1.17647058823529)
7283 * x834))))
7284 < IKFAST_ATAN2_MAGTHRESH
7285 && IKabs((
7286 (x838.value)
7287 * (((0.2125)
7288 + (((-0.294117647058824)
7289 * x836))
7290 + x835
7291 + (((-2.58823529411765)
7292 * cj4
7293 * px
7294 * x834))
7295 + ((x836
7296 * x837))
7297 + (((-1.0)
7298 * x835
7299 * x837))))))
7300 < IKFAST_ATAN2_MAGTHRESH
7301 && IKabs(
7302 IKsqr((
7303 (((1.17647058823529)
7304 * x833))
7305 + (((1.17647058823529)
7306 * x834))))
7307 + IKsqr((
7308 (x838.value)
7309 * (((0.2125)
7310 + (((-0.294117647058824)
7311 * x836))
7312 + x835
7313 + (((-2.58823529411765)
7314 * cj4
7315 * px
7316 * x834))
7317 + ((x836
7318 * x837))
7319 + (((-1.0)
7320 * x835
7321 * x837))))))
7322 - 1)
7323 <= IKFAST_SINCOS_THRESH)
7324 continue;
7325 j6array[0] = IKatan2(
7326 ((((1.17647058823529)
7327 * x833))
7328 + (((1.17647058823529)
7329 * x834))),
7330 ((x838.value)
7331 * (((0.2125)
7332 + (((-0.294117647058824)
7333 * x836))
7334 + x835
7335 + (((-2.58823529411765)
7336 * cj4
7337 * px
7338 * x834))
7339 + ((x836
7340 * x837))
7341 + (((-1.0)
7342 * x835
7343 * x837))))));
7344 sj6array[0] = IKsin(
7345 j6array
7346 [0]);
7347 cj6array[0] = IKcos(
7348 j6array
7349 [0]);
7350 if (j6array
7351 [0]
7352 > IKPI)
7353 {
7354 j6array
7355 [0]
7356 -= IK2PI;
7357 }
7358 else if (
7359 j6array
7360 [0]
7361 < -IKPI)
7362 {
7363 j6array
7364 [0]
7365 += IK2PI;
7366 }
7367 j6valid[0]
7368 = true;
7369 for (
7370 int ij6
7371 = 0;
7372 ij6
7373 < 1;
7374 ++ij6)
7375 {
7376 if (!j6valid
7377 [ij6])
7378 {
7379 continue;
7380 }
7381 _ij6[0]
7382 = ij6;
7383 _ij6[1]
7384 = -1;
7385 for (
7386 int iij6
7387 = ij6
7388 + 1;
7389 iij6
7390 < 1;
7391 ++iij6)
7392 {
7393 if (j6valid
7394 [iij6]
7395 && IKabs(
7396 cj6array
7397 [ij6]
7398 - cj6array
7399 [iij6])
7400 < IKFAST_SOLUTION_THRESH
7401 && IKabs(
7402 sj6array
7403 [ij6]
7404 - sj6array
7405 [iij6])
7406 < IKFAST_SOLUTION_THRESH)
7407 {
7408 j6valid
7409 [iij6]
7410 = false;
7411 _ij6[1]
7412 = iij6;
7413 break;
7414 }
7415 }
7416 j6 = j6array
7417 [ij6];
7418 cj6 = cj6array
7419 [ij6];
7420 sj6 = sj6array
7421 [ij6];
7422 {
7423 IkReal evalcond
7424 [5];
7425 IkReal
7426 x839
7427 = IKcos(
7428 j6);
7429 IkReal
7430 x840
7431 = (cj4
7432 * px);
7433 IkReal
7434 x841
7435 = (x839
7436 * x840);
7437 IkReal
7438 x842
7439 = (py
7440 * sj4);
7441 IkReal
7442 x843
7443 = (x839
7444 * x842);
7445 IkReal
7446 x844
7447 = IKsin(
7448 j6);
7449 IkReal
7450 x845
7451 = (x840
7452 * x844);
7453 IkReal
7454 x846
7455 = (x842
7456 * x844);
7457 IkReal
7458 x847
7459 = px
7460 * px;
7461 IkReal
7462 x848
7463 = ((3.92156862745098)
7464 * x844);
7465 IkReal
7466 x849
7467 = ((0.588235294117647)
7468 * x839);
7469 IkReal
7470 x850
7471 = py
7472 * py;
7473 evalcond
7474 [0]
7475 = (x843
7476 + x841);
7477 evalcond
7478 [1]
7479 = ((-0.85)
7480 + x846
7481 + x845);
7482 evalcond
7483 [2]
7484 = ((((0.85)
7485 * x844))
7486 + (((-1.0)
7487 * x842))
7488 + (((-1.0)
7489 * x840)));
7490 evalcond
7491 [3]
7492 = ((((-1.0)
7493 * x847
7494 * x849))
7495 + (((-1.0)
7496 * x847
7497 * x848))
7498 + (((-0.425)
7499 * x839))
7500 + (((-1.0)
7501 * x848
7502 * x850))
7503 + (((-1.0)
7504 * x849
7505 * x850))
7506 + (((2.83333333333333)
7507 * x844)));
7508 evalcond
7509 [4]
7510 = ((-0.2125)
7511 + (((-1.0)
7512 * x850))
7513 + (((-0.09)
7514 * x843))
7515 + (((1.1)
7516 * x846))
7517 + (((1.1)
7518 * x845))
7519 + (((-1.0)
7520 * x847))
7521 + (((-0.09)
7522 * x841)));
7523 if (IKabs(
7524 evalcond
7525 [0])
7526 > IKFAST_EVALCOND_THRESH
7527 || IKabs(
7528 evalcond
7529 [1])
7530 > IKFAST_EVALCOND_THRESH
7531 || IKabs(
7532 evalcond
7533 [2])
7534 > IKFAST_EVALCOND_THRESH
7535 || IKabs(
7536 evalcond
7537 [3])
7538 > IKFAST_EVALCOND_THRESH
7539 || IKabs(
7540 evalcond
7541 [4])
7542 > IKFAST_EVALCOND_THRESH)
7543 {
7544 continue;
7545 }
7546 }
7547
7548 rotationfunction0(
7549 solutions);
7550 }
7551 }
7552 }
7553 }
7554 }
7555 } while (0);
7556 if (bgotonextstatement)
7557 {
7558 bool
7559 bgotonextstatement
7560 = true;
7561 do
7562 {
7563 if (1)
7564 {
7565 bgotonextstatement
7566 = false;
7567 continue; // branch miss [j6]
7568 }
7569 } while (0);
7570 if (bgotonextstatement)
7571 {
7572 }
7573 }
7574 }
7575 }
7576 else
7577 {
7578 {
7579 IkReal j6array[1],
7580 cj6array[1],
7581 sj6array[1];
7582 bool j6valid[1]
7583 = {false};
7584 _nj6 = 1;
7585 IkReal x851
7586 = (cj4 * px);
7587 IkReal x852
7588 = (py * sj4);
7589 IkReal x853
7590 = ((0.108264705882353)
7591 * cj9);
7592 IkReal x854
7593 = ((0.588235294117647)
7594 * pp);
7595 IkReal x855
7596 = (cj9 * pp);
7597 IkReal x856
7598 = (cj9 * sj9);
7599 IkReal x857
7600 = (pp * sj9);
7601 IkReal x858
7602 = cj9 * cj9;
7603 IkReal x859
7604 = ((1.0) * pz);
7605 CheckValue<IkReal> x860 = IKatan2WithCheck(
7606 IkReal((
7607 (-0.174204411764706)
7608 + (pz * pz)
7609 + (((-0.176470588235294)
7610 * x855))
7611 + (((-1.0)
7612 * (0.154566176470588)
7613 * cj9))
7614 + (((-1.0)
7615 * (0.323529411764706)
7616 * pp))
7617 + (((-0.0264705882352941)
7618 * x857))
7619 + (((-0.00487191176470588)
7620 * x856))
7621 + (((-1.0)
7622 * (0.0142530882352941)
7623 * sj9))
7624 + (((-0.0324794117647059)
7625 * x858)))),
7626 ((-0.830553921568627)
7627 + (((-0.396970588235294)
7628 * x858))
7629 + (((-1.0)
7630 * (0.0679544117647059)
7631 * sj9))
7632 + (((-1.0)
7633 * (1.18080882352941)
7634 * cj9))
7635 + (((0.176470588235294)
7636 * x857))
7637 + (((-1.0)
7638 * x852
7639 * x859))
7640 + (((2.15686274509804)
7641 * pp))
7642 + (((1.17647058823529)
7643 * x855))
7644 + (((-1.0)
7645 * x851
7646 * x859))
7647 + (((-0.0595455882352941)
7648 * x856))),
7649 IKFAST_ATAN2_MAGTHRESH);
7650 if (!x860.valid)
7651 {
7652 continue;
7653 }
7654 CheckValue<IkReal> x861 = IKPowWithIntegerCheck(
7655 IKsign((
7656 (((-1.0)
7657 * x851
7658 * x854))
7659 + (((-1.0)
7660 * (1.32323529411765)
7661 * cj9
7662 * pz))
7663 + (((3.92156862745098)
7664 * pp
7665 * pz))
7666 + (((-1.0)
7667 * (1.51009803921569)
7668 * pz))
7669 + (((-0.316735294117647)
7670 * x852))
7671 + (((-1.0)
7672 * x852
7673 * x853))
7674 + (((-0.316735294117647)
7675 * x851))
7676 + (((-1.0)
7677 * x851
7678 * x853))
7679 + (((-1.0)
7680 * x852
7681 * x854)))),
7682 -1);
7683 if (!x861.valid)
7684 {
7685 continue;
7686 }
7687 j6array[0]
7688 = ((-1.5707963267949)
7689 + (x860.value)
7690 + (((1.5707963267949)
7691 * (x861.value))));
7692 sj6array[0] = IKsin(
7693 j6array[0]);
7694 cj6array[0] = IKcos(
7695 j6array[0]);
7696 if (j6array[0]
7697 > IKPI)
7698 {
7699 j6array[0]
7700 -= IK2PI;
7701 }
7702 else if (
7703 j6array[0]
7704 < -IKPI)
7705 {
7706 j6array[0]
7707 += IK2PI;
7708 }
7709 j6valid[0] = true;
7710 for (int ij6 = 0;
7711 ij6 < 1;
7712 ++ij6)
7713 {
7714 if (!j6valid[ij6])
7715 {
7716 continue;
7717 }
7718 _ij6[0] = ij6;
7719 _ij6[1] = -1;
7720 for (int iij6
7721 = ij6 + 1;
7722 iij6 < 1;
7723 ++iij6)
7724 {
7725 if (j6valid
7726 [iij6]
7727 && IKabs(
7728 cj6array
7729 [ij6]
7730 - cj6array
7731 [iij6])
7732 < IKFAST_SOLUTION_THRESH
7733 && IKabs(
7734 sj6array
7735 [ij6]
7736 - sj6array
7737 [iij6])
7738 < IKFAST_SOLUTION_THRESH)
7739 {
7740 j6valid[iij6]
7741 = false;
7742 _ij6[1]
7743 = iij6;
7744 break;
7745 }
7746 }
7747 j6 = j6array[ij6];
7748 cj6 = cj6array
7749 [ij6];
7750 sj6 = sj6array
7751 [ij6];
7752 {
7753 IkReal
7754 evalcond[5];
7755 IkReal x862
7756 = ((0.3)
7757 * cj9);
7758 IkReal x863
7759 = ((0.045)
7760 * sj9);
7761 IkReal x864
7762 = IKcos(j6);
7763 IkReal x865
7764 = (pz
7765 * x864);
7766 IkReal x866
7767 = IKsin(j6);
7768 IkReal x867
7769 = (cj4
7770 * px);
7771 IkReal x868
7772 = (x866
7773 * x867);
7774 IkReal x869
7775 = (py
7776 * sj4);
7777 IkReal x870
7778 = (x866
7779 * x869);
7780 IkReal x871
7781 = ((0.045)
7782 * cj9);
7783 IkReal x872
7784 = ((0.3)
7785 * sj9);
7786 IkReal x873
7787 = (pz
7788 * x866);
7789 IkReal x874
7790 = (x864
7791 * x867);
7792 IkReal x875
7793 = (x864
7794 * x869);
7795 evalcond[0]
7796 = ((-0.55)
7797 + (((-1.0)
7798 * x862))
7799 + x868
7800 + x865
7801 + x870
7802 + (((-1.0)
7803 * x863)));
7804 evalcond[1]
7805 = ((0.045)
7806 + (((-1.0)
7807 * x873))
7808 + x872
7809 + x874
7810 + x875
7811 + (((-1.0)
7812 * x871)));
7813 evalcond[2]
7814 = ((((1.51009803921569)
7815 * x866))
7816 + (((-0.316735294117647)
7817 * x864))
7818 + pz
7819 + (((-3.92156862745098)
7820 * pp
7821 * x866))
7822 + (((-0.108264705882353)
7823 * cj9
7824 * x864))
7825 + (((-0.588235294117647)
7826 * pp
7827 * x864))
7828 + (((1.32323529411765)
7829 * cj9
7830 * x866)));
7831 evalcond[3]
7832 = ((((0.55)
7833 * x866))
7834 + (((-1.0)
7835 * x869))
7836 + (((-0.045)
7837 * x864))
7838 + (((-1.0)
7839 * x867))
7840 + (((-1.0)
7841 * x864
7842 * x872))
7843 + ((x862
7844 * x866))
7845 + ((x864
7846 * x871))
7847 + ((x863
7848 * x866)));
7849 evalcond[4]
7850 = ((-0.2125)
7851 + (((-0.09)
7852 * x875))
7853 + (((1.1)
7854 * x865))
7855 + (((0.09)
7856 * x873))
7857 + (((-0.09)
7858 * x874))
7859 + (((1.1)
7860 * x870))
7861 + (((1.1)
7862 * x868))
7863 + (((-1.0)
7864 * (1.0)
7865 * pp)));
7866 if (IKabs(
7867 evalcond
7868 [0])
7869 > IKFAST_EVALCOND_THRESH
7870 || IKabs(
7871 evalcond
7872 [1])
7873 > IKFAST_EVALCOND_THRESH
7874 || IKabs(
7875 evalcond
7876 [2])
7877 > IKFAST_EVALCOND_THRESH
7878 || IKabs(
7879 evalcond
7880 [3])
7881 > IKFAST_EVALCOND_THRESH
7882 || IKabs(
7883 evalcond
7884 [4])
7885 > IKFAST_EVALCOND_THRESH)
7886 {
7887 continue;
7888 }
7889 }
7890
7891 rotationfunction0(
7892 solutions);
7893 }
7894 }
7895 }
7896 }
7897 }
7898 else
7899 {
7900 {
7901 IkReal j6array[1],
7902 cj6array[1],
7903 sj6array[1];
7904 bool j6valid[1]
7905 = {false};
7906 _nj6 = 1;
7907 IkReal x876
7908 = ((0.045) * cj4
7909 * px);
7910 IkReal x877
7911 = ((0.045) * py
7912 * sj4);
7913 IkReal x878
7914 = ((0.3) * sj9);
7915 IkReal x879
7916 = (cj4 * px);
7917 IkReal x880
7918 = (py * sj4);
7919 IkReal x881
7920 = (cj9 * sj9);
7921 IkReal x882 = cj9 * cj9;
7922 IkReal x883
7923 = ((1.0) * pz);
7924 IkReal x884 = py * py;
7925 IkReal x885 = cj4 * cj4;
7926 CheckValue<IkReal> x886
7927 = IKatan2WithCheck(
7928 IkReal((
7929 (-0.03825)
7930 + (((0.027)
7931 * x882))
7932 + (((-0.087975)
7933 * x881))
7934 + (((-1.0)
7935 * (0.167025)
7936 * sj9))
7937 + (((-1.0)
7938 * x879
7939 * x883))
7940 + (((-1.0)
7941 * x880
7942 * x883))
7943 + (((0.01125)
7944 * cj9)))),
7945 ((-0.304525)
7946 + (((-1.0)
7947 * x884
7948 * x885))
7949 + (((2.0) * cj4
7950 * px
7951 * x880))
7952 + (((-1.0)
7953 * (0.0495)
7954 * sj9))
7955 + (((-0.087975)
7956 * x882))
7957 + ((x885
7958 * (px
7959 * px)))
7960 + x884
7961 + (((-1.0)
7962 * (0.33)
7963 * cj9))
7964 + (((-0.027)
7965 * x881))),
7966 IKFAST_ATAN2_MAGTHRESH);
7967 if (!x886.valid)
7968 {
7969 continue;
7970 }
7971 CheckValue<IkReal> x887
7972 = IKPowWithIntegerCheck(
7973 IKsign((
7974 (((-1.0)
7975 * x878
7976 * x879))
7977 + (((-1.0)
7978 * (0.55)
7979 * pz))
7980 + (((-1.0)
7981 * x877))
7982 + (((-1.0)
7983 * x876))
7984 + (((-1.0)
7985 * x878
7986 * x880))
7987 + ((cj9
7988 * x877))
7989 + ((cj9
7990 * x876))
7991 + (((-1.0)
7992 * (0.045)
7993 * pz
7994 * sj9))
7995 + (((-1.0)
7996 * (0.3)
7997 * cj9
7998 * pz)))),
7999 -1);
8000 if (!x887.valid)
8001 {
8002 continue;
8003 }
8004 j6array[0]
8005 = ((-1.5707963267949)
8006 + (x886.value)
8007 + (((1.5707963267949)
8008 * (x887.value))));
8009 sj6array[0]
8010 = IKsin(j6array[0]);
8011 cj6array[0]
8012 = IKcos(j6array[0]);
8013 if (j6array[0] > IKPI)
8014 {
8015 j6array[0] -= IK2PI;
8016 }
8017 else if (
8018 j6array[0] < -IKPI)
8019 {
8020 j6array[0] += IK2PI;
8021 }
8022 j6valid[0] = true;
8023 for (int ij6 = 0;
8024 ij6 < 1;
8025 ++ij6)
8026 {
8027 if (!j6valid[ij6])
8028 {
8029 continue;
8030 }
8031 _ij6[0] = ij6;
8032 _ij6[1] = -1;
8033 for (int iij6
8034 = ij6 + 1;
8035 iij6 < 1;
8036 ++iij6)
8037 {
8038 if (j6valid[iij6]
8039 && IKabs(
8040 cj6array
8041 [ij6]
8042 - cj6array
8043 [iij6])
8044 < IKFAST_SOLUTION_THRESH
8045 && IKabs(
8046 sj6array
8047 [ij6]
8048 - sj6array
8049 [iij6])
8050 < IKFAST_SOLUTION_THRESH)
8051 {
8052 j6valid[iij6]
8053 = false;
8054 _ij6[1] = iij6;
8055 break;
8056 }
8057 }
8058 j6 = j6array[ij6];
8059 cj6 = cj6array[ij6];
8060 sj6 = sj6array[ij6];
8061 {
8062 IkReal evalcond[5];
8063 IkReal x888
8064 = ((0.3) * cj9);
8065 IkReal x889
8066 = ((0.045)
8067 * sj9);
8068 IkReal x890
8069 = IKcos(j6);
8070 IkReal x891
8071 = (pz * x890);
8072 IkReal x892
8073 = IKsin(j6);
8074 IkReal x893
8075 = (cj4 * px);
8076 IkReal x894
8077 = (x892 * x893);
8078 IkReal x895
8079 = (py * sj4);
8080 IkReal x896
8081 = (x892 * x895);
8082 IkReal x897
8083 = ((0.045)
8084 * cj9);
8085 IkReal x898
8086 = ((0.3) * sj9);
8087 IkReal x899
8088 = (pz * x892);
8089 IkReal x900
8090 = (x890 * x893);
8091 IkReal x901
8092 = (x890 * x895);
8093 evalcond[0]
8094 = ((-0.55)
8095 + (((-1.0)
8096 * x888))
8097 + x894 + x896
8098 + x891
8099 + (((-1.0)
8100 * x889)));
8101 evalcond[1]
8102 = ((0.045)
8103 + (((-1.0)
8104 * x897))
8105 + x900 + x901
8106 + x898
8107 + (((-1.0)
8108 * x899)));
8109 evalcond[2]
8110 = ((((-0.588235294117647)
8111 * pp
8112 * x890))
8113 + (((-0.316735294117647)
8114 * x890))
8115 + (((1.32323529411765)
8116 * cj9
8117 * x892))
8118 + pz
8119 + (((-0.108264705882353)
8120 * cj9
8121 * x890))
8122 + (((-3.92156862745098)
8123 * pp
8124 * x892))
8125 + (((1.51009803921569)
8126 * x892)));
8127 evalcond[3]
8128 = ((((-1.0)
8129 * x893))
8130 + ((x890
8131 * x897))
8132 + (((0.55)
8133 * x892))
8134 + ((x888
8135 * x892))
8136 + (((-1.0)
8137 * x890
8138 * x898))
8139 + ((x889
8140 * x892))
8141 + (((-1.0)
8142 * x895))
8143 + (((-0.045)
8144 * x890)));
8145 evalcond[4]
8146 = ((-0.2125)
8147 + (((-0.09)
8148 * x900))
8149 + (((1.1)
8150 * x891))
8151 + (((0.09)
8152 * x899))
8153 + (((-0.09)
8154 * x901))
8155 + (((1.1)
8156 * x896))
8157 + (((-1.0)
8158 * (1.0)
8159 * pp))
8160 + (((1.1)
8161 * x894)));
8162 if (IKabs(
8163 evalcond[0])
8164 > IKFAST_EVALCOND_THRESH
8165 || IKabs(
8166 evalcond
8167 [1])
8168 > IKFAST_EVALCOND_THRESH
8169 || IKabs(
8170 evalcond
8171 [2])
8172 > IKFAST_EVALCOND_THRESH
8173 || IKabs(
8174 evalcond
8175 [3])
8176 > IKFAST_EVALCOND_THRESH
8177 || IKabs(
8178 evalcond
8179 [4])
8180 > IKFAST_EVALCOND_THRESH)
8181 {
8182 continue;
8183 }
8184 }
8185
8186 rotationfunction0(
8187 solutions);
8188 }
8189 }
8190 }
8191 }
8192 }
8193 else
8194 {
8195 {
8196 IkReal j6array[1],
8197 cj6array[1],
8198 sj6array[1];
8199 bool j6valid[1] = {false};
8200 _nj6 = 1;
8201 IkReal x902 = py * py;
8202 IkReal x903 = (py * sj4);
8203 IkReal x904 = cj4 * cj4;
8204 IkReal x905
8205 = ((0.045) * pz);
8206 IkReal x906 = (cj4 * px);
8207 IkReal x907 = ((0.3) * pz);
8208 IkReal x908
8209 = ((0.3) * cj4 * px);
8210 IkReal x909
8211 = ((0.045) * x906);
8212 IkReal x910
8213 = ((0.3) * py * sj4);
8214 IkReal x911
8215 = ((0.045) * x903);
8216 CheckValue<IkReal> x912
8217 = IKatan2WithCheck(
8218 IkReal((
8219 ((sj9 * x911))
8220 + ((sj9 * x909))
8221 + ((cj9 * x910))
8222 + ((sj9 * x907))
8223 + x905
8224 + (((0.55)
8225 * x906))
8226 + (((0.55)
8227 * x903))
8228 + ((cj9 * x908))
8229 + (((-1.0) * cj9
8230 * x905)))),
8231 ((((-1.0) * x911))
8232 + ((cj9 * x907))
8233 + ((cj9 * x911))
8234 + ((cj9 * x909))
8235 + (((-1.0) * sj9
8236 * x910))
8237 + (((0.55) * pz))
8238 + (((-1.0) * sj9
8239 * x908))
8240 + (((-1.0) * x909))
8241 + ((sj9 * x905))),
8242 IKFAST_ATAN2_MAGTHRESH);
8243 if (!x912.valid)
8244 {
8245 continue;
8246 }
8247 CheckValue<IkReal> x913
8248 = IKPowWithIntegerCheck(
8249 IKsign(
8250 ((((2.0) * cj4
8251 * px * x903))
8252 + (pz * pz)
8253 + x902
8254 + ((x904
8255 * (px
8256 * px)))
8257 + (((-1.0)
8258 * x902
8259 * x904)))),
8260 -1);
8261 if (!x913.valid)
8262 {
8263 continue;
8264 }
8265 j6array[0]
8266 = ((-1.5707963267949)
8267 + (x912.value)
8268 + (((1.5707963267949)
8269 * (x913.value))));
8270 sj6array[0]
8271 = IKsin(j6array[0]);
8272 cj6array[0]
8273 = IKcos(j6array[0]);
8274 if (j6array[0] > IKPI)
8275 {
8276 j6array[0] -= IK2PI;
8277 }
8278 else if (j6array[0] < -IKPI)
8279 {
8280 j6array[0] += IK2PI;
8281 }
8282 j6valid[0] = true;
8283 for (int ij6 = 0; ij6 < 1;
8284 ++ij6)
8285 {
8286 if (!j6valid[ij6])
8287 {
8288 continue;
8289 }
8290 _ij6[0] = ij6;
8291 _ij6[1] = -1;
8292 for (int iij6 = ij6 + 1;
8293 iij6 < 1;
8294 ++iij6)
8295 {
8296 if (j6valid[iij6]
8297 && IKabs(
8298 cj6array[ij6]
8299 - cj6array
8300 [iij6])
8301 < IKFAST_SOLUTION_THRESH
8302 && IKabs(
8303 sj6array[ij6]
8304 - sj6array
8305 [iij6])
8306 < IKFAST_SOLUTION_THRESH)
8307 {
8308 j6valid[iij6] = false;
8309 _ij6[1] = iij6;
8310 break;
8311 }
8312 }
8313 j6 = j6array[ij6];
8314 cj6 = cj6array[ij6];
8315 sj6 = sj6array[ij6];
8316 {
8317 IkReal evalcond[5];
8318 IkReal x914
8319 = ((0.3) * cj9);
8320 IkReal x915
8321 = ((0.045) * sj9);
8322 IkReal x916 = IKcos(j6);
8323 IkReal x917
8324 = (pz * x916);
8325 IkReal x918 = IKsin(j6);
8326 IkReal x919
8327 = (cj4 * px);
8328 IkReal x920
8329 = (x918 * x919);
8330 IkReal x921
8331 = (py * sj4);
8332 IkReal x922
8333 = (x918 * x921);
8334 IkReal x923
8335 = ((0.045) * cj9);
8336 IkReal x924
8337 = ((0.3) * sj9);
8338 IkReal x925
8339 = (pz * x918);
8340 IkReal x926
8341 = (x916 * x919);
8342 IkReal x927
8343 = (x916 * x921);
8344 evalcond[0]
8345 = ((-0.55)
8346 + (((-1.0)
8347 * x914))
8348 + x917
8349 + (((-1.0)
8350 * x915))
8351 + x922 + x920);
8352 evalcond[1]
8353 = ((0.045)
8354 + (((-1.0)
8355 * x925))
8356 + (((-1.0)
8357 * x923))
8358 + x927 + x924
8359 + x926);
8360 evalcond[2]
8361 = ((((-0.316735294117647)
8362 * x916))
8363 + (((-3.92156862745098)
8364 * pp * x918))
8365 + (((-0.588235294117647)
8366 * pp * x916))
8367 + pz
8368 + (((-0.108264705882353)
8369 * cj9
8370 * x916))
8371 + (((1.32323529411765)
8372 * cj9
8373 * x918))
8374 + (((1.51009803921569)
8375 * x918)));
8376 evalcond[3]
8377 = ((((0.55) * x918))
8378 + (((-0.045)
8379 * x916))
8380 + ((x914 * x918))
8381 + ((x915 * x918))
8382 + (((-1.0)
8383 * x921))
8384 + ((x916 * x923))
8385 + (((-1.0)
8386 * x919))
8387 + (((-1.0) * x916
8388 * x924)));
8389 evalcond[4]
8390 = ((-0.2125)
8391 + (((-0.09)
8392 * x927))
8393 + (((1.1)
8394 * x920))
8395 + (((0.09)
8396 * x925))
8397 + (((-0.09)
8398 * x926))
8399 + (((1.1)
8400 * x917))
8401 + (((1.1)
8402 * x922))
8403 + (((-1.0)
8404 * (1.0)
8405 * pp)));
8406 if (IKabs(evalcond[0])
8407 > IKFAST_EVALCOND_THRESH
8408 || IKabs(
8409 evalcond[1])
8410 > IKFAST_EVALCOND_THRESH
8411 || IKabs(
8412 evalcond[2])
8413 > IKFAST_EVALCOND_THRESH
8414 || IKabs(
8415 evalcond[3])
8416 > IKFAST_EVALCOND_THRESH
8417 || IKabs(
8418 evalcond[4])
8419 > IKFAST_EVALCOND_THRESH)
8420 {
8421 continue;
8422 }
8423 }
8424
8425 rotationfunction0(
8426 solutions);
8427 }
8428 }
8429 }
8430 }
8431 }
8432 } while (0);
8433 if (bgotonextstatement)
8434 {
8435 bool bgotonextstatement = true;
8436 do
8437 {
8438 if (1)
8439 {
8440 bgotonextstatement = false;
8441 continue; // branch miss [j6]
8442 }
8443 } while (0);
8444 if (bgotonextstatement)
8445 {
8446 }
8447 }
8448 }
8449 }
8450 }
8451 else
8452 {
8453 {
8454 IkReal j6array[1], cj6array[1],
8455 sj6array[1];
8456 bool j6valid[1] = {false};
8457 _nj6 = 1;
8458 IkReal x928 = (pz * sj8);
8459 IkReal x929 = ((0.3) * cj9);
8460 IkReal x930 = ((0.045) * sj9);
8461 IkReal x931 = (cj4 * px);
8462 IkReal x932 = ((0.045) * cj8 * sj8);
8463 IkReal x933 = (x931 * x932);
8464 IkReal x934 = (py * sj4);
8465 IkReal x935 = (x932 * x934);
8466 IkReal x936 = ((0.3) * cj8 * sj8 * sj9);
8467 IkReal x937 = ((0.55) * cj8);
8468 IkReal x938 = (cj4 * py);
8469 IkReal x939 = (px * sj4);
8470 IkReal x940 = (cj4 * cj8 * py);
8471 IkReal x941 = ((1.0) * pz * sj8);
8472 IkReal x942 = (cj8 * px * sj4);
8473 IkReal x943 = cj8 * cj8;
8474 IkReal x944 = ((0.045) * x943);
8475 IkReal x945 = (x938 * x944);
8476 IkReal x946 = (x939 * x944);
8477 IkReal x947 = ((0.3) * sj9 * x943);
8478 CheckValue<IkReal> x948
8479 = IKPowWithIntegerCheck(
8480 IKsign(
8481 ((((-0.55) * x928))
8482 + (((-1.0) * cj9 * x933))
8483 + (((-1.0) * cj9 * x935))
8484 + (((-1.0) * x928 * x929))
8485 + ((x931 * x936))
8486 + (((-1.0) * x928 * x930))
8487 + x933 + x935
8488 + ((x934 * x936)))),
8489 -1);
8490 if (!x948.valid)
8491 {
8492 continue;
8493 }
8494 CheckValue<IkReal> x949
8495 = IKatan2WithCheck(
8496 IkReal(
8497 ((((-1.0) * x937 * x939))
8498 + ((x937 * x938))
8499 + ((x930 * x940))
8500 + (((-1.0) * x930 * x942))
8501 + (((-1.0) * x931 * x941))
8502 + (((-1.0) * x934 * x941))
8503 + (((-1.0) * x929 * x942))
8504 + ((x929 * x940)))),
8505 (((cj9 * x946))
8506 + (((-1.0) * (1.0) * sj8
8507 * (pz * pz)))
8508 + ((x938 * x947))
8509 + (((-1.0) * x939 * x947))
8510 + (((-1.0) * cj9 * x945))
8511 + (((-1.0) * x946)) + x945),
8512 IKFAST_ATAN2_MAGTHRESH);
8513 if (!x949.valid)
8514 {
8515 continue;
8516 }
8517 j6array[0]
8518 = ((-1.5707963267949)
8519 + (((1.5707963267949)
8520 * (x948.value)))
8521 + (x949.value));
8522 sj6array[0] = IKsin(j6array[0]);
8523 cj6array[0] = IKcos(j6array[0]);
8524 if (j6array[0] > IKPI)
8525 {
8526 j6array[0] -= IK2PI;
8527 }
8528 else if (j6array[0] < -IKPI)
8529 {
8530 j6array[0] += IK2PI;
8531 }
8532 j6valid[0] = true;
8533 for (int ij6 = 0; ij6 < 1; ++ij6)
8534 {
8535 if (!j6valid[ij6])
8536 {
8537 continue;
8538 }
8539 _ij6[0] = ij6;
8540 _ij6[1] = -1;
8541 for (int iij6 = ij6 + 1; iij6 < 1;
8542 ++iij6)
8543 {
8544 if (j6valid[iij6]
8545 && IKabs(
8546 cj6array[ij6]
8547 - cj6array[iij6])
8548 < IKFAST_SOLUTION_THRESH
8549 && IKabs(
8550 sj6array[ij6]
8551 - sj6array[iij6])
8552 < IKFAST_SOLUTION_THRESH)
8553 {
8554 j6valid[iij6] = false;
8555 _ij6[1] = iij6;
8556 break;
8557 }
8558 }
8559 j6 = j6array[ij6];
8560 cj6 = cj6array[ij6];
8561 sj6 = sj6array[ij6];
8562 {
8563 IkReal evalcond[6];
8564 IkReal x950 = ((0.3) * cj9);
8565 IkReal x951 = ((0.045) * sj9);
8566 IkReal x952 = IKcos(j6);
8567 IkReal x953 = (pz * x952);
8568 IkReal x954 = IKsin(j6);
8569 IkReal x955 = (cj4 * px * x954);
8570 IkReal x956 = (py * sj4);
8571 IkReal x957 = (x954 * x956);
8572 IkReal x958 = (px * sj4);
8573 IkReal x959 = ((1.0) * cj4 * py);
8574 IkReal x960 = (cj4 * sj8);
8575 IkReal x961 = ((0.045) * cj8);
8576 IkReal x962 = ((0.045) * cj9);
8577 IkReal x963 = (cj8 * x954);
8578 IkReal x964 = ((0.3) * sj9);
8579 IkReal x965 = (sj8 * x958);
8580 IkReal x966 = (pz * x963);
8581 IkReal x967
8582 = (px * (((1.0) * cj4)));
8583 IkReal x968 = (cj8 * x952);
8584 IkReal x969 = ((1.0) * x956);
8585 IkReal x970 = ((0.09) * cj8 * x952);
8586 evalcond[0]
8587 = ((-0.55) + x953 + x955 + x957
8588 + (((-1.0) * x951))
8589 + (((-1.0) * x950)));
8590 evalcond[1]
8591 = ((((-1.0) * cj8 * x959))
8592 + ((cj8 * x958))
8593 + (((-1.0) * pz * sj8
8594 * x954))
8595 + ((sj8 * x952 * x956))
8596 + ((px * x952 * x960)));
8597 evalcond[2]
8598 = ((((-0.55) * x952)) + pz
8599 + (((-1.0) * x962 * x963))
8600 + (((-1.0) * x951 * x952))
8601 + ((x954 * x961))
8602 + ((x963 * x964))
8603 + (((-1.0) * x950 * x952)));
8604 evalcond[3]
8605 = ((0.045)
8606 + (((-1.0) * x968 * x969))
8607 + (((-1.0) * sj8 * x959))
8608 + x966 + x964 + x965
8609 + (((-1.0) * x962))
8610 + (((-1.0) * x967 * x968)));
8611 evalcond[4]
8612 = (((x952 * x961))
8613 + ((x964 * x968))
8614 + (((0.55) * x954))
8615 + (((-1.0) * x962 * x968))
8616 + (((-1.0) * x969))
8617 + ((x951 * x954))
8618 + ((x950 * x954))
8619 + (((-1.0) * x967)));
8620 evalcond[5]
8621 = ((-0.2125) + (((1.1) * x957))
8622 + (((1.1) * x953))
8623 + (((-0.09) * x965))
8624 + (((1.1) * x955))
8625 + (((0.09) * py * x960))
8626 + ((cj4 * px * x970))
8627 + ((x956 * x970))
8628 + (((-0.09) * x966))
8629 + (((-1.0) * (1.0) * pp)));
8630 if (IKabs(evalcond[0])
8631 > IKFAST_EVALCOND_THRESH
8632 || IKabs(evalcond[1])
8633 > IKFAST_EVALCOND_THRESH
8634 || IKabs(evalcond[2])
8635 > IKFAST_EVALCOND_THRESH
8636 || IKabs(evalcond[3])
8637 > IKFAST_EVALCOND_THRESH
8638 || IKabs(evalcond[4])
8639 > IKFAST_EVALCOND_THRESH
8640 || IKabs(evalcond[5])
8641 > IKFAST_EVALCOND_THRESH)
8642 {
8643 continue;
8644 }
8645 }
8646
8647 rotationfunction0(solutions);
8648 }
8649 }
8650 }
8651 }
8652 }
8653 else
8654 {
8655 {
8656 IkReal j6array[1], cj6array[1], sj6array[1];
8657 bool j6valid[1] = {false};
8658 _nj6 = 1;
8659 IkReal x971 = py * py;
8660 IkReal x972 = (sj8 * x971);
8661 IkReal x973 = (cj4 * px * sj8);
8662 IkReal x974 = cj4 * cj4;
8663 IkReal x975 = px * px;
8664 IkReal x976 = ((0.55) * sj8);
8665 IkReal x977 = (cj8 * px);
8666 IkReal x978 = ((0.3) * cj9);
8667 IkReal x979 = ((0.045) * sj9);
8668 IkReal x980 = (py * sj4 * sj8);
8669 IkReal x981 = (pz * sj8);
8670 IkReal x982 = (cj4 * cj8 * sj4);
8671 CheckValue<IkReal> x983 = IKatan2WithCheck(
8672 IkReal(
8673 (((x973 * x978)) + ((x973 * x979))
8674 + ((pz * sj4 * x977))
8675 + (((-1.0) * cj4 * cj8 * py * pz))
8676 + ((cj4 * px * x976))
8677 + ((x979 * x980)) + ((x978 * x980))
8678 + ((py * sj4 * x976)))),
8679 (((pz * x976))
8680 + (((2.0) * py * x974 * x977))
8681 + ((x979 * x981)) + ((x971 * x982))
8682 + (((-1.0) * py * x977))
8683 + ((x978 * x981))
8684 + (((-1.0) * x975 * x982))),
8685 IKFAST_ATAN2_MAGTHRESH);
8686 if (!x983.valid)
8687 {
8688 continue;
8689 }
8690 CheckValue<IkReal> x984
8691 = IKPowWithIntegerCheck(
8692 IKsign(
8693 ((((2.0) * py * sj4 * x973))
8694 + ((sj8 * (pz * pz))) + x972
8695 + (((-1.0) * x972 * x974))
8696 + ((sj8 * x974 * x975)))),
8697 -1);
8698 if (!x984.valid)
8699 {
8700 continue;
8701 }
8702 j6array[0]
8703 = ((-1.5707963267949) + (x983.value)
8704 + (((1.5707963267949)
8705 * (x984.value))));
8706 sj6array[0] = IKsin(j6array[0]);
8707 cj6array[0] = IKcos(j6array[0]);
8708 if (j6array[0] > IKPI)
8709 {
8710 j6array[0] -= IK2PI;
8711 }
8712 else if (j6array[0] < -IKPI)
8713 {
8714 j6array[0] += IK2PI;
8715 }
8716 j6valid[0] = true;
8717 for (int ij6 = 0; ij6 < 1; ++ij6)
8718 {
8719 if (!j6valid[ij6])
8720 {
8721 continue;
8722 }
8723 _ij6[0] = ij6;
8724 _ij6[1] = -1;
8725 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
8726 {
8727 if (j6valid[iij6]
8728 && IKabs(
8729 cj6array[ij6]
8730 - cj6array[iij6])
8731 < IKFAST_SOLUTION_THRESH
8732 && IKabs(
8733 sj6array[ij6]
8734 - sj6array[iij6])
8735 < IKFAST_SOLUTION_THRESH)
8736 {
8737 j6valid[iij6] = false;
8738 _ij6[1] = iij6;
8739 break;
8740 }
8741 }
8742 j6 = j6array[ij6];
8743 cj6 = cj6array[ij6];
8744 sj6 = sj6array[ij6];
8745 {
8746 IkReal evalcond[6];
8747 IkReal x985 = ((0.3) * cj9);
8748 IkReal x986 = ((0.045) * sj9);
8749 IkReal x987 = IKcos(j6);
8750 IkReal x988 = (pz * x987);
8751 IkReal x989 = IKsin(j6);
8752 IkReal x990 = (cj4 * px * x989);
8753 IkReal x991 = (py * sj4);
8754 IkReal x992 = (x989 * x991);
8755 IkReal x993 = (px * sj4);
8756 IkReal x994 = ((1.0) * cj4 * py);
8757 IkReal x995 = (cj4 * sj8);
8758 IkReal x996 = ((0.045) * cj8);
8759 IkReal x997 = ((0.045) * cj9);
8760 IkReal x998 = (cj8 * x989);
8761 IkReal x999 = ((0.3) * sj9);
8762 IkReal x1000 = (sj8 * x993);
8763 IkReal x1001 = (pz * x998);
8764 IkReal x1002 = (px * (((1.0) * cj4)));
8765 IkReal x1003 = (cj8 * x987);
8766 IkReal x1004 = ((1.0) * x991);
8767 IkReal x1005 = ((0.09) * cj8 * x987);
8768 evalcond[0]
8769 = ((-0.55) + (((-1.0) * x985))
8770 + (((-1.0) * x986)) + x988 + x990
8771 + x992);
8772 evalcond[1]
8773 = ((((-1.0) * cj8 * x994))
8774 + (((-1.0) * pz * sj8 * x989))
8775 + ((px * x987 * x995))
8776 + ((sj8 * x987 * x991))
8777 + ((cj8 * x993)));
8778 evalcond[2]
8779 = ((((-1.0) * x986 * x987)) + pz
8780 + (((-0.55) * x987))
8781 + (((-1.0) * x997 * x998))
8782 + ((x998 * x999))
8783 + (((-1.0) * x985 * x987))
8784 + ((x989 * x996)));
8785 evalcond[3]
8786 = ((0.045) + (((-1.0) * sj8 * x994))
8787 + (((-1.0) * x997)) + x1000
8788 + x1001
8789 + (((-1.0) * x1002 * x1003))
8790 + (((-1.0) * x1003 * x1004))
8791 + x999);
8792 evalcond[4]
8793 = ((((-1.0) * x1003 * x997))
8794 + ((x985 * x989))
8795 + ((x1003 * x999))
8796 + (((0.55) * x989))
8797 + (((-1.0) * x1004))
8798 + (((-1.0) * x1002))
8799 + ((x986 * x989))
8800 + ((x987 * x996)));
8801 evalcond[5]
8802 = ((-0.2125) + (((1.1) * x990))
8803 + ((cj4 * px * x1005))
8804 + ((x1005 * x991))
8805 + (((-0.09) * x1001))
8806 + (((0.09) * py * x995))
8807 + (((-0.09) * x1000))
8808 + (((-1.0) * (1.0) * pp))
8809 + (((1.1) * x988))
8810 + (((1.1) * x992)));
8811 if (IKabs(evalcond[0])
8812 > IKFAST_EVALCOND_THRESH
8813 || IKabs(evalcond[1])
8814 > IKFAST_EVALCOND_THRESH
8815 || IKabs(evalcond[2])
8816 > IKFAST_EVALCOND_THRESH
8817 || IKabs(evalcond[3])
8818 > IKFAST_EVALCOND_THRESH
8819 || IKabs(evalcond[4])
8820 > IKFAST_EVALCOND_THRESH
8821 || IKabs(evalcond[5])
8822 > IKFAST_EVALCOND_THRESH)
8823 {
8824 continue;
8825 }
8826 }
8827
8828 rotationfunction0(solutions);
8829 }
8830 }
8831 }
8832 }
8833 }
8834 else
8835 {
8836 {
8837 IkReal j6array[1], cj6array[1], sj6array[1];
8838 bool j6valid[1] = {false};
8839 _nj6 = 1;
8840 IkReal x1006 = (cj4 * px);
8841 IkReal x1007 = ((0.045) * pz);
8842 IkReal x1008 = (py * sj4);
8843 IkReal x1009 = ((0.3) * cj9);
8844 IkReal x1010 = ((0.045) * sj9);
8845 IkReal x1011 = (cj8 * cj9);
8846 IkReal x1012 = (cj8 * sj9);
8847 IkReal x1013 = cj9 * cj9;
8848 IkReal x1014 = ((1.0) * pz);
8849 CheckValue<IkReal> x1015 = IKatan2WithCheck(
8850 IkReal(
8851 ((-0.304525)
8852 + (((-1.0) * (0.027) * cj9 * sj9))
8853 + (((-1.0) * (0.0495) * sj9))
8854 + (pz * pz) + (((-0.087975) * x1013))
8855 + (((-1.0) * (0.33) * cj9)))),
8856 ((((-0.087975) * sj9 * x1011))
8857 + (((-1.0) * x1008 * x1014))
8858 + (((-1.0) * x1006 * x1014))
8859 + (((0.027) * cj8 * x1013))
8860 + (((0.01125) * x1011))
8861 + (((-0.167025) * x1012))
8862 + (((-1.0) * (0.03825) * cj8))),
8863 IKFAST_ATAN2_MAGTHRESH);
8864 if (!x1015.valid)
8865 {
8866 continue;
8867 }
8868 CheckValue<IkReal> x1016
8869 = IKPowWithIntegerCheck(
8870 IKsign(
8871 ((((-1.0) * cj8 * x1007))
8872 + ((x1007 * x1011))
8873 + (((-0.3) * pz * x1012))
8874 + (((-0.55) * x1008))
8875 + (((-1.0) * x1008 * x1010))
8876 + (((-0.55) * x1006))
8877 + (((-1.0) * x1008 * x1009))
8878 + (((-1.0) * x1006 * x1009))
8879 + (((-1.0) * x1006 * x1010)))),
8880 -1);
8881 if (!x1016.valid)
8882 {
8883 continue;
8884 }
8885 j6array[0]
8886 = ((-1.5707963267949) + (x1015.value)
8887 + (((1.5707963267949) * (x1016.value))));
8888 sj6array[0] = IKsin(j6array[0]);
8889 cj6array[0] = IKcos(j6array[0]);
8890 if (j6array[0] > IKPI)
8891 {
8892 j6array[0] -= IK2PI;
8893 }
8894 else if (j6array[0] < -IKPI)
8895 {
8896 j6array[0] += IK2PI;
8897 }
8898 j6valid[0] = true;
8899 for (int ij6 = 0; ij6 < 1; ++ij6)
8900 {
8901 if (!j6valid[ij6])
8902 {
8903 continue;
8904 }
8905 _ij6[0] = ij6;
8906 _ij6[1] = -1;
8907 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
8908 {
8909 if (j6valid[iij6]
8910 && IKabs(cj6array[ij6] - cj6array[iij6])
8911 < IKFAST_SOLUTION_THRESH
8912 && IKabs(sj6array[ij6] - sj6array[iij6])
8913 < IKFAST_SOLUTION_THRESH)
8914 {
8915 j6valid[iij6] = false;
8916 _ij6[1] = iij6;
8917 break;
8918 }
8919 }
8920 j6 = j6array[ij6];
8921 cj6 = cj6array[ij6];
8922 sj6 = sj6array[ij6];
8923 {
8924 IkReal evalcond[6];
8925 IkReal x1017 = ((0.3) * cj9);
8926 IkReal x1018 = ((0.045) * sj9);
8927 IkReal x1019 = IKcos(j6);
8928 IkReal x1020 = (pz * x1019);
8929 IkReal x1021 = IKsin(j6);
8930 IkReal x1022 = (cj4 * px * x1021);
8931 IkReal x1023 = (py * sj4);
8932 IkReal x1024 = (x1021 * x1023);
8933 IkReal x1025 = (px * sj4);
8934 IkReal x1026 = ((1.0) * cj4 * py);
8935 IkReal x1027 = (cj4 * sj8);
8936 IkReal x1028 = ((0.045) * cj8);
8937 IkReal x1029 = ((0.045) * cj9);
8938 IkReal x1030 = (cj8 * x1021);
8939 IkReal x1031 = ((0.3) * sj9);
8940 IkReal x1032 = (sj8 * x1025);
8941 IkReal x1033 = (pz * x1030);
8942 IkReal x1034 = (px * (((1.0) * cj4)));
8943 IkReal x1035 = (cj8 * x1019);
8944 IkReal x1036 = ((1.0) * x1023);
8945 IkReal x1037 = ((0.09) * cj8 * x1019);
8946 evalcond[0]
8947 = ((-0.55) + x1020 + x1022 + x1024
8948 + (((-1.0) * x1018))
8949 + (((-1.0) * x1017)));
8950 evalcond[1]
8951 = ((((-1.0) * cj8 * x1026))
8952 + (((-1.0) * pz * sj8 * x1021))
8953 + ((px * x1019 * x1027))
8954 + ((cj8 * x1025))
8955 + ((sj8 * x1019 * x1023)));
8956 evalcond[2]
8957 = ((((-1.0) * x1018 * x1019))
8958 + (((-1.0) * x1029 * x1030))
8959 + ((x1030 * x1031))
8960 + ((x1021 * x1028))
8961 + (((-0.55) * x1019)) + pz
8962 + (((-1.0) * x1017 * x1019)));
8963 evalcond[3]
8964 = ((0.045) + (((-1.0) * x1029))
8965 + (((-1.0) * x1035 * x1036))
8966 + (((-1.0) * x1034 * x1035)) + x1031
8967 + x1033 + x1032
8968 + (((-1.0) * sj8 * x1026)));
8969 evalcond[4]
8970 = (((x1031 * x1035))
8971 + (((0.55) * x1021))
8972 + (((-1.0) * x1036))
8973 + ((x1019 * x1028))
8974 + ((x1017 * x1021))
8975 + ((x1018 * x1021))
8976 + (((-1.0) * x1029 * x1035))
8977 + (((-1.0) * x1034)));
8978 evalcond[5]
8979 = ((-0.2125) + ((cj4 * px * x1037))
8980 + ((x1023 * x1037))
8981 + (((1.1) * x1024))
8982 + (((-0.09) * x1032))
8983 + (((0.09) * py * x1027))
8984 + (((1.1) * x1020))
8985 + (((-1.0) * (1.0) * pp))
8986 + (((1.1) * x1022))
8987 + (((-0.09) * x1033)));
8988 if (IKabs(evalcond[0])
8989 > IKFAST_EVALCOND_THRESH
8990 || IKabs(evalcond[1])
8991 > IKFAST_EVALCOND_THRESH
8992 || IKabs(evalcond[2])
8993 > IKFAST_EVALCOND_THRESH
8994 || IKabs(evalcond[3])
8995 > IKFAST_EVALCOND_THRESH
8996 || IKabs(evalcond[4])
8997 > IKFAST_EVALCOND_THRESH
8998 || IKabs(evalcond[5])
8999 > IKFAST_EVALCOND_THRESH)
9000 {
9001 continue;
9002 }
9003 }
9004
9005 rotationfunction0(solutions);
9006 }
9007 }
9008 }
9009 }
9010 }
9011 }
9012 }
9013 }
9014 }
9015 else
9016 {
9017 {
9018 IkReal j6array[2], cj6array[2], sj6array[2];
9019 bool j6valid[2] = {false};
9020 _nj6 = 2;
9021 IkReal x1038
9022 = ((-0.55) + (((-1.0) * (0.3) * cj9))
9023 + (((-1.0) * (0.045) * sj9)));
9024 IkReal x1039 = ((0.045) * cj8);
9025 IkReal x1040
9026 = ((((-1.0) * cj9 * x1039)) + x1039
9027 + (((0.3) * cj8 * sj9)));
9028 CheckValue<IkReal> x1043 = IKatan2WithCheck(
9029 IkReal(x1038), x1040, IKFAST_ATAN2_MAGTHRESH);
9030 if (!x1043.valid)
9031 {
9032 continue;
9033 }
9034 IkReal x1041 = ((-1.0) * (x1043.value));
9035 if ((((x1040 * x1040) + (x1038 * x1038))) < -0.00001)
9036 continue;
9037 CheckValue<IkReal> x1044 = IKPowWithIntegerCheck(
9038 IKabs(IKsqrt(((x1040 * x1040) + (x1038 * x1038)))), -1);
9039 if (!x1044.valid)
9040 {
9041 continue;
9042 }
9043 if (((pz * (x1044.value))) < -1 - IKFAST_SINCOS_THRESH
9044 || ((pz * (x1044.value))) > 1 + IKFAST_SINCOS_THRESH)
9045 continue;
9046 IkReal x1042 = IKasin((pz * (x1044.value)));
9047 j6array[0] = ((((-1.0) * x1042)) + x1041);
9048 sj6array[0] = IKsin(j6array[0]);
9049 cj6array[0] = IKcos(j6array[0]);
9050 j6array[1] = ((3.14159265358979) + x1042 + x1041);
9051 sj6array[1] = IKsin(j6array[1]);
9052 cj6array[1] = IKcos(j6array[1]);
9053 if (j6array[0] > IKPI)
9054 {
9055 j6array[0] -= IK2PI;
9056 }
9057 else if (j6array[0] < -IKPI)
9058 {
9059 j6array[0] += IK2PI;
9060 }
9061 j6valid[0] = true;
9062 if (j6array[1] > IKPI)
9063 {
9064 j6array[1] -= IK2PI;
9065 }
9066 else if (j6array[1] < -IKPI)
9067 {
9068 j6array[1] += IK2PI;
9069 }
9070 j6valid[1] = true;
9071 for (int ij6 = 0; ij6 < 2; ++ij6)
9072 {
9073 if (!j6valid[ij6])
9074 {
9075 continue;
9076 }
9077 _ij6[0] = ij6;
9078 _ij6[1] = -1;
9079 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
9080 {
9081 if (j6valid[iij6]
9082 && IKabs(cj6array[ij6] - cj6array[iij6])
9083 < IKFAST_SOLUTION_THRESH
9084 && IKabs(sj6array[ij6] - sj6array[iij6])
9085 < IKFAST_SOLUTION_THRESH)
9086 {
9087 j6valid[iij6] = false;
9088 _ij6[1] = iij6;
9089 break;
9090 }
9091 }
9092 j6 = j6array[ij6];
9093 cj6 = cj6array[ij6];
9094 sj6 = sj6array[ij6];
9095
9096 {
9097 IkReal j4eval[2];
9098 IkReal x1045
9099 = ((((-1.0) * (1.0) * sj6 * (pz * pz)))
9100 + ((pp * sj6)));
9101 j4eval[0] = x1045;
9102 j4eval[1] = IKsign(x1045);
9103 if (IKabs(j4eval[0]) < 0.0000010000000000
9104 || IKabs(j4eval[1]) < 0.0000010000000000)
9105 {
9106 {
9107 IkReal j4eval[2];
9108 IkReal x1046 = (cj8 * sj6);
9109 IkReal x1047
9110 = (((x1046 * (pz * pz)))
9111 + (((-1.0) * pp * x1046)));
9112 j4eval[0] = x1047;
9113 j4eval[1] = IKsign(x1047);
9114 if (IKabs(j4eval[0]) < 0.0000010000000000
9115 || IKabs(j4eval[1]) < 0.0000010000000000)
9116 {
9117 {
9118 IkReal j4eval[2];
9119 IkReal x1048
9120 = (pp + (((-1.0) * (1.0) * (pz * pz))));
9121 j4eval[0] = x1048;
9122 j4eval[1] = IKsign(x1048);
9123 if (IKabs(j4eval[0]) < 0.0000010000000000
9124 || IKabs(j4eval[1]) < 0.0000010000000000)
9125 {
9126 {
9127 IkReal evalcond[4];
9128 bool bgotonextstatement = true;
9129 do
9130 {
9131 evalcond[0]
9132 = ((-3.14159265358979)
9133 + (IKfmod(
9134 ((3.14159265358979)
9135 + (IKabs(
9136 ((-1.5707963267949)
9137 + j8)))),
9138 6.28318530717959)));
9139 evalcond[1]
9140 = ((0.39655) + (((0.0765) * sj9))
9141 + (((0.32595) * cj9))
9142 + (((-1.0) * (1.0) * pp)));
9143 evalcond[2]
9144 = ((((-1.0) * (0.3) * cj6 * cj9))
9145 + (((-1.0) * (0.55) * cj6)) + pz
9146 + (((-1.0) * (0.045) * cj6
9147 * sj9)));
9148 if (IKabs(evalcond[0])
9149 < 0.0000010000000000
9150 && IKabs(evalcond[1])
9151 < 0.0000010000000000
9152 && IKabs(evalcond[2])
9153 < 0.0000010000000000)
9154 {
9155 bgotonextstatement = false;
9156 {
9157 IkReal j4eval[3];
9158 sj8 = 1.0;
9159 cj8 = 0;
9160 j8 = 1.5707963267949;
9161 IkReal x1049
9162 = ((((-1.0) * (1.0) * cj6
9163 * (pz * pz)))
9164 + ((cj6 * pp)));
9165 IkReal x1050
9166 = ((1.51009803921569) * cj6);
9167 IkReal x1051 = (pz * sj6);
9168 IkReal x1052
9169 = ((1.32323529411765) * cj6
9170 * cj9);
9171 IkReal x1053
9172 = ((3.92156862745098) * cj6 * pp);
9173 j4eval[0] = x1049;
9174 j4eval[1] = IKsign(x1049);
9175 j4eval[2]
9176 = ((IKabs(
9177 (((py * x1053))
9178 + (((-1.0) * py * x1052))
9179 + (((-1.0) * py * x1050))
9180 + ((px * x1051)))))
9181 + (IKabs(
9182 (((px * x1052))
9183 + ((px * x1050))
9184 + ((py * x1051))
9185 + (((-1.0) * px
9186 * x1053))))));
9187 if (IKabs(j4eval[0])
9188 < 0.0000010000000000
9189 || IKabs(j4eval[1])
9190 < 0.0000010000000000
9191 || IKabs(j4eval[2])
9192 < 0.0000010000000000)
9193 {
9194 {
9195 IkReal j4eval[3];
9196 sj8 = 1.0;
9197 cj8 = 0;
9198 j8 = 1.5707963267949;
9199 IkReal x1054 = (cj6 * pp);
9200 IkReal x1055 = (cj6 * (pz * pz));
9201 IkReal x1056 = ((0.2125) * cj6);
9202 IkReal x1057 = ((1.1) * pz);
9203 IkReal x1058
9204 = ((0.09) * pz * sj6);
9205 j4eval[0]
9206 = ((((-1.0) * x1055))
9207 + x1054);
9208 j4eval[1] = IKsign(
9209 ((((-0.09) * x1055))
9210 + (((0.09) * x1054))));
9211 j4eval[2]
9212 = ((IKabs(
9213 (((py * x1058))
9214 + ((px * x1057))
9215 + (((-1.0) * px
9216 * x1054))
9217 + (((-1.0) * px
9218 * x1056)))))
9219 + (IKabs(
9220 (((py * x1056))
9221 + ((py * x1054))
9222 + (((-1.0) * py
9223 * x1057))
9224 + ((px * x1058))))));
9225 if (IKabs(j4eval[0])
9226 < 0.0000010000000000
9227 || IKabs(j4eval[1])
9228 < 0.0000010000000000
9229 || IKabs(j4eval[2])
9230 < 0.0000010000000000)
9231 {
9232 {
9233 IkReal j4eval[3];
9234 sj8 = 1.0;
9235 cj8 = 0;
9236 j8 = 1.5707963267949;
9237 IkReal x1059
9238 = (pp
9239 + (((-1.0) * (1.0)
9240 * (pz * pz))));
9241 IkReal x1060
9242 = ((1.32323529411765)
9243 * cj9);
9244 IkReal x1061
9245 = ((3.92156862745098)
9246 * pp);
9247 IkReal x1062
9248 = ((0.316735294117647)
9249 * sj6);
9250 IkReal x1063
9251 = ((0.108264705882353)
9252 * cj9 * sj6);
9253 IkReal x1064
9254 = ((0.588235294117647)
9255 * pp * sj6);
9256 j4eval[0] = x1059;
9257 j4eval[1]
9258 = ((IKabs((
9259 (((-1.0)
9260 * (1.51009803921569)
9261 * py))
9262 + ((px * x1064))
9263 + (((-1.0) * py
9264 * x1060))
9265 + ((px * x1063))
9266 + ((py * x1061))
9267 + ((px * x1062)))))
9268 + (IKabs((
9269 ((px * x1060))
9270 + (((-1.0) * px
9271 * x1061))
9272 + (((1.51009803921569)
9273 * px))
9274 + ((py * x1063))
9275 + ((py * x1062))
9276 + ((py
9277 * x1064))))));
9278 j4eval[2] = IKsign(x1059);
9279 if (IKabs(j4eval[0])
9280 < 0.0000010000000000
9281 || IKabs(j4eval[1])
9282 < 0.0000010000000000
9283 || IKabs(j4eval[2])
9284 < 0.0000010000000000)
9285 {
9286 {
9287 IkReal evalcond[7];
9288 bool bgotonextstatement
9289 = true;
9290 do
9291 {
9292 evalcond[0]
9293 = ((-3.14159265358979)
9294 + (IKfmod(
9295 ((3.14159265358979)
9296 + (IKabs((
9297 (-1.5707963267949)
9298 + j6)))),
9299 6.28318530717959)));
9300 evalcond[1]
9301 = ((-1.0)
9302 * (((1.0)
9303 * pz)));
9304 if (IKabs(evalcond[0])
9305 < 0.0000010000000000
9306 && IKabs(
9307 evalcond[1])
9308 < 0.0000010000000000)
9309 {
9310 bgotonextstatement
9311 = false;
9312 {
9313 IkReal j4eval[3];
9314 sj8 = 1.0;
9315 cj8 = 0;
9316 j8 = 1.5707963267949;
9317 sj6 = 1.0;
9318 cj6 = 0;
9319 j6 = 1.5707963267949;
9320 IkReal x1065
9321 = (pp
9322 + (((-1.0)
9323 * (1.0)
9324 * (pz
9325 * pz))));
9326 IkReal x1066
9327 = (cj9 * px);
9328 IkReal x1067
9329 = (cj9 * py);
9330 IkReal x1068
9331 = ((3.92156862745098)
9332 * pp);
9333 IkReal x1069
9334 = ((0.045)
9335 * sj9);
9336 j4eval[0] = x1065;
9337 j4eval[1]
9338 = ((IKabs((
9339 (((1.51009803921569)
9340 * px))
9341 + ((py
9342 * x1069))
9343 + (((0.55)
9344 * py))
9345 + (((0.3)
9346 * x1067))
9347 + (((1.32323529411765)
9348 * x1066))
9349 + (((-1.0)
9350 * px
9351 * x1068)))))
9352 + (IKabs((
9353 (((0.3)
9354 * x1066))
9355 + (((-1.0)
9356 * (1.51009803921569)
9357 * py))
9358 + (((-1.32323529411765)
9359 * x1067))
9360 + (((0.55)
9361 * px))
9362 + ((py
9363 * x1068))
9364 + ((px
9365 * x1069))))));
9366 j4eval[2]
9367 = IKsign(x1065);
9368 if (IKabs(j4eval[0])
9369 < 0.0000010000000000
9370 || IKabs(
9371 j4eval
9372 [1])
9373 < 0.0000010000000000
9374 || IKabs(
9375 j4eval
9376 [2])
9377 < 0.0000010000000000)
9378 {
9379 {
9380 IkReal
9381 j4eval[3];
9382 sj8 = 1.0;
9383 cj8 = 0;
9384 j8 = 1.5707963267949;
9385 sj6 = 1.0;
9386 cj6 = 0;
9387 j6 = 1.5707963267949;
9388 IkReal x1070
9389 = (pp
9390 + (((-1.0)
9391 * (1.0)
9392 * (pz
9393 * pz))));
9394 IkReal x1071
9395 = (cj9
9396 * px);
9397 IkReal x1072
9398 = (cj9
9399 * py);
9400 IkReal x1073
9401 = (pp * px);
9402 IkReal x1074
9403 = (pp * py);
9404 j4eval[0]
9405 = x1070;
9406 j4eval[1]
9407 = ((IKabs((
9408 (((0.108264705882353)
9409 * x1072))
9410 + (((0.588235294117647)
9411 * x1074))
9412 + (((1.32323529411765)
9413 * x1071))
9414 + (((0.316735294117647)
9415 * py))
9416 + (((1.51009803921569)
9417 * px))
9418 + (((-3.92156862745098)
9419 * x1073)))))
9420 + (IKabs((
9421 (((-1.32323529411765)
9422 * x1072))
9423 + (((0.588235294117647)
9424 * x1073))
9425 + (((3.92156862745098)
9426 * x1074))
9427 + (((0.316735294117647)
9428 * px))
9429 + (((0.108264705882353)
9430 * x1071))
9431 + (((-1.0)
9432 * (1.51009803921569)
9433 * py))))));
9434 j4eval[2]
9435 = IKsign(
9436 x1070);
9437 if (IKabs(
9438 j4eval
9439 [0])
9440 < 0.0000010000000000
9441 || IKabs(
9442 j4eval
9443 [1])
9444 < 0.0000010000000000
9445 || IKabs(
9446 j4eval
9447 [2])
9448 < 0.0000010000000000)
9449 {
9450 {
9451 IkReal
9452 j4eval
9453 [3];
9454 sj8 = 1.0;
9455 cj8 = 0;
9456 j8 = 1.5707963267949;
9457 sj6 = 1.0;
9458 cj6 = 0;
9459 j6 = 1.5707963267949;
9460 IkReal x1075
9461 = pz
9462 * pz;
9463 IkReal x1076
9464 = (cj9
9465 * px);
9466 IkReal x1077
9467 = (cj9
9468 * py);
9469 IkReal x1078
9470 = (pp
9471 * px);
9472 IkReal x1079
9473 = (pp
9474 * py);
9475 j4eval[0]
9476 = ((((-1.0)
9477 * x1075))
9478 + pp);
9479 j4eval[1]
9480 = ((IKabs((
9481 (((0.348408823529412)
9482 * py))
9483 + (((1.66110784313725)
9484 * px))
9485 + (((1.45555882352941)
9486 * x1076))
9487 + (((0.647058823529412)
9488 * x1079))
9489 + (((-4.31372549019608)
9490 * x1078))
9491 + (((0.119091176470588)
9492 * x1077)))))
9493 + (IKabs((
9494 (((0.348408823529412)
9495 * px))
9496 + (((-1.45555882352941)
9497 * x1077))
9498 + (((4.31372549019608)
9499 * x1079))
9500 + (((0.647058823529412)
9501 * x1078))
9502 + (((-1.0)
9503 * (1.66110784313725)
9504 * py))
9505 + (((0.119091176470588)
9506 * x1076))))));
9507 j4eval[2] = IKsign((
9508 (((1.1)
9509 * pp))
9510 + (((-1.1)
9511 * x1075))));
9512 if (IKabs(
9513 j4eval
9514 [0])
9515 < 0.0000010000000000
9516 || IKabs(
9517 j4eval
9518 [1])
9519 < 0.0000010000000000
9520 || IKabs(
9521 j4eval
9522 [2])
9523 < 0.0000010000000000)
9524 {
9525 {
9526 IkReal evalcond
9527 [6];
9528 bool
9529 bgotonextstatement
9530 = true;
9531 do
9532 {
9533 IkReal
9534 x1080
9535 = ((-1.51009803921569)
9536 + (((-1.0)
9537 * (1.32323529411765)
9538 * cj9))
9539 + (((3.92156862745098)
9540 * pp)));
9541 evalcond
9542 [0]
9543 = ((IKabs(
9544 py))
9545 + (IKabs(
9546 px)));
9547 evalcond
9548 [1]
9549 = ((-0.55)
9550 + (((-1.0)
9551 * (0.3)
9552 * cj9))
9553 + (((-1.0)
9554 * (0.045)
9555 * sj9)));
9556 evalcond
9557 [2]
9558 = x1080;
9559 evalcond
9560 [3]
9561 = x1080;
9562 evalcond
9563 [4]
9564 = ((0.316735294117647)
9565 + (((0.108264705882353)
9566 * cj9))
9567 + (((0.588235294117647)
9568 * pp)));
9569 evalcond
9570 [5]
9571 = ((-0.2125)
9572 + (((-1.0)
9573 * (1.0)
9574 * pp)));
9575 if (IKabs(
9576 evalcond
9577 [0])
9578 < 0.0000010000000000
9579 && IKabs(
9580 evalcond
9581 [1])
9582 < 0.0000010000000000
9583 && IKabs(
9584 evalcond
9585 [2])
9586 < 0.0000010000000000
9587 && IKabs(
9588 evalcond
9589 [3])
9590 < 0.0000010000000000
9591 && IKabs(
9592 evalcond
9593 [4])
9594 < 0.0000010000000000
9595 && IKabs(
9596 evalcond
9597 [5])
9598 < 0.0000010000000000)
9599 {
9600 bgotonextstatement
9601 = false;
9602 {
9603 IkReal j4array
9604 [4],
9605 cj4array
9606 [4],
9607 sj4array
9608 [4];
9609 bool j4valid
9610 [4]
9611 = {false};
9612 _nj4
9613 = 4;
9614 j4array
9615 [0]
9616 = 0;
9617 sj4array
9618 [0]
9619 = IKsin(
9620 j4array
9621 [0]);
9622 cj4array
9623 [0]
9624 = IKcos(
9625 j4array
9626 [0]);
9627 j4array
9628 [1]
9629 = 1.5707963267949;
9630 sj4array
9631 [1]
9632 = IKsin(
9633 j4array
9634 [1]);
9635 cj4array
9636 [1]
9637 = IKcos(
9638 j4array
9639 [1]);
9640 j4array
9641 [2]
9642 = 3.14159265358979;
9643 sj4array
9644 [2]
9645 = IKsin(
9646 j4array
9647 [2]);
9648 cj4array
9649 [2]
9650 = IKcos(
9651 j4array
9652 [2]);
9653 j4array
9654 [3]
9655 = -1.5707963267949;
9656 sj4array
9657 [3]
9658 = IKsin(
9659 j4array
9660 [3]);
9661 cj4array
9662 [3]
9663 = IKcos(
9664 j4array
9665 [3]);
9666 if (j4array
9667 [0]
9668 > IKPI)
9669 {
9670 j4array
9671 [0]
9672 -= IK2PI;
9673 }
9674 else if (
9675 j4array
9676 [0]
9677 < -IKPI)
9678 {
9679 j4array
9680 [0]
9681 += IK2PI;
9682 }
9683 j4valid
9684 [0]
9685 = true;
9686 if (j4array
9687 [1]
9688 > IKPI)
9689 {
9690 j4array
9691 [1]
9692 -= IK2PI;
9693 }
9694 else if (
9695 j4array
9696 [1]
9697 < -IKPI)
9698 {
9699 j4array
9700 [1]
9701 += IK2PI;
9702 }
9703 j4valid
9704 [1]
9705 = true;
9706 if (j4array
9707 [2]
9708 > IKPI)
9709 {
9710 j4array
9711 [2]
9712 -= IK2PI;
9713 }
9714 else if (
9715 j4array
9716 [2]
9717 < -IKPI)
9718 {
9719 j4array
9720 [2]
9721 += IK2PI;
9722 }
9723 j4valid
9724 [2]
9725 = true;
9726 if (j4array
9727 [3]
9728 > IKPI)
9729 {
9730 j4array
9731 [3]
9732 -= IK2PI;
9733 }
9734 else if (
9735 j4array
9736 [3]
9737 < -IKPI)
9738 {
9739 j4array
9740 [3]
9741 += IK2PI;
9742 }
9743 j4valid
9744 [3]
9745 = true;
9746 for (
9747 int ij4
9748 = 0;
9749 ij4
9750 < 4;
9751 ++ij4)
9752 {
9753 if (!j4valid
9754 [ij4])
9755 {
9756 continue;
9757 }
9758 _ij4[0]
9759 = ij4;
9760 _ij4[1]
9761 = -1;
9762 for (
9763 int iij4
9764 = ij4
9765 + 1;
9766 iij4
9767 < 4;
9768 ++iij4)
9769 {
9770 if (j4valid
9771 [iij4]
9772 && IKabs(
9773 cj4array
9774 [ij4]
9775 - cj4array
9776 [iij4])
9777 < IKFAST_SOLUTION_THRESH
9778 && IKabs(
9779 sj4array
9780 [ij4]
9781 - sj4array
9782 [iij4])
9783 < IKFAST_SOLUTION_THRESH)
9784 {
9785 j4valid
9786 [iij4]
9787 = false;
9788 _ij4[1]
9789 = iij4;
9790 break;
9791 }
9792 }
9793 j4 = j4array
9794 [ij4];
9795 cj4 = cj4array
9796 [ij4];
9797 sj4 = sj4array
9798 [ij4];
9799
9800 rotationfunction0(
9801 solutions);
9802 }
9803 }
9804 }
9805 } while (
9806 0);
9807 if (bgotonextstatement)
9808 {
9809 bool
9810 bgotonextstatement
9811 = true;
9812 do
9813 {
9814 if (1)
9815 {
9816 bgotonextstatement
9817 = false;
9818 continue; // branch miss [j4]
9819 }
9820 } while (
9821 0);
9822 if (bgotonextstatement)
9823 {
9824 }
9825 }
9826 }
9827 }
9828 else
9829 {
9830 {
9831 IkReal j4array
9832 [1],
9833 cj4array
9834 [1],
9835 sj4array
9836 [1];
9837 bool j4valid
9838 [1]
9839 = {false};
9840 _nj4
9841 = 1;
9842 IkReal
9843 x1081
9844 = (cj9
9845 * px);
9846 IkReal
9847 x1082
9848 = (cj9
9849 * py);
9850 IkReal
9851 x1083
9852 = (pp
9853 * px);
9854 IkReal
9855 x1084
9856 = (pp
9857 * py);
9858 CheckValue<IkReal> x1085 = IKatan2WithCheck(
9859 IkReal((
9860 (((0.119091176470588)
9861 * x1082))
9862 + (((0.348408823529412)
9863 * py))
9864 + (((1.66110784313725)
9865 * px))
9866 + (((0.647058823529412)
9867 * x1084))
9868 + (((-4.31372549019608)
9869 * x1083))
9870 + (((1.45555882352941)
9871 * x1081)))),
9872 ((((4.31372549019608)
9873 * x1084))
9874 + (((0.119091176470588)
9875 * x1081))
9876 + (((0.348408823529412)
9877 * px))
9878 + (((0.647058823529412)
9879 * x1083))
9880 + (((-1.45555882352941)
9881 * x1082))
9882 + (((-1.0)
9883 * (1.66110784313725)
9884 * py))),
9885 IKFAST_ATAN2_MAGTHRESH);
9886 if (!x1085
9887 .valid)
9888 {
9889 continue;
9890 }
9891 CheckValue<IkReal> x1086 = IKPowWithIntegerCheck(
9892 IKsign((
9893 (((1.1)
9894 * pp))
9895 + (((-1.0)
9896 * (1.1)
9897 * (pz
9898 * pz))))),
9899 -1);
9900 if (!x1086
9901 .valid)
9902 {
9903 continue;
9904 }
9905 j4array
9906 [0]
9907 = ((-1.5707963267949)
9908 + (x1085
9909 .value)
9910 + (((1.5707963267949)
9911 * (x1086
9912 .value))));
9913 sj4array
9914 [0]
9915 = IKsin(
9916 j4array
9917 [0]);
9918 cj4array
9919 [0]
9920 = IKcos(
9921 j4array
9922 [0]);
9923 if (j4array
9924 [0]
9925 > IKPI)
9926 {
9927 j4array
9928 [0]
9929 -= IK2PI;
9930 }
9931 else if (
9932 j4array
9933 [0]
9934 < -IKPI)
9935 {
9936 j4array
9937 [0]
9938 += IK2PI;
9939 }
9940 j4valid
9941 [0]
9942 = true;
9943 for (
9944 int ij4
9945 = 0;
9946 ij4
9947 < 1;
9948 ++ij4)
9949 {
9950 if (!j4valid
9951 [ij4])
9952 {
9953 continue;
9954 }
9955 _ij4[0]
9956 = ij4;
9957 _ij4[1]
9958 = -1;
9959 for (
9960 int iij4
9961 = ij4
9962 + 1;
9963 iij4
9964 < 1;
9965 ++iij4)
9966 {
9967 if (j4valid
9968 [iij4]
9969 && IKabs(
9970 cj4array
9971 [ij4]
9972 - cj4array
9973 [iij4])
9974 < IKFAST_SOLUTION_THRESH
9975 && IKabs(
9976 sj4array
9977 [ij4]
9978 - sj4array
9979 [iij4])
9980 < IKFAST_SOLUTION_THRESH)
9981 {
9982 j4valid
9983 [iij4]
9984 = false;
9985 _ij4[1]
9986 = iij4;
9987 break;
9988 }
9989 }
9990 j4 = j4array
9991 [ij4];
9992 cj4 = cj4array
9993 [ij4];
9994 sj4 = sj4array
9995 [ij4];
9996 {
9997 IkReal evalcond
9998 [4];
9999 IkReal
10000 x1087
10001 = IKcos(
10002 j4);
10003 IkReal
10004 x1088
10005 = (px
10006 * x1087);
10007 IkReal
10008 x1089
10009 = IKsin(
10010 j4);
10011 IkReal
10012 x1090
10013 = (py
10014 * x1089);
10015 IkReal
10016 x1091
10017 = (px
10018 * x1089);
10019 IkReal
10020 x1092
10021 = (py
10022 * x1087);
10023 evalcond
10024 [0]
10025 = ((-0.55)
10026 + x1088
10027 + (((-1.0)
10028 * (0.3)
10029 * cj9))
10030 + x1090
10031 + (((-1.0)
10032 * (0.045)
10033 * sj9)));
10034 evalcond
10035 [1]
10036 = ((-1.51009803921569)
10037 + (((-1.0)
10038 * x1092))
10039 + (((-1.0)
10040 * (1.32323529411765)
10041 * cj9))
10042 + (((3.92156862745098)
10043 * pp))
10044 + x1091);
10045 evalcond
10046 [2]
10047 = ((0.316735294117647)
10048 + (((-1.0)
10049 * x1090))
10050 + (((-1.0)
10051 * x1088))
10052 + (((0.108264705882353)
10053 * cj9))
10054 + (((0.588235294117647)
10055 * pp)));
10056 evalcond
10057 [3]
10058 = ((-0.2125)
10059 + (((1.1)
10060 * x1090))
10061 + (((0.09)
10062 * x1092))
10063 + (((-0.09)
10064 * x1091))
10065 + (((1.1)
10066 * x1088))
10067 + (((-1.0)
10068 * (1.0)
10069 * pp)));
10070 if (IKabs(
10071 evalcond
10072 [0])
10073 > IKFAST_EVALCOND_THRESH
10074 || IKabs(
10075 evalcond
10076 [1])
10077 > IKFAST_EVALCOND_THRESH
10078 || IKabs(
10079 evalcond
10080 [2])
10081 > IKFAST_EVALCOND_THRESH
10082 || IKabs(
10083 evalcond
10084 [3])
10085 > IKFAST_EVALCOND_THRESH)
10086 {
10087 continue;
10088 }
10089 }
10090
10091 rotationfunction0(
10092 solutions);
10093 }
10094 }
10095 }
10096 }
10097 }
10098 else
10099 {
10100 {
10101 IkReal
10102 j4array
10103 [1],
10104 cj4array
10105 [1],
10106 sj4array
10107 [1];
10108 bool j4valid
10109 [1]
10110 = {false};
10111 _nj4 = 1;
10112 IkReal x1093
10113 = (cj9
10114 * px);
10115 IkReal x1094
10116 = (cj9
10117 * py);
10118 IkReal x1095
10119 = (pp
10120 * px);
10121 IkReal x1096
10122 = (pp
10123 * py);
10124 CheckValue<IkReal> x1097 = IKatan2WithCheck(
10125 IkReal((
10126 (((-3.92156862745098)
10127 * x1095))
10128 + (((0.316735294117647)
10129 * py))
10130 + (((1.51009803921569)
10131 * px))
10132 + (((0.108264705882353)
10133 * x1094))
10134 + (((1.32323529411765)
10135 * x1093))
10136 + (((0.588235294117647)
10137 * x1096)))),
10138 ((((0.108264705882353)
10139 * x1093))
10140 + (((0.588235294117647)
10141 * x1095))
10142 + (((-1.32323529411765)
10143 * x1094))
10144 + (((0.316735294117647)
10145 * px))
10146 + (((-1.0)
10147 * (1.51009803921569)
10148 * py))
10149 + (((3.92156862745098)
10150 * x1096))),
10151 IKFAST_ATAN2_MAGTHRESH);
10152 if (!x1097
10153 .valid)
10154 {
10155 continue;
10156 }
10157 CheckValue<IkReal> x1098 = IKPowWithIntegerCheck(
10158 IKsign((
10159 pp
10160 + (((-1.0)
10161 * (1.0)
10162 * (pz
10163 * pz))))),
10164 -1);
10165 if (!x1098
10166 .valid)
10167 {
10168 continue;
10169 }
10170 j4array[0]
10171 = ((-1.5707963267949)
10172 + (x1097
10173 .value)
10174 + (((1.5707963267949)
10175 * (x1098
10176 .value))));
10177 sj4array[0] = IKsin(
10178 j4array
10179 [0]);
10180 cj4array[0] = IKcos(
10181 j4array
10182 [0]);
10183 if (j4array
10184 [0]
10185 > IKPI)
10186 {
10187 j4array[0]
10188 -= IK2PI;
10189 }
10190 else if (
10191 j4array
10192 [0]
10193 < -IKPI)
10194 {
10195 j4array[0]
10196 += IK2PI;
10197 }
10198 j4valid[0]
10199 = true;
10200 for (int ij4
10201 = 0;
10202 ij4
10203 < 1;
10204 ++ij4)
10205 {
10206 if (!j4valid
10207 [ij4])
10208 {
10209 continue;
10210 }
10211 _ij4[0]
10212 = ij4;
10213 _ij4[1]
10214 = -1;
10215 for (
10216 int iij4
10217 = ij4
10218 + 1;
10219 iij4
10220 < 1;
10221 ++iij4)
10222 {
10223 if (j4valid
10224 [iij4]
10225 && IKabs(
10226 cj4array
10227 [ij4]
10228 - cj4array
10229 [iij4])
10230 < IKFAST_SOLUTION_THRESH
10231 && IKabs(
10232 sj4array
10233 [ij4]
10234 - sj4array
10235 [iij4])
10236 < IKFAST_SOLUTION_THRESH)
10237 {
10238 j4valid
10239 [iij4]
10240 = false;
10241 _ij4[1]
10242 = iij4;
10243 break;
10244 }
10245 }
10246 j4 = j4array
10247 [ij4];
10248 cj4 = cj4array
10249 [ij4];
10250 sj4 = sj4array
10251 [ij4];
10252 {
10253 IkReal evalcond
10254 [4];
10255 IkReal
10256 x1099
10257 = IKcos(
10258 j4);
10259 IkReal
10260 x1100
10261 = (px
10262 * x1099);
10263 IkReal
10264 x1101
10265 = IKsin(
10266 j4);
10267 IkReal
10268 x1102
10269 = (py
10270 * x1101);
10271 IkReal
10272 x1103
10273 = (px
10274 * x1101);
10275 IkReal
10276 x1104
10277 = (py
10278 * x1099);
10279 evalcond
10280 [0]
10281 = ((-0.55)
10282 + x1102
10283 + x1100
10284 + (((-1.0)
10285 * (0.3)
10286 * cj9))
10287 + (((-1.0)
10288 * (0.045)
10289 * sj9)));
10290 evalcond
10291 [1]
10292 = ((-1.51009803921569)
10293 + (((-1.0)
10294 * x1104))
10295 + (((-1.0)
10296 * (1.32323529411765)
10297 * cj9))
10298 + x1103
10299 + (((3.92156862745098)
10300 * pp)));
10301 evalcond
10302 [2]
10303 = ((0.316735294117647)
10304 + (((-1.0)
10305 * x1102))
10306 + (((-1.0)
10307 * x1100))
10308 + (((0.108264705882353)
10309 * cj9))
10310 + (((0.588235294117647)
10311 * pp)));
10312 evalcond
10313 [3]
10314 = ((-0.2125)
10315 + (((-0.09)
10316 * x1103))
10317 + (((1.1)
10318 * x1100))
10319 + (((1.1)
10320 * x1102))
10321 + (((-1.0)
10322 * (1.0)
10323 * pp))
10324 + (((0.09)
10325 * x1104)));
10326 if (IKabs(
10327 evalcond
10328 [0])
10329 > IKFAST_EVALCOND_THRESH
10330 || IKabs(
10331 evalcond
10332 [1])
10333 > IKFAST_EVALCOND_THRESH
10334 || IKabs(
10335 evalcond
10336 [2])
10337 > IKFAST_EVALCOND_THRESH
10338 || IKabs(
10339 evalcond
10340 [3])
10341 > IKFAST_EVALCOND_THRESH)
10342 {
10343 continue;
10344 }
10345 }
10346
10347 rotationfunction0(
10348 solutions);
10349 }
10350 }
10351 }
10352 }
10353 }
10354 else
10355 {
10356 {
10357 IkReal
10358 j4array[1],
10359 cj4array[1],
10360 sj4array[1];
10361 bool j4valid[1]
10362 = {false};
10363 _nj4 = 1;
10364 IkReal x1105
10365 = (cj9
10366 * px);
10367 IkReal x1106
10368 = (cj9
10369 * py);
10370 IkReal x1107
10371 = ((3.92156862745098)
10372 * pp);
10373 IkReal x1108
10374 = ((0.045)
10375 * sj9);
10376 CheckValue<IkReal> x1109 = IKPowWithIntegerCheck(
10377 IKsign((
10378 pp
10379 + (((-1.0)
10380 * (1.0)
10381 * (pz
10382 * pz))))),
10383 -1);
10384 if (!x1109
10385 .valid)
10386 {
10387 continue;
10388 }
10389 CheckValue<IkReal> x1110 = IKatan2WithCheck(
10390 IkReal((
10391 (((-1.0)
10392 * px
10393 * x1107))
10394 + ((py
10395 * x1108))
10396 + (((1.51009803921569)
10397 * px))
10398 + (((0.55)
10399 * py))
10400 + (((1.32323529411765)
10401 * x1105))
10402 + (((0.3)
10403 * x1106)))),
10404 ((((-1.32323529411765)
10405 * x1106))
10406 + (((-1.0)
10407 * (1.51009803921569)
10408 * py))
10409 + ((py
10410 * x1107))
10411 + (((0.55)
10412 * px))
10413 + ((px
10414 * x1108))
10415 + (((0.3)
10416 * x1105))),
10417 IKFAST_ATAN2_MAGTHRESH);
10418 if (!x1110
10419 .valid)
10420 {
10421 continue;
10422 }
10423 j4array[0]
10424 = ((-1.5707963267949)
10425 + (((1.5707963267949)
10426 * (x1109
10427 .value)))
10428 + (x1110
10429 .value));
10430 sj4array[0]
10431 = IKsin(
10432 j4array
10433 [0]);
10434 cj4array[0]
10435 = IKcos(
10436 j4array
10437 [0]);
10438 if (j4array[0]
10439 > IKPI)
10440 {
10441 j4array[0]
10442 -= IK2PI;
10443 }
10444 else if (
10445 j4array[0]
10446 < -IKPI)
10447 {
10448 j4array[0]
10449 += IK2PI;
10450 }
10451 j4valid[0]
10452 = true;
10453 for (int ij4
10454 = 0;
10455 ij4 < 1;
10456 ++ij4)
10457 {
10458 if (!j4valid
10459 [ij4])
10460 {
10461 continue;
10462 }
10463 _ij4[0] = ij4;
10464 _ij4[1] = -1;
10465 for (int iij4
10466 = ij4
10467 + 1;
10468 iij4 < 1;
10469 ++iij4)
10470 {
10471 if (j4valid
10472 [iij4]
10473 && IKabs(
10474 cj4array
10475 [ij4]
10476 - cj4array
10477 [iij4])
10478 < IKFAST_SOLUTION_THRESH
10479 && IKabs(
10480 sj4array
10481 [ij4]
10482 - sj4array
10483 [iij4])
10484 < IKFAST_SOLUTION_THRESH)
10485 {
10486 j4valid
10487 [iij4]
10488 = false;
10489 _ij4[1]
10490 = iij4;
10491 break;
10492 }
10493 }
10494 j4 = j4array
10495 [ij4];
10496 cj4 = cj4array
10497 [ij4];
10498 sj4 = sj4array
10499 [ij4];
10500 {
10501 IkReal
10502 evalcond
10503 [4];
10504 IkReal x1111
10505 = IKcos(
10506 j4);
10507 IkReal x1112
10508 = (px
10509 * x1111);
10510 IkReal x1113
10511 = IKsin(
10512 j4);
10513 IkReal x1114
10514 = (py
10515 * x1113);
10516 IkReal x1115
10517 = (px
10518 * x1113);
10519 IkReal x1116
10520 = (py
10521 * x1111);
10522 evalcond[0]
10523 = ((-0.55)
10524 + x1112
10525 + x1114
10526 + (((-1.0)
10527 * (0.3)
10528 * cj9))
10529 + (((-1.0)
10530 * (0.045)
10531 * sj9)));
10532 evalcond[1]
10533 = ((-1.51009803921569)
10534 + x1115
10535 + (((-1.0)
10536 * (1.32323529411765)
10537 * cj9))
10538 + (((-1.0)
10539 * x1116))
10540 + (((3.92156862745098)
10541 * pp)));
10542 evalcond[2]
10543 = ((0.316735294117647)
10544 + (((-1.0)
10545 * x1114))
10546 + (((0.108264705882353)
10547 * cj9))
10548 + (((0.588235294117647)
10549 * pp))
10550 + (((-1.0)
10551 * x1112)));
10552 evalcond[3]
10553 = ((-0.2125)
10554 + (((-0.09)
10555 * x1115))
10556 + (((1.1)
10557 * x1114))
10558 + (((1.1)
10559 * x1112))
10560 + (((-1.0)
10561 * (1.0)
10562 * pp))
10563 + (((0.09)
10564 * x1116)));
10565 if (IKabs(
10566 evalcond
10567 [0])
10568 > IKFAST_EVALCOND_THRESH
10569 || IKabs(
10570 evalcond
10571 [1])
10572 > IKFAST_EVALCOND_THRESH
10573 || IKabs(
10574 evalcond
10575 [2])
10576 > IKFAST_EVALCOND_THRESH
10577 || IKabs(
10578 evalcond
10579 [3])
10580 > IKFAST_EVALCOND_THRESH)
10581 {
10582 continue;
10583 }
10584 }
10585
10586 rotationfunction0(
10587 solutions);
10588 }
10589 }
10590 }
10591 }
10592 }
10593 } while (0);
10594 if (bgotonextstatement)
10595 {
10596 bool bgotonextstatement
10597 = true;
10598 do
10599 {
10600 evalcond[0]
10601 = ((-3.14159265358979)
10602 + (IKfmod(
10603 ((3.14159265358979)
10604 + (IKabs((
10605 (1.5707963267949)
10606 + j6)))),
10607 6.28318530717959)));
10608 evalcond[1] = pz;
10609 if (IKabs(evalcond[0])
10610 < 0.0000010000000000
10611 && IKabs(evalcond
10612 [1])
10613 < 0.0000010000000000)
10614 {
10615 bgotonextstatement
10616 = false;
10617 {
10618 IkReal j4eval[3];
10619 sj8 = 1.0;
10620 cj8 = 0;
10621 j8 = 1.5707963267949;
10622 sj6 = -1.0;
10623 cj6 = 0;
10624 j6 = -1.5707963267949;
10625 IkReal x1117
10626 = (pp
10627 + (((-1.0)
10628 * (1.0)
10629 * (pz
10630 * pz))));
10631 IkReal x1118
10632 = (cj9 * px);
10633 IkReal x1119
10634 = (cj9 * py);
10635 IkReal x1120
10636 = ((3.92156862745098)
10637 * pp);
10638 IkReal x1121
10639 = ((0.045)
10640 * sj9);
10641 j4eval[0] = x1117;
10642 j4eval[1]
10643 = ((IKabs((
10644 (((-0.3)
10645 * x1119))
10646 + (((1.51009803921569)
10647 * px))
10648 + (((1.32323529411765)
10649 * x1118))
10650 + (((-1.0)
10651 * px
10652 * x1120))
10653 + (((-1.0)
10654 * py
10655 * x1121))
10656 + (((-1.0)
10657 * (0.55)
10658 * py)))))
10659 + (IKabs((
10660 (((-1.0)
10661 * px
10662 * x1121))
10663 + (((-1.0)
10664 * (0.55)
10665 * px))
10666 + (((-0.3)
10667 * x1118))
10668 + (((-1.0)
10669 * (1.51009803921569)
10670 * py))
10671 + ((py
10672 * x1120))
10673 + (((-1.32323529411765)
10674 * x1119))))));
10675 j4eval[2]
10676 = IKsign(
10677 x1117);
10678 if (IKabs(
10679 j4eval[0])
10680 < 0.0000010000000000
10681 || IKabs(
10682 j4eval
10683 [1])
10684 < 0.0000010000000000
10685 || IKabs(
10686 j4eval
10687 [2])
10688 < 0.0000010000000000)
10689 {
10690 {
10691 IkReal
10692 j4eval[3];
10693 sj8 = 1.0;
10694 cj8 = 0;
10695 j8 = 1.5707963267949;
10696 sj6 = -1.0;
10697 cj6 = 0;
10698 j6 = -1.5707963267949;
10699 IkReal x1122
10700 = (pp
10701 + (((-1.0)
10702 * (1.0)
10703 * (pz
10704 * pz))));
10705 IkReal x1123
10706 = (cj9
10707 * px);
10708 IkReal x1124
10709 = (cj9
10710 * py);
10711 IkReal x1125
10712 = (pp
10713 * px);
10714 IkReal x1126
10715 = (pp
10716 * py);
10717 j4eval[0]
10718 = x1122;
10719 j4eval[1]
10720 = ((IKabs((
10721 (((-0.108264705882353)
10722 * x1124))
10723 + (((1.51009803921569)
10724 * px))
10725 + (((-1.0)
10726 * (0.316735294117647)
10727 * py))
10728 + (((-3.92156862745098)
10729 * x1125))
10730 + (((-0.588235294117647)
10731 * x1126))
10732 + (((1.32323529411765)
10733 * x1123)))))
10734 + (IKabs((
10735 (((-0.108264705882353)
10736 * x1123))
10737 + (((-1.0)
10738 * (1.51009803921569)
10739 * py))
10740 + (((-1.0)
10741 * (0.316735294117647)
10742 * px))
10743 + (((-0.588235294117647)
10744 * x1125))
10745 + (((3.92156862745098)
10746 * x1126))
10747 + (((-1.32323529411765)
10748 * x1124))))));
10749 j4eval[2]
10750 = IKsign(
10751 x1122);
10752 if (IKabs(
10753 j4eval
10754 [0])
10755 < 0.0000010000000000
10756 || IKabs(
10757 j4eval
10758 [1])
10759 < 0.0000010000000000
10760 || IKabs(
10761 j4eval
10762 [2])
10763 < 0.0000010000000000)
10764 {
10765 {
10766 IkReal j4eval
10767 [3];
10768 sj8 = 1.0;
10769 cj8 = 0;
10770 j8 = 1.5707963267949;
10771 sj6 = -1.0;
10772 cj6 = 0;
10773 j6 = -1.5707963267949;
10774 IkReal
10775 x1127
10776 = pz
10777 * pz;
10778 IkReal
10779 x1128
10780 = (cj9
10781 * px);
10782 IkReal
10783 x1129
10784 = (cj9
10785 * py);
10786 IkReal
10787 x1130
10788 = (pp
10789 * px);
10790 IkReal
10791 x1131
10792 = (pp
10793 * py);
10794 j4eval[0]
10795 = (x1127
10796 + (((-1.0)
10797 * (1.0)
10798 * pp)));
10799 j4eval[1] = IKsign((
10800 (((1.1)
10801 * x1127))
10802 + (((-1.0)
10803 * (1.1)
10804 * pp))));
10805 j4eval[2]
10806 = ((IKabs((
10807 (((0.348408823529412)
10808 * px))
10809 + (((1.66110784313725)
10810 * py))
10811 + (((0.119091176470588)
10812 * x1128))
10813 + (((0.647058823529412)
10814 * x1130))
10815 + (((1.45555882352941)
10816 * x1129))
10817 + (((-4.31372549019608)
10818 * x1131)))))
10819 + (IKabs((
10820 (((0.647058823529412)
10821 * x1131))
10822 + (((0.348408823529412)
10823 * py))
10824 + (((0.119091176470588)
10825 * x1129))
10826 + (((4.31372549019608)
10827 * x1130))
10828 + (((-1.0)
10829 * (1.66110784313725)
10830 * px))
10831 + (((-1.45555882352941)
10832 * x1128))))));
10833 if (IKabs(
10834 j4eval
10835 [0])
10836 < 0.0000010000000000
10837 || IKabs(
10838 j4eval
10839 [1])
10840 < 0.0000010000000000
10841 || IKabs(
10842 j4eval
10843 [2])
10844 < 0.0000010000000000)
10845 {
10846 {
10847 IkReal evalcond
10848 [6];
10849 bool
10850 bgotonextstatement
10851 = true;
10852 do
10853 {
10854 IkReal
10855 x1132
10856 = ((-1.51009803921569)
10857 + (((-1.0)
10858 * (1.32323529411765)
10859 * cj9))
10860 + (((3.92156862745098)
10861 * pp)));
10862 evalcond
10863 [0]
10864 = ((IKabs(
10865 py))
10866 + (IKabs(
10867 px)));
10868 evalcond
10869 [1]
10870 = ((-0.55)
10871 + (((-1.0)
10872 * (0.3)
10873 * cj9))
10874 + (((-1.0)
10875 * (0.045)
10876 * sj9)));
10877 evalcond
10878 [2]
10879 = x1132;
10880 evalcond
10881 [3]
10882 = x1132;
10883 evalcond
10884 [4]
10885 = ((-0.316735294117647)
10886 + (((-1.0)
10887 * (0.588235294117647)
10888 * pp))
10889 + (((-1.0)
10890 * (0.108264705882353)
10891 * cj9)));
10892 evalcond
10893 [5]
10894 = ((-0.2125)
10895 + (((-1.0)
10896 * (1.0)
10897 * pp)));
10898 if (IKabs(
10899 evalcond
10900 [0])
10901 < 0.0000010000000000
10902 && IKabs(
10903 evalcond
10904 [1])
10905 < 0.0000010000000000
10906 && IKabs(
10907 evalcond
10908 [2])
10909 < 0.0000010000000000
10910 && IKabs(
10911 evalcond
10912 [3])
10913 < 0.0000010000000000
10914 && IKabs(
10915 evalcond
10916 [4])
10917 < 0.0000010000000000
10918 && IKabs(
10919 evalcond
10920 [5])
10921 < 0.0000010000000000)
10922 {
10923 bgotonextstatement
10924 = false;
10925 {
10926 IkReal j4array
10927 [4],
10928 cj4array
10929 [4],
10930 sj4array
10931 [4];
10932 bool j4valid
10933 [4]
10934 = {false};
10935 _nj4
10936 = 4;
10937 j4array
10938 [0]
10939 = 0;
10940 sj4array
10941 [0]
10942 = IKsin(
10943 j4array
10944 [0]);
10945 cj4array
10946 [0]
10947 = IKcos(
10948 j4array
10949 [0]);
10950 j4array
10951 [1]
10952 = 1.5707963267949;
10953 sj4array
10954 [1]
10955 = IKsin(
10956 j4array
10957 [1]);
10958 cj4array
10959 [1]
10960 = IKcos(
10961 j4array
10962 [1]);
10963 j4array
10964 [2]
10965 = 3.14159265358979;
10966 sj4array
10967 [2]
10968 = IKsin(
10969 j4array
10970 [2]);
10971 cj4array
10972 [2]
10973 = IKcos(
10974 j4array
10975 [2]);
10976 j4array
10977 [3]
10978 = -1.5707963267949;
10979 sj4array
10980 [3]
10981 = IKsin(
10982 j4array
10983 [3]);
10984 cj4array
10985 [3]
10986 = IKcos(
10987 j4array
10988 [3]);
10989 if (j4array
10990 [0]
10991 > IKPI)
10992 {
10993 j4array
10994 [0]
10995 -= IK2PI;
10996 }
10997 else if (
10998 j4array
10999 [0]
11000 < -IKPI)
11001 {
11002 j4array
11003 [0]
11004 += IK2PI;
11005 }
11006 j4valid
11007 [0]
11008 = true;
11009 if (j4array
11010 [1]
11011 > IKPI)
11012 {
11013 j4array
11014 [1]
11015 -= IK2PI;
11016 }
11017 else if (
11018 j4array
11019 [1]
11020 < -IKPI)
11021 {
11022 j4array
11023 [1]
11024 += IK2PI;
11025 }
11026 j4valid
11027 [1]
11028 = true;
11029 if (j4array
11030 [2]
11031 > IKPI)
11032 {
11033 j4array
11034 [2]
11035 -= IK2PI;
11036 }
11037 else if (
11038 j4array
11039 [2]
11040 < -IKPI)
11041 {
11042 j4array
11043 [2]
11044 += IK2PI;
11045 }
11046 j4valid
11047 [2]
11048 = true;
11049 if (j4array
11050 [3]
11051 > IKPI)
11052 {
11053 j4array
11054 [3]
11055 -= IK2PI;
11056 }
11057 else if (
11058 j4array
11059 [3]
11060 < -IKPI)
11061 {
11062 j4array
11063 [3]
11064 += IK2PI;
11065 }
11066 j4valid
11067 [3]
11068 = true;
11069 for (
11070 int ij4
11071 = 0;
11072 ij4
11073 < 4;
11074 ++ij4)
11075 {
11076 if (!j4valid
11077 [ij4])
11078 {
11079 continue;
11080 }
11081 _ij4[0]
11082 = ij4;
11083 _ij4[1]
11084 = -1;
11085 for (
11086 int iij4
11087 = ij4
11088 + 1;
11089 iij4
11090 < 4;
11091 ++iij4)
11092 {
11093 if (j4valid
11094 [iij4]
11095 && IKabs(
11096 cj4array
11097 [ij4]
11098 - cj4array
11099 [iij4])
11100 < IKFAST_SOLUTION_THRESH
11101 && IKabs(
11102 sj4array
11103 [ij4]
11104 - sj4array
11105 [iij4])
11106 < IKFAST_SOLUTION_THRESH)
11107 {
11108 j4valid
11109 [iij4]
11110 = false;
11111 _ij4[1]
11112 = iij4;
11113 break;
11114 }
11115 }
11116 j4 = j4array
11117 [ij4];
11118 cj4 = cj4array
11119 [ij4];
11120 sj4 = sj4array
11121 [ij4];
11122
11123 rotationfunction0(
11124 solutions);
11125 }
11126 }
11127 }
11128 } while (
11129 0);
11130 if (bgotonextstatement)
11131 {
11132 bool
11133 bgotonextstatement
11134 = true;
11135 do
11136 {
11137 if (1)
11138 {
11139 bgotonextstatement
11140 = false;
11141 continue; // branch miss [j4]
11142 }
11143 } while (
11144 0);
11145 if (bgotonextstatement)
11146 {
11147 }
11148 }
11149 }
11150 }
11151 else
11152 {
11153 {
11154 IkReal j4array
11155 [1],
11156 cj4array
11157 [1],
11158 sj4array
11159 [1];
11160 bool j4valid
11161 [1]
11162 = {false};
11163 _nj4
11164 = 1;
11165 IkReal
11166 x1133
11167 = (cj9
11168 * px);
11169 IkReal
11170 x1134
11171 = (cj9
11172 * py);
11173 IkReal
11174 x1135
11175 = (pp
11176 * px);
11177 IkReal
11178 x1136
11179 = (pp
11180 * py);
11181 CheckValue<IkReal> x1137 = IKatan2WithCheck(
11182 IkReal((
11183 (((4.31372549019608)
11184 * x1135))
11185 + (((-1.45555882352941)
11186 * x1133))
11187 + (((0.348408823529412)
11188 * py))
11189 + (((0.119091176470588)
11190 * x1134))
11191 + (((-1.0)
11192 * (1.66110784313725)
11193 * px))
11194 + (((0.647058823529412)
11195 * x1136)))),
11196 ((((0.348408823529412)
11197 * px))
11198 + (((0.119091176470588)
11199 * x1133))
11200 + (((1.66110784313725)
11201 * py))
11202 + (((1.45555882352941)
11203 * x1134))
11204 + (((-4.31372549019608)
11205 * x1136))
11206 + (((0.647058823529412)
11207 * x1135))),
11208 IKFAST_ATAN2_MAGTHRESH);
11209 if (!x1137
11210 .valid)
11211 {
11212 continue;
11213 }
11214 CheckValue<IkReal> x1138 = IKPowWithIntegerCheck(
11215 IKsign((
11216 (((1.1)
11217 * (pz
11218 * pz)))
11219 + (((-1.0)
11220 * (1.1)
11221 * pp)))),
11222 -1);
11223 if (!x1138
11224 .valid)
11225 {
11226 continue;
11227 }
11228 j4array
11229 [0]
11230 = ((-1.5707963267949)
11231 + (x1137
11232 .value)
11233 + (((1.5707963267949)
11234 * (x1138
11235 .value))));
11236 sj4array
11237 [0]
11238 = IKsin(
11239 j4array
11240 [0]);
11241 cj4array
11242 [0]
11243 = IKcos(
11244 j4array
11245 [0]);
11246 if (j4array
11247 [0]
11248 > IKPI)
11249 {
11250 j4array
11251 [0]
11252 -= IK2PI;
11253 }
11254 else if (
11255 j4array
11256 [0]
11257 < -IKPI)
11258 {
11259 j4array
11260 [0]
11261 += IK2PI;
11262 }
11263 j4valid
11264 [0]
11265 = true;
11266 for (
11267 int ij4
11268 = 0;
11269 ij4
11270 < 1;
11271 ++ij4)
11272 {
11273 if (!j4valid
11274 [ij4])
11275 {
11276 continue;
11277 }
11278 _ij4[0]
11279 = ij4;
11280 _ij4[1]
11281 = -1;
11282 for (
11283 int iij4
11284 = ij4
11285 + 1;
11286 iij4
11287 < 1;
11288 ++iij4)
11289 {
11290 if (j4valid
11291 [iij4]
11292 && IKabs(
11293 cj4array
11294 [ij4]
11295 - cj4array
11296 [iij4])
11297 < IKFAST_SOLUTION_THRESH
11298 && IKabs(
11299 sj4array
11300 [ij4]
11301 - sj4array
11302 [iij4])
11303 < IKFAST_SOLUTION_THRESH)
11304 {
11305 j4valid
11306 [iij4]
11307 = false;
11308 _ij4[1]
11309 = iij4;
11310 break;
11311 }
11312 }
11313 j4 = j4array
11314 [ij4];
11315 cj4 = cj4array
11316 [ij4];
11317 sj4 = sj4array
11318 [ij4];
11319 {
11320 IkReal evalcond
11321 [4];
11322 IkReal
11323 x1139
11324 = IKsin(
11325 j4);
11326 IkReal
11327 x1140
11328 = (px
11329 * x1139);
11330 IkReal
11331 x1141
11332 = IKcos(
11333 j4);
11334 IkReal
11335 x1142
11336 = (py
11337 * x1141);
11338 IkReal
11339 x1143
11340 = (px
11341 * x1141);
11342 IkReal
11343 x1144
11344 = (py
11345 * x1139);
11346 IkReal
11347 x1145
11348 = ((((-1.0)
11349 * x1144))
11350 + (((-1.0)
11351 * x1143)));
11352 evalcond
11353 [0]
11354 = ((-1.51009803921569)
11355 + (((-1.0)
11356 * (1.32323529411765)
11357 * cj9))
11358 + x1140
11359 + (((3.92156862745098)
11360 * pp))
11361 + (((-1.0)
11362 * x1142)));
11363 evalcond
11364 [1]
11365 = ((-0.55)
11366 + x1145
11367 + (((-1.0)
11368 * (0.3)
11369 * cj9))
11370 + (((-1.0)
11371 * (0.045)
11372 * sj9)));
11373 evalcond
11374 [2]
11375 = ((-0.316735294117647)
11376 + x1145
11377 + (((-1.0)
11378 * (0.588235294117647)
11379 * pp))
11380 + (((-1.0)
11381 * (0.108264705882353)
11382 * cj9)));
11383 evalcond
11384 [3]
11385 = ((-0.2125)
11386 + (((-0.09)
11387 * x1140))
11388 + (((-1.1)
11389 * x1144))
11390 + (((-1.1)
11391 * x1143))
11392 + (((0.09)
11393 * x1142))
11394 + (((-1.0)
11395 * (1.0)
11396 * pp)));
11397 if (IKabs(
11398 evalcond
11399 [0])
11400 > IKFAST_EVALCOND_THRESH
11401 || IKabs(
11402 evalcond
11403 [1])
11404 > IKFAST_EVALCOND_THRESH
11405 || IKabs(
11406 evalcond
11407 [2])
11408 > IKFAST_EVALCOND_THRESH
11409 || IKabs(
11410 evalcond
11411 [3])
11412 > IKFAST_EVALCOND_THRESH)
11413 {
11414 continue;
11415 }
11416 }
11417
11418 rotationfunction0(
11419 solutions);
11420 }
11421 }
11422 }
11423 }
11424 }
11425 else
11426 {
11427 {
11428 IkReal j4array
11429 [1],
11430 cj4array
11431 [1],
11432 sj4array
11433 [1];
11434 bool j4valid
11435 [1]
11436 = {false};
11437 _nj4 = 1;
11438 IkReal
11439 x1146
11440 = (cj9
11441 * px);
11442 IkReal
11443 x1147
11444 = (cj9
11445 * py);
11446 IkReal
11447 x1148
11448 = (pp
11449 * px);
11450 IkReal
11451 x1149
11452 = (pp
11453 * py);
11454 CheckValue<IkReal> x1150 = IKatan2WithCheck(
11455 IkReal((
11456 (((1.51009803921569)
11457 * px))
11458 + (((-1.0)
11459 * (0.316735294117647)
11460 * py))
11461 + (((-0.108264705882353)
11462 * x1147))
11463 + (((-0.588235294117647)
11464 * x1149))
11465 + (((1.32323529411765)
11466 * x1146))
11467 + (((-3.92156862745098)
11468 * x1148)))),
11469 ((((-0.108264705882353)
11470 * x1146))
11471 + (((-1.32323529411765)
11472 * x1147))
11473 + (((3.92156862745098)
11474 * x1149))
11475 + (((-1.0)
11476 * (1.51009803921569)
11477 * py))
11478 + (((-1.0)
11479 * (0.316735294117647)
11480 * px))
11481 + (((-0.588235294117647)
11482 * x1148))),
11483 IKFAST_ATAN2_MAGTHRESH);
11484 if (!x1150
11485 .valid)
11486 {
11487 continue;
11488 }
11489 CheckValue<IkReal> x1151 = IKPowWithIntegerCheck(
11490 IKsign((
11491 pp
11492 + (((-1.0)
11493 * (1.0)
11494 * (pz
11495 * pz))))),
11496 -1);
11497 if (!x1151
11498 .valid)
11499 {
11500 continue;
11501 }
11502 j4array[0]
11503 = ((-1.5707963267949)
11504 + (x1150
11505 .value)
11506 + (((1.5707963267949)
11507 * (x1151
11508 .value))));
11509 sj4array[0] = IKsin(
11510 j4array
11511 [0]);
11512 cj4array[0] = IKcos(
11513 j4array
11514 [0]);
11515 if (j4array
11516 [0]
11517 > IKPI)
11518 {
11519 j4array
11520 [0]
11521 -= IK2PI;
11522 }
11523 else if (
11524 j4array
11525 [0]
11526 < -IKPI)
11527 {
11528 j4array
11529 [0]
11530 += IK2PI;
11531 }
11532 j4valid[0]
11533 = true;
11534 for (
11535 int ij4
11536 = 0;
11537 ij4
11538 < 1;
11539 ++ij4)
11540 {
11541 if (!j4valid
11542 [ij4])
11543 {
11544 continue;
11545 }
11546 _ij4[0]
11547 = ij4;
11548 _ij4[1]
11549 = -1;
11550 for (
11551 int iij4
11552 = ij4
11553 + 1;
11554 iij4
11555 < 1;
11556 ++iij4)
11557 {
11558 if (j4valid
11559 [iij4]
11560 && IKabs(
11561 cj4array
11562 [ij4]
11563 - cj4array
11564 [iij4])
11565 < IKFAST_SOLUTION_THRESH
11566 && IKabs(
11567 sj4array
11568 [ij4]
11569 - sj4array
11570 [iij4])
11571 < IKFAST_SOLUTION_THRESH)
11572 {
11573 j4valid
11574 [iij4]
11575 = false;
11576 _ij4[1]
11577 = iij4;
11578 break;
11579 }
11580 }
11581 j4 = j4array
11582 [ij4];
11583 cj4 = cj4array
11584 [ij4];
11585 sj4 = sj4array
11586 [ij4];
11587 {
11588 IkReal evalcond
11589 [4];
11590 IkReal
11591 x1152
11592 = IKsin(
11593 j4);
11594 IkReal
11595 x1153
11596 = (px
11597 * x1152);
11598 IkReal
11599 x1154
11600 = IKcos(
11601 j4);
11602 IkReal
11603 x1155
11604 = (py
11605 * x1154);
11606 IkReal
11607 x1156
11608 = (px
11609 * x1154);
11610 IkReal
11611 x1157
11612 = (py
11613 * x1152);
11614 IkReal
11615 x1158
11616 = ((((-1.0)
11617 * x1156))
11618 + (((-1.0)
11619 * x1157)));
11620 evalcond
11621 [0]
11622 = ((-1.51009803921569)
11623 + (((-1.0)
11624 * x1155))
11625 + x1153
11626 + (((-1.0)
11627 * (1.32323529411765)
11628 * cj9))
11629 + (((3.92156862745098)
11630 * pp)));
11631 evalcond
11632 [1]
11633 = ((-0.55)
11634 + x1158
11635 + (((-1.0)
11636 * (0.3)
11637 * cj9))
11638 + (((-1.0)
11639 * (0.045)
11640 * sj9)));
11641 evalcond
11642 [2]
11643 = ((-0.316735294117647)
11644 + x1158
11645 + (((-1.0)
11646 * (0.588235294117647)
11647 * pp))
11648 + (((-1.0)
11649 * (0.108264705882353)
11650 * cj9)));
11651 evalcond
11652 [3]
11653 = ((-0.2125)
11654 + (((-1.1)
11655 * x1156))
11656 + (((-1.1)
11657 * x1157))
11658 + (((-0.09)
11659 * x1153))
11660 + (((-1.0)
11661 * (1.0)
11662 * pp))
11663 + (((0.09)
11664 * x1155)));
11665 if (IKabs(
11666 evalcond
11667 [0])
11668 > IKFAST_EVALCOND_THRESH
11669 || IKabs(
11670 evalcond
11671 [1])
11672 > IKFAST_EVALCOND_THRESH
11673 || IKabs(
11674 evalcond
11675 [2])
11676 > IKFAST_EVALCOND_THRESH
11677 || IKabs(
11678 evalcond
11679 [3])
11680 > IKFAST_EVALCOND_THRESH)
11681 {
11682 continue;
11683 }
11684 }
11685
11686 rotationfunction0(
11687 solutions);
11688 }
11689 }
11690 }
11691 }
11692 }
11693 else
11694 {
11695 {
11696 IkReal j4array
11697 [1],
11698 cj4array
11699 [1],
11700 sj4array
11701 [1];
11702 bool
11703 j4valid[1]
11704 = {false};
11705 _nj4 = 1;
11706 IkReal x1159
11707 = (cj9
11708 * px);
11709 IkReal x1160
11710 = (cj9
11711 * py);
11712 IkReal x1161
11713 = ((3.92156862745098)
11714 * pp);
11715 IkReal x1162
11716 = ((0.045)
11717 * sj9);
11718 CheckValue<IkReal> x1163 = IKPowWithIntegerCheck(
11719 IKsign((
11720 pp
11721 + (((-1.0)
11722 * (1.0)
11723 * (pz
11724 * pz))))),
11725 -1);
11726 if (!x1163
11727 .valid)
11728 {
11729 continue;
11730 }
11731 CheckValue<IkReal> x1164 = IKatan2WithCheck(
11732 IkReal((
11733 (((-0.3)
11734 * x1160))
11735 + (((1.51009803921569)
11736 * px))
11737 + (((-1.0)
11738 * py
11739 * x1162))
11740 + (((-1.0)
11741 * px
11742 * x1161))
11743 + (((1.32323529411765)
11744 * x1159))
11745 + (((-1.0)
11746 * (0.55)
11747 * py)))),
11748 ((((-1.0)
11749 * (0.55)
11750 * px))
11751 + (((-1.32323529411765)
11752 * x1160))
11753 + (((-1.0)
11754 * (1.51009803921569)
11755 * py))
11756 + (((-0.3)
11757 * x1159))
11758 + ((py
11759 * x1161))
11760 + (((-1.0)
11761 * px
11762 * x1162))),
11763 IKFAST_ATAN2_MAGTHRESH);
11764 if (!x1164
11765 .valid)
11766 {
11767 continue;
11768 }
11769 j4array[0]
11770 = ((-1.5707963267949)
11771 + (((1.5707963267949)
11772 * (x1163
11773 .value)))
11774 + (x1164
11775 .value));
11776 sj4array[0]
11777 = IKsin(
11778 j4array
11779 [0]);
11780 cj4array[0]
11781 = IKcos(
11782 j4array
11783 [0]);
11784 if (j4array[0]
11785 > IKPI)
11786 {
11787 j4array[0]
11788 -= IK2PI;
11789 }
11790 else if (
11791 j4array[0]
11792 < -IKPI)
11793 {
11794 j4array[0]
11795 += IK2PI;
11796 }
11797 j4valid[0]
11798 = true;
11799 for (int ij4
11800 = 0;
11801 ij4 < 1;
11802 ++ij4)
11803 {
11804 if (!j4valid
11805 [ij4])
11806 {
11807 continue;
11808 }
11809 _ij4[0]
11810 = ij4;
11811 _ij4[1]
11812 = -1;
11813 for (
11814 int iij4
11815 = ij4
11816 + 1;
11817 iij4
11818 < 1;
11819 ++iij4)
11820 {
11821 if (j4valid
11822 [iij4]
11823 && IKabs(
11824 cj4array
11825 [ij4]
11826 - cj4array
11827 [iij4])
11828 < IKFAST_SOLUTION_THRESH
11829 && IKabs(
11830 sj4array
11831 [ij4]
11832 - sj4array
11833 [iij4])
11834 < IKFAST_SOLUTION_THRESH)
11835 {
11836 j4valid
11837 [iij4]
11838 = false;
11839 _ij4[1]
11840 = iij4;
11841 break;
11842 }
11843 }
11844 j4 = j4array
11845 [ij4];
11846 cj4 = cj4array
11847 [ij4];
11848 sj4 = sj4array
11849 [ij4];
11850 {
11851 IkReal evalcond
11852 [4];
11853 IkReal
11854 x1165
11855 = IKsin(
11856 j4);
11857 IkReal
11858 x1166
11859 = (px
11860 * x1165);
11861 IkReal
11862 x1167
11863 = IKcos(
11864 j4);
11865 IkReal
11866 x1168
11867 = (py
11868 * x1167);
11869 IkReal
11870 x1169
11871 = (px
11872 * x1167);
11873 IkReal
11874 x1170
11875 = (py
11876 * x1165);
11877 IkReal
11878 x1171
11879 = ((((-1.0)
11880 * x1169))
11881 + (((-1.0)
11882 * x1170)));
11883 evalcond
11884 [0]
11885 = ((-1.51009803921569)
11886 + (((-1.0)
11887 * (1.32323529411765)
11888 * cj9))
11889 + (((-1.0)
11890 * x1168))
11891 + (((3.92156862745098)
11892 * pp))
11893 + x1166);
11894 evalcond
11895 [1]
11896 = ((-0.55)
11897 + (((-1.0)
11898 * (0.3)
11899 * cj9))
11900 + x1171
11901 + (((-1.0)
11902 * (0.045)
11903 * sj9)));
11904 evalcond
11905 [2]
11906 = ((-0.316735294117647)
11907 + (((-1.0)
11908 * (0.588235294117647)
11909 * pp))
11910 + (((-1.0)
11911 * (0.108264705882353)
11912 * cj9))
11913 + x1171);
11914 evalcond
11915 [3]
11916 = ((-0.2125)
11917 + (((-1.1)
11918 * x1170))
11919 + (((-1.1)
11920 * x1169))
11921 + (((0.09)
11922 * x1168))
11923 + (((-0.09)
11924 * x1166))
11925 + (((-1.0)
11926 * (1.0)
11927 * pp)));
11928 if (IKabs(
11929 evalcond
11930 [0])
11931 > IKFAST_EVALCOND_THRESH
11932 || IKabs(
11933 evalcond
11934 [1])
11935 > IKFAST_EVALCOND_THRESH
11936 || IKabs(
11937 evalcond
11938 [2])
11939 > IKFAST_EVALCOND_THRESH
11940 || IKabs(
11941 evalcond
11942 [3])
11943 > IKFAST_EVALCOND_THRESH)
11944 {
11945 continue;
11946 }
11947 }
11948
11949 rotationfunction0(
11950 solutions);
11951 }
11952 }
11953 }
11954 }
11955 }
11956 } while (0);
11957 if (bgotonextstatement)
11958 {
11959 bool
11960 bgotonextstatement
11961 = true;
11962 do
11963 {
11964 IkReal x1172
11965 = (cj6 * pz);
11966 IkReal x1173
11967 = ((-1.51009803921569)
11968 + (((-1.0)
11969 * (1.32323529411765)
11970 * cj9))
11971 + (((3.92156862745098)
11972 * pp)));
11973 evalcond[0]
11974 = ((IKabs(py))
11975 + (IKabs(
11976 px)));
11977 evalcond[1]
11978 = ((-0.55)
11979 + (((-1.0)
11980 * (0.3)
11981 * cj9))
11982 + x1172
11983 + (((-1.0)
11984 * (0.045)
11985 * sj9)));
11986 evalcond[2] = x1173;
11987 evalcond[3]
11988 = ((-1.0)
11989 * (((1.0)
11990 * pz
11991 * sj6)));
11992 evalcond[4] = x1173;
11993 evalcond[5]
11994 = ((((0.108264705882353)
11995 * cj9
11996 * sj6))
11997 + (((0.588235294117647)
11998 * pp
11999 * sj6))
12000 + (((0.316735294117647)
12001 * sj6)));
12002 evalcond[6]
12003 = ((-0.2125)
12004 + (((1.1)
12005 * x1172))
12006 + (((-1.0)
12007 * (1.0)
12008 * pp)));
12009 if (IKabs(
12010 evalcond[0])
12011 < 0.0000010000000000
12012 && IKabs(
12013 evalcond
12014 [1])
12015 < 0.0000010000000000
12016 && IKabs(
12017 evalcond
12018 [2])
12019 < 0.0000010000000000
12020 && IKabs(
12021 evalcond
12022 [3])
12023 < 0.0000010000000000
12024 && IKabs(
12025 evalcond
12026 [4])
12027 < 0.0000010000000000
12028 && IKabs(
12029 evalcond
12030 [5])
12031 < 0.0000010000000000
12032 && IKabs(
12033 evalcond
12034 [6])
12035 < 0.0000010000000000)
12036 {
12037 bgotonextstatement
12038 = false;
12039 {
12040 IkReal
12041 j4array[4],
12042 cj4array[4],
12043 sj4array[4];
12044 bool j4valid[4]
12045 = {false};
12046 _nj4 = 4;
12047 j4array[0] = 0;
12048 sj4array[0]
12049 = IKsin(
12050 j4array
12051 [0]);
12052 cj4array[0]
12053 = IKcos(
12054 j4array
12055 [0]);
12056 j4array[1]
12057 = 1.5707963267949;
12058 sj4array[1]
12059 = IKsin(
12060 j4array
12061 [1]);
12062 cj4array[1]
12063 = IKcos(
12064 j4array
12065 [1]);
12066 j4array[2]
12067 = 3.14159265358979;
12068 sj4array[2]
12069 = IKsin(
12070 j4array
12071 [2]);
12072 cj4array[2]
12073 = IKcos(
12074 j4array
12075 [2]);
12076 j4array[3]
12077 = -1.5707963267949;
12078 sj4array[3]
12079 = IKsin(
12080 j4array
12081 [3]);
12082 cj4array[3]
12083 = IKcos(
12084 j4array
12085 [3]);
12086 if (j4array[0]
12087 > IKPI)
12088 {
12089 j4array[0]
12090 -= IK2PI;
12091 }
12092 else if (
12093 j4array[0]
12094 < -IKPI)
12095 {
12096 j4array[0]
12097 += IK2PI;
12098 }
12099 j4valid[0]
12100 = true;
12101 if (j4array[1]
12102 > IKPI)
12103 {
12104 j4array[1]
12105 -= IK2PI;
12106 }
12107 else if (
12108 j4array[1]
12109 < -IKPI)
12110 {
12111 j4array[1]
12112 += IK2PI;
12113 }
12114 j4valid[1]
12115 = true;
12116 if (j4array[2]
12117 > IKPI)
12118 {
12119 j4array[2]
12120 -= IK2PI;
12121 }
12122 else if (
12123 j4array[2]
12124 < -IKPI)
12125 {
12126 j4array[2]
12127 += IK2PI;
12128 }
12129 j4valid[2]
12130 = true;
12131 if (j4array[3]
12132 > IKPI)
12133 {
12134 j4array[3]
12135 -= IK2PI;
12136 }
12137 else if (
12138 j4array[3]
12139 < -IKPI)
12140 {
12141 j4array[3]
12142 += IK2PI;
12143 }
12144 j4valid[3]
12145 = true;
12146 for (int ij4
12147 = 0;
12148 ij4 < 4;
12149 ++ij4)
12150 {
12151 if (!j4valid
12152 [ij4])
12153 {
12154 continue;
12155 }
12156 _ij4[0] = ij4;
12157 _ij4[1] = -1;
12158 for (int iij4
12159 = ij4
12160 + 1;
12161 iij4 < 4;
12162 ++iij4)
12163 {
12164 if (j4valid
12165 [iij4]
12166 && IKabs(
12167 cj4array
12168 [ij4]
12169 - cj4array
12170 [iij4])
12171 < IKFAST_SOLUTION_THRESH
12172 && IKabs(
12173 sj4array
12174 [ij4]
12175 - sj4array
12176 [iij4])
12177 < IKFAST_SOLUTION_THRESH)
12178 {
12179 j4valid
12180 [iij4]
12181 = false;
12182 _ij4[1]
12183 = iij4;
12184 break;
12185 }
12186 }
12187 j4 = j4array
12188 [ij4];
12189 cj4 = cj4array
12190 [ij4];
12191 sj4 = sj4array
12192 [ij4];
12193
12194 rotationfunction0(
12195 solutions);
12196 }
12197 }
12198 }
12199 } while (0);
12200 if (bgotonextstatement)
12201 {
12202 bool
12203 bgotonextstatement
12204 = true;
12205 do
12206 {
12207 if (1)
12208 {
12209 bgotonextstatement
12210 = false;
12211 continue; // branch
12212 // miss
12213 // [j4]
12214 }
12215 } while (0);
12216 if (bgotonextstatement)
12217 {
12218 }
12219 }
12220 }
12221 }
12222 }
12223 }
12224 else
12225 {
12226 {
12227 IkReal j4array[1],
12228 cj4array[1],
12229 sj4array[1];
12230 bool j4valid[1] = {false};
12231 _nj4 = 1;
12232 IkReal x1174
12233 = ((1.32323529411765)
12234 * cj9);
12235 IkReal x1175
12236 = ((3.92156862745098)
12237 * pp);
12238 IkReal x1176
12239 = ((0.316735294117647)
12240 * sj6);
12241 IkReal x1177
12242 = ((0.108264705882353)
12243 * cj9 * sj6);
12244 IkReal x1178
12245 = ((0.588235294117647)
12246 * pp * sj6);
12247 CheckValue<IkReal> x1179
12248 = IKatan2WithCheck(
12249 IkReal((
12250 (((-1.0) * px
12251 * x1175))
12252 + ((px
12253 * x1174))
12254 + (((1.51009803921569)
12255 * px))
12256 + ((py
12257 * x1178))
12258 + ((py
12259 * x1177))
12260 + ((py
12261 * x1176)))),
12262 (((px * x1176))
12263 + (((-1.0)
12264 * (1.51009803921569)
12265 * py))
12266 + ((px * x1178))
12267 + ((py * x1175))
12268 + (((-1.0) * py
12269 * x1174))
12270 + ((px
12271 * x1177))),
12272 IKFAST_ATAN2_MAGTHRESH);
12273 if (!x1179.valid)
12274 {
12275 continue;
12276 }
12277 CheckValue<IkReal> x1180
12278 = IKPowWithIntegerCheck(
12279 IKsign((
12280 pp
12281 + (((-1.0)
12282 * (1.0)
12283 * (pz
12284 * pz))))),
12285 -1);
12286 if (!x1180.valid)
12287 {
12288 continue;
12289 }
12290 j4array[0]
12291 = ((-1.5707963267949)
12292 + (x1179.value)
12293 + (((1.5707963267949)
12294 * (x1180
12295 .value))));
12296 sj4array[0]
12297 = IKsin(j4array[0]);
12298 cj4array[0]
12299 = IKcos(j4array[0]);
12300 if (j4array[0] > IKPI)
12301 {
12302 j4array[0] -= IK2PI;
12303 }
12304 else if (
12305 j4array[0] < -IKPI)
12306 {
12307 j4array[0] += IK2PI;
12308 }
12309 j4valid[0] = true;
12310 for (int ij4 = 0; ij4 < 1;
12311 ++ij4)
12312 {
12313 if (!j4valid[ij4])
12314 {
12315 continue;
12316 }
12317 _ij4[0] = ij4;
12318 _ij4[1] = -1;
12319 for (int iij4 = ij4 + 1;
12320 iij4 < 1;
12321 ++iij4)
12322 {
12323 if (j4valid[iij4]
12324 && IKabs(
12325 cj4array
12326 [ij4]
12327 - cj4array
12328 [iij4])
12329 < IKFAST_SOLUTION_THRESH
12330 && IKabs(
12331 sj4array
12332 [ij4]
12333 - sj4array
12334 [iij4])
12335 < IKFAST_SOLUTION_THRESH)
12336 {
12337 j4valid[iij4]
12338 = false;
12339 _ij4[1] = iij4;
12340 break;
12341 }
12342 }
12343 j4 = j4array[ij4];
12344 cj4 = cj4array[ij4];
12345 sj4 = sj4array[ij4];
12346 {
12347 IkReal evalcond[5];
12348 IkReal x1181
12349 = IKcos(j4);
12350 IkReal x1182
12351 = (px * x1181);
12352 IkReal x1183
12353 = IKsin(j4);
12354 IkReal x1184
12355 = (py * x1183);
12356 IkReal x1185
12357 = (px * x1183);
12358 IkReal x1186
12359 = ((1.0) * x1181);
12360 IkReal x1187
12361 = (cj6 * pz);
12362 IkReal x1188
12363 = (sj6 * x1182);
12364 IkReal x1189
12365 = (sj6 * x1184);
12366 evalcond[0]
12367 = (((cj6 * x1184))
12368 + ((cj6
12369 * x1182))
12370 + (((-1.0)
12371 * (1.0)
12372 * pz
12373 * sj6)));
12374 evalcond[1]
12375 = ((-1.51009803921569)
12376 + (((-1.0) * py
12377 * x1186))
12378 + (((-1.0)
12379 * (1.32323529411765)
12380 * cj9))
12381 + (((3.92156862745098)
12382 * pp))
12383 + x1185);
12384 evalcond[2]
12385 = ((-0.55)
12386 + (((-1.0)
12387 * (0.3)
12388 * cj9))
12389 + x1187 + x1188
12390 + x1189
12391 + (((-1.0)
12392 * (0.045)
12393 * sj9)));
12394 evalcond[3]
12395 = ((((0.108264705882353)
12396 * cj9 * sj6))
12397 + (((-1.0)
12398 * x1184))
12399 + (((-1.0) * px
12400 * x1186))
12401 + (((0.588235294117647)
12402 * pp
12403 * sj6))
12404 + (((0.316735294117647)
12405 * sj6)));
12406 evalcond[4]
12407 = ((-0.2125)
12408 + (((1.1)
12409 * x1188))
12410 + (((-0.09)
12411 * x1185))
12412 + (((0.09) * py
12413 * x1181))
12414 + (((-1.0)
12415 * (1.0)
12416 * pp))
12417 + (((1.1)
12418 * x1187))
12419 + (((1.1)
12420 * x1189)));
12421 if (IKabs(evalcond[0])
12422 > IKFAST_EVALCOND_THRESH
12423 || IKabs(evalcond
12424 [1])
12425 > IKFAST_EVALCOND_THRESH
12426 || IKabs(evalcond
12427 [2])
12428 > IKFAST_EVALCOND_THRESH
12429 || IKabs(evalcond
12430 [3])
12431 > IKFAST_EVALCOND_THRESH
12432 || IKabs(evalcond
12433 [4])
12434 > IKFAST_EVALCOND_THRESH)
12435 {
12436 continue;
12437 }
12438 }
12439
12440 rotationfunction0(
12441 solutions);
12442 }
12443 }
12444 }
12445 }
12446 }
12447 else
12448 {
12449 {
12450 IkReal j4array[1],
12451 cj4array[1], sj4array[1];
12452 bool j4valid[1] = {false};
12453 _nj4 = 1;
12454 IkReal x1190 = (cj6 * pp);
12455 IkReal x1191
12456 = ((0.2125) * cj6);
12457 IkReal x1192 = ((1.1) * pz);
12458 IkReal x1193
12459 = ((0.09) * pz * sj6);
12460 CheckValue<IkReal> x1194
12461 = IKatan2WithCheck(
12462 IkReal(
12463 (((px * x1192))
12464 + (((-1.0) * px
12465 * x1190))
12466 + ((py * x1193))
12467 + (((-1.0) * px
12468 * x1191)))),
12469 (((py * x1190))
12470 + (((-1.0) * py
12471 * x1192))
12472 + ((py * x1191))
12473 + ((px * x1193))),
12474 IKFAST_ATAN2_MAGTHRESH);
12475 if (!x1194.valid)
12476 {
12477 continue;
12478 }
12479 CheckValue<IkReal> x1195
12480 = IKPowWithIntegerCheck(
12481 IKsign((
12482 (((0.09) * x1190))
12483 + (((-1.0)
12484 * (0.09) * cj6
12485 * (pz
12486 * pz))))),
12487 -1);
12488 if (!x1195.valid)
12489 {
12490 continue;
12491 }
12492 j4array[0]
12493 = ((-1.5707963267949)
12494 + (x1194.value)
12495 + (((1.5707963267949)
12496 * (x1195.value))));
12497 sj4array[0]
12498 = IKsin(j4array[0]);
12499 cj4array[0]
12500 = IKcos(j4array[0]);
12501 if (j4array[0] > IKPI)
12502 {
12503 j4array[0] -= IK2PI;
12504 }
12505 else if (j4array[0] < -IKPI)
12506 {
12507 j4array[0] += IK2PI;
12508 }
12509 j4valid[0] = true;
12510 for (int ij4 = 0; ij4 < 1;
12511 ++ij4)
12512 {
12513 if (!j4valid[ij4])
12514 {
12515 continue;
12516 }
12517 _ij4[0] = ij4;
12518 _ij4[1] = -1;
12519 for (int iij4 = ij4 + 1;
12520 iij4 < 1;
12521 ++iij4)
12522 {
12523 if (j4valid[iij4]
12524 && IKabs(
12525 cj4array[ij4]
12526 - cj4array
12527 [iij4])
12528 < IKFAST_SOLUTION_THRESH
12529 && IKabs(
12530 sj4array[ij4]
12531 - sj4array
12532 [iij4])
12533 < IKFAST_SOLUTION_THRESH)
12534 {
12535 j4valid[iij4] = false;
12536 _ij4[1] = iij4;
12537 break;
12538 }
12539 }
12540 j4 = j4array[ij4];
12541 cj4 = cj4array[ij4];
12542 sj4 = sj4array[ij4];
12543 {
12544 IkReal evalcond[5];
12545 IkReal x1196 = IKcos(j4);
12546 IkReal x1197
12547 = (px * x1196);
12548 IkReal x1198 = IKsin(j4);
12549 IkReal x1199
12550 = (py * x1198);
12551 IkReal x1200
12552 = (px * x1198);
12553 IkReal x1201
12554 = ((1.0) * x1196);
12555 IkReal x1202 = (cj6 * pz);
12556 IkReal x1203
12557 = (sj6 * x1197);
12558 IkReal x1204
12559 = (sj6 * x1199);
12560 evalcond[0]
12561 = (((cj6 * x1199))
12562 + ((cj6 * x1197))
12563 + (((-1.0) * (1.0)
12564 * pz * sj6)));
12565 evalcond[1]
12566 = ((-1.51009803921569)
12567 + x1200
12568 + (((-1.0)
12569 * (1.32323529411765)
12570 * cj9))
12571 + (((-1.0) * py
12572 * x1201))
12573 + (((3.92156862745098)
12574 * pp)));
12575 evalcond[2]
12576 = ((-0.55) + x1202
12577 + x1203 + x1204
12578 + (((-1.0) * (0.3)
12579 * cj9))
12580 + (((-1.0)
12581 * (0.045)
12582 * sj9)));
12583 evalcond[3]
12584 = ((((0.108264705882353)
12585 * cj9 * sj6))
12586 + (((0.588235294117647)
12587 * pp * sj6))
12588 + (((-1.0) * px
12589 * x1201))
12590 + (((0.316735294117647)
12591 * sj6))
12592 + (((-1.0)
12593 * x1199)));
12594 evalcond[4]
12595 = ((-0.2125)
12596 + (((1.1) * x1204))
12597 + (((1.1) * x1203))
12598 + (((-0.09)
12599 * x1200))
12600 + (((0.09) * py
12601 * x1196))
12602 + (((1.1) * x1202))
12603 + (((-1.0) * (1.0)
12604 * pp)));
12605 if (IKabs(evalcond[0])
12606 > IKFAST_EVALCOND_THRESH
12607 || IKabs(evalcond[1])
12608 > IKFAST_EVALCOND_THRESH
12609 || IKabs(evalcond[2])
12610 > IKFAST_EVALCOND_THRESH
12611 || IKabs(evalcond[3])
12612 > IKFAST_EVALCOND_THRESH
12613 || IKabs(evalcond[4])
12614 > IKFAST_EVALCOND_THRESH)
12615 {
12616 continue;
12617 }
12618 }
12619
12620 rotationfunction0(
12621 solutions);
12622 }
12623 }
12624 }
12625 }
12626 }
12627 else
12628 {
12629 {
12630 IkReal j4array[1], cj4array[1],
12631 sj4array[1];
12632 bool j4valid[1] = {false};
12633 _nj4 = 1;
12634 IkReal x1205
12635 = ((1.51009803921569) * cj6);
12636 IkReal x1206 = (pz * sj6);
12637 IkReal x1207
12638 = ((1.32323529411765) * cj6
12639 * cj9);
12640 IkReal x1208
12641 = ((3.92156862745098) * cj6
12642 * pp);
12643 CheckValue<IkReal> x1209
12644 = IKatan2WithCheck(
12645 IkReal(
12646 (((px * x1205))
12647 + ((px * x1207))
12648 + ((py * x1206))
12649 + (((-1.0) * px
12650 * x1208)))),
12651 ((((-1.0) * py * x1205))
12652 + ((py * x1208))
12653 + ((px * x1206))
12654 + (((-1.0) * py
12655 * x1207))),
12656 IKFAST_ATAN2_MAGTHRESH);
12657 if (!x1209.valid)
12658 {
12659 continue;
12660 }
12661 CheckValue<IkReal> x1210
12662 = IKPowWithIntegerCheck(
12663 IKsign(
12664 ((((-1.0) * (1.0)
12665 * cj6 * (pz * pz)))
12666 + ((cj6 * pp)))),
12667 -1);
12668 if (!x1210.valid)
12669 {
12670 continue;
12671 }
12672 j4array[0]
12673 = ((-1.5707963267949)
12674 + (x1209.value)
12675 + (((1.5707963267949)
12676 * (x1210.value))));
12677 sj4array[0] = IKsin(j4array[0]);
12678 cj4array[0] = IKcos(j4array[0]);
12679 if (j4array[0] > IKPI)
12680 {
12681 j4array[0] -= IK2PI;
12682 }
12683 else if (j4array[0] < -IKPI)
12684 {
12685 j4array[0] += IK2PI;
12686 }
12687 j4valid[0] = true;
12688 for (int ij4 = 0; ij4 < 1; ++ij4)
12689 {
12690 if (!j4valid[ij4])
12691 {
12692 continue;
12693 }
12694 _ij4[0] = ij4;
12695 _ij4[1] = -1;
12696 for (int iij4 = ij4 + 1;
12697 iij4 < 1;
12698 ++iij4)
12699 {
12700 if (j4valid[iij4]
12701 && IKabs(
12702 cj4array[ij4]
12703 - cj4array[iij4])
12704 < IKFAST_SOLUTION_THRESH
12705 && IKabs(
12706 sj4array[ij4]
12707 - sj4array[iij4])
12708 < IKFAST_SOLUTION_THRESH)
12709 {
12710 j4valid[iij4] = false;
12711 _ij4[1] = iij4;
12712 break;
12713 }
12714 }
12715 j4 = j4array[ij4];
12716 cj4 = cj4array[ij4];
12717 sj4 = sj4array[ij4];
12718 {
12719 IkReal evalcond[5];
12720 IkReal x1211 = IKcos(j4);
12721 IkReal x1212 = (px * x1211);
12722 IkReal x1213 = IKsin(j4);
12723 IkReal x1214 = (py * x1213);
12724 IkReal x1215 = (px * x1213);
12725 IkReal x1216
12726 = ((1.0) * x1211);
12727 IkReal x1217 = (cj6 * pz);
12728 IkReal x1218 = (sj6 * x1212);
12729 IkReal x1219 = (sj6 * x1214);
12730 evalcond[0]
12731 = (((cj6 * x1212))
12732 + ((cj6 * x1214))
12733 + (((-1.0) * (1.0) * pz
12734 * sj6)));
12735 evalcond[1]
12736 = ((-1.51009803921569)
12737 + (((-1.0)
12738 * (1.32323529411765)
12739 * cj9))
12740 + (((3.92156862745098)
12741 * pp))
12742 + x1215
12743 + (((-1.0) * py
12744 * x1216)));
12745 evalcond[2]
12746 = ((-0.55)
12747 + (((-1.0) * (0.3)
12748 * cj9))
12749 + x1218 + x1219 + x1217
12750 + (((-1.0) * (0.045)
12751 * sj9)));
12752 evalcond[3]
12753 = ((((0.108264705882353)
12754 * cj9 * sj6))
12755 + (((-1.0) * x1214))
12756 + (((0.588235294117647)
12757 * pp * sj6))
12758 + (((-1.0) * px
12759 * x1216))
12760 + (((0.316735294117647)
12761 * sj6)));
12762 evalcond[4]
12763 = ((-0.2125)
12764 + (((1.1) * x1217))
12765 + (((1.1) * x1218))
12766 + (((-0.09) * x1215))
12767 + (((1.1) * x1219))
12768 + (((-1.0) * (1.0)
12769 * pp))
12770 + (((0.09) * py
12771 * x1211)));
12772 if (IKabs(evalcond[0])
12773 > IKFAST_EVALCOND_THRESH
12774 || IKabs(evalcond[1])
12775 > IKFAST_EVALCOND_THRESH
12776 || IKabs(evalcond[2])
12777 > IKFAST_EVALCOND_THRESH
12778 || IKabs(evalcond[3])
12779 > IKFAST_EVALCOND_THRESH
12780 || IKabs(evalcond[4])
12781 > IKFAST_EVALCOND_THRESH)
12782 {
12783 continue;
12784 }
12785 }
12786
12787 rotationfunction0(solutions);
12788 }
12789 }
12790 }
12791 }
12792 }
12793 } while (0);
12794 if (bgotonextstatement)
12795 {
12796 bool bgotonextstatement = true;
12797 do
12798 {
12799 evalcond[0]
12800 = ((-3.14159265358979)
12801 + (IKfmod(
12802 ((3.14159265358979)
12803 + (IKabs(
12804 ((1.5707963267949)
12805 + j8)))),
12806 6.28318530717959)));
12807 evalcond[1]
12808 = ((0.39655) + (((0.0765) * sj9))
12809 + (((0.32595) * cj9))
12810 + (((-1.0) * (1.0) * pp)));
12811 evalcond[2]
12812 = ((((-1.0) * (0.3) * cj6 * cj9))
12813 + (((-1.0) * (0.55) * cj6)) + pz
12814 + (((-1.0) * (0.045) * cj6
12815 * sj9)));
12816 if (IKabs(evalcond[0])
12817 < 0.0000010000000000
12818 && IKabs(evalcond[1])
12819 < 0.0000010000000000
12820 && IKabs(evalcond[2])
12821 < 0.0000010000000000)
12822 {
12823 bgotonextstatement = false;
12824 {
12825 IkReal j4eval[3];
12826 sj8 = -1.0;
12827 cj8 = 0;
12828 j8 = -1.5707963267949;
12829 IkReal x1220
12830 = ((((-1.0) * (1.0) * cj6
12831 * (pz * pz)))
12832 + ((cj6 * pp)));
12833 IkReal x1221
12834 = ((1.51009803921569) * cj6);
12835 IkReal x1222 = (pz * sj6);
12836 IkReal x1223
12837 = ((1.32323529411765) * cj6
12838 * cj9);
12839 IkReal x1224
12840 = ((3.92156862745098) * cj6
12841 * pp);
12842 j4eval[0] = x1220;
12843 j4eval[1]
12844 = ((IKabs(
12845 (((py * x1221))
12846 + ((py * x1223))
12847 + ((px * x1222))
12848 + (((-1.0) * py
12849 * x1224)))))
12850 + (IKabs((
12851 (((-1.0) * px * x1221))
12852 + ((py * x1222))
12853 + (((-1.0) * px * x1223))
12854 + ((px * x1224))))));
12855 j4eval[2] = IKsign(x1220);
12856 if (IKabs(j4eval[0])
12857 < 0.0000010000000000
12858 || IKabs(j4eval[1])
12859 < 0.0000010000000000
12860 || IKabs(j4eval[2])
12861 < 0.0000010000000000)
12862 {
12863 {
12864 IkReal j4eval[3];
12865 sj8 = -1.0;
12866 cj8 = 0;
12867 j8 = -1.5707963267949;
12868 IkReal x1225 = (cj6 * pp);
12869 IkReal x1226 = ((1.0) * x1225);
12870 IkReal x1227
12871 = (cj6 * (pz * pz));
12872 IkReal x1228 = ((0.2125) * cj6);
12873 IkReal x1229 = ((1.1) * pz);
12874 IkReal x1230
12875 = ((0.09) * pz * sj6);
12876 j4eval[0]
12877 = ((((-1.0) * x1226))
12878 + x1227);
12879 j4eval[1] = IKsign(
12880 ((((-0.09) * x1225))
12881 + (((0.09) * x1227))));
12882 j4eval[2]
12883 = ((IKabs(
12884 (((py * x1228))
12885 + ((py * x1225))
12886 + (((-1.0) * py
12887 * x1229))
12888 + (((-1.0) * px
12889 * x1230)))))
12890 + (IKabs(
12891 ((((-1.0) * px
12892 * x1228))
12893 + (((-1.0) * px
12894 * x1226))
12895 + ((px * x1229))
12896 + (((-1.0) * py
12897 * x1230))))));
12898 if (IKabs(j4eval[0])
12899 < 0.0000010000000000
12900 || IKabs(j4eval[1])
12901 < 0.0000010000000000
12902 || IKabs(j4eval[2])
12903 < 0.0000010000000000)
12904 {
12905 {
12906 IkReal j4eval[3];
12907 sj8 = -1.0;
12908 cj8 = 0;
12909 j8 = -1.5707963267949;
12910 IkReal x1231
12911 = (pp
12912 + (((-1.0) * (1.0)
12913 * (pz * pz))));
12914 IkReal x1232
12915 = ((1.32323529411765)
12916 * cj9);
12917 IkReal x1233
12918 = ((3.92156862745098)
12919 * pp);
12920 IkReal x1234
12921 = ((0.316735294117647)
12922 * sj6);
12923 IkReal x1235
12924 = ((0.108264705882353)
12925 * cj9 * sj6);
12926 IkReal x1236
12927 = ((0.588235294117647)
12928 * pp * sj6);
12929 j4eval[0] = x1231;
12930 j4eval[1]
12931 = ((IKabs((
12932 ((px * x1233))
12933 + (((-1.0)
12934 * (1.51009803921569)
12935 * px))
12936 + (((-1.0) * px
12937 * x1232))
12938 + ((py * x1234))
12939 + ((py * x1236))
12940 + ((py
12941 * x1235)))))
12942 + (IKabs((
12943 (((-1.0) * py
12944 * x1233))
12945 + ((py * x1232))
12946 + ((px * x1236))
12947 + ((px * x1235))
12948 + (((1.51009803921569)
12949 * py))
12950 + ((px
12951 * x1234))))));
12952 j4eval[2] = IKsign(x1231);
12953 if (IKabs(j4eval[0])
12954 < 0.0000010000000000
12955 || IKabs(j4eval[1])
12956 < 0.0000010000000000
12957 || IKabs(j4eval[2])
12958 < 0.0000010000000000)
12959 {
12960 {
12961 IkReal evalcond[7];
12962 bool bgotonextstatement
12963 = true;
12964 do
12965 {
12966 evalcond[0]
12967 = ((-3.14159265358979)
12968 + (IKfmod(
12969 ((3.14159265358979)
12970 + (IKabs((
12971 (-1.5707963267949)
12972 + j6)))),
12973 6.28318530717959)));
12974 evalcond[1] = pz;
12975 if (IKabs(evalcond[0])
12976 < 0.0000010000000000
12977 && IKabs(evalcond
12978 [1])
12979 < 0.0000010000000000)
12980 {
12981 bgotonextstatement
12982 = false;
12983 {
12984 IkReal j4eval[3];
12985 sj8 = -1.0;
12986 cj8 = 0;
12987 j8 = -1.5707963267949;
12988 sj6 = 1.0;
12989 cj6 = 0;
12990 j6 = 1.5707963267949;
12991 IkReal x1237
12992 = (pp
12993 + (((-1.0)
12994 * (1.0)
12995 * (pz
12996 * pz))));
12997 IkReal x1238
12998 = (cj9 * px);
12999 IkReal x1239
13000 = (cj9 * py);
13001 IkReal x1240
13002 = ((3.92156862745098)
13003 * pp);
13004 IkReal x1241
13005 = ((0.045)
13006 * sj9);
13007 j4eval[0] = x1237;
13008 j4eval[1]
13009 = ((IKabs((
13010 ((px
13011 * x1240))
13012 + (((-1.0)
13013 * (1.51009803921569)
13014 * px))
13015 + (((0.55)
13016 * py))
13017 + (((0.3)
13018 * x1239))
13019 + ((py
13020 * x1241))
13021 + (((-1.32323529411765)
13022 * x1238)))))
13023 + (IKabs((
13024 ((px
13025 * x1241))
13026 + (((1.51009803921569)
13027 * py))
13028 + (((1.32323529411765)
13029 * x1239))
13030 + (((-1.0)
13031 * py
13032 * x1240))
13033 + (((0.55)
13034 * px))
13035 + (((0.3)
13036 * x1238))))));
13037 j4eval[2]
13038 = IKsign(
13039 x1237);
13040 if (IKabs(
13041 j4eval[0])
13042 < 0.0000010000000000
13043 || IKabs(
13044 j4eval
13045 [1])
13046 < 0.0000010000000000
13047 || IKabs(
13048 j4eval
13049 [2])
13050 < 0.0000010000000000)
13051 {
13052 {
13053 IkReal
13054 j4eval[3];
13055 sj8 = -1.0;
13056 cj8 = 0;
13057 j8 = -1.5707963267949;
13058 sj6 = 1.0;
13059 cj6 = 0;
13060 j6 = 1.5707963267949;
13061 IkReal x1242
13062 = (pp
13063 + (((-1.0)
13064 * (1.0)
13065 * (pz
13066 * pz))));
13067 IkReal x1243
13068 = (cj9
13069 * px);
13070 IkReal x1244
13071 = (cj9
13072 * py);
13073 IkReal x1245
13074 = (pp
13075 * px);
13076 IkReal x1246
13077 = (pp
13078 * py);
13079 j4eval[0]
13080 = x1242;
13081 j4eval[1]
13082 = ((IKabs((
13083 (((-1.32323529411765)
13084 * x1243))
13085 + (((0.316735294117647)
13086 * py))
13087 + (((-1.0)
13088 * (1.51009803921569)
13089 * px))
13090 + (((0.108264705882353)
13091 * x1244))
13092 + (((0.588235294117647)
13093 * x1246))
13094 + (((3.92156862745098)
13095 * x1245)))))
13096 + (IKabs((
13097 (((0.588235294117647)
13098 * x1245))
13099 + (((1.32323529411765)
13100 * x1244))
13101 + (((-3.92156862745098)
13102 * x1246))
13103 + (((0.316735294117647)
13104 * px))
13105 + (((1.51009803921569)
13106 * py))
13107 + (((0.108264705882353)
13108 * x1243))))));
13109 j4eval[2]
13110 = IKsign(
13111 x1242);
13112 if (IKabs(
13113 j4eval
13114 [0])
13115 < 0.0000010000000000
13116 || IKabs(
13117 j4eval
13118 [1])
13119 < 0.0000010000000000
13120 || IKabs(
13121 j4eval
13122 [2])
13123 < 0.0000010000000000)
13124 {
13125 {
13126 IkReal j4eval
13127 [3];
13128 sj8 = -1.0;
13129 cj8 = 0;
13130 j8 = -1.5707963267949;
13131 sj6 = 1.0;
13132 cj6 = 0;
13133 j6 = 1.5707963267949;
13134 IkReal
13135 x1247
13136 = pz
13137 * pz;
13138 IkReal
13139 x1248
13140 = (cj9
13141 * px);
13142 IkReal
13143 x1249
13144 = (cj9
13145 * py);
13146 IkReal
13147 x1250
13148 = (pp
13149 * px);
13150 IkReal
13151 x1251
13152 = (pp
13153 * py);
13154 j4eval[0]
13155 = (pp
13156 + (((-1.0)
13157 * x1247)));
13158 j4eval[1]
13159 = ((IKabs((
13160 (((0.348408823529412)
13161 * py))
13162 + (((0.119091176470588)
13163 * x1249))
13164 + (((0.647058823529412)
13165 * x1251))
13166 + (((-1.45555882352941)
13167 * x1248))
13168 + (((4.31372549019608)
13169 * x1250))
13170 + (((-1.0)
13171 * (1.66110784313725)
13172 * px)))))
13173 + (IKabs((
13174 (((1.45555882352941)
13175 * x1249))
13176 + (((0.348408823529412)
13177 * px))
13178 + (((1.66110784313725)
13179 * py))
13180 + (((0.647058823529412)
13181 * x1250))
13182 + (((0.119091176470588)
13183 * x1248))
13184 + (((-4.31372549019608)
13185 * x1251))))));
13186 j4eval[2] = IKsign((
13187 (((1.1)
13188 * pp))
13189 + (((-1.1)
13190 * x1247))));
13191 if (IKabs(
13192 j4eval
13193 [0])
13194 < 0.0000010000000000
13195 || IKabs(
13196 j4eval
13197 [1])
13198 < 0.0000010000000000
13199 || IKabs(
13200 j4eval
13201 [2])
13202 < 0.0000010000000000)
13203 {
13204 {
13205 IkReal evalcond
13206 [6];
13207 bool
13208 bgotonextstatement
13209 = true;
13210 do
13211 {
13212 IkReal
13213 x1252
13214 = ((1.32323529411765)
13215 * cj9);
13216 IkReal
13217 x1253
13218 = ((3.92156862745098)
13219 * pp);
13220 evalcond
13221 [0]
13222 = ((IKabs(
13223 py))
13224 + (IKabs(
13225 px)));
13226 evalcond
13227 [1]
13228 = ((-0.55)
13229 + (((-1.0)
13230 * (0.3)
13231 * cj9))
13232 + (((-1.0)
13233 * (0.045)
13234 * sj9)));
13235 evalcond
13236 [2]
13237 = ((1.51009803921569)
13238 + (((-1.0)
13239 * x1253))
13240 + x1252);
13241 evalcond
13242 [3]
13243 = ((-1.51009803921569)
13244 + (((-1.0)
13245 * x1252))
13246 + x1253);
13247 evalcond
13248 [4]
13249 = ((0.316735294117647)
13250 + (((0.108264705882353)
13251 * cj9))
13252 + (((0.588235294117647)
13253 * pp)));
13254 evalcond
13255 [5]
13256 = ((-0.2125)
13257 + (((-1.0)
13258 * (1.0)
13259 * pp)));
13260 if (IKabs(
13261 evalcond
13262 [0])
13263 < 0.0000010000000000
13264 && IKabs(
13265 evalcond
13266 [1])
13267 < 0.0000010000000000
13268 && IKabs(
13269 evalcond
13270 [2])
13271 < 0.0000010000000000
13272 && IKabs(
13273 evalcond
13274 [3])
13275 < 0.0000010000000000
13276 && IKabs(
13277 evalcond
13278 [4])
13279 < 0.0000010000000000
13280 && IKabs(
13281 evalcond
13282 [5])
13283 < 0.0000010000000000)
13284 {
13285 bgotonextstatement
13286 = false;
13287 {
13288 IkReal j4array
13289 [4],
13290 cj4array
13291 [4],
13292 sj4array
13293 [4];
13294 bool j4valid
13295 [4]
13296 = {false};
13297 _nj4
13298 = 4;
13299 j4array
13300 [0]
13301 = 0;
13302 sj4array
13303 [0]
13304 = IKsin(
13305 j4array
13306 [0]);
13307 cj4array
13308 [0]
13309 = IKcos(
13310 j4array
13311 [0]);
13312 j4array
13313 [1]
13314 = 1.5707963267949;
13315 sj4array
13316 [1]
13317 = IKsin(
13318 j4array
13319 [1]);
13320 cj4array
13321 [1]
13322 = IKcos(
13323 j4array
13324 [1]);
13325 j4array
13326 [2]
13327 = 3.14159265358979;
13328 sj4array
13329 [2]
13330 = IKsin(
13331 j4array
13332 [2]);
13333 cj4array
13334 [2]
13335 = IKcos(
13336 j4array
13337 [2]);
13338 j4array
13339 [3]
13340 = -1.5707963267949;
13341 sj4array
13342 [3]
13343 = IKsin(
13344 j4array
13345 [3]);
13346 cj4array
13347 [3]
13348 = IKcos(
13349 j4array
13350 [3]);
13351 if (j4array
13352 [0]
13353 > IKPI)
13354 {
13355 j4array
13356 [0]
13357 -= IK2PI;
13358 }
13359 else if (
13360 j4array
13361 [0]
13362 < -IKPI)
13363 {
13364 j4array
13365 [0]
13366 += IK2PI;
13367 }
13368 j4valid
13369 [0]
13370 = true;
13371 if (j4array
13372 [1]
13373 > IKPI)
13374 {
13375 j4array
13376 [1]
13377 -= IK2PI;
13378 }
13379 else if (
13380 j4array
13381 [1]
13382 < -IKPI)
13383 {
13384 j4array
13385 [1]
13386 += IK2PI;
13387 }
13388 j4valid
13389 [1]
13390 = true;
13391 if (j4array
13392 [2]
13393 > IKPI)
13394 {
13395 j4array
13396 [2]
13397 -= IK2PI;
13398 }
13399 else if (
13400 j4array
13401 [2]
13402 < -IKPI)
13403 {
13404 j4array
13405 [2]
13406 += IK2PI;
13407 }
13408 j4valid
13409 [2]
13410 = true;
13411 if (j4array
13412 [3]
13413 > IKPI)
13414 {
13415 j4array
13416 [3]
13417 -= IK2PI;
13418 }
13419 else if (
13420 j4array
13421 [3]
13422 < -IKPI)
13423 {
13424 j4array
13425 [3]
13426 += IK2PI;
13427 }
13428 j4valid
13429 [3]
13430 = true;
13431 for (
13432 int ij4
13433 = 0;
13434 ij4
13435 < 4;
13436 ++ij4)
13437 {
13438 if (!j4valid
13439 [ij4])
13440 {
13441 continue;
13442 }
13443 _ij4[0]
13444 = ij4;
13445 _ij4[1]
13446 = -1;
13447 for (
13448 int iij4
13449 = ij4
13450 + 1;
13451 iij4
13452 < 4;
13453 ++iij4)
13454 {
13455 if (j4valid
13456 [iij4]
13457 && IKabs(
13458 cj4array
13459 [ij4]
13460 - cj4array
13461 [iij4])
13462 < IKFAST_SOLUTION_THRESH
13463 && IKabs(
13464 sj4array
13465 [ij4]
13466 - sj4array
13467 [iij4])
13468 < IKFAST_SOLUTION_THRESH)
13469 {
13470 j4valid
13471 [iij4]
13472 = false;
13473 _ij4[1]
13474 = iij4;
13475 break;
13476 }
13477 }
13478 j4 = j4array
13479 [ij4];
13480 cj4 = cj4array
13481 [ij4];
13482 sj4 = sj4array
13483 [ij4];
13484
13485 rotationfunction0(
13486 solutions);
13487 }
13488 }
13489 }
13490 } while (
13491 0);
13492 if (bgotonextstatement)
13493 {
13494 bool
13495 bgotonextstatement
13496 = true;
13497 do
13498 {
13499 if (1)
13500 {
13501 bgotonextstatement
13502 = false;
13503 continue; // branch miss [j4]
13504 }
13505 } while (
13506 0);
13507 if (bgotonextstatement)
13508 {
13509 }
13510 }
13511 }
13512 }
13513 else
13514 {
13515 {
13516 IkReal j4array
13517 [1],
13518 cj4array
13519 [1],
13520 sj4array
13521 [1];
13522 bool j4valid
13523 [1]
13524 = {false};
13525 _nj4
13526 = 1;
13527 IkReal
13528 x1254
13529 = (cj9
13530 * px);
13531 IkReal
13532 x1255
13533 = (cj9
13534 * py);
13535 IkReal
13536 x1256
13537 = (pp
13538 * px);
13539 IkReal
13540 x1257
13541 = (pp
13542 * py);
13543 CheckValue<IkReal> x1258 = IKatan2WithCheck(
13544 IkReal((
13545 (((0.348408823529412)
13546 * py))
13547 + (((0.647058823529412)
13548 * x1257))
13549 + (((4.31372549019608)
13550 * x1256))
13551 + (((0.119091176470588)
13552 * x1255))
13553 + (((-1.0)
13554 * (1.66110784313725)
13555 * px))
13556 + (((-1.45555882352941)
13557 * x1254)))),
13558 ((((0.348408823529412)
13559 * px))
13560 + (((1.66110784313725)
13561 * py))
13562 + (((0.119091176470588)
13563 * x1254))
13564 + (((-4.31372549019608)
13565 * x1257))
13566 + (((0.647058823529412)
13567 * x1256))
13568 + (((1.45555882352941)
13569 * x1255))),
13570 IKFAST_ATAN2_MAGTHRESH);
13571 if (!x1258
13572 .valid)
13573 {
13574 continue;
13575 }
13576 CheckValue<IkReal> x1259 = IKPowWithIntegerCheck(
13577 IKsign((
13578 (((1.1)
13579 * pp))
13580 + (((-1.0)
13581 * (1.1)
13582 * (pz
13583 * pz))))),
13584 -1);
13585 if (!x1259
13586 .valid)
13587 {
13588 continue;
13589 }
13590 j4array
13591 [0]
13592 = ((-1.5707963267949)
13593 + (x1258
13594 .value)
13595 + (((1.5707963267949)
13596 * (x1259
13597 .value))));
13598 sj4array
13599 [0]
13600 = IKsin(
13601 j4array
13602 [0]);
13603 cj4array
13604 [0]
13605 = IKcos(
13606 j4array
13607 [0]);
13608 if (j4array
13609 [0]
13610 > IKPI)
13611 {
13612 j4array
13613 [0]
13614 -= IK2PI;
13615 }
13616 else if (
13617 j4array
13618 [0]
13619 < -IKPI)
13620 {
13621 j4array
13622 [0]
13623 += IK2PI;
13624 }
13625 j4valid
13626 [0]
13627 = true;
13628 for (
13629 int ij4
13630 = 0;
13631 ij4
13632 < 1;
13633 ++ij4)
13634 {
13635 if (!j4valid
13636 [ij4])
13637 {
13638 continue;
13639 }
13640 _ij4[0]
13641 = ij4;
13642 _ij4[1]
13643 = -1;
13644 for (
13645 int iij4
13646 = ij4
13647 + 1;
13648 iij4
13649 < 1;
13650 ++iij4)
13651 {
13652 if (j4valid
13653 [iij4]
13654 && IKabs(
13655 cj4array
13656 [ij4]
13657 - cj4array
13658 [iij4])
13659 < IKFAST_SOLUTION_THRESH
13660 && IKabs(
13661 sj4array
13662 [ij4]
13663 - sj4array
13664 [iij4])
13665 < IKFAST_SOLUTION_THRESH)
13666 {
13667 j4valid
13668 [iij4]
13669 = false;
13670 _ij4[1]
13671 = iij4;
13672 break;
13673 }
13674 }
13675 j4 = j4array
13676 [ij4];
13677 cj4 = cj4array
13678 [ij4];
13679 sj4 = sj4array
13680 [ij4];
13681 {
13682 IkReal evalcond
13683 [4];
13684 IkReal
13685 x1260
13686 = IKcos(
13687 j4);
13688 IkReal
13689 x1261
13690 = (px
13691 * x1260);
13692 IkReal
13693 x1262
13694 = IKsin(
13695 j4);
13696 IkReal
13697 x1263
13698 = (py
13699 * x1262);
13700 IkReal
13701 x1264
13702 = (px
13703 * x1262);
13704 IkReal
13705 x1265
13706 = (py
13707 * x1260);
13708 evalcond
13709 [0]
13710 = ((-0.55)
13711 + x1261
13712 + x1263
13713 + (((-1.0)
13714 * (0.3)
13715 * cj9))
13716 + (((-1.0)
13717 * (0.045)
13718 * sj9)));
13719 evalcond
13720 [1]
13721 = ((1.51009803921569)
13722 + (((-1.0)
13723 * (3.92156862745098)
13724 * pp))
13725 + (((-1.0)
13726 * x1265))
13727 + x1264
13728 + (((1.32323529411765)
13729 * cj9)));
13730 evalcond
13731 [2]
13732 = ((0.316735294117647)
13733 + (((-1.0)
13734 * x1261))
13735 + (((-1.0)
13736 * x1263))
13737 + (((0.108264705882353)
13738 * cj9))
13739 + (((0.588235294117647)
13740 * pp)));
13741 evalcond
13742 [3]
13743 = ((-0.2125)
13744 + (((0.09)
13745 * x1264))
13746 + (((-0.09)
13747 * x1265))
13748 + (((-1.0)
13749 * (1.0)
13750 * pp))
13751 + (((1.1)
13752 * x1263))
13753 + (((1.1)
13754 * x1261)));
13755 if (IKabs(
13756 evalcond
13757 [0])
13758 > IKFAST_EVALCOND_THRESH
13759 || IKabs(
13760 evalcond
13761 [1])
13762 > IKFAST_EVALCOND_THRESH
13763 || IKabs(
13764 evalcond
13765 [2])
13766 > IKFAST_EVALCOND_THRESH
13767 || IKabs(
13768 evalcond
13769 [3])
13770 > IKFAST_EVALCOND_THRESH)
13771 {
13772 continue;
13773 }
13774 }
13775
13776 rotationfunction0(
13777 solutions);
13778 }
13779 }
13780 }
13781 }
13782 }
13783 else
13784 {
13785 {
13786 IkReal j4array
13787 [1],
13788 cj4array
13789 [1],
13790 sj4array
13791 [1];
13792 bool j4valid
13793 [1]
13794 = {false};
13795 _nj4 = 1;
13796 IkReal
13797 x1266
13798 = (cj9
13799 * px);
13800 IkReal
13801 x1267
13802 = (cj9
13803 * py);
13804 IkReal
13805 x1268
13806 = (pp
13807 * px);
13808 IkReal
13809 x1269
13810 = (pp
13811 * py);
13812 CheckValue<IkReal> x1270 = IKatan2WithCheck(
13813 IkReal((
13814 (((0.316735294117647)
13815 * py))
13816 + (((0.108264705882353)
13817 * x1267))
13818 + (((-1.32323529411765)
13819 * x1266))
13820 + (((3.92156862745098)
13821 * x1268))
13822 + (((-1.0)
13823 * (1.51009803921569)
13824 * px))
13825 + (((0.588235294117647)
13826 * x1269)))),
13827 ((((0.588235294117647)
13828 * x1268))
13829 + (((1.32323529411765)
13830 * x1267))
13831 + (((0.316735294117647)
13832 * px))
13833 + (((1.51009803921569)
13834 * py))
13835 + (((-3.92156862745098)
13836 * x1269))
13837 + (((0.108264705882353)
13838 * x1266))),
13839 IKFAST_ATAN2_MAGTHRESH);
13840 if (!x1270
13841 .valid)
13842 {
13843 continue;
13844 }
13845 CheckValue<IkReal> x1271 = IKPowWithIntegerCheck(
13846 IKsign((
13847 pp
13848 + (((-1.0)
13849 * (1.0)
13850 * (pz
13851 * pz))))),
13852 -1);
13853 if (!x1271
13854 .valid)
13855 {
13856 continue;
13857 }
13858 j4array[0]
13859 = ((-1.5707963267949)
13860 + (x1270
13861 .value)
13862 + (((1.5707963267949)
13863 * (x1271
13864 .value))));
13865 sj4array[0] = IKsin(
13866 j4array
13867 [0]);
13868 cj4array[0] = IKcos(
13869 j4array
13870 [0]);
13871 if (j4array
13872 [0]
13873 > IKPI)
13874 {
13875 j4array
13876 [0]
13877 -= IK2PI;
13878 }
13879 else if (
13880 j4array
13881 [0]
13882 < -IKPI)
13883 {
13884 j4array
13885 [0]
13886 += IK2PI;
13887 }
13888 j4valid[0]
13889 = true;
13890 for (
13891 int ij4
13892 = 0;
13893 ij4
13894 < 1;
13895 ++ij4)
13896 {
13897 if (!j4valid
13898 [ij4])
13899 {
13900 continue;
13901 }
13902 _ij4[0]
13903 = ij4;
13904 _ij4[1]
13905 = -1;
13906 for (
13907 int iij4
13908 = ij4
13909 + 1;
13910 iij4
13911 < 1;
13912 ++iij4)
13913 {
13914 if (j4valid
13915 [iij4]
13916 && IKabs(
13917 cj4array
13918 [ij4]
13919 - cj4array
13920 [iij4])
13921 < IKFAST_SOLUTION_THRESH
13922 && IKabs(
13923 sj4array
13924 [ij4]
13925 - sj4array
13926 [iij4])
13927 < IKFAST_SOLUTION_THRESH)
13928 {
13929 j4valid
13930 [iij4]
13931 = false;
13932 _ij4[1]
13933 = iij4;
13934 break;
13935 }
13936 }
13937 j4 = j4array
13938 [ij4];
13939 cj4 = cj4array
13940 [ij4];
13941 sj4 = sj4array
13942 [ij4];
13943 {
13944 IkReal evalcond
13945 [4];
13946 IkReal
13947 x1272
13948 = IKcos(
13949 j4);
13950 IkReal
13951 x1273
13952 = (px
13953 * x1272);
13954 IkReal
13955 x1274
13956 = IKsin(
13957 j4);
13958 IkReal
13959 x1275
13960 = (py
13961 * x1274);
13962 IkReal
13963 x1276
13964 = (px
13965 * x1274);
13966 IkReal
13967 x1277
13968 = (py
13969 * x1272);
13970 evalcond
13971 [0]
13972 = ((-0.55)
13973 + x1275
13974 + x1273
13975 + (((-1.0)
13976 * (0.3)
13977 * cj9))
13978 + (((-1.0)
13979 * (0.045)
13980 * sj9)));
13981 evalcond
13982 [1]
13983 = ((1.51009803921569)
13984 + (((-1.0)
13985 * (3.92156862745098)
13986 * pp))
13987 + x1276
13988 + (((1.32323529411765)
13989 * cj9))
13990 + (((-1.0)
13991 * x1277)));
13992 evalcond
13993 [2]
13994 = ((0.316735294117647)
13995 + (((0.108264705882353)
13996 * cj9))
13997 + (((-1.0)
13998 * x1273))
13999 + (((0.588235294117647)
14000 * pp))
14001 + (((-1.0)
14002 * x1275)));
14003 evalcond
14004 [3]
14005 = ((-0.2125)
14006 + (((0.09)
14007 * x1276))
14008 + (((1.1)
14009 * x1275))
14010 + (((1.1)
14011 * x1273))
14012 + (((-1.0)
14013 * (1.0)
14014 * pp))
14015 + (((-0.09)
14016 * x1277)));
14017 if (IKabs(
14018 evalcond
14019 [0])
14020 > IKFAST_EVALCOND_THRESH
14021 || IKabs(
14022 evalcond
14023 [1])
14024 > IKFAST_EVALCOND_THRESH
14025 || IKabs(
14026 evalcond
14027 [2])
14028 > IKFAST_EVALCOND_THRESH
14029 || IKabs(
14030 evalcond
14031 [3])
14032 > IKFAST_EVALCOND_THRESH)
14033 {
14034 continue;
14035 }
14036 }
14037
14038 rotationfunction0(
14039 solutions);
14040 }
14041 }
14042 }
14043 }
14044 }
14045 else
14046 {
14047 {
14048 IkReal j4array
14049 [1],
14050 cj4array
14051 [1],
14052 sj4array
14053 [1];
14054 bool
14055 j4valid[1]
14056 = {false};
14057 _nj4 = 1;
14058 IkReal x1278
14059 = (cj9
14060 * px);
14061 IkReal x1279
14062 = (cj9
14063 * py);
14064 IkReal x1280
14065 = ((3.92156862745098)
14066 * pp);
14067 IkReal x1281
14068 = ((0.045)
14069 * sj9);
14070 CheckValue<IkReal> x1282 = IKPowWithIntegerCheck(
14071 IKsign((
14072 pp
14073 + (((-1.0)
14074 * (1.0)
14075 * (pz
14076 * pz))))),
14077 -1);
14078 if (!x1282
14079 .valid)
14080 {
14081 continue;
14082 }
14083 CheckValue<IkReal> x1283 = IKatan2WithCheck(
14084 IkReal((
14085 (((-1.0)
14086 * (1.51009803921569)
14087 * px))
14088 + (((-1.32323529411765)
14089 * x1278))
14090 + (((0.55)
14091 * py))
14092 + ((px
14093 * x1280))
14094 + ((py
14095 * x1281))
14096 + (((0.3)
14097 * x1279)))),
14098 ((((-1.0)
14099 * py
14100 * x1280))
14101 + (((1.32323529411765)
14102 * x1279))
14103 + (((1.51009803921569)
14104 * py))
14105 + (((0.3)
14106 * x1278))
14107 + (((0.55)
14108 * px))
14109 + ((px
14110 * x1281))),
14111 IKFAST_ATAN2_MAGTHRESH);
14112 if (!x1283
14113 .valid)
14114 {
14115 continue;
14116 }
14117 j4array[0]
14118 = ((-1.5707963267949)
14119 + (((1.5707963267949)
14120 * (x1282
14121 .value)))
14122 + (x1283
14123 .value));
14124 sj4array[0]
14125 = IKsin(
14126 j4array
14127 [0]);
14128 cj4array[0]
14129 = IKcos(
14130 j4array
14131 [0]);
14132 if (j4array[0]
14133 > IKPI)
14134 {
14135 j4array[0]
14136 -= IK2PI;
14137 }
14138 else if (
14139 j4array[0]
14140 < -IKPI)
14141 {
14142 j4array[0]
14143 += IK2PI;
14144 }
14145 j4valid[0]
14146 = true;
14147 for (int ij4
14148 = 0;
14149 ij4 < 1;
14150 ++ij4)
14151 {
14152 if (!j4valid
14153 [ij4])
14154 {
14155 continue;
14156 }
14157 _ij4[0]
14158 = ij4;
14159 _ij4[1]
14160 = -1;
14161 for (
14162 int iij4
14163 = ij4
14164 + 1;
14165 iij4
14166 < 1;
14167 ++iij4)
14168 {
14169 if (j4valid
14170 [iij4]
14171 && IKabs(
14172 cj4array
14173 [ij4]
14174 - cj4array
14175 [iij4])
14176 < IKFAST_SOLUTION_THRESH
14177 && IKabs(
14178 sj4array
14179 [ij4]
14180 - sj4array
14181 [iij4])
14182 < IKFAST_SOLUTION_THRESH)
14183 {
14184 j4valid
14185 [iij4]
14186 = false;
14187 _ij4[1]
14188 = iij4;
14189 break;
14190 }
14191 }
14192 j4 = j4array
14193 [ij4];
14194 cj4 = cj4array
14195 [ij4];
14196 sj4 = sj4array
14197 [ij4];
14198 {
14199 IkReal evalcond
14200 [4];
14201 IkReal
14202 x1284
14203 = IKcos(
14204 j4);
14205 IkReal
14206 x1285
14207 = (px
14208 * x1284);
14209 IkReal
14210 x1286
14211 = IKsin(
14212 j4);
14213 IkReal
14214 x1287
14215 = (py
14216 * x1286);
14217 IkReal
14218 x1288
14219 = (px
14220 * x1286);
14221 IkReal
14222 x1289
14223 = (py
14224 * x1284);
14225 evalcond
14226 [0]
14227 = ((-0.55)
14228 + x1287
14229 + x1285
14230 + (((-1.0)
14231 * (0.3)
14232 * cj9))
14233 + (((-1.0)
14234 * (0.045)
14235 * sj9)));
14236 evalcond
14237 [1]
14238 = ((1.51009803921569)
14239 + (((-1.0)
14240 * x1289))
14241 + (((-1.0)
14242 * (3.92156862745098)
14243 * pp))
14244 + x1288
14245 + (((1.32323529411765)
14246 * cj9)));
14247 evalcond
14248 [2]
14249 = ((0.316735294117647)
14250 + (((-1.0)
14251 * x1285))
14252 + (((0.108264705882353)
14253 * cj9))
14254 + (((0.588235294117647)
14255 * pp))
14256 + (((-1.0)
14257 * x1287)));
14258 evalcond
14259 [3]
14260 = ((-0.2125)
14261 + (((0.09)
14262 * x1288))
14263 + (((-0.09)
14264 * x1289))
14265 + (((1.1)
14266 * x1287))
14267 + (((-1.0)
14268 * (1.0)
14269 * pp))
14270 + (((1.1)
14271 * x1285)));
14272 if (IKabs(
14273 evalcond
14274 [0])
14275 > IKFAST_EVALCOND_THRESH
14276 || IKabs(
14277 evalcond
14278 [1])
14279 > IKFAST_EVALCOND_THRESH
14280 || IKabs(
14281 evalcond
14282 [2])
14283 > IKFAST_EVALCOND_THRESH
14284 || IKabs(
14285 evalcond
14286 [3])
14287 > IKFAST_EVALCOND_THRESH)
14288 {
14289 continue;
14290 }
14291 }
14292
14293 rotationfunction0(
14294 solutions);
14295 }
14296 }
14297 }
14298 }
14299 }
14300 } while (0);
14301 if (bgotonextstatement)
14302 {
14303 bool
14304 bgotonextstatement
14305 = true;
14306 do
14307 {
14308 evalcond[0]
14309 = ((-3.14159265358979)
14310 + (IKfmod(
14311 ((3.14159265358979)
14312 + (IKabs((
14313 (1.5707963267949)
14314 + j6)))),
14315 6.28318530717959)));
14316 evalcond[1]
14317 = ((-1.0)
14318 * (((1.0)
14319 * pz)));
14320 if (IKabs(
14321 evalcond[0])
14322 < 0.0000010000000000
14323 && IKabs(
14324 evalcond
14325 [1])
14326 < 0.0000010000000000)
14327 {
14328 bgotonextstatement
14329 = false;
14330 {
14331 IkReal
14332 j4eval[3];
14333 sj8 = -1.0;
14334 cj8 = 0;
14335 j8 = -1.5707963267949;
14336 sj6 = -1.0;
14337 cj6 = 0;
14338 j6 = -1.5707963267949;
14339 IkReal x1290
14340 = (pp
14341 + (((-1.0)
14342 * (1.0)
14343 * (pz
14344 * pz))));
14345 IkReal x1291
14346 = (cj9
14347 * px);
14348 IkReal x1292
14349 = (cj9
14350 * py);
14351 IkReal x1293
14352 = ((3.92156862745098)
14353 * pp);
14354 IkReal x1294
14355 = ((0.045)
14356 * sj9);
14357 j4eval[0]
14358 = x1290;
14359 j4eval[1]
14360 = ((IKabs((
14361 (((-1.32323529411765)
14362 * x1291))
14363 + ((px
14364 * x1293))
14365 + (((-1.0)
14366 * (1.51009803921569)
14367 * px))
14368 + (((-1.0)
14369 * py
14370 * x1294))
14371 + (((-0.3)
14372 * x1292))
14373 + (((-1.0)
14374 * (0.55)
14375 * py)))))
14376 + (IKabs((
14377 (((-1.0)
14378 * (0.55)
14379 * px))
14380 + (((-0.3)
14381 * x1291))
14382 + (((-1.0)
14383 * px
14384 * x1294))
14385 + (((1.51009803921569)
14386 * py))
14387 + (((-1.0)
14388 * py
14389 * x1293))
14390 + (((1.32323529411765)
14391 * x1292))))));
14392 j4eval[2]
14393 = IKsign(
14394 x1290);
14395 if (IKabs(
14396 j4eval
14397 [0])
14398 < 0.0000010000000000
14399 || IKabs(
14400 j4eval
14401 [1])
14402 < 0.0000010000000000
14403 || IKabs(
14404 j4eval
14405 [2])
14406 < 0.0000010000000000)
14407 {
14408 {
14409 IkReal
14410 j4eval
14411 [3];
14412 sj8 = -1.0;
14413 cj8 = 0;
14414 j8 = -1.5707963267949;
14415 sj6 = -1.0;
14416 cj6 = 0;
14417 j6 = -1.5707963267949;
14418 IkReal x1295
14419 = (pp
14420 + (((-1.0)
14421 * (1.0)
14422 * (pz
14423 * pz))));
14424 IkReal x1296
14425 = (cj9
14426 * px);
14427 IkReal x1297
14428 = (cj9
14429 * py);
14430 IkReal x1298
14431 = (pp
14432 * px);
14433 IkReal x1299
14434 = (pp
14435 * py);
14436 j4eval[0]
14437 = x1295;
14438 j4eval[1]
14439 = ((IKabs((
14440 (((-3.92156862745098)
14441 * x1299))
14442 + (((1.51009803921569)
14443 * py))
14444 + (((-0.108264705882353)
14445 * x1296))
14446 + (((-1.0)
14447 * (0.316735294117647)
14448 * px))
14449 + (((1.32323529411765)
14450 * x1297))
14451 + (((-0.588235294117647)
14452 * x1298)))))
14453 + (IKabs((
14454 (((-0.588235294117647)
14455 * x1299))
14456 + (((-1.32323529411765)
14457 * x1296))
14458 + (((-1.0)
14459 * (1.51009803921569)
14460 * px))
14461 + (((-1.0)
14462 * (0.316735294117647)
14463 * py))
14464 + (((-0.108264705882353)
14465 * x1297))
14466 + (((3.92156862745098)
14467 * x1298))))));
14468 j4eval[2]
14469 = IKsign(
14470 x1295);
14471 if (IKabs(
14472 j4eval
14473 [0])
14474 < 0.0000010000000000
14475 || IKabs(
14476 j4eval
14477 [1])
14478 < 0.0000010000000000
14479 || IKabs(
14480 j4eval
14481 [2])
14482 < 0.0000010000000000)
14483 {
14484 {
14485 IkReal j4eval
14486 [3];
14487 sj8 = -1.0;
14488 cj8 = 0;
14489 j8 = -1.5707963267949;
14490 sj6 = -1.0;
14491 cj6 = 0;
14492 j6 = -1.5707963267949;
14493 IkReal
14494 x1300
14495 = pz
14496 * pz;
14497 IkReal
14498 x1301
14499 = (cj9
14500 * px);
14501 IkReal
14502 x1302
14503 = (cj9
14504 * py);
14505 IkReal
14506 x1303
14507 = (pp
14508 * px);
14509 IkReal
14510 x1304
14511 = (pp
14512 * py);
14513 j4eval
14514 [0]
14515 = (x1300
14516 + (((-1.0)
14517 * (1.0)
14518 * pp)));
14519 j4eval[1] = IKsign((
14520 (((1.1)
14521 * x1300))
14522 + (((-1.0)
14523 * (1.1)
14524 * pp))));
14525 j4eval
14526 [2]
14527 = ((IKabs((
14528 (((4.31372549019608)
14529 * x1304))
14530 + (((0.348408823529412)
14531 * px))
14532 + (((0.119091176470588)
14533 * x1301))
14534 + (((-1.0)
14535 * (1.66110784313725)
14536 * py))
14537 + (((0.647058823529412)
14538 * x1303))
14539 + (((-1.45555882352941)
14540 * x1302)))))
14541 + (IKabs((
14542 (((0.348408823529412)
14543 * py))
14544 + (((1.66110784313725)
14545 * px))
14546 + (((1.45555882352941)
14547 * x1301))
14548 + (((-4.31372549019608)
14549 * x1303))
14550 + (((0.647058823529412)
14551 * x1304))
14552 + (((0.119091176470588)
14553 * x1302))))));
14554 if (IKabs(
14555 j4eval
14556 [0])
14557 < 0.0000010000000000
14558 || IKabs(
14559 j4eval
14560 [1])
14561 < 0.0000010000000000
14562 || IKabs(
14563 j4eval
14564 [2])
14565 < 0.0000010000000000)
14566 {
14567 {
14568 IkReal evalcond
14569 [6];
14570 bool
14571 bgotonextstatement
14572 = true;
14573 do
14574 {
14575 IkReal
14576 x1305
14577 = ((1.32323529411765)
14578 * cj9);
14579 IkReal
14580 x1306
14581 = ((3.92156862745098)
14582 * pp);
14583 evalcond
14584 [0]
14585 = ((IKabs(
14586 py))
14587 + (IKabs(
14588 px)));
14589 evalcond
14590 [1]
14591 = ((-0.55)
14592 + (((-1.0)
14593 * (0.3)
14594 * cj9))
14595 + (((-1.0)
14596 * (0.045)
14597 * sj9)));
14598 evalcond
14599 [2]
14600 = ((1.51009803921569)
14601 + (((-1.0)
14602 * x1306))
14603 + x1305);
14604 evalcond
14605 [3]
14606 = ((-1.51009803921569)
14607 + x1306
14608 + (((-1.0)
14609 * x1305)));
14610 evalcond
14611 [4]
14612 = ((-0.316735294117647)
14613 + (((-1.0)
14614 * (0.588235294117647)
14615 * pp))
14616 + (((-1.0)
14617 * (0.108264705882353)
14618 * cj9)));
14619 evalcond
14620 [5]
14621 = ((-0.2125)
14622 + (((-1.0)
14623 * (1.0)
14624 * pp)));
14625 if (IKabs(
14626 evalcond
14627 [0])
14628 < 0.0000010000000000
14629 && IKabs(
14630 evalcond
14631 [1])
14632 < 0.0000010000000000
14633 && IKabs(
14634 evalcond
14635 [2])
14636 < 0.0000010000000000
14637 && IKabs(
14638 evalcond
14639 [3])
14640 < 0.0000010000000000
14641 && IKabs(
14642 evalcond
14643 [4])
14644 < 0.0000010000000000
14645 && IKabs(
14646 evalcond
14647 [5])
14648 < 0.0000010000000000)
14649 {
14650 bgotonextstatement
14651 = false;
14652 {
14653 IkReal j4array
14654 [4],
14655 cj4array
14656 [4],
14657 sj4array
14658 [4];
14659 bool j4valid
14660 [4]
14661 = {false};
14662 _nj4
14663 = 4;
14664 j4array
14665 [0]
14666 = 0;
14667 sj4array
14668 [0]
14669 = IKsin(
14670 j4array
14671 [0]);
14672 cj4array
14673 [0]
14674 = IKcos(
14675 j4array
14676 [0]);
14677 j4array
14678 [1]
14679 = 1.5707963267949;
14680 sj4array
14681 [1]
14682 = IKsin(
14683 j4array
14684 [1]);
14685 cj4array
14686 [1]
14687 = IKcos(
14688 j4array
14689 [1]);
14690 j4array
14691 [2]
14692 = 3.14159265358979;
14693 sj4array
14694 [2]
14695 = IKsin(
14696 j4array
14697 [2]);
14698 cj4array
14699 [2]
14700 = IKcos(
14701 j4array
14702 [2]);
14703 j4array
14704 [3]
14705 = -1.5707963267949;
14706 sj4array
14707 [3]
14708 = IKsin(
14709 j4array
14710 [3]);
14711 cj4array
14712 [3]
14713 = IKcos(
14714 j4array
14715 [3]);
14716 if (j4array
14717 [0]
14718 > IKPI)
14719 {
14720 j4array
14721 [0]
14722 -= IK2PI;
14723 }
14724 else if (
14725 j4array
14726 [0]
14727 < -IKPI)
14728 {
14729 j4array
14730 [0]
14731 += IK2PI;
14732 }
14733 j4valid
14734 [0]
14735 = true;
14736 if (j4array
14737 [1]
14738 > IKPI)
14739 {
14740 j4array
14741 [1]
14742 -= IK2PI;
14743 }
14744 else if (
14745 j4array
14746 [1]
14747 < -IKPI)
14748 {
14749 j4array
14750 [1]
14751 += IK2PI;
14752 }
14753 j4valid
14754 [1]
14755 = true;
14756 if (j4array
14757 [2]
14758 > IKPI)
14759 {
14760 j4array
14761 [2]
14762 -= IK2PI;
14763 }
14764 else if (
14765 j4array
14766 [2]
14767 < -IKPI)
14768 {
14769 j4array
14770 [2]
14771 += IK2PI;
14772 }
14773 j4valid
14774 [2]
14775 = true;
14776 if (j4array
14777 [3]
14778 > IKPI)
14779 {
14780 j4array
14781 [3]
14782 -= IK2PI;
14783 }
14784 else if (
14785 j4array
14786 [3]
14787 < -IKPI)
14788 {
14789 j4array
14790 [3]
14791 += IK2PI;
14792 }
14793 j4valid
14794 [3]
14795 = true;
14796 for (
14797 int ij4
14798 = 0;
14799 ij4
14800 < 4;
14801 ++ij4)
14802 {
14803 if (!j4valid
14804 [ij4])
14805 {
14806 continue;
14807 }
14808 _ij4[0]
14809 = ij4;
14810 _ij4[1]
14811 = -1;
14812 for (
14813 int iij4
14814 = ij4
14815 + 1;
14816 iij4
14817 < 4;
14818 ++iij4)
14819 {
14820 if (j4valid
14821 [iij4]
14822 && IKabs(
14823 cj4array
14824 [ij4]
14825 - cj4array
14826 [iij4])
14827 < IKFAST_SOLUTION_THRESH
14828 && IKabs(
14829 sj4array
14830 [ij4]
14831 - sj4array
14832 [iij4])
14833 < IKFAST_SOLUTION_THRESH)
14834 {
14835 j4valid
14836 [iij4]
14837 = false;
14838 _ij4[1]
14839 = iij4;
14840 break;
14841 }
14842 }
14843 j4 = j4array
14844 [ij4];
14845 cj4 = cj4array
14846 [ij4];
14847 sj4 = sj4array
14848 [ij4];
14849
14850 rotationfunction0(
14851 solutions);
14852 }
14853 }
14854 }
14855 } while (
14856 0);
14857 if (bgotonextstatement)
14858 {
14859 bool
14860 bgotonextstatement
14861 = true;
14862 do
14863 {
14864 if (1)
14865 {
14866 bgotonextstatement
14867 = false;
14868 continue; // branch miss [j4]
14869 }
14870 } while (
14871 0);
14872 if (bgotonextstatement)
14873 {
14874 }
14875 }
14876 }
14877 }
14878 else
14879 {
14880 {
14881 IkReal j4array
14882 [1],
14883 cj4array
14884 [1],
14885 sj4array
14886 [1];
14887 bool j4valid
14888 [1]
14889 = {false};
14890 _nj4
14891 = 1;
14892 IkReal
14893 x1307
14894 = (cj9
14895 * px);
14896 IkReal
14897 x1308
14898 = (cj9
14899 * py);
14900 IkReal
14901 x1309
14902 = (pp
14903 * px);
14904 IkReal
14905 x1310
14906 = (pp
14907 * py);
14908 CheckValue<IkReal> x1311 = IKPowWithIntegerCheck(
14909 IKsign((
14910 (((1.1)
14911 * (pz
14912 * pz)))
14913 + (((-1.0)
14914 * (1.1)
14915 * pp)))),
14916 -1);
14917 if (!x1311
14918 .valid)
14919 {
14920 continue;
14921 }
14922 CheckValue<IkReal> x1312 = IKatan2WithCheck(
14923 IkReal((
14924 (((1.45555882352941)
14925 * x1307))
14926 + (((0.348408823529412)
14927 * py))
14928 + (((1.66110784313725)
14929 * px))
14930 + (((0.119091176470588)
14931 * x1308))
14932 + (((-4.31372549019608)
14933 * x1309))
14934 + (((0.647058823529412)
14935 * x1310)))),
14936 ((((0.647058823529412)
14937 * x1309))
14938 + (((0.348408823529412)
14939 * px))
14940 + (((4.31372549019608)
14941 * x1310))
14942 + (((-1.45555882352941)
14943 * x1308))
14944 + (((0.119091176470588)
14945 * x1307))
14946 + (((-1.0)
14947 * (1.66110784313725)
14948 * py))),
14949 IKFAST_ATAN2_MAGTHRESH);
14950 if (!x1312
14951 .valid)
14952 {
14953 continue;
14954 }
14955 j4array
14956 [0]
14957 = ((-1.5707963267949)
14958 + (((1.5707963267949)
14959 * (x1311
14960 .value)))
14961 + (x1312
14962 .value));
14963 sj4array
14964 [0]
14965 = IKsin(
14966 j4array
14967 [0]);
14968 cj4array
14969 [0]
14970 = IKcos(
14971 j4array
14972 [0]);
14973 if (j4array
14974 [0]
14975 > IKPI)
14976 {
14977 j4array
14978 [0]
14979 -= IK2PI;
14980 }
14981 else if (
14982 j4array
14983 [0]
14984 < -IKPI)
14985 {
14986 j4array
14987 [0]
14988 += IK2PI;
14989 }
14990 j4valid
14991 [0]
14992 = true;
14993 for (
14994 int ij4
14995 = 0;
14996 ij4
14997 < 1;
14998 ++ij4)
14999 {
15000 if (!j4valid
15001 [ij4])
15002 {
15003 continue;
15004 }
15005 _ij4[0]
15006 = ij4;
15007 _ij4[1]
15008 = -1;
15009 for (
15010 int iij4
15011 = ij4
15012 + 1;
15013 iij4
15014 < 1;
15015 ++iij4)
15016 {
15017 if (j4valid
15018 [iij4]
15019 && IKabs(
15020 cj4array
15021 [ij4]
15022 - cj4array
15023 [iij4])
15024 < IKFAST_SOLUTION_THRESH
15025 && IKabs(
15026 sj4array
15027 [ij4]
15028 - sj4array
15029 [iij4])
15030 < IKFAST_SOLUTION_THRESH)
15031 {
15032 j4valid
15033 [iij4]
15034 = false;
15035 _ij4[1]
15036 = iij4;
15037 break;
15038 }
15039 }
15040 j4 = j4array
15041 [ij4];
15042 cj4 = cj4array
15043 [ij4];
15044 sj4 = sj4array
15045 [ij4];
15046 {
15047 IkReal evalcond
15048 [4];
15049 IkReal
15050 x1313
15051 = IKsin(
15052 j4);
15053 IkReal
15054 x1314
15055 = (px
15056 * x1313);
15057 IkReal
15058 x1315
15059 = IKcos(
15060 j4);
15061 IkReal
15062 x1316
15063 = (py
15064 * x1315);
15065 IkReal
15066 x1317
15067 = (px
15068 * x1315);
15069 IkReal
15070 x1318
15071 = (py
15072 * x1313);
15073 IkReal
15074 x1319
15075 = ((((-1.0)
15076 * x1318))
15077 + (((-1.0)
15078 * x1317)));
15079 evalcond
15080 [0]
15081 = ((1.51009803921569)
15082 + (((-1.0)
15083 * (3.92156862745098)
15084 * pp))
15085 + (((-1.0)
15086 * x1316))
15087 + (((1.32323529411765)
15088 * cj9))
15089 + x1314);
15090 evalcond
15091 [1]
15092 = ((-0.55)
15093 + (((-1.0)
15094 * (0.3)
15095 * cj9))
15096 + (((-1.0)
15097 * (0.045)
15098 * sj9))
15099 + x1319);
15100 evalcond
15101 [2]
15102 = ((-0.316735294117647)
15103 + (((-1.0)
15104 * (0.588235294117647)
15105 * pp))
15106 + (((-1.0)
15107 * (0.108264705882353)
15108 * cj9))
15109 + x1319);
15110 evalcond
15111 [3]
15112 = ((-0.2125)
15113 + (((-1.1)
15114 * x1317))
15115 + (((-1.0)
15116 * (1.0)
15117 * pp))
15118 + (((0.09)
15119 * x1314))
15120 + (((-0.09)
15121 * x1316))
15122 + (((-1.1)
15123 * x1318)));
15124 if (IKabs(
15125 evalcond
15126 [0])
15127 > IKFAST_EVALCOND_THRESH
15128 || IKabs(
15129 evalcond
15130 [1])
15131 > IKFAST_EVALCOND_THRESH
15132 || IKabs(
15133 evalcond
15134 [2])
15135 > IKFAST_EVALCOND_THRESH
15136 || IKabs(
15137 evalcond
15138 [3])
15139 > IKFAST_EVALCOND_THRESH)
15140 {
15141 continue;
15142 }
15143 }
15144
15145 rotationfunction0(
15146 solutions);
15147 }
15148 }
15149 }
15150 }
15151 }
15152 else
15153 {
15154 {
15155 IkReal j4array
15156 [1],
15157 cj4array
15158 [1],
15159 sj4array
15160 [1];
15161 bool j4valid
15162 [1]
15163 = {false};
15164 _nj4
15165 = 1;
15166 IkReal
15167 x1320
15168 = (cj9
15169 * px);
15170 IkReal
15171 x1321
15172 = (cj9
15173 * py);
15174 IkReal
15175 x1322
15176 = (pp
15177 * px);
15178 IkReal
15179 x1323
15180 = (pp
15181 * py);
15182 CheckValue<IkReal> x1324 = IKPowWithIntegerCheck(
15183 IKsign((
15184 pp
15185 + (((-1.0)
15186 * (1.0)
15187 * (pz
15188 * pz))))),
15189 -1);
15190 if (!x1324
15191 .valid)
15192 {
15193 continue;
15194 }
15195 CheckValue<IkReal> x1325 = IKatan2WithCheck(
15196 IkReal((
15197 (((-0.108264705882353)
15198 * x1321))
15199 + (((-1.0)
15200 * (1.51009803921569)
15201 * px))
15202 + (((-1.32323529411765)
15203 * x1320))
15204 + (((-1.0)
15205 * (0.316735294117647)
15206 * py))
15207 + (((3.92156862745098)
15208 * x1322))
15209 + (((-0.588235294117647)
15210 * x1323)))),
15211 ((((-3.92156862745098)
15212 * x1323))
15213 + (((-0.588235294117647)
15214 * x1322))
15215 + (((-0.108264705882353)
15216 * x1320))
15217 + (((1.51009803921569)
15218 * py))
15219 + (((1.32323529411765)
15220 * x1321))
15221 + (((-1.0)
15222 * (0.316735294117647)
15223 * px))),
15224 IKFAST_ATAN2_MAGTHRESH);
15225 if (!x1325
15226 .valid)
15227 {
15228 continue;
15229 }
15230 j4array
15231 [0]
15232 = ((-1.5707963267949)
15233 + (((1.5707963267949)
15234 * (x1324
15235 .value)))
15236 + (x1325
15237 .value));
15238 sj4array
15239 [0]
15240 = IKsin(
15241 j4array
15242 [0]);
15243 cj4array
15244 [0]
15245 = IKcos(
15246 j4array
15247 [0]);
15248 if (j4array
15249 [0]
15250 > IKPI)
15251 {
15252 j4array
15253 [0]
15254 -= IK2PI;
15255 }
15256 else if (
15257 j4array
15258 [0]
15259 < -IKPI)
15260 {
15261 j4array
15262 [0]
15263 += IK2PI;
15264 }
15265 j4valid
15266 [0]
15267 = true;
15268 for (
15269 int ij4
15270 = 0;
15271 ij4
15272 < 1;
15273 ++ij4)
15274 {
15275 if (!j4valid
15276 [ij4])
15277 {
15278 continue;
15279 }
15280 _ij4[0]
15281 = ij4;
15282 _ij4[1]
15283 = -1;
15284 for (
15285 int iij4
15286 = ij4
15287 + 1;
15288 iij4
15289 < 1;
15290 ++iij4)
15291 {
15292 if (j4valid
15293 [iij4]
15294 && IKabs(
15295 cj4array
15296 [ij4]
15297 - cj4array
15298 [iij4])
15299 < IKFAST_SOLUTION_THRESH
15300 && IKabs(
15301 sj4array
15302 [ij4]
15303 - sj4array
15304 [iij4])
15305 < IKFAST_SOLUTION_THRESH)
15306 {
15307 j4valid
15308 [iij4]
15309 = false;
15310 _ij4[1]
15311 = iij4;
15312 break;
15313 }
15314 }
15315 j4 = j4array
15316 [ij4];
15317 cj4 = cj4array
15318 [ij4];
15319 sj4 = sj4array
15320 [ij4];
15321 {
15322 IkReal evalcond
15323 [4];
15324 IkReal
15325 x1326
15326 = IKsin(
15327 j4);
15328 IkReal
15329 x1327
15330 = (px
15331 * x1326);
15332 IkReal
15333 x1328
15334 = IKcos(
15335 j4);
15336 IkReal
15337 x1329
15338 = (py
15339 * x1328);
15340 IkReal
15341 x1330
15342 = (px
15343 * x1328);
15344 IkReal
15345 x1331
15346 = (py
15347 * x1326);
15348 IkReal
15349 x1332
15350 = ((((-1.0)
15351 * x1330))
15352 + (((-1.0)
15353 * x1331)));
15354 evalcond
15355 [0]
15356 = ((1.51009803921569)
15357 + x1327
15358 + (((-1.0)
15359 * x1329))
15360 + (((-1.0)
15361 * (3.92156862745098)
15362 * pp))
15363 + (((1.32323529411765)
15364 * cj9)));
15365 evalcond
15366 [1]
15367 = ((-0.55)
15368 + x1332
15369 + (((-1.0)
15370 * (0.3)
15371 * cj9))
15372 + (((-1.0)
15373 * (0.045)
15374 * sj9)));
15375 evalcond
15376 [2]
15377 = ((-0.316735294117647)
15378 + x1332
15379 + (((-1.0)
15380 * (0.588235294117647)
15381 * pp))
15382 + (((-1.0)
15383 * (0.108264705882353)
15384 * cj9)));
15385 evalcond
15386 [3]
15387 = ((-0.2125)
15388 + (((-1.1)
15389 * x1330))
15390 + (((0.09)
15391 * x1327))
15392 + (((-0.09)
15393 * x1329))
15394 + (((-1.0)
15395 * (1.0)
15396 * pp))
15397 + (((-1.1)
15398 * x1331)));
15399 if (IKabs(
15400 evalcond
15401 [0])
15402 > IKFAST_EVALCOND_THRESH
15403 || IKabs(
15404 evalcond
15405 [1])
15406 > IKFAST_EVALCOND_THRESH
15407 || IKabs(
15408 evalcond
15409 [2])
15410 > IKFAST_EVALCOND_THRESH
15411 || IKabs(
15412 evalcond
15413 [3])
15414 > IKFAST_EVALCOND_THRESH)
15415 {
15416 continue;
15417 }
15418 }
15419
15420 rotationfunction0(
15421 solutions);
15422 }
15423 }
15424 }
15425 }
15426 }
15427 else
15428 {
15429 {
15430 IkReal
15431 j4array
15432 [1],
15433 cj4array
15434 [1],
15435 sj4array
15436 [1];
15437 bool j4valid
15438 [1]
15439 = {false};
15440 _nj4 = 1;
15441 IkReal x1333
15442 = (cj9
15443 * px);
15444 IkReal x1334
15445 = (cj9
15446 * py);
15447 IkReal x1335
15448 = ((3.92156862745098)
15449 * pp);
15450 IkReal x1336
15451 = ((0.045)
15452 * sj9);
15453 CheckValue<IkReal> x1337 = IKatan2WithCheck(
15454 IkReal((
15455 (((-1.32323529411765)
15456 * x1333))
15457 + (((-0.3)
15458 * x1334))
15459 + ((px
15460 * x1335))
15461 + (((-1.0)
15462 * (1.51009803921569)
15463 * px))
15464 + (((-1.0)
15465 * py
15466 * x1336))
15467 + (((-1.0)
15468 * (0.55)
15469 * py)))),
15470 ((((-1.0)
15471 * (0.55)
15472 * px))
15473 + (((-1.0)
15474 * py
15475 * x1335))
15476 + (((-1.0)
15477 * px
15478 * x1336))
15479 + (((1.51009803921569)
15480 * py))
15481 + (((-0.3)
15482 * x1333))
15483 + (((1.32323529411765)
15484 * x1334))),
15485 IKFAST_ATAN2_MAGTHRESH);
15486 if (!x1337
15487 .valid)
15488 {
15489 continue;
15490 }
15491 CheckValue<IkReal> x1338 = IKPowWithIntegerCheck(
15492 IKsign((
15493 pp
15494 + (((-1.0)
15495 * (1.0)
15496 * (pz
15497 * pz))))),
15498 -1);
15499 if (!x1338
15500 .valid)
15501 {
15502 continue;
15503 }
15504 j4array[0]
15505 = ((-1.5707963267949)
15506 + (x1337
15507 .value)
15508 + (((1.5707963267949)
15509 * (x1338
15510 .value))));
15511 sj4array[0] = IKsin(
15512 j4array
15513 [0]);
15514 cj4array[0] = IKcos(
15515 j4array
15516 [0]);
15517 if (j4array
15518 [0]
15519 > IKPI)
15520 {
15521 j4array[0]
15522 -= IK2PI;
15523 }
15524 else if (
15525 j4array
15526 [0]
15527 < -IKPI)
15528 {
15529 j4array[0]
15530 += IK2PI;
15531 }
15532 j4valid[0]
15533 = true;
15534 for (int ij4
15535 = 0;
15536 ij4
15537 < 1;
15538 ++ij4)
15539 {
15540 if (!j4valid
15541 [ij4])
15542 {
15543 continue;
15544 }
15545 _ij4[0]
15546 = ij4;
15547 _ij4[1]
15548 = -1;
15549 for (
15550 int iij4
15551 = ij4
15552 + 1;
15553 iij4
15554 < 1;
15555 ++iij4)
15556 {
15557 if (j4valid
15558 [iij4]
15559 && IKabs(
15560 cj4array
15561 [ij4]
15562 - cj4array
15563 [iij4])
15564 < IKFAST_SOLUTION_THRESH
15565 && IKabs(
15566 sj4array
15567 [ij4]
15568 - sj4array
15569 [iij4])
15570 < IKFAST_SOLUTION_THRESH)
15571 {
15572 j4valid
15573 [iij4]
15574 = false;
15575 _ij4[1]
15576 = iij4;
15577 break;
15578 }
15579 }
15580 j4 = j4array
15581 [ij4];
15582 cj4 = cj4array
15583 [ij4];
15584 sj4 = sj4array
15585 [ij4];
15586 {
15587 IkReal evalcond
15588 [4];
15589 IkReal
15590 x1339
15591 = IKsin(
15592 j4);
15593 IkReal
15594 x1340
15595 = (px
15596 * x1339);
15597 IkReal
15598 x1341
15599 = IKcos(
15600 j4);
15601 IkReal
15602 x1342
15603 = (py
15604 * x1341);
15605 IkReal
15606 x1343
15607 = (px
15608 * x1341);
15609 IkReal
15610 x1344
15611 = (py
15612 * x1339);
15613 IkReal
15614 x1345
15615 = ((((-1.0)
15616 * x1343))
15617 + (((-1.0)
15618 * x1344)));
15619 evalcond
15620 [0]
15621 = ((1.51009803921569)
15622 + (((-1.0)
15623 * (3.92156862745098)
15624 * pp))
15625 + (((-1.0)
15626 * x1342))
15627 + x1340
15628 + (((1.32323529411765)
15629 * cj9)));
15630 evalcond
15631 [1]
15632 = ((-0.55)
15633 + (((-1.0)
15634 * (0.3)
15635 * cj9))
15636 + (((-1.0)
15637 * (0.045)
15638 * sj9))
15639 + x1345);
15640 evalcond
15641 [2]
15642 = ((-0.316735294117647)
15643 + (((-1.0)
15644 * (0.588235294117647)
15645 * pp))
15646 + (((-1.0)
15647 * (0.108264705882353)
15648 * cj9))
15649 + x1345);
15650 evalcond
15651 [3]
15652 = ((-0.2125)
15653 + (((-1.1)
15654 * x1344))
15655 + (((-1.1)
15656 * x1343))
15657 + (((0.09)
15658 * x1340))
15659 + (((-0.09)
15660 * x1342))
15661 + (((-1.0)
15662 * (1.0)
15663 * pp)));
15664 if (IKabs(
15665 evalcond
15666 [0])
15667 > IKFAST_EVALCOND_THRESH
15668 || IKabs(
15669 evalcond
15670 [1])
15671 > IKFAST_EVALCOND_THRESH
15672 || IKabs(
15673 evalcond
15674 [2])
15675 > IKFAST_EVALCOND_THRESH
15676 || IKabs(
15677 evalcond
15678 [3])
15679 > IKFAST_EVALCOND_THRESH)
15680 {
15681 continue;
15682 }
15683 }
15684
15685 rotationfunction0(
15686 solutions);
15687 }
15688 }
15689 }
15690 }
15691 }
15692 } while (0);
15693 if (bgotonextstatement)
15694 {
15695 bool
15696 bgotonextstatement
15697 = true;
15698 do
15699 {
15700 IkReal x1346
15701 = (cj6 * pz);
15702 IkReal x1347
15703 = ((1.32323529411765)
15704 * cj9);
15705 IkReal x1348
15706 = ((3.92156862745098)
15707 * pp);
15708 evalcond[0]
15709 = ((IKabs(py))
15710 + (IKabs(
15711 px)));
15712 evalcond[1]
15713 = ((-0.55)
15714 + (((-1.0)
15715 * (0.3)
15716 * cj9))
15717 + (((-1.0)
15718 * (0.045)
15719 * sj9))
15720 + x1346);
15721 evalcond[2]
15722 = ((1.51009803921569)
15723 + (((-1.0)
15724 * x1348))
15725 + x1347);
15726 evalcond[3]
15727 = (pz * sj6);
15728 evalcond[4]
15729 = ((-1.51009803921569)
15730 + (((-1.0)
15731 * x1347))
15732 + x1348);
15733 evalcond[5]
15734 = ((((0.108264705882353)
15735 * cj9
15736 * sj6))
15737 + (((0.588235294117647)
15738 * pp
15739 * sj6))
15740 + (((0.316735294117647)
15741 * sj6)));
15742 evalcond[6]
15743 = ((-0.2125)
15744 + (((1.1)
15745 * x1346))
15746 + (((-1.0)
15747 * (1.0)
15748 * pp)));
15749 if (IKabs(evalcond
15750 [0])
15751 < 0.0000010000000000
15752 && IKabs(
15753 evalcond
15754 [1])
15755 < 0.0000010000000000
15756 && IKabs(
15757 evalcond
15758 [2])
15759 < 0.0000010000000000
15760 && IKabs(
15761 evalcond
15762 [3])
15763 < 0.0000010000000000
15764 && IKabs(
15765 evalcond
15766 [4])
15767 < 0.0000010000000000
15768 && IKabs(
15769 evalcond
15770 [5])
15771 < 0.0000010000000000
15772 && IKabs(
15773 evalcond
15774 [6])
15775 < 0.0000010000000000)
15776 {
15777 bgotonextstatement
15778 = false;
15779 {
15780 IkReal j4array
15781 [4],
15782 cj4array
15783 [4],
15784 sj4array
15785 [4];
15786 bool
15787 j4valid[4]
15788 = {false};
15789 _nj4 = 4;
15790 j4array[0]
15791 = 0;
15792 sj4array[0]
15793 = IKsin(
15794 j4array
15795 [0]);
15796 cj4array[0]
15797 = IKcos(
15798 j4array
15799 [0]);
15800 j4array[1]
15801 = 1.5707963267949;
15802 sj4array[1]
15803 = IKsin(
15804 j4array
15805 [1]);
15806 cj4array[1]
15807 = IKcos(
15808 j4array
15809 [1]);
15810 j4array[2]
15811 = 3.14159265358979;
15812 sj4array[2]
15813 = IKsin(
15814 j4array
15815 [2]);
15816 cj4array[2]
15817 = IKcos(
15818 j4array
15819 [2]);
15820 j4array[3]
15821 = -1.5707963267949;
15822 sj4array[3]
15823 = IKsin(
15824 j4array
15825 [3]);
15826 cj4array[3]
15827 = IKcos(
15828 j4array
15829 [3]);
15830 if (j4array[0]
15831 > IKPI)
15832 {
15833 j4array[0]
15834 -= IK2PI;
15835 }
15836 else if (
15837 j4array[0]
15838 < -IKPI)
15839 {
15840 j4array[0]
15841 += IK2PI;
15842 }
15843 j4valid[0]
15844 = true;
15845 if (j4array[1]
15846 > IKPI)
15847 {
15848 j4array[1]
15849 -= IK2PI;
15850 }
15851 else if (
15852 j4array[1]
15853 < -IKPI)
15854 {
15855 j4array[1]
15856 += IK2PI;
15857 }
15858 j4valid[1]
15859 = true;
15860 if (j4array[2]
15861 > IKPI)
15862 {
15863 j4array[2]
15864 -= IK2PI;
15865 }
15866 else if (
15867 j4array[2]
15868 < -IKPI)
15869 {
15870 j4array[2]
15871 += IK2PI;
15872 }
15873 j4valid[2]
15874 = true;
15875 if (j4array[3]
15876 > IKPI)
15877 {
15878 j4array[3]
15879 -= IK2PI;
15880 }
15881 else if (
15882 j4array[3]
15883 < -IKPI)
15884 {
15885 j4array[3]
15886 += IK2PI;
15887 }
15888 j4valid[3]
15889 = true;
15890 for (int ij4
15891 = 0;
15892 ij4 < 4;
15893 ++ij4)
15894 {
15895 if (!j4valid
15896 [ij4])
15897 {
15898 continue;
15899 }
15900 _ij4[0]
15901 = ij4;
15902 _ij4[1]
15903 = -1;
15904 for (
15905 int iij4
15906 = ij4
15907 + 1;
15908 iij4
15909 < 4;
15910 ++iij4)
15911 {
15912 if (j4valid
15913 [iij4]
15914 && IKabs(
15915 cj4array
15916 [ij4]
15917 - cj4array
15918 [iij4])
15919 < IKFAST_SOLUTION_THRESH
15920 && IKabs(
15921 sj4array
15922 [ij4]
15923 - sj4array
15924 [iij4])
15925 < IKFAST_SOLUTION_THRESH)
15926 {
15927 j4valid
15928 [iij4]
15929 = false;
15930 _ij4[1]
15931 = iij4;
15932 break;
15933 }
15934 }
15935 j4 = j4array
15936 [ij4];
15937 cj4 = cj4array
15938 [ij4];
15939 sj4 = sj4array
15940 [ij4];
15941
15942 rotationfunction0(
15943 solutions);
15944 }
15945 }
15946 }
15947 } while (0);
15948 if (bgotonextstatement)
15949 {
15950 bool
15951 bgotonextstatement
15952 = true;
15953 do
15954 {
15955 if (1)
15956 {
15957 bgotonextstatement
15958 = false;
15959 continue; // branch miss [j4]
15960 }
15961 } while (0);
15962 if (bgotonextstatement)
15963 {
15964 }
15965 }
15966 }
15967 }
15968 }
15969 }
15970 else
15971 {
15972 {
15973 IkReal j4array[1],
15974 cj4array[1],
15975 sj4array[1];
15976 bool j4valid[1]
15977 = {false};
15978 _nj4 = 1;
15979 IkReal x1349
15980 = ((1.32323529411765)
15981 * cj9);
15982 IkReal x1350
15983 = ((3.92156862745098)
15984 * pp);
15985 IkReal x1351
15986 = ((0.316735294117647)
15987 * sj6);
15988 IkReal x1352
15989 = ((0.108264705882353)
15990 * cj9 * sj6);
15991 IkReal x1353
15992 = ((0.588235294117647)
15993 * pp * sj6);
15994 CheckValue<IkReal> x1354 = IKatan2WithCheck(
15995 IkReal((
15996 ((py * x1353))
15997 + (((-1.0)
15998 * (1.51009803921569)
15999 * px))
16000 + (((-1.0) * px
16001 * x1349))
16002 + ((py * x1352))
16003 + ((px * x1350))
16004 + ((py
16005 * x1351)))),
16006 (((px * x1351))
16007 + ((px * x1353))
16008 + (((1.51009803921569)
16009 * py))
16010 + ((py * x1349))
16011 + ((px * x1352))
16012 + (((-1.0) * py
16013 * x1350))),
16014 IKFAST_ATAN2_MAGTHRESH);
16015 if (!x1354.valid)
16016 {
16017 continue;
16018 }
16019 CheckValue<IkReal> x1355
16020 = IKPowWithIntegerCheck(
16021 IKsign((
16022 pp
16023 + (((-1.0)
16024 * (1.0)
16025 * (pz
16026 * pz))))),
16027 -1);
16028 if (!x1355.valid)
16029 {
16030 continue;
16031 }
16032 j4array[0]
16033 = ((-1.5707963267949)
16034 + (x1354.value)
16035 + (((1.5707963267949)
16036 * (x1355
16037 .value))));
16038 sj4array[0]
16039 = IKsin(j4array[0]);
16040 cj4array[0]
16041 = IKcos(j4array[0]);
16042 if (j4array[0] > IKPI)
16043 {
16044 j4array[0] -= IK2PI;
16045 }
16046 else if (
16047 j4array[0] < -IKPI)
16048 {
16049 j4array[0] += IK2PI;
16050 }
16051 j4valid[0] = true;
16052 for (int ij4 = 0;
16053 ij4 < 1;
16054 ++ij4)
16055 {
16056 if (!j4valid[ij4])
16057 {
16058 continue;
16059 }
16060 _ij4[0] = ij4;
16061 _ij4[1] = -1;
16062 for (int iij4
16063 = ij4 + 1;
16064 iij4 < 1;
16065 ++iij4)
16066 {
16067 if (j4valid[iij4]
16068 && IKabs(
16069 cj4array
16070 [ij4]
16071 - cj4array
16072 [iij4])
16073 < IKFAST_SOLUTION_THRESH
16074 && IKabs(
16075 sj4array
16076 [ij4]
16077 - sj4array
16078 [iij4])
16079 < IKFAST_SOLUTION_THRESH)
16080 {
16081 j4valid[iij4]
16082 = false;
16083 _ij4[1] = iij4;
16084 break;
16085 }
16086 }
16087 j4 = j4array[ij4];
16088 cj4 = cj4array[ij4];
16089 sj4 = sj4array[ij4];
16090 {
16091 IkReal evalcond[5];
16092 IkReal x1356
16093 = IKcos(j4);
16094 IkReal x1357
16095 = ((1.0)
16096 * x1356);
16097 IkReal x1358
16098 = (px * x1357);
16099 IkReal x1359
16100 = IKsin(j4);
16101 IkReal x1360
16102 = (py * x1359);
16103 IkReal x1361
16104 = ((1.0)
16105 * x1360);
16106 IkReal x1362
16107 = (px * x1359);
16108 IkReal x1363
16109 = (cj6 * pz);
16110 IkReal x1364
16111 = (px * sj6
16112 * x1356);
16113 IkReal x1365
16114 = (sj6 * x1360);
16115 evalcond[0]
16116 = ((((-1.0)
16117 * cj6
16118 * x1361))
16119 + (((-1.0)
16120 * cj6
16121 * x1358))
16122 + ((pz
16123 * sj6)));
16124 evalcond[1]
16125 = ((1.51009803921569)
16126 + (((-1.0)
16127 * py
16128 * x1357))
16129 + (((-1.0)
16130 * (3.92156862745098)
16131 * pp))
16132 + x1362
16133 + (((1.32323529411765)
16134 * cj9)));
16135 evalcond[2]
16136 = ((-0.55)
16137 + (((-1.0)
16138 * (0.3)
16139 * cj9))
16140 + x1363
16141 + x1365
16142 + x1364
16143 + (((-1.0)
16144 * (0.045)
16145 * sj9)));
16146 evalcond[3]
16147 = ((((0.108264705882353)
16148 * cj9
16149 * sj6))
16150 + (((0.588235294117647)
16151 * pp
16152 * sj6))
16153 + (((-1.0)
16154 * x1361))
16155 + (((-1.0)
16156 * x1358))
16157 + (((0.316735294117647)
16158 * sj6)));
16159 evalcond[4]
16160 = ((-0.2125)
16161 + (((1.1)
16162 * x1364))
16163 + (((1.1)
16164 * x1365))
16165 + (((-0.09)
16166 * py
16167 * x1356))
16168 + (((-1.0)
16169 * (1.0)
16170 * pp))
16171 + (((1.1)
16172 * x1363))
16173 + (((0.09)
16174 * x1362)));
16175 if (IKabs(
16176 evalcond[0])
16177 > IKFAST_EVALCOND_THRESH
16178 || IKabs(
16179 evalcond
16180 [1])
16181 > IKFAST_EVALCOND_THRESH
16182 || IKabs(
16183 evalcond
16184 [2])
16185 > IKFAST_EVALCOND_THRESH
16186 || IKabs(
16187 evalcond
16188 [3])
16189 > IKFAST_EVALCOND_THRESH
16190 || IKabs(
16191 evalcond
16192 [4])
16193 > IKFAST_EVALCOND_THRESH)
16194 {
16195 continue;
16196 }
16197 }
16198
16199 rotationfunction0(
16200 solutions);
16201 }
16202 }
16203 }
16204 }
16205 }
16206 else
16207 {
16208 {
16209 IkReal j4array[1],
16210 cj4array[1],
16211 sj4array[1];
16212 bool j4valid[1] = {false};
16213 _nj4 = 1;
16214 IkReal x1366 = (cj6 * pp);
16215 IkReal x1367
16216 = ((0.2125) * cj6);
16217 IkReal x1368 = ((1.1) * pz);
16218 IkReal x1369
16219 = ((0.09) * pz * sj6);
16220 CheckValue<IkReal> x1370
16221 = IKatan2WithCheck(
16222 IkReal((
16223 ((px * x1368))
16224 + (((-1.0) * px
16225 * x1367))
16226 + (((-1.0) * px
16227 * x1366))
16228 + (((-1.0) * py
16229 * x1369)))),
16230 ((((-1.0) * px
16231 * x1369))
16232 + (((-1.0) * py
16233 * x1368))
16234 + ((py * x1367))
16235 + ((py * x1366))),
16236 IKFAST_ATAN2_MAGTHRESH);
16237 if (!x1370.valid)
16238 {
16239 continue;
16240 }
16241 CheckValue<IkReal> x1371
16242 = IKPowWithIntegerCheck(
16243 IKsign((
16244 (((0.09) * cj6
16245 * (pz * pz)))
16246 + (((-0.09)
16247 * x1366)))),
16248 -1);
16249 if (!x1371.valid)
16250 {
16251 continue;
16252 }
16253 j4array[0]
16254 = ((-1.5707963267949)
16255 + (x1370.value)
16256 + (((1.5707963267949)
16257 * (x1371
16258 .value))));
16259 sj4array[0]
16260 = IKsin(j4array[0]);
16261 cj4array[0]
16262 = IKcos(j4array[0]);
16263 if (j4array[0] > IKPI)
16264 {
16265 j4array[0] -= IK2PI;
16266 }
16267 else if (j4array[0] < -IKPI)
16268 {
16269 j4array[0] += IK2PI;
16270 }
16271 j4valid[0] = true;
16272 for (int ij4 = 0; ij4 < 1;
16273 ++ij4)
16274 {
16275 if (!j4valid[ij4])
16276 {
16277 continue;
16278 }
16279 _ij4[0] = ij4;
16280 _ij4[1] = -1;
16281 for (int iij4 = ij4 + 1;
16282 iij4 < 1;
16283 ++iij4)
16284 {
16285 if (j4valid[iij4]
16286 && IKabs(
16287 cj4array[ij4]
16288 - cj4array
16289 [iij4])
16290 < IKFAST_SOLUTION_THRESH
16291 && IKabs(
16292 sj4array[ij4]
16293 - sj4array
16294 [iij4])
16295 < IKFAST_SOLUTION_THRESH)
16296 {
16297 j4valid[iij4] = false;
16298 _ij4[1] = iij4;
16299 break;
16300 }
16301 }
16302 j4 = j4array[ij4];
16303 cj4 = cj4array[ij4];
16304 sj4 = sj4array[ij4];
16305 {
16306 IkReal evalcond[5];
16307 IkReal x1372
16308 = IKcos(j4);
16309 IkReal x1373
16310 = ((1.0) * x1372);
16311 IkReal x1374
16312 = (px * x1373);
16313 IkReal x1375
16314 = IKsin(j4);
16315 IkReal x1376
16316 = (py * x1375);
16317 IkReal x1377
16318 = ((1.0) * x1376);
16319 IkReal x1378
16320 = (px * x1375);
16321 IkReal x1379
16322 = (cj6 * pz);
16323 IkReal x1380
16324 = (px * sj6
16325 * x1372);
16326 IkReal x1381
16327 = (sj6 * x1376);
16328 evalcond[0]
16329 = ((((-1.0) * cj6
16330 * x1374))
16331 + ((pz * sj6))
16332 + (((-1.0) * cj6
16333 * x1377)));
16334 evalcond[1]
16335 = ((1.51009803921569)
16336 + x1378
16337 + (((-1.0)
16338 * (3.92156862745098)
16339 * pp))
16340 + (((-1.0) * py
16341 * x1373))
16342 + (((1.32323529411765)
16343 * cj9)));
16344 evalcond[2]
16345 = ((-0.55) + x1379
16346 + x1380 + x1381
16347 + (((-1.0)
16348 * (0.3)
16349 * cj9))
16350 + (((-1.0)
16351 * (0.045)
16352 * sj9)));
16353 evalcond[3]
16354 = ((((0.108264705882353)
16355 * cj9 * sj6))
16356 + (((-1.0)
16357 * x1374))
16358 + (((0.588235294117647)
16359 * pp * sj6))
16360 + (((0.316735294117647)
16361 * sj6))
16362 + (((-1.0)
16363 * x1377)));
16364 evalcond[4]
16365 = ((-0.2125)
16366 + (((-0.09) * py
16367 * x1372))
16368 + (((0.09)
16369 * x1378))
16370 + (((1.1)
16371 * x1379))
16372 + (((1.1)
16373 * x1381))
16374 + (((1.1)
16375 * x1380))
16376 + (((-1.0)
16377 * (1.0)
16378 * pp)));
16379 if (IKabs(evalcond[0])
16380 > IKFAST_EVALCOND_THRESH
16381 || IKabs(
16382 evalcond[1])
16383 > IKFAST_EVALCOND_THRESH
16384 || IKabs(
16385 evalcond[2])
16386 > IKFAST_EVALCOND_THRESH
16387 || IKabs(
16388 evalcond[3])
16389 > IKFAST_EVALCOND_THRESH
16390 || IKabs(
16391 evalcond[4])
16392 > IKFAST_EVALCOND_THRESH)
16393 {
16394 continue;
16395 }
16396 }
16397
16398 rotationfunction0(
16399 solutions);
16400 }
16401 }
16402 }
16403 }
16404 }
16405 else
16406 {
16407 {
16408 IkReal j4array[1], cj4array[1],
16409 sj4array[1];
16410 bool j4valid[1] = {false};
16411 _nj4 = 1;
16412 IkReal x1382
16413 = ((1.51009803921569)
16414 * cj6);
16415 IkReal x1383 = (pz * sj6);
16416 IkReal x1384
16417 = ((1.32323529411765) * cj6
16418 * cj9);
16419 IkReal x1385
16420 = ((3.92156862745098) * cj6
16421 * pp);
16422 CheckValue<IkReal> x1386
16423 = IKPowWithIntegerCheck(
16424 IKsign(
16425 ((((-1.0) * (1.0)
16426 * cj6
16427 * (pz * pz)))
16428 + ((cj6 * pp)))),
16429 -1);
16430 if (!x1386.valid)
16431 {
16432 continue;
16433 }
16434 CheckValue<IkReal> x1387
16435 = IKatan2WithCheck(
16436 IkReal(
16437 ((((-1.0) * px
16438 * x1384))
16439 + (((-1.0) * px
16440 * x1382))
16441 + ((py * x1383))
16442 + ((px * x1385)))),
16443 ((((-1.0) * py * x1385))
16444 + ((py * x1384))
16445 + ((px * x1383))
16446 + ((py * x1382))),
16447 IKFAST_ATAN2_MAGTHRESH);
16448 if (!x1387.valid)
16449 {
16450 continue;
16451 }
16452 j4array[0]
16453 = ((-1.5707963267949)
16454 + (((1.5707963267949)
16455 * (x1386.value)))
16456 + (x1387.value));
16457 sj4array[0] = IKsin(j4array[0]);
16458 cj4array[0] = IKcos(j4array[0]);
16459 if (j4array[0] > IKPI)
16460 {
16461 j4array[0] -= IK2PI;
16462 }
16463 else if (j4array[0] < -IKPI)
16464 {
16465 j4array[0] += IK2PI;
16466 }
16467 j4valid[0] = true;
16468 for (int ij4 = 0; ij4 < 1;
16469 ++ij4)
16470 {
16471 if (!j4valid[ij4])
16472 {
16473 continue;
16474 }
16475 _ij4[0] = ij4;
16476 _ij4[1] = -1;
16477 for (int iij4 = ij4 + 1;
16478 iij4 < 1;
16479 ++iij4)
16480 {
16481 if (j4valid[iij4]
16482 && IKabs(
16483 cj4array[ij4]
16484 - cj4array[iij4])
16485 < IKFAST_SOLUTION_THRESH
16486 && IKabs(
16487 sj4array[ij4]
16488 - sj4array[iij4])
16489 < IKFAST_SOLUTION_THRESH)
16490 {
16491 j4valid[iij4] = false;
16492 _ij4[1] = iij4;
16493 break;
16494 }
16495 }
16496 j4 = j4array[ij4];
16497 cj4 = cj4array[ij4];
16498 sj4 = sj4array[ij4];
16499 {
16500 IkReal evalcond[5];
16501 IkReal x1388 = IKcos(j4);
16502 IkReal x1389
16503 = ((1.0) * x1388);
16504 IkReal x1390 = (px * x1389);
16505 IkReal x1391 = IKsin(j4);
16506 IkReal x1392 = (py * x1391);
16507 IkReal x1393
16508 = ((1.0) * x1392);
16509 IkReal x1394 = (px * x1391);
16510 IkReal x1395 = (cj6 * pz);
16511 IkReal x1396
16512 = (px * sj6 * x1388);
16513 IkReal x1397
16514 = (sj6 * x1392);
16515 evalcond[0]
16516 = ((((-1.0) * cj6
16517 * x1393))
16518 + ((pz * sj6))
16519 + (((-1.0) * cj6
16520 * x1390)));
16521 evalcond[1]
16522 = ((1.51009803921569)
16523 + (((-1.0) * py
16524 * x1389))
16525 + (((-1.0)
16526 * (3.92156862745098)
16527 * pp))
16528 + x1394
16529 + (((1.32323529411765)
16530 * cj9)));
16531 evalcond[2]
16532 = ((-0.55)
16533 + (((-1.0) * (0.3)
16534 * cj9))
16535 + x1397 + x1396
16536 + x1395
16537 + (((-1.0) * (0.045)
16538 * sj9)));
16539 evalcond[3]
16540 = ((((-1.0) * x1393))
16541 + (((0.108264705882353)
16542 * cj9 * sj6))
16543 + (((0.588235294117647)
16544 * pp * sj6))
16545 + (((0.316735294117647)
16546 * sj6))
16547 + (((-1.0)
16548 * x1390)));
16549 evalcond[4]
16550 = ((-0.2125)
16551 + (((1.1) * x1395))
16552 + (((0.09) * x1394))
16553 + (((-0.09) * py
16554 * x1388))
16555 + (((1.1) * x1396))
16556 + (((1.1) * x1397))
16557 + (((-1.0) * (1.0)
16558 * pp)));
16559 if (IKabs(evalcond[0])
16560 > IKFAST_EVALCOND_THRESH
16561 || IKabs(evalcond[1])
16562 > IKFAST_EVALCOND_THRESH
16563 || IKabs(evalcond[2])
16564 > IKFAST_EVALCOND_THRESH
16565 || IKabs(evalcond[3])
16566 > IKFAST_EVALCOND_THRESH
16567 || IKabs(evalcond[4])
16568 > IKFAST_EVALCOND_THRESH)
16569 {
16570 continue;
16571 }
16572 }
16573
16574 rotationfunction0(solutions);
16575 }
16576 }
16577 }
16578 }
16579 }
16580 } while (0);
16581 if (bgotonextstatement)
16582 {
16583 bool bgotonextstatement = true;
16584 do
16585 {
16586 IkReal x1398
16587 = ((-0.55) + pz
16588 + (((-1.0) * (0.3) * cj9))
16589 + (((-1.0) * (0.045) * sj9)));
16590 evalcond[0]
16591 = ((-3.14159265358979)
16592 + (IKfmod(
16593 ((3.14159265358979)
16594 + (IKabs(j6))),
16595 6.28318530717959)));
16596 evalcond[1]
16597 = ((0.39655) + (((0.0765) * sj9))
16598 + (((0.32595) * cj9))
16599 + (((-1.0) * (1.0) * pp)));
16600 evalcond[2] = x1398;
16601 evalcond[3] = x1398;
16602 if (IKabs(evalcond[0])
16603 < 0.0000010000000000
16604 && IKabs(evalcond[1])
16605 < 0.0000010000000000
16606 && IKabs(evalcond[2])
16607 < 0.0000010000000000
16608 && IKabs(evalcond[3])
16609 < 0.0000010000000000)
16610 {
16611 bgotonextstatement = false;
16612 {
16613 IkReal j4eval[3];
16614 sj6 = 0;
16615 cj6 = 1.0;
16616 j6 = 0;
16617 IkReal x1399
16618 = (pp
16619 + (((-1.0) * (1.0)
16620 * (pz * pz))));
16621 IkReal x1400
16622 = ((1.51009803921569) * cj8);
16623 IkReal x1401
16624 = ((1.51009803921569) * sj8);
16625 IkReal x1402
16626 = ((1.32323529411765) * cj8
16627 * cj9);
16628 IkReal x1403
16629 = ((3.92156862745098) * cj8
16630 * pp);
16631 IkReal x1404
16632 = ((1.32323529411765) * cj9
16633 * sj8);
16634 IkReal x1405
16635 = ((3.92156862745098) * pp
16636 * sj8);
16637 j4eval[0] = x1399;
16638 j4eval[1]
16639 = ((IKabs(
16640 (((py * x1403))
16641 + ((px * x1401))
16642 + (((-1.0) * py
16643 * x1402))
16644 + ((px * x1404))
16645 + (((-1.0) * px
16646 * x1405))
16647 + (((-1.0) * py
16648 * x1400)))))
16649 + (IKabs((
16650 (((-1.0) * px * x1400))
16651 + (((-1.0) * py
16652 * x1404))
16653 + (((-1.0) * px
16654 * x1402))
16655 + (((-1.0) * py
16656 * x1401))
16657 + ((py * x1405))
16658 + ((px * x1403))))));
16659 j4eval[2] = IKsign(x1399);
16660 if (IKabs(j4eval[0])
16661 < 0.0000010000000000
16662 || IKabs(j4eval[1])
16663 < 0.0000010000000000
16664 || IKabs(j4eval[2])
16665 < 0.0000010000000000)
16666 {
16667 {
16668 IkReal j4eval[2];
16669 sj6 = 0;
16670 cj6 = 1.0;
16671 j6 = 0;
16672 IkReal x1406
16673 = (((cj8 * (pz * pz)))
16674 + (((-1.0) * (1.0)
16675 * cj8 * pp)));
16676 j4eval[0] = x1406;
16677 j4eval[1] = IKsign(x1406);
16678 if (IKabs(j4eval[0])
16679 < 0.0000010000000000
16680 || IKabs(j4eval[1])
16681 < 0.0000010000000000)
16682 {
16683 {
16684 IkReal j4eval[2];
16685 sj6 = 0;
16686 cj6 = 1.0;
16687 j6 = 0;
16688 IkReal x1407
16689 = ((((-1.0) * (1.0)
16690 * sj8
16691 * (pz * pz)))
16692 + ((pp * sj8)));
16693 j4eval[0] = x1407;
16694 j4eval[1] = IKsign(x1407);
16695 if (IKabs(j4eval[0])
16696 < 0.0000010000000000
16697 || IKabs(j4eval[1])
16698 < 0.0000010000000000)
16699 {
16700 {
16701 IkReal evalcond[6];
16702 bool
16703 bgotonextstatement
16704 = true;
16705 do
16706 {
16707 evalcond[0]
16708 = ((-3.14159265358979)
16709 + (IKfmod(
16710 ((3.14159265358979)
16711 + (IKabs(
16712 j8))),
16713 6.28318530717959)));
16714 if (IKabs(
16715 evalcond[0])
16716 < 0.0000010000000000)
16717 {
16718 bgotonextstatement
16719 = false;
16720 {
16721 IkReal
16722 j4eval[3];
16723 sj6 = 0;
16724 cj6 = 1.0;
16725 j6 = 0;
16726 sj8 = 0;
16727 cj8 = 1.0;
16728 j8 = 0;
16729 IkReal x1408
16730 = (pp
16731 + (((-1.0)
16732 * (1.0)
16733 * (pz
16734 * pz))));
16735 IkReal x1409
16736 = ((13497.0)
16737 * cj9);
16738 IkReal x1410
16739 = ((40000.0)
16740 * pp);
16741 j4eval[0]
16742 = x1408;
16743 j4eval[1]
16744 = ((IKabs((
16745 ((py
16746 * x1410))
16747 + (((-1.0)
16748 * (15403.0)
16749 * py))
16750 + (((-1.0)
16751 * py
16752 * x1409)))))
16753 + (IKabs((
16754 (((-1.0)
16755 * px
16756 * x1409))
16757 + (((-1.0)
16758 * (15403.0)
16759 * px))
16760 + ((px
16761 * x1410))))));
16762 j4eval[2]
16763 = IKsign(
16764 x1408);
16765 if (IKabs(
16766 j4eval
16767 [0])
16768 < 0.0000010000000000
16769 || IKabs(
16770 j4eval
16771 [1])
16772 < 0.0000010000000000
16773 || IKabs(
16774 j4eval
16775 [2])
16776 < 0.0000010000000000)
16777 {
16778 {
16779 IkReal
16780 j4eval
16781 [3];
16782 sj6 = 0;
16783 cj6 = 1.0;
16784 j6 = 0;
16785 sj8 = 0;
16786 cj8 = 1.0;
16787 j8 = 0;
16788 IkReal x1411
16789 = pz
16790 * pz;
16791 IkReal x1412
16792 = ((80.0)
16793 * pp);
16794 IkReal x1413
16795 = ((88.0)
16796 * pz);
16797 j4eval[0]
16798 = ((((-1.0)
16799 * x1411))
16800 + pp);
16801 j4eval[1]
16802 = ((IKabs((
16803 (((-1.0)
16804 * px
16805 * x1413))
16806 + ((px
16807 * x1412))
16808 + (((17.0)
16809 * px)))))
16810 + (IKabs((
16811 ((py
16812 * x1412))
16813 + (((17.0)
16814 * py))
16815 + (((-1.0)
16816 * py
16817 * x1413))))));
16818 j4eval[2] = IKsign((
16819 (((9.0)
16820 * pp))
16821 + (((-9.0)
16822 * x1411))));
16823 if (IKabs(
16824 j4eval
16825 [0])
16826 < 0.0000010000000000
16827 || IKabs(
16828 j4eval
16829 [1])
16830 < 0.0000010000000000
16831 || IKabs(
16832 j4eval
16833 [2])
16834 < 0.0000010000000000)
16835 {
16836 {
16837 IkReal evalcond
16838 [5];
16839 bool
16840 bgotonextstatement
16841 = true;
16842 do
16843 {
16844 IkReal
16845 x1414
16846 = ((-1.51009803921569)
16847 + (((-1.0)
16848 * (1.32323529411765)
16849 * cj9))
16850 + (((3.92156862745098)
16851 * pp)));
16852 evalcond
16853 [0]
16854 = ((IKabs(
16855 py))
16856 + (IKabs(
16857 px)));
16858 evalcond
16859 [1]
16860 = 0;
16861 evalcond
16862 [2]
16863 = x1414;
16864 evalcond
16865 [3]
16866 = x1414;
16867 evalcond
16868 [4]
16869 = ((-0.2125)
16870 + (((1.1)
16871 * pz))
16872 + (((-1.0)
16873 * (1.0)
16874 * pp)));
16875 if (IKabs(
16876 evalcond
16877 [0])
16878 < 0.0000010000000000
16879 && IKabs(
16880 evalcond
16881 [1])
16882 < 0.0000010000000000
16883 && IKabs(
16884 evalcond
16885 [2])
16886 < 0.0000010000000000
16887 && IKabs(
16888 evalcond
16889 [3])
16890 < 0.0000010000000000
16891 && IKabs(
16892 evalcond
16893 [4])
16894 < 0.0000010000000000)
16895 {
16896 bgotonextstatement
16897 = false;
16898 {
16899 IkReal j4array
16900 [4],
16901 cj4array
16902 [4],
16903 sj4array
16904 [4];
16905 bool j4valid
16906 [4]
16907 = {false};
16908 _nj4
16909 = 4;
16910 j4array
16911 [0]
16912 = 0;
16913 sj4array
16914 [0]
16915 = IKsin(
16916 j4array
16917 [0]);
16918 cj4array
16919 [0]
16920 = IKcos(
16921 j4array
16922 [0]);
16923 j4array
16924 [1]
16925 = 1.5707963267949;
16926 sj4array
16927 [1]
16928 = IKsin(
16929 j4array
16930 [1]);
16931 cj4array
16932 [1]
16933 = IKcos(
16934 j4array
16935 [1]);
16936 j4array
16937 [2]
16938 = 3.14159265358979;
16939 sj4array
16940 [2]
16941 = IKsin(
16942 j4array
16943 [2]);
16944 cj4array
16945 [2]
16946 = IKcos(
16947 j4array
16948 [2]);
16949 j4array
16950 [3]
16951 = -1.5707963267949;
16952 sj4array
16953 [3]
16954 = IKsin(
16955 j4array
16956 [3]);
16957 cj4array
16958 [3]
16959 = IKcos(
16960 j4array
16961 [3]);
16962 if (j4array
16963 [0]
16964 > IKPI)
16965 {
16966 j4array
16967 [0]
16968 -= IK2PI;
16969 }
16970 else if (
16971 j4array
16972 [0]
16973 < -IKPI)
16974 {
16975 j4array
16976 [0]
16977 += IK2PI;
16978 }
16979 j4valid
16980 [0]
16981 = true;
16982 if (j4array
16983 [1]
16984 > IKPI)
16985 {
16986 j4array
16987 [1]
16988 -= IK2PI;
16989 }
16990 else if (
16991 j4array
16992 [1]
16993 < -IKPI)
16994 {
16995 j4array
16996 [1]
16997 += IK2PI;
16998 }
16999 j4valid
17000 [1]
17001 = true;
17002 if (j4array
17003 [2]
17004 > IKPI)
17005 {
17006 j4array
17007 [2]
17008 -= IK2PI;
17009 }
17010 else if (
17011 j4array
17012 [2]
17013 < -IKPI)
17014 {
17015 j4array
17016 [2]
17017 += IK2PI;
17018 }
17019 j4valid
17020 [2]
17021 = true;
17022 if (j4array
17023 [3]
17024 > IKPI)
17025 {
17026 j4array
17027 [3]
17028 -= IK2PI;
17029 }
17030 else if (
17031 j4array
17032 [3]
17033 < -IKPI)
17034 {
17035 j4array
17036 [3]
17037 += IK2PI;
17038 }
17039 j4valid
17040 [3]
17041 = true;
17042 for (
17043 int ij4
17044 = 0;
17045 ij4
17046 < 4;
17047 ++ij4)
17048 {
17049 if (!j4valid
17050 [ij4])
17051 {
17052 continue;
17053 }
17054 _ij4[0]
17055 = ij4;
17056 _ij4[1]
17057 = -1;
17058 for (
17059 int iij4
17060 = ij4
17061 + 1;
17062 iij4
17063 < 4;
17064 ++iij4)
17065 {
17066 if (j4valid
17067 [iij4]
17068 && IKabs(
17069 cj4array
17070 [ij4]
17071 - cj4array
17072 [iij4])
17073 < IKFAST_SOLUTION_THRESH
17074 && IKabs(
17075 sj4array
17076 [ij4]
17077 - sj4array
17078 [iij4])
17079 < IKFAST_SOLUTION_THRESH)
17080 {
17081 j4valid
17082 [iij4]
17083 = false;
17084 _ij4[1]
17085 = iij4;
17086 break;
17087 }
17088 }
17089 j4 = j4array
17090 [ij4];
17091 cj4 = cj4array
17092 [ij4];
17093 sj4 = sj4array
17094 [ij4];
17095
17096 rotationfunction0(
17097 solutions);
17098 }
17099 }
17100 }
17101 } while (
17102 0);
17103 if (bgotonextstatement)
17104 {
17105 bool
17106 bgotonextstatement
17107 = true;
17108 do
17109 {
17110 if (1)
17111 {
17112 bgotonextstatement
17113 = false;
17114 continue; // branch miss [j4]
17115 }
17116 } while (
17117 0);
17118 if (bgotonextstatement)
17119 {
17120 }
17121 }
17122 }
17123 }
17124 else
17125 {
17126 {
17127 IkReal j4array
17128 [1],
17129 cj4array
17130 [1],
17131 sj4array
17132 [1];
17133 bool j4valid
17134 [1]
17135 = {false};
17136 _nj4
17137 = 1;
17138 IkReal
17139 x1415
17140 = ((100.0)
17141 * pp);
17142 IkReal
17143 x1416
17144 = ((110.0)
17145 * pz);
17146 CheckValue<IkReal> x1417 = IKPowWithIntegerCheck(
17147 IKsign((
17148 (((-1.0)
17149 * (9.0)
17150 * (pz
17151 * pz)))
17152 + (((9.0)
17153 * pp)))),
17154 -1);
17155 if (!x1417
17156 .valid)
17157 {
17158 continue;
17159 }
17160 CheckValue<IkReal> x1418 = IKatan2WithCheck(
17161 IkReal((
17162 ((py
17163 * x1415))
17164 + (((21.25)
17165 * py))
17166 + (((-1.0)
17167 * py
17168 * x1416)))),
17169 (((px
17170 * x1415))
17171 + (((-1.0)
17172 * px
17173 * x1416))
17174 + (((21.25)
17175 * px))),
17176 IKFAST_ATAN2_MAGTHRESH);
17177 if (!x1418
17178 .valid)
17179 {
17180 continue;
17181 }
17182 j4array
17183 [0]
17184 = ((-1.5707963267949)
17185 + (((1.5707963267949)
17186 * (x1417
17187 .value)))
17188 + (x1418
17189 .value));
17190 sj4array
17191 [0]
17192 = IKsin(
17193 j4array
17194 [0]);
17195 cj4array
17196 [0]
17197 = IKcos(
17198 j4array
17199 [0]);
17200 if (j4array
17201 [0]
17202 > IKPI)
17203 {
17204 j4array
17205 [0]
17206 -= IK2PI;
17207 }
17208 else if (
17209 j4array
17210 [0]
17211 < -IKPI)
17212 {
17213 j4array
17214 [0]
17215 += IK2PI;
17216 }
17217 j4valid
17218 [0]
17219 = true;
17220 for (
17221 int ij4
17222 = 0;
17223 ij4
17224 < 1;
17225 ++ij4)
17226 {
17227 if (!j4valid
17228 [ij4])
17229 {
17230 continue;
17231 }
17232 _ij4[0]
17233 = ij4;
17234 _ij4[1]
17235 = -1;
17236 for (
17237 int iij4
17238 = ij4
17239 + 1;
17240 iij4
17241 < 1;
17242 ++iij4)
17243 {
17244 if (j4valid
17245 [iij4]
17246 && IKabs(
17247 cj4array
17248 [ij4]
17249 - cj4array
17250 [iij4])
17251 < IKFAST_SOLUTION_THRESH
17252 && IKabs(
17253 sj4array
17254 [ij4]
17255 - sj4array
17256 [iij4])
17257 < IKFAST_SOLUTION_THRESH)
17258 {
17259 j4valid
17260 [iij4]
17261 = false;
17262 _ij4[1]
17263 = iij4;
17264 break;
17265 }
17266 }
17267 j4 = j4array
17268 [ij4];
17269 cj4 = cj4array
17270 [ij4];
17271 sj4 = sj4array
17272 [ij4];
17273 {
17274 IkReal evalcond
17275 [3];
17276 IkReal
17277 x1419
17278 = IKsin(
17279 j4);
17280 IkReal
17281 x1420
17282 = IKcos(
17283 j4);
17284 IkReal
17285 x1421
17286 = (px
17287 * x1420);
17288 IkReal
17289 x1422
17290 = (py
17291 * x1419);
17292 evalcond
17293 [0]
17294 = ((((-1.0)
17295 * py
17296 * x1420))
17297 + ((px
17298 * x1419)));
17299 evalcond
17300 [1]
17301 = ((-1.51009803921569)
17302 + (((-1.0)
17303 * (1.32323529411765)
17304 * cj9))
17305 + (((-1.0)
17306 * x1421))
17307 + (((-1.0)
17308 * x1422))
17309 + (((3.92156862745098)
17310 * pp)));
17311 evalcond
17312 [2]
17313 = ((-0.2125)
17314 + (((1.1)
17315 * pz))
17316 + (((0.09)
17317 * x1421))
17318 + (((0.09)
17319 * x1422))
17320 + (((-1.0)
17321 * (1.0)
17322 * pp)));
17323 if (IKabs(
17324 evalcond
17325 [0])
17326 > IKFAST_EVALCOND_THRESH
17327 || IKabs(
17328 evalcond
17329 [1])
17330 > IKFAST_EVALCOND_THRESH
17331 || IKabs(
17332 evalcond
17333 [2])
17334 > IKFAST_EVALCOND_THRESH)
17335 {
17336 continue;
17337 }
17338 }
17339
17340 rotationfunction0(
17341 solutions);
17342 }
17343 }
17344 }
17345 }
17346 }
17347 else
17348 {
17349 {
17350 IkReal
17351 j4array
17352 [1],
17353 cj4array
17354 [1],
17355 sj4array
17356 [1];
17357 bool j4valid
17358 [1]
17359 = {false};
17360 _nj4 = 1;
17361 IkReal x1423
17362 = ((1.32323529411765)
17363 * cj9);
17364 IkReal x1424
17365 = ((3.92156862745098)
17366 * pp);
17367 CheckValue<IkReal> x1425 = IKatan2WithCheck(
17368 IkReal((
17369 ((py
17370 * x1424))
17371 + (((-1.0)
17372 * (1.51009803921569)
17373 * py))
17374 + (((-1.0)
17375 * py
17376 * x1423)))),
17377 ((((-1.0)
17378 * px
17379 * x1423))
17380 + (((-1.0)
17381 * (1.51009803921569)
17382 * px))
17383 + ((px
17384 * x1424))),
17385 IKFAST_ATAN2_MAGTHRESH);
17386 if (!x1425
17387 .valid)
17388 {
17389 continue;
17390 }
17391 CheckValue<IkReal> x1426 = IKPowWithIntegerCheck(
17392 IKsign((
17393 pp
17394 + (((-1.0)
17395 * (1.0)
17396 * (pz
17397 * pz))))),
17398 -1);
17399 if (!x1426
17400 .valid)
17401 {
17402 continue;
17403 }
17404 j4array[0]
17405 = ((-1.5707963267949)
17406 + (x1425
17407 .value)
17408 + (((1.5707963267949)
17409 * (x1426
17410 .value))));
17411 sj4array[0] = IKsin(
17412 j4array
17413 [0]);
17414 cj4array[0] = IKcos(
17415 j4array
17416 [0]);
17417 if (j4array
17418 [0]
17419 > IKPI)
17420 {
17421 j4array[0]
17422 -= IK2PI;
17423 }
17424 else if (
17425 j4array
17426 [0]
17427 < -IKPI)
17428 {
17429 j4array[0]
17430 += IK2PI;
17431 }
17432 j4valid[0]
17433 = true;
17434 for (int ij4
17435 = 0;
17436 ij4
17437 < 1;
17438 ++ij4)
17439 {
17440 if (!j4valid
17441 [ij4])
17442 {
17443 continue;
17444 }
17445 _ij4[0]
17446 = ij4;
17447 _ij4[1]
17448 = -1;
17449 for (
17450 int iij4
17451 = ij4
17452 + 1;
17453 iij4
17454 < 1;
17455 ++iij4)
17456 {
17457 if (j4valid
17458 [iij4]
17459 && IKabs(
17460 cj4array
17461 [ij4]
17462 - cj4array
17463 [iij4])
17464 < IKFAST_SOLUTION_THRESH
17465 && IKabs(
17466 sj4array
17467 [ij4]
17468 - sj4array
17469 [iij4])
17470 < IKFAST_SOLUTION_THRESH)
17471 {
17472 j4valid
17473 [iij4]
17474 = false;
17475 _ij4[1]
17476 = iij4;
17477 break;
17478 }
17479 }
17480 j4 = j4array
17481 [ij4];
17482 cj4 = cj4array
17483 [ij4];
17484 sj4 = sj4array
17485 [ij4];
17486 {
17487 IkReal evalcond
17488 [3];
17489 IkReal
17490 x1427
17491 = IKsin(
17492 j4);
17493 IkReal
17494 x1428
17495 = IKcos(
17496 j4);
17497 IkReal
17498 x1429
17499 = (px
17500 * x1428);
17501 IkReal
17502 x1430
17503 = (py
17504 * x1427);
17505 evalcond
17506 [0]
17507 = ((((-1.0)
17508 * py
17509 * x1428))
17510 + ((px
17511 * x1427)));
17512 evalcond
17513 [1]
17514 = ((-1.51009803921569)
17515 + (((-1.0)
17516 * (1.32323529411765)
17517 * cj9))
17518 + (((3.92156862745098)
17519 * pp))
17520 + (((-1.0)
17521 * x1430))
17522 + (((-1.0)
17523 * x1429)));
17524 evalcond
17525 [2]
17526 = ((-0.2125)
17527 + (((1.1)
17528 * pz))
17529 + (((0.09)
17530 * x1430))
17531 + (((-1.0)
17532 * (1.0)
17533 * pp))
17534 + (((0.09)
17535 * x1429)));
17536 if (IKabs(
17537 evalcond
17538 [0])
17539 > IKFAST_EVALCOND_THRESH
17540 || IKabs(
17541 evalcond
17542 [1])
17543 > IKFAST_EVALCOND_THRESH
17544 || IKabs(
17545 evalcond
17546 [2])
17547 > IKFAST_EVALCOND_THRESH)
17548 {
17549 continue;
17550 }
17551 }
17552
17553 rotationfunction0(
17554 solutions);
17555 }
17556 }
17557 }
17558 }
17559 }
17560 } while (0);
17561 if (bgotonextstatement)
17562 {
17563 bool
17564 bgotonextstatement
17565 = true;
17566 do
17567 {
17568 evalcond[0]
17569 = ((-3.14159265358979)
17570 + (IKfmod(
17571 ((3.14159265358979)
17572 + (IKabs((
17573 (-3.14159265358979)
17574 + j8)))),
17575 6.28318530717959)));
17576 if (IKabs(evalcond
17577 [0])
17578 < 0.0000010000000000)
17579 {
17580 bgotonextstatement
17581 = false;
17582 {
17583 IkReal
17584 j4eval[3];
17585 sj6 = 0;
17586 cj6 = 1.0;
17587 j6 = 0;
17588 sj8 = 0;
17589 cj8 = -1.0;
17590 j8 = 3.14159265358979;
17591 IkReal x1431
17592 = (pp
17593 + (((-1.0)
17594 * (1.0)
17595 * (pz
17596 * pz))));
17597 IkReal x1432
17598 = ((13497.0)
17599 * cj9);
17600 IkReal x1433
17601 = ((40000.0)
17602 * pp);
17603 j4eval[0]
17604 = x1431;
17605 j4eval[1]
17606 = ((IKabs((
17607 (((15403.0)
17608 * px))
17609 + ((px
17610 * x1432))
17611 + (((-1.0)
17612 * px
17613 * x1433)))))
17614 + (IKabs((
17615 (((15403.0)
17616 * py))
17617 + ((py
17618 * x1432))
17619 + (((-1.0)
17620 * py
17621 * x1433))))));
17622 j4eval[2]
17623 = IKsign(
17624 x1431);
17625 if (IKabs(
17626 j4eval
17627 [0])
17628 < 0.0000010000000000
17629 || IKabs(
17630 j4eval
17631 [1])
17632 < 0.0000010000000000
17633 || IKabs(
17634 j4eval
17635 [2])
17636 < 0.0000010000000000)
17637 {
17638 {
17639 IkReal j4eval
17640 [3];
17641 sj6 = 0;
17642 cj6 = 1.0;
17643 j6 = 0;
17644 sj8 = 0;
17645 cj8 = -1.0;
17646 j8 = 3.14159265358979;
17647 IkReal
17648 x1434
17649 = pz
17650 * pz;
17651 IkReal
17652 x1435
17653 = ((80.0)
17654 * pp);
17655 IkReal
17656 x1436
17657 = ((88.0)
17658 * pz);
17659 j4eval[0]
17660 = (pp
17661 + (((-1.0)
17662 * x1434)));
17663 j4eval[1] = IKsign((
17664 (((9.0)
17665 * pp))
17666 + (((-9.0)
17667 * x1434))));
17668 j4eval[2]
17669 = ((IKabs((
17670 (((-1.0)
17671 * (17.0)
17672 * px))
17673 + ((px
17674 * x1436))
17675 + (((-1.0)
17676 * px
17677 * x1435)))))
17678 + (IKabs((
17679 ((py
17680 * x1436))
17681 + (((-1.0)
17682 * py
17683 * x1435))
17684 + (((-1.0)
17685 * (17.0)
17686 * py))))));
17687 if (IKabs(
17688 j4eval
17689 [0])
17690 < 0.0000010000000000
17691 || IKabs(
17692 j4eval
17693 [1])
17694 < 0.0000010000000000
17695 || IKabs(
17696 j4eval
17697 [2])
17698 < 0.0000010000000000)
17699 {
17700 {
17701 IkReal evalcond
17702 [5];
17703 bool
17704 bgotonextstatement
17705 = true;
17706 do
17707 {
17708 IkReal
17709 x1437
17710 = ((1.32323529411765)
17711 * cj9);
17712 IkReal
17713 x1438
17714 = ((3.92156862745098)
17715 * pp);
17716 evalcond
17717 [0]
17718 = ((IKabs(
17719 py))
17720 + (IKabs(
17721 px)));
17722 evalcond
17723 [1]
17724 = 0;
17725 evalcond
17726 [2]
17727 = ((-1.51009803921569)
17728 + x1438
17729 + (((-1.0)
17730 * x1437)));
17731 evalcond
17732 [3]
17733 = ((1.51009803921569)
17734 + (((-1.0)
17735 * x1438))
17736 + x1437);
17737 evalcond
17738 [4]
17739 = ((-0.2125)
17740 + (((1.1)
17741 * pz))
17742 + (((-1.0)
17743 * (1.0)
17744 * pp)));
17745 if (IKabs(
17746 evalcond
17747 [0])
17748 < 0.0000010000000000
17749 && IKabs(
17750 evalcond
17751 [1])
17752 < 0.0000010000000000
17753 && IKabs(
17754 evalcond
17755 [2])
17756 < 0.0000010000000000
17757 && IKabs(
17758 evalcond
17759 [3])
17760 < 0.0000010000000000
17761 && IKabs(
17762 evalcond
17763 [4])
17764 < 0.0000010000000000)
17765 {
17766 bgotonextstatement
17767 = false;
17768 {
17769 IkReal j4array
17770 [4],
17771 cj4array
17772 [4],
17773 sj4array
17774 [4];
17775 bool j4valid
17776 [4]
17777 = {false};
17778 _nj4
17779 = 4;
17780 j4array
17781 [0]
17782 = 0;
17783 sj4array
17784 [0]
17785 = IKsin(
17786 j4array
17787 [0]);
17788 cj4array
17789 [0]
17790 = IKcos(
17791 j4array
17792 [0]);
17793 j4array
17794 [1]
17795 = 1.5707963267949;
17796 sj4array
17797 [1]
17798 = IKsin(
17799 j4array
17800 [1]);
17801 cj4array
17802 [1]
17803 = IKcos(
17804 j4array
17805 [1]);
17806 j4array
17807 [2]
17808 = 3.14159265358979;
17809 sj4array
17810 [2]
17811 = IKsin(
17812 j4array
17813 [2]);
17814 cj4array
17815 [2]
17816 = IKcos(
17817 j4array
17818 [2]);
17819 j4array
17820 [3]
17821 = -1.5707963267949;
17822 sj4array
17823 [3]
17824 = IKsin(
17825 j4array
17826 [3]);
17827 cj4array
17828 [3]
17829 = IKcos(
17830 j4array
17831 [3]);
17832 if (j4array
17833 [0]
17834 > IKPI)
17835 {
17836 j4array
17837 [0]
17838 -= IK2PI;
17839 }
17840 else if (
17841 j4array
17842 [0]
17843 < -IKPI)
17844 {
17845 j4array
17846 [0]
17847 += IK2PI;
17848 }
17849 j4valid
17850 [0]
17851 = true;
17852 if (j4array
17853 [1]
17854 > IKPI)
17855 {
17856 j4array
17857 [1]
17858 -= IK2PI;
17859 }
17860 else if (
17861 j4array
17862 [1]
17863 < -IKPI)
17864 {
17865 j4array
17866 [1]
17867 += IK2PI;
17868 }
17869 j4valid
17870 [1]
17871 = true;
17872 if (j4array
17873 [2]
17874 > IKPI)
17875 {
17876 j4array
17877 [2]
17878 -= IK2PI;
17879 }
17880 else if (
17881 j4array
17882 [2]
17883 < -IKPI)
17884 {
17885 j4array
17886 [2]
17887 += IK2PI;
17888 }
17889 j4valid
17890 [2]
17891 = true;
17892 if (j4array
17893 [3]
17894 > IKPI)
17895 {
17896 j4array
17897 [3]
17898 -= IK2PI;
17899 }
17900 else if (
17901 j4array
17902 [3]
17903 < -IKPI)
17904 {
17905 j4array
17906 [3]
17907 += IK2PI;
17908 }
17909 j4valid
17910 [3]
17911 = true;
17912 for (
17913 int ij4
17914 = 0;
17915 ij4
17916 < 4;
17917 ++ij4)
17918 {
17919 if (!j4valid
17920 [ij4])
17921 {
17922 continue;
17923 }
17924 _ij4[0]
17925 = ij4;
17926 _ij4[1]
17927 = -1;
17928 for (
17929 int iij4
17930 = ij4
17931 + 1;
17932 iij4
17933 < 4;
17934 ++iij4)
17935 {
17936 if (j4valid
17937 [iij4]
17938 && IKabs(
17939 cj4array
17940 [ij4]
17941 - cj4array
17942 [iij4])
17943 < IKFAST_SOLUTION_THRESH
17944 && IKabs(
17945 sj4array
17946 [ij4]
17947 - sj4array
17948 [iij4])
17949 < IKFAST_SOLUTION_THRESH)
17950 {
17951 j4valid
17952 [iij4]
17953 = false;
17954 _ij4[1]
17955 = iij4;
17956 break;
17957 }
17958 }
17959 j4 = j4array
17960 [ij4];
17961 cj4 = cj4array
17962 [ij4];
17963 sj4 = sj4array
17964 [ij4];
17965
17966 rotationfunction0(
17967 solutions);
17968 }
17969 }
17970 }
17971 } while (
17972 0);
17973 if (bgotonextstatement)
17974 {
17975 bool
17976 bgotonextstatement
17977 = true;
17978 do
17979 {
17980 if (1)
17981 {
17982 bgotonextstatement
17983 = false;
17984 continue; // branch miss [j4]
17985 }
17986 } while (
17987 0);
17988 if (bgotonextstatement)
17989 {
17990 }
17991 }
17992 }
17993 }
17994 else
17995 {
17996 {
17997 IkReal j4array
17998 [1],
17999 cj4array
18000 [1],
18001 sj4array
18002 [1];
18003 bool j4valid
18004 [1]
18005 = {false};
18006 _nj4
18007 = 1;
18008 IkReal
18009 x1439
18010 = ((100.0)
18011 * pp);
18012 IkReal
18013 x1440
18014 = ((110.0)
18015 * pz);
18016 CheckValue<IkReal> x1441 = IKPowWithIntegerCheck(
18017 IKsign((
18018 (((-1.0)
18019 * (9.0)
18020 * (pz
18021 * pz)))
18022 + (((9.0)
18023 * pp)))),
18024 -1);
18025 if (!x1441
18026 .valid)
18027 {
18028 continue;
18029 }
18030 CheckValue<IkReal> x1442 = IKatan2WithCheck(
18031 IkReal((
18032 ((py
18033 * x1440))
18034 + (((-1.0)
18035 * (21.25)
18036 * py))
18037 + (((-1.0)
18038 * py
18039 * x1439)))),
18040 (((px
18041 * x1440))
18042 + (((-1.0)
18043 * (21.25)
18044 * px))
18045 + (((-1.0)
18046 * px
18047 * x1439))),
18048 IKFAST_ATAN2_MAGTHRESH);
18049 if (!x1442
18050 .valid)
18051 {
18052 continue;
18053 }
18054 j4array
18055 [0]
18056 = ((-1.5707963267949)
18057 + (((1.5707963267949)
18058 * (x1441
18059 .value)))
18060 + (x1442
18061 .value));
18062 sj4array
18063 [0]
18064 = IKsin(
18065 j4array
18066 [0]);
18067 cj4array
18068 [0]
18069 = IKcos(
18070 j4array
18071 [0]);
18072 if (j4array
18073 [0]
18074 > IKPI)
18075 {
18076 j4array
18077 [0]
18078 -= IK2PI;
18079 }
18080 else if (
18081 j4array
18082 [0]
18083 < -IKPI)
18084 {
18085 j4array
18086 [0]
18087 += IK2PI;
18088 }
18089 j4valid
18090 [0]
18091 = true;
18092 for (
18093 int ij4
18094 = 0;
18095 ij4
18096 < 1;
18097 ++ij4)
18098 {
18099 if (!j4valid
18100 [ij4])
18101 {
18102 continue;
18103 }
18104 _ij4[0]
18105 = ij4;
18106 _ij4[1]
18107 = -1;
18108 for (
18109 int iij4
18110 = ij4
18111 + 1;
18112 iij4
18113 < 1;
18114 ++iij4)
18115 {
18116 if (j4valid
18117 [iij4]
18118 && IKabs(
18119 cj4array
18120 [ij4]
18121 - cj4array
18122 [iij4])
18123 < IKFAST_SOLUTION_THRESH
18124 && IKabs(
18125 sj4array
18126 [ij4]
18127 - sj4array
18128 [iij4])
18129 < IKFAST_SOLUTION_THRESH)
18130 {
18131 j4valid
18132 [iij4]
18133 = false;
18134 _ij4[1]
18135 = iij4;
18136 break;
18137 }
18138 }
18139 j4 = j4array
18140 [ij4];
18141 cj4 = cj4array
18142 [ij4];
18143 sj4 = sj4array
18144 [ij4];
18145 {
18146 IkReal evalcond
18147 [3];
18148 IkReal
18149 x1443
18150 = IKsin(
18151 j4);
18152 IkReal
18153 x1444
18154 = IKcos(
18155 j4);
18156 IkReal
18157 x1445
18158 = (px
18159 * x1444);
18160 IkReal
18161 x1446
18162 = (py
18163 * x1443);
18164 evalcond
18165 [0]
18166 = ((((-1.0)
18167 * py
18168 * x1444))
18169 + ((px
18170 * x1443)));
18171 evalcond
18172 [1]
18173 = ((-1.51009803921569)
18174 + (((-1.0)
18175 * (1.32323529411765)
18176 * cj9))
18177 + (((3.92156862745098)
18178 * pp))
18179 + x1446
18180 + x1445);
18181 evalcond
18182 [2]
18183 = ((-0.2125)
18184 + (((1.1)
18185 * pz))
18186 + (((-0.09)
18187 * x1446))
18188 + (((-1.0)
18189 * (1.0)
18190 * pp))
18191 + (((-0.09)
18192 * x1445)));
18193 if (IKabs(
18194 evalcond
18195 [0])
18196 > IKFAST_EVALCOND_THRESH
18197 || IKabs(
18198 evalcond
18199 [1])
18200 > IKFAST_EVALCOND_THRESH
18201 || IKabs(
18202 evalcond
18203 [2])
18204 > IKFAST_EVALCOND_THRESH)
18205 {
18206 continue;
18207 }
18208 }
18209
18210 rotationfunction0(
18211 solutions);
18212 }
18213 }
18214 }
18215 }
18216 }
18217 else
18218 {
18219 {
18220 IkReal j4array
18221 [1],
18222 cj4array
18223 [1],
18224 sj4array
18225 [1];
18226 bool j4valid
18227 [1]
18228 = {false};
18229 _nj4 = 1;
18230 IkReal
18231 x1447
18232 = ((1.32323529411765)
18233 * cj9);
18234 IkReal
18235 x1448
18236 = ((3.92156862745098)
18237 * pp);
18238 CheckValue<IkReal> x1449 = IKPowWithIntegerCheck(
18239 IKsign((
18240 pp
18241 + (((-1.0)
18242 * (1.0)
18243 * (pz
18244 * pz))))),
18245 -1);
18246 if (!x1449
18247 .valid)
18248 {
18249 continue;
18250 }
18251 CheckValue<IkReal> x1450 = IKatan2WithCheck(
18252 IkReal((
18253 (((-1.0)
18254 * py
18255 * x1448))
18256 + (((1.51009803921569)
18257 * py))
18258 + ((py
18259 * x1447)))),
18260 ((((-1.0)
18261 * px
18262 * x1448))
18263 + (((1.51009803921569)
18264 * px))
18265 + ((px
18266 * x1447))),
18267 IKFAST_ATAN2_MAGTHRESH);
18268 if (!x1450
18269 .valid)
18270 {
18271 continue;
18272 }
18273 j4array[0]
18274 = ((-1.5707963267949)
18275 + (((1.5707963267949)
18276 * (x1449
18277 .value)))
18278 + (x1450
18279 .value));
18280 sj4array[0] = IKsin(
18281 j4array
18282 [0]);
18283 cj4array[0] = IKcos(
18284 j4array
18285 [0]);
18286 if (j4array
18287 [0]
18288 > IKPI)
18289 {
18290 j4array
18291 [0]
18292 -= IK2PI;
18293 }
18294 else if (
18295 j4array
18296 [0]
18297 < -IKPI)
18298 {
18299 j4array
18300 [0]
18301 += IK2PI;
18302 }
18303 j4valid[0]
18304 = true;
18305 for (
18306 int ij4
18307 = 0;
18308 ij4
18309 < 1;
18310 ++ij4)
18311 {
18312 if (!j4valid
18313 [ij4])
18314 {
18315 continue;
18316 }
18317 _ij4[0]
18318 = ij4;
18319 _ij4[1]
18320 = -1;
18321 for (
18322 int iij4
18323 = ij4
18324 + 1;
18325 iij4
18326 < 1;
18327 ++iij4)
18328 {
18329 if (j4valid
18330 [iij4]
18331 && IKabs(
18332 cj4array
18333 [ij4]
18334 - cj4array
18335 [iij4])
18336 < IKFAST_SOLUTION_THRESH
18337 && IKabs(
18338 sj4array
18339 [ij4]
18340 - sj4array
18341 [iij4])
18342 < IKFAST_SOLUTION_THRESH)
18343 {
18344 j4valid
18345 [iij4]
18346 = false;
18347 _ij4[1]
18348 = iij4;
18349 break;
18350 }
18351 }
18352 j4 = j4array
18353 [ij4];
18354 cj4 = cj4array
18355 [ij4];
18356 sj4 = sj4array
18357 [ij4];
18358 {
18359 IkReal evalcond
18360 [3];
18361 IkReal
18362 x1451
18363 = IKsin(
18364 j4);
18365 IkReal
18366 x1452
18367 = IKcos(
18368 j4);
18369 IkReal
18370 x1453
18371 = (px
18372 * x1452);
18373 IkReal
18374 x1454
18375 = (py
18376 * x1451);
18377 evalcond
18378 [0]
18379 = (((px
18380 * x1451))
18381 + (((-1.0)
18382 * py
18383 * x1452)));
18384 evalcond
18385 [1]
18386 = ((-1.51009803921569)
18387 + (((-1.0)
18388 * (1.32323529411765)
18389 * cj9))
18390 + (((3.92156862745098)
18391 * pp))
18392 + x1453
18393 + x1454);
18394 evalcond
18395 [2]
18396 = ((-0.2125)
18397 + (((-0.09)
18398 * x1453))
18399 + (((1.1)
18400 * pz))
18401 + (((-1.0)
18402 * (1.0)
18403 * pp))
18404 + (((-0.09)
18405 * x1454)));
18406 if (IKabs(
18407 evalcond
18408 [0])
18409 > IKFAST_EVALCOND_THRESH
18410 || IKabs(
18411 evalcond
18412 [1])
18413 > IKFAST_EVALCOND_THRESH
18414 || IKabs(
18415 evalcond
18416 [2])
18417 > IKFAST_EVALCOND_THRESH)
18418 {
18419 continue;
18420 }
18421 }
18422
18423 rotationfunction0(
18424 solutions);
18425 }
18426 }
18427 }
18428 }
18429 }
18430 } while (0);
18431 if (bgotonextstatement)
18432 {
18433 bool
18434 bgotonextstatement
18435 = true;
18436 do
18437 {
18438 evalcond[0]
18439 = ((-3.14159265358979)
18440 + (IKfmod(
18441 ((3.14159265358979)
18442 + (IKabs((
18443 (-1.5707963267949)
18444 + j8)))),
18445 6.28318530717959)));
18446 if (IKabs(
18447 evalcond
18448 [0])
18449 < 0.0000010000000000)
18450 {
18451 bgotonextstatement
18452 = false;
18453 {
18454 IkReal
18455 j4eval
18456 [3];
18457 sj6 = 0;
18458 cj6 = 1.0;
18459 j6 = 0;
18460 sj8 = 1.0;
18461 cj8 = 0;
18462 j8 = 1.5707963267949;
18463 IkReal x1455
18464 = (pp
18465 + (((-1.0)
18466 * (1.0)
18467 * (pz
18468 * pz))));
18469 IkReal x1456
18470 = ((13497.0)
18471 * cj9);
18472 IkReal x1457
18473 = ((40000.0)
18474 * pp);
18475 j4eval[0]
18476 = x1455;
18477 j4eval[1]
18478 = ((IKabs((
18479 (((15403.0)
18480 * px))
18481 + (((-1.0)
18482 * px
18483 * x1457))
18484 + ((px
18485 * x1456)))))
18486 + (IKabs((
18487 (((-1.0)
18488 * py
18489 * x1456))
18490 + ((py
18491 * x1457))
18492 + (((-1.0)
18493 * (15403.0)
18494 * py))))));
18495 j4eval[2]
18496 = IKsign(
18497 x1455);
18498 if (IKabs(
18499 j4eval
18500 [0])
18501 < 0.0000010000000000
18502 || IKabs(
18503 j4eval
18504 [1])
18505 < 0.0000010000000000
18506 || IKabs(
18507 j4eval
18508 [2])
18509 < 0.0000010000000000)
18510 {
18511 {
18512 IkReal j4eval
18513 [3];
18514 sj6 = 0;
18515 cj6 = 1.0;
18516 j6 = 0;
18517 sj8 = 1.0;
18518 cj8 = 0;
18519 j8 = 1.5707963267949;
18520 IkReal
18521 x1458
18522 = pz
18523 * pz;
18524 IkReal
18525 x1459
18526 = ((80.0)
18527 * pp);
18528 IkReal
18529 x1460
18530 = ((88.0)
18531 * pz);
18532 j4eval
18533 [0]
18534 = (pp
18535 + (((-1.0)
18536 * x1458)));
18537 j4eval[1] = IKsign((
18538 (((-9.0)
18539 * x1458))
18540 + (((9.0)
18541 * pp))));
18542 j4eval
18543 [2]
18544 = ((IKabs((
18545 (((-1.0)
18546 * py
18547 * x1460))
18548 + ((py
18549 * x1459))
18550 + (((17.0)
18551 * py)))))
18552 + (IKabs((
18553 (((-1.0)
18554 * (17.0)
18555 * px))
18556 + (((-1.0)
18557 * px
18558 * x1459))
18559 + ((px
18560 * x1460))))));
18561 if (IKabs(
18562 j4eval
18563 [0])
18564 < 0.0000010000000000
18565 || IKabs(
18566 j4eval
18567 [1])
18568 < 0.0000010000000000
18569 || IKabs(
18570 j4eval
18571 [2])
18572 < 0.0000010000000000)
18573 {
18574 {
18575 IkReal evalcond
18576 [5];
18577 bool
18578 bgotonextstatement
18579 = true;
18580 do
18581 {
18582 IkReal
18583 x1461
18584 = ((-1.51009803921569)
18585 + (((-1.0)
18586 * (1.32323529411765)
18587 * cj9))
18588 + (((3.92156862745098)
18589 * pp)));
18590 evalcond
18591 [0]
18592 = ((IKabs(
18593 py))
18594 + (IKabs(
18595 px)));
18596 evalcond
18597 [1]
18598 = x1461;
18599 evalcond
18600 [2]
18601 = 0;
18602 evalcond
18603 [3]
18604 = x1461;
18605 evalcond
18606 [4]
18607 = ((-0.2125)
18608 + (((1.1)
18609 * pz))
18610 + (((-1.0)
18611 * (1.0)
18612 * pp)));
18613 if (IKabs(
18614 evalcond
18615 [0])
18616 < 0.0000010000000000
18617 && IKabs(
18618 evalcond
18619 [1])
18620 < 0.0000010000000000
18621 && IKabs(
18622 evalcond
18623 [2])
18624 < 0.0000010000000000
18625 && IKabs(
18626 evalcond
18627 [3])
18628 < 0.0000010000000000
18629 && IKabs(
18630 evalcond
18631 [4])
18632 < 0.0000010000000000)
18633 {
18634 bgotonextstatement
18635 = false;
18636 {
18637 IkReal j4array
18638 [4],
18639 cj4array
18640 [4],
18641 sj4array
18642 [4];
18643 bool j4valid
18644 [4]
18645 = {false};
18646 _nj4
18647 = 4;
18648 j4array
18649 [0]
18650 = 0;
18651 sj4array
18652 [0]
18653 = IKsin(
18654 j4array
18655 [0]);
18656 cj4array
18657 [0]
18658 = IKcos(
18659 j4array
18660 [0]);
18661 j4array
18662 [1]
18663 = 1.5707963267949;
18664 sj4array
18665 [1]
18666 = IKsin(
18667 j4array
18668 [1]);
18669 cj4array
18670 [1]
18671 = IKcos(
18672 j4array
18673 [1]);
18674 j4array
18675 [2]
18676 = 3.14159265358979;
18677 sj4array
18678 [2]
18679 = IKsin(
18680 j4array
18681 [2]);
18682 cj4array
18683 [2]
18684 = IKcos(
18685 j4array
18686 [2]);
18687 j4array
18688 [3]
18689 = -1.5707963267949;
18690 sj4array
18691 [3]
18692 = IKsin(
18693 j4array
18694 [3]);
18695 cj4array
18696 [3]
18697 = IKcos(
18698 j4array
18699 [3]);
18700 if (j4array
18701 [0]
18702 > IKPI)
18703 {
18704 j4array
18705 [0]
18706 -= IK2PI;
18707 }
18708 else if (
18709 j4array
18710 [0]
18711 < -IKPI)
18712 {
18713 j4array
18714 [0]
18715 += IK2PI;
18716 }
18717 j4valid
18718 [0]
18719 = true;
18720 if (j4array
18721 [1]
18722 > IKPI)
18723 {
18724 j4array
18725 [1]
18726 -= IK2PI;
18727 }
18728 else if (
18729 j4array
18730 [1]
18731 < -IKPI)
18732 {
18733 j4array
18734 [1]
18735 += IK2PI;
18736 }
18737 j4valid
18738 [1]
18739 = true;
18740 if (j4array
18741 [2]
18742 > IKPI)
18743 {
18744 j4array
18745 [2]
18746 -= IK2PI;
18747 }
18748 else if (
18749 j4array
18750 [2]
18751 < -IKPI)
18752 {
18753 j4array
18754 [2]
18755 += IK2PI;
18756 }
18757 j4valid
18758 [2]
18759 = true;
18760 if (j4array
18761 [3]
18762 > IKPI)
18763 {
18764 j4array
18765 [3]
18766 -= IK2PI;
18767 }
18768 else if (
18769 j4array
18770 [3]
18771 < -IKPI)
18772 {
18773 j4array
18774 [3]
18775 += IK2PI;
18776 }
18777 j4valid
18778 [3]
18779 = true;
18780 for (
18781 int ij4
18782 = 0;
18783 ij4
18784 < 4;
18785 ++ij4)
18786 {
18787 if (!j4valid
18788 [ij4])
18789 {
18790 continue;
18791 }
18792 _ij4[0]
18793 = ij4;
18794 _ij4[1]
18795 = -1;
18796 for (
18797 int iij4
18798 = ij4
18799 + 1;
18800 iij4
18801 < 4;
18802 ++iij4)
18803 {
18804 if (j4valid
18805 [iij4]
18806 && IKabs(
18807 cj4array
18808 [ij4]
18809 - cj4array
18810 [iij4])
18811 < IKFAST_SOLUTION_THRESH
18812 && IKabs(
18813 sj4array
18814 [ij4]
18815 - sj4array
18816 [iij4])
18817 < IKFAST_SOLUTION_THRESH)
18818 {
18819 j4valid
18820 [iij4]
18821 = false;
18822 _ij4[1]
18823 = iij4;
18824 break;
18825 }
18826 }
18827 j4 = j4array
18828 [ij4];
18829 cj4 = cj4array
18830 [ij4];
18831 sj4 = sj4array
18832 [ij4];
18833
18834 rotationfunction0(
18835 solutions);
18836 }
18837 }
18838 }
18839 } while (
18840 0);
18841 if (bgotonextstatement)
18842 {
18843 bool
18844 bgotonextstatement
18845 = true;
18846 do
18847 {
18848 if (1)
18849 {
18850 bgotonextstatement
18851 = false;
18852 continue; // branch miss [j4]
18853 }
18854 } while (
18855 0);
18856 if (bgotonextstatement)
18857 {
18858 }
18859 }
18860 }
18861 }
18862 else
18863 {
18864 {
18865 IkReal j4array
18866 [1],
18867 cj4array
18868 [1],
18869 sj4array
18870 [1];
18871 bool j4valid
18872 [1]
18873 = {false};
18874 _nj4
18875 = 1;
18876 IkReal
18877 x1462
18878 = ((100.0)
18879 * pp);
18880 IkReal
18881 x1463
18882 = ((110.0)
18883 * pz);
18884 CheckValue<IkReal> x1464 = IKPowWithIntegerCheck(
18885 IKsign((
18886 (((-1.0)
18887 * (9.0)
18888 * (pz
18889 * pz)))
18890 + (((9.0)
18891 * pp)))),
18892 -1);
18893 if (!x1464
18894 .valid)
18895 {
18896 continue;
18897 }
18898 CheckValue<IkReal> x1465 = IKatan2WithCheck(
18899 IkReal((
18900 (((-1.0)
18901 * px
18902 * x1462))
18903 + (((-1.0)
18904 * (21.25)
18905 * px))
18906 + ((px
18907 * x1463)))),
18908 (((py
18909 * x1462))
18910 + (((21.25)
18911 * py))
18912 + (((-1.0)
18913 * py
18914 * x1463))),
18915 IKFAST_ATAN2_MAGTHRESH);
18916 if (!x1465
18917 .valid)
18918 {
18919 continue;
18920 }
18921 j4array
18922 [0]
18923 = ((-1.5707963267949)
18924 + (((1.5707963267949)
18925 * (x1464
18926 .value)))
18927 + (x1465
18928 .value));
18929 sj4array
18930 [0]
18931 = IKsin(
18932 j4array
18933 [0]);
18934 cj4array
18935 [0]
18936 = IKcos(
18937 j4array
18938 [0]);
18939 if (j4array
18940 [0]
18941 > IKPI)
18942 {
18943 j4array
18944 [0]
18945 -= IK2PI;
18946 }
18947 else if (
18948 j4array
18949 [0]
18950 < -IKPI)
18951 {
18952 j4array
18953 [0]
18954 += IK2PI;
18955 }
18956 j4valid
18957 [0]
18958 = true;
18959 for (
18960 int ij4
18961 = 0;
18962 ij4
18963 < 1;
18964 ++ij4)
18965 {
18966 if (!j4valid
18967 [ij4])
18968 {
18969 continue;
18970 }
18971 _ij4[0]
18972 = ij4;
18973 _ij4[1]
18974 = -1;
18975 for (
18976 int iij4
18977 = ij4
18978 + 1;
18979 iij4
18980 < 1;
18981 ++iij4)
18982 {
18983 if (j4valid
18984 [iij4]
18985 && IKabs(
18986 cj4array
18987 [ij4]
18988 - cj4array
18989 [iij4])
18990 < IKFAST_SOLUTION_THRESH
18991 && IKabs(
18992 sj4array
18993 [ij4]
18994 - sj4array
18995 [iij4])
18996 < IKFAST_SOLUTION_THRESH)
18997 {
18998 j4valid
18999 [iij4]
19000 = false;
19001 _ij4[1]
19002 = iij4;
19003 break;
19004 }
19005 }
19006 j4 = j4array
19007 [ij4];
19008 cj4 = cj4array
19009 [ij4];
19010 sj4 = sj4array
19011 [ij4];
19012 {
19013 IkReal evalcond
19014 [3];
19015 IkReal
19016 x1466
19017 = IKcos(
19018 j4);
19019 IkReal
19020 x1467
19021 = IKsin(
19022 j4);
19023 IkReal
19024 x1468
19025 = (px
19026 * x1467);
19027 IkReal
19028 x1469
19029 = (py
19030 * x1466);
19031 evalcond
19032 [0]
19033 = (((py
19034 * x1467))
19035 + ((px
19036 * x1466)));
19037 evalcond
19038 [1]
19039 = ((-1.51009803921569)
19040 + (((-1.0)
19041 * x1469))
19042 + (((-1.0)
19043 * (1.32323529411765)
19044 * cj9))
19045 + (((3.92156862745098)
19046 * pp))
19047 + x1468);
19048 evalcond
19049 [2]
19050 = ((-0.2125)
19051 + (((1.1)
19052 * pz))
19053 + (((0.09)
19054 * x1469))
19055 + (((-0.09)
19056 * x1468))
19057 + (((-1.0)
19058 * (1.0)
19059 * pp)));
19060 if (IKabs(
19061 evalcond
19062 [0])
19063 > IKFAST_EVALCOND_THRESH
19064 || IKabs(
19065 evalcond
19066 [1])
19067 > IKFAST_EVALCOND_THRESH
19068 || IKabs(
19069 evalcond
19070 [2])
19071 > IKFAST_EVALCOND_THRESH)
19072 {
19073 continue;
19074 }
19075 }
19076
19077 rotationfunction0(
19078 solutions);
19079 }
19080 }
19081 }
19082 }
19083 }
19084 else
19085 {
19086 {
19087 IkReal j4array
19088 [1],
19089 cj4array
19090 [1],
19091 sj4array
19092 [1];
19093 bool j4valid
19094 [1]
19095 = {false};
19096 _nj4
19097 = 1;
19098 IkReal
19099 x1470
19100 = ((1.32323529411765)
19101 * cj9);
19102 IkReal
19103 x1471
19104 = ((3.92156862745098)
19105 * pp);
19106 CheckValue<IkReal> x1472 = IKPowWithIntegerCheck(
19107 IKsign((
19108 pp
19109 + (((-1.0)
19110 * (1.0)
19111 * (pz
19112 * pz))))),
19113 -1);
19114 if (!x1472
19115 .valid)
19116 {
19117 continue;
19118 }
19119 CheckValue<IkReal> x1473 = IKatan2WithCheck(
19120 IkReal((
19121 ((px
19122 * x1470))
19123 + (((-1.0)
19124 * px
19125 * x1471))
19126 + (((1.51009803921569)
19127 * px)))),
19128 (((py
19129 * x1471))
19130 + (((-1.0)
19131 * (1.51009803921569)
19132 * py))
19133 + (((-1.0)
19134 * py
19135 * x1470))),
19136 IKFAST_ATAN2_MAGTHRESH);
19137 if (!x1473
19138 .valid)
19139 {
19140 continue;
19141 }
19142 j4array
19143 [0]
19144 = ((-1.5707963267949)
19145 + (((1.5707963267949)
19146 * (x1472
19147 .value)))
19148 + (x1473
19149 .value));
19150 sj4array
19151 [0]
19152 = IKsin(
19153 j4array
19154 [0]);
19155 cj4array
19156 [0]
19157 = IKcos(
19158 j4array
19159 [0]);
19160 if (j4array
19161 [0]
19162 > IKPI)
19163 {
19164 j4array
19165 [0]
19166 -= IK2PI;
19167 }
19168 else if (
19169 j4array
19170 [0]
19171 < -IKPI)
19172 {
19173 j4array
19174 [0]
19175 += IK2PI;
19176 }
19177 j4valid
19178 [0]
19179 = true;
19180 for (
19181 int ij4
19182 = 0;
19183 ij4
19184 < 1;
19185 ++ij4)
19186 {
19187 if (!j4valid
19188 [ij4])
19189 {
19190 continue;
19191 }
19192 _ij4[0]
19193 = ij4;
19194 _ij4[1]
19195 = -1;
19196 for (
19197 int iij4
19198 = ij4
19199 + 1;
19200 iij4
19201 < 1;
19202 ++iij4)
19203 {
19204 if (j4valid
19205 [iij4]
19206 && IKabs(
19207 cj4array
19208 [ij4]
19209 - cj4array
19210 [iij4])
19211 < IKFAST_SOLUTION_THRESH
19212 && IKabs(
19213 sj4array
19214 [ij4]
19215 - sj4array
19216 [iij4])
19217 < IKFAST_SOLUTION_THRESH)
19218 {
19219 j4valid
19220 [iij4]
19221 = false;
19222 _ij4[1]
19223 = iij4;
19224 break;
19225 }
19226 }
19227 j4 = j4array
19228 [ij4];
19229 cj4 = cj4array
19230 [ij4];
19231 sj4 = sj4array
19232 [ij4];
19233 {
19234 IkReal evalcond
19235 [3];
19236 IkReal
19237 x1474
19238 = IKcos(
19239 j4);
19240 IkReal
19241 x1475
19242 = IKsin(
19243 j4);
19244 IkReal
19245 x1476
19246 = (px
19247 * x1475);
19248 IkReal
19249 x1477
19250 = (py
19251 * x1474);
19252 evalcond
19253 [0]
19254 = (((px
19255 * x1474))
19256 + ((py
19257 * x1475)));
19258 evalcond
19259 [1]
19260 = ((-1.51009803921569)
19261 + (((-1.0)
19262 * (1.32323529411765)
19263 * cj9))
19264 + (((3.92156862745098)
19265 * pp))
19266 + (((-1.0)
19267 * x1477))
19268 + x1476);
19269 evalcond
19270 [2]
19271 = ((-0.2125)
19272 + (((-0.09)
19273 * x1476))
19274 + (((0.09)
19275 * x1477))
19276 + (((1.1)
19277 * pz))
19278 + (((-1.0)
19279 * (1.0)
19280 * pp)));
19281 if (IKabs(
19282 evalcond
19283 [0])
19284 > IKFAST_EVALCOND_THRESH
19285 || IKabs(
19286 evalcond
19287 [1])
19288 > IKFAST_EVALCOND_THRESH
19289 || IKabs(
19290 evalcond
19291 [2])
19292 > IKFAST_EVALCOND_THRESH)
19293 {
19294 continue;
19295 }
19296 }
19297
19298 rotationfunction0(
19299 solutions);
19300 }
19301 }
19302 }
19303 }
19304 }
19305 } while (0);
19306 if (bgotonextstatement)
19307 {
19308 bool
19309 bgotonextstatement
19310 = true;
19311 do
19312 {
19313 evalcond[0]
19314 = ((-3.14159265358979)
19315 + (IKfmod(
19316 ((3.14159265358979)
19317 + (IKabs((
19318 (1.5707963267949)
19319 + j8)))),
19320 6.28318530717959)));
19321 if (IKabs(
19322 evalcond
19323 [0])
19324 < 0.0000010000000000)
19325 {
19326 bgotonextstatement
19327 = false;
19328 {
19329 IkReal j4eval
19330 [3];
19331 sj6 = 0;
19332 cj6 = 1.0;
19333 j6 = 0;
19334 sj8 = -1.0;
19335 cj8 = 0;
19336 j8 = -1.5707963267949;
19337 IkReal
19338 x1478
19339 = (pp
19340 + (((-1.0)
19341 * (1.0)
19342 * (pz
19343 * pz))));
19344 IkReal
19345 x1479
19346 = ((13497.0)
19347 * cj9);
19348 IkReal
19349 x1480
19350 = ((40000.0)
19351 * pp);
19352 j4eval[0]
19353 = x1478;
19354 j4eval[1]
19355 = ((IKabs((
19356 (((-1.0)
19357 * py
19358 * x1480))
19359 + (((15403.0)
19360 * py))
19361 + ((py
19362 * x1479)))))
19363 + (IKabs((
19364 (((-1.0)
19365 * px
19366 * x1479))
19367 + ((px
19368 * x1480))
19369 + (((-1.0)
19370 * (15403.0)
19371 * px))))));
19372 j4eval[2]
19373 = IKsign(
19374 x1478);
19375 if (IKabs(
19376 j4eval
19377 [0])
19378 < 0.0000010000000000
19379 || IKabs(
19380 j4eval
19381 [1])
19382 < 0.0000010000000000
19383 || IKabs(
19384 j4eval
19385 [2])
19386 < 0.0000010000000000)
19387 {
19388 {
19389 IkReal j4eval
19390 [3];
19391 sj6 = 0;
19392 cj6 = 1.0;
19393 j6 = 0;
19394 sj8 = -1.0;
19395 cj8 = 0;
19396 j8 = -1.5707963267949;
19397 IkReal
19398 x1481
19399 = pz
19400 * pz;
19401 IkReal
19402 x1482
19403 = ((80.0)
19404 * pp);
19405 IkReal
19406 x1483
19407 = ((88.0)
19408 * pz);
19409 j4eval
19410 [0]
19411 = ((((-1.0)
19412 * x1481))
19413 + pp);
19414 j4eval
19415 [1]
19416 = ((IKabs((
19417 (((-1.0)
19418 * py
19419 * x1482))
19420 + ((py
19421 * x1483))
19422 + (((-1.0)
19423 * (17.0)
19424 * py)))))
19425 + (IKabs((
19426 (((17.0)
19427 * px))
19428 + ((px
19429 * x1482))
19430 + (((-1.0)
19431 * px
19432 * x1483))))));
19433 j4eval[2] = IKsign((
19434 (((-9.0)
19435 * x1481))
19436 + (((9.0)
19437 * pp))));
19438 if (IKabs(
19439 j4eval
19440 [0])
19441 < 0.0000010000000000
19442 || IKabs(
19443 j4eval
19444 [1])
19445 < 0.0000010000000000
19446 || IKabs(
19447 j4eval
19448 [2])
19449 < 0.0000010000000000)
19450 {
19451 {
19452 IkReal evalcond
19453 [5];
19454 bool
19455 bgotonextstatement
19456 = true;
19457 do
19458 {
19459 IkReal
19460 x1484
19461 = ((1.32323529411765)
19462 * cj9);
19463 IkReal
19464 x1485
19465 = ((3.92156862745098)
19466 * pp);
19467 evalcond
19468 [0]
19469 = ((IKabs(
19470 py))
19471 + (IKabs(
19472 px)));
19473 evalcond
19474 [1]
19475 = ((1.51009803921569)
19476 + (((-1.0)
19477 * x1485))
19478 + x1484);
19479 evalcond
19480 [2]
19481 = 0;
19482 evalcond
19483 [3]
19484 = ((-1.51009803921569)
19485 + x1485
19486 + (((-1.0)
19487 * x1484)));
19488 evalcond
19489 [4]
19490 = ((-0.2125)
19491 + (((1.1)
19492 * pz))
19493 + (((-1.0)
19494 * (1.0)
19495 * pp)));
19496 if (IKabs(
19497 evalcond
19498 [0])
19499 < 0.0000010000000000
19500 && IKabs(
19501 evalcond
19502 [1])
19503 < 0.0000010000000000
19504 && IKabs(
19505 evalcond
19506 [2])
19507 < 0.0000010000000000
19508 && IKabs(
19509 evalcond
19510 [3])
19511 < 0.0000010000000000
19512 && IKabs(
19513 evalcond
19514 [4])
19515 < 0.0000010000000000)
19516 {
19517 bgotonextstatement
19518 = false;
19519 {
19520 IkReal j4array
19521 [4],
19522 cj4array
19523 [4],
19524 sj4array
19525 [4];
19526 bool j4valid
19527 [4]
19528 = {false};
19529 _nj4
19530 = 4;
19531 j4array
19532 [0]
19533 = 0;
19534 sj4array
19535 [0]
19536 = IKsin(
19537 j4array
19538 [0]);
19539 cj4array
19540 [0]
19541 = IKcos(
19542 j4array
19543 [0]);
19544 j4array
19545 [1]
19546 = 1.5707963267949;
19547 sj4array
19548 [1]
19549 = IKsin(
19550 j4array
19551 [1]);
19552 cj4array
19553 [1]
19554 = IKcos(
19555 j4array
19556 [1]);
19557 j4array
19558 [2]
19559 = 3.14159265358979;
19560 sj4array
19561 [2]
19562 = IKsin(
19563 j4array
19564 [2]);
19565 cj4array
19566 [2]
19567 = IKcos(
19568 j4array
19569 [2]);
19570 j4array
19571 [3]
19572 = -1.5707963267949;
19573 sj4array
19574 [3]
19575 = IKsin(
19576 j4array
19577 [3]);
19578 cj4array
19579 [3]
19580 = IKcos(
19581 j4array
19582 [3]);
19583 if (j4array
19584 [0]
19585 > IKPI)
19586 {
19587 j4array
19588 [0]
19589 -= IK2PI;
19590 }
19591 else if (
19592 j4array
19593 [0]
19594 < -IKPI)
19595 {
19596 j4array
19597 [0]
19598 += IK2PI;
19599 }
19600 j4valid
19601 [0]
19602 = true;
19603 if (j4array
19604 [1]
19605 > IKPI)
19606 {
19607 j4array
19608 [1]
19609 -= IK2PI;
19610 }
19611 else if (
19612 j4array
19613 [1]
19614 < -IKPI)
19615 {
19616 j4array
19617 [1]
19618 += IK2PI;
19619 }
19620 j4valid
19621 [1]
19622 = true;
19623 if (j4array
19624 [2]
19625 > IKPI)
19626 {
19627 j4array
19628 [2]
19629 -= IK2PI;
19630 }
19631 else if (
19632 j4array
19633 [2]
19634 < -IKPI)
19635 {
19636 j4array
19637 [2]
19638 += IK2PI;
19639 }
19640 j4valid
19641 [2]
19642 = true;
19643 if (j4array
19644 [3]
19645 > IKPI)
19646 {
19647 j4array
19648 [3]
19649 -= IK2PI;
19650 }
19651 else if (
19652 j4array
19653 [3]
19654 < -IKPI)
19655 {
19656 j4array
19657 [3]
19658 += IK2PI;
19659 }
19660 j4valid
19661 [3]
19662 = true;
19663 for (
19664 int ij4
19665 = 0;
19666 ij4
19667 < 4;
19668 ++ij4)
19669 {
19670 if (!j4valid
19671 [ij4])
19672 {
19673 continue;
19674 }
19675 _ij4[0]
19676 = ij4;
19677 _ij4[1]
19678 = -1;
19679 for (
19680 int iij4
19681 = ij4
19682 + 1;
19683 iij4
19684 < 4;
19685 ++iij4)
19686 {
19687 if (j4valid
19688 [iij4]
19689 && IKabs(
19690 cj4array
19691 [ij4]
19692 - cj4array
19693 [iij4])
19694 < IKFAST_SOLUTION_THRESH
19695 && IKabs(
19696 sj4array
19697 [ij4]
19698 - sj4array
19699 [iij4])
19700 < IKFAST_SOLUTION_THRESH)
19701 {
19702 j4valid
19703 [iij4]
19704 = false;
19705 _ij4[1]
19706 = iij4;
19707 break;
19708 }
19709 }
19710 j4 = j4array
19711 [ij4];
19712 cj4 = cj4array
19713 [ij4];
19714 sj4 = sj4array
19715 [ij4];
19716
19717 rotationfunction0(
19718 solutions);
19719 }
19720 }
19721 }
19722 } while (
19723 0);
19724 if (bgotonextstatement)
19725 {
19726 bool
19727 bgotonextstatement
19728 = true;
19729 do
19730 {
19731 if (1)
19732 {
19733 bgotonextstatement
19734 = false;
19735 continue; // branch miss [j4]
19736 }
19737 } while (
19738 0);
19739 if (bgotonextstatement)
19740 {
19741 }
19742 }
19743 }
19744 }
19745 else
19746 {
19747 {
19748 IkReal j4array
19749 [1],
19750 cj4array
19751 [1],
19752 sj4array
19753 [1];
19754 bool j4valid
19755 [1]
19756 = {false};
19757 _nj4
19758 = 1;
19759 IkReal
19760 x1486
19761 = ((100.0)
19762 * pp);
19763 IkReal
19764 x1487
19765 = ((110.0)
19766 * pz);
19767 CheckValue<IkReal> x1488 = IKatan2WithCheck(
19768 IkReal((
19769 (((-1.0)
19770 * px
19771 * x1487))
19772 + ((px
19773 * x1486))
19774 + (((21.25)
19775 * px)))),
19776 ((((-1.0)
19777 * py
19778 * x1486))
19779 + (((-1.0)
19780 * (21.25)
19781 * py))
19782 + ((py
19783 * x1487))),
19784 IKFAST_ATAN2_MAGTHRESH);
19785 if (!x1488
19786 .valid)
19787 {
19788 continue;
19789 }
19790 CheckValue<IkReal> x1489 = IKPowWithIntegerCheck(
19791 IKsign((
19792 (((-1.0)
19793 * (9.0)
19794 * (pz
19795 * pz)))
19796 + (((9.0)
19797 * pp)))),
19798 -1);
19799 if (!x1489
19800 .valid)
19801 {
19802 continue;
19803 }
19804 j4array
19805 [0]
19806 = ((-1.5707963267949)
19807 + (x1488
19808 .value)
19809 + (((1.5707963267949)
19810 * (x1489
19811 .value))));
19812 sj4array
19813 [0]
19814 = IKsin(
19815 j4array
19816 [0]);
19817 cj4array
19818 [0]
19819 = IKcos(
19820 j4array
19821 [0]);
19822 if (j4array
19823 [0]
19824 > IKPI)
19825 {
19826 j4array
19827 [0]
19828 -= IK2PI;
19829 }
19830 else if (
19831 j4array
19832 [0]
19833 < -IKPI)
19834 {
19835 j4array
19836 [0]
19837 += IK2PI;
19838 }
19839 j4valid
19840 [0]
19841 = true;
19842 for (
19843 int ij4
19844 = 0;
19845 ij4
19846 < 1;
19847 ++ij4)
19848 {
19849 if (!j4valid
19850 [ij4])
19851 {
19852 continue;
19853 }
19854 _ij4[0]
19855 = ij4;
19856 _ij4[1]
19857 = -1;
19858 for (
19859 int iij4
19860 = ij4
19861 + 1;
19862 iij4
19863 < 1;
19864 ++iij4)
19865 {
19866 if (j4valid
19867 [iij4]
19868 && IKabs(
19869 cj4array
19870 [ij4]
19871 - cj4array
19872 [iij4])
19873 < IKFAST_SOLUTION_THRESH
19874 && IKabs(
19875 sj4array
19876 [ij4]
19877 - sj4array
19878 [iij4])
19879 < IKFAST_SOLUTION_THRESH)
19880 {
19881 j4valid
19882 [iij4]
19883 = false;
19884 _ij4[1]
19885 = iij4;
19886 break;
19887 }
19888 }
19889 j4 = j4array
19890 [ij4];
19891 cj4 = cj4array
19892 [ij4];
19893 sj4 = sj4array
19894 [ij4];
19895 {
19896 IkReal evalcond
19897 [3];
19898 IkReal
19899 x1490
19900 = IKcos(
19901 j4);
19902 IkReal
19903 x1491
19904 = ((1.0)
19905 * x1490);
19906 IkReal
19907 x1492
19908 = IKsin(
19909 j4);
19910 IkReal
19911 x1493
19912 = (px
19913 * x1492);
19914 evalcond
19915 [0]
19916 = ((((-1.0)
19917 * py
19918 * x1492))
19919 + (((-1.0)
19920 * px
19921 * x1491)));
19922 evalcond
19923 [1]
19924 = ((1.51009803921569)
19925 + x1493
19926 + (((-1.0)
19927 * py
19928 * x1491))
19929 + (((-1.0)
19930 * (3.92156862745098)
19931 * pp))
19932 + (((1.32323529411765)
19933 * cj9)));
19934 evalcond
19935 [2]
19936 = ((-0.2125)
19937 + (((1.1)
19938 * pz))
19939 + (((-1.0)
19940 * (1.0)
19941 * pp))
19942 + (((-0.09)
19943 * py
19944 * x1490))
19945 + (((0.09)
19946 * x1493)));
19947 if (IKabs(
19948 evalcond
19949 [0])
19950 > IKFAST_EVALCOND_THRESH
19951 || IKabs(
19952 evalcond
19953 [1])
19954 > IKFAST_EVALCOND_THRESH
19955 || IKabs(
19956 evalcond
19957 [2])
19958 > IKFAST_EVALCOND_THRESH)
19959 {
19960 continue;
19961 }
19962 }
19963
19964 rotationfunction0(
19965 solutions);
19966 }
19967 }
19968 }
19969 }
19970 }
19971 else
19972 {
19973 {
19974 IkReal j4array
19975 [1],
19976 cj4array
19977 [1],
19978 sj4array
19979 [1];
19980 bool j4valid
19981 [1]
19982 = {false};
19983 _nj4
19984 = 1;
19985 IkReal
19986 x1494
19987 = ((1.32323529411765)
19988 * cj9);
19989 IkReal
19990 x1495
19991 = ((3.92156862745098)
19992 * pp);
19993 CheckValue<IkReal> x1496 = IKatan2WithCheck(
19994 IkReal((
19995 (((-1.0)
19996 * (1.51009803921569)
19997 * px))
19998 + (((-1.0)
19999 * px
20000 * x1494))
20001 + ((px
20002 * x1495)))),
20003 ((((-1.0)
20004 * py
20005 * x1495))
20006 + (((1.51009803921569)
20007 * py))
20008 + ((py
20009 * x1494))),
20010 IKFAST_ATAN2_MAGTHRESH);
20011 if (!x1496
20012 .valid)
20013 {
20014 continue;
20015 }
20016 CheckValue<IkReal> x1497 = IKPowWithIntegerCheck(
20017 IKsign((
20018 pp
20019 + (((-1.0)
20020 * (1.0)
20021 * (pz
20022 * pz))))),
20023 -1);
20024 if (!x1497
20025 .valid)
20026 {
20027 continue;
20028 }
20029 j4array
20030 [0]
20031 = ((-1.5707963267949)
20032 + (x1496
20033 .value)
20034 + (((1.5707963267949)
20035 * (x1497
20036 .value))));
20037 sj4array
20038 [0]
20039 = IKsin(
20040 j4array
20041 [0]);
20042 cj4array
20043 [0]
20044 = IKcos(
20045 j4array
20046 [0]);
20047 if (j4array
20048 [0]
20049 > IKPI)
20050 {
20051 j4array
20052 [0]
20053 -= IK2PI;
20054 }
20055 else if (
20056 j4array
20057 [0]
20058 < -IKPI)
20059 {
20060 j4array
20061 [0]
20062 += IK2PI;
20063 }
20064 j4valid
20065 [0]
20066 = true;
20067 for (
20068 int ij4
20069 = 0;
20070 ij4
20071 < 1;
20072 ++ij4)
20073 {
20074 if (!j4valid
20075 [ij4])
20076 {
20077 continue;
20078 }
20079 _ij4[0]
20080 = ij4;
20081 _ij4[1]
20082 = -1;
20083 for (
20084 int iij4
20085 = ij4
20086 + 1;
20087 iij4
20088 < 1;
20089 ++iij4)
20090 {
20091 if (j4valid
20092 [iij4]
20093 && IKabs(
20094 cj4array
20095 [ij4]
20096 - cj4array
20097 [iij4])
20098 < IKFAST_SOLUTION_THRESH
20099 && IKabs(
20100 sj4array
20101 [ij4]
20102 - sj4array
20103 [iij4])
20104 < IKFAST_SOLUTION_THRESH)
20105 {
20106 j4valid
20107 [iij4]
20108 = false;
20109 _ij4[1]
20110 = iij4;
20111 break;
20112 }
20113 }
20114 j4 = j4array
20115 [ij4];
20116 cj4 = cj4array
20117 [ij4];
20118 sj4 = sj4array
20119 [ij4];
20120 {
20121 IkReal evalcond
20122 [3];
20123 IkReal
20124 x1498
20125 = IKcos(
20126 j4);
20127 IkReal
20128 x1499
20129 = ((1.0)
20130 * x1498);
20131 IkReal
20132 x1500
20133 = IKsin(
20134 j4);
20135 IkReal
20136 x1501
20137 = (px
20138 * x1500);
20139 evalcond
20140 [0]
20141 = ((((-1.0)
20142 * px
20143 * x1499))
20144 + (((-1.0)
20145 * py
20146 * x1500)));
20147 evalcond
20148 [1]
20149 = ((1.51009803921569)
20150 + (((-1.0)
20151 * (3.92156862745098)
20152 * pp))
20153 + (((-1.0)
20154 * py
20155 * x1499))
20156 + x1501
20157 + (((1.32323529411765)
20158 * cj9)));
20159 evalcond
20160 [2]
20161 = ((-0.2125)
20162 + (((-0.09)
20163 * py
20164 * x1498))
20165 + (((1.1)
20166 * pz))
20167 + (((-1.0)
20168 * (1.0)
20169 * pp))
20170 + (((0.09)
20171 * x1501)));
20172 if (IKabs(
20173 evalcond
20174 [0])
20175 > IKFAST_EVALCOND_THRESH
20176 || IKabs(
20177 evalcond
20178 [1])
20179 > IKFAST_EVALCOND_THRESH
20180 || IKabs(
20181 evalcond
20182 [2])
20183 > IKFAST_EVALCOND_THRESH)
20184 {
20185 continue;
20186 }
20187 }
20188
20189 rotationfunction0(
20190 solutions);
20191 }
20192 }
20193 }
20194 }
20195 }
20196 } while (0);
20197 if (bgotonextstatement)
20198 {
20199 bool
20200 bgotonextstatement
20201 = true;
20202 do
20203 {
20204 IkReal x1502
20205 = ((1.32323529411765)
20206 * cj9);
20207 IkReal x1503
20208 = ((3.92156862745098)
20209 * pp);
20210 evalcond[0]
20211 = ((IKabs(
20212 py))
20213 + (IKabs(
20214 px)));
20215 evalcond[1]
20216 = (((sj8
20217 * x1503))
20218 + (((-1.0)
20219 * (1.51009803921569)
20220 * sj8))
20221 + (((-1.0)
20222 * sj8
20223 * x1502)));
20224 evalcond[2]
20225 = 0;
20226 evalcond[3]
20227 = ((-1.51009803921569)
20228 + (((-1.0)
20229 * x1502))
20230 + x1503);
20231 evalcond[4]
20232 = ((((-1.0)
20233 * (1.51009803921569)
20234 * cj8))
20235 + ((cj8
20236 * x1503))
20237 + (((-1.0)
20238 * cj8
20239 * x1502)));
20240 evalcond[5]
20241 = ((-0.2125)
20242 + (((1.1)
20243 * pz))
20244 + (((-1.0)
20245 * (1.0)
20246 * pp)));
20247 if (IKabs(
20248 evalcond
20249 [0])
20250 < 0.0000010000000000
20251 && IKabs(
20252 evalcond
20253 [1])
20254 < 0.0000010000000000
20255 && IKabs(
20256 evalcond
20257 [2])
20258 < 0.0000010000000000
20259 && IKabs(
20260 evalcond
20261 [3])
20262 < 0.0000010000000000
20263 && IKabs(
20264 evalcond
20265 [4])
20266 < 0.0000010000000000
20267 && IKabs(
20268 evalcond
20269 [5])
20270 < 0.0000010000000000)
20271 {
20272 bgotonextstatement
20273 = false;
20274 {
20275 IkReal j4array
20276 [4],
20277 cj4array
20278 [4],
20279 sj4array
20280 [4];
20281 bool j4valid
20282 [4]
20283 = {false};
20284 _nj4
20285 = 4;
20286 j4array
20287 [0]
20288 = 0;
20289 sj4array
20290 [0]
20291 = IKsin(
20292 j4array
20293 [0]);
20294 cj4array
20295 [0]
20296 = IKcos(
20297 j4array
20298 [0]);
20299 j4array
20300 [1]
20301 = 1.5707963267949;
20302 sj4array
20303 [1]
20304 = IKsin(
20305 j4array
20306 [1]);
20307 cj4array
20308 [1]
20309 = IKcos(
20310 j4array
20311 [1]);
20312 j4array
20313 [2]
20314 = 3.14159265358979;
20315 sj4array
20316 [2]
20317 = IKsin(
20318 j4array
20319 [2]);
20320 cj4array
20321 [2]
20322 = IKcos(
20323 j4array
20324 [2]);
20325 j4array
20326 [3]
20327 = -1.5707963267949;
20328 sj4array
20329 [3]
20330 = IKsin(
20331 j4array
20332 [3]);
20333 cj4array
20334 [3]
20335 = IKcos(
20336 j4array
20337 [3]);
20338 if (j4array
20339 [0]
20340 > IKPI)
20341 {
20342 j4array
20343 [0]
20344 -= IK2PI;
20345 }
20346 else if (
20347 j4array
20348 [0]
20349 < -IKPI)
20350 {
20351 j4array
20352 [0]
20353 += IK2PI;
20354 }
20355 j4valid
20356 [0]
20357 = true;
20358 if (j4array
20359 [1]
20360 > IKPI)
20361 {
20362 j4array
20363 [1]
20364 -= IK2PI;
20365 }
20366 else if (
20367 j4array
20368 [1]
20369 < -IKPI)
20370 {
20371 j4array
20372 [1]
20373 += IK2PI;
20374 }
20375 j4valid
20376 [1]
20377 = true;
20378 if (j4array
20379 [2]
20380 > IKPI)
20381 {
20382 j4array
20383 [2]
20384 -= IK2PI;
20385 }
20386 else if (
20387 j4array
20388 [2]
20389 < -IKPI)
20390 {
20391 j4array
20392 [2]
20393 += IK2PI;
20394 }
20395 j4valid
20396 [2]
20397 = true;
20398 if (j4array
20399 [3]
20400 > IKPI)
20401 {
20402 j4array
20403 [3]
20404 -= IK2PI;
20405 }
20406 else if (
20407 j4array
20408 [3]
20409 < -IKPI)
20410 {
20411 j4array
20412 [3]
20413 += IK2PI;
20414 }
20415 j4valid
20416 [3]
20417 = true;
20418 for (
20419 int ij4
20420 = 0;
20421 ij4
20422 < 4;
20423 ++ij4)
20424 {
20425 if (!j4valid
20426 [ij4])
20427 {
20428 continue;
20429 }
20430 _ij4[0]
20431 = ij4;
20432 _ij4[1]
20433 = -1;
20434 for (
20435 int iij4
20436 = ij4
20437 + 1;
20438 iij4
20439 < 4;
20440 ++iij4)
20441 {
20442 if (j4valid
20443 [iij4]
20444 && IKabs(
20445 cj4array
20446 [ij4]
20447 - cj4array
20448 [iij4])
20449 < IKFAST_SOLUTION_THRESH
20450 && IKabs(
20451 sj4array
20452 [ij4]
20453 - sj4array
20454 [iij4])
20455 < IKFAST_SOLUTION_THRESH)
20456 {
20457 j4valid
20458 [iij4]
20459 = false;
20460 _ij4[1]
20461 = iij4;
20462 break;
20463 }
20464 }
20465 j4 = j4array
20466 [ij4];
20467 cj4 = cj4array
20468 [ij4];
20469 sj4 = sj4array
20470 [ij4];
20471
20472 rotationfunction0(
20473 solutions);
20474 }
20475 }
20476 }
20477 } while (0);
20478 if (bgotonextstatement)
20479 {
20480 bool
20481 bgotonextstatement
20482 = true;
20483 do
20484 {
20485 if (1)
20486 {
20487 bgotonextstatement
20488 = false;
20489 continue; // branch miss [j4]
20490 }
20491 } while (0);
20492 if (bgotonextstatement)
20493 {
20494 }
20495 }
20496 }
20497 }
20498 }
20499 }
20500 }
20501 }
20502 else
20503 {
20504 {
20505 IkReal j4array[1],
20506 cj4array[1],
20507 sj4array[1];
20508 bool j4valid[1]
20509 = {false};
20510 _nj4 = 1;
20511 IkReal x1504
20512 = ((1.51009803921569)
20513 * px);
20514 IkReal x1505
20515 = ((1.32323529411765)
20516 * cj9);
20517 IkReal x1506
20518 = (px * x1505);
20519 IkReal x1507
20520 = ((3.92156862745098)
20521 * pp);
20522 IkReal x1508
20523 = (px * x1507);
20524 IkReal x1509
20525 = ((1.51009803921569)
20526 * py);
20527 IkReal x1510
20528 = (cj8 * sj8);
20529 IkReal x1511
20530 = cj8 * cj8;
20531 IkReal x1512
20532 = (py * x1505);
20533 IkReal x1513
20534 = ((3.92156862745098)
20535 * cj8 * pp
20536 * sj8);
20537 IkReal x1514
20538 = (py * x1507);
20539 CheckValue<IkReal>
20540 x1515
20541 = IKPowWithIntegerCheck(
20542 IKsign((
20543 (((-1.0)
20544 * (1.0)
20545 * sj8
20546 * (pz
20547 * pz)))
20548 + ((pp
20549 * sj8)))),
20550 -1);
20551 if (!x1515.valid)
20552 {
20553 continue;
20554 }
20555 CheckValue<IkReal> x1516 = IKatan2WithCheck(
20556 IkReal(
20557 ((((-1.0)
20558 * x1506
20559 * x1511))
20560 + (((-1.0)
20561 * x1510
20562 * x1512))
20563 + (((-1.0)
20564 * x1508))
20565 + (((-1.0)
20566 * x1504
20567 * x1511))
20568 + ((x1508
20569 * x1511))
20570 + ((py
20571 * x1513))
20572 + (((-1.0)
20573 * x1509
20574 * x1510))
20575 + x1504
20576 + x1506)),
20577 ((((-1.0)
20578 * x1512))
20579 + ((px * x1513))
20580 + (((-1.0)
20581 * x1506
20582 * x1510))
20583 + x1514
20584 + (((-1.0)
20585 * x1511
20586 * x1514))
20587 + ((x1509
20588 * x1511))
20589 + (((-1.0)
20590 * x1504
20591 * x1510))
20592 + (((-1.0)
20593 * x1509))
20594 + ((x1511
20595 * x1512))),
20596 IKFAST_ATAN2_MAGTHRESH);
20597 if (!x1516.valid)
20598 {
20599 continue;
20600 }
20601 j4array[0]
20602 = ((-1.5707963267949)
20603 + (((1.5707963267949)
20604 * (x1515
20605 .value)))
20606 + (x1516
20607 .value));
20608 sj4array[0] = IKsin(
20609 j4array[0]);
20610 cj4array[0] = IKcos(
20611 j4array[0]);
20612 if (j4array[0] > IKPI)
20613 {
20614 j4array[0] -= IK2PI;
20615 }
20616 else if (
20617 j4array[0]
20618 < -IKPI)
20619 {
20620 j4array[0] += IK2PI;
20621 }
20622 j4valid[0] = true;
20623 for (int ij4 = 0;
20624 ij4 < 1;
20625 ++ij4)
20626 {
20627 if (!j4valid[ij4])
20628 {
20629 continue;
20630 }
20631 _ij4[0] = ij4;
20632 _ij4[1] = -1;
20633 for (int iij4
20634 = ij4 + 1;
20635 iij4 < 1;
20636 ++iij4)
20637 {
20638 if (j4valid[iij4]
20639 && IKabs(
20640 cj4array
20641 [ij4]
20642 - cj4array
20643 [iij4])
20644 < IKFAST_SOLUTION_THRESH
20645 && IKabs(
20646 sj4array
20647 [ij4]
20648 - sj4array
20649 [iij4])
20650 < IKFAST_SOLUTION_THRESH)
20651 {
20652 j4valid[iij4]
20653 = false;
20654 _ij4[1] = iij4;
20655 break;
20656 }
20657 }
20658 j4 = j4array[ij4];
20659 cj4 = cj4array[ij4];
20660 sj4 = sj4array[ij4];
20661 {
20662 IkReal
20663 evalcond[5];
20664 IkReal x1517
20665 = ((1.32323529411765)
20666 * cj9);
20667 IkReal x1518
20668 = ((3.92156862745098)
20669 * pp);
20670 IkReal x1519
20671 = IKsin(j4);
20672 IkReal x1520
20673 = (px
20674 * x1519);
20675 IkReal x1521
20676 = IKcos(j4);
20677 IkReal x1522
20678 = ((1.0)
20679 * x1521);
20680 IkReal x1523
20681 = (py
20682 * x1522);
20683 IkReal x1524
20684 = (px
20685 * x1522);
20686 IkReal x1525
20687 = (py
20688 * x1519);
20689 IkReal x1526
20690 = ((1.0)
20691 * x1525);
20692 IkReal x1527
20693 = (px
20694 * x1521);
20695 IkReal x1528
20696 = (sj8
20697 * x1520);
20698 IkReal x1529
20699 = ((0.09)
20700 * cj8);
20701 evalcond[0]
20702 = ((((-1.0)
20703 * sj8
20704 * x1517))
20705 + (((-1.0)
20706 * x1523))
20707 + ((sj8
20708 * x1518))
20709 + x1520
20710 + (((-1.0)
20711 * (1.51009803921569)
20712 * sj8)));
20713 evalcond[1]
20714 = ((((-1.0)
20715 * (1.51009803921569)
20716 * cj8))
20717 + (((-1.0)
20718 * x1526))
20719 + ((cj8
20720 * x1518))
20721 + (((-1.0)
20722 * x1524))
20723 + (((-1.0)
20724 * cj8
20725 * x1517)));
20726 evalcond[2]
20727 = (((sj8
20728 * x1527))
20729 + (((-1.0)
20730 * cj8
20731 * x1523))
20732 + ((cj8
20733 * x1520))
20734 + ((sj8
20735 * x1525)));
20736 evalcond[3]
20737 = ((-1.51009803921569)
20738 + x1518
20739 + (((-1.0)
20740 * cj8
20741 * x1526))
20742 + (((-1.0)
20743 * sj8
20744 * x1523))
20745 + (((-1.0)
20746 * x1517))
20747 + (((-1.0)
20748 * cj8
20749 * x1524))
20750 + x1528);
20751 evalcond[4]
20752 = ((-0.2125)
20753 + ((x1527
20754 * x1529))
20755 + (((0.09)
20756 * py
20757 * sj8
20758 * x1521))
20759 + ((x1525
20760 * x1529))
20761 + (((1.1)
20762 * pz))
20763 + (((-1.0)
20764 * (1.0)
20765 * pp))
20766 + (((-0.09)
20767 * x1528)));
20768 if (IKabs(evalcond
20769 [0])
20770 > IKFAST_EVALCOND_THRESH
20771 || IKabs(
20772 evalcond
20773 [1])
20774 > IKFAST_EVALCOND_THRESH
20775 || IKabs(
20776 evalcond
20777 [2])
20778 > IKFAST_EVALCOND_THRESH
20779 || IKabs(
20780 evalcond
20781 [3])
20782 > IKFAST_EVALCOND_THRESH
20783 || IKabs(
20784 evalcond
20785 [4])
20786 > IKFAST_EVALCOND_THRESH)
20787 {
20788 continue;
20789 }
20790 }
20791
20792 rotationfunction0(
20793 solutions);
20794 }
20795 }
20796 }
20797 }
20798 }
20799 else
20800 {
20801 {
20802 IkReal j4array[1],
20803 cj4array[1],
20804 sj4array[1];
20805 bool j4valid[1] = {false};
20806 _nj4 = 1;
20807 IkReal x1530
20808 = ((1.51009803921569)
20809 * cj8 * sj8);
20810 IkReal x1531 = cj8 * cj8;
20811 IkReal x1532
20812 = ((1.51009803921569)
20813 * x1531);
20814 IkReal x1533
20815 = ((1.32323529411765)
20816 * cj8 * cj9 * sj8);
20817 IkReal x1534
20818 = ((3.92156862745098)
20819 * cj8 * pp * sj8);
20820 IkReal x1535
20821 = ((1.32323529411765)
20822 * cj9 * x1531);
20823 IkReal x1536
20824 = ((3.92156862745098)
20825 * pp * x1531);
20826 CheckValue<IkReal> x1537
20827 = IKatan2WithCheck(
20828 IkReal((
20829 (((-1.0) * py
20830 * x1536))
20831 + ((px
20832 * x1534))
20833 + (((-1.0)
20834 * px
20835 * x1530))
20836 + (((-1.0)
20837 * px
20838 * x1533))
20839 + ((py
20840 * x1535))
20841 + ((py
20842 * x1532)))),
20843 (((px * x1532))
20844 + (((-1.0) * py
20845 * x1534))
20846 + (((-1.0) * px
20847 * x1536))
20848 + ((px * x1535))
20849 + ((py * x1533))
20850 + ((py
20851 * x1530))),
20852 IKFAST_ATAN2_MAGTHRESH);
20853 if (!x1537.valid)
20854 {
20855 continue;
20856 }
20857 CheckValue<IkReal> x1538
20858 = IKPowWithIntegerCheck(
20859 IKsign(
20860 (((cj8
20861 * (pz
20862 * pz)))
20863 + (((-1.0)
20864 * cj8
20865 * pp)))),
20866 -1);
20867 if (!x1538.valid)
20868 {
20869 continue;
20870 }
20871 j4array[0]
20872 = ((-1.5707963267949)
20873 + (x1537.value)
20874 + (((1.5707963267949)
20875 * (x1538
20876 .value))));
20877 sj4array[0]
20878 = IKsin(j4array[0]);
20879 cj4array[0]
20880 = IKcos(j4array[0]);
20881 if (j4array[0] > IKPI)
20882 {
20883 j4array[0] -= IK2PI;
20884 }
20885 else if (
20886 j4array[0] < -IKPI)
20887 {
20888 j4array[0] += IK2PI;
20889 }
20890 j4valid[0] = true;
20891 for (int ij4 = 0; ij4 < 1;
20892 ++ij4)
20893 {
20894 if (!j4valid[ij4])
20895 {
20896 continue;
20897 }
20898 _ij4[0] = ij4;
20899 _ij4[1] = -1;
20900 for (int iij4 = ij4 + 1;
20901 iij4 < 1;
20902 ++iij4)
20903 {
20904 if (j4valid[iij4]
20905 && IKabs(
20906 cj4array
20907 [ij4]
20908 - cj4array
20909 [iij4])
20910 < IKFAST_SOLUTION_THRESH
20911 && IKabs(
20912 sj4array
20913 [ij4]
20914 - sj4array
20915 [iij4])
20916 < IKFAST_SOLUTION_THRESH)
20917 {
20918 j4valid[iij4]
20919 = false;
20920 _ij4[1] = iij4;
20921 break;
20922 }
20923 }
20924 j4 = j4array[ij4];
20925 cj4 = cj4array[ij4];
20926 sj4 = sj4array[ij4];
20927 {
20928 IkReal evalcond[5];
20929 IkReal x1539
20930 = ((1.32323529411765)
20931 * cj9);
20932 IkReal x1540
20933 = ((3.92156862745098)
20934 * pp);
20935 IkReal x1541
20936 = IKsin(j4);
20937 IkReal x1542
20938 = (px * x1541);
20939 IkReal x1543
20940 = IKcos(j4);
20941 IkReal x1544
20942 = ((1.0) * x1543);
20943 IkReal x1545
20944 = (py * x1544);
20945 IkReal x1546
20946 = (px * x1544);
20947 IkReal x1547
20948 = (py * x1541);
20949 IkReal x1548
20950 = ((1.0) * x1547);
20951 IkReal x1549
20952 = (px * x1543);
20953 IkReal x1550
20954 = (sj8 * x1542);
20955 IkReal x1551
20956 = ((0.09) * cj8);
20957 evalcond[0]
20958 = (x1542
20959 + (((-1.0)
20960 * sj8
20961 * x1539))
20962 + ((sj8
20963 * x1540))
20964 + (((-1.0)
20965 * x1545))
20966 + (((-1.0)
20967 * (1.51009803921569)
20968 * sj8)));
20969 evalcond[1]
20970 = ((((-1.0)
20971 * (1.51009803921569)
20972 * cj8))
20973 + ((cj8
20974 * x1540))
20975 + (((-1.0)
20976 * x1546))
20977 + (((-1.0)
20978 * cj8
20979 * x1539))
20980 + (((-1.0)
20981 * x1548)));
20982 evalcond[2]
20983 = (((sj8 * x1547))
20984 + (((-1.0)
20985 * cj8
20986 * x1545))
20987 + ((cj8
20988 * x1542))
20989 + ((sj8
20990 * x1549)));
20991 evalcond[3]
20992 = ((-1.51009803921569)
20993 + (((-1.0)
20994 * cj8
20995 * x1546))
20996 + x1540 + x1550
20997 + (((-1.0)
20998 * x1539))
20999 + (((-1.0)
21000 * cj8
21001 * x1548))
21002 + (((-1.0)
21003 * sj8
21004 * x1545)));
21005 evalcond[4]
21006 = ((-0.2125)
21007 + (((1.1)
21008 * pz))
21009 + ((x1549
21010 * x1551))
21011 + (((-1.0)
21012 * (1.0)
21013 * pp))
21014 + (((-0.09)
21015 * x1550))
21016 + (((0.09) * py
21017 * sj8
21018 * x1543))
21019 + ((x1547
21020 * x1551)));
21021 if (IKabs(evalcond[0])
21022 > IKFAST_EVALCOND_THRESH
21023 || IKabs(evalcond
21024 [1])
21025 > IKFAST_EVALCOND_THRESH
21026 || IKabs(evalcond
21027 [2])
21028 > IKFAST_EVALCOND_THRESH
21029 || IKabs(evalcond
21030 [3])
21031 > IKFAST_EVALCOND_THRESH
21032 || IKabs(evalcond
21033 [4])
21034 > IKFAST_EVALCOND_THRESH)
21035 {
21036 continue;
21037 }
21038 }
21039
21040 rotationfunction0(
21041 solutions);
21042 }
21043 }
21044 }
21045 }
21046 }
21047 else
21048 {
21049 {
21050 IkReal j4array[1],
21051 cj4array[1], sj4array[1];
21052 bool j4valid[1] = {false};
21053 _nj4 = 1;
21054 IkReal x1552
21055 = ((1.51009803921569)
21056 * cj8);
21057 IkReal x1553
21058 = ((1.51009803921569)
21059 * sj8);
21060 IkReal x1554
21061 = ((1.32323529411765)
21062 * cj8 * cj9);
21063 IkReal x1555
21064 = ((3.92156862745098)
21065 * cj8 * pp);
21066 IkReal x1556
21067 = ((1.32323529411765)
21068 * cj9 * sj8);
21069 IkReal x1557
21070 = ((3.92156862745098) * pp
21071 * sj8);
21072 CheckValue<IkReal> x1558
21073 = IKatan2WithCheck(
21074 IkReal(
21075 ((((-1.0) * py
21076 * x1554))
21077 + (((-1.0) * py
21078 * x1552))
21079 + (((-1.0) * px
21080 * x1557))
21081 + ((py * x1555))
21082 + ((px * x1556))
21083 + ((px
21084 * x1553)))),
21085 ((((-1.0) * px
21086 * x1552))
21087 + ((py * x1557))
21088 + (((-1.0) * py
21089 * x1553))
21090 + (((-1.0) * py
21091 * x1556))
21092 + (((-1.0) * px
21093 * x1554))
21094 + ((px * x1555))),
21095 IKFAST_ATAN2_MAGTHRESH);
21096 if (!x1558.valid)
21097 {
21098 continue;
21099 }
21100 CheckValue<IkReal> x1559
21101 = IKPowWithIntegerCheck(
21102 IKsign((
21103 pp
21104 + (((-1.0) * (1.0)
21105 * (pz
21106 * pz))))),
21107 -1);
21108 if (!x1559.valid)
21109 {
21110 continue;
21111 }
21112 j4array[0]
21113 = ((-1.5707963267949)
21114 + (x1558.value)
21115 + (((1.5707963267949)
21116 * (x1559.value))));
21117 sj4array[0]
21118 = IKsin(j4array[0]);
21119 cj4array[0]
21120 = IKcos(j4array[0]);
21121 if (j4array[0] > IKPI)
21122 {
21123 j4array[0] -= IK2PI;
21124 }
21125 else if (j4array[0] < -IKPI)
21126 {
21127 j4array[0] += IK2PI;
21128 }
21129 j4valid[0] = true;
21130 for (int ij4 = 0; ij4 < 1;
21131 ++ij4)
21132 {
21133 if (!j4valid[ij4])
21134 {
21135 continue;
21136 }
21137 _ij4[0] = ij4;
21138 _ij4[1] = -1;
21139 for (int iij4 = ij4 + 1;
21140 iij4 < 1;
21141 ++iij4)
21142 {
21143 if (j4valid[iij4]
21144 && IKabs(
21145 cj4array[ij4]
21146 - cj4array
21147 [iij4])
21148 < IKFAST_SOLUTION_THRESH
21149 && IKabs(
21150 sj4array[ij4]
21151 - sj4array
21152 [iij4])
21153 < IKFAST_SOLUTION_THRESH)
21154 {
21155 j4valid[iij4] = false;
21156 _ij4[1] = iij4;
21157 break;
21158 }
21159 }
21160 j4 = j4array[ij4];
21161 cj4 = cj4array[ij4];
21162 sj4 = sj4array[ij4];
21163 {
21164 IkReal evalcond[5];
21165 IkReal x1560
21166 = ((1.32323529411765)
21167 * cj9);
21168 IkReal x1561
21169 = ((3.92156862745098)
21170 * pp);
21171 IkReal x1562 = IKsin(j4);
21172 IkReal x1563
21173 = (px * x1562);
21174 IkReal x1564 = IKcos(j4);
21175 IkReal x1565
21176 = ((1.0) * x1564);
21177 IkReal x1566
21178 = (py * x1565);
21179 IkReal x1567
21180 = (px * x1565);
21181 IkReal x1568
21182 = (py * x1562);
21183 IkReal x1569
21184 = ((1.0) * x1568);
21185 IkReal x1570
21186 = (px * x1564);
21187 IkReal x1571
21188 = (sj8 * x1563);
21189 IkReal x1572
21190 = ((0.09) * cj8);
21191 evalcond[0]
21192 = (((sj8 * x1561))
21193 + x1563
21194 + (((-1.0)
21195 * (1.51009803921569)
21196 * sj8))
21197 + (((-1.0) * sj8
21198 * x1560))
21199 + (((-1.0)
21200 * x1566)));
21201 evalcond[1]
21202 = ((((-1.0)
21203 * (1.51009803921569)
21204 * cj8))
21205 + (((-1.0) * cj8
21206 * x1560))
21207 + (((-1.0)
21208 * x1569))
21209 + ((cj8 * x1561))
21210 + (((-1.0)
21211 * x1567)));
21212 evalcond[2]
21213 = ((((-1.0) * cj8
21214 * x1566))
21215 + ((sj8 * x1570))
21216 + ((cj8 * x1563))
21217 + ((sj8 * x1568)));
21218 evalcond[3]
21219 = ((-1.51009803921569)
21220 + (((-1.0)
21221 * x1560))
21222 + (((-1.0) * sj8
21223 * x1566))
21224 + x1571 + x1561
21225 + (((-1.0) * cj8
21226 * x1569))
21227 + (((-1.0) * cj8
21228 * x1567)));
21229 evalcond[4]
21230 = ((-0.2125)
21231 + (((-0.09)
21232 * x1571))
21233 + ((x1570 * x1572))
21234 + (((0.09) * py
21235 * sj8 * x1564))
21236 + (((1.1) * pz))
21237 + (((-1.0) * (1.0)
21238 * pp))
21239 + ((x1568
21240 * x1572)));
21241 if (IKabs(evalcond[0])
21242 > IKFAST_EVALCOND_THRESH
21243 || IKabs(evalcond[1])
21244 > IKFAST_EVALCOND_THRESH
21245 || IKabs(evalcond[2])
21246 > IKFAST_EVALCOND_THRESH
21247 || IKabs(evalcond[3])
21248 > IKFAST_EVALCOND_THRESH
21249 || IKabs(evalcond[4])
21250 > IKFAST_EVALCOND_THRESH)
21251 {
21252 continue;
21253 }
21254 }
21255
21256 rotationfunction0(
21257 solutions);
21258 }
21259 }
21260 }
21261 }
21262 }
21263 } while (0);
21264 if (bgotonextstatement)
21265 {
21266 bool bgotonextstatement = true;
21267 do
21268 {
21269 IkReal x1573 = ((0.3) * cj9);
21270 IkReal x1574 = ((0.045) * sj9);
21271 evalcond[0]
21272 = ((-3.14159265358979)
21273 + (IKfmod(
21274 ((3.14159265358979)
21275 + (IKabs(
21276 ((-3.14159265358979)
21277 + j6)))),
21278 6.28318530717959)));
21279 evalcond[1]
21280 = ((0.39655)
21281 + (((0.0765) * sj9))
21282 + (((0.32595) * cj9))
21283 + (((-1.0) * (1.0) * pp)));
21284 evalcond[2]
21285 = ((-0.55)
21286 + (((-1.0) * (1.0) * pz))
21287 + (((-1.0) * x1573))
21288 + (((-1.0) * x1574)));
21289 evalcond[3]
21290 = ((0.55) + x1574 + x1573 + pz);
21291 if (IKabs(evalcond[0])
21292 < 0.0000010000000000
21293 && IKabs(evalcond[1])
21294 < 0.0000010000000000
21295 && IKabs(evalcond[2])
21296 < 0.0000010000000000
21297 && IKabs(evalcond[3])
21298 < 0.0000010000000000)
21299 {
21300 bgotonextstatement = false;
21301 {
21302 IkReal j4eval[3];
21303 sj6 = 0;
21304 cj6 = -1.0;
21305 j6 = 3.14159265358979;
21306 IkReal x1575
21307 = (pp
21308 + (((-1.0) * (1.0)
21309 * (pz * pz))));
21310 IkReal x1576
21311 = ((1.51009803921569)
21312 * cj8);
21313 IkReal x1577
21314 = ((1.51009803921569)
21315 * sj8);
21316 IkReal x1578
21317 = ((1.32323529411765) * cj8
21318 * cj9);
21319 IkReal x1579
21320 = ((3.92156862745098) * cj8
21321 * pp);
21322 IkReal x1580
21323 = ((1.32323529411765) * cj9
21324 * sj8);
21325 IkReal x1581
21326 = ((3.92156862745098) * pp
21327 * sj8);
21328 j4eval[0] = x1575;
21329 j4eval[1]
21330 = ((IKabs(
21331 ((((-1.0) * py
21332 * x1579))
21333 + ((px * x1577))
21334 + ((py * x1578))
21335 + (((-1.0) * px
21336 * x1581))
21337 + ((py * x1576))
21338 + ((px * x1580)))))
21339 + (IKabs(
21340 (((px * x1578))
21341 + (((-1.0) * py
21342 * x1580))
21343 + ((px * x1576))
21344 + (((-1.0) * px
21345 * x1579))
21346 + ((py * x1581))
21347 + (((-1.0) * py
21348 * x1577))))));
21349 j4eval[2] = IKsign(x1575);
21350 if (IKabs(j4eval[0])
21351 < 0.0000010000000000
21352 || IKabs(j4eval[1])
21353 < 0.0000010000000000
21354 || IKabs(j4eval[2])
21355 < 0.0000010000000000)
21356 {
21357 {
21358 IkReal j4eval[2];
21359 sj6 = 0;
21360 cj6 = -1.0;
21361 j6 = 3.14159265358979;
21362 IkReal x1582
21363 = (((cj8 * (pz * pz)))
21364 + (((-1.0) * (1.0)
21365 * cj8 * pp)));
21366 j4eval[0] = x1582;
21367 j4eval[1] = IKsign(x1582);
21368 if (IKabs(j4eval[0])
21369 < 0.0000010000000000
21370 || IKabs(j4eval[1])
21371 < 0.0000010000000000)
21372 {
21373 {
21374 IkReal j4eval[2];
21375 sj6 = 0;
21376 cj6 = -1.0;
21377 j6 = 3.14159265358979;
21378 IkReal x1583
21379 = ((((-1.0) * (1.0)
21380 * cj8
21381 * (pz * pz)))
21382 + ((cj8 * pp)));
21383 j4eval[0] = x1583;
21384 j4eval[1]
21385 = IKsign(x1583);
21386 if (IKabs(j4eval[0])
21387 < 0.0000010000000000
21388 || IKabs(j4eval[1])
21389 < 0.0000010000000000)
21390 {
21391 {
21392 IkReal evalcond[6];
21393 bool
21394 bgotonextstatement
21395 = true;
21396 do
21397 {
21398 evalcond[0]
21399 = ((-3.14159265358979)
21400 + (IKfmod(
21401 ((3.14159265358979)
21402 + (IKabs((
21403 (-1.5707963267949)
21404 + j8)))),
21405 6.28318530717959)));
21406 if (IKabs(evalcond
21407 [0])
21408 < 0.0000010000000000)
21409 {
21410 bgotonextstatement
21411 = false;
21412 {
21413 IkReal
21414 j4eval[3];
21415 sj6 = 0;
21416 cj6 = -1.0;
21417 j6 = 3.14159265358979;
21418 sj8 = 1.0;
21419 cj8 = 0;
21420 j8 = 1.5707963267949;
21421 IkReal x1584
21422 = (pp
21423 + (((-1.0)
21424 * (1.0)
21425 * (pz
21426 * pz))));
21427 IkReal x1585
21428 = ((13497.0)
21429 * cj9);
21430 IkReal x1586
21431 = ((40000.0)
21432 * pp);
21433 j4eval[0]
21434 = x1584;
21435 j4eval[1]
21436 = ((IKabs((
21437 (((15403.0)
21438 * px))
21439 + ((px
21440 * x1585))
21441 + (((-1.0)
21442 * px
21443 * x1586)))))
21444 + (IKabs((
21445 (((-1.0)
21446 * py
21447 * x1585))
21448 + ((py
21449 * x1586))
21450 + (((-1.0)
21451 * (15403.0)
21452 * py))))));
21453 j4eval[2]
21454 = IKsign(
21455 x1584);
21456 if (IKabs(
21457 j4eval
21458 [0])
21459 < 0.0000010000000000
21460 || IKabs(
21461 j4eval
21462 [1])
21463 < 0.0000010000000000
21464 || IKabs(
21465 j4eval
21466 [2])
21467 < 0.0000010000000000)
21468 {
21469 {
21470 IkReal j4eval
21471 [3];
21472 sj6 = 0;
21473 cj6 = -1.0;
21474 j6 = 3.14159265358979;
21475 sj8 = 1.0;
21476 cj8 = 0;
21477 j8 = 1.5707963267949;
21478 IkReal
21479 x1587
21480 = pz
21481 * pz;
21482 IkReal
21483 x1588
21484 = ((80.0)
21485 * pp);
21486 IkReal
21487 x1589
21488 = ((88.0)
21489 * pz);
21490 j4eval[0]
21491 = (pp
21492 + (((-1.0)
21493 * x1587)));
21494 j4eval[1]
21495 = ((IKabs((
21496 ((py
21497 * x1589))
21498 + (((17.0)
21499 * py))
21500 + ((py
21501 * x1588)))))
21502 + (IKabs((
21503 ((px
21504 * x1588))
21505 + (((17.0)
21506 * px))
21507 + ((px
21508 * x1589))))));
21509 j4eval[2] = IKsign((
21510 (((9.0)
21511 * pp))
21512 + (((-9.0)
21513 * x1587))));
21514 if (IKabs(
21515 j4eval
21516 [0])
21517 < 0.0000010000000000
21518 || IKabs(
21519 j4eval
21520 [1])
21521 < 0.0000010000000000
21522 || IKabs(
21523 j4eval
21524 [2])
21525 < 0.0000010000000000)
21526 {
21527 {
21528 IkReal evalcond
21529 [5];
21530 bool
21531 bgotonextstatement
21532 = true;
21533 do
21534 {
21535 IkReal
21536 x1590
21537 = ((-1.51009803921569)
21538 + (((-1.0)
21539 * (1.32323529411765)
21540 * cj9))
21541 + (((3.92156862745098)
21542 * pp)));
21543 evalcond
21544 [0]
21545 = ((IKabs(
21546 py))
21547 + (IKabs(
21548 px)));
21549 evalcond
21550 [1]
21551 = x1590;
21552 evalcond
21553 [2]
21554 = 0;
21555 evalcond
21556 [3]
21557 = x1590;
21558 evalcond
21559 [4]
21560 = ((-0.2125)
21561 + (((-1.0)
21562 * (1.1)
21563 * pz))
21564 + (((-1.0)
21565 * (1.0)
21566 * pp)));
21567 if (IKabs(
21568 evalcond
21569 [0])
21570 < 0.0000010000000000
21571 && IKabs(
21572 evalcond
21573 [1])
21574 < 0.0000010000000000
21575 && IKabs(
21576 evalcond
21577 [2])
21578 < 0.0000010000000000
21579 && IKabs(
21580 evalcond
21581 [3])
21582 < 0.0000010000000000
21583 && IKabs(
21584 evalcond
21585 [4])
21586 < 0.0000010000000000)
21587 {
21588 bgotonextstatement
21589 = false;
21590 {
21591 IkReal j4array
21592 [4],
21593 cj4array
21594 [4],
21595 sj4array
21596 [4];
21597 bool j4valid
21598 [4]
21599 = {false};
21600 _nj4
21601 = 4;
21602 j4array
21603 [0]
21604 = 0;
21605 sj4array
21606 [0]
21607 = IKsin(
21608 j4array
21609 [0]);
21610 cj4array
21611 [0]
21612 = IKcos(
21613 j4array
21614 [0]);
21615 j4array
21616 [1]
21617 = 1.5707963267949;
21618 sj4array
21619 [1]
21620 = IKsin(
21621 j4array
21622 [1]);
21623 cj4array
21624 [1]
21625 = IKcos(
21626 j4array
21627 [1]);
21628 j4array
21629 [2]
21630 = 3.14159265358979;
21631 sj4array
21632 [2]
21633 = IKsin(
21634 j4array
21635 [2]);
21636 cj4array
21637 [2]
21638 = IKcos(
21639 j4array
21640 [2]);
21641 j4array
21642 [3]
21643 = -1.5707963267949;
21644 sj4array
21645 [3]
21646 = IKsin(
21647 j4array
21648 [3]);
21649 cj4array
21650 [3]
21651 = IKcos(
21652 j4array
21653 [3]);
21654 if (j4array
21655 [0]
21656 > IKPI)
21657 {
21658 j4array
21659 [0]
21660 -= IK2PI;
21661 }
21662 else if (
21663 j4array
21664 [0]
21665 < -IKPI)
21666 {
21667 j4array
21668 [0]
21669 += IK2PI;
21670 }
21671 j4valid
21672 [0]
21673 = true;
21674 if (j4array
21675 [1]
21676 > IKPI)
21677 {
21678 j4array
21679 [1]
21680 -= IK2PI;
21681 }
21682 else if (
21683 j4array
21684 [1]
21685 < -IKPI)
21686 {
21687 j4array
21688 [1]
21689 += IK2PI;
21690 }
21691 j4valid
21692 [1]
21693 = true;
21694 if (j4array
21695 [2]
21696 > IKPI)
21697 {
21698 j4array
21699 [2]
21700 -= IK2PI;
21701 }
21702 else if (
21703 j4array
21704 [2]
21705 < -IKPI)
21706 {
21707 j4array
21708 [2]
21709 += IK2PI;
21710 }
21711 j4valid
21712 [2]
21713 = true;
21714 if (j4array
21715 [3]
21716 > IKPI)
21717 {
21718 j4array
21719 [3]
21720 -= IK2PI;
21721 }
21722 else if (
21723 j4array
21724 [3]
21725 < -IKPI)
21726 {
21727 j4array
21728 [3]
21729 += IK2PI;
21730 }
21731 j4valid
21732 [3]
21733 = true;
21734 for (
21735 int ij4
21736 = 0;
21737 ij4
21738 < 4;
21739 ++ij4)
21740 {
21741 if (!j4valid
21742 [ij4])
21743 {
21744 continue;
21745 }
21746 _ij4[0]
21747 = ij4;
21748 _ij4[1]
21749 = -1;
21750 for (
21751 int iij4
21752 = ij4
21753 + 1;
21754 iij4
21755 < 4;
21756 ++iij4)
21757 {
21758 if (j4valid
21759 [iij4]
21760 && IKabs(
21761 cj4array
21762 [ij4]
21763 - cj4array
21764 [iij4])
21765 < IKFAST_SOLUTION_THRESH
21766 && IKabs(
21767 sj4array
21768 [ij4]
21769 - sj4array
21770 [iij4])
21771 < IKFAST_SOLUTION_THRESH)
21772 {
21773 j4valid
21774 [iij4]
21775 = false;
21776 _ij4[1]
21777 = iij4;
21778 break;
21779 }
21780 }
21781 j4 = j4array
21782 [ij4];
21783 cj4 = cj4array
21784 [ij4];
21785 sj4 = sj4array
21786 [ij4];
21787
21788 rotationfunction0(
21789 solutions);
21790 }
21791 }
21792 }
21793 } while (
21794 0);
21795 if (bgotonextstatement)
21796 {
21797 bool
21798 bgotonextstatement
21799 = true;
21800 do
21801 {
21802 if (1)
21803 {
21804 bgotonextstatement
21805 = false;
21806 continue; // branch miss [j4]
21807 }
21808 } while (
21809 0);
21810 if (bgotonextstatement)
21811 {
21812 }
21813 }
21814 }
21815 }
21816 else
21817 {
21818 {
21819 IkReal j4array
21820 [1],
21821 cj4array
21822 [1],
21823 sj4array
21824 [1];
21825 bool j4valid
21826 [1]
21827 = {false};
21828 _nj4
21829 = 1;
21830 IkReal
21831 x1591
21832 = ((100.0)
21833 * pp);
21834 IkReal
21835 x1592
21836 = ((110.0)
21837 * pz);
21838 CheckValue<IkReal> x1593 = IKatan2WithCheck(
21839 IkReal((
21840 (((-1.0)
21841 * px
21842 * x1591))
21843 + (((-1.0)
21844 * (21.25)
21845 * px))
21846 + (((-1.0)
21847 * px
21848 * x1592)))),
21849 (((py
21850 * x1591))
21851 + ((py
21852 * x1592))
21853 + (((21.25)
21854 * py))),
21855 IKFAST_ATAN2_MAGTHRESH);
21856 if (!x1593
21857 .valid)
21858 {
21859 continue;
21860 }
21861 CheckValue<IkReal> x1594 = IKPowWithIntegerCheck(
21862 IKsign((
21863 (((-1.0)
21864 * (9.0)
21865 * (pz
21866 * pz)))
21867 + (((9.0)
21868 * pp)))),
21869 -1);
21870 if (!x1594
21871 .valid)
21872 {
21873 continue;
21874 }
21875 j4array
21876 [0]
21877 = ((-1.5707963267949)
21878 + (x1593
21879 .value)
21880 + (((1.5707963267949)
21881 * (x1594
21882 .value))));
21883 sj4array
21884 [0]
21885 = IKsin(
21886 j4array
21887 [0]);
21888 cj4array
21889 [0]
21890 = IKcos(
21891 j4array
21892 [0]);
21893 if (j4array
21894 [0]
21895 > IKPI)
21896 {
21897 j4array
21898 [0]
21899 -= IK2PI;
21900 }
21901 else if (
21902 j4array
21903 [0]
21904 < -IKPI)
21905 {
21906 j4array
21907 [0]
21908 += IK2PI;
21909 }
21910 j4valid
21911 [0]
21912 = true;
21913 for (
21914 int ij4
21915 = 0;
21916 ij4
21917 < 1;
21918 ++ij4)
21919 {
21920 if (!j4valid
21921 [ij4])
21922 {
21923 continue;
21924 }
21925 _ij4[0]
21926 = ij4;
21927 _ij4[1]
21928 = -1;
21929 for (
21930 int iij4
21931 = ij4
21932 + 1;
21933 iij4
21934 < 1;
21935 ++iij4)
21936 {
21937 if (j4valid
21938 [iij4]
21939 && IKabs(
21940 cj4array
21941 [ij4]
21942 - cj4array
21943 [iij4])
21944 < IKFAST_SOLUTION_THRESH
21945 && IKabs(
21946 sj4array
21947 [ij4]
21948 - sj4array
21949 [iij4])
21950 < IKFAST_SOLUTION_THRESH)
21951 {
21952 j4valid
21953 [iij4]
21954 = false;
21955 _ij4[1]
21956 = iij4;
21957 break;
21958 }
21959 }
21960 j4 = j4array
21961 [ij4];
21962 cj4 = cj4array
21963 [ij4];
21964 sj4 = sj4array
21965 [ij4];
21966 {
21967 IkReal evalcond
21968 [3];
21969 IkReal
21970 x1595
21971 = IKcos(
21972 j4);
21973 IkReal
21974 x1596
21975 = ((1.0)
21976 * x1595);
21977 IkReal
21978 x1597
21979 = IKsin(
21980 j4);
21981 IkReal
21982 x1598
21983 = (px
21984 * x1597);
21985 evalcond
21986 [0]
21987 = ((((-1.0)
21988 * px
21989 * x1596))
21990 + (((-1.0)
21991 * py
21992 * x1597)));
21993 evalcond
21994 [1]
21995 = ((-1.51009803921569)
21996 + (((-1.0)
21997 * py
21998 * x1596))
21999 + (((-1.0)
22000 * (1.32323529411765)
22001 * cj9))
22002 + (((3.92156862745098)
22003 * pp))
22004 + x1598);
22005 evalcond
22006 [2]
22007 = ((-0.2125)
22008 + (((-1.0)
22009 * (1.1)
22010 * pz))
22011 + (((-0.09)
22012 * x1598))
22013 + (((0.09)
22014 * py
22015 * x1595))
22016 + (((-1.0)
22017 * (1.0)
22018 * pp)));
22019 if (IKabs(
22020 evalcond
22021 [0])
22022 > IKFAST_EVALCOND_THRESH
22023 || IKabs(
22024 evalcond
22025 [1])
22026 > IKFAST_EVALCOND_THRESH
22027 || IKabs(
22028 evalcond
22029 [2])
22030 > IKFAST_EVALCOND_THRESH)
22031 {
22032 continue;
22033 }
22034 }
22035
22036 rotationfunction0(
22037 solutions);
22038 }
22039 }
22040 }
22041 }
22042 }
22043 else
22044 {
22045 {
22046 IkReal j4array
22047 [1],
22048 cj4array
22049 [1],
22050 sj4array
22051 [1];
22052 bool j4valid
22053 [1]
22054 = {false};
22055 _nj4 = 1;
22056 IkReal
22057 x1599
22058 = ((1.32323529411765)
22059 * cj9);
22060 IkReal
22061 x1600
22062 = ((3.92156862745098)
22063 * pp);
22064 CheckValue<IkReal> x1601 = IKPowWithIntegerCheck(
22065 IKsign((
22066 pp
22067 + (((-1.0)
22068 * (1.0)
22069 * (pz
22070 * pz))))),
22071 -1);
22072 if (!x1601
22073 .valid)
22074 {
22075 continue;
22076 }
22077 CheckValue<IkReal> x1602 = IKatan2WithCheck(
22078 IkReal((
22079 ((px
22080 * x1599))
22081 + (((1.51009803921569)
22082 * px))
22083 + (((-1.0)
22084 * px
22085 * x1600)))),
22086 ((((-1.0)
22087 * (1.51009803921569)
22088 * py))
22089 + (((-1.0)
22090 * py
22091 * x1599))
22092 + ((py
22093 * x1600))),
22094 IKFAST_ATAN2_MAGTHRESH);
22095 if (!x1602
22096 .valid)
22097 {
22098 continue;
22099 }
22100 j4array[0]
22101 = ((-1.5707963267949)
22102 + (((1.5707963267949)
22103 * (x1601
22104 .value)))
22105 + (x1602
22106 .value));
22107 sj4array[0] = IKsin(
22108 j4array
22109 [0]);
22110 cj4array[0] = IKcos(
22111 j4array
22112 [0]);
22113 if (j4array
22114 [0]
22115 > IKPI)
22116 {
22117 j4array
22118 [0]
22119 -= IK2PI;
22120 }
22121 else if (
22122 j4array
22123 [0]
22124 < -IKPI)
22125 {
22126 j4array
22127 [0]
22128 += IK2PI;
22129 }
22130 j4valid[0]
22131 = true;
22132 for (
22133 int ij4
22134 = 0;
22135 ij4
22136 < 1;
22137 ++ij4)
22138 {
22139 if (!j4valid
22140 [ij4])
22141 {
22142 continue;
22143 }
22144 _ij4[0]
22145 = ij4;
22146 _ij4[1]
22147 = -1;
22148 for (
22149 int iij4
22150 = ij4
22151 + 1;
22152 iij4
22153 < 1;
22154 ++iij4)
22155 {
22156 if (j4valid
22157 [iij4]
22158 && IKabs(
22159 cj4array
22160 [ij4]
22161 - cj4array
22162 [iij4])
22163 < IKFAST_SOLUTION_THRESH
22164 && IKabs(
22165 sj4array
22166 [ij4]
22167 - sj4array
22168 [iij4])
22169 < IKFAST_SOLUTION_THRESH)
22170 {
22171 j4valid
22172 [iij4]
22173 = false;
22174 _ij4[1]
22175 = iij4;
22176 break;
22177 }
22178 }
22179 j4 = j4array
22180 [ij4];
22181 cj4 = cj4array
22182 [ij4];
22183 sj4 = sj4array
22184 [ij4];
22185 {
22186 IkReal evalcond
22187 [3];
22188 IkReal
22189 x1603
22190 = IKcos(
22191 j4);
22192 IkReal
22193 x1604
22194 = ((1.0)
22195 * x1603);
22196 IkReal
22197 x1605
22198 = IKsin(
22199 j4);
22200 IkReal
22201 x1606
22202 = (px
22203 * x1605);
22204 evalcond
22205 [0]
22206 = ((((-1.0)
22207 * px
22208 * x1604))
22209 + (((-1.0)
22210 * py
22211 * x1605)));
22212 evalcond
22213 [1]
22214 = ((-1.51009803921569)
22215 + (((-1.0)
22216 * (1.32323529411765)
22217 * cj9))
22218 + (((3.92156862745098)
22219 * pp))
22220 + x1606
22221 + (((-1.0)
22222 * py
22223 * x1604)));
22224 evalcond
22225 [2]
22226 = ((-0.2125)
22227 + (((-1.0)
22228 * (1.1)
22229 * pz))
22230 + (((-0.09)
22231 * x1606))
22232 + (((-1.0)
22233 * (1.0)
22234 * pp))
22235 + (((0.09)
22236 * py
22237 * x1603)));
22238 if (IKabs(
22239 evalcond
22240 [0])
22241 > IKFAST_EVALCOND_THRESH
22242 || IKabs(
22243 evalcond
22244 [1])
22245 > IKFAST_EVALCOND_THRESH
22246 || IKabs(
22247 evalcond
22248 [2])
22249 > IKFAST_EVALCOND_THRESH)
22250 {
22251 continue;
22252 }
22253 }
22254
22255 rotationfunction0(
22256 solutions);
22257 }
22258 }
22259 }
22260 }
22261 }
22262 } while (0);
22263 if (bgotonextstatement)
22264 {
22265 bool
22266 bgotonextstatement
22267 = true;
22268 do
22269 {
22270 evalcond[0]
22271 = ((-3.14159265358979)
22272 + (IKfmod(
22273 ((3.14159265358979)
22274 + (IKabs((
22275 (1.5707963267949)
22276 + j8)))),
22277 6.28318530717959)));
22278 if (IKabs(
22279 evalcond
22280 [0])
22281 < 0.0000010000000000)
22282 {
22283 bgotonextstatement
22284 = false;
22285 {
22286 IkReal
22287 j4eval
22288 [3];
22289 sj6 = 0;
22290 cj6 = -1.0;
22291 j6 = 3.14159265358979;
22292 sj8 = -1.0;
22293 cj8 = 0;
22294 j8 = -1.5707963267949;
22295 IkReal x1607
22296 = (pp
22297 + (((-1.0)
22298 * (1.0)
22299 * (pz
22300 * pz))));
22301 IkReal x1608
22302 = ((13497.0)
22303 * cj9);
22304 IkReal x1609
22305 = ((40000.0)
22306 * pp);
22307 j4eval[0]
22308 = x1607;
22309 j4eval[1]
22310 = ((IKabs((
22311 (((-1.0)
22312 * py
22313 * x1609))
22314 + (((15403.0)
22315 * py))
22316 + ((py
22317 * x1608)))))
22318 + (IKabs((
22319 (((-1.0)
22320 * px
22321 * x1608))
22322 + (((-1.0)
22323 * (15403.0)
22324 * px))
22325 + ((px
22326 * x1609))))));
22327 j4eval[2]
22328 = IKsign(
22329 x1607);
22330 if (IKabs(
22331 j4eval
22332 [0])
22333 < 0.0000010000000000
22334 || IKabs(
22335 j4eval
22336 [1])
22337 < 0.0000010000000000
22338 || IKabs(
22339 j4eval
22340 [2])
22341 < 0.0000010000000000)
22342 {
22343 {
22344 IkReal j4eval
22345 [3];
22346 sj6 = 0;
22347 cj6 = -1.0;
22348 j6 = 3.14159265358979;
22349 sj8 = -1.0;
22350 cj8 = 0;
22351 j8 = -1.5707963267949;
22352 IkReal
22353 x1610
22354 = pz
22355 * pz;
22356 IkReal
22357 x1611
22358 = ((80.0)
22359 * pp);
22360 IkReal
22361 x1612
22362 = ((88.0)
22363 * pz);
22364 j4eval
22365 [0]
22366 = (pp
22367 + (((-1.0)
22368 * x1610)));
22369 j4eval
22370 [1]
22371 = ((IKabs((
22372 (((17.0)
22373 * py))
22374 + ((py
22375 * x1612))
22376 + ((py
22377 * x1611)))))
22378 + (IKabs((
22379 (((17.0)
22380 * px))
22381 + ((px
22382 * x1612))
22383 + ((px
22384 * x1611))))));
22385 j4eval[2] = IKsign((
22386 (((-9.0)
22387 * x1610))
22388 + (((9.0)
22389 * pp))));
22390 if (IKabs(
22391 j4eval
22392 [0])
22393 < 0.0000010000000000
22394 || IKabs(
22395 j4eval
22396 [1])
22397 < 0.0000010000000000
22398 || IKabs(
22399 j4eval
22400 [2])
22401 < 0.0000010000000000)
22402 {
22403 {
22404 IkReal evalcond
22405 [5];
22406 bool
22407 bgotonextstatement
22408 = true;
22409 do
22410 {
22411 IkReal
22412 x1613
22413 = ((1.32323529411765)
22414 * cj9);
22415 IkReal
22416 x1614
22417 = ((3.92156862745098)
22418 * pp);
22419 evalcond
22420 [0]
22421 = ((IKabs(
22422 py))
22423 + (IKabs(
22424 px)));
22425 evalcond
22426 [1]
22427 = ((1.51009803921569)
22428 + (((-1.0)
22429 * x1614))
22430 + x1613);
22431 evalcond
22432 [2]
22433 = 0;
22434 evalcond
22435 [3]
22436 = ((-1.51009803921569)
22437 + (((-1.0)
22438 * x1613))
22439 + x1614);
22440 evalcond
22441 [4]
22442 = ((-0.2125)
22443 + (((-1.0)
22444 * (1.1)
22445 * pz))
22446 + (((-1.0)
22447 * (1.0)
22448 * pp)));
22449 if (IKabs(
22450 evalcond
22451 [0])
22452 < 0.0000010000000000
22453 && IKabs(
22454 evalcond
22455 [1])
22456 < 0.0000010000000000
22457 && IKabs(
22458 evalcond
22459 [2])
22460 < 0.0000010000000000
22461 && IKabs(
22462 evalcond
22463 [3])
22464 < 0.0000010000000000
22465 && IKabs(
22466 evalcond
22467 [4])
22468 < 0.0000010000000000)
22469 {
22470 bgotonextstatement
22471 = false;
22472 {
22473 IkReal j4array
22474 [4],
22475 cj4array
22476 [4],
22477 sj4array
22478 [4];
22479 bool j4valid
22480 [4]
22481 = {false};
22482 _nj4
22483 = 4;
22484 j4array
22485 [0]
22486 = 0;
22487 sj4array
22488 [0]
22489 = IKsin(
22490 j4array
22491 [0]);
22492 cj4array
22493 [0]
22494 = IKcos(
22495 j4array
22496 [0]);
22497 j4array
22498 [1]
22499 = 1.5707963267949;
22500 sj4array
22501 [1]
22502 = IKsin(
22503 j4array
22504 [1]);
22505 cj4array
22506 [1]
22507 = IKcos(
22508 j4array
22509 [1]);
22510 j4array
22511 [2]
22512 = 3.14159265358979;
22513 sj4array
22514 [2]
22515 = IKsin(
22516 j4array
22517 [2]);
22518 cj4array
22519 [2]
22520 = IKcos(
22521 j4array
22522 [2]);
22523 j4array
22524 [3]
22525 = -1.5707963267949;
22526 sj4array
22527 [3]
22528 = IKsin(
22529 j4array
22530 [3]);
22531 cj4array
22532 [3]
22533 = IKcos(
22534 j4array
22535 [3]);
22536 if (j4array
22537 [0]
22538 > IKPI)
22539 {
22540 j4array
22541 [0]
22542 -= IK2PI;
22543 }
22544 else if (
22545 j4array
22546 [0]
22547 < -IKPI)
22548 {
22549 j4array
22550 [0]
22551 += IK2PI;
22552 }
22553 j4valid
22554 [0]
22555 = true;
22556 if (j4array
22557 [1]
22558 > IKPI)
22559 {
22560 j4array
22561 [1]
22562 -= IK2PI;
22563 }
22564 else if (
22565 j4array
22566 [1]
22567 < -IKPI)
22568 {
22569 j4array
22570 [1]
22571 += IK2PI;
22572 }
22573 j4valid
22574 [1]
22575 = true;
22576 if (j4array
22577 [2]
22578 > IKPI)
22579 {
22580 j4array
22581 [2]
22582 -= IK2PI;
22583 }
22584 else if (
22585 j4array
22586 [2]
22587 < -IKPI)
22588 {
22589 j4array
22590 [2]
22591 += IK2PI;
22592 }
22593 j4valid
22594 [2]
22595 = true;
22596 if (j4array
22597 [3]
22598 > IKPI)
22599 {
22600 j4array
22601 [3]
22602 -= IK2PI;
22603 }
22604 else if (
22605 j4array
22606 [3]
22607 < -IKPI)
22608 {
22609 j4array
22610 [3]
22611 += IK2PI;
22612 }
22613 j4valid
22614 [3]
22615 = true;
22616 for (
22617 int ij4
22618 = 0;
22619 ij4
22620 < 4;
22621 ++ij4)
22622 {
22623 if (!j4valid
22624 [ij4])
22625 {
22626 continue;
22627 }
22628 _ij4[0]
22629 = ij4;
22630 _ij4[1]
22631 = -1;
22632 for (
22633 int iij4
22634 = ij4
22635 + 1;
22636 iij4
22637 < 4;
22638 ++iij4)
22639 {
22640 if (j4valid
22641 [iij4]
22642 && IKabs(
22643 cj4array
22644 [ij4]
22645 - cj4array
22646 [iij4])
22647 < IKFAST_SOLUTION_THRESH
22648 && IKabs(
22649 sj4array
22650 [ij4]
22651 - sj4array
22652 [iij4])
22653 < IKFAST_SOLUTION_THRESH)
22654 {
22655 j4valid
22656 [iij4]
22657 = false;
22658 _ij4[1]
22659 = iij4;
22660 break;
22661 }
22662 }
22663 j4 = j4array
22664 [ij4];
22665 cj4 = cj4array
22666 [ij4];
22667 sj4 = sj4array
22668 [ij4];
22669
22670 rotationfunction0(
22671 solutions);
22672 }
22673 }
22674 }
22675 } while (
22676 0);
22677 if (bgotonextstatement)
22678 {
22679 bool
22680 bgotonextstatement
22681 = true;
22682 do
22683 {
22684 if (1)
22685 {
22686 bgotonextstatement
22687 = false;
22688 continue; // branch miss [j4]
22689 }
22690 } while (
22691 0);
22692 if (bgotonextstatement)
22693 {
22694 }
22695 }
22696 }
22697 }
22698 else
22699 {
22700 {
22701 IkReal j4array
22702 [1],
22703 cj4array
22704 [1],
22705 sj4array
22706 [1];
22707 bool j4valid
22708 [1]
22709 = {false};
22710 _nj4
22711 = 1;
22712 IkReal
22713 x1615
22714 = ((100.0)
22715 * pp);
22716 IkReal
22717 x1616
22718 = ((110.0)
22719 * pz);
22720 CheckValue<IkReal> x1617 = IKatan2WithCheck(
22721 IkReal((
22722 ((px
22723 * x1616))
22724 + (((21.25)
22725 * px))
22726 + ((px
22727 * x1615)))),
22728 ((((-1.0)
22729 * py
22730 * x1616))
22731 + (((-1.0)
22732 * (21.25)
22733 * py))
22734 + (((-1.0)
22735 * py
22736 * x1615))),
22737 IKFAST_ATAN2_MAGTHRESH);
22738 if (!x1617
22739 .valid)
22740 {
22741 continue;
22742 }
22743 CheckValue<IkReal> x1618 = IKPowWithIntegerCheck(
22744 IKsign((
22745 (((-1.0)
22746 * (9.0)
22747 * (pz
22748 * pz)))
22749 + (((9.0)
22750 * pp)))),
22751 -1);
22752 if (!x1618
22753 .valid)
22754 {
22755 continue;
22756 }
22757 j4array
22758 [0]
22759 = ((-1.5707963267949)
22760 + (x1617
22761 .value)
22762 + (((1.5707963267949)
22763 * (x1618
22764 .value))));
22765 sj4array
22766 [0]
22767 = IKsin(
22768 j4array
22769 [0]);
22770 cj4array
22771 [0]
22772 = IKcos(
22773 j4array
22774 [0]);
22775 if (j4array
22776 [0]
22777 > IKPI)
22778 {
22779 j4array
22780 [0]
22781 -= IK2PI;
22782 }
22783 else if (
22784 j4array
22785 [0]
22786 < -IKPI)
22787 {
22788 j4array
22789 [0]
22790 += IK2PI;
22791 }
22792 j4valid
22793 [0]
22794 = true;
22795 for (
22796 int ij4
22797 = 0;
22798 ij4
22799 < 1;
22800 ++ij4)
22801 {
22802 if (!j4valid
22803 [ij4])
22804 {
22805 continue;
22806 }
22807 _ij4[0]
22808 = ij4;
22809 _ij4[1]
22810 = -1;
22811 for (
22812 int iij4
22813 = ij4
22814 + 1;
22815 iij4
22816 < 1;
22817 ++iij4)
22818 {
22819 if (j4valid
22820 [iij4]
22821 && IKabs(
22822 cj4array
22823 [ij4]
22824 - cj4array
22825 [iij4])
22826 < IKFAST_SOLUTION_THRESH
22827 && IKabs(
22828 sj4array
22829 [ij4]
22830 - sj4array
22831 [iij4])
22832 < IKFAST_SOLUTION_THRESH)
22833 {
22834 j4valid
22835 [iij4]
22836 = false;
22837 _ij4[1]
22838 = iij4;
22839 break;
22840 }
22841 }
22842 j4 = j4array
22843 [ij4];
22844 cj4 = cj4array
22845 [ij4];
22846 sj4 = sj4array
22847 [ij4];
22848 {
22849 IkReal evalcond
22850 [3];
22851 IkReal
22852 x1619
22853 = IKcos(
22854 j4);
22855 IkReal
22856 x1620
22857 = IKsin(
22858 j4);
22859 IkReal
22860 x1621
22861 = (px
22862 * x1620);
22863 IkReal
22864 x1622
22865 = (py
22866 * x1619);
22867 evalcond
22868 [0]
22869 = (((px
22870 * x1619))
22871 + ((py
22872 * x1620)));
22873 evalcond
22874 [1]
22875 = ((1.51009803921569)
22876 + (((-1.0)
22877 * x1622))
22878 + (((-1.0)
22879 * (3.92156862745098)
22880 * pp))
22881 + x1621
22882 + (((1.32323529411765)
22883 * cj9)));
22884 evalcond
22885 [2]
22886 = ((-0.2125)
22887 + (((-1.0)
22888 * (1.1)
22889 * pz))
22890 + (((0.09)
22891 * x1621))
22892 + (((-0.09)
22893 * x1622))
22894 + (((-1.0)
22895 * (1.0)
22896 * pp)));
22897 if (IKabs(
22898 evalcond
22899 [0])
22900 > IKFAST_EVALCOND_THRESH
22901 || IKabs(
22902 evalcond
22903 [1])
22904 > IKFAST_EVALCOND_THRESH
22905 || IKabs(
22906 evalcond
22907 [2])
22908 > IKFAST_EVALCOND_THRESH)
22909 {
22910 continue;
22911 }
22912 }
22913
22914 rotationfunction0(
22915 solutions);
22916 }
22917 }
22918 }
22919 }
22920 }
22921 else
22922 {
22923 {
22924 IkReal j4array
22925 [1],
22926 cj4array
22927 [1],
22928 sj4array
22929 [1];
22930 bool j4valid
22931 [1]
22932 = {false};
22933 _nj4
22934 = 1;
22935 IkReal
22936 x1623
22937 = ((1.32323529411765)
22938 * cj9);
22939 IkReal
22940 x1624
22941 = ((3.92156862745098)
22942 * pp);
22943 CheckValue<IkReal> x1625 = IKatan2WithCheck(
22944 IkReal((
22945 (((-1.0)
22946 * (1.51009803921569)
22947 * px))
22948 + (((-1.0)
22949 * px
22950 * x1623))
22951 + ((px
22952 * x1624)))),
22953 (((py
22954 * x1623))
22955 + (((1.51009803921569)
22956 * py))
22957 + (((-1.0)
22958 * py
22959 * x1624))),
22960 IKFAST_ATAN2_MAGTHRESH);
22961 if (!x1625
22962 .valid)
22963 {
22964 continue;
22965 }
22966 CheckValue<IkReal> x1626 = IKPowWithIntegerCheck(
22967 IKsign((
22968 pp
22969 + (((-1.0)
22970 * (1.0)
22971 * (pz
22972 * pz))))),
22973 -1);
22974 if (!x1626
22975 .valid)
22976 {
22977 continue;
22978 }
22979 j4array
22980 [0]
22981 = ((-1.5707963267949)
22982 + (x1625
22983 .value)
22984 + (((1.5707963267949)
22985 * (x1626
22986 .value))));
22987 sj4array
22988 [0]
22989 = IKsin(
22990 j4array
22991 [0]);
22992 cj4array
22993 [0]
22994 = IKcos(
22995 j4array
22996 [0]);
22997 if (j4array
22998 [0]
22999 > IKPI)
23000 {
23001 j4array
23002 [0]
23003 -= IK2PI;
23004 }
23005 else if (
23006 j4array
23007 [0]
23008 < -IKPI)
23009 {
23010 j4array
23011 [0]
23012 += IK2PI;
23013 }
23014 j4valid
23015 [0]
23016 = true;
23017 for (
23018 int ij4
23019 = 0;
23020 ij4
23021 < 1;
23022 ++ij4)
23023 {
23024 if (!j4valid
23025 [ij4])
23026 {
23027 continue;
23028 }
23029 _ij4[0]
23030 = ij4;
23031 _ij4[1]
23032 = -1;
23033 for (
23034 int iij4
23035 = ij4
23036 + 1;
23037 iij4
23038 < 1;
23039 ++iij4)
23040 {
23041 if (j4valid
23042 [iij4]
23043 && IKabs(
23044 cj4array
23045 [ij4]
23046 - cj4array
23047 [iij4])
23048 < IKFAST_SOLUTION_THRESH
23049 && IKabs(
23050 sj4array
23051 [ij4]
23052 - sj4array
23053 [iij4])
23054 < IKFAST_SOLUTION_THRESH)
23055 {
23056 j4valid
23057 [iij4]
23058 = false;
23059 _ij4[1]
23060 = iij4;
23061 break;
23062 }
23063 }
23064 j4 = j4array
23065 [ij4];
23066 cj4 = cj4array
23067 [ij4];
23068 sj4 = sj4array
23069 [ij4];
23070 {
23071 IkReal evalcond
23072 [3];
23073 IkReal
23074 x1627
23075 = IKcos(
23076 j4);
23077 IkReal
23078 x1628
23079 = IKsin(
23080 j4);
23081 IkReal
23082 x1629
23083 = (px
23084 * x1628);
23085 IkReal
23086 x1630
23087 = (py
23088 * x1627);
23089 evalcond
23090 [0]
23091 = (((py
23092 * x1628))
23093 + ((px
23094 * x1627)));
23095 evalcond
23096 [1]
23097 = ((1.51009803921569)
23098 + (((-1.0)
23099 * (3.92156862745098)
23100 * pp))
23101 + (((-1.0)
23102 * x1630))
23103 + x1629
23104 + (((1.32323529411765)
23105 * cj9)));
23106 evalcond
23107 [2]
23108 = ((-0.2125)
23109 + (((-1.0)
23110 * (1.1)
23111 * pz))
23112 + (((0.09)
23113 * x1629))
23114 + (((-0.09)
23115 * x1630))
23116 + (((-1.0)
23117 * (1.0)
23118 * pp)));
23119 if (IKabs(
23120 evalcond
23121 [0])
23122 > IKFAST_EVALCOND_THRESH
23123 || IKabs(
23124 evalcond
23125 [1])
23126 > IKFAST_EVALCOND_THRESH
23127 || IKabs(
23128 evalcond
23129 [2])
23130 > IKFAST_EVALCOND_THRESH)
23131 {
23132 continue;
23133 }
23134 }
23135
23136 rotationfunction0(
23137 solutions);
23138 }
23139 }
23140 }
23141 }
23142 }
23143 } while (0);
23144 if (bgotonextstatement)
23145 {
23146 bool
23147 bgotonextstatement
23148 = true;
23149 do
23150 {
23151 IkReal x1631
23152 = ((1.32323529411765)
23153 * cj9);
23154 IkReal x1632
23155 = ((3.92156862745098)
23156 * pp);
23157 evalcond[0]
23158 = ((IKabs(
23159 py))
23160 + (IKabs(
23161 px)));
23162 evalcond[1]
23163 = ((((-1.0)
23164 * sj8
23165 * x1631))
23166 + ((sj8
23167 * x1632))
23168 + (((-1.0)
23169 * (1.51009803921569)
23170 * sj8)));
23171 evalcond[2]
23172 = 0;
23173 evalcond[3]
23174 = ((-1.51009803921569)
23175 + (((-1.0)
23176 * x1631))
23177 + x1632);
23178 evalcond[4]
23179 = ((((1.51009803921569)
23180 * cj8))
23181 + (((-1.0)
23182 * cj8
23183 * x1632))
23184 + ((cj8
23185 * x1631)));
23186 evalcond[5]
23187 = ((-0.2125)
23188 + (((-1.0)
23189 * (1.1)
23190 * pz))
23191 + (((-1.0)
23192 * (1.0)
23193 * pp)));
23194 if (IKabs(
23195 evalcond
23196 [0])
23197 < 0.0000010000000000
23198 && IKabs(
23199 evalcond
23200 [1])
23201 < 0.0000010000000000
23202 && IKabs(
23203 evalcond
23204 [2])
23205 < 0.0000010000000000
23206 && IKabs(
23207 evalcond
23208 [3])
23209 < 0.0000010000000000
23210 && IKabs(
23211 evalcond
23212 [4])
23213 < 0.0000010000000000
23214 && IKabs(
23215 evalcond
23216 [5])
23217 < 0.0000010000000000)
23218 {
23219 bgotonextstatement
23220 = false;
23221 {
23222 IkReal j4array
23223 [4],
23224 cj4array
23225 [4],
23226 sj4array
23227 [4];
23228 bool j4valid
23229 [4]
23230 = {false};
23231 _nj4 = 4;
23232 j4array[0]
23233 = 0;
23234 sj4array[0] = IKsin(
23235 j4array
23236 [0]);
23237 cj4array[0] = IKcos(
23238 j4array
23239 [0]);
23240 j4array[1]
23241 = 1.5707963267949;
23242 sj4array[1] = IKsin(
23243 j4array
23244 [1]);
23245 cj4array[1] = IKcos(
23246 j4array
23247 [1]);
23248 j4array[2]
23249 = 3.14159265358979;
23250 sj4array[2] = IKsin(
23251 j4array
23252 [2]);
23253 cj4array[2] = IKcos(
23254 j4array
23255 [2]);
23256 j4array[3]
23257 = -1.5707963267949;
23258 sj4array[3] = IKsin(
23259 j4array
23260 [3]);
23261 cj4array[3] = IKcos(
23262 j4array
23263 [3]);
23264 if (j4array
23265 [0]
23266 > IKPI)
23267 {
23268 j4array
23269 [0]
23270 -= IK2PI;
23271 }
23272 else if (
23273 j4array
23274 [0]
23275 < -IKPI)
23276 {
23277 j4array
23278 [0]
23279 += IK2PI;
23280 }
23281 j4valid[0]
23282 = true;
23283 if (j4array
23284 [1]
23285 > IKPI)
23286 {
23287 j4array
23288 [1]
23289 -= IK2PI;
23290 }
23291 else if (
23292 j4array
23293 [1]
23294 < -IKPI)
23295 {
23296 j4array
23297 [1]
23298 += IK2PI;
23299 }
23300 j4valid[1]
23301 = true;
23302 if (j4array
23303 [2]
23304 > IKPI)
23305 {
23306 j4array
23307 [2]
23308 -= IK2PI;
23309 }
23310 else if (
23311 j4array
23312 [2]
23313 < -IKPI)
23314 {
23315 j4array
23316 [2]
23317 += IK2PI;
23318 }
23319 j4valid[2]
23320 = true;
23321 if (j4array
23322 [3]
23323 > IKPI)
23324 {
23325 j4array
23326 [3]
23327 -= IK2PI;
23328 }
23329 else if (
23330 j4array
23331 [3]
23332 < -IKPI)
23333 {
23334 j4array
23335 [3]
23336 += IK2PI;
23337 }
23338 j4valid[3]
23339 = true;
23340 for (
23341 int ij4
23342 = 0;
23343 ij4
23344 < 4;
23345 ++ij4)
23346 {
23347 if (!j4valid
23348 [ij4])
23349 {
23350 continue;
23351 }
23352 _ij4[0]
23353 = ij4;
23354 _ij4[1]
23355 = -1;
23356 for (
23357 int iij4
23358 = ij4
23359 + 1;
23360 iij4
23361 < 4;
23362 ++iij4)
23363 {
23364 if (j4valid
23365 [iij4]
23366 && IKabs(
23367 cj4array
23368 [ij4]
23369 - cj4array
23370 [iij4])
23371 < IKFAST_SOLUTION_THRESH
23372 && IKabs(
23373 sj4array
23374 [ij4]
23375 - sj4array
23376 [iij4])
23377 < IKFAST_SOLUTION_THRESH)
23378 {
23379 j4valid
23380 [iij4]
23381 = false;
23382 _ij4[1]
23383 = iij4;
23384 break;
23385 }
23386 }
23387 j4 = j4array
23388 [ij4];
23389 cj4 = cj4array
23390 [ij4];
23391 sj4 = sj4array
23392 [ij4];
23393
23394 rotationfunction0(
23395 solutions);
23396 }
23397 }
23398 }
23399 } while (0);
23400 if (bgotonextstatement)
23401 {
23402 bool
23403 bgotonextstatement
23404 = true;
23405 do
23406 {
23407 if (1)
23408 {
23409 bgotonextstatement
23410 = false;
23411 continue; // branch miss [j4]
23412 }
23413 } while (0);
23414 if (bgotonextstatement)
23415 {
23416 }
23417 }
23418 }
23419 }
23420 }
23421 }
23422 else
23423 {
23424 {
23425 IkReal j4array[1],
23426 cj4array[1],
23427 sj4array[1];
23428 bool j4valid[1]
23429 = {false};
23430 _nj4 = 1;
23431 IkReal x1633
23432 = ((1.51009803921569)
23433 * cj8 * sj8);
23434 IkReal x1634
23435 = cj8 * cj8;
23436 IkReal x1635
23437 = ((1.51009803921569)
23438 * x1634);
23439 IkReal x1636
23440 = ((1.32323529411765)
23441 * cj8 * cj9
23442 * sj8);
23443 IkReal x1637
23444 = ((3.92156862745098)
23445 * cj8 * pp
23446 * sj8);
23447 IkReal x1638
23448 = ((1.32323529411765)
23449 * cj9
23450 * x1634);
23451 IkReal x1639
23452 = ((3.92156862745098)
23453 * pp
23454 * x1634);
23455 CheckValue<IkReal> x1640 = IKatan2WithCheck(
23456 IkReal((
23457 (((-1.0)
23458 * px
23459 * x1637))
23460 + ((py
23461 * x1635))
23462 + ((py
23463 * x1638))
23464 + ((px
23465 * x1633))
23466 + ((px
23467 * x1636))
23468 + (((-1.0)
23469 * py
23470 * x1639)))),
23471 (((px * x1638))
23472 + ((py
23473 * x1637))
23474 + (((-1.0) * px
23475 * x1639))
23476 + (((-1.0) * py
23477 * x1633))
23478 + ((px
23479 * x1635))
23480 + (((-1.0) * py
23481 * x1636))),
23482 IKFAST_ATAN2_MAGTHRESH);
23483 if (!x1640.valid)
23484 {
23485 continue;
23486 }
23487 CheckValue<IkReal>
23488 x1641
23489 = IKPowWithIntegerCheck(
23490 IKsign((
23491 (((-1.0)
23492 * (1.0)
23493 * cj8
23494 * (pz
23495 * pz)))
23496 + ((cj8
23497 * pp)))),
23498 -1);
23499 if (!x1641.valid)
23500 {
23501 continue;
23502 }
23503 j4array[0]
23504 = ((-1.5707963267949)
23505 + (x1640
23506 .value)
23507 + (((1.5707963267949)
23508 * (x1641
23509 .value))));
23510 sj4array[0] = IKsin(
23511 j4array[0]);
23512 cj4array[0] = IKcos(
23513 j4array[0]);
23514 if (j4array[0]
23515 > IKPI)
23516 {
23517 j4array[0]
23518 -= IK2PI;
23519 }
23520 else if (
23521 j4array[0]
23522 < -IKPI)
23523 {
23524 j4array[0]
23525 += IK2PI;
23526 }
23527 j4valid[0] = true;
23528 for (int ij4 = 0;
23529 ij4 < 1;
23530 ++ij4)
23531 {
23532 if (!j4valid[ij4])
23533 {
23534 continue;
23535 }
23536 _ij4[0] = ij4;
23537 _ij4[1] = -1;
23538 for (int iij4
23539 = ij4 + 1;
23540 iij4 < 1;
23541 ++iij4)
23542 {
23543 if (j4valid
23544 [iij4]
23545 && IKabs(
23546 cj4array
23547 [ij4]
23548 - cj4array
23549 [iij4])
23550 < IKFAST_SOLUTION_THRESH
23551 && IKabs(
23552 sj4array
23553 [ij4]
23554 - sj4array
23555 [iij4])
23556 < IKFAST_SOLUTION_THRESH)
23557 {
23558 j4valid[iij4]
23559 = false;
23560 _ij4[1]
23561 = iij4;
23562 break;
23563 }
23564 }
23565 j4 = j4array[ij4];
23566 cj4 = cj4array
23567 [ij4];
23568 sj4 = sj4array
23569 [ij4];
23570 {
23571 IkReal
23572 evalcond[5];
23573 IkReal x1642
23574 = ((1.32323529411765)
23575 * cj9);
23576 IkReal x1643
23577 = ((3.92156862745098)
23578 * pp);
23579 IkReal x1644
23580 = IKsin(j4);
23581 IkReal x1645
23582 = (px
23583 * x1644);
23584 IkReal x1646
23585 = IKcos(j4);
23586 IkReal x1647
23587 = ((1.0)
23588 * x1646);
23589 IkReal x1648
23590 = (py
23591 * x1647);
23592 IkReal x1649
23593 = (px
23594 * x1647);
23595 IkReal x1650
23596 = (py
23597 * x1644);
23598 IkReal x1651
23599 = ((1.0)
23600 * x1650);
23601 IkReal x1652
23602 = (cj8 * px
23603 * x1646);
23604 IkReal x1653
23605 = (cj8
23606 * x1650);
23607 IkReal x1654
23608 = (sj8
23609 * x1645);
23610 evalcond[0]
23611 = (((sj8
23612 * x1643))
23613 + (((-1.0)
23614 * sj8
23615 * x1642))
23616 + (((-1.0)
23617 * x1648))
23618 + (((-1.0)
23619 * (1.51009803921569)
23620 * sj8))
23621 + x1645);
23622 evalcond[1]
23623 = (((cj8
23624 * x1642))
23625 + (((1.51009803921569)
23626 * cj8))
23627 + (((-1.0)
23628 * cj8
23629 * x1643))
23630 + (((-1.0)
23631 * x1651))
23632 + (((-1.0)
23633 * x1649)));
23634 evalcond[2]
23635 = ((((-1.0)
23636 * sj8
23637 * x1649))
23638 + (((-1.0)
23639 * sj8
23640 * x1651))
23641 + ((cj8
23642 * x1645))
23643 + (((-1.0)
23644 * cj8
23645 * x1648)));
23646 evalcond[3]
23647 = ((-1.51009803921569)
23648 + (((-1.0)
23649 * sj8
23650 * x1648))
23651 + x1654
23652 + x1652
23653 + x1653
23654 + (((-1.0)
23655 * x1642))
23656 + x1643);
23657 evalcond[4]
23658 = ((-0.2125)
23659 + (((-1.0)
23660 * (1.1)
23661 * pz))
23662 + (((-0.09)
23663 * x1654))
23664 + (((0.09)
23665 * py
23666 * sj8
23667 * x1646))
23668 + (((-1.0)
23669 * (1.0)
23670 * pp))
23671 + (((-0.09)
23672 * x1652))
23673 + (((-0.09)
23674 * x1653)));
23675 if (IKabs(
23676 evalcond
23677 [0])
23678 > IKFAST_EVALCOND_THRESH
23679 || IKabs(
23680 evalcond
23681 [1])
23682 > IKFAST_EVALCOND_THRESH
23683 || IKabs(
23684 evalcond
23685 [2])
23686 > IKFAST_EVALCOND_THRESH
23687 || IKabs(
23688 evalcond
23689 [3])
23690 > IKFAST_EVALCOND_THRESH
23691 || IKabs(
23692 evalcond
23693 [4])
23694 > IKFAST_EVALCOND_THRESH)
23695 {
23696 continue;
23697 }
23698 }
23699
23700 rotationfunction0(
23701 solutions);
23702 }
23703 }
23704 }
23705 }
23706 }
23707 else
23708 {
23709 {
23710 IkReal j4array[1],
23711 cj4array[1],
23712 sj4array[1];
23713 bool j4valid[1]
23714 = {false};
23715 _nj4 = 1;
23716 IkReal x1655
23717 = ((1.51009803921569)
23718 * cj8 * sj8);
23719 IkReal x1656
23720 = cj8 * cj8;
23721 IkReal x1657
23722 = ((1.51009803921569)
23723 * x1656);
23724 IkReal x1658
23725 = ((1.32323529411765)
23726 * cj8 * cj9
23727 * sj8);
23728 IkReal x1659
23729 = ((3.92156862745098)
23730 * cj8 * pp
23731 * sj8);
23732 IkReal x1660
23733 = ((1.32323529411765)
23734 * cj9 * x1656);
23735 IkReal x1661
23736 = ((3.92156862745098)
23737 * pp * x1656);
23738 CheckValue<IkReal> x1662
23739 = IKPowWithIntegerCheck(
23740 IKsign((
23741 ((cj8
23742 * (pz
23743 * pz)))
23744 + (((-1.0)
23745 * cj8
23746 * pp)))),
23747 -1);
23748 if (!x1662.valid)
23749 {
23750 continue;
23751 }
23752 CheckValue<IkReal> x1663
23753 = IKatan2WithCheck(
23754 IkReal((
23755 (((-1.0)
23756 * px
23757 * x1658))
23758 + ((py
23759 * x1661))
23760 + ((px
23761 * x1659))
23762 + (((-1.0)
23763 * py
23764 * x1660))
23765 + (((-1.0)
23766 * px
23767 * x1655))
23768 + (((-1.0)
23769 * py
23770 * x1657)))),
23771 ((((-1.0) * px
23772 * x1660))
23773 + ((py
23774 * x1658))
23775 + (((-1.0) * py
23776 * x1659))
23777 + ((px
23778 * x1661))
23779 + (((-1.0) * px
23780 * x1657))
23781 + ((py
23782 * x1655))),
23783 IKFAST_ATAN2_MAGTHRESH);
23784 if (!x1663.valid)
23785 {
23786 continue;
23787 }
23788 j4array[0]
23789 = ((-1.5707963267949)
23790 + (((1.5707963267949)
23791 * (x1662
23792 .value)))
23793 + (x1663.value));
23794 sj4array[0]
23795 = IKsin(j4array[0]);
23796 cj4array[0]
23797 = IKcos(j4array[0]);
23798 if (j4array[0] > IKPI)
23799 {
23800 j4array[0] -= IK2PI;
23801 }
23802 else if (
23803 j4array[0] < -IKPI)
23804 {
23805 j4array[0] += IK2PI;
23806 }
23807 j4valid[0] = true;
23808 for (int ij4 = 0;
23809 ij4 < 1;
23810 ++ij4)
23811 {
23812 if (!j4valid[ij4])
23813 {
23814 continue;
23815 }
23816 _ij4[0] = ij4;
23817 _ij4[1] = -1;
23818 for (int iij4
23819 = ij4 + 1;
23820 iij4 < 1;
23821 ++iij4)
23822 {
23823 if (j4valid[iij4]
23824 && IKabs(
23825 cj4array
23826 [ij4]
23827 - cj4array
23828 [iij4])
23829 < IKFAST_SOLUTION_THRESH
23830 && IKabs(
23831 sj4array
23832 [ij4]
23833 - sj4array
23834 [iij4])
23835 < IKFAST_SOLUTION_THRESH)
23836 {
23837 j4valid[iij4]
23838 = false;
23839 _ij4[1] = iij4;
23840 break;
23841 }
23842 }
23843 j4 = j4array[ij4];
23844 cj4 = cj4array[ij4];
23845 sj4 = sj4array[ij4];
23846 {
23847 IkReal evalcond[5];
23848 IkReal x1664
23849 = ((1.32323529411765)
23850 * cj9);
23851 IkReal x1665
23852 = ((3.92156862745098)
23853 * pp);
23854 IkReal x1666
23855 = IKsin(j4);
23856 IkReal x1667
23857 = (px * x1666);
23858 IkReal x1668
23859 = IKcos(j4);
23860 IkReal x1669
23861 = ((1.0)
23862 * x1668);
23863 IkReal x1670
23864 = (py * x1669);
23865 IkReal x1671
23866 = (px * x1669);
23867 IkReal x1672
23868 = (py * x1666);
23869 IkReal x1673
23870 = ((1.0)
23871 * x1672);
23872 IkReal x1674
23873 = (cj8 * px
23874 * x1668);
23875 IkReal x1675
23876 = (cj8 * x1672);
23877 IkReal x1676
23878 = (sj8 * x1667);
23879 evalcond[0]
23880 = (((sj8
23881 * x1665))
23882 + (((-1.0)
23883 * sj8
23884 * x1664))
23885 + (((-1.0)
23886 * x1670))
23887 + (((-1.0)
23888 * (1.51009803921569)
23889 * sj8))
23890 + x1667);
23891 evalcond[1]
23892 = ((((-1.0)
23893 * cj8
23894 * x1665))
23895 + (((1.51009803921569)
23896 * cj8))
23897 + (((-1.0)
23898 * x1673))
23899 + ((cj8
23900 * x1664))
23901 + (((-1.0)
23902 * x1671)));
23903 evalcond[2]
23904 = (((cj8
23905 * x1667))
23906 + (((-1.0)
23907 * sj8
23908 * x1673))
23909 + (((-1.0)
23910 * cj8
23911 * x1670))
23912 + (((-1.0)
23913 * sj8
23914 * x1671)));
23915 evalcond[3]
23916 = ((-1.51009803921569)
23917 + x1676
23918 + x1674
23919 + x1675
23920 + (((-1.0)
23921 * x1664))
23922 + (((-1.0)
23923 * sj8
23924 * x1670))
23925 + x1665);
23926 evalcond[4]
23927 = ((-0.2125)
23928 + (((-1.0)
23929 * (1.1)
23930 * pz))
23931 + (((0.09)
23932 * py
23933 * sj8
23934 * x1668))
23935 + (((-0.09)
23936 * x1674))
23937 + (((-0.09)
23938 * x1676))
23939 + (((-1.0)
23940 * (1.0)
23941 * pp))
23942 + (((-0.09)
23943 * x1675)));
23944 if (IKabs(
23945 evalcond[0])
23946 > IKFAST_EVALCOND_THRESH
23947 || IKabs(
23948 evalcond
23949 [1])
23950 > IKFAST_EVALCOND_THRESH
23951 || IKabs(
23952 evalcond
23953 [2])
23954 > IKFAST_EVALCOND_THRESH
23955 || IKabs(
23956 evalcond
23957 [3])
23958 > IKFAST_EVALCOND_THRESH
23959 || IKabs(
23960 evalcond
23961 [4])
23962 > IKFAST_EVALCOND_THRESH)
23963 {
23964 continue;
23965 }
23966 }
23967
23968 rotationfunction0(
23969 solutions);
23970 }
23971 }
23972 }
23973 }
23974 }
23975 else
23976 {
23977 {
23978 IkReal j4array[1],
23979 cj4array[1],
23980 sj4array[1];
23981 bool j4valid[1] = {false};
23982 _nj4 = 1;
23983 IkReal x1677
23984 = ((1.51009803921569)
23985 * cj8);
23986 IkReal x1678
23987 = ((1.51009803921569)
23988 * sj8);
23989 IkReal x1679
23990 = ((1.32323529411765)
23991 * cj8 * cj9);
23992 IkReal x1680
23993 = ((3.92156862745098)
23994 * cj8 * pp);
23995 IkReal x1681
23996 = ((1.32323529411765)
23997 * cj9 * sj8);
23998 IkReal x1682
23999 = ((3.92156862745098)
24000 * pp * sj8);
24001 CheckValue<IkReal> x1683
24002 = IKPowWithIntegerCheck(
24003 IKsign((
24004 pp
24005 + (((-1.0)
24006 * (1.0)
24007 * (pz
24008 * pz))))),
24009 -1);
24010 if (!x1683.valid)
24011 {
24012 continue;
24013 }
24014 CheckValue<IkReal> x1684
24015 = IKatan2WithCheck(
24016 IkReal((
24017 ((py * x1677))
24018 + (((-1.0) * px
24019 * x1682))
24020 + ((px * x1678))
24021 + ((py * x1679))
24022 + ((px * x1681))
24023 + (((-1.0) * py
24024 * x1680)))),
24025 ((((-1.0) * py
24026 * x1678))
24027 + ((py * x1682))
24028 + (((-1.0) * px
24029 * x1680))
24030 + ((px * x1679))
24031 + ((px * x1677))
24032 + (((-1.0) * py
24033 * x1681))),
24034 IKFAST_ATAN2_MAGTHRESH);
24035 if (!x1684.valid)
24036 {
24037 continue;
24038 }
24039 j4array[0]
24040 = ((-1.5707963267949)
24041 + (((1.5707963267949)
24042 * (x1683.value)))
24043 + (x1684.value));
24044 sj4array[0]
24045 = IKsin(j4array[0]);
24046 cj4array[0]
24047 = IKcos(j4array[0]);
24048 if (j4array[0] > IKPI)
24049 {
24050 j4array[0] -= IK2PI;
24051 }
24052 else if (j4array[0] < -IKPI)
24053 {
24054 j4array[0] += IK2PI;
24055 }
24056 j4valid[0] = true;
24057 for (int ij4 = 0; ij4 < 1;
24058 ++ij4)
24059 {
24060 if (!j4valid[ij4])
24061 {
24062 continue;
24063 }
24064 _ij4[0] = ij4;
24065 _ij4[1] = -1;
24066 for (int iij4 = ij4 + 1;
24067 iij4 < 1;
24068 ++iij4)
24069 {
24070 if (j4valid[iij4]
24071 && IKabs(
24072 cj4array[ij4]
24073 - cj4array
24074 [iij4])
24075 < IKFAST_SOLUTION_THRESH
24076 && IKabs(
24077 sj4array[ij4]
24078 - sj4array
24079 [iij4])
24080 < IKFAST_SOLUTION_THRESH)
24081 {
24082 j4valid[iij4] = false;
24083 _ij4[1] = iij4;
24084 break;
24085 }
24086 }
24087 j4 = j4array[ij4];
24088 cj4 = cj4array[ij4];
24089 sj4 = sj4array[ij4];
24090 {
24091 IkReal evalcond[5];
24092 IkReal x1685
24093 = ((1.32323529411765)
24094 * cj9);
24095 IkReal x1686
24096 = ((3.92156862745098)
24097 * pp);
24098 IkReal x1687
24099 = IKsin(j4);
24100 IkReal x1688
24101 = (px * x1687);
24102 IkReal x1689
24103 = IKcos(j4);
24104 IkReal x1690
24105 = ((1.0) * x1689);
24106 IkReal x1691
24107 = (py * x1690);
24108 IkReal x1692
24109 = (px * x1690);
24110 IkReal x1693
24111 = (py * x1687);
24112 IkReal x1694
24113 = ((1.0) * x1693);
24114 IkReal x1695
24115 = (cj8 * px
24116 * x1689);
24117 IkReal x1696
24118 = (cj8 * x1693);
24119 IkReal x1697
24120 = (sj8 * x1688);
24121 evalcond[0]
24122 = (x1688
24123 + (((-1.0)
24124 * x1691))
24125 + (((-1.0) * sj8
24126 * x1685))
24127 + ((sj8 * x1686))
24128 + (((-1.0)
24129 * (1.51009803921569)
24130 * sj8)));
24131 evalcond[1]
24132 = (((cj8 * x1685))
24133 + (((-1.0) * cj8
24134 * x1686))
24135 + (((-1.0)
24136 * x1692))
24137 + (((1.51009803921569)
24138 * cj8))
24139 + (((-1.0)
24140 * x1694)));
24141 evalcond[2]
24142 = ((((-1.0) * cj8
24143 * x1691))
24144 + (((-1.0) * sj8
24145 * x1692))
24146 + ((cj8 * x1688))
24147 + (((-1.0) * sj8
24148 * x1694)));
24149 evalcond[3]
24150 = ((-1.51009803921569)
24151 + x1695 + x1696
24152 + x1697 + x1686
24153 + (((-1.0) * sj8
24154 * x1691))
24155 + (((-1.0)
24156 * x1685)));
24157 evalcond[4]
24158 = ((-0.2125)
24159 + (((-1.0)
24160 * (1.1)
24161 * pz))
24162 + (((-0.09)
24163 * x1697))
24164 + (((-0.09)
24165 * x1696))
24166 + (((-0.09)
24167 * x1695))
24168 + (((0.09) * py
24169 * sj8
24170 * x1689))
24171 + (((-1.0)
24172 * (1.0)
24173 * pp)));
24174 if (IKabs(evalcond[0])
24175 > IKFAST_EVALCOND_THRESH
24176 || IKabs(
24177 evalcond[1])
24178 > IKFAST_EVALCOND_THRESH
24179 || IKabs(
24180 evalcond[2])
24181 > IKFAST_EVALCOND_THRESH
24182 || IKabs(
24183 evalcond[3])
24184 > IKFAST_EVALCOND_THRESH
24185 || IKabs(
24186 evalcond[4])
24187 > IKFAST_EVALCOND_THRESH)
24188 {
24189 continue;
24190 }
24191 }
24192
24193 rotationfunction0(
24194 solutions);
24195 }
24196 }
24197 }
24198 }
24199 }
24200 } while (0);
24201 if (bgotonextstatement)
24202 {
24203 bool bgotonextstatement = true;
24204 do
24205 {
24206 if (1)
24207 {
24208 bgotonextstatement = false;
24209 continue; // branch miss [j4]
24210 }
24211 } while (0);
24212 if (bgotonextstatement)
24213 {
24214 }
24215 }
24216 }
24217 }
24218 }
24219 }
24220 }
24221 else
24222 {
24223 {
24224 IkReal j4array[1], cj4array[1], sj4array[1];
24225 bool j4valid[1] = {false};
24226 _nj4 = 1;
24227 IkReal x1698 = ((0.045) * sj8);
24228 IkReal x1699 = (px * x1698);
24229 IkReal x1700 = ((0.55) * sj6);
24230 IkReal x1701 = ((0.045) * cj6 * cj8);
24231 IkReal x1702 = (py * x1701);
24232 IkReal x1703 = ((0.3) * cj9 * sj6);
24233 IkReal x1704 = ((0.3) * sj8);
24234 IkReal x1705 = (px * sj9);
24235 IkReal x1706 = ((0.045) * sj6);
24236 IkReal x1707 = (py * sj9);
24237 IkReal x1708 = ((0.3) * cj6 * cj8);
24238 IkReal x1709 = (py * x1698);
24239 IkReal x1710 = (px * x1701);
24240 CheckValue<IkReal> x1711 = IKatan2WithCheck(
24241 IkReal(
24242 ((((-1.0) * x1699))
24243 + (((-1.0) * x1704 * x1705))
24244 + (((-1.0) * cj9 * x1702))
24245 + ((py * x1700)) + x1702
24246 + ((x1706 * x1707))
24247 + ((py * x1703)) + ((cj9 * x1699))
24248 + ((x1707 * x1708)))),
24249 (x1709 + (((-1.0) * cj9 * x1709))
24250 + x1710 + ((px * x1700))
24251 + ((px * x1703)) + ((x1705 * x1708))
24252 + ((x1704 * x1707))
24253 + (((-1.0) * cj9 * x1710))
24254 + ((x1705 * x1706))),
24255 IKFAST_ATAN2_MAGTHRESH);
24256 if (!x1711.valid)
24257 {
24258 continue;
24259 }
24260 CheckValue<IkReal> x1712
24261 = IKPowWithIntegerCheck(
24262 IKsign(
24263 (pp
24264 + (((-1.0) * (1.0)
24265 * (pz * pz))))),
24266 -1);
24267 if (!x1712.valid)
24268 {
24269 continue;
24270 }
24271 j4array[0]
24272 = ((-1.5707963267949) + (x1711.value)
24273 + (((1.5707963267949)
24274 * (x1712.value))));
24275 sj4array[0] = IKsin(j4array[0]);
24276 cj4array[0] = IKcos(j4array[0]);
24277 if (j4array[0] > IKPI)
24278 {
24279 j4array[0] -= IK2PI;
24280 }
24281 else if (j4array[0] < -IKPI)
24282 {
24283 j4array[0] += IK2PI;
24284 }
24285 j4valid[0] = true;
24286 for (int ij4 = 0; ij4 < 1; ++ij4)
24287 {
24288 if (!j4valid[ij4])
24289 {
24290 continue;
24291 }
24292 _ij4[0] = ij4;
24293 _ij4[1] = -1;
24294 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24295 {
24296 if (j4valid[iij4]
24297 && IKabs(
24298 cj4array[ij4]
24299 - cj4array[iij4])
24300 < IKFAST_SOLUTION_THRESH
24301 && IKabs(
24302 sj4array[ij4]
24303 - sj4array[iij4])
24304 < IKFAST_SOLUTION_THRESH)
24305 {
24306 j4valid[iij4] = false;
24307 _ij4[1] = iij4;
24308 break;
24309 }
24310 }
24311 j4 = j4array[ij4];
24312 cj4 = cj4array[ij4];
24313 sj4 = sj4array[ij4];
24314 {
24315 IkReal evalcond[6];
24316 IkReal x1713 = ((0.3) * cj9);
24317 IkReal x1714 = ((0.045) * sj9);
24318 IkReal x1715 = (cj6 * pz);
24319 IkReal x1716 = IKcos(j4);
24320 IkReal x1717 = (px * sj6 * x1716);
24321 IkReal x1718 = IKsin(j4);
24322 IkReal x1719 = (py * x1718);
24323 IkReal x1720 = (sj6 * x1719);
24324 IkReal x1721 = ((0.045) * cj9);
24325 IkReal x1722 = (px * x1718);
24326 IkReal x1723 = ((0.3) * sj9);
24327 IkReal x1724 = ((1.0) * x1716);
24328 IkReal x1725 = (py * x1724);
24329 IkReal x1726 = (sj8 * x1716);
24330 IkReal x1727 = (cj6 * cj8);
24331 IkReal x1728 = (px * x1724);
24332 IkReal x1729 = ((1.0) * x1719);
24333 IkReal x1730 = (cj8 * pz * sj6);
24334 IkReal x1731 = (sj8 * x1722);
24335 IkReal x1732 = ((0.09) * cj6 * cj8);
24336 evalcond[0]
24337 = ((-0.55) + (((-1.0) * x1714))
24338 + x1715 + x1717
24339 + (((-1.0) * x1713)) + x1720);
24340 evalcond[1]
24341 = ((((-1.0) * sj8 * x1721))
24342 + (((-1.0) * x1725))
24343 + (((0.045) * sj8)) + x1722
24344 + ((sj8 * x1723)));
24345 evalcond[2]
24346 = (((cj6 * sj8 * x1719))
24347 + ((cj6 * px * x1726))
24348 + (((-1.0) * (1.0) * pz * sj6
24349 * sj8))
24350 + (((-1.0) * cj8 * x1725))
24351 + ((cj8 * x1722)));
24352 evalcond[3]
24353 = ((((0.045) * x1727))
24354 + ((x1723 * x1727))
24355 + ((sj6 * x1714))
24356 + (((0.55) * sj6))
24357 + (((-1.0) * x1721 * x1727))
24358 + (((-1.0) * x1729))
24359 + (((-1.0) * x1728))
24360 + ((sj6 * x1713)));
24361 evalcond[4]
24362 = ((0.045) + (((-1.0) * x1721))
24363 + (((-1.0) * x1727 * x1729))
24364 + (((-1.0) * x1727 * x1728))
24365 + (((-1.0) * sj8 * x1725))
24366 + x1723 + x1731 + x1730);
24367 evalcond[5]
24368 = ((-0.2125)
24369 + (((0.09) * py * x1726))
24370 + (((-0.09) * x1731))
24371 + ((x1719 * x1732))
24372 + (((1.1) * x1717))
24373 + ((px * x1716 * x1732))
24374 + (((1.1) * x1720))
24375 + (((-1.0) * (1.0) * pp))
24376 + (((1.1) * x1715))
24377 + (((-0.09) * x1730)));
24378 if (IKabs(evalcond[0])
24379 > IKFAST_EVALCOND_THRESH
24380 || IKabs(evalcond[1])
24381 > IKFAST_EVALCOND_THRESH
24382 || IKabs(evalcond[2])
24383 > IKFAST_EVALCOND_THRESH
24384 || IKabs(evalcond[3])
24385 > IKFAST_EVALCOND_THRESH
24386 || IKabs(evalcond[4])
24387 > IKFAST_EVALCOND_THRESH
24388 || IKabs(evalcond[5])
24389 > IKFAST_EVALCOND_THRESH)
24390 {
24391 continue;
24392 }
24393 }
24394
24395 rotationfunction0(solutions);
24396 }
24397 }
24398 }
24399 }
24400 }
24401 else
24402 {
24403 {
24404 IkReal j4array[1], cj4array[1], sj4array[1];
24405 bool j4valid[1] = {false};
24406 _nj4 = 1;
24407 IkReal x1733 = (cj8 * sj6);
24408 IkReal x1734 = ((0.55) * cj8);
24409 IkReal x1735 = ((0.55) * cj6);
24410 IkReal x1736 = (px * sj8);
24411 IkReal x1737 = ((0.3) * cj8 * cj9);
24412 IkReal x1738 = ((0.045) * cj8 * sj9);
24413 IkReal x1739 = (cj6 * cj8 * pz);
24414 IkReal x1740 = ((0.3) * cj6 * cj9);
24415 IkReal x1741 = ((0.045) * cj6 * sj9);
24416 IkReal x1742 = (py * sj8);
24417 CheckValue<IkReal> x1743 = IKatan2WithCheck(
24418 IkReal(
24419 ((((-1.0) * pz * x1736))
24420 + ((x1735 * x1736)) + ((py * x1739))
24421 + (((-1.0) * py * x1738))
24422 + ((x1736 * x1740))
24423 + (((-1.0) * py * x1737))
24424 + (((-1.0) * py * x1734))
24425 + ((x1736 * x1741)))),
24426 ((((-1.0) * px * x1738)) + ((px * x1739))
24427 + (((-1.0) * px * x1737))
24428 + (((-1.0) * px * x1734))
24429 + (((-1.0) * x1740 * x1742))
24430 + (((-1.0) * x1735 * x1742))
24431 + (((-1.0) * x1741 * x1742))
24432 + ((pz * x1742))),
24433 IKFAST_ATAN2_MAGTHRESH);
24434 if (!x1743.valid)
24435 {
24436 continue;
24437 }
24438 CheckValue<IkReal> x1744
24439 = IKPowWithIntegerCheck(
24440 IKsign(
24441 (((x1733 * (pz * pz)))
24442 + (((-1.0) * pp * x1733)))),
24443 -1);
24444 if (!x1744.valid)
24445 {
24446 continue;
24447 }
24448 j4array[0]
24449 = ((-1.5707963267949) + (x1743.value)
24450 + (((1.5707963267949) * (x1744.value))));
24451 sj4array[0] = IKsin(j4array[0]);
24452 cj4array[0] = IKcos(j4array[0]);
24453 if (j4array[0] > IKPI)
24454 {
24455 j4array[0] -= IK2PI;
24456 }
24457 else if (j4array[0] < -IKPI)
24458 {
24459 j4array[0] += IK2PI;
24460 }
24461 j4valid[0] = true;
24462 for (int ij4 = 0; ij4 < 1; ++ij4)
24463 {
24464 if (!j4valid[ij4])
24465 {
24466 continue;
24467 }
24468 _ij4[0] = ij4;
24469 _ij4[1] = -1;
24470 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24471 {
24472 if (j4valid[iij4]
24473 && IKabs(cj4array[ij4] - cj4array[iij4])
24474 < IKFAST_SOLUTION_THRESH
24475 && IKabs(sj4array[ij4] - sj4array[iij4])
24476 < IKFAST_SOLUTION_THRESH)
24477 {
24478 j4valid[iij4] = false;
24479 _ij4[1] = iij4;
24480 break;
24481 }
24482 }
24483 j4 = j4array[ij4];
24484 cj4 = cj4array[ij4];
24485 sj4 = sj4array[ij4];
24486 {
24487 IkReal evalcond[6];
24488 IkReal x1745 = ((0.3) * cj9);
24489 IkReal x1746 = ((0.045) * sj9);
24490 IkReal x1747 = (cj6 * pz);
24491 IkReal x1748 = IKcos(j4);
24492 IkReal x1749 = (px * sj6 * x1748);
24493 IkReal x1750 = IKsin(j4);
24494 IkReal x1751 = (py * x1750);
24495 IkReal x1752 = (sj6 * x1751);
24496 IkReal x1753 = ((0.045) * cj9);
24497 IkReal x1754 = (px * x1750);
24498 IkReal x1755 = ((0.3) * sj9);
24499 IkReal x1756 = ((1.0) * x1748);
24500 IkReal x1757 = (py * x1756);
24501 IkReal x1758 = (sj8 * x1748);
24502 IkReal x1759 = (cj6 * cj8);
24503 IkReal x1760 = (px * x1756);
24504 IkReal x1761 = ((1.0) * x1751);
24505 IkReal x1762 = (cj8 * pz * sj6);
24506 IkReal x1763 = (sj8 * x1754);
24507 IkReal x1764 = ((0.09) * cj6 * cj8);
24508 evalcond[0]
24509 = ((-0.55) + (((-1.0) * x1746))
24510 + (((-1.0) * x1745)) + x1747 + x1749
24511 + x1752);
24512 evalcond[1]
24513 = ((((0.045) * sj8))
24514 + (((-1.0) * sj8 * x1753))
24515 + ((sj8 * x1755))
24516 + (((-1.0) * x1757)) + x1754);
24517 evalcond[2]
24518 = (((cj6 * sj8 * x1751))
24519 + (((-1.0) * cj8 * x1757))
24520 + ((cj8 * x1754))
24521 + (((-1.0) * (1.0) * pz * sj6 * sj8))
24522 + ((cj6 * px * x1758)));
24523 evalcond[3]
24524 = ((((-1.0) * x1761))
24525 + (((-1.0) * x1753 * x1759))
24526 + (((0.55) * sj6))
24527 + ((x1755 * x1759)) + ((sj6 * x1746))
24528 + (((0.045) * x1759))
24529 + ((sj6 * x1745))
24530 + (((-1.0) * x1760)));
24531 evalcond[4]
24532 = ((0.045) + (((-1.0) * sj8 * x1757))
24533 + (((-1.0) * x1759 * x1760))
24534 + (((-1.0) * x1759 * x1761)) + x1762
24535 + x1763 + (((-1.0) * x1753))
24536 + x1755);
24537 evalcond[5]
24538 = ((-0.2125) + (((-0.09) * x1762))
24539 + ((px * x1748 * x1764))
24540 + (((1.1) * x1749))
24541 + ((x1751 * x1764))
24542 + (((0.09) * py * x1758))
24543 + (((-1.0) * (1.0) * pp))
24544 + (((1.1) * x1747))
24545 + (((1.1) * x1752))
24546 + (((-0.09) * x1763)));
24547 if (IKabs(evalcond[0])
24548 > IKFAST_EVALCOND_THRESH
24549 || IKabs(evalcond[1])
24550 > IKFAST_EVALCOND_THRESH
24551 || IKabs(evalcond[2])
24552 > IKFAST_EVALCOND_THRESH
24553 || IKabs(evalcond[3])
24554 > IKFAST_EVALCOND_THRESH
24555 || IKabs(evalcond[4])
24556 > IKFAST_EVALCOND_THRESH
24557 || IKabs(evalcond[5])
24558 > IKFAST_EVALCOND_THRESH)
24559 {
24560 continue;
24561 }
24562 }
24563
24564 rotationfunction0(solutions);
24565 }
24566 }
24567 }
24568 }
24569 }
24570 else
24571 {
24572 {
24573 IkReal j4array[1], cj4array[1], sj4array[1];
24574 bool j4valid[1] = {false};
24575 _nj4 = 1;
24576 IkReal x1765 = ((0.3) * cj9);
24577 IkReal x1766 = ((0.045) * sj9);
24578 IkReal x1767 = ((1.0) * cj6 * pz);
24579 IkReal x1768 = ((0.045) * sj6 * sj8);
24580 IkReal x1769 = (px * x1768);
24581 IkReal x1770 = ((0.3) * sj6 * sj8 * sj9);
24582 IkReal x1771 = (py * x1768);
24583 CheckValue<IkReal> x1772 = IKatan2WithCheck(
24584 IkReal(
24585 (((py * x1765)) + ((py * x1766))
24586 + ((cj9 * x1769)) + (((0.55) * py))
24587 + (((-1.0) * x1769))
24588 + (((-1.0) * py * x1767))
24589 + (((-1.0) * px * x1770)))),
24590 (((px * x1766)) + ((py * x1770))
24591 + (((-1.0) * cj9 * x1771)) + (((0.55) * px))
24592 + (((-1.0) * px * x1767)) + ((px * x1765))
24593 + x1771),
24594 IKFAST_ATAN2_MAGTHRESH);
24595 if (!x1772.valid)
24596 {
24597 continue;
24598 }
24599 CheckValue<IkReal> x1773 = IKPowWithIntegerCheck(
24600 IKsign(
24601 ((((-1.0) * (1.0) * sj6 * (pz * pz)))
24602 + ((pp * sj6)))),
24603 -1);
24604 if (!x1773.valid)
24605 {
24606 continue;
24607 }
24608 j4array[0]
24609 = ((-1.5707963267949) + (x1772.value)
24610 + (((1.5707963267949) * (x1773.value))));
24611 sj4array[0] = IKsin(j4array[0]);
24612 cj4array[0] = IKcos(j4array[0]);
24613 if (j4array[0] > IKPI)
24614 {
24615 j4array[0] -= IK2PI;
24616 }
24617 else if (j4array[0] < -IKPI)
24618 {
24619 j4array[0] += IK2PI;
24620 }
24621 j4valid[0] = true;
24622 for (int ij4 = 0; ij4 < 1; ++ij4)
24623 {
24624 if (!j4valid[ij4])
24625 {
24626 continue;
24627 }
24628 _ij4[0] = ij4;
24629 _ij4[1] = -1;
24630 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24631 {
24632 if (j4valid[iij4]
24633 && IKabs(cj4array[ij4] - cj4array[iij4])
24634 < IKFAST_SOLUTION_THRESH
24635 && IKabs(sj4array[ij4] - sj4array[iij4])
24636 < IKFAST_SOLUTION_THRESH)
24637 {
24638 j4valid[iij4] = false;
24639 _ij4[1] = iij4;
24640 break;
24641 }
24642 }
24643 j4 = j4array[ij4];
24644 cj4 = cj4array[ij4];
24645 sj4 = sj4array[ij4];
24646 {
24647 IkReal evalcond[6];
24648 IkReal x1774 = ((0.3) * cj9);
24649 IkReal x1775 = ((0.045) * sj9);
24650 IkReal x1776 = (cj6 * pz);
24651 IkReal x1777 = IKcos(j4);
24652 IkReal x1778 = (px * sj6 * x1777);
24653 IkReal x1779 = IKsin(j4);
24654 IkReal x1780 = (py * x1779);
24655 IkReal x1781 = (sj6 * x1780);
24656 IkReal x1782 = ((0.045) * cj9);
24657 IkReal x1783 = (px * x1779);
24658 IkReal x1784 = ((0.3) * sj9);
24659 IkReal x1785 = ((1.0) * x1777);
24660 IkReal x1786 = (py * x1785);
24661 IkReal x1787 = (sj8 * x1777);
24662 IkReal x1788 = (cj6 * cj8);
24663 IkReal x1789 = (px * x1785);
24664 IkReal x1790 = ((1.0) * x1780);
24665 IkReal x1791 = (cj8 * pz * sj6);
24666 IkReal x1792 = (sj8 * x1783);
24667 IkReal x1793 = ((0.09) * cj6 * cj8);
24668 evalcond[0]
24669 = ((-0.55) + x1781 + (((-1.0) * x1775))
24670 + (((-1.0) * x1774)) + x1778 + x1776);
24671 evalcond[1]
24672 = ((((0.045) * sj8)) + (((-1.0) * x1786))
24673 + x1783 + (((-1.0) * sj8 * x1782))
24674 + ((sj8 * x1784)));
24675 evalcond[2]
24676 = ((((-1.0) * cj8 * x1786))
24677 + ((cj8 * x1783)) + ((cj6 * px * x1787))
24678 + (((-1.0) * (1.0) * pz * sj6 * sj8))
24679 + ((cj6 * sj8 * x1780)));
24680 evalcond[3]
24681 = ((((-1.0) * x1790)) + ((x1784 * x1788))
24682 + (((-1.0) * x1782 * x1788))
24683 + (((0.55) * sj6)) + ((sj6 * x1775))
24684 + (((-1.0) * x1789))
24685 + (((0.045) * x1788)) + ((sj6 * x1774)));
24686 evalcond[4]
24687 = ((0.045) + (((-1.0) * x1782))
24688 + (((-1.0) * x1788 * x1790)) + x1784
24689 + (((-1.0) * x1788 * x1789))
24690 + (((-1.0) * sj8 * x1786)) + x1792
24691 + x1791);
24692 evalcond[5]
24693 = ((-0.2125) + (((1.1) * x1776))
24694 + (((1.1) * x1778)) + ((x1780 * x1793))
24695 + (((1.1) * x1781))
24696 + (((-1.0) * (1.0) * pp))
24697 + (((0.09) * py * x1787))
24698 + (((-0.09) * x1792))
24699 + ((px * x1777 * x1793))
24700 + (((-0.09) * x1791)));
24701 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
24702 || IKabs(evalcond[1])
24703 > IKFAST_EVALCOND_THRESH
24704 || IKabs(evalcond[2])
24705 > IKFAST_EVALCOND_THRESH
24706 || IKabs(evalcond[3])
24707 > IKFAST_EVALCOND_THRESH
24708 || IKabs(evalcond[4])
24709 > IKFAST_EVALCOND_THRESH
24710 || IKabs(evalcond[5])
24711 > IKFAST_EVALCOND_THRESH)
24712 {
24713 continue;
24714 }
24715 }
24716
24717 rotationfunction0(solutions);
24718 }
24719 }
24720 }
24721 }
24722 }
24723 }
24724 }
24725 }
24726 }
24727 else
24728 {
24729 {
24730 IkReal j4array[2], cj4array[2], sj4array[2];
24731 bool j4valid[2] = {false};
24732 _nj4 = 2;
24733 CheckValue<IkReal> x1797 = IKatan2WithCheck(
24734 IkReal(((-1.0) * (((1.0) * py)))),
24735 px,
24736 IKFAST_ATAN2_MAGTHRESH);
24737 if (!x1797.valid)
24738 {
24739 continue;
24740 }
24741 IkReal x1794 = ((-1.0) * (x1797.value));
24742 IkReal x1795 = ((0.045) * sj8);
24743 if ((((py * py) + (px * px))) < -0.00001)
24744 continue;
24745 CheckValue<IkReal> x1798 = IKPowWithIntegerCheck(
24746 IKabs(IKsqrt(((py * py) + (px * px)))), -1);
24747 if (!x1798.valid)
24748 {
24749 continue;
24750 }
24751 if ((((x1798.value)
24752 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24753 + x1795))))
24754 < -1 - IKFAST_SINCOS_THRESH
24755 || (((x1798.value)
24756 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24757 + x1795))))
24758 > 1 + IKFAST_SINCOS_THRESH)
24759 continue;
24760 IkReal x1796 = IKasin(
24761 ((x1798.value)
24762 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24763 + x1795))));
24764 j4array[0] = ((((-1.0) * x1796)) + x1794);
24765 sj4array[0] = IKsin(j4array[0]);
24766 cj4array[0] = IKcos(j4array[0]);
24767 j4array[1] = ((3.14159265358979) + x1794 + x1796);
24768 sj4array[1] = IKsin(j4array[1]);
24769 cj4array[1] = IKcos(j4array[1]);
24770 if (j4array[0] > IKPI)
24771 {
24772 j4array[0] -= IK2PI;
24773 }
24774 else if (j4array[0] < -IKPI)
24775 {
24776 j4array[0] += IK2PI;
24777 }
24778 j4valid[0] = true;
24779 if (j4array[1] > IKPI)
24780 {
24781 j4array[1] -= IK2PI;
24782 }
24783 else if (j4array[1] < -IKPI)
24784 {
24785 j4array[1] += IK2PI;
24786 }
24787 j4valid[1] = true;
24788 for (int ij4 = 0; ij4 < 2; ++ij4)
24789 {
24790 if (!j4valid[ij4])
24791 {
24792 continue;
24793 }
24794 _ij4[0] = ij4;
24795 _ij4[1] = -1;
24796 for (int iij4 = ij4 + 1; iij4 < 2; ++iij4)
24797 {
24798 if (j4valid[iij4]
24799 && IKabs(cj4array[ij4] - cj4array[iij4])
24800 < IKFAST_SOLUTION_THRESH
24801 && IKabs(sj4array[ij4] - sj4array[iij4])
24802 < IKFAST_SOLUTION_THRESH)
24803 {
24804 j4valid[iij4] = false;
24805 _ij4[1] = iij4;
24806 break;
24807 }
24808 }
24809 j4 = j4array[ij4];
24810 cj4 = cj4array[ij4];
24811 sj4 = sj4array[ij4];
24812
24813 {
24814 IkReal j6eval[2];
24815 IkReal x1799 = (cj4 * px);
24816 IkReal x1800 = (cj8 * pz);
24817 IkReal x1801 = (py * sj4);
24818 IkReal x1802 = (cj4 * cj9 * px);
24819 IkReal x1803 = (cj4 * px * sj9);
24820 IkReal x1804 = (cj8 * pz * sj9);
24821 IkReal x1805 = (cj9 * py * sj4);
24822 IkReal x1806 = (py * sj4 * sj9);
24823 IkReal x1807 = ((0.045) * x1800);
24824 j6eval[0]
24825 = ((((-6.66666666666667) * x1802)) + (((-1.0) * x1806))
24826 + (((-12.2222222222222) * x1799))
24827 + (((-1.0) * x1803))
24828 + (((-6.66666666666667) * x1804))
24829 + (((-1.0) * x1800))
24830 + (((-6.66666666666667) * x1805))
24831 + (((-12.2222222222222) * x1801)) + ((cj9 * x1800)));
24832 j6eval[1] = IKsign(
24833 ((((-0.3) * x1804)) + (((-0.3) * x1802))
24834 + (((-0.045) * x1806)) + (((-0.55) * x1799))
24835 + ((cj9 * x1807)) + (((-0.3) * x1805))
24836 + (((-1.0) * x1807)) + (((-0.55) * x1801))
24837 + (((-0.045) * x1803))));
24838 if (IKabs(j6eval[0]) < 0.0000010000000000
24839 || IKabs(j6eval[1]) < 0.0000010000000000)
24840 {
24841 {
24842 IkReal j6eval[2];
24843 IkReal x1808 = (sj8 * (py * py));
24844 IkReal x1809 = cj4 * cj4;
24845 IkReal x1810
24846 = ((((-1.0) * x1808 * x1809)) + ((sj8 * (pz * pz)))
24847 + x1808 + ((sj8 * x1809 * (px * px)))
24848 + (((2.0) * cj4 * px * py * sj4 * sj8)));
24849 j6eval[0] = x1810;
24850 j6eval[1] = IKsign(x1810);
24851 if (IKabs(j6eval[0]) < 0.0000010000000000
24852 || IKabs(j6eval[1]) < 0.0000010000000000)
24853 {
24854 {
24855 IkReal j6eval[2];
24856 IkReal x1811 = (pz * sj8);
24857 IkReal x1812 = (cj9 * pz * sj8);
24858 IkReal x1813 = (pz * sj8 * sj9);
24859 IkReal x1814 = (cj8 * sj8);
24860 IkReal x1815 = (cj4 * px * x1814);
24861 IkReal x1816 = (py * sj4 * x1814);
24862 IkReal x1817 = ((1.0) * cj9);
24863 IkReal x1818 = (cj4 * cj8 * px * sj8 * sj9);
24864 IkReal x1819 = (cj8 * py * sj4 * sj8 * sj9);
24865 IkReal x1820 = ((0.045) * x1815);
24866 IkReal x1821 = ((0.045) * x1816);
24867 j6eval[0]
24868 = ((((-1.0) * x1816 * x1817))
24869 + (((6.66666666666667) * x1818))
24870 + (((-1.0) * x1813)) + x1815 + x1816
24871 + (((-1.0) * x1815 * x1817))
24872 + (((-6.66666666666667) * x1812))
24873 + (((-12.2222222222222) * x1811))
24874 + (((6.66666666666667) * x1819)));
24875 j6eval[1] = IKsign(
24876 ((((-1.0) * cj9 * x1821)) + x1821 + x1820
24877 + (((0.3) * x1819)) + (((-1.0) * cj9 * x1820))
24878 + (((-0.045) * x1813)) + (((0.3) * x1818))
24879 + (((-0.55) * x1811)) + (((-0.3) * x1812))));
24880 if (IKabs(j6eval[0]) < 0.0000010000000000
24881 || IKabs(j6eval[1]) < 0.0000010000000000)
24882 {
24883 {
24884 IkReal evalcond[4];
24885 bool bgotonextstatement = true;
24886 do
24887 {
24888 IkReal x1822
24889 = (((px * sj4))
24890 + (((-1.0) * (1.0) * cj4 * py)));
24891 evalcond[0]
24892 = ((-3.14159265358979)
24893 + (IKfmod(
24894 ((3.14159265358979) + (IKabs(j8))),
24895 6.28318530717959)));
24896 evalcond[1]
24897 = ((0.39655) + (((0.0765) * sj9))
24898 + (((0.32595) * cj9))
24899 + (((-1.0) * (1.0) * pp)));
24900 evalcond[2] = x1822;
24901 evalcond[3] = x1822;
24902 if (IKabs(evalcond[0]) < 0.0000010000000000
24903 && IKabs(evalcond[1]) < 0.0000010000000000
24904 && IKabs(evalcond[2]) < 0.0000010000000000
24905 && IKabs(evalcond[3])
24906 < 0.0000010000000000)
24907 {
24908 bgotonextstatement = false;
24909 {
24910 IkReal j6eval[2];
24911 sj8 = 0;
24912 cj8 = 1.0;
24913 j8 = 0;
24914 IkReal x1823 = (cj9 * pz);
24915 IkReal x1824 = (cj4 * px);
24916 IkReal x1825 = (pp * pz);
24917 IkReal x1826 = (py * sj4);
24918 IkReal x1827 = (cj4 * cj9 * px);
24919 IkReal x1828 = (cj4 * pp * px);
24920 IkReal x1829 = (cj9 * py * sj4);
24921 IkReal x1830 = (pp * py * sj4);
24922 j6eval[0]
24923 = ((((-36.2220411120167) * x1830))
24924 + (((13.9482024812098) * x1826))
24925 + (((5.4333061668025) * x1825))
24926 + x1823
24927 + (((13.9482024812098) * x1824))
24928 + (((-36.2220411120167) * x1828))
24929 + (((12.2222222222222) * x1827))
24930 + (((12.2222222222222) * x1829))
24931 + (((2.92556370551481) * pz)));
24932 j6eval[1] = IKsign(
24933 ((((-3.92156862745098) * x1830))
24934 + (((0.316735294117647) * pz))
24935 + (((-3.92156862745098) * x1828))
24936 + (((1.32323529411765) * x1827))
24937 + (((0.588235294117647) * x1825))
24938 + (((1.51009803921569) * x1824))
24939 + (((1.32323529411765) * x1829))
24940 + (((1.51009803921569) * x1826))
24941 + (((0.108264705882353) * x1823))));
24942 if (IKabs(j6eval[0]) < 0.0000010000000000
24943 || IKabs(j6eval[1])
24944 < 0.0000010000000000)
24945 {
24946 {
24947 IkReal j6eval[2];
24948 sj8 = 0;
24949 cj8 = 1.0;
24950 j8 = 0;
24951 IkReal x1831 = (cj4 * px);
24952 IkReal x1832 = (py * sj4);
24953 IkReal x1833 = (cj9 * pz);
24954 IkReal x1834 = (pz * sj9);
24955 IkReal x1835 = ((1.0) * cj9);
24956 IkReal x1836 = (cj4 * px * sj9);
24957 IkReal x1837 = (py * sj4 * sj9);
24958 IkReal x1838 = ((0.045) * x1831);
24959 IkReal x1839 = ((0.045) * x1832);
24960 j6eval[0]
24961 = ((((-1.0) * (12.2222222222222)
24962 * pz))
24963 + (((-1.0) * x1831 * x1835))
24964 + (((-1.0) * x1834))
24965 + (((-6.66666666666667)
24966 * x1833))
24967 + x1832 + x1831
24968 + (((6.66666666666667)
24969 * x1837))
24970 + (((-1.0) * x1832 * x1835))
24971 + (((6.66666666666667)
24972 * x1836)));
24973 j6eval[1] = IKsign(
24974 ((((-1.0) * (0.55) * pz))
24975 + (((0.3) * x1836))
24976 + (((-1.0) * cj9 * x1839))
24977 + (((-1.0) * cj9 * x1838))
24978 + (((0.3) * x1837))
24979 + (((-0.045) * x1834))
24980 + (((-0.3) * x1833)) + x1839
24981 + x1838));
24982 if (IKabs(j6eval[0])
24983 < 0.0000010000000000
24984 || IKabs(j6eval[1])
24985 < 0.0000010000000000)
24986 {
24987 {
24988 IkReal j6eval[2];
24989 sj8 = 0;
24990 cj8 = 1.0;
24991 j8 = 0;
24992 IkReal x1840 = (cj4 * px);
24993 IkReal x1841 = (cj9 * pz);
24994 IkReal x1842 = (pp * pz);
24995 IkReal x1843 = (py * sj4);
24996 IkReal x1844 = (cj4 * cj9 * px);
24997 IkReal x1845 = (cj4 * pp * px);
24998 IkReal x1846 = (cj9 * py * sj4);
24999 IkReal x1847 = (pp * py * sj4);
25000 j6eval[0]
25001 = ((((-5.4333061668025)
25002 * x1847))
25003 + (((-5.4333061668025)
25004 * x1845))
25005 + (((-36.2220411120167)
25006 * x1842))
25007 + (((-2.92556370551481)
25008 * x1843))
25009 + (((-1.0) * x1846))
25010 + (((-1.0) * x1844))
25011 + (((13.9482024812098)
25012 * pz))
25013 + (((-2.92556370551481)
25014 * x1840))
25015 + (((12.2222222222222)
25016 * x1841)));
25017 j6eval[1] = IKsign(
25018 ((((-0.108264705882353)
25019 * x1844))
25020 + (((1.51009803921569) * pz))
25021 + (((1.32323529411765)
25022 * x1841))
25023 + (((-0.588235294117647)
25024 * x1845))
25025 + (((-0.108264705882353)
25026 * x1846))
25027 + (((-0.316735294117647)
25028 * x1843))
25029 + (((-0.588235294117647)
25030 * x1847))
25031 + (((-0.316735294117647)
25032 * x1840))
25033 + (((-3.92156862745098)
25034 * x1842))));
25035 if (IKabs(j6eval[0])
25036 < 0.0000010000000000
25037 || IKabs(j6eval[1])
25038 < 0.0000010000000000)
25039 {
25040 {
25041 IkReal evalcond[1];
25042 bool bgotonextstatement
25043 = true;
25044 do
25045 {
25046 evalcond[0]
25047 = ((IKabs((
25048 (-3.14159265358979)
25049 + (IKfmod(
25050 ((3.14159265358979)
25051 + j9),
25052 6.28318530717959)))))
25053 + (IKabs(pz)));
25054 if (IKabs(evalcond[0])
25055 < 0.0000010000000000)
25056 {
25057 bgotonextstatement
25058 = false;
25059 {
25060 IkReal j6eval[1];
25061 IkReal x1848
25062 = ((1.0) * py);
25063 sj8 = 0;
25064 cj8 = 1.0;
25065 j8 = 0;
25066 pz = 0;
25067 j9 = 0;
25068 sj9 = 0;
25069 cj9 = 1.0;
25070 pp
25071 = ((py * py)
25072 + (px * px));
25073 npx
25074 = (((py * r10))
25075 + ((px * r00)));
25076 npy
25077 = (((py * r11))
25078 + ((px * r01)));
25079 npz
25080 = (((px * r02))
25081 + ((py * r12)));
25082 rxp0_0
25083 = ((-1.0) * r20
25084 * x1848);
25085 rxp0_1 = (px * r20);
25086 rxp1_0
25087 = ((-1.0) * r21
25088 * x1848);
25089 rxp1_1 = (px * r21);
25090 rxp2_0
25091 = ((-1.0) * r22
25092 * x1848);
25093 rxp2_1 = (px * r22);
25094 j6eval[0]
25095 = (((py * sj4))
25096 + ((cj4 * px)));
25097 if (IKabs(j6eval[0])
25098 < 0.0000010000000000)
25099 {
25100 {
25101 IkReal j6eval[1];
25102 IkReal x1849
25103 = ((1.0) * py);
25104 sj8 = 0;
25105 cj8 = 1.0;
25106 j8 = 0;
25107 pz = 0;
25108 j9 = 0;
25109 sj9 = 0;
25110 cj9 = 1.0;
25111 pp
25112 = ((py * py)
25113 + (px * px));
25114 npx
25115 = (((py * r10))
25116 + ((px
25117 * r00)));
25118 npy
25119 = (((py * r11))
25120 + ((px
25121 * r01)));
25122 npz
25123 = (((px * r02))
25124 + ((py
25125 * r12)));
25126 rxp0_0
25127 = ((-1.0) * r20
25128 * x1849);
25129 rxp0_1 = (px * r20);
25130 rxp1_0
25131 = ((-1.0) * r21
25132 * x1849);
25133 rxp1_1 = (px * r21);
25134 rxp2_0
25135 = ((-1.0) * r22
25136 * x1849);
25137 rxp2_1 = (px * r22);
25138 j6eval[0]
25139 = ((-1.0)
25140 + (((-1.0)
25141 * (1.3840830449827)
25142 * (px
25143 * px)))
25144 + (((-1.0)
25145 * (1.3840830449827)
25146 * (py
25147 * py))));
25148 if (IKabs(j6eval[0])
25149 < 0.0000010000000000)
25150 {
25151 {
25152 IkReal
25153 j6eval[2];
25154 IkReal x1850
25155 = ((1.0)
25156 * py);
25157 sj8 = 0;
25158 cj8 = 1.0;
25159 j8 = 0;
25160 pz = 0;
25161 j9 = 0;
25162 sj9 = 0;
25163 cj9 = 1.0;
25164 pp
25165 = ((py * py)
25166 + (px
25167 * px));
25168 npx
25169 = (((py
25170 * r10))
25171 + ((px
25172 * r00)));
25173 npy
25174 = (((py
25175 * r11))
25176 + ((px
25177 * r01)));
25178 npz
25179 = (((px
25180 * r02))
25181 + ((py
25182 * r12)));
25183 rxp0_0
25184 = ((-1.0)
25185 * r20
25186 * x1850);
25187 rxp0_1
25188 = (px
25189 * r20);
25190 rxp1_0
25191 = ((-1.0)
25192 * r21
25193 * x1850);
25194 rxp1_1
25195 = (px
25196 * r21);
25197 rxp2_0
25198 = ((-1.0)
25199 * r22
25200 * x1850);
25201 rxp2_1
25202 = (px
25203 * r22);
25204 IkReal x1851
25205 = (cj4
25206 * px);
25207 IkReal x1852
25208 = (py
25209 * sj4);
25210 j6eval[0]
25211 = (x1852
25212 + x1851);
25213 j6eval[1]
25214 = ((((-1.0)
25215 * (1.3840830449827)
25216 * cj4
25217 * (px
25218 * px
25219 * px)))
25220 + (((-1.0)
25221 * x1852))
25222 + (((-1.0)
25223 * x1851))
25224 + (((-1.3840830449827)
25225 * x1852
25226 * (px
25227 * px)))
25228 + (((-1.0)
25229 * (1.3840830449827)
25230 * sj4
25231 * (py
25232 * py
25233 * py)))
25234 + (((-1.3840830449827)
25235 * x1851
25236 * (py
25237 * py))));
25238 if (IKabs(
25239 j6eval
25240 [0])
25241 < 0.0000010000000000
25242 || IKabs(
25243 j6eval
25244 [1])
25245 < 0.0000010000000000)
25246 {
25247 {
25248 IkReal
25249 evalcond
25250 [4];
25251 bool
25252 bgotonextstatement
25253 = true;
25254 do
25255 {
25256 evalcond
25257 [0]
25258 = ((IKabs(
25259 py))
25260 + (IKabs(
25261 px)));
25262 evalcond
25263 [1]
25264 = -0.85;
25265 evalcond
25266 [2]
25267 = 0;
25268 evalcond
25269 [3]
25270 = -0.2125;
25271 if (IKabs(
25272 evalcond
25273 [0])
25274 < 0.0000010000000000
25275 && IKabs(
25276 evalcond
25277 [1])
25278 < 0.0000010000000000
25279 && IKabs(
25280 evalcond
25281 [2])
25282 < 0.0000010000000000
25283 && IKabs(
25284 evalcond
25285 [3])
25286 < 0.0000010000000000)
25287 {
25288 bgotonextstatement
25289 = false;
25290 {
25291 IkReal j6array
25292 [2],
25293 cj6array
25294 [2],
25295 sj6array
25296 [2];
25297 bool j6valid
25298 [2]
25299 = {false};
25300 _nj6
25301 = 2;
25302 j6array
25303 [0]
25304 = 2.9927027059803;
25305 sj6array
25306 [0]
25307 = IKsin(
25308 j6array
25309 [0]);
25310 cj6array
25311 [0]
25312 = IKcos(
25313 j6array
25314 [0]);
25315 j6array
25316 [1]
25317 = 6.13429535957009;
25318 sj6array
25319 [1]
25320 = IKsin(
25321 j6array
25322 [1]);
25323 cj6array
25324 [1]
25325 = IKcos(
25326 j6array
25327 [1]);
25328 if (j6array
25329 [0]
25330 > IKPI)
25331 {
25332 j6array
25333 [0]
25334 -= IK2PI;
25335 }
25336 else if (
25337 j6array
25338 [0]
25339 < -IKPI)
25340 {
25341 j6array
25342 [0]
25343 += IK2PI;
25344 }
25345 j6valid
25346 [0]
25347 = true;
25348 if (j6array
25349 [1]
25350 > IKPI)
25351 {
25352 j6array
25353 [1]
25354 -= IK2PI;
25355 }
25356 else if (
25357 j6array
25358 [1]
25359 < -IKPI)
25360 {
25361 j6array
25362 [1]
25363 += IK2PI;
25364 }
25365 j6valid
25366 [1]
25367 = true;
25368 for (
25369 int ij6
25370 = 0;
25371 ij6
25372 < 2;
25373 ++ij6)
25374 {
25375 if (!j6valid
25376 [ij6])
25377 {
25378 continue;
25379 }
25380 _ij6[0]
25381 = ij6;
25382 _ij6[1]
25383 = -1;
25384 for (
25385 int iij6
25386 = ij6
25387 + 1;
25388 iij6
25389 < 2;
25390 ++iij6)
25391 {
25392 if (j6valid
25393 [iij6]
25394 && IKabs(
25395 cj6array
25396 [ij6]
25397 - cj6array
25398 [iij6])
25399 < IKFAST_SOLUTION_THRESH
25400 && IKabs(
25401 sj6array
25402 [ij6]
25403 - sj6array
25404 [iij6])
25405 < IKFAST_SOLUTION_THRESH)
25406 {
25407 j6valid
25408 [iij6]
25409 = false;
25410 _ij6[1]
25411 = iij6;
25412 break;
25413 }
25414 }
25415 j6 = j6array
25416 [ij6];
25417 cj6 = cj6array
25418 [ij6];
25419 sj6 = sj6array
25420 [ij6];
25421 {
25422 IkReal evalcond
25423 [1];
25424 evalcond
25425 [0]
25426 = ((0.85)
25427 * (IKsin(
25428 j6)));
25429 if (IKabs(
25430 evalcond
25431 [0])
25432 > IKFAST_EVALCOND_THRESH)
25433 {
25434 continue;
25435 }
25436 }
25437
25438 rotationfunction0(
25439 solutions);
25440 }
25441 }
25442 }
25443 } while (0);
25444 if (bgotonextstatement)
25445 {
25446 bool
25447 bgotonextstatement
25448 = true;
25449 do
25450 {
25451 evalcond
25452 [0]
25453 = ((IKabs((
25454 (-3.14159265358979)
25455 + (IKfmod(
25456 ((3.14159265358979)
25457 + j4),
25458 6.28318530717959)))))
25459 + (IKabs(
25460 px)));
25461 evalcond
25462 [1]
25463 = -0.85;
25464 evalcond
25465 [2]
25466 = 0;
25467 evalcond
25468 [3]
25469 = ((-0.2125)
25470 + (((-1.0)
25471 * (1.0)
25472 * (py
25473 * py))));
25474 if (IKabs(
25475 evalcond
25476 [0])
25477 < 0.0000010000000000
25478 && IKabs(
25479 evalcond
25480 [1])
25481 < 0.0000010000000000
25482 && IKabs(
25483 evalcond
25484 [2])
25485 < 0.0000010000000000
25486 && IKabs(
25487 evalcond
25488 [3])
25489 < 0.0000010000000000)
25490 {
25491 bgotonextstatement
25492 = false;
25493 {
25494 IkReal j6eval
25495 [1];
25496 IkReal
25497 x1853
25498 = ((1.0)
25499 * py);
25500 sj8 = 0;
25501 cj8 = 1.0;
25502 j8 = 0;
25503 pz = 0;
25504 j9 = 0;
25505 sj9 = 0;
25506 cj9 = 1.0;
25507 pp = py
25508 * py;
25509 npx
25510 = (py
25511 * r10);
25512 npy
25513 = (py
25514 * r11);
25515 npz
25516 = (py
25517 * r12);
25518 rxp0_0
25519 = ((-1.0)
25520 * r20
25521 * x1853);
25522 rxp0_1
25523 = 0;
25524 rxp1_0
25525 = ((-1.0)
25526 * r21
25527 * x1853);
25528 rxp1_1
25529 = 0;
25530 rxp2_0
25531 = ((-1.0)
25532 * r22
25533 * x1853);
25534 rxp2_1
25535 = 0;
25536 px = 0;
25537 j4 = 0;
25538 sj4 = 0;
25539 cj4 = 1.0;
25540 rxp0_2
25541 = (py
25542 * r00);
25543 rxp1_2
25544 = (py
25545 * r01);
25546 rxp2_2
25547 = (py
25548 * r02);
25549 j6eval
25550 [0]
25551 = ((1.0)
25552 + (((1.91568587540858)
25553 * (py
25554 * py
25555 * py
25556 * py)))
25557 + (((-1.0)
25558 * (2.64633970947792)
25559 * (py
25560 * py))));
25561 if (IKabs(
25562 j6eval
25563 [0])
25564 < 0.0000010000000000)
25565 {
25566 continue; // no branches [j6]
25567 }
25568 else
25569 {
25570 {
25571 IkReal j6array
25572 [2],
25573 cj6array
25574 [2],
25575 sj6array
25576 [2];
25577 bool j6valid
25578 [2]
25579 = {false};
25580 _nj6
25581 = 2;
25582 IkReal
25583 x1854
25584 = py
25585 * py;
25586 CheckValue<IkReal> x1856 = IKatan2WithCheck(
25587 IkReal((
25588 (-0.425)
25589 + (((-0.588235294117647)
25590 * x1854)))),
25591 ((-2.83333333333333)
25592 + (((3.92156862745098)
25593 * x1854))),
25594 IKFAST_ATAN2_MAGTHRESH);
25595 if (!x1856
25596 .valid)
25597 {
25598 continue;
25599 }
25600 IkReal
25601 x1855
25602 = ((-1.0)
25603 * (x1856
25604 .value));
25605 j6array
25606 [0]
25607 = x1855;
25608 sj6array
25609 [0]
25610 = IKsin(
25611 j6array
25612 [0]);
25613 cj6array
25614 [0]
25615 = IKcos(
25616 j6array
25617 [0]);
25618 j6array
25619 [1]
25620 = ((3.14159265358979)
25621 + x1855);
25622 sj6array
25623 [1]
25624 = IKsin(
25625 j6array
25626 [1]);
25627 cj6array
25628 [1]
25629 = IKcos(
25630 j6array
25631 [1]);
25632 if (j6array
25633 [0]
25634 > IKPI)
25635 {
25636 j6array
25637 [0]
25638 -= IK2PI;
25639 }
25640 else if (
25641 j6array
25642 [0]
25643 < -IKPI)
25644 {
25645 j6array
25646 [0]
25647 += IK2PI;
25648 }
25649 j6valid
25650 [0]
25651 = true;
25652 if (j6array
25653 [1]
25654 > IKPI)
25655 {
25656 j6array
25657 [1]
25658 -= IK2PI;
25659 }
25660 else if (
25661 j6array
25662 [1]
25663 < -IKPI)
25664 {
25665 j6array
25666 [1]
25667 += IK2PI;
25668 }
25669 j6valid
25670 [1]
25671 = true;
25672 for (
25673 int ij6
25674 = 0;
25675 ij6
25676 < 2;
25677 ++ij6)
25678 {
25679 if (!j6valid
25680 [ij6])
25681 {
25682 continue;
25683 }
25684 _ij6[0]
25685 = ij6;
25686 _ij6[1]
25687 = -1;
25688 for (
25689 int iij6
25690 = ij6
25691 + 1;
25692 iij6
25693 < 2;
25694 ++iij6)
25695 {
25696 if (j6valid
25697 [iij6]
25698 && IKabs(
25699 cj6array
25700 [ij6]
25701 - cj6array
25702 [iij6])
25703 < IKFAST_SOLUTION_THRESH
25704 && IKabs(
25705 sj6array
25706 [ij6]
25707 - sj6array
25708 [iij6])
25709 < IKFAST_SOLUTION_THRESH)
25710 {
25711 j6valid
25712 [iij6]
25713 = false;
25714 _ij6[1]
25715 = iij6;
25716 break;
25717 }
25718 }
25719 j6 = j6array
25720 [ij6];
25721 cj6 = cj6array
25722 [ij6];
25723 sj6 = sj6array
25724 [ij6];
25725 {
25726 IkReal evalcond
25727 [1];
25728 evalcond
25729 [0]
25730 = ((0.85)
25731 * (IKsin(
25732 j6)));
25733 if (IKabs(
25734 evalcond
25735 [0])
25736 > IKFAST_EVALCOND_THRESH)
25737 {
25738 continue;
25739 }
25740 }
25741
25742 rotationfunction0(
25743 solutions);
25744 }
25745 }
25746 }
25747 }
25748 }
25749 } while (
25750 0);
25751 if (bgotonextstatement)
25752 {
25753 bool
25754 bgotonextstatement
25755 = true;
25756 do
25757 {
25758 evalcond
25759 [0]
25760 = ((IKabs((
25761 (-3.14159265358979)
25762 + (IKfmod(
25763 j4,
25764 6.28318530717959)))))
25765 + (IKabs(
25766 px)));
25767 evalcond
25768 [1]
25769 = -0.85;
25770 evalcond
25771 [2]
25772 = 0;
25773 evalcond
25774 [3]
25775 = ((-0.2125)
25776 + (((-1.0)
25777 * (1.0)
25778 * (py
25779 * py))));
25780 if (IKabs(
25781 evalcond
25782 [0])
25783 < 0.0000010000000000
25784 && IKabs(
25785 evalcond
25786 [1])
25787 < 0.0000010000000000
25788 && IKabs(
25789 evalcond
25790 [2])
25791 < 0.0000010000000000
25792 && IKabs(
25793 evalcond
25794 [3])
25795 < 0.0000010000000000)
25796 {
25797 bgotonextstatement
25798 = false;
25799 {
25800 IkReal j6eval
25801 [1];
25802 IkReal
25803 x1857
25804 = ((1.0)
25805 * py);
25806 sj8 = 0;
25807 cj8 = 1.0;
25808 j8 = 0;
25809 pz = 0;
25810 j9 = 0;
25811 sj9 = 0;
25812 cj9 = 1.0;
25813 pp = py
25814 * py;
25815 npx
25816 = (py
25817 * r10);
25818 npy
25819 = (py
25820 * r11);
25821 npz
25822 = (py
25823 * r12);
25824 rxp0_0
25825 = ((-1.0)
25826 * r20
25827 * x1857);
25828 rxp0_1
25829 = 0;
25830 rxp1_0
25831 = ((-1.0)
25832 * r21
25833 * x1857);
25834 rxp1_1
25835 = 0;
25836 rxp2_0
25837 = ((-1.0)
25838 * r22
25839 * x1857);
25840 rxp2_1
25841 = 0;
25842 px = 0;
25843 j4 = 3.14159265358979;
25844 sj4 = 0;
25845 cj4 = -1.0;
25846 rxp0_2
25847 = (py
25848 * r00);
25849 rxp1_2
25850 = (py
25851 * r01);
25852 rxp2_2
25853 = (py
25854 * r02);
25855 j6eval
25856 [0]
25857 = ((1.0)
25858 + (((1.91568587540858)
25859 * (py
25860 * py
25861 * py
25862 * py)))
25863 + (((-1.0)
25864 * (2.64633970947792)
25865 * (py
25866 * py))));
25867 if (IKabs(
25868 j6eval
25869 [0])
25870 < 0.0000010000000000)
25871 {
25872 continue; // no branches [j6]
25873 }
25874 else
25875 {
25876 {
25877 IkReal j6array
25878 [2],
25879 cj6array
25880 [2],
25881 sj6array
25882 [2];
25883 bool j6valid
25884 [2]
25885 = {false};
25886 _nj6
25887 = 2;
25888 IkReal
25889 x1858
25890 = py
25891 * py;
25892 CheckValue<IkReal> x1860 = IKatan2WithCheck(
25893 IkReal((
25894 (-0.425)
25895 + (((-0.588235294117647)
25896 * x1858)))),
25897 ((-2.83333333333333)
25898 + (((3.92156862745098)
25899 * x1858))),
25900 IKFAST_ATAN2_MAGTHRESH);
25901 if (!x1860
25902 .valid)
25903 {
25904 continue;
25905 }
25906 IkReal
25907 x1859
25908 = ((-1.0)
25909 * (x1860
25910 .value));
25911 j6array
25912 [0]
25913 = x1859;
25914 sj6array
25915 [0]
25916 = IKsin(
25917 j6array
25918 [0]);
25919 cj6array
25920 [0]
25921 = IKcos(
25922 j6array
25923 [0]);
25924 j6array
25925 [1]
25926 = ((3.14159265358979)
25927 + x1859);
25928 sj6array
25929 [1]
25930 = IKsin(
25931 j6array
25932 [1]);
25933 cj6array
25934 [1]
25935 = IKcos(
25936 j6array
25937 [1]);
25938 if (j6array
25939 [0]
25940 > IKPI)
25941 {
25942 j6array
25943 [0]
25944 -= IK2PI;
25945 }
25946 else if (
25947 j6array
25948 [0]
25949 < -IKPI)
25950 {
25951 j6array
25952 [0]
25953 += IK2PI;
25954 }
25955 j6valid
25956 [0]
25957 = true;
25958 if (j6array
25959 [1]
25960 > IKPI)
25961 {
25962 j6array
25963 [1]
25964 -= IK2PI;
25965 }
25966 else if (
25967 j6array
25968 [1]
25969 < -IKPI)
25970 {
25971 j6array
25972 [1]
25973 += IK2PI;
25974 }
25975 j6valid
25976 [1]
25977 = true;
25978 for (
25979 int ij6
25980 = 0;
25981 ij6
25982 < 2;
25983 ++ij6)
25984 {
25985 if (!j6valid
25986 [ij6])
25987 {
25988 continue;
25989 }
25990 _ij6[0]
25991 = ij6;
25992 _ij6[1]
25993 = -1;
25994 for (
25995 int iij6
25996 = ij6
25997 + 1;
25998 iij6
25999 < 2;
26000 ++iij6)
26001 {
26002 if (j6valid
26003 [iij6]
26004 && IKabs(
26005 cj6array
26006 [ij6]
26007 - cj6array
26008 [iij6])
26009 < IKFAST_SOLUTION_THRESH
26010 && IKabs(
26011 sj6array
26012 [ij6]
26013 - sj6array
26014 [iij6])
26015 < IKFAST_SOLUTION_THRESH)
26016 {
26017 j6valid
26018 [iij6]
26019 = false;
26020 _ij6[1]
26021 = iij6;
26022 break;
26023 }
26024 }
26025 j6 = j6array
26026 [ij6];
26027 cj6 = cj6array
26028 [ij6];
26029 sj6 = sj6array
26030 [ij6];
26031 {
26032 IkReal evalcond
26033 [1];
26034 evalcond
26035 [0]
26036 = ((0.85)
26037 * (IKsin(
26038 j6)));
26039 if (IKabs(
26040 evalcond
26041 [0])
26042 > IKFAST_EVALCOND_THRESH)
26043 {
26044 continue;
26045 }
26046 }
26047
26048 rotationfunction0(
26049 solutions);
26050 }
26051 }
26052 }
26053 }
26054 }
26055 } while (
26056 0);
26057 if (bgotonextstatement)
26058 {
26059 bool
26060 bgotonextstatement
26061 = true;
26062 do
26063 {
26064 evalcond
26065 [0]
26066 = ((IKabs((
26067 (-3.14159265358979)
26068 + (IKfmod(
26069 ((1.5707963267949)
26070 + j4),
26071 6.28318530717959)))))
26072 + (IKabs(
26073 py)));
26074 evalcond
26075 [1]
26076 = -0.85;
26077 evalcond
26078 [2]
26079 = 0;
26080 evalcond
26081 [3]
26082 = ((-0.2125)
26083 + (((-1.0)
26084 * (1.0)
26085 * (px
26086 * px))));
26087 if (IKabs(
26088 evalcond
26089 [0])
26090 < 0.0000010000000000
26091 && IKabs(
26092 evalcond
26093 [1])
26094 < 0.0000010000000000
26095 && IKabs(
26096 evalcond
26097 [2])
26098 < 0.0000010000000000
26099 && IKabs(
26100 evalcond
26101 [3])
26102 < 0.0000010000000000)
26103 {
26104 bgotonextstatement
26105 = false;
26106 {
26107 IkReal j6eval
26108 [1];
26109 IkReal
26110 x1861
26111 = ((1.0)
26112 * px);
26113 sj8 = 0;
26114 cj8 = 1.0;
26115 j8 = 0;
26116 pz = 0;
26117 j9 = 0;
26118 sj9 = 0;
26119 cj9 = 1.0;
26120 pp = px
26121 * px;
26122 npx
26123 = (px
26124 * r00);
26125 npy
26126 = (px
26127 * r01);
26128 npz
26129 = (px
26130 * r02);
26131 rxp0_0
26132 = 0;
26133 rxp0_1
26134 = (px
26135 * r20);
26136 rxp1_0
26137 = 0;
26138 rxp1_1
26139 = (px
26140 * r21);
26141 rxp2_0
26142 = 0;
26143 rxp2_1
26144 = (px
26145 * r22);
26146 py = 0;
26147 j4 = 1.5707963267949;
26148 sj4 = 1.0;
26149 cj4 = 0;
26150 rxp0_2
26151 = ((-1.0)
26152 * r10
26153 * x1861);
26154 rxp1_2
26155 = ((-1.0)
26156 * r11
26157 * x1861);
26158 rxp2_2
26159 = ((-1.0)
26160 * r12
26161 * x1861);
26162 j6eval
26163 [0]
26164 = ((1.0)
26165 + (((1.91568587540858)
26166 * (px
26167 * px
26168 * px
26169 * px)))
26170 + (((-1.0)
26171 * (2.64633970947792)
26172 * (px
26173 * px))));
26174 if (IKabs(
26175 j6eval
26176 [0])
26177 < 0.0000010000000000)
26178 {
26179 continue; // no branches [j6]
26180 }
26181 else
26182 {
26183 {
26184 IkReal j6array
26185 [2],
26186 cj6array
26187 [2],
26188 sj6array
26189 [2];
26190 bool j6valid
26191 [2]
26192 = {false};
26193 _nj6
26194 = 2;
26195 IkReal
26196 x1862
26197 = px
26198 * px;
26199 CheckValue<IkReal> x1864 = IKatan2WithCheck(
26200 IkReal((
26201 (-0.425)
26202 + (((-0.588235294117647)
26203 * x1862)))),
26204 ((-2.83333333333333)
26205 + (((3.92156862745098)
26206 * x1862))),
26207 IKFAST_ATAN2_MAGTHRESH);
26208 if (!x1864
26209 .valid)
26210 {
26211 continue;
26212 }
26213 IkReal
26214 x1863
26215 = ((-1.0)
26216 * (x1864
26217 .value));
26218 j6array
26219 [0]
26220 = x1863;
26221 sj6array
26222 [0]
26223 = IKsin(
26224 j6array
26225 [0]);
26226 cj6array
26227 [0]
26228 = IKcos(
26229 j6array
26230 [0]);
26231 j6array
26232 [1]
26233 = ((3.14159265358979)
26234 + x1863);
26235 sj6array
26236 [1]
26237 = IKsin(
26238 j6array
26239 [1]);
26240 cj6array
26241 [1]
26242 = IKcos(
26243 j6array
26244 [1]);
26245 if (j6array
26246 [0]
26247 > IKPI)
26248 {
26249 j6array
26250 [0]
26251 -= IK2PI;
26252 }
26253 else if (
26254 j6array
26255 [0]
26256 < -IKPI)
26257 {
26258 j6array
26259 [0]
26260 += IK2PI;
26261 }
26262 j6valid
26263 [0]
26264 = true;
26265 if (j6array
26266 [1]
26267 > IKPI)
26268 {
26269 j6array
26270 [1]
26271 -= IK2PI;
26272 }
26273 else if (
26274 j6array
26275 [1]
26276 < -IKPI)
26277 {
26278 j6array
26279 [1]
26280 += IK2PI;
26281 }
26282 j6valid
26283 [1]
26284 = true;
26285 for (
26286 int ij6
26287 = 0;
26288 ij6
26289 < 2;
26290 ++ij6)
26291 {
26292 if (!j6valid
26293 [ij6])
26294 {
26295 continue;
26296 }
26297 _ij6[0]
26298 = ij6;
26299 _ij6[1]
26300 = -1;
26301 for (
26302 int iij6
26303 = ij6
26304 + 1;
26305 iij6
26306 < 2;
26307 ++iij6)
26308 {
26309 if (j6valid
26310 [iij6]
26311 && IKabs(
26312 cj6array
26313 [ij6]
26314 - cj6array
26315 [iij6])
26316 < IKFAST_SOLUTION_THRESH
26317 && IKabs(
26318 sj6array
26319 [ij6]
26320 - sj6array
26321 [iij6])
26322 < IKFAST_SOLUTION_THRESH)
26323 {
26324 j6valid
26325 [iij6]
26326 = false;
26327 _ij6[1]
26328 = iij6;
26329 break;
26330 }
26331 }
26332 j6 = j6array
26333 [ij6];
26334 cj6 = cj6array
26335 [ij6];
26336 sj6 = sj6array
26337 [ij6];
26338 {
26339 IkReal evalcond
26340 [1];
26341 evalcond
26342 [0]
26343 = ((0.85)
26344 * (IKsin(
26345 j6)));
26346 if (IKabs(
26347 evalcond
26348 [0])
26349 > IKFAST_EVALCOND_THRESH)
26350 {
26351 continue;
26352 }
26353 }
26354
26355 rotationfunction0(
26356 solutions);
26357 }
26358 }
26359 }
26360 }
26361 }
26362 } while (
26363 0);
26364 if (bgotonextstatement)
26365 {
26366 bool
26367 bgotonextstatement
26368 = true;
26369 do
26370 {
26371 evalcond
26372 [0]
26373 = ((IKabs(
26374 py))
26375 + (IKabs((
26376 (-3.14159265358979)
26377 + (IKfmod(
26378 ((4.71238898038469)
26379 + j4),
26380 6.28318530717959))))));
26381 evalcond
26382 [1]
26383 = -0.85;
26384 evalcond
26385 [2]
26386 = 0;
26387 evalcond
26388 [3]
26389 = ((-0.2125)
26390 + (((-1.0)
26391 * (1.0)
26392 * (px
26393 * px))));
26394 if (IKabs(
26395 evalcond
26396 [0])
26397 < 0.0000010000000000
26398 && IKabs(
26399 evalcond
26400 [1])
26401 < 0.0000010000000000
26402 && IKabs(
26403 evalcond
26404 [2])
26405 < 0.0000010000000000
26406 && IKabs(
26407 evalcond
26408 [3])
26409 < 0.0000010000000000)
26410 {
26411 bgotonextstatement
26412 = false;
26413 {
26414 IkReal j6eval
26415 [1];
26416 IkReal
26417 x1865
26418 = ((1.0)
26419 * px);
26420 sj8 = 0;
26421 cj8 = 1.0;
26422 j8 = 0;
26423 pz = 0;
26424 j9 = 0;
26425 sj9 = 0;
26426 cj9 = 1.0;
26427 pp = px
26428 * px;
26429 npx
26430 = (px
26431 * r00);
26432 npy
26433 = (px
26434 * r01);
26435 npz
26436 = (px
26437 * r02);
26438 rxp0_0
26439 = 0;
26440 rxp0_1
26441 = (px
26442 * r20);
26443 rxp1_0
26444 = 0;
26445 rxp1_1
26446 = (px
26447 * r21);
26448 rxp2_0
26449 = 0;
26450 rxp2_1
26451 = (px
26452 * r22);
26453 py = 0;
26454 j4 = -1.5707963267949;
26455 sj4 = -1.0;
26456 cj4 = 0;
26457 rxp0_2
26458 = ((-1.0)
26459 * r10
26460 * x1865);
26461 rxp1_2
26462 = ((-1.0)
26463 * r11
26464 * x1865);
26465 rxp2_2
26466 = ((-1.0)
26467 * r12
26468 * x1865);
26469 j6eval
26470 [0]
26471 = ((1.0)
26472 + (((1.91568587540858)
26473 * (px
26474 * px
26475 * px
26476 * px)))
26477 + (((-1.0)
26478 * (2.64633970947792)
26479 * (px
26480 * px))));
26481 if (IKabs(
26482 j6eval
26483 [0])
26484 < 0.0000010000000000)
26485 {
26486 continue; // no branches [j6]
26487 }
26488 else
26489 {
26490 {
26491 IkReal j6array
26492 [2],
26493 cj6array
26494 [2],
26495 sj6array
26496 [2];
26497 bool j6valid
26498 [2]
26499 = {false};
26500 _nj6
26501 = 2;
26502 IkReal
26503 x1866
26504 = px
26505 * px;
26506 CheckValue<IkReal> x1868 = IKatan2WithCheck(
26507 IkReal((
26508 (-0.425)
26509 + (((-0.588235294117647)
26510 * x1866)))),
26511 ((-2.83333333333333)
26512 + (((3.92156862745098)
26513 * x1866))),
26514 IKFAST_ATAN2_MAGTHRESH);
26515 if (!x1868
26516 .valid)
26517 {
26518 continue;
26519 }
26520 IkReal
26521 x1867
26522 = ((-1.0)
26523 * (x1868
26524 .value));
26525 j6array
26526 [0]
26527 = x1867;
26528 sj6array
26529 [0]
26530 = IKsin(
26531 j6array
26532 [0]);
26533 cj6array
26534 [0]
26535 = IKcos(
26536 j6array
26537 [0]);
26538 j6array
26539 [1]
26540 = ((3.14159265358979)
26541 + x1867);
26542 sj6array
26543 [1]
26544 = IKsin(
26545 j6array
26546 [1]);
26547 cj6array
26548 [1]
26549 = IKcos(
26550 j6array
26551 [1]);
26552 if (j6array
26553 [0]
26554 > IKPI)
26555 {
26556 j6array
26557 [0]
26558 -= IK2PI;
26559 }
26560 else if (
26561 j6array
26562 [0]
26563 < -IKPI)
26564 {
26565 j6array
26566 [0]
26567 += IK2PI;
26568 }
26569 j6valid
26570 [0]
26571 = true;
26572 if (j6array
26573 [1]
26574 > IKPI)
26575 {
26576 j6array
26577 [1]
26578 -= IK2PI;
26579 }
26580 else if (
26581 j6array
26582 [1]
26583 < -IKPI)
26584 {
26585 j6array
26586 [1]
26587 += IK2PI;
26588 }
26589 j6valid
26590 [1]
26591 = true;
26592 for (
26593 int ij6
26594 = 0;
26595 ij6
26596 < 2;
26597 ++ij6)
26598 {
26599 if (!j6valid
26600 [ij6])
26601 {
26602 continue;
26603 }
26604 _ij6[0]
26605 = ij6;
26606 _ij6[1]
26607 = -1;
26608 for (
26609 int iij6
26610 = ij6
26611 + 1;
26612 iij6
26613 < 2;
26614 ++iij6)
26615 {
26616 if (j6valid
26617 [iij6]
26618 && IKabs(
26619 cj6array
26620 [ij6]
26621 - cj6array
26622 [iij6])
26623 < IKFAST_SOLUTION_THRESH
26624 && IKabs(
26625 sj6array
26626 [ij6]
26627 - sj6array
26628 [iij6])
26629 < IKFAST_SOLUTION_THRESH)
26630 {
26631 j6valid
26632 [iij6]
26633 = false;
26634 _ij6[1]
26635 = iij6;
26636 break;
26637 }
26638 }
26639 j6 = j6array
26640 [ij6];
26641 cj6 = cj6array
26642 [ij6];
26643 sj6 = sj6array
26644 [ij6];
26645 {
26646 IkReal evalcond
26647 [1];
26648 evalcond
26649 [0]
26650 = ((0.85)
26651 * (IKsin(
26652 j6)));
26653 if (IKabs(
26654 evalcond
26655 [0])
26656 > IKFAST_EVALCOND_THRESH)
26657 {
26658 continue;
26659 }
26660 }
26661
26662 rotationfunction0(
26663 solutions);
26664 }
26665 }
26666 }
26667 }
26668 }
26669 } while (
26670 0);
26671 if (bgotonextstatement)
26672 {
26673 bool
26674 bgotonextstatement
26675 = true;
26676 do
26677 {
26678 if (1)
26679 {
26680 bgotonextstatement
26681 = false;
26682 continue; // branch miss [j6]
26683 }
26684 } while (
26685 0);
26686 if (bgotonextstatement)
26687 {
26688 }
26689 }
26690 }
26691 }
26692 }
26693 }
26694 }
26695 }
26696 else
26697 {
26698 {
26699 IkReal
26700 j6array
26701 [1],
26702 cj6array
26703 [1],
26704 sj6array
26705 [1];
26706 bool j6valid
26707 [1]
26708 = {false};
26709 _nj6 = 1;
26710 IkReal x1869
26711 = (cj4
26712 * px);
26713 IkReal x1870
26714 = (py
26715 * sj4);
26716 IkReal x1871
26717 = px
26718 * px;
26719 IkReal x1872
26720 = py
26721 * py;
26722 CheckValue<
26723 IkReal>
26724 x1873
26725 = IKPowWithIntegerCheck(
26726 ((((20.0)
26727 * x1870))
26728 + (((20.0)
26729 * x1869))),
26730 -1);
26731 if (!x1873
26732 .valid)
26733 {
26734 continue;
26735 }
26736 CheckValue<IkReal> x1874 = IKPowWithIntegerCheck(
26737 ((((-1.0)
26738 * (11.7647058823529)
26739 * cj4
26740 * (px
26741 * px
26742 * px)))
26743 + (((-11.7647058823529)
26744 * x1869
26745 * x1872))
26746 + (((-8.5)
26747 * x1869))
26748 + (((-8.5)
26749 * x1870))
26750 + (((-11.7647058823529)
26751 * x1870
26752 * x1871))
26753 + (((-1.0)
26754 * (11.7647058823529)
26755 * sj4
26756 * (py
26757 * py
26758 * py)))),
26759 -1);
26760 if (!x1874
26761 .valid)
26762 {
26763 continue;
26764 }
26765 if (IKabs((
26766 (17.0)
26767 * (x1873
26768 .value)))
26769 < IKFAST_ATAN2_MAGTHRESH
26770 && IKabs((
26771 (x1874
26772 .value)
26773 * (((48.1666666666667)
26774 + (((-66.6666666666667)
26775 * x1872))
26776 + (((-66.6666666666667)
26777 * x1871))))))
26778 < IKFAST_ATAN2_MAGTHRESH
26779 && IKabs(
26780 IKsqr((
26781 (17.0)
26782 * (x1873
26783 .value)))
26784 + IKsqr((
26785 (x1874
26786 .value)
26787 * (((48.1666666666667)
26788 + (((-66.6666666666667)
26789 * x1872))
26790 + (((-66.6666666666667)
26791 * x1871))))))
26792 - 1)
26793 <= IKFAST_SINCOS_THRESH)
26794 continue;
26795 j6array[0] = IKatan2(
26796 ((17.0)
26797 * (x1873
26798 .value)),
26799 ((x1874
26800 .value)
26801 * (((48.1666666666667)
26802 + (((-66.6666666666667)
26803 * x1872))
26804 + (((-66.6666666666667)
26805 * x1871))))));
26806 sj6array[0] = IKsin(
26807 j6array
26808 [0]);
26809 cj6array[0] = IKcos(
26810 j6array
26811 [0]);
26812 if (j6array
26813 [0]
26814 > IKPI)
26815 {
26816 j6array[0]
26817 -= IK2PI;
26818 }
26819 else if (
26820 j6array
26821 [0]
26822 < -IKPI)
26823 {
26824 j6array[0]
26825 += IK2PI;
26826 }
26827 j6valid[0]
26828 = true;
26829 for (int ij6
26830 = 0;
26831 ij6
26832 < 1;
26833 ++ij6)
26834 {
26835 if (!j6valid
26836 [ij6])
26837 {
26838 continue;
26839 }
26840 _ij6[0]
26841 = ij6;
26842 _ij6[1]
26843 = -1;
26844 for (
26845 int iij6
26846 = ij6
26847 + 1;
26848 iij6
26849 < 1;
26850 ++iij6)
26851 {
26852 if (j6valid
26853 [iij6]
26854 && IKabs(
26855 cj6array
26856 [ij6]
26857 - cj6array
26858 [iij6])
26859 < IKFAST_SOLUTION_THRESH
26860 && IKabs(
26861 sj6array
26862 [ij6]
26863 - sj6array
26864 [iij6])
26865 < IKFAST_SOLUTION_THRESH)
26866 {
26867 j6valid
26868 [iij6]
26869 = false;
26870 _ij6[1]
26871 = iij6;
26872 break;
26873 }
26874 }
26875 j6 = j6array
26876 [ij6];
26877 cj6 = cj6array
26878 [ij6];
26879 sj6 = sj6array
26880 [ij6];
26881 {
26882 IkReal evalcond
26883 [5];
26884 IkReal
26885 x1875
26886 = IKsin(
26887 j6);
26888 IkReal
26889 x1876
26890 = (cj4
26891 * px);
26892 IkReal
26893 x1877
26894 = (x1875
26895 * x1876);
26896 IkReal
26897 x1878
26898 = (py
26899 * sj4);
26900 IkReal
26901 x1879
26902 = (x1875
26903 * x1878);
26904 IkReal
26905 x1880
26906 = ((1.0)
26907 * x1876);
26908 IkReal
26909 x1881
26910 = ((1.0)
26911 * x1878);
26912 IkReal
26913 x1882
26914 = IKcos(
26915 j6);
26916 IkReal
26917 x1883
26918 = px
26919 * px;
26920 IkReal
26921 x1884
26922 = ((3.92156862745098)
26923 * x1875);
26924 IkReal
26925 x1885
26926 = ((0.588235294117647)
26927 * x1882);
26928 IkReal
26929 x1886
26930 = py
26931 * py;
26932 IkReal
26933 x1887
26934 = ((0.09)
26935 * x1882);
26936 evalcond
26937 [0]
26938 = ((-0.85)
26939 + x1879
26940 + x1877);
26941 evalcond
26942 [1]
26943 = ((((-1.0)
26944 * x1880))
26945 + (((0.85)
26946 * x1875))
26947 + (((-1.0)
26948 * x1881)));
26949 evalcond
26950 [2]
26951 = ((((-1.0)
26952 * x1881
26953 * x1882))
26954 + (((-1.0)
26955 * x1880
26956 * x1882)));
26957 evalcond
26958 [3]
26959 = ((((-0.425)
26960 * x1882))
26961 + (((-1.0)
26962 * x1885
26963 * x1886))
26964 + ((x1884
26965 * x1886))
26966 + (((-1.0)
26967 * x1883
26968 * x1885))
26969 + ((x1883
26970 * x1884))
26971 + (((-2.83333333333333)
26972 * x1875)));
26973 evalcond
26974 [4]
26975 = ((-0.2125)
26976 + (((1.1)
26977 * x1879))
26978 + (((-1.0)
26979 * x1883))
26980 + (((-1.0)
26981 * x1886))
26982 + (((1.1)
26983 * x1877))
26984 + ((x1876
26985 * x1887))
26986 + ((x1878
26987 * x1887)));
26988 if (IKabs(
26989 evalcond
26990 [0])
26991 > IKFAST_EVALCOND_THRESH
26992 || IKabs(
26993 evalcond
26994 [1])
26995 > IKFAST_EVALCOND_THRESH
26996 || IKabs(
26997 evalcond
26998 [2])
26999 > IKFAST_EVALCOND_THRESH
27000 || IKabs(
27001 evalcond
27002 [3])
27003 > IKFAST_EVALCOND_THRESH
27004 || IKabs(
27005 evalcond
27006 [4])
27007 > IKFAST_EVALCOND_THRESH)
27008 {
27009 continue;
27010 }
27011 }
27012
27013 rotationfunction0(
27014 solutions);
27015 }
27016 }
27017 }
27018 }
27019 }
27020 else
27021 {
27022 {
27023 IkReal
27024 j6array[1],
27025 cj6array[1],
27026 sj6array[1];
27027 bool j6valid[1]
27028 = {false};
27029 _nj6 = 1;
27030 IkReal x1888
27031 = (cj4
27032 * px);
27033 IkReal x1889
27034 = (py
27035 * sj4);
27036 IkReal x1890
27037 = px * px;
27038 IkReal x1891
27039 = py * py;
27040 CheckValue<
27041 IkReal>
27042 x1892
27043 = IKPowWithIntegerCheck(
27044 ((-7.225)
27045 + (((-10.0)
27046 * x1891))
27047 + (((-10.0)
27048 * x1890))),
27049 -1);
27050 if (!x1892
27051 .valid)
27052 {
27053 continue;
27054 }
27055 if (IKabs((
27056 (((1.17647058823529)
27057 * x1888))
27058 + (((1.17647058823529)
27059 * x1889))))
27060 < IKFAST_ATAN2_MAGTHRESH
27061 && IKabs((
27062 (x1892
27063 .value)
27064 * (((((56.6666666666667)
27065 * x1889))
27066 + (((56.6666666666667)
27067 * x1888))
27068 + (((-1.0)
27069 * (78.4313725490196)
27070 * sj4
27071 * (py
27072 * py
27073 * py)))
27074 + (((-78.4313725490196)
27075 * x1888
27076 * x1891))
27077 + (((-1.0)
27078 * (78.4313725490196)
27079 * cj4
27080 * (px
27081 * px
27082 * px)))
27083 + (((-78.4313725490196)
27084 * x1889
27085 * x1890))))))
27086 < IKFAST_ATAN2_MAGTHRESH
27087 && IKabs(
27088 IKsqr((
27089 (((1.17647058823529)
27090 * x1888))
27091 + (((1.17647058823529)
27092 * x1889))))
27093 + IKsqr((
27094 (x1892
27095 .value)
27096 * (((((56.6666666666667)
27097 * x1889))
27098 + (((56.6666666666667)
27099 * x1888))
27100 + (((-1.0)
27101 * (78.4313725490196)
27102 * sj4
27103 * (py
27104 * py
27105 * py)))
27106 + (((-78.4313725490196)
27107 * x1888
27108 * x1891))
27109 + (((-1.0)
27110 * (78.4313725490196)
27111 * cj4
27112 * (px
27113 * px
27114 * px)))
27115 + (((-78.4313725490196)
27116 * x1889
27117 * x1890))))))
27118 - 1)
27119 <= IKFAST_SINCOS_THRESH)
27120 continue;
27121 j6array[0] = IKatan2(
27122 ((((1.17647058823529)
27123 * x1888))
27124 + (((1.17647058823529)
27125 * x1889))),
27126 ((x1892
27127 .value)
27128 * (((((56.6666666666667)
27129 * x1889))
27130 + (((56.6666666666667)
27131 * x1888))
27132 + (((-1.0)
27133 * (78.4313725490196)
27134 * sj4
27135 * (py
27136 * py
27137 * py)))
27138 + (((-78.4313725490196)
27139 * x1888
27140 * x1891))
27141 + (((-1.0)
27142 * (78.4313725490196)
27143 * cj4
27144 * (px
27145 * px
27146 * px)))
27147 + (((-78.4313725490196)
27148 * x1889
27149 * x1890))))));
27150 sj6array[0]
27151 = IKsin(
27152 j6array
27153 [0]);
27154 cj6array[0]
27155 = IKcos(
27156 j6array
27157 [0]);
27158 if (j6array[0]
27159 > IKPI)
27160 {
27161 j6array[0]
27162 -= IK2PI;
27163 }
27164 else if (
27165 j6array[0]
27166 < -IKPI)
27167 {
27168 j6array[0]
27169 += IK2PI;
27170 }
27171 j6valid[0]
27172 = true;
27173 for (int ij6
27174 = 0;
27175 ij6 < 1;
27176 ++ij6)
27177 {
27178 if (!j6valid
27179 [ij6])
27180 {
27181 continue;
27182 }
27183 _ij6[0] = ij6;
27184 _ij6[1] = -1;
27185 for (int iij6
27186 = ij6
27187 + 1;
27188 iij6 < 1;
27189 ++iij6)
27190 {
27191 if (j6valid
27192 [iij6]
27193 && IKabs(
27194 cj6array
27195 [ij6]
27196 - cj6array
27197 [iij6])
27198 < IKFAST_SOLUTION_THRESH
27199 && IKabs(
27200 sj6array
27201 [ij6]
27202 - sj6array
27203 [iij6])
27204 < IKFAST_SOLUTION_THRESH)
27205 {
27206 j6valid
27207 [iij6]
27208 = false;
27209 _ij6[1]
27210 = iij6;
27211 break;
27212 }
27213 }
27214 j6 = j6array
27215 [ij6];
27216 cj6 = cj6array
27217 [ij6];
27218 sj6 = sj6array
27219 [ij6];
27220 {
27221 IkReal
27222 evalcond
27223 [5];
27224 IkReal x1893
27225 = IKsin(
27226 j6);
27227 IkReal x1894
27228 = (cj4
27229 * px);
27230 IkReal x1895
27231 = (x1893
27232 * x1894);
27233 IkReal x1896
27234 = (py
27235 * sj4);
27236 IkReal x1897
27237 = (x1893
27238 * x1896);
27239 IkReal x1898
27240 = ((1.0)
27241 * x1894);
27242 IkReal x1899
27243 = ((1.0)
27244 * x1896);
27245 IkReal x1900
27246 = IKcos(
27247 j6);
27248 IkReal x1901
27249 = px
27250 * px;
27251 IkReal x1902
27252 = ((3.92156862745098)
27253 * x1893);
27254 IkReal x1903
27255 = ((0.588235294117647)
27256 * x1900);
27257 IkReal x1904
27258 = py
27259 * py;
27260 IkReal x1905
27261 = ((0.09)
27262 * x1900);
27263 evalcond[0]
27264 = ((-0.85)
27265 + x1897
27266 + x1895);
27267 evalcond[1]
27268 = ((((-1.0)
27269 * x1898))
27270 + (((0.85)
27271 * x1893))
27272 + (((-1.0)
27273 * x1899)));
27274 evalcond[2]
27275 = ((((-1.0)
27276 * x1898
27277 * x1900))
27278 + (((-1.0)
27279 * x1899
27280 * x1900)));
27281 evalcond[3]
27282 = ((((-1.0)
27283 * x1901
27284 * x1903))
27285 + (((-1.0)
27286 * x1903
27287 * x1904))
27288 + ((x1902
27289 * x1904))
27290 + ((x1901
27291 * x1902))
27292 + (((-0.425)
27293 * x1900))
27294 + (((-2.83333333333333)
27295 * x1893)));
27296 evalcond[4]
27297 = ((-0.2125)
27298 + (((-1.0)
27299 * x1901))
27300 + (((1.1)
27301 * x1895))
27302 + (((1.1)
27303 * x1897))
27304 + (((-1.0)
27305 * x1904))
27306 + ((x1894
27307 * x1905))
27308 + ((x1896
27309 * x1905)));
27310 if (IKabs(
27311 evalcond
27312 [0])
27313 > IKFAST_EVALCOND_THRESH
27314 || IKabs(
27315 evalcond
27316 [1])
27317 > IKFAST_EVALCOND_THRESH
27318 || IKabs(
27319 evalcond
27320 [2])
27321 > IKFAST_EVALCOND_THRESH
27322 || IKabs(
27323 evalcond
27324 [3])
27325 > IKFAST_EVALCOND_THRESH
27326 || IKabs(
27327 evalcond
27328 [4])
27329 > IKFAST_EVALCOND_THRESH)
27330 {
27331 continue;
27332 }
27333 }
27334
27335 rotationfunction0(
27336 solutions);
27337 }
27338 }
27339 }
27340 }
27341 }
27342 else
27343 {
27344 {
27345 IkReal j6array[1],
27346 cj6array[1],
27347 sj6array[1];
27348 bool j6valid[1]
27349 = {false};
27350 _nj6 = 1;
27351 IkReal x1906
27352 = (cj4 * px);
27353 IkReal x1907
27354 = (py * sj4);
27355 IkReal x1908
27356 = px * px;
27357 IkReal x1909
27358 = py * py;
27359 IkReal x1910
27360 = ((1.29411764705882)
27361 * (cj4
27362 * cj4));
27363 CheckValue<IkReal>
27364 x1911
27365 = IKPowWithIntegerCheck(
27366 ((((0.09)
27367 * x1907))
27368 + (((0.09)
27369 * x1906))),
27370 -1);
27371 if (!x1911.valid)
27372 {
27373 continue;
27374 }
27375 if (IKabs((
27376 (((1.17647058823529)
27377 * x1907))
27378 + (((1.17647058823529)
27379 * x1906))))
27380 < IKFAST_ATAN2_MAGTHRESH
27381 && IKabs((
27382 (x1911
27383 .value)
27384 * (((0.2125)
27385 + (((-1.0)
27386 * x1908
27387 * x1910))
27388 + ((x1909
27389 * x1910))
27390 + x1908
27391 + (((-0.294117647058824)
27392 * x1909))
27393 + (((-2.58823529411765)
27394 * cj4
27395 * px
27396 * x1907))))))
27397 < IKFAST_ATAN2_MAGTHRESH
27398 && IKabs(
27399 IKsqr((
27400 (((1.17647058823529)
27401 * x1907))
27402 + (((1.17647058823529)
27403 * x1906))))
27404 + IKsqr((
27405 (x1911
27406 .value)
27407 * (((0.2125)
27408 + (((-1.0)
27409 * x1908
27410 * x1910))
27411 + ((x1909
27412 * x1910))
27413 + x1908
27414 + (((-0.294117647058824)
27415 * x1909))
27416 + (((-2.58823529411765)
27417 * cj4
27418 * px
27419 * x1907))))))
27420 - 1)
27421 <= IKFAST_SINCOS_THRESH)
27422 continue;
27423 j6array[0] = IKatan2(
27424 ((((1.17647058823529)
27425 * x1907))
27426 + (((1.17647058823529)
27427 * x1906))),
27428 ((x1911.value)
27429 * (((0.2125)
27430 + (((-1.0)
27431 * x1908
27432 * x1910))
27433 + ((x1909
27434 * x1910))
27435 + x1908
27436 + (((-0.294117647058824)
27437 * x1909))
27438 + (((-2.58823529411765)
27439 * cj4
27440 * px
27441 * x1907))))));
27442 sj6array[0] = IKsin(
27443 j6array[0]);
27444 cj6array[0] = IKcos(
27445 j6array[0]);
27446 if (j6array[0]
27447 > IKPI)
27448 {
27449 j6array[0]
27450 -= IK2PI;
27451 }
27452 else if (
27453 j6array[0]
27454 < -IKPI)
27455 {
27456 j6array[0]
27457 += IK2PI;
27458 }
27459 j6valid[0] = true;
27460 for (int ij6 = 0;
27461 ij6 < 1;
27462 ++ij6)
27463 {
27464 if (!j6valid[ij6])
27465 {
27466 continue;
27467 }
27468 _ij6[0] = ij6;
27469 _ij6[1] = -1;
27470 for (int iij6
27471 = ij6 + 1;
27472 iij6 < 1;
27473 ++iij6)
27474 {
27475 if (j6valid
27476 [iij6]
27477 && IKabs(
27478 cj6array
27479 [ij6]
27480 - cj6array
27481 [iij6])
27482 < IKFAST_SOLUTION_THRESH
27483 && IKabs(
27484 sj6array
27485 [ij6]
27486 - sj6array
27487 [iij6])
27488 < IKFAST_SOLUTION_THRESH)
27489 {
27490 j6valid[iij6]
27491 = false;
27492 _ij6[1]
27493 = iij6;
27494 break;
27495 }
27496 }
27497 j6 = j6array[ij6];
27498 cj6 = cj6array
27499 [ij6];
27500 sj6 = sj6array
27501 [ij6];
27502 {
27503 IkReal
27504 evalcond[5];
27505 IkReal x1912
27506 = IKsin(j6);
27507 IkReal x1913
27508 = (cj4
27509 * px);
27510 IkReal x1914
27511 = (x1912
27512 * x1913);
27513 IkReal x1915
27514 = (py
27515 * sj4);
27516 IkReal x1916
27517 = (x1912
27518 * x1915);
27519 IkReal x1917
27520 = ((1.0)
27521 * x1913);
27522 IkReal x1918
27523 = ((1.0)
27524 * x1915);
27525 IkReal x1919
27526 = IKcos(j6);
27527 IkReal x1920
27528 = px * px;
27529 IkReal x1921
27530 = ((3.92156862745098)
27531 * x1912);
27532 IkReal x1922
27533 = ((0.588235294117647)
27534 * x1919);
27535 IkReal x1923
27536 = py * py;
27537 IkReal x1924
27538 = ((0.09)
27539 * x1919);
27540 evalcond[0]
27541 = ((-0.85)
27542 + x1914
27543 + x1916);
27544 evalcond[1]
27545 = ((((0.85)
27546 * x1912))
27547 + (((-1.0)
27548 * x1918))
27549 + (((-1.0)
27550 * x1917)));
27551 evalcond[2]
27552 = ((((-1.0)
27553 * x1917
27554 * x1919))
27555 + (((-1.0)
27556 * x1918
27557 * x1919)));
27558 evalcond[3]
27559 = ((((-1.0)
27560 * x1920
27561 * x1922))
27562 + (((-1.0)
27563 * x1922
27564 * x1923))
27565 + ((x1921
27566 * x1923))
27567 + ((x1920
27568 * x1921))
27569 + (((-0.425)
27570 * x1919))
27571 + (((-2.83333333333333)
27572 * x1912)));
27573 evalcond[4]
27574 = ((-0.2125)
27575 + ((x1913
27576 * x1924))
27577 + (((-1.0)
27578 * x1923))
27579 + (((1.1)
27580 * x1916))
27581 + ((x1915
27582 * x1924))
27583 + (((1.1)
27584 * x1914))
27585 + (((-1.0)
27586 * x1920)));
27587 if (IKabs(
27588 evalcond
27589 [0])
27590 > IKFAST_EVALCOND_THRESH
27591 || IKabs(
27592 evalcond
27593 [1])
27594 > IKFAST_EVALCOND_THRESH
27595 || IKabs(
27596 evalcond
27597 [2])
27598 > IKFAST_EVALCOND_THRESH
27599 || IKabs(
27600 evalcond
27601 [3])
27602 > IKFAST_EVALCOND_THRESH
27603 || IKabs(
27604 evalcond
27605 [4])
27606 > IKFAST_EVALCOND_THRESH)
27607 {
27608 continue;
27609 }
27610 }
27611
27612 rotationfunction0(
27613 solutions);
27614 }
27615 }
27616 }
27617 }
27618 }
27619 } while (0);
27620 if (bgotonextstatement)
27621 {
27622 bool bgotonextstatement
27623 = true;
27624 do
27625 {
27626 if (1)
27627 {
27628 bgotonextstatement
27629 = false;
27630 continue; // branch miss
27631 // [j6]
27632 }
27633 } while (0);
27634 if (bgotonextstatement)
27635 {
27636 }
27637 }
27638 }
27639 }
27640 else
27641 {
27642 {
27643 IkReal j6array[1],
27644 cj6array[1], sj6array[1];
27645 bool j6valid[1] = {false};
27646 _nj6 = 1;
27647 IkReal x1925 = (cj4 * px);
27648 IkReal x1926 = (py * sj4);
27649 IkReal x1927
27650 = ((0.108264705882353)
27651 * cj9);
27652 IkReal x1928
27653 = ((0.588235294117647)
27654 * pp);
27655 IkReal x1929 = (cj9 * pp);
27656 IkReal x1930 = (cj9 * sj9);
27657 IkReal x1931 = (pp * sj9);
27658 IkReal x1932 = cj9 * cj9;
27659 IkReal x1933 = ((1.0) * pz);
27660 CheckValue<IkReal> x1934
27661 = IKPowWithIntegerCheck(
27662 IKsign((
27663 (((1.51009803921569)
27664 * pz))
27665 + (((1.32323529411765)
27666 * cj9 * pz))
27667 + (((-1.0)
27668 * (3.92156862745098)
27669 * pp * pz))
27670 + (((-1.0) * x1926
27671 * x1928))
27672 + (((-1.0) * x1926
27673 * x1927))
27674 + (((-1.0) * x1925
27675 * x1927))
27676 + (((-0.316735294117647)
27677 * x1925))
27678 + (((-0.316735294117647)
27679 * x1926))
27680 + (((-1.0) * x1925
27681 * x1928)))),
27682 -1);
27683 if (!x1934.valid)
27684 {
27685 continue;
27686 }
27687 CheckValue<IkReal> x1935 = IKatan2WithCheck(
27688 IkReal((
27689 (-0.174204411764706)
27690 + (((-0.00487191176470588)
27691 * x1930))
27692 + (((-0.0264705882352941)
27693 * x1931))
27694 + (pz * pz)
27695 + (((-0.176470588235294)
27696 * x1929))
27697 + (((-1.0)
27698 * (0.154566176470588)
27699 * cj9))
27700 + (((-0.0324794117647059)
27701 * x1932))
27702 + (((-1.0)
27703 * (0.323529411764706)
27704 * pp))
27705 + (((-1.0)
27706 * (0.0142530882352941)
27707 * sj9)))),
27708 ((0.830553921568627)
27709 + (((-1.17647058823529)
27710 * x1929))
27711 + (((-0.176470588235294)
27712 * x1931))
27713 + (((0.396970588235294)
27714 * x1932))
27715 + (((-1.0) * x1926
27716 * x1933))
27717 + (((1.18080882352941)
27718 * cj9))
27719 + (((-1.0)
27720 * (2.15686274509804)
27721 * pp))
27722 + (((-1.0) * x1925
27723 * x1933))
27724 + (((0.0679544117647059)
27725 * sj9))
27726 + (((0.0595455882352941)
27727 * x1930))),
27728 IKFAST_ATAN2_MAGTHRESH);
27729 if (!x1935.valid)
27730 {
27731 continue;
27732 }
27733 j6array[0]
27734 = ((-1.5707963267949)
27735 + (((1.5707963267949)
27736 * (x1934.value)))
27737 + (x1935.value));
27738 sj6array[0]
27739 = IKsin(j6array[0]);
27740 cj6array[0]
27741 = IKcos(j6array[0]);
27742 if (j6array[0] > IKPI)
27743 {
27744 j6array[0] -= IK2PI;
27745 }
27746 else if (j6array[0] < -IKPI)
27747 {
27748 j6array[0] += IK2PI;
27749 }
27750 j6valid[0] = true;
27751 for (int ij6 = 0; ij6 < 1;
27752 ++ij6)
27753 {
27754 if (!j6valid[ij6])
27755 {
27756 continue;
27757 }
27758 _ij6[0] = ij6;
27759 _ij6[1] = -1;
27760 for (int iij6 = ij6 + 1;
27761 iij6 < 1;
27762 ++iij6)
27763 {
27764 if (j6valid[iij6]
27765 && IKabs(
27766 cj6array[ij6]
27767 - cj6array
27768 [iij6])
27769 < IKFAST_SOLUTION_THRESH
27770 && IKabs(
27771 sj6array[ij6]
27772 - sj6array
27773 [iij6])
27774 < IKFAST_SOLUTION_THRESH)
27775 {
27776 j6valid[iij6] = false;
27777 _ij6[1] = iij6;
27778 break;
27779 }
27780 }
27781 j6 = j6array[ij6];
27782 cj6 = cj6array[ij6];
27783 sj6 = sj6array[ij6];
27784 {
27785 IkReal evalcond[5];
27786 IkReal x1936
27787 = ((0.3) * cj9);
27788 IkReal x1937
27789 = ((0.045) * sj9);
27790 IkReal x1938 = IKcos(j6);
27791 IkReal x1939
27792 = (pz * x1938);
27793 IkReal x1940 = IKsin(j6);
27794 IkReal x1941 = (cj4 * px);
27795 IkReal x1942
27796 = (x1940 * x1941);
27797 IkReal x1943 = (py * sj4);
27798 IkReal x1944
27799 = (x1940 * x1943);
27800 IkReal x1945
27801 = ((0.045) * cj9);
27802 IkReal x1946
27803 = ((0.3) * sj9);
27804 IkReal x1947
27805 = (pz * x1940);
27806 IkReal x1948
27807 = ((1.0) * x1941);
27808 IkReal x1949
27809 = ((1.0) * x1943);
27810 IkReal x1950
27811 = ((0.09) * x1938);
27812 evalcond[0]
27813 = ((-0.55) + x1944
27814 + x1942 + x1939
27815 + (((-1.0)
27816 * x1937))
27817 + (((-1.0)
27818 * x1936)));
27819 evalcond[1]
27820 = ((0.045) + x1946
27821 + x1947
27822 + (((-1.0)
27823 * x1945))
27824 + (((-1.0) * x1938
27825 * x1948))
27826 + (((-1.0) * x1938
27827 * x1949)));
27828 evalcond[2]
27829 = ((((-1.51009803921569)
27830 * x1940))
27831 + pz
27832 + (((3.92156862745098)
27833 * pp * x1940))
27834 + (((-0.588235294117647)
27835 * pp * x1938))
27836 + (((-0.108264705882353)
27837 * cj9 * x1938))
27838 + (((-1.32323529411765)
27839 * cj9 * x1940))
27840 + (((-0.316735294117647)
27841 * x1938)));
27842 evalcond[3]
27843 = ((((-1.0) * x1948))
27844 + ((x1936 * x1940))
27845 + (((0.55)
27846 * x1940))
27847 + (((-1.0) * x1938
27848 * x1945))
27849 + (((-1.0)
27850 * x1949))
27851 + (((0.045)
27852 * x1938))
27853 + ((x1937 * x1940))
27854 + ((x1938
27855 * x1946)));
27856 evalcond[4]
27857 = ((-0.2125)
27858 + (((1.1) * x1944))
27859 + ((x1943 * x1950))
27860 + ((x1941 * x1950))
27861 + (((1.1) * x1942))
27862 + (((1.1) * x1939))
27863 + (((-0.09)
27864 * x1947))
27865 + (((-1.0) * (1.0)
27866 * pp)));
27867 if (IKabs(evalcond[0])
27868 > IKFAST_EVALCOND_THRESH
27869 || IKabs(evalcond[1])
27870 > IKFAST_EVALCOND_THRESH
27871 || IKabs(evalcond[2])
27872 > IKFAST_EVALCOND_THRESH
27873 || IKabs(evalcond[3])
27874 > IKFAST_EVALCOND_THRESH
27875 || IKabs(evalcond[4])
27876 > IKFAST_EVALCOND_THRESH)
27877 {
27878 continue;
27879 }
27880 }
27881
27882 rotationfunction0(
27883 solutions);
27884 }
27885 }
27886 }
27887 }
27888 }
27889 else
27890 {
27891 {
27892 IkReal j6array[1], cj6array[1],
27893 sj6array[1];
27894 bool j6valid[1] = {false};
27895 _nj6 = 1;
27896 IkReal x1951
27897 = ((0.045) * cj4 * px);
27898 IkReal x1952
27899 = ((0.045) * py * sj4);
27900 IkReal x1953 = ((0.3) * sj9);
27901 IkReal x1954 = (cj4 * px);
27902 IkReal x1955 = (py * sj4);
27903 IkReal x1956 = (cj9 * sj9);
27904 IkReal x1957 = cj9 * cj9;
27905 IkReal x1958 = ((1.0) * pz);
27906 IkReal x1959 = py * py;
27907 IkReal x1960 = cj4 * cj4;
27908 CheckValue<IkReal> x1961
27909 = IKatan2WithCheck(
27910 IkReal((
27911 (0.03825)
27912 + (((-1.0) * x1955
27913 * x1958))
27914 + (((0.087975)
27915 * x1956))
27916 + (((-0.027) * x1957))
27917 + (((0.167025) * sj9))
27918 + (((-1.0) * (0.01125)
27919 * cj9))
27920 + (((-1.0) * x1954
27921 * x1958)))),
27922 ((-0.304525)
27923 + (((-1.0) * x1959
27924 * x1960))
27925 + ((x1960 * (px * px)))
27926 + (((-1.0) * (0.0495)
27927 * sj9))
27928 + (((-0.087975) * x1957))
27929 + (((-1.0) * (0.33)
27930 * cj9))
27931 + (((2.0) * cj4 * px
27932 * x1955))
27933 + (((-0.027) * x1956))
27934 + x1959),
27935 IKFAST_ATAN2_MAGTHRESH);
27936 if (!x1961.valid)
27937 {
27938 continue;
27939 }
27940 CheckValue<IkReal> x1962
27941 = IKPowWithIntegerCheck(
27942 IKsign(
27943 ((((-1.0) * (0.55)
27944 * pz))
27945 + ((x1953 * x1954))
27946 + (((-1.0) * cj9
27947 * x1952))
27948 + ((x1953 * x1955))
27949 + (((-1.0) * cj9
27950 * x1951))
27951 + x1951 + x1952
27952 + (((-1.0) * (0.045)
27953 * pz * sj9))
27954 + (((-1.0) * (0.3)
27955 * cj9 * pz)))),
27956 -1);
27957 if (!x1962.valid)
27958 {
27959 continue;
27960 }
27961 j6array[0]
27962 = ((-1.5707963267949)
27963 + (x1961.value)
27964 + (((1.5707963267949)
27965 * (x1962.value))));
27966 sj6array[0] = IKsin(j6array[0]);
27967 cj6array[0] = IKcos(j6array[0]);
27968 if (j6array[0] > IKPI)
27969 {
27970 j6array[0] -= IK2PI;
27971 }
27972 else if (j6array[0] < -IKPI)
27973 {
27974 j6array[0] += IK2PI;
27975 }
27976 j6valid[0] = true;
27977 for (int ij6 = 0; ij6 < 1; ++ij6)
27978 {
27979 if (!j6valid[ij6])
27980 {
27981 continue;
27982 }
27983 _ij6[0] = ij6;
27984 _ij6[1] = -1;
27985 for (int iij6 = ij6 + 1;
27986 iij6 < 1;
27987 ++iij6)
27988 {
27989 if (j6valid[iij6]
27990 && IKabs(
27991 cj6array[ij6]
27992 - cj6array[iij6])
27993 < IKFAST_SOLUTION_THRESH
27994 && IKabs(
27995 sj6array[ij6]
27996 - sj6array[iij6])
27997 < IKFAST_SOLUTION_THRESH)
27998 {
27999 j6valid[iij6] = false;
28000 _ij6[1] = iij6;
28001 break;
28002 }
28003 }
28004 j6 = j6array[ij6];
28005 cj6 = cj6array[ij6];
28006 sj6 = sj6array[ij6];
28007 {
28008 IkReal evalcond[5];
28009 IkReal x1963 = ((0.3) * cj9);
28010 IkReal x1964
28011 = ((0.045) * sj9);
28012 IkReal x1965 = IKcos(j6);
28013 IkReal x1966 = (pz * x1965);
28014 IkReal x1967 = IKsin(j6);
28015 IkReal x1968 = (cj4 * px);
28016 IkReal x1969
28017 = (x1967 * x1968);
28018 IkReal x1970 = (py * sj4);
28019 IkReal x1971
28020 = (x1967 * x1970);
28021 IkReal x1972
28022 = ((0.045) * cj9);
28023 IkReal x1973 = ((0.3) * sj9);
28024 IkReal x1974 = (pz * x1967);
28025 IkReal x1975
28026 = ((1.0) * x1968);
28027 IkReal x1976
28028 = ((1.0) * x1970);
28029 IkReal x1977
28030 = ((0.09) * x1965);
28031 evalcond[0]
28032 = ((-0.55) + x1966 + x1969
28033 + (((-1.0) * x1964))
28034 + x1971
28035 + (((-1.0) * x1963)));
28036 evalcond[1]
28037 = ((0.045)
28038 + (((-1.0) * x1965
28039 * x1976))
28040 + (((-1.0) * x1965
28041 * x1975))
28042 + (((-1.0) * x1972))
28043 + x1973 + x1974);
28044 evalcond[2]
28045 = ((((-0.316735294117647)
28046 * x1965))
28047 + (((-0.588235294117647)
28048 * pp * x1965))
28049 + pz
28050 + (((-1.51009803921569)
28051 * x1967))
28052 + (((-0.108264705882353)
28053 * cj9 * x1965))
28054 + (((3.92156862745098)
28055 * pp * x1967))
28056 + (((-1.32323529411765)
28057 * cj9 * x1967)));
28058 evalcond[3]
28059 = ((((0.045) * x1965))
28060 + ((x1964 * x1967))
28061 + ((x1963 * x1967))
28062 + (((-1.0) * x1976))
28063 + (((-1.0) * x1965
28064 * x1972))
28065 + (((-1.0) * x1975))
28066 + (((0.55) * x1967))
28067 + ((x1965 * x1973)));
28068 evalcond[4]
28069 = ((-0.2125)
28070 + (((1.1) * x1966))
28071 + (((1.1) * x1969))
28072 + (((1.1) * x1971))
28073 + (((-0.09) * x1974))
28074 + ((x1970 * x1977))
28075 + ((x1968 * x1977))
28076 + (((-1.0) * (1.0)
28077 * pp)));
28078 if (IKabs(evalcond[0])
28079 > IKFAST_EVALCOND_THRESH
28080 || IKabs(evalcond[1])
28081 > IKFAST_EVALCOND_THRESH
28082 || IKabs(evalcond[2])
28083 > IKFAST_EVALCOND_THRESH
28084 || IKabs(evalcond[3])
28085 > IKFAST_EVALCOND_THRESH
28086 || IKabs(evalcond[4])
28087 > IKFAST_EVALCOND_THRESH)
28088 {
28089 continue;
28090 }
28091 }
28092
28093 rotationfunction0(solutions);
28094 }
28095 }
28096 }
28097 }
28098 }
28099 else
28100 {
28101 {
28102 IkReal j6array[1], cj6array[1],
28103 sj6array[1];
28104 bool j6valid[1] = {false};
28105 _nj6 = 1;
28106 IkReal x1978 = (cj4 * px);
28107 IkReal x1979 = (py * sj4);
28108 IkReal x1980
28109 = ((1.32323529411765) * cj9);
28110 IkReal x1981
28111 = ((3.92156862745098) * pp);
28112 IkReal x1982
28113 = ((0.0264705882352941) * pp);
28114 IkReal x1983 = (cj9 * sj9);
28115 IkReal x1984
28116 = ((0.176470588235294) * pp);
28117 IkReal x1985 = cj9 * cj9;
28118 CheckValue<IkReal> x1986
28119 = IKatan2WithCheck(
28120 IkReal((
28121 (-0.0142530882352941)
28122 + ((pz * x1979))
28123 + (((0.00938117647058823)
28124 * cj9))
28125 + ((cj9 * x1982))
28126 + (((-0.0324794117647059)
28127 * x1983))
28128 + (((0.00487191176470588)
28129 * x1985))
28130 + (((-1.0) * x1982))
28131 + ((pz * x1978))
28132 + (((-1.0) * sj9 * x1984))
28133 + (((-1.0)
28134 * (0.0950205882352941)
28135 * sj9)))),
28136 ((0.0679544117647059)
28137 + (((0.396970588235294)
28138 * x1983))
28139 + (((-0.0595455882352941)
28140 * x1985))
28141 + (pz * pz) + ((cj9 * x1984))
28142 + (((-1.0)
28143 * (0.00840882352941177)
28144 * cj9))
28145 + (((0.453029411764706)
28146 * sj9))
28147 + (((-1.0) * x1984))
28148 + (((-1.0)
28149 * (1.17647058823529) * pp
28150 * sj9))),
28151 IKFAST_ATAN2_MAGTHRESH);
28152 if (!x1986.valid)
28153 {
28154 continue;
28155 }
28156 CheckValue<IkReal> x1987
28157 = IKPowWithIntegerCheck(
28158 IKsign((
28159 (((-1.0) * x1978 * x1981))
28160 + (((1.51009803921569)
28161 * x1978))
28162 + (((1.51009803921569)
28163 * x1979))
28164 + (((0.316735294117647)
28165 * pz))
28166 + (((0.108264705882353)
28167 * cj9 * pz))
28168 + (((0.588235294117647)
28169 * pp * pz))
28170 + ((x1978 * x1980))
28171 + (((-1.0) * x1979
28172 * x1981))
28173 + ((x1979 * x1980)))),
28174 -1);
28175 if (!x1987.valid)
28176 {
28177 continue;
28178 }
28179 j6array[0]
28180 = ((-1.5707963267949)
28181 + (x1986.value)
28182 + (((1.5707963267949)
28183 * (x1987.value))));
28184 sj6array[0] = IKsin(j6array[0]);
28185 cj6array[0] = IKcos(j6array[0]);
28186 if (j6array[0] > IKPI)
28187 {
28188 j6array[0] -= IK2PI;
28189 }
28190 else if (j6array[0] < -IKPI)
28191 {
28192 j6array[0] += IK2PI;
28193 }
28194 j6valid[0] = true;
28195 for (int ij6 = 0; ij6 < 1; ++ij6)
28196 {
28197 if (!j6valid[ij6])
28198 {
28199 continue;
28200 }
28201 _ij6[0] = ij6;
28202 _ij6[1] = -1;
28203 for (int iij6 = ij6 + 1; iij6 < 1;
28204 ++iij6)
28205 {
28206 if (j6valid[iij6]
28207 && IKabs(
28208 cj6array[ij6]
28209 - cj6array[iij6])
28210 < IKFAST_SOLUTION_THRESH
28211 && IKabs(
28212 sj6array[ij6]
28213 - sj6array[iij6])
28214 < IKFAST_SOLUTION_THRESH)
28215 {
28216 j6valid[iij6] = false;
28217 _ij6[1] = iij6;
28218 break;
28219 }
28220 }
28221 j6 = j6array[ij6];
28222 cj6 = cj6array[ij6];
28223 sj6 = sj6array[ij6];
28224 {
28225 IkReal evalcond[5];
28226 IkReal x1988 = ((0.3) * cj9);
28227 IkReal x1989 = ((0.045) * sj9);
28228 IkReal x1990 = IKcos(j6);
28229 IkReal x1991 = (pz * x1990);
28230 IkReal x1992 = IKsin(j6);
28231 IkReal x1993 = (cj4 * px);
28232 IkReal x1994 = (x1992 * x1993);
28233 IkReal x1995 = (py * sj4);
28234 IkReal x1996 = (x1992 * x1995);
28235 IkReal x1997 = ((0.045) * cj9);
28236 IkReal x1998 = ((0.3) * sj9);
28237 IkReal x1999 = (pz * x1992);
28238 IkReal x2000 = ((1.0) * x1993);
28239 IkReal x2001 = ((1.0) * x1995);
28240 IkReal x2002 = ((0.09) * x1990);
28241 evalcond[0]
28242 = ((-0.55)
28243 + (((-1.0) * x1989))
28244 + x1996 + x1994 + x1991
28245 + (((-1.0) * x1988)));
28246 evalcond[1]
28247 = ((0.045)
28248 + (((-1.0) * x1990
28249 * x2000))
28250 + (((-1.0) * x1997))
28251 + x1998 + x1999
28252 + (((-1.0) * x1990
28253 * x2001)));
28254 evalcond[2]
28255 = ((((-0.108264705882353)
28256 * cj9 * x1990))
28257 + (((-0.588235294117647)
28258 * pp * x1990))
28259 + pz
28260 + (((-1.51009803921569)
28261 * x1992))
28262 + (((-1.32323529411765)
28263 * cj9 * x1992))
28264 + (((3.92156862745098) * pp
28265 * x1992))
28266 + (((-0.316735294117647)
28267 * x1990)));
28268 evalcond[3]
28269 = ((((0.045) * x1990))
28270 + (((-1.0) * x2001))
28271 + (((0.55) * x1992))
28272 + (((-1.0) * x2000))
28273 + (((-1.0) * x1990
28274 * x1997))
28275 + ((x1990 * x1998))
28276 + ((x1989 * x1992))
28277 + ((x1988 * x1992)));
28278 evalcond[4]
28279 = ((-0.2125)
28280 + (((1.1) * x1996))
28281 + (((1.1) * x1991))
28282 + ((x1995 * x2002))
28283 + (((-0.09) * x1999))
28284 + ((x1993 * x2002))
28285 + (((-1.0) * (1.0) * pp))
28286 + (((1.1) * x1994)));
28287 if (IKabs(evalcond[0])
28288 > IKFAST_EVALCOND_THRESH
28289 || IKabs(evalcond[1])
28290 > IKFAST_EVALCOND_THRESH
28291 || IKabs(evalcond[2])
28292 > IKFAST_EVALCOND_THRESH
28293 || IKabs(evalcond[3])
28294 > IKFAST_EVALCOND_THRESH
28295 || IKabs(evalcond[4])
28296 > IKFAST_EVALCOND_THRESH)
28297 {
28298 continue;
28299 }
28300 }
28301
28302 rotationfunction0(solutions);
28303 }
28304 }
28305 }
28306 }
28307 }
28308 } while (0);
28309 if (bgotonextstatement)
28310 {
28311 bool bgotonextstatement = true;
28312 do
28313 {
28314 IkReal x2003 = (px * sj4);
28315 IkReal x2004 = (cj4 * py);
28316 evalcond[0]
28317 = ((-3.14159265358979)
28318 + (IKfmod(
28319 ((3.14159265358979)
28320 + (IKabs(
28321 ((-3.14159265358979)
28322 + j8)))),
28323 6.28318530717959)));
28324 evalcond[1]
28325 = ((0.39655) + (((0.0765) * sj9))
28326 + (((0.32595) * cj9))
28327 + (((-1.0) * (1.0) * pp)));
28328 evalcond[2] = (x2003 + (((-1.0) * x2004)));
28329 evalcond[3] = ((((-1.0) * x2003)) + x2004);
28330 if (IKabs(evalcond[0]) < 0.0000010000000000
28331 && IKabs(evalcond[1])
28332 < 0.0000010000000000
28333 && IKabs(evalcond[2])
28334 < 0.0000010000000000
28335 && IKabs(evalcond[3])
28336 < 0.0000010000000000)
28337 {
28338 bgotonextstatement = false;
28339 {
28340 IkReal j6eval[2];
28341 sj8 = 0;
28342 cj8 = -1.0;
28343 j8 = 3.14159265358979;
28344 IkReal x2005 = py * py;
28345 IkReal x2006 = cj4 * cj4;
28346 IkReal x2007
28347 = (x2005
28348 + (((-1.0) * x2005 * x2006))
28349 + (pz * pz)
28350 + (((2.0) * cj4 * px * py * sj4))
28351 + ((x2006 * (px * px))));
28352 j6eval[0] = x2007;
28353 j6eval[1] = IKsign(x2007);
28354 if (IKabs(j6eval[0])
28355 < 0.0000010000000000
28356 || IKabs(j6eval[1])
28357 < 0.0000010000000000)
28358 {
28359 {
28360 IkReal j6eval[2];
28361 sj8 = 0;
28362 cj8 = -1.0;
28363 j8 = 3.14159265358979;
28364 IkReal x2008 = (cj4 * px);
28365 IkReal x2009 = (cj9 * pz);
28366 IkReal x2010 = (py * sj4);
28367 IkReal x2011 = (pz * sj9);
28368 IkReal x2012 = (cj4 * px * sj9);
28369 IkReal x2013 = (py * sj4 * sj9);
28370 IkReal x2014 = ((0.045) * x2008);
28371 IkReal x2015 = ((0.045) * x2010);
28372 j6eval[0]
28373 = (((cj9 * x2010))
28374 + (((-1.0) * x2010))
28375 + ((cj9 * x2008))
28376 + (((-1.0)
28377 * (12.2222222222222)
28378 * pz))
28379 + (((-1.0) * x2011))
28380 + (((-6.66666666666667)
28381 * x2009))
28382 + (((-6.66666666666667)
28383 * x2013))
28384 + (((-1.0) * x2008))
28385 + (((-6.66666666666667)
28386 * x2012)));
28387 j6eval[1] = IKsign(
28388 (((cj9 * x2014))
28389 + (((-0.3) * x2012))
28390 + (((-0.3) * x2013))
28391 + (((-1.0) * (0.55) * pz))
28392 + (((-0.045) * x2011))
28393 + ((cj9 * x2015))
28394 + (((-1.0) * x2014))
28395 + (((-1.0) * x2015))
28396 + (((-0.3) * x2009))));
28397 if (IKabs(j6eval[0])
28398 < 0.0000010000000000
28399 || IKabs(j6eval[1])
28400 < 0.0000010000000000)
28401 {
28402 {
28403 IkReal j6eval[2];
28404 sj8 = 0;
28405 cj8 = -1.0;
28406 j8 = 3.14159265358979;
28407 IkReal x2016 = (cj4 * px);
28408 IkReal x2017 = (cj9 * pz);
28409 IkReal x2018 = (pp * pz);
28410 IkReal x2019 = (py * sj4);
28411 IkReal x2020 = (cj4 * cj9 * px);
28412 IkReal x2021 = (cj4 * pp * px);
28413 IkReal x2022 = (cj9 * py * sj4);
28414 IkReal x2023 = (pp * py * sj4);
28415 j6eval[0]
28416 = ((((-2.92556370551481)
28417 * x2016))
28418 + (((-1.0)
28419 * (13.9482024812098)
28420 * pz))
28421 + (((-5.4333061668025)
28422 * x2021))
28423 + (((-1.0) * x2022))
28424 + (((-12.2222222222222)
28425 * x2017))
28426 + (((-5.4333061668025)
28427 * x2023))
28428 + (((-2.92556370551481)
28429 * x2019))
28430 + (((36.2220411120167)
28431 * x2018))
28432 + (((-1.0) * x2020)));
28433 j6eval[1] = IKsign(
28434 ((((-0.588235294117647)
28435 * x2021))
28436 + (((-1.32323529411765)
28437 * x2017))
28438 + (((3.92156862745098)
28439 * x2018))
28440 + (((-0.316735294117647)
28441 * x2019))
28442 + (((-1.0)
28443 * (1.51009803921569)
28444 * pz))
28445 + (((-0.108264705882353)
28446 * x2020))
28447 + (((-0.108264705882353)
28448 * x2022))
28449 + (((-0.588235294117647)
28450 * x2023))
28451 + (((-0.316735294117647)
28452 * x2016))));
28453 if (IKabs(j6eval[0])
28454 < 0.0000010000000000
28455 || IKabs(j6eval[1])
28456 < 0.0000010000000000)
28457 {
28458 {
28459 IkReal evalcond[1];
28460 bool bgotonextstatement
28461 = true;
28462 do
28463 {
28464 evalcond[0]
28465 = ((IKabs((
28466 (-3.14159265358979)
28467 + (IKfmod(
28468 ((3.14159265358979)
28469 + j9),
28470 6.28318530717959)))))
28471 + (IKabs(pz)));
28472 if (IKabs(evalcond[0])
28473 < 0.0000010000000000)
28474 {
28475 bgotonextstatement
28476 = false;
28477 {
28478 IkReal j6eval[1];
28479 IkReal x2024
28480 = ((1.0) * py);
28481 sj8 = 0;
28482 cj8 = -1.0;
28483 j8 = 3.14159265358979;
28484 pz = 0;
28485 j9 = 0;
28486 sj9 = 0;
28487 cj9 = 1.0;
28488 pp
28489 = ((py * py)
28490 + (px * px));
28491 npx
28492 = (((py * r10))
28493 + ((px
28494 * r00)));
28495 npy
28496 = (((py * r11))
28497 + ((px
28498 * r01)));
28499 npz
28500 = (((px * r02))
28501 + ((py
28502 * r12)));
28503 rxp0_0
28504 = ((-1.0) * r20
28505 * x2024);
28506 rxp0_1 = (px * r20);
28507 rxp1_0
28508 = ((-1.0) * r21
28509 * x2024);
28510 rxp1_1 = (px * r21);
28511 rxp2_0
28512 = ((-1.0) * r22
28513 * x2024);
28514 rxp2_1 = (px * r22);
28515 j6eval[0]
28516 = ((((-1.0)
28517 * (1.0) * cj4
28518 * px))
28519 + (((-1.0)
28520 * (1.0)
28521 * py
28522 * sj4)));
28523 if (IKabs(j6eval[0])
28524 < 0.0000010000000000)
28525 {
28526 {
28527 IkReal j6eval[1];
28528 IkReal x2025
28529 = ((1.0)
28530 * py);
28531 sj8 = 0;
28532 cj8 = -1.0;
28533 j8 = 3.14159265358979;
28534 pz = 0;
28535 j9 = 0;
28536 sj9 = 0;
28537 cj9 = 1.0;
28538 pp
28539 = ((py * py)
28540 + (px
28541 * px));
28542 npx
28543 = (((py
28544 * r10))
28545 + ((px
28546 * r00)));
28547 npy
28548 = (((py
28549 * r11))
28550 + ((px
28551 * r01)));
28552 npz
28553 = (((px
28554 * r02))
28555 + ((py
28556 * r12)));
28557 rxp0_0
28558 = ((-1.0)
28559 * r20
28560 * x2025);
28561 rxp0_1
28562 = (px * r20);
28563 rxp1_0
28564 = ((-1.0)
28565 * r21
28566 * x2025);
28567 rxp1_1
28568 = (px * r21);
28569 rxp2_0
28570 = ((-1.0)
28571 * r22
28572 * x2025);
28573 rxp2_1
28574 = (px * r22);
28575 j6eval[0]
28576 = ((-1.0)
28577 + (((-1.0)
28578 * (1.3840830449827)
28579 * (px
28580 * px)))
28581 + (((-1.0)
28582 * (1.3840830449827)
28583 * (py
28584 * py))));
28585 if (IKabs(
28586 j6eval[0])
28587 < 0.0000010000000000)
28588 {
28589 {
28590 IkReal
28591 j6eval[2];
28592 IkReal x2026
28593 = ((1.0)
28594 * py);
28595 sj8 = 0;
28596 cj8 = -1.0;
28597 j8 = 3.14159265358979;
28598 pz = 0;
28599 j9 = 0;
28600 sj9 = 0;
28601 cj9 = 1.0;
28602 pp
28603 = ((py
28604 * py)
28605 + (px
28606 * px));
28607 npx
28608 = (((py
28609 * r10))
28610 + ((px
28611 * r00)));
28612 npy
28613 = (((py
28614 * r11))
28615 + ((px
28616 * r01)));
28617 npz
28618 = (((px
28619 * r02))
28620 + ((py
28621 * r12)));
28622 rxp0_0
28623 = ((-1.0)
28624 * r20
28625 * x2026);
28626 rxp0_1
28627 = (px
28628 * r20);
28629 rxp1_0
28630 = ((-1.0)
28631 * r21
28632 * x2026);
28633 rxp1_1
28634 = (px
28635 * r21);
28636 rxp2_0
28637 = ((-1.0)
28638 * r22
28639 * x2026);
28640 rxp2_1
28641 = (px
28642 * r22);
28643 IkReal x2027
28644 = (cj4
28645 * px);
28646 IkReal x2028
28647 = (py
28648 * sj4);
28649 j6eval[0]
28650 = (x2027
28651 + x2028);
28652 j6eval[1]
28653 = ((((-1.0)
28654 * (1.3840830449827)
28655 * cj4
28656 * (px
28657 * px
28658 * px)))
28659 + (((-1.0)
28660 * x2028))
28661 + (((-1.3840830449827)
28662 * x2027
28663 * (py
28664 * py)))
28665 + (((-1.0)
28666 * (1.3840830449827)
28667 * sj4
28668 * (py
28669 * py
28670 * py)))
28671 + (((-1.0)
28672 * x2027))
28673 + (((-1.3840830449827)
28674 * x2028
28675 * (px
28676 * px))));
28677 if (IKabs(
28678 j6eval
28679 [0])
28680 < 0.0000010000000000
28681 || IKabs(
28682 j6eval
28683 [1])
28684 < 0.0000010000000000)
28685 {
28686 {
28687 IkReal evalcond
28688 [4];
28689 bool
28690 bgotonextstatement
28691 = true;
28692 do
28693 {
28694 evalcond
28695 [0]
28696 = ((IKabs(
28697 py))
28698 + (IKabs(
28699 px)));
28700 evalcond
28701 [1]
28702 = -0.85;
28703 evalcond
28704 [2]
28705 = 0;
28706 evalcond
28707 [3]
28708 = -0.2125;
28709 if (IKabs(
28710 evalcond
28711 [0])
28712 < 0.0000010000000000
28713 && IKabs(
28714 evalcond
28715 [1])
28716 < 0.0000010000000000
28717 && IKabs(
28718 evalcond
28719 [2])
28720 < 0.0000010000000000
28721 && IKabs(
28722 evalcond
28723 [3])
28724 < 0.0000010000000000)
28725 {
28726 bgotonextstatement
28727 = false;
28728 {
28729 IkReal j6array
28730 [2],
28731 cj6array
28732 [2],
28733 sj6array
28734 [2];
28735 bool j6valid
28736 [2]
28737 = {false};
28738 _nj6
28739 = 2;
28740 j6array
28741 [0]
28742 = 0.148889947609497;
28743 sj6array
28744 [0]
28745 = IKsin(
28746 j6array
28747 [0]);
28748 cj6array
28749 [0]
28750 = IKcos(
28751 j6array
28752 [0]);
28753 j6array
28754 [1]
28755 = 3.29048260119929;
28756 sj6array
28757 [1]
28758 = IKsin(
28759 j6array
28760 [1]);
28761 cj6array
28762 [1]
28763 = IKcos(
28764 j6array
28765 [1]);
28766 if (j6array
28767 [0]
28768 > IKPI)
28769 {
28770 j6array
28771 [0]
28772 -= IK2PI;
28773 }
28774 else if (
28775 j6array
28776 [0]
28777 < -IKPI)
28778 {
28779 j6array
28780 [0]
28781 += IK2PI;
28782 }
28783 j6valid
28784 [0]
28785 = true;
28786 if (j6array
28787 [1]
28788 > IKPI)
28789 {
28790 j6array
28791 [1]
28792 -= IK2PI;
28793 }
28794 else if (
28795 j6array
28796 [1]
28797 < -IKPI)
28798 {
28799 j6array
28800 [1]
28801 += IK2PI;
28802 }
28803 j6valid
28804 [1]
28805 = true;
28806 for (
28807 int ij6
28808 = 0;
28809 ij6
28810 < 2;
28811 ++ij6)
28812 {
28813 if (!j6valid
28814 [ij6])
28815 {
28816 continue;
28817 }
28818 _ij6[0]
28819 = ij6;
28820 _ij6[1]
28821 = -1;
28822 for (
28823 int iij6
28824 = ij6
28825 + 1;
28826 iij6
28827 < 2;
28828 ++iij6)
28829 {
28830 if (j6valid
28831 [iij6]
28832 && IKabs(
28833 cj6array
28834 [ij6]
28835 - cj6array
28836 [iij6])
28837 < IKFAST_SOLUTION_THRESH
28838 && IKabs(
28839 sj6array
28840 [ij6]
28841 - sj6array
28842 [iij6])
28843 < IKFAST_SOLUTION_THRESH)
28844 {
28845 j6valid
28846 [iij6]
28847 = false;
28848 _ij6[1]
28849 = iij6;
28850 break;
28851 }
28852 }
28853 j6 = j6array
28854 [ij6];
28855 cj6 = cj6array
28856 [ij6];
28857 sj6 = sj6array
28858 [ij6];
28859 {
28860 IkReal evalcond
28861 [1];
28862 evalcond
28863 [0]
28864 = ((0.85)
28865 * (IKsin(
28866 j6)));
28867 if (IKabs(
28868 evalcond
28869 [0])
28870 > IKFAST_EVALCOND_THRESH)
28871 {
28872 continue;
28873 }
28874 }
28875
28876 rotationfunction0(
28877 solutions);
28878 }
28879 }
28880 }
28881 } while (
28882 0);
28883 if (bgotonextstatement)
28884 {
28885 bool
28886 bgotonextstatement
28887 = true;
28888 do
28889 {
28890 evalcond
28891 [0]
28892 = ((IKabs((
28893 (-3.14159265358979)
28894 + (IKfmod(
28895 ((3.14159265358979)
28896 + j4),
28897 6.28318530717959)))))
28898 + (IKabs(
28899 px)));
28900 evalcond
28901 [1]
28902 = -0.85;
28903 evalcond
28904 [2]
28905 = 0;
28906 evalcond
28907 [3]
28908 = ((-0.2125)
28909 + (((-1.0)
28910 * (1.0)
28911 * (py
28912 * py))));
28913 if (IKabs(
28914 evalcond
28915 [0])
28916 < 0.0000010000000000
28917 && IKabs(
28918 evalcond
28919 [1])
28920 < 0.0000010000000000
28921 && IKabs(
28922 evalcond
28923 [2])
28924 < 0.0000010000000000
28925 && IKabs(
28926 evalcond
28927 [3])
28928 < 0.0000010000000000)
28929 {
28930 bgotonextstatement
28931 = false;
28932 {
28933 IkReal j6eval
28934 [1];
28935 IkReal
28936 x2029
28937 = ((1.0)
28938 * py);
28939 sj8 = 0;
28940 cj8 = -1.0;
28941 j8 = 3.14159265358979;
28942 pz = 0;
28943 j9 = 0;
28944 sj9 = 0;
28945 cj9 = 1.0;
28946 pp = py
28947 * py;
28948 npx
28949 = (py
28950 * r10);
28951 npy
28952 = (py
28953 * r11);
28954 npz
28955 = (py
28956 * r12);
28957 rxp0_0
28958 = ((-1.0)
28959 * r20
28960 * x2029);
28961 rxp0_1
28962 = 0;
28963 rxp1_0
28964 = ((-1.0)
28965 * r21
28966 * x2029);
28967 rxp1_1
28968 = 0;
28969 rxp2_0
28970 = ((-1.0)
28971 * r22
28972 * x2029);
28973 rxp2_1
28974 = 0;
28975 px = 0;
28976 j4 = 0;
28977 sj4 = 0;
28978 cj4 = 1.0;
28979 rxp0_2
28980 = (py
28981 * r00);
28982 rxp1_2
28983 = (py
28984 * r01);
28985 rxp2_2
28986 = (py
28987 * r02);
28988 j6eval
28989 [0]
28990 = ((1.0)
28991 + (((1.91568587540858)
28992 * (py
28993 * py
28994 * py
28995 * py)))
28996 + (((-1.0)
28997 * (2.64633970947792)
28998 * (py
28999 * py))));
29000 if (IKabs(
29001 j6eval
29002 [0])
29003 < 0.0000010000000000)
29004 {
29005 continue; // no branches [j6]
29006 }
29007 else
29008 {
29009 {
29010 IkReal j6array
29011 [2],
29012 cj6array
29013 [2],
29014 sj6array
29015 [2];
29016 bool j6valid
29017 [2]
29018 = {false};
29019 _nj6
29020 = 2;
29021 IkReal
29022 x2030
29023 = py
29024 * py;
29025 CheckValue<IkReal> x2032 = IKatan2WithCheck(
29026 IkReal((
29027 (-0.425)
29028 + (((-0.588235294117647)
29029 * x2030)))),
29030 ((2.83333333333333)
29031 + (((-3.92156862745098)
29032 * x2030))),
29033 IKFAST_ATAN2_MAGTHRESH);
29034 if (!x2032
29035 .valid)
29036 {
29037 continue;
29038 }
29039 IkReal
29040 x2031
29041 = ((-1.0)
29042 * (x2032
29043 .value));
29044 j6array
29045 [0]
29046 = x2031;
29047 sj6array
29048 [0]
29049 = IKsin(
29050 j6array
29051 [0]);
29052 cj6array
29053 [0]
29054 = IKcos(
29055 j6array
29056 [0]);
29057 j6array
29058 [1]
29059 = ((3.14159265358979)
29060 + x2031);
29061 sj6array
29062 [1]
29063 = IKsin(
29064 j6array
29065 [1]);
29066 cj6array
29067 [1]
29068 = IKcos(
29069 j6array
29070 [1]);
29071 if (j6array
29072 [0]
29073 > IKPI)
29074 {
29075 j6array
29076 [0]
29077 -= IK2PI;
29078 }
29079 else if (
29080 j6array
29081 [0]
29082 < -IKPI)
29083 {
29084 j6array
29085 [0]
29086 += IK2PI;
29087 }
29088 j6valid
29089 [0]
29090 = true;
29091 if (j6array
29092 [1]
29093 > IKPI)
29094 {
29095 j6array
29096 [1]
29097 -= IK2PI;
29098 }
29099 else if (
29100 j6array
29101 [1]
29102 < -IKPI)
29103 {
29104 j6array
29105 [1]
29106 += IK2PI;
29107 }
29108 j6valid
29109 [1]
29110 = true;
29111 for (
29112 int ij6
29113 = 0;
29114 ij6
29115 < 2;
29116 ++ij6)
29117 {
29118 if (!j6valid
29119 [ij6])
29120 {
29121 continue;
29122 }
29123 _ij6[0]
29124 = ij6;
29125 _ij6[1]
29126 = -1;
29127 for (
29128 int iij6
29129 = ij6
29130 + 1;
29131 iij6
29132 < 2;
29133 ++iij6)
29134 {
29135 if (j6valid
29136 [iij6]
29137 && IKabs(
29138 cj6array
29139 [ij6]
29140 - cj6array
29141 [iij6])
29142 < IKFAST_SOLUTION_THRESH
29143 && IKabs(
29144 sj6array
29145 [ij6]
29146 - sj6array
29147 [iij6])
29148 < IKFAST_SOLUTION_THRESH)
29149 {
29150 j6valid
29151 [iij6]
29152 = false;
29153 _ij6[1]
29154 = iij6;
29155 break;
29156 }
29157 }
29158 j6 = j6array
29159 [ij6];
29160 cj6 = cj6array
29161 [ij6];
29162 sj6 = sj6array
29163 [ij6];
29164 {
29165 IkReal evalcond
29166 [1];
29167 evalcond
29168 [0]
29169 = ((0.85)
29170 * (IKsin(
29171 j6)));
29172 if (IKabs(
29173 evalcond
29174 [0])
29175 > IKFAST_EVALCOND_THRESH)
29176 {
29177 continue;
29178 }
29179 }
29180
29181 rotationfunction0(
29182 solutions);
29183 }
29184 }
29185 }
29186 }
29187 }
29188 } while (
29189 0);
29190 if (bgotonextstatement)
29191 {
29192 bool
29193 bgotonextstatement
29194 = true;
29195 do
29196 {
29197 evalcond
29198 [0]
29199 = ((IKabs((
29200 (-3.14159265358979)
29201 + (IKfmod(
29202 j4,
29203 6.28318530717959)))))
29204 + (IKabs(
29205 px)));
29206 evalcond
29207 [1]
29208 = -0.85;
29209 evalcond
29210 [2]
29211 = 0;
29212 evalcond
29213 [3]
29214 = ((-0.2125)
29215 + (((-1.0)
29216 * (1.0)
29217 * (py
29218 * py))));
29219 if (IKabs(
29220 evalcond
29221 [0])
29222 < 0.0000010000000000
29223 && IKabs(
29224 evalcond
29225 [1])
29226 < 0.0000010000000000
29227 && IKabs(
29228 evalcond
29229 [2])
29230 < 0.0000010000000000
29231 && IKabs(
29232 evalcond
29233 [3])
29234 < 0.0000010000000000)
29235 {
29236 bgotonextstatement
29237 = false;
29238 {
29239 IkReal j6eval
29240 [1];
29241 IkReal
29242 x2033
29243 = ((1.0)
29244 * py);
29245 sj8 = 0;
29246 cj8 = -1.0;
29247 j8 = 3.14159265358979;
29248 pz = 0;
29249 j9 = 0;
29250 sj9 = 0;
29251 cj9 = 1.0;
29252 pp = py
29253 * py;
29254 npx
29255 = (py
29256 * r10);
29257 npy
29258 = (py
29259 * r11);
29260 npz
29261 = (py
29262 * r12);
29263 rxp0_0
29264 = ((-1.0)
29265 * r20
29266 * x2033);
29267 rxp0_1
29268 = 0;
29269 rxp1_0
29270 = ((-1.0)
29271 * r21
29272 * x2033);
29273 rxp1_1
29274 = 0;
29275 rxp2_0
29276 = ((-1.0)
29277 * r22
29278 * x2033);
29279 rxp2_1
29280 = 0;
29281 px = 0;
29282 j4 = 3.14159265358979;
29283 sj4 = 0;
29284 cj4 = -1.0;
29285 rxp0_2
29286 = (py
29287 * r00);
29288 rxp1_2
29289 = (py
29290 * r01);
29291 rxp2_2
29292 = (py
29293 * r02);
29294 j6eval
29295 [0]
29296 = ((1.0)
29297 + (((1.91568587540858)
29298 * (py
29299 * py
29300 * py
29301 * py)))
29302 + (((-1.0)
29303 * (2.64633970947792)
29304 * (py
29305 * py))));
29306 if (IKabs(
29307 j6eval
29308 [0])
29309 < 0.0000010000000000)
29310 {
29311 continue; // no branches [j6]
29312 }
29313 else
29314 {
29315 {
29316 IkReal j6array
29317 [2],
29318 cj6array
29319 [2],
29320 sj6array
29321 [2];
29322 bool j6valid
29323 [2]
29324 = {false};
29325 _nj6
29326 = 2;
29327 IkReal
29328 x2034
29329 = py
29330 * py;
29331 CheckValue<IkReal> x2036 = IKatan2WithCheck(
29332 IkReal((
29333 (-0.425)
29334 + (((-0.588235294117647)
29335 * x2034)))),
29336 ((2.83333333333333)
29337 + (((-3.92156862745098)
29338 * x2034))),
29339 IKFAST_ATAN2_MAGTHRESH);
29340 if (!x2036
29341 .valid)
29342 {
29343 continue;
29344 }
29345 IkReal
29346 x2035
29347 = ((-1.0)
29348 * (x2036
29349 .value));
29350 j6array
29351 [0]
29352 = x2035;
29353 sj6array
29354 [0]
29355 = IKsin(
29356 j6array
29357 [0]);
29358 cj6array
29359 [0]
29360 = IKcos(
29361 j6array
29362 [0]);
29363 j6array
29364 [1]
29365 = ((3.14159265358979)
29366 + x2035);
29367 sj6array
29368 [1]
29369 = IKsin(
29370 j6array
29371 [1]);
29372 cj6array
29373 [1]
29374 = IKcos(
29375 j6array
29376 [1]);
29377 if (j6array
29378 [0]
29379 > IKPI)
29380 {
29381 j6array
29382 [0]
29383 -= IK2PI;
29384 }
29385 else if (
29386 j6array
29387 [0]
29388 < -IKPI)
29389 {
29390 j6array
29391 [0]
29392 += IK2PI;
29393 }
29394 j6valid
29395 [0]
29396 = true;
29397 if (j6array
29398 [1]
29399 > IKPI)
29400 {
29401 j6array
29402 [1]
29403 -= IK2PI;
29404 }
29405 else if (
29406 j6array
29407 [1]
29408 < -IKPI)
29409 {
29410 j6array
29411 [1]
29412 += IK2PI;
29413 }
29414 j6valid
29415 [1]
29416 = true;
29417 for (
29418 int ij6
29419 = 0;
29420 ij6
29421 < 2;
29422 ++ij6)
29423 {
29424 if (!j6valid
29425 [ij6])
29426 {
29427 continue;
29428 }
29429 _ij6[0]
29430 = ij6;
29431 _ij6[1]
29432 = -1;
29433 for (
29434 int iij6
29435 = ij6
29436 + 1;
29437 iij6
29438 < 2;
29439 ++iij6)
29440 {
29441 if (j6valid
29442 [iij6]
29443 && IKabs(
29444 cj6array
29445 [ij6]
29446 - cj6array
29447 [iij6])
29448 < IKFAST_SOLUTION_THRESH
29449 && IKabs(
29450 sj6array
29451 [ij6]
29452 - sj6array
29453 [iij6])
29454 < IKFAST_SOLUTION_THRESH)
29455 {
29456 j6valid
29457 [iij6]
29458 = false;
29459 _ij6[1]
29460 = iij6;
29461 break;
29462 }
29463 }
29464 j6 = j6array
29465 [ij6];
29466 cj6 = cj6array
29467 [ij6];
29468 sj6 = sj6array
29469 [ij6];
29470 {
29471 IkReal evalcond
29472 [1];
29473 evalcond
29474 [0]
29475 = ((0.85)
29476 * (IKsin(
29477 j6)));
29478 if (IKabs(
29479 evalcond
29480 [0])
29481 > IKFAST_EVALCOND_THRESH)
29482 {
29483 continue;
29484 }
29485 }
29486
29487 rotationfunction0(
29488 solutions);
29489 }
29490 }
29491 }
29492 }
29493 }
29494 } while (
29495 0);
29496 if (bgotonextstatement)
29497 {
29498 bool
29499 bgotonextstatement
29500 = true;
29501 do
29502 {
29503 evalcond
29504 [0]
29505 = ((IKabs((
29506 (-3.14159265358979)
29507 + (IKfmod(
29508 ((1.5707963267949)
29509 + j4),
29510 6.28318530717959)))))
29511 + (IKabs(
29512 py)));
29513 evalcond
29514 [1]
29515 = -0.85;
29516 evalcond
29517 [2]
29518 = 0;
29519 evalcond
29520 [3]
29521 = ((-0.2125)
29522 + (((-1.0)
29523 * (1.0)
29524 * (px
29525 * px))));
29526 if (IKabs(
29527 evalcond
29528 [0])
29529 < 0.0000010000000000
29530 && IKabs(
29531 evalcond
29532 [1])
29533 < 0.0000010000000000
29534 && IKabs(
29535 evalcond
29536 [2])
29537 < 0.0000010000000000
29538 && IKabs(
29539 evalcond
29540 [3])
29541 < 0.0000010000000000)
29542 {
29543 bgotonextstatement
29544 = false;
29545 {
29546 IkReal j6eval
29547 [1];
29548 IkReal
29549 x2037
29550 = ((1.0)
29551 * px);
29552 sj8 = 0;
29553 cj8 = -1.0;
29554 j8 = 3.14159265358979;
29555 pz = 0;
29556 j9 = 0;
29557 sj9 = 0;
29558 cj9 = 1.0;
29559 pp = px
29560 * px;
29561 npx
29562 = (px
29563 * r00);
29564 npy
29565 = (px
29566 * r01);
29567 npz
29568 = (px
29569 * r02);
29570 rxp0_0
29571 = 0;
29572 rxp0_1
29573 = (px
29574 * r20);
29575 rxp1_0
29576 = 0;
29577 rxp1_1
29578 = (px
29579 * r21);
29580 rxp2_0
29581 = 0;
29582 rxp2_1
29583 = (px
29584 * r22);
29585 py = 0;
29586 j4 = 1.5707963267949;
29587 sj4 = 1.0;
29588 cj4 = 0;
29589 rxp0_2
29590 = ((-1.0)
29591 * r10
29592 * x2037);
29593 rxp1_2
29594 = ((-1.0)
29595 * r11
29596 * x2037);
29597 rxp2_2
29598 = ((-1.0)
29599 * r12
29600 * x2037);
29601 j6eval
29602 [0]
29603 = ((1.0)
29604 + (((1.91568587540858)
29605 * (px
29606 * px
29607 * px
29608 * px)))
29609 + (((-1.0)
29610 * (2.64633970947792)
29611 * (px
29612 * px))));
29613 if (IKabs(
29614 j6eval
29615 [0])
29616 < 0.0000010000000000)
29617 {
29618 continue; // no branches [j6]
29619 }
29620 else
29621 {
29622 {
29623 IkReal j6array
29624 [2],
29625 cj6array
29626 [2],
29627 sj6array
29628 [2];
29629 bool j6valid
29630 [2]
29631 = {false};
29632 _nj6
29633 = 2;
29634 IkReal
29635 x2038
29636 = px
29637 * px;
29638 CheckValue<IkReal> x2040 = IKatan2WithCheck(
29639 IkReal((
29640 (-0.425)
29641 + (((-0.588235294117647)
29642 * x2038)))),
29643 ((2.83333333333333)
29644 + (((-3.92156862745098)
29645 * x2038))),
29646 IKFAST_ATAN2_MAGTHRESH);
29647 if (!x2040
29648 .valid)
29649 {
29650 continue;
29651 }
29652 IkReal
29653 x2039
29654 = ((-1.0)
29655 * (x2040
29656 .value));
29657 j6array
29658 [0]
29659 = x2039;
29660 sj6array
29661 [0]
29662 = IKsin(
29663 j6array
29664 [0]);
29665 cj6array
29666 [0]
29667 = IKcos(
29668 j6array
29669 [0]);
29670 j6array
29671 [1]
29672 = ((3.14159265358979)
29673 + x2039);
29674 sj6array
29675 [1]
29676 = IKsin(
29677 j6array
29678 [1]);
29679 cj6array
29680 [1]
29681 = IKcos(
29682 j6array
29683 [1]);
29684 if (j6array
29685 [0]
29686 > IKPI)
29687 {
29688 j6array
29689 [0]
29690 -= IK2PI;
29691 }
29692 else if (
29693 j6array
29694 [0]
29695 < -IKPI)
29696 {
29697 j6array
29698 [0]
29699 += IK2PI;
29700 }
29701 j6valid
29702 [0]
29703 = true;
29704 if (j6array
29705 [1]
29706 > IKPI)
29707 {
29708 j6array
29709 [1]
29710 -= IK2PI;
29711 }
29712 else if (
29713 j6array
29714 [1]
29715 < -IKPI)
29716 {
29717 j6array
29718 [1]
29719 += IK2PI;
29720 }
29721 j6valid
29722 [1]
29723 = true;
29724 for (
29725 int ij6
29726 = 0;
29727 ij6
29728 < 2;
29729 ++ij6)
29730 {
29731 if (!j6valid
29732 [ij6])
29733 {
29734 continue;
29735 }
29736 _ij6[0]
29737 = ij6;
29738 _ij6[1]
29739 = -1;
29740 for (
29741 int iij6
29742 = ij6
29743 + 1;
29744 iij6
29745 < 2;
29746 ++iij6)
29747 {
29748 if (j6valid
29749 [iij6]
29750 && IKabs(
29751 cj6array
29752 [ij6]
29753 - cj6array
29754 [iij6])
29755 < IKFAST_SOLUTION_THRESH
29756 && IKabs(
29757 sj6array
29758 [ij6]
29759 - sj6array
29760 [iij6])
29761 < IKFAST_SOLUTION_THRESH)
29762 {
29763 j6valid
29764 [iij6]
29765 = false;
29766 _ij6[1]
29767 = iij6;
29768 break;
29769 }
29770 }
29771 j6 = j6array
29772 [ij6];
29773 cj6 = cj6array
29774 [ij6];
29775 sj6 = sj6array
29776 [ij6];
29777 {
29778 IkReal evalcond
29779 [1];
29780 evalcond
29781 [0]
29782 = ((0.85)
29783 * (IKsin(
29784 j6)));
29785 if (IKabs(
29786 evalcond
29787 [0])
29788 > IKFAST_EVALCOND_THRESH)
29789 {
29790 continue;
29791 }
29792 }
29793
29794 rotationfunction0(
29795 solutions);
29796 }
29797 }
29798 }
29799 }
29800 }
29801 } while (
29802 0);
29803 if (bgotonextstatement)
29804 {
29805 bool
29806 bgotonextstatement
29807 = true;
29808 do
29809 {
29810 evalcond
29811 [0]
29812 = ((IKabs(
29813 py))
29814 + (IKabs((
29815 (-3.14159265358979)
29816 + (IKfmod(
29817 ((4.71238898038469)
29818 + j4),
29819 6.28318530717959))))));
29820 evalcond
29821 [1]
29822 = -0.85;
29823 evalcond
29824 [2]
29825 = 0;
29826 evalcond
29827 [3]
29828 = ((-0.2125)
29829 + (((-1.0)
29830 * (1.0)
29831 * (px
29832 * px))));
29833 if (IKabs(
29834 evalcond
29835 [0])
29836 < 0.0000010000000000
29837 && IKabs(
29838 evalcond
29839 [1])
29840 < 0.0000010000000000
29841 && IKabs(
29842 evalcond
29843 [2])
29844 < 0.0000010000000000
29845 && IKabs(
29846 evalcond
29847 [3])
29848 < 0.0000010000000000)
29849 {
29850 bgotonextstatement
29851 = false;
29852 {
29853 IkReal j6eval
29854 [1];
29855 IkReal
29856 x2041
29857 = ((1.0)
29858 * px);
29859 sj8 = 0;
29860 cj8 = -1.0;
29861 j8 = 3.14159265358979;
29862 pz = 0;
29863 j9 = 0;
29864 sj9 = 0;
29865 cj9 = 1.0;
29866 pp = px
29867 * px;
29868 npx
29869 = (px
29870 * r00);
29871 npy
29872 = (px
29873 * r01);
29874 npz
29875 = (px
29876 * r02);
29877 rxp0_0
29878 = 0;
29879 rxp0_1
29880 = (px
29881 * r20);
29882 rxp1_0
29883 = 0;
29884 rxp1_1
29885 = (px
29886 * r21);
29887 rxp2_0
29888 = 0;
29889 rxp2_1
29890 = (px
29891 * r22);
29892 py = 0;
29893 j4 = -1.5707963267949;
29894 sj4 = -1.0;
29895 cj4 = 0;
29896 rxp0_2
29897 = ((-1.0)
29898 * r10
29899 * x2041);
29900 rxp1_2
29901 = ((-1.0)
29902 * r11
29903 * x2041);
29904 rxp2_2
29905 = ((-1.0)
29906 * r12
29907 * x2041);
29908 j6eval
29909 [0]
29910 = ((1.0)
29911 + (((1.91568587540858)
29912 * (px
29913 * px
29914 * px
29915 * px)))
29916 + (((-1.0)
29917 * (2.64633970947792)
29918 * (px
29919 * px))));
29920 if (IKabs(
29921 j6eval
29922 [0])
29923 < 0.0000010000000000)
29924 {
29925 continue; // no branches [j6]
29926 }
29927 else
29928 {
29929 {
29930 IkReal j6array
29931 [2],
29932 cj6array
29933 [2],
29934 sj6array
29935 [2];
29936 bool j6valid
29937 [2]
29938 = {false};
29939 _nj6
29940 = 2;
29941 IkReal
29942 x2042
29943 = px
29944 * px;
29945 CheckValue<IkReal> x2044 = IKatan2WithCheck(
29946 IkReal((
29947 (-0.425)
29948 + (((-0.588235294117647)
29949 * x2042)))),
29950 ((2.83333333333333)
29951 + (((-3.92156862745098)
29952 * x2042))),
29953 IKFAST_ATAN2_MAGTHRESH);
29954 if (!x2044
29955 .valid)
29956 {
29957 continue;
29958 }
29959 IkReal
29960 x2043
29961 = ((-1.0)
29962 * (x2044
29963 .value));
29964 j6array
29965 [0]
29966 = x2043;
29967 sj6array
29968 [0]
29969 = IKsin(
29970 j6array
29971 [0]);
29972 cj6array
29973 [0]
29974 = IKcos(
29975 j6array
29976 [0]);
29977 j6array
29978 [1]
29979 = ((3.14159265358979)
29980 + x2043);
29981 sj6array
29982 [1]
29983 = IKsin(
29984 j6array
29985 [1]);
29986 cj6array
29987 [1]
29988 = IKcos(
29989 j6array
29990 [1]);
29991 if (j6array
29992 [0]
29993 > IKPI)
29994 {
29995 j6array
29996 [0]
29997 -= IK2PI;
29998 }
29999 else if (
30000 j6array
30001 [0]
30002 < -IKPI)
30003 {
30004 j6array
30005 [0]
30006 += IK2PI;
30007 }
30008 j6valid
30009 [0]
30010 = true;
30011 if (j6array
30012 [1]
30013 > IKPI)
30014 {
30015 j6array
30016 [1]
30017 -= IK2PI;
30018 }
30019 else if (
30020 j6array
30021 [1]
30022 < -IKPI)
30023 {
30024 j6array
30025 [1]
30026 += IK2PI;
30027 }
30028 j6valid
30029 [1]
30030 = true;
30031 for (
30032 int ij6
30033 = 0;
30034 ij6
30035 < 2;
30036 ++ij6)
30037 {
30038 if (!j6valid
30039 [ij6])
30040 {
30041 continue;
30042 }
30043 _ij6[0]
30044 = ij6;
30045 _ij6[1]
30046 = -1;
30047 for (
30048 int iij6
30049 = ij6
30050 + 1;
30051 iij6
30052 < 2;
30053 ++iij6)
30054 {
30055 if (j6valid
30056 [iij6]
30057 && IKabs(
30058 cj6array
30059 [ij6]
30060 - cj6array
30061 [iij6])
30062 < IKFAST_SOLUTION_THRESH
30063 && IKabs(
30064 sj6array
30065 [ij6]
30066 - sj6array
30067 [iij6])
30068 < IKFAST_SOLUTION_THRESH)
30069 {
30070 j6valid
30071 [iij6]
30072 = false;
30073 _ij6[1]
30074 = iij6;
30075 break;
30076 }
30077 }
30078 j6 = j6array
30079 [ij6];
30080 cj6 = cj6array
30081 [ij6];
30082 sj6 = sj6array
30083 [ij6];
30084 {
30085 IkReal evalcond
30086 [1];
30087 evalcond
30088 [0]
30089 = ((0.85)
30090 * (IKsin(
30091 j6)));
30092 if (IKabs(
30093 evalcond
30094 [0])
30095 > IKFAST_EVALCOND_THRESH)
30096 {
30097 continue;
30098 }
30099 }
30100
30101 rotationfunction0(
30102 solutions);
30103 }
30104 }
30105 }
30106 }
30107 }
30108 } while (
30109 0);
30110 if (bgotonextstatement)
30111 {
30112 bool
30113 bgotonextstatement
30114 = true;
30115 do
30116 {
30117 if (1)
30118 {
30119 bgotonextstatement
30120 = false;
30121 continue; // branch miss [j6]
30122 }
30123 } while (
30124 0);
30125 if (bgotonextstatement)
30126 {
30127 }
30128 }
30129 }
30130 }
30131 }
30132 }
30133 }
30134 }
30135 else
30136 {
30137 {
30138 IkReal j6array
30139 [1],
30140 cj6array
30141 [1],
30142 sj6array
30143 [1];
30144 bool j6valid
30145 [1]
30146 = {false};
30147 _nj6 = 1;
30148 IkReal
30149 x2045
30150 = (cj4
30151 * px);
30152 IkReal
30153 x2046
30154 = (py
30155 * sj4);
30156 IkReal
30157 x2047
30158 = px
30159 * px;
30160 IkReal
30161 x2048
30162 = py
30163 * py;
30164 CheckValue<
30165 IkReal>
30166 x2049
30167 = IKPowWithIntegerCheck(
30168 ((((20.0)
30169 * x2046))
30170 + (((20.0)
30171 * x2045))),
30172 -1);
30173 if (!x2049
30174 .valid)
30175 {
30176 continue;
30177 }
30178 CheckValue<IkReal> x2050 = IKPowWithIntegerCheck(
30179 ((((-8.5)
30180 * x2046))
30181 + (((-1.0)
30182 * (11.7647058823529)
30183 * cj4
30184 * (px
30185 * px
30186 * px)))
30187 + (((-8.5)
30188 * x2045))
30189 + (((-11.7647058823529)
30190 * x2045
30191 * x2048))
30192 + (((-11.7647058823529)
30193 * x2046
30194 * x2047))
30195 + (((-1.0)
30196 * (11.7647058823529)
30197 * sj4
30198 * (py
30199 * py
30200 * py)))),
30201 -1);
30202 if (!x2050
30203 .valid)
30204 {
30205 continue;
30206 }
30207 if (IKabs((
30208 (17.0)
30209 * (x2049
30210 .value)))
30211 < IKFAST_ATAN2_MAGTHRESH
30212 && IKabs((
30213 (x2050
30214 .value)
30215 * (((-48.1666666666667)
30216 + (((66.6666666666667)
30217 * x2047))
30218 + (((66.6666666666667)
30219 * x2048))))))
30220 < IKFAST_ATAN2_MAGTHRESH
30221 && IKabs(
30222 IKsqr((
30223 (17.0)
30224 * (x2049
30225 .value)))
30226 + IKsqr((
30227 (x2050
30228 .value)
30229 * (((-48.1666666666667)
30230 + (((66.6666666666667)
30231 * x2047))
30232 + (((66.6666666666667)
30233 * x2048))))))
30234 - 1)
30235 <= IKFAST_SINCOS_THRESH)
30236 continue;
30237 j6array[0] = IKatan2(
30238 ((17.0)
30239 * (x2049
30240 .value)),
30241 ((x2050
30242 .value)
30243 * (((-48.1666666666667)
30244 + (((66.6666666666667)
30245 * x2047))
30246 + (((66.6666666666667)
30247 * x2048))))));
30248 sj6array[0] = IKsin(
30249 j6array
30250 [0]);
30251 cj6array[0] = IKcos(
30252 j6array
30253 [0]);
30254 if (j6array
30255 [0]
30256 > IKPI)
30257 {
30258 j6array
30259 [0]
30260 -= IK2PI;
30261 }
30262 else if (
30263 j6array
30264 [0]
30265 < -IKPI)
30266 {
30267 j6array
30268 [0]
30269 += IK2PI;
30270 }
30271 j6valid[0]
30272 = true;
30273 for (
30274 int ij6
30275 = 0;
30276 ij6
30277 < 1;
30278 ++ij6)
30279 {
30280 if (!j6valid
30281 [ij6])
30282 {
30283 continue;
30284 }
30285 _ij6[0]
30286 = ij6;
30287 _ij6[1]
30288 = -1;
30289 for (
30290 int iij6
30291 = ij6
30292 + 1;
30293 iij6
30294 < 1;
30295 ++iij6)
30296 {
30297 if (j6valid
30298 [iij6]
30299 && IKabs(
30300 cj6array
30301 [ij6]
30302 - cj6array
30303 [iij6])
30304 < IKFAST_SOLUTION_THRESH
30305 && IKabs(
30306 sj6array
30307 [ij6]
30308 - sj6array
30309 [iij6])
30310 < IKFAST_SOLUTION_THRESH)
30311 {
30312 j6valid
30313 [iij6]
30314 = false;
30315 _ij6[1]
30316 = iij6;
30317 break;
30318 }
30319 }
30320 j6 = j6array
30321 [ij6];
30322 cj6 = cj6array
30323 [ij6];
30324 sj6 = sj6array
30325 [ij6];
30326 {
30327 IkReal evalcond
30328 [5];
30329 IkReal
30330 x2051
30331 = IKcos(
30332 j6);
30333 IkReal
30334 x2052
30335 = (cj4
30336 * px);
30337 IkReal
30338 x2053
30339 = (x2051
30340 * x2052);
30341 IkReal
30342 x2054
30343 = (py
30344 * sj4);
30345 IkReal
30346 x2055
30347 = (x2051
30348 * x2054);
30349 IkReal
30350 x2056
30351 = IKsin(
30352 j6);
30353 IkReal
30354 x2057
30355 = (x2052
30356 * x2056);
30357 IkReal
30358 x2058
30359 = (x2054
30360 * x2056);
30361 IkReal
30362 x2059
30363 = px
30364 * px;
30365 IkReal
30366 x2060
30367 = ((3.92156862745098)
30368 * x2056);
30369 IkReal
30370 x2061
30371 = ((0.588235294117647)
30372 * x2051);
30373 IkReal
30374 x2062
30375 = py
30376 * py;
30377 evalcond
30378 [0]
30379 = (x2055
30380 + x2053);
30381 evalcond
30382 [1]
30383 = ((-0.85)
30384 + x2057
30385 + x2058);
30386 evalcond
30387 [2]
30388 = ((((0.85)
30389 * x2056))
30390 + (((-1.0)
30391 * x2052))
30392 + (((-1.0)
30393 * x2054)));
30394 evalcond
30395 [3]
30396 = ((((-1.0)
30397 * x2061
30398 * x2062))
30399 + (((-1.0)
30400 * x2059
30401 * x2061))
30402 + (((-0.425)
30403 * x2051))
30404 + (((-1.0)
30405 * x2059
30406 * x2060))
30407 + (((2.83333333333333)
30408 * x2056))
30409 + (((-1.0)
30410 * x2060
30411 * x2062)));
30412 evalcond
30413 [4]
30414 = ((-0.2125)
30415 + (((1.1)
30416 * x2057))
30417 + (((-1.0)
30418 * x2059))
30419 + (((-0.09)
30420 * x2055))
30421 + (((1.1)
30422 * x2058))
30423 + (((-1.0)
30424 * x2062))
30425 + (((-0.09)
30426 * x2053)));
30427 if (IKabs(
30428 evalcond
30429 [0])
30430 > IKFAST_EVALCOND_THRESH
30431 || IKabs(
30432 evalcond
30433 [1])
30434 > IKFAST_EVALCOND_THRESH
30435 || IKabs(
30436 evalcond
30437 [2])
30438 > IKFAST_EVALCOND_THRESH
30439 || IKabs(
30440 evalcond
30441 [3])
30442 > IKFAST_EVALCOND_THRESH
30443 || IKabs(
30444 evalcond
30445 [4])
30446 > IKFAST_EVALCOND_THRESH)
30447 {
30448 continue;
30449 }
30450 }
30451
30452 rotationfunction0(
30453 solutions);
30454 }
30455 }
30456 }
30457 }
30458 }
30459 else
30460 {
30461 {
30462 IkReal j6array
30463 [1],
30464 cj6array
30465 [1],
30466 sj6array
30467 [1];
30468 bool
30469 j6valid[1]
30470 = {false};
30471 _nj6 = 1;
30472 IkReal x2063
30473 = (cj4
30474 * px);
30475 IkReal x2064
30476 = (py
30477 * sj4);
30478 IkReal x2065
30479 = px * px;
30480 IkReal x2066
30481 = py * py;
30482 CheckValue<
30483 IkReal>
30484 x2067
30485 = IKPowWithIntegerCheck(
30486 ((-7.225)
30487 + (((-10.0)
30488 * x2066))
30489 + (((-10.0)
30490 * x2065))),
30491 -1);
30492 if (!x2067
30493 .valid)
30494 {
30495 continue;
30496 }
30497 if (IKabs((
30498 (((1.17647058823529)
30499 * x2063))
30500 + (((1.17647058823529)
30501 * x2064))))
30502 < IKFAST_ATAN2_MAGTHRESH
30503 && IKabs((
30504 (x2067
30505 .value)
30506 * (((((-56.6666666666667)
30507 * x2064))
30508 + (((78.4313725490196)
30509 * sj4
30510 * (py
30511 * py
30512 * py)))
30513 + (((78.4313725490196)
30514 * cj4
30515 * (px
30516 * px
30517 * px)))
30518 + (((78.4313725490196)
30519 * x2063
30520 * x2066))
30521 + (((78.4313725490196)
30522 * x2064
30523 * x2065))
30524 + (((-56.6666666666667)
30525 * x2063))))))
30526 < IKFAST_ATAN2_MAGTHRESH
30527 && IKabs(
30528 IKsqr((
30529 (((1.17647058823529)
30530 * x2063))
30531 + (((1.17647058823529)
30532 * x2064))))
30533 + IKsqr((
30534 (x2067
30535 .value)
30536 * (((((-56.6666666666667)
30537 * x2064))
30538 + (((78.4313725490196)
30539 * sj4
30540 * (py
30541 * py
30542 * py)))
30543 + (((78.4313725490196)
30544 * cj4
30545 * (px
30546 * px
30547 * px)))
30548 + (((78.4313725490196)
30549 * x2063
30550 * x2066))
30551 + (((78.4313725490196)
30552 * x2064
30553 * x2065))
30554 + (((-56.6666666666667)
30555 * x2063))))))
30556 - 1)
30557 <= IKFAST_SINCOS_THRESH)
30558 continue;
30559 j6array[0] = IKatan2(
30560 ((((1.17647058823529)
30561 * x2063))
30562 + (((1.17647058823529)
30563 * x2064))),
30564 ((x2067
30565 .value)
30566 * (((((-56.6666666666667)
30567 * x2064))
30568 + (((78.4313725490196)
30569 * sj4
30570 * (py
30571 * py
30572 * py)))
30573 + (((78.4313725490196)
30574 * cj4
30575 * (px
30576 * px
30577 * px)))
30578 + (((78.4313725490196)
30579 * x2063
30580 * x2066))
30581 + (((78.4313725490196)
30582 * x2064
30583 * x2065))
30584 + (((-56.6666666666667)
30585 * x2063))))));
30586 sj6array[0]
30587 = IKsin(
30588 j6array
30589 [0]);
30590 cj6array[0]
30591 = IKcos(
30592 j6array
30593 [0]);
30594 if (j6array[0]
30595 > IKPI)
30596 {
30597 j6array[0]
30598 -= IK2PI;
30599 }
30600 else if (
30601 j6array[0]
30602 < -IKPI)
30603 {
30604 j6array[0]
30605 += IK2PI;
30606 }
30607 j6valid[0]
30608 = true;
30609 for (int ij6
30610 = 0;
30611 ij6 < 1;
30612 ++ij6)
30613 {
30614 if (!j6valid
30615 [ij6])
30616 {
30617 continue;
30618 }
30619 _ij6[0]
30620 = ij6;
30621 _ij6[1]
30622 = -1;
30623 for (
30624 int iij6
30625 = ij6
30626 + 1;
30627 iij6
30628 < 1;
30629 ++iij6)
30630 {
30631 if (j6valid
30632 [iij6]
30633 && IKabs(
30634 cj6array
30635 [ij6]
30636 - cj6array
30637 [iij6])
30638 < IKFAST_SOLUTION_THRESH
30639 && IKabs(
30640 sj6array
30641 [ij6]
30642 - sj6array
30643 [iij6])
30644 < IKFAST_SOLUTION_THRESH)
30645 {
30646 j6valid
30647 [iij6]
30648 = false;
30649 _ij6[1]
30650 = iij6;
30651 break;
30652 }
30653 }
30654 j6 = j6array
30655 [ij6];
30656 cj6 = cj6array
30657 [ij6];
30658 sj6 = sj6array
30659 [ij6];
30660 {
30661 IkReal evalcond
30662 [5];
30663 IkReal
30664 x2068
30665 = IKcos(
30666 j6);
30667 IkReal
30668 x2069
30669 = (cj4
30670 * px);
30671 IkReal
30672 x2070
30673 = (x2068
30674 * x2069);
30675 IkReal
30676 x2071
30677 = (py
30678 * sj4);
30679 IkReal
30680 x2072
30681 = (x2068
30682 * x2071);
30683 IkReal
30684 x2073
30685 = IKsin(
30686 j6);
30687 IkReal
30688 x2074
30689 = (x2069
30690 * x2073);
30691 IkReal
30692 x2075
30693 = (x2071
30694 * x2073);
30695 IkReal
30696 x2076
30697 = px
30698 * px;
30699 IkReal
30700 x2077
30701 = ((3.92156862745098)
30702 * x2073);
30703 IkReal
30704 x2078
30705 = ((0.588235294117647)
30706 * x2068);
30707 IkReal
30708 x2079
30709 = py
30710 * py;
30711 evalcond
30712 [0]
30713 = (x2070
30714 + x2072);
30715 evalcond
30716 [1]
30717 = ((-0.85)
30718 + x2075
30719 + x2074);
30720 evalcond
30721 [2]
30722 = ((((-1.0)
30723 * x2071))
30724 + (((0.85)
30725 * x2073))
30726 + (((-1.0)
30727 * x2069)));
30728 evalcond
30729 [3]
30730 = ((((-1.0)
30731 * x2076
30732 * x2077))
30733 + (((-1.0)
30734 * x2076
30735 * x2078))
30736 + (((-0.425)
30737 * x2068))
30738 + (((-1.0)
30739 * x2078
30740 * x2079))
30741 + (((2.83333333333333)
30742 * x2073))
30743 + (((-1.0)
30744 * x2077
30745 * x2079)));
30746 evalcond
30747 [4]
30748 = ((-0.2125)
30749 + (((-1.0)
30750 * x2079))
30751 + (((-1.0)
30752 * x2076))
30753 + (((1.1)
30754 * x2075))
30755 + (((-0.09)
30756 * x2070))
30757 + (((1.1)
30758 * x2074))
30759 + (((-0.09)
30760 * x2072)));
30761 if (IKabs(
30762 evalcond
30763 [0])
30764 > IKFAST_EVALCOND_THRESH
30765 || IKabs(
30766 evalcond
30767 [1])
30768 > IKFAST_EVALCOND_THRESH
30769 || IKabs(
30770 evalcond
30771 [2])
30772 > IKFAST_EVALCOND_THRESH
30773 || IKabs(
30774 evalcond
30775 [3])
30776 > IKFAST_EVALCOND_THRESH
30777 || IKabs(
30778 evalcond
30779 [4])
30780 > IKFAST_EVALCOND_THRESH)
30781 {
30782 continue;
30783 }
30784 }
30785
30786 rotationfunction0(
30787 solutions);
30788 }
30789 }
30790 }
30791 }
30792 }
30793 else
30794 {
30795 {
30796 IkReal j6array[1],
30797 cj6array[1],
30798 sj6array[1];
30799 bool j6valid[1]
30800 = {false};
30801 _nj6 = 1;
30802 IkReal x2080
30803 = (cj4 * px);
30804 IkReal x2081
30805 = (py * sj4);
30806 IkReal x2082
30807 = px * px;
30808 IkReal x2083
30809 = py * py;
30810 IkReal x2084
30811 = ((1.29411764705882)
30812 * (cj4
30813 * cj4));
30814 CheckValue<IkReal>
30815 x2085
30816 = IKPowWithIntegerCheck(
30817 ((((-0.09)
30818 * x2080))
30819 + (((-0.09)
30820 * x2081))),
30821 -1);
30822 if (!x2085.valid)
30823 {
30824 continue;
30825 }
30826 if (IKabs((
30827 (((1.17647058823529)
30828 * x2080))
30829 + (((1.17647058823529)
30830 * x2081))))
30831 < IKFAST_ATAN2_MAGTHRESH
30832 && IKabs((
30833 (x2085
30834 .value)
30835 * (((0.2125)
30836 + (((-2.58823529411765)
30837 * cj4
30838 * px
30839 * x2081))
30840 + (((-0.294117647058824)
30841 * x2083))
30842 + ((x2083
30843 * x2084))
30844 + (((-1.0)
30845 * x2082
30846 * x2084))
30847 + x2082))))
30848 < IKFAST_ATAN2_MAGTHRESH
30849 && IKabs(
30850 IKsqr((
30851 (((1.17647058823529)
30852 * x2080))
30853 + (((1.17647058823529)
30854 * x2081))))
30855 + IKsqr((
30856 (x2085
30857 .value)
30858 * (((0.2125)
30859 + (((-2.58823529411765)
30860 * cj4
30861 * px
30862 * x2081))
30863 + (((-0.294117647058824)
30864 * x2083))
30865 + ((x2083
30866 * x2084))
30867 + (((-1.0)
30868 * x2082
30869 * x2084))
30870 + x2082))))
30871 - 1)
30872 <= IKFAST_SINCOS_THRESH)
30873 continue;
30874 j6array[0] = IKatan2(
30875 ((((1.17647058823529)
30876 * x2080))
30877 + (((1.17647058823529)
30878 * x2081))),
30879 ((x2085.value)
30880 * (((0.2125)
30881 + (((-2.58823529411765)
30882 * cj4
30883 * px
30884 * x2081))
30885 + (((-0.294117647058824)
30886 * x2083))
30887 + ((x2083
30888 * x2084))
30889 + (((-1.0)
30890 * x2082
30891 * x2084))
30892 + x2082))));
30893 sj6array[0]
30894 = IKsin(
30895 j6array
30896 [0]);
30897 cj6array[0]
30898 = IKcos(
30899 j6array
30900 [0]);
30901 if (j6array[0]
30902 > IKPI)
30903 {
30904 j6array[0]
30905 -= IK2PI;
30906 }
30907 else if (
30908 j6array[0]
30909 < -IKPI)
30910 {
30911 j6array[0]
30912 += IK2PI;
30913 }
30914 j6valid[0] = true;
30915 for (int ij6 = 0;
30916 ij6 < 1;
30917 ++ij6)
30918 {
30919 if (!j6valid
30920 [ij6])
30921 {
30922 continue;
30923 }
30924 _ij6[0] = ij6;
30925 _ij6[1] = -1;
30926 for (int iij6
30927 = ij6 + 1;
30928 iij6 < 1;
30929 ++iij6)
30930 {
30931 if (j6valid
30932 [iij6]
30933 && IKabs(
30934 cj6array
30935 [ij6]
30936 - cj6array
30937 [iij6])
30938 < IKFAST_SOLUTION_THRESH
30939 && IKabs(
30940 sj6array
30941 [ij6]
30942 - sj6array
30943 [iij6])
30944 < IKFAST_SOLUTION_THRESH)
30945 {
30946 j6valid
30947 [iij6]
30948 = false;
30949 _ij6[1]
30950 = iij6;
30951 break;
30952 }
30953 }
30954 j6 = j6array
30955 [ij6];
30956 cj6 = cj6array
30957 [ij6];
30958 sj6 = sj6array
30959 [ij6];
30960 {
30961 IkReal
30962 evalcond
30963 [5];
30964 IkReal x2086
30965 = IKcos(
30966 j6);
30967 IkReal x2087
30968 = (cj4
30969 * px);
30970 IkReal x2088
30971 = (x2086
30972 * x2087);
30973 IkReal x2089
30974 = (py
30975 * sj4);
30976 IkReal x2090
30977 = (x2086
30978 * x2089);
30979 IkReal x2091
30980 = IKsin(
30981 j6);
30982 IkReal x2092
30983 = (x2087
30984 * x2091);
30985 IkReal x2093
30986 = (x2089
30987 * x2091);
30988 IkReal x2094
30989 = px * px;
30990 IkReal x2095
30991 = ((3.92156862745098)
30992 * x2091);
30993 IkReal x2096
30994 = ((0.588235294117647)
30995 * x2086);
30996 IkReal x2097
30997 = py * py;
30998 evalcond[0]
30999 = (x2090
31000 + x2088);
31001 evalcond[1]
31002 = ((-0.85)
31003 + x2093
31004 + x2092);
31005 evalcond[2]
31006 = ((((-1.0)
31007 * x2089))
31008 + (((-1.0)
31009 * x2087))
31010 + (((0.85)
31011 * x2091)));
31012 evalcond[3]
31013 = ((((-1.0)
31014 * x2095
31015 * x2097))
31016 + (((-1.0)
31017 * x2094
31018 * x2096))
31019 + (((-1.0)
31020 * x2096
31021 * x2097))
31022 + (((2.83333333333333)
31023 * x2091))
31024 + (((-1.0)
31025 * x2094
31026 * x2095))
31027 + (((-0.425)
31028 * x2086)));
31029 evalcond[4]
31030 = ((-0.2125)
31031 + (((-1.0)
31032 * x2094))
31033 + (((1.1)
31034 * x2093))
31035 + (((1.1)
31036 * x2092))
31037 + (((-1.0)
31038 * x2097))
31039 + (((-0.09)
31040 * x2090))
31041 + (((-0.09)
31042 * x2088)));
31043 if (IKabs(
31044 evalcond
31045 [0])
31046 > IKFAST_EVALCOND_THRESH
31047 || IKabs(
31048 evalcond
31049 [1])
31050 > IKFAST_EVALCOND_THRESH
31051 || IKabs(
31052 evalcond
31053 [2])
31054 > IKFAST_EVALCOND_THRESH
31055 || IKabs(
31056 evalcond
31057 [3])
31058 > IKFAST_EVALCOND_THRESH
31059 || IKabs(
31060 evalcond
31061 [4])
31062 > IKFAST_EVALCOND_THRESH)
31063 {
31064 continue;
31065 }
31066 }
31067
31068 rotationfunction0(
31069 solutions);
31070 }
31071 }
31072 }
31073 }
31074 }
31075 } while (0);
31076 if (bgotonextstatement)
31077 {
31078 bool bgotonextstatement
31079 = true;
31080 do
31081 {
31082 if (1)
31083 {
31084 bgotonextstatement
31085 = false;
31086 continue; // branch
31087 // miss [j6]
31088 }
31089 } while (0);
31090 if (bgotonextstatement)
31091 {
31092 }
31093 }
31094 }
31095 }
31096 else
31097 {
31098 {
31099 IkReal j6array[1],
31100 cj6array[1],
31101 sj6array[1];
31102 bool j6valid[1] = {false};
31103 _nj6 = 1;
31104 IkReal x2098 = (cj4 * px);
31105 IkReal x2099 = (py * sj4);
31106 IkReal x2100
31107 = ((0.108264705882353)
31108 * cj9);
31109 IkReal x2101
31110 = ((0.588235294117647)
31111 * pp);
31112 IkReal x2102 = (cj9 * pp);
31113 IkReal x2103 = (cj9 * sj9);
31114 IkReal x2104 = (pp * sj9);
31115 IkReal x2105 = cj9 * cj9;
31116 IkReal x2106 = ((1.0) * pz);
31117 CheckValue<IkReal> x2107
31118 = IKPowWithIntegerCheck(
31119 IKsign((
31120 (((-1.0) * x2099
31121 * x2100))
31122 + (((-1.0)
31123 * x2098
31124 * x2101))
31125 + (((-1.0)
31126 * (1.32323529411765)
31127 * cj9 * pz))
31128 + (((3.92156862745098)
31129 * pp * pz))
31130 + (((-1.0)
31131 * x2098
31132 * x2100))
31133 + (((-1.0)
31134 * (1.51009803921569)
31135 * pz))
31136 + (((-0.316735294117647)
31137 * x2098))
31138 + (((-1.0)
31139 * x2099
31140 * x2101))
31141 + (((-0.316735294117647)
31142 * x2099)))),
31143 -1);
31144 if (!x2107.valid)
31145 {
31146 continue;
31147 }
31148 CheckValue<IkReal> x2108 = IKatan2WithCheck(
31149 IkReal((
31150 (-0.174204411764706)
31151 + (((-0.0324794117647059)
31152 * x2105))
31153 + (pz * pz)
31154 + (((-0.0264705882352941)
31155 * x2104))
31156 + (((-1.0)
31157 * (0.154566176470588)
31158 * cj9))
31159 + (((-1.0)
31160 * (0.323529411764706)
31161 * pp))
31162 + (((-0.00487191176470588)
31163 * x2103))
31164 + (((-1.0)
31165 * (0.0142530882352941)
31166 * sj9))
31167 + (((-0.176470588235294)
31168 * x2102)))),
31169 ((-0.830553921568627)
31170 + (((-1.0)
31171 * (0.0679544117647059)
31172 * sj9))
31173 + (((-1.0)
31174 * (1.18080882352941)
31175 * cj9))
31176 + (((-0.396970588235294)
31177 * x2105))
31178 + (((-1.0) * x2099
31179 * x2106))
31180 + (((-0.0595455882352941)
31181 * x2103))
31182 + (((2.15686274509804)
31183 * pp))
31184 + (((-1.0) * x2098
31185 * x2106))
31186 + (((0.176470588235294)
31187 * x2104))
31188 + (((1.17647058823529)
31189 * x2102))),
31190 IKFAST_ATAN2_MAGTHRESH);
31191 if (!x2108.valid)
31192 {
31193 continue;
31194 }
31195 j6array[0]
31196 = ((-1.5707963267949)
31197 + (((1.5707963267949)
31198 * (x2107.value)))
31199 + (x2108.value));
31200 sj6array[0]
31201 = IKsin(j6array[0]);
31202 cj6array[0]
31203 = IKcos(j6array[0]);
31204 if (j6array[0] > IKPI)
31205 {
31206 j6array[0] -= IK2PI;
31207 }
31208 else if (j6array[0] < -IKPI)
31209 {
31210 j6array[0] += IK2PI;
31211 }
31212 j6valid[0] = true;
31213 for (int ij6 = 0; ij6 < 1;
31214 ++ij6)
31215 {
31216 if (!j6valid[ij6])
31217 {
31218 continue;
31219 }
31220 _ij6[0] = ij6;
31221 _ij6[1] = -1;
31222 for (int iij6 = ij6 + 1;
31223 iij6 < 1;
31224 ++iij6)
31225 {
31226 if (j6valid[iij6]
31227 && IKabs(
31228 cj6array[ij6]
31229 - cj6array
31230 [iij6])
31231 < IKFAST_SOLUTION_THRESH
31232 && IKabs(
31233 sj6array[ij6]
31234 - sj6array
31235 [iij6])
31236 < IKFAST_SOLUTION_THRESH)
31237 {
31238 j6valid[iij6] = false;
31239 _ij6[1] = iij6;
31240 break;
31241 }
31242 }
31243 j6 = j6array[ij6];
31244 cj6 = cj6array[ij6];
31245 sj6 = sj6array[ij6];
31246 {
31247 IkReal evalcond[5];
31248 IkReal x2109
31249 = ((0.3) * cj9);
31250 IkReal x2110
31251 = ((0.045) * sj9);
31252 IkReal x2111
31253 = IKcos(j6);
31254 IkReal x2112
31255 = (pz * x2111);
31256 IkReal x2113
31257 = IKsin(j6);
31258 IkReal x2114
31259 = (cj4 * px);
31260 IkReal x2115
31261 = (x2113 * x2114);
31262 IkReal x2116
31263 = (py * sj4);
31264 IkReal x2117
31265 = (x2113 * x2116);
31266 IkReal x2118
31267 = ((0.045) * cj9);
31268 IkReal x2119
31269 = ((0.3) * sj9);
31270 IkReal x2120
31271 = (pz * x2113);
31272 IkReal x2121
31273 = (x2111 * x2114);
31274 IkReal x2122
31275 = (x2111 * x2116);
31276 evalcond[0]
31277 = ((-0.55) + x2115
31278 + x2117 + x2112
31279 + (((-1.0)
31280 * x2110))
31281 + (((-1.0)
31282 * x2109)));
31283 evalcond[1]
31284 = ((0.045) + x2119
31285 + x2121 + x2122
31286 + (((-1.0)
31287 * x2118))
31288 + (((-1.0)
31289 * x2120)));
31290 evalcond[2]
31291 = ((((-3.92156862745098)
31292 * pp * x2113))
31293 + pz
31294 + (((-0.316735294117647)
31295 * x2111))
31296 + (((-0.588235294117647)
31297 * pp
31298 * x2111))
31299 + (((1.32323529411765)
31300 * cj9
31301 * x2113))
31302 + (((-0.108264705882353)
31303 * cj9
31304 * x2111))
31305 + (((1.51009803921569)
31306 * x2113)));
31307 evalcond[3]
31308 = ((((-1.0)
31309 * x2116))
31310 + (((-1.0)
31311 * x2114))
31312 + ((x2111
31313 * x2118))
31314 + (((-1.0)
31315 * x2111
31316 * x2119))
31317 + ((x2110
31318 * x2113))
31319 + (((0.55)
31320 * x2113))
31321 + (((-0.045)
31322 * x2111))
31323 + ((x2109
31324 * x2113)));
31325 evalcond[4]
31326 = ((-0.2125)
31327 + (((1.1)
31328 * x2115))
31329 + (((1.1)
31330 * x2112))
31331 + (((1.1)
31332 * x2117))
31333 + (((-0.09)
31334 * x2122))
31335 + (((-1.0)
31336 * (1.0)
31337 * pp))
31338 + (((0.09)
31339 * x2120))
31340 + (((-0.09)
31341 * x2121)));
31342 if (IKabs(evalcond[0])
31343 > IKFAST_EVALCOND_THRESH
31344 || IKabs(
31345 evalcond[1])
31346 > IKFAST_EVALCOND_THRESH
31347 || IKabs(
31348 evalcond[2])
31349 > IKFAST_EVALCOND_THRESH
31350 || IKabs(
31351 evalcond[3])
31352 > IKFAST_EVALCOND_THRESH
31353 || IKabs(
31354 evalcond[4])
31355 > IKFAST_EVALCOND_THRESH)
31356 {
31357 continue;
31358 }
31359 }
31360
31361 rotationfunction0(
31362 solutions);
31363 }
31364 }
31365 }
31366 }
31367 }
31368 else
31369 {
31370 {
31371 IkReal j6array[1], cj6array[1],
31372 sj6array[1];
31373 bool j6valid[1] = {false};
31374 _nj6 = 1;
31375 IkReal x2123
31376 = ((0.045) * cj4 * px);
31377 IkReal x2124
31378 = ((0.045) * py * sj4);
31379 IkReal x2125 = ((0.3) * sj9);
31380 IkReal x2126 = (cj4 * px);
31381 IkReal x2127 = (py * sj4);
31382 IkReal x2128 = (cj9 * sj9);
31383 IkReal x2129 = cj9 * cj9;
31384 IkReal x2130 = ((1.0) * pz);
31385 IkReal x2131 = py * py;
31386 IkReal x2132 = cj4 * cj4;
31387 CheckValue<IkReal> x2133
31388 = IKPowWithIntegerCheck(
31389 IKsign((
31390 (((-1.0) * (0.55)
31391 * pz))
31392 + (((-1.0) * x2125
31393 * x2127))
31394 + (((-1.0) * x2125
31395 * x2126))
31396 + ((cj9 * x2123))
31397 + (((-1.0) * x2123))
31398 + ((cj9 * x2124))
31399 + (((-1.0) * x2124))
31400 + (((-1.0) * (0.045)
31401 * pz * sj9))
31402 + (((-1.0) * (0.3)
31403 * cj9 * pz)))),
31404 -1);
31405 if (!x2133.valid)
31406 {
31407 continue;
31408 }
31409 CheckValue<IkReal> x2134
31410 = IKatan2WithCheck(
31411 IkReal(
31412 ((-0.03825)
31413 + (((-0.087975)
31414 * x2128))
31415 + (((0.027)
31416 * x2129))
31417 + (((-1.0)
31418 * (0.167025)
31419 * sj9))
31420 + (((-1.0) * x2126
31421 * x2130))
31422 + (((-1.0) * x2127
31423 * x2130))
31424 + (((0.01125)
31425 * cj9)))),
31426 ((-0.304525)
31427 + ((x2132 * (px * px)))
31428 + (((-1.0) * (0.0495)
31429 * sj9))
31430 + (((-0.027) * x2128))
31431 + (((-1.0) * x2131
31432 * x2132))
31433 + (((-1.0) * (0.33)
31434 * cj9))
31435 + (((-0.087975)
31436 * x2129))
31437 + (((2.0) * cj4 * px
31438 * x2127))
31439 + x2131),
31440 IKFAST_ATAN2_MAGTHRESH);
31441 if (!x2134.valid)
31442 {
31443 continue;
31444 }
31445 j6array[0]
31446 = ((-1.5707963267949)
31447 + (((1.5707963267949)
31448 * (x2133.value)))
31449 + (x2134.value));
31450 sj6array[0] = IKsin(j6array[0]);
31451 cj6array[0] = IKcos(j6array[0]);
31452 if (j6array[0] > IKPI)
31453 {
31454 j6array[0] -= IK2PI;
31455 }
31456 else if (j6array[0] < -IKPI)
31457 {
31458 j6array[0] += IK2PI;
31459 }
31460 j6valid[0] = true;
31461 for (int ij6 = 0; ij6 < 1;
31462 ++ij6)
31463 {
31464 if (!j6valid[ij6])
31465 {
31466 continue;
31467 }
31468 _ij6[0] = ij6;
31469 _ij6[1] = -1;
31470 for (int iij6 = ij6 + 1;
31471 iij6 < 1;
31472 ++iij6)
31473 {
31474 if (j6valid[iij6]
31475 && IKabs(
31476 cj6array[ij6]
31477 - cj6array[iij6])
31478 < IKFAST_SOLUTION_THRESH
31479 && IKabs(
31480 sj6array[ij6]
31481 - sj6array[iij6])
31482 < IKFAST_SOLUTION_THRESH)
31483 {
31484 j6valid[iij6] = false;
31485 _ij6[1] = iij6;
31486 break;
31487 }
31488 }
31489 j6 = j6array[ij6];
31490 cj6 = cj6array[ij6];
31491 sj6 = sj6array[ij6];
31492 {
31493 IkReal evalcond[5];
31494 IkReal x2135
31495 = ((0.3) * cj9);
31496 IkReal x2136
31497 = ((0.045) * sj9);
31498 IkReal x2137 = IKcos(j6);
31499 IkReal x2138 = (pz * x2137);
31500 IkReal x2139 = IKsin(j6);
31501 IkReal x2140 = (cj4 * px);
31502 IkReal x2141
31503 = (x2139 * x2140);
31504 IkReal x2142 = (py * sj4);
31505 IkReal x2143
31506 = (x2139 * x2142);
31507 IkReal x2144
31508 = ((0.045) * cj9);
31509 IkReal x2145
31510 = ((0.3) * sj9);
31511 IkReal x2146 = (pz * x2139);
31512 IkReal x2147
31513 = (x2137 * x2140);
31514 IkReal x2148
31515 = (x2137 * x2142);
31516 evalcond[0]
31517 = ((-0.55)
31518 + (((-1.0) * x2136))
31519 + x2141 + x2143
31520 + (((-1.0) * x2135))
31521 + x2138);
31522 evalcond[1]
31523 = ((0.045) + x2148
31524 + x2145 + x2147
31525 + (((-1.0) * x2144))
31526 + (((-1.0)
31527 * x2146)));
31528 evalcond[2]
31529 = ((((-0.588235294117647)
31530 * pp * x2137))
31531 + (((1.32323529411765)
31532 * cj9 * x2139))
31533 + (((1.51009803921569)
31534 * x2139))
31535 + pz
31536 + (((-3.92156862745098)
31537 * pp * x2139))
31538 + (((-0.316735294117647)
31539 * x2137))
31540 + (((-0.108264705882353)
31541 * cj9 * x2137)));
31542 evalcond[3]
31543 = ((((0.55) * x2139))
31544 + (((-0.045)
31545 * x2137))
31546 + (((-1.0) * x2140))
31547 + ((x2135 * x2139))
31548 + (((-1.0) * x2142))
31549 + ((x2136 * x2139))
31550 + ((x2137 * x2144))
31551 + (((-1.0) * x2137
31552 * x2145)));
31553 evalcond[4]
31554 = ((-0.2125)
31555 + (((-0.09) * x2148))
31556 + (((1.1) * x2143))
31557 + (((-0.09) * x2147))
31558 + (((1.1) * x2141))
31559 + (((0.09) * x2146))
31560 + (((1.1) * x2138))
31561 + (((-1.0) * (1.0)
31562 * pp)));
31563 if (IKabs(evalcond[0])
31564 > IKFAST_EVALCOND_THRESH
31565 || IKabs(evalcond[1])
31566 > IKFAST_EVALCOND_THRESH
31567 || IKabs(evalcond[2])
31568 > IKFAST_EVALCOND_THRESH
31569 || IKabs(evalcond[3])
31570 > IKFAST_EVALCOND_THRESH
31571 || IKabs(evalcond[4])
31572 > IKFAST_EVALCOND_THRESH)
31573 {
31574 continue;
31575 }
31576 }
31577
31578 rotationfunction0(solutions);
31579 }
31580 }
31581 }
31582 }
31583 }
31584 else
31585 {
31586 {
31587 IkReal j6array[1], cj6array[1],
31588 sj6array[1];
31589 bool j6valid[1] = {false};
31590 _nj6 = 1;
31591 IkReal x2149 = py * py;
31592 IkReal x2150 = (py * sj4);
31593 IkReal x2151 = cj4 * cj4;
31594 IkReal x2152 = ((0.045) * pz);
31595 IkReal x2153 = (cj4 * px);
31596 IkReal x2154 = ((0.3) * pz);
31597 IkReal x2155 = ((0.3) * cj4 * px);
31598 IkReal x2156 = ((0.045) * x2153);
31599 IkReal x2157 = ((0.3) * py * sj4);
31600 IkReal x2158 = ((0.045) * x2150);
31601 CheckValue<IkReal> x2159
31602 = IKPowWithIntegerCheck(
31603 IKsign(
31604 ((((-1.0) * x2149
31605 * x2151))
31606 + (pz * pz) + x2149
31607 + (((2.0) * cj4 * px
31608 * x2150))
31609 + ((x2151
31610 * (px * px))))),
31611 -1);
31612 if (!x2159.valid)
31613 {
31614 continue;
31615 }
31616 CheckValue<IkReal> x2160
31617 = IKatan2WithCheck(
31618 IkReal(
31619 (((sj9 * x2158)) + x2152
31620 + (((0.55) * x2150))
31621 + ((cj9 * x2155))
31622 + (((0.55) * x2153))
31623 + ((sj9 * x2156))
31624 + ((sj9 * x2154))
31625 + (((-1.0) * cj9
31626 * x2152))
31627 + ((cj9 * x2157)))),
31628 ((((-1.0) * sj9 * x2157))
31629 + (((-1.0) * x2156))
31630 + ((cj9 * x2154))
31631 + ((sj9 * x2152))
31632 + (((-1.0) * sj9 * x2155))
31633 + ((cj9 * x2156))
31634 + (((-1.0) * x2158))
31635 + (((0.55) * pz))
31636 + ((cj9 * x2158))),
31637 IKFAST_ATAN2_MAGTHRESH);
31638 if (!x2160.valid)
31639 {
31640 continue;
31641 }
31642 j6array[0]
31643 = ((-1.5707963267949)
31644 + (((1.5707963267949)
31645 * (x2159.value)))
31646 + (x2160.value));
31647 sj6array[0] = IKsin(j6array[0]);
31648 cj6array[0] = IKcos(j6array[0]);
31649 if (j6array[0] > IKPI)
31650 {
31651 j6array[0] -= IK2PI;
31652 }
31653 else if (j6array[0] < -IKPI)
31654 {
31655 j6array[0] += IK2PI;
31656 }
31657 j6valid[0] = true;
31658 for (int ij6 = 0; ij6 < 1; ++ij6)
31659 {
31660 if (!j6valid[ij6])
31661 {
31662 continue;
31663 }
31664 _ij6[0] = ij6;
31665 _ij6[1] = -1;
31666 for (int iij6 = ij6 + 1; iij6 < 1;
31667 ++iij6)
31668 {
31669 if (j6valid[iij6]
31670 && IKabs(
31671 cj6array[ij6]
31672 - cj6array[iij6])
31673 < IKFAST_SOLUTION_THRESH
31674 && IKabs(
31675 sj6array[ij6]
31676 - sj6array[iij6])
31677 < IKFAST_SOLUTION_THRESH)
31678 {
31679 j6valid[iij6] = false;
31680 _ij6[1] = iij6;
31681 break;
31682 }
31683 }
31684 j6 = j6array[ij6];
31685 cj6 = cj6array[ij6];
31686 sj6 = sj6array[ij6];
31687 {
31688 IkReal evalcond[5];
31689 IkReal x2161 = ((0.3) * cj9);
31690 IkReal x2162 = ((0.045) * sj9);
31691 IkReal x2163 = IKcos(j6);
31692 IkReal x2164 = (pz * x2163);
31693 IkReal x2165 = IKsin(j6);
31694 IkReal x2166 = (cj4 * px);
31695 IkReal x2167 = (x2165 * x2166);
31696 IkReal x2168 = (py * sj4);
31697 IkReal x2169 = (x2165 * x2168);
31698 IkReal x2170 = ((0.045) * cj9);
31699 IkReal x2171 = ((0.3) * sj9);
31700 IkReal x2172 = (pz * x2165);
31701 IkReal x2173 = (x2163 * x2166);
31702 IkReal x2174 = (x2163 * x2168);
31703 evalcond[0]
31704 = ((-0.55)
31705 + (((-1.0) * x2162))
31706 + x2169 + x2164 + x2167
31707 + (((-1.0) * x2161)));
31708 evalcond[1]
31709 = ((0.045) + x2173 + x2174
31710 + x2171
31711 + (((-1.0) * x2172))
31712 + (((-1.0) * x2170)));
31713 evalcond[2]
31714 = ((((1.32323529411765)
31715 * cj9 * x2165))
31716 + (((-0.108264705882353)
31717 * cj9 * x2163))
31718 + (((-3.92156862745098)
31719 * pp * x2165))
31720 + (((-0.588235294117647)
31721 * pp * x2163))
31722 + pz
31723 + (((1.51009803921569)
31724 * x2165))
31725 + (((-0.316735294117647)
31726 * x2163)));
31727 evalcond[3]
31728 = (((x2163 * x2170))
31729 + (((-1.0) * x2168))
31730 + ((x2161 * x2165))
31731 + (((-1.0) * x2163
31732 * x2171))
31733 + (((-0.045) * x2163))
31734 + ((x2162 * x2165))
31735 + (((-1.0) * x2166))
31736 + (((0.55) * x2165)));
31737 evalcond[4]
31738 = ((-0.2125)
31739 + (((-0.09) * x2174))
31740 + (((0.09) * x2172))
31741 + (((1.1) * x2164))
31742 + (((1.1) * x2167))
31743 + (((1.1) * x2169))
31744 + (((-1.0) * (1.0) * pp))
31745 + (((-0.09) * x2173)));
31746 if (IKabs(evalcond[0])
31747 > IKFAST_EVALCOND_THRESH
31748 || IKabs(evalcond[1])
31749 > IKFAST_EVALCOND_THRESH
31750 || IKabs(evalcond[2])
31751 > IKFAST_EVALCOND_THRESH
31752 || IKabs(evalcond[3])
31753 > IKFAST_EVALCOND_THRESH
31754 || IKabs(evalcond[4])
31755 > IKFAST_EVALCOND_THRESH)
31756 {
31757 continue;
31758 }
31759 }
31760
31761 rotationfunction0(solutions);
31762 }
31763 }
31764 }
31765 }
31766 }
31767 } while (0);
31768 if (bgotonextstatement)
31769 {
31770 bool bgotonextstatement = true;
31771 do
31772 {
31773 if (1)
31774 {
31775 bgotonextstatement = false;
31776 continue; // branch miss [j6]
31777 }
31778 } while (0);
31779 if (bgotonextstatement)
31780 {
31781 }
31782 }
31783 }
31784 }
31785 }
31786 else
31787 {
31788 {
31789 IkReal j6array[1], cj6array[1], sj6array[1];
31790 bool j6valid[1] = {false};
31791 _nj6 = 1;
31792 IkReal x2175 = (pz * sj8);
31793 IkReal x2176 = ((0.3) * cj9);
31794 IkReal x2177 = ((0.045) * sj9);
31795 IkReal x2178 = (cj4 * px);
31796 IkReal x2179 = ((0.045) * cj8 * sj8);
31797 IkReal x2180 = (x2178 * x2179);
31798 IkReal x2181 = (py * sj4);
31799 IkReal x2182 = (x2179 * x2181);
31800 IkReal x2183 = ((0.3) * cj8 * sj8 * sj9);
31801 IkReal x2184 = ((0.55) * cj8);
31802 IkReal x2185 = (cj4 * py);
31803 IkReal x2186 = (px * sj4);
31804 IkReal x2187 = (cj4 * cj8 * py);
31805 IkReal x2188 = ((1.0) * pz * sj8);
31806 IkReal x2189 = (cj8 * px * sj4);
31807 IkReal x2190 = cj8 * cj8;
31808 IkReal x2191 = ((0.045) * x2190);
31809 IkReal x2192 = (x2185 * x2191);
31810 IkReal x2193 = (x2186 * x2191);
31811 IkReal x2194 = ((0.3) * sj9 * x2190);
31812 CheckValue<IkReal> x2195
31813 = IKPowWithIntegerCheck(
31814 IKsign(
31815 ((((-1.0) * cj9 * x2182))
31816 + ((x2181 * x2183))
31817 + (((-0.55) * x2175))
31818 + (((-1.0) * x2175 * x2177))
31819 + (((-1.0) * x2175 * x2176))
31820 + (((-1.0) * cj9 * x2180))
31821 + ((x2178 * x2183)) + x2182
31822 + x2180)),
31823 -1);
31824 if (!x2195.valid)
31825 {
31826 continue;
31827 }
31828 CheckValue<IkReal> x2196 = IKatan2WithCheck(
31829 IkReal(
31830 ((((-1.0) * x2176 * x2189))
31831 + ((x2184 * x2185)) + ((x2176 * x2187))
31832 + (((-1.0) * x2178 * x2188))
31833 + ((x2177 * x2187))
31834 + (((-1.0) * x2181 * x2188))
31835 + (((-1.0) * x2184 * x2186))
31836 + (((-1.0) * x2177 * x2189)))),
31837 ((((-1.0) * (1.0) * sj8 * (pz * pz)))
31838 + (((-1.0) * x2193)) + x2192
31839 + (((-1.0) * x2186 * x2194))
31840 + (((-1.0) * cj9 * x2192))
31841 + ((x2185 * x2194)) + ((cj9 * x2193))),
31842 IKFAST_ATAN2_MAGTHRESH);
31843 if (!x2196.valid)
31844 {
31845 continue;
31846 }
31847 j6array[0]
31848 = ((-1.5707963267949)
31849 + (((1.5707963267949) * (x2195.value)))
31850 + (x2196.value));
31851 sj6array[0] = IKsin(j6array[0]);
31852 cj6array[0] = IKcos(j6array[0]);
31853 if (j6array[0] > IKPI)
31854 {
31855 j6array[0] -= IK2PI;
31856 }
31857 else if (j6array[0] < -IKPI)
31858 {
31859 j6array[0] += IK2PI;
31860 }
31861 j6valid[0] = true;
31862 for (int ij6 = 0; ij6 < 1; ++ij6)
31863 {
31864 if (!j6valid[ij6])
31865 {
31866 continue;
31867 }
31868 _ij6[0] = ij6;
31869 _ij6[1] = -1;
31870 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
31871 {
31872 if (j6valid[iij6]
31873 && IKabs(cj6array[ij6] - cj6array[iij6])
31874 < IKFAST_SOLUTION_THRESH
31875 && IKabs(sj6array[ij6] - sj6array[iij6])
31876 < IKFAST_SOLUTION_THRESH)
31877 {
31878 j6valid[iij6] = false;
31879 _ij6[1] = iij6;
31880 break;
31881 }
31882 }
31883 j6 = j6array[ij6];
31884 cj6 = cj6array[ij6];
31885 sj6 = sj6array[ij6];
31886 {
31887 IkReal evalcond[6];
31888 IkReal x2197 = ((0.3) * cj9);
31889 IkReal x2198 = ((0.045) * sj9);
31890 IkReal x2199 = IKcos(j6);
31891 IkReal x2200 = (pz * x2199);
31892 IkReal x2201 = IKsin(j6);
31893 IkReal x2202 = (cj4 * px * x2201);
31894 IkReal x2203 = (py * sj4);
31895 IkReal x2204 = (x2201 * x2203);
31896 IkReal x2205 = (px * sj4);
31897 IkReal x2206 = ((1.0) * cj4 * py);
31898 IkReal x2207 = (cj4 * sj8);
31899 IkReal x2208 = ((0.045) * cj8);
31900 IkReal x2209 = ((0.045) * cj9);
31901 IkReal x2210 = (cj8 * x2201);
31902 IkReal x2211 = ((0.3) * sj9);
31903 IkReal x2212 = (sj8 * x2205);
31904 IkReal x2213 = (pz * x2210);
31905 IkReal x2214 = (px * (((1.0) * cj4)));
31906 IkReal x2215 = (cj8 * x2199);
31907 IkReal x2216 = ((1.0) * x2203);
31908 IkReal x2217 = ((0.09) * cj8 * x2199);
31909 evalcond[0]
31910 = ((-0.55) + x2204 + x2202 + x2200
31911 + (((-1.0) * x2198))
31912 + (((-1.0) * x2197)));
31913 evalcond[1]
31914 = (((cj8 * x2205))
31915 + (((-1.0) * pz * sj8 * x2201))
31916 + (((-1.0) * cj8 * x2206))
31917 + ((sj8 * x2199 * x2203))
31918 + ((px * x2199 * x2207)));
31919 evalcond[2]
31920 = ((((-0.55) * x2199))
31921 + (((-1.0) * x2197 * x2199))
31922 + ((x2201 * x2208)) + pz
31923 + (((-1.0) * x2209 * x2210))
31924 + (((-1.0) * x2198 * x2199))
31925 + ((x2210 * x2211)));
31926 evalcond[3]
31927 = ((0.045) + (((-1.0) * x2215 * x2216))
31928 + (((-1.0) * x2209)) + x2211 + x2213
31929 + x2212 + (((-1.0) * x2214 * x2215))
31930 + (((-1.0) * sj8 * x2206)));
31931 evalcond[4]
31932 = (((x2211 * x2215)) + ((x2198 * x2201))
31933 + (((-1.0) * x2214))
31934 + (((-1.0) * x2209 * x2215))
31935 + (((0.55) * x2201))
31936 + ((x2199 * x2208))
31937 + (((-1.0) * x2216))
31938 + ((x2197 * x2201)));
31939 evalcond[5]
31940 = ((-0.2125) + (((1.1) * x2204))
31941 + (((1.1) * x2202))
31942 + (((0.09) * py * x2207))
31943 + (((-0.09) * x2213))
31944 + (((-0.09) * x2212))
31945 + ((x2203 * x2217))
31946 + (((-1.0) * (1.0) * pp))
31947 + ((cj4 * px * x2217))
31948 + (((1.1) * x2200)));
31949 if (IKabs(evalcond[0])
31950 > IKFAST_EVALCOND_THRESH
31951 || IKabs(evalcond[1])
31952 > IKFAST_EVALCOND_THRESH
31953 || IKabs(evalcond[2])
31954 > IKFAST_EVALCOND_THRESH
31955 || IKabs(evalcond[3])
31956 > IKFAST_EVALCOND_THRESH
31957 || IKabs(evalcond[4])
31958 > IKFAST_EVALCOND_THRESH
31959 || IKabs(evalcond[5])
31960 > IKFAST_EVALCOND_THRESH)
31961 {
31962 continue;
31963 }
31964 }
31965
31966 rotationfunction0(solutions);
31967 }
31968 }
31969 }
31970 }
31971 }
31972 else
31973 {
31974 {
31975 IkReal j6array[1], cj6array[1], sj6array[1];
31976 bool j6valid[1] = {false};
31977 _nj6 = 1;
31978 IkReal x2218 = py * py;
31979 IkReal x2219 = (sj8 * x2218);
31980 IkReal x2220 = (cj4 * px * sj8);
31981 IkReal x2221 = cj4 * cj4;
31982 IkReal x2222 = px * px;
31983 IkReal x2223 = ((0.55) * sj8);
31984 IkReal x2224 = (cj8 * px);
31985 IkReal x2225 = ((0.3) * cj9);
31986 IkReal x2226 = ((0.045) * sj9);
31987 IkReal x2227 = (py * sj4 * sj8);
31988 IkReal x2228 = (pz * sj8);
31989 IkReal x2229 = (cj4 * cj8 * sj4);
31990 CheckValue<IkReal> x2230 = IKatan2WithCheck(
31991 IkReal(
31992 (((py * sj4 * x2223)) + ((x2220 * x2225))
31993 + ((pz * sj4 * x2224))
31994 + (((-1.0) * cj4 * cj8 * py * pz))
31995 + ((cj4 * px * x2223)) + ((x2226 * x2227))
31996 + ((x2225 * x2227)) + ((x2220 * x2226)))),
31997 ((((2.0) * py * x2221 * x2224))
31998 + ((x2225 * x2228)) + ((x2218 * x2229))
31999 + ((x2226 * x2228)) + ((pz * x2223))
32000 + (((-1.0) * x2222 * x2229))
32001 + (((-1.0) * py * x2224))),
32002 IKFAST_ATAN2_MAGTHRESH);
32003 if (!x2230.valid)
32004 {
32005 continue;
32006 }
32007 CheckValue<IkReal> x2231 = IKPowWithIntegerCheck(
32008 IKsign(
32009 ((((-1.0) * x2219 * x2221))
32010 + (((2.0) * py * sj4 * x2220)) + x2219
32011 + ((sj8 * (pz * pz)))
32012 + ((sj8 * x2221 * x2222)))),
32013 -1);
32014 if (!x2231.valid)
32015 {
32016 continue;
32017 }
32018 j6array[0]
32019 = ((-1.5707963267949) + (x2230.value)
32020 + (((1.5707963267949) * (x2231.value))));
32021 sj6array[0] = IKsin(j6array[0]);
32022 cj6array[0] = IKcos(j6array[0]);
32023 if (j6array[0] > IKPI)
32024 {
32025 j6array[0] -= IK2PI;
32026 }
32027 else if (j6array[0] < -IKPI)
32028 {
32029 j6array[0] += IK2PI;
32030 }
32031 j6valid[0] = true;
32032 for (int ij6 = 0; ij6 < 1; ++ij6)
32033 {
32034 if (!j6valid[ij6])
32035 {
32036 continue;
32037 }
32038 _ij6[0] = ij6;
32039 _ij6[1] = -1;
32040 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
32041 {
32042 if (j6valid[iij6]
32043 && IKabs(cj6array[ij6] - cj6array[iij6])
32044 < IKFAST_SOLUTION_THRESH
32045 && IKabs(sj6array[ij6] - sj6array[iij6])
32046 < IKFAST_SOLUTION_THRESH)
32047 {
32048 j6valid[iij6] = false;
32049 _ij6[1] = iij6;
32050 break;
32051 }
32052 }
32053 j6 = j6array[ij6];
32054 cj6 = cj6array[ij6];
32055 sj6 = sj6array[ij6];
32056 {
32057 IkReal evalcond[6];
32058 IkReal x2232 = ((0.3) * cj9);
32059 IkReal x2233 = ((0.045) * sj9);
32060 IkReal x2234 = IKcos(j6);
32061 IkReal x2235 = (pz * x2234);
32062 IkReal x2236 = IKsin(j6);
32063 IkReal x2237 = (cj4 * px * x2236);
32064 IkReal x2238 = (py * sj4);
32065 IkReal x2239 = (x2236 * x2238);
32066 IkReal x2240 = (px * sj4);
32067 IkReal x2241 = ((1.0) * cj4 * py);
32068 IkReal x2242 = (cj4 * sj8);
32069 IkReal x2243 = ((0.045) * cj8);
32070 IkReal x2244 = ((0.045) * cj9);
32071 IkReal x2245 = (cj8 * x2236);
32072 IkReal x2246 = ((0.3) * sj9);
32073 IkReal x2247 = (sj8 * x2240);
32074 IkReal x2248 = (pz * x2245);
32075 IkReal x2249 = (px * (((1.0) * cj4)));
32076 IkReal x2250 = (cj8 * x2234);
32077 IkReal x2251 = ((1.0) * x2238);
32078 IkReal x2252 = ((0.09) * cj8 * x2234);
32079 evalcond[0]
32080 = ((-0.55) + x2237 + x2235 + x2239
32081 + (((-1.0) * x2232))
32082 + (((-1.0) * x2233)));
32083 evalcond[1]
32084 = (((px * x2234 * x2242))
32085 + ((sj8 * x2234 * x2238))
32086 + (((-1.0) * cj8 * x2241))
32087 + ((cj8 * x2240))
32088 + (((-1.0) * pz * sj8 * x2236)));
32089 evalcond[2]
32090 = ((((-1.0) * x2244 * x2245))
32091 + ((x2236 * x2243)) + pz
32092 + (((-1.0) * x2233 * x2234))
32093 + (((-1.0) * x2232 * x2234))
32094 + (((-0.55) * x2234))
32095 + ((x2245 * x2246)));
32096 evalcond[3]
32097 = ((0.045) + (((-1.0) * x2244))
32098 + (((-1.0) * x2249 * x2250))
32099 + (((-1.0) * sj8 * x2241)) + x2247
32100 + x2246 + x2248
32101 + (((-1.0) * x2250 * x2251)));
32102 evalcond[4]
32103 = ((((0.55) * x2236)) + (((-1.0) * x2249))
32104 + ((x2233 * x2236))
32105 + (((-1.0) * x2244 * x2250))
32106 + ((x2246 * x2250)) + (((-1.0) * x2251))
32107 + ((x2232 * x2236)) + ((x2234 * x2243)));
32108 evalcond[5]
32109 = ((-0.2125) + (((-0.09) * x2247))
32110 + (((-0.09) * x2248)) + ((x2238 * x2252))
32111 + (((1.1) * x2239)) + (((1.1) * x2237))
32112 + (((1.1) * x2235))
32113 + ((cj4 * px * x2252))
32114 + (((-1.0) * (1.0) * pp))
32115 + (((0.09) * py * x2242)));
32116 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
32117 || IKabs(evalcond[1])
32118 > IKFAST_EVALCOND_THRESH
32119 || IKabs(evalcond[2])
32120 > IKFAST_EVALCOND_THRESH
32121 || IKabs(evalcond[3])
32122 > IKFAST_EVALCOND_THRESH
32123 || IKabs(evalcond[4])
32124 > IKFAST_EVALCOND_THRESH
32125 || IKabs(evalcond[5])
32126 > IKFAST_EVALCOND_THRESH)
32127 {
32128 continue;
32129 }
32130 }
32131
32132 rotationfunction0(solutions);
32133 }
32134 }
32135 }
32136 }
32137 }
32138 else
32139 {
32140 {
32141 IkReal j6array[1], cj6array[1], sj6array[1];
32142 bool j6valid[1] = {false};
32143 _nj6 = 1;
32144 IkReal x2253 = (cj4 * px);
32145 IkReal x2254 = ((0.045) * pz);
32146 IkReal x2255 = (py * sj4);
32147 IkReal x2256 = ((0.3) * cj9);
32148 IkReal x2257 = ((0.045) * sj9);
32149 IkReal x2258 = (cj8 * cj9);
32150 IkReal x2259 = (cj8 * sj9);
32151 IkReal x2260 = cj9 * cj9;
32152 IkReal x2261 = ((1.0) * pz);
32153 CheckValue<IkReal> x2262 = IKatan2WithCheck(
32154 IkReal(
32155 ((-0.304525) + (((-1.0) * (0.027) * cj9 * sj9))
32156 + (((-1.0) * (0.0495) * sj9)) + (pz * pz)
32157 + (((-0.087975) * x2260))
32158 + (((-1.0) * (0.33) * cj9)))),
32159 ((((-1.0) * x2255 * x2261))
32160 + (((0.027) * cj8 * x2260)) + (((0.01125) * x2258))
32161 + (((-1.0) * x2253 * x2261))
32162 + (((-1.0) * (0.03825) * cj8))
32163 + (((-0.087975) * sj9 * x2258))
32164 + (((-0.167025) * x2259))),
32165 IKFAST_ATAN2_MAGTHRESH);
32166 if (!x2262.valid)
32167 {
32168 continue;
32169 }
32170 CheckValue<IkReal> x2263 = IKPowWithIntegerCheck(
32171 IKsign((
32172 (((-1.0) * x2255 * x2257))
32173 + (((-1.0) * x2255 * x2256)) + ((x2254 * x2258))
32174 + (((-1.0) * x2253 * x2257))
32175 + (((-0.55) * x2255)) + (((-0.3) * pz * x2259))
32176 + (((-1.0) * x2253 * x2256))
32177 + (((-0.55) * x2253))
32178 + (((-1.0) * cj8 * x2254)))),
32179 -1);
32180 if (!x2263.valid)
32181 {
32182 continue;
32183 }
32184 j6array[0]
32185 = ((-1.5707963267949) + (x2262.value)
32186 + (((1.5707963267949) * (x2263.value))));
32187 sj6array[0] = IKsin(j6array[0]);
32188 cj6array[0] = IKcos(j6array[0]);
32189 if (j6array[0] > IKPI)
32190 {
32191 j6array[0] -= IK2PI;
32192 }
32193 else if (j6array[0] < -IKPI)
32194 {
32195 j6array[0] += IK2PI;
32196 }
32197 j6valid[0] = true;
32198 for (int ij6 = 0; ij6 < 1; ++ij6)
32199 {
32200 if (!j6valid[ij6])
32201 {
32202 continue;
32203 }
32204 _ij6[0] = ij6;
32205 _ij6[1] = -1;
32206 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
32207 {
32208 if (j6valid[iij6]
32209 && IKabs(cj6array[ij6] - cj6array[iij6])
32210 < IKFAST_SOLUTION_THRESH
32211 && IKabs(sj6array[ij6] - sj6array[iij6])
32212 < IKFAST_SOLUTION_THRESH)
32213 {
32214 j6valid[iij6] = false;
32215 _ij6[1] = iij6;
32216 break;
32217 }
32218 }
32219 j6 = j6array[ij6];
32220 cj6 = cj6array[ij6];
32221 sj6 = sj6array[ij6];
32222 {
32223 IkReal evalcond[6];
32224 IkReal x2264 = ((0.3) * cj9);
32225 IkReal x2265 = ((0.045) * sj9);
32226 IkReal x2266 = IKcos(j6);
32227 IkReal x2267 = (pz * x2266);
32228 IkReal x2268 = IKsin(j6);
32229 IkReal x2269 = (cj4 * px * x2268);
32230 IkReal x2270 = (py * sj4);
32231 IkReal x2271 = (x2268 * x2270);
32232 IkReal x2272 = (px * sj4);
32233 IkReal x2273 = ((1.0) * cj4 * py);
32234 IkReal x2274 = (cj4 * sj8);
32235 IkReal x2275 = ((0.045) * cj8);
32236 IkReal x2276 = ((0.045) * cj9);
32237 IkReal x2277 = (cj8 * x2268);
32238 IkReal x2278 = ((0.3) * sj9);
32239 IkReal x2279 = (sj8 * x2272);
32240 IkReal x2280 = (pz * x2277);
32241 IkReal x2281 = (px * (((1.0) * cj4)));
32242 IkReal x2282 = (cj8 * x2266);
32243 IkReal x2283 = ((1.0) * x2270);
32244 IkReal x2284 = ((0.09) * cj8 * x2266);
32245 evalcond[0]
32246 = ((-0.55) + x2267 + x2269 + (((-1.0) * x2265))
32247 + x2271 + (((-1.0) * x2264)));
32248 evalcond[1]
32249 = (((cj8 * x2272)) + (((-1.0) * cj8 * x2273))
32250 + (((-1.0) * pz * sj8 * x2268))
32251 + ((sj8 * x2266 * x2270))
32252 + ((px * x2266 * x2274)));
32253 evalcond[2]
32254 = ((((-1.0) * x2264 * x2266))
32255 + ((x2268 * x2275)) + pz
32256 + (((-1.0) * x2265 * x2266))
32257 + ((x2277 * x2278)) + (((-0.55) * x2266))
32258 + (((-1.0) * x2276 * x2277)));
32259 evalcond[3]
32260 = ((0.045) + (((-1.0) * x2282 * x2283)) + x2280
32261 + (((-1.0) * sj8 * x2273))
32262 + (((-1.0) * x2281 * x2282))
32263 + (((-1.0) * x2276)) + x2278 + x2279);
32264 evalcond[4]
32265 = ((((-1.0) * x2281)) + ((x2266 * x2275))
32266 + (((-1.0) * x2276 * x2282))
32267 + ((x2264 * x2268)) + ((x2278 * x2282))
32268 + ((x2265 * x2268)) + (((-1.0) * x2283))
32269 + (((0.55) * x2268)));
32270 evalcond[5]
32271 = ((-0.2125) + (((-0.09) * x2279))
32272 + (((-0.09) * x2280)) + ((cj4 * px * x2284))
32273 + (((0.09) * py * x2274)) + (((1.1) * x2267))
32274 + ((x2270 * x2284)) + (((-1.0) * (1.0) * pp))
32275 + (((1.1) * x2269)) + (((1.1) * x2271)));
32276 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
32277 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
32278 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
32279 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
32280 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
32281 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH)
32282 {
32283 continue;
32284 }
32285 }
32286
32287 rotationfunction0(solutions);
32288 }
32289 }
32290 }
32291 }
32292 }
32293 }
32294 }
32295 }
32296 }
32297 }
32298 }
32299 return solutions.GetNumSolutions() > 0;
32300 }
32301 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions)
32302 {
32303 for (int rotationiter = 0; rotationiter < 1; ++rotationiter)
32304 {
32305 IkReal x158 = ((1.0) * sj9);
32306 IkReal x159 = (cj9 * sj6);
32307 IkReal x160 = ((((-1.0) * cj6 * x158)) + (((-1.0) * cj8 * x159)));
32308 IkReal x161 = (cj4 * sj8);
32309 IkReal x162 = (sj6 * x158);
32310 IkReal x163 = (cj6 * cj9);
32311 IkReal x164 = (((cj8 * x163)) + (((-1.0) * x162)));
32312 IkReal x165 = (((cj9 * x161)) + ((sj4 * x164)));
32313 IkReal x166 = ((1.0) * sj4 * sj8);
32314 IkReal x167 = ((((-1.0) * cj9 * x166)) + ((cj4 * x164)));
32315 IkReal x168 = (sj6 * sj8);
32316 IkReal x169 = (((cj4 * cj8)) + (((-1.0) * cj6 * x166)));
32317 IkReal x170 = ((((-1.0) * cj6 * x161)) + (((-1.0) * cj8 * sj4)));
32318 IkReal x171 = ((((-1.0) * cj8 * x162)) + x163);
32319 IkReal x172 = (x159 + ((cj6 * cj8 * sj9)));
32320 IkReal x173 = (((sj4 * x172)) + ((sj9 * x161)));
32321 IkReal x174 = (((cj4 * x172)) + (((-1.0) * sj4 * sj8 * x158)));
32322 new_r00 = (((r00 * x167)) + ((r20 * x160)) + ((r10 * x165)));
32323 new_r01 = (((r11 * x165)) + ((r21 * x160)) + ((r01 * x167)));
32324 new_r02 = (((r22 * x160)) + ((r12 * x165)) + ((r02 * x167)));
32325 new_r10 = (((r20 * x168)) + ((r10 * x169)) + ((r00 * x170)));
32326 new_r11 = (((r01 * x170)) + ((r21 * x168)) + ((r11 * x169)));
32327 new_r12 = (((r22 * x168)) + ((r02 * x170)) + ((r12 * x169)));
32328 new_r20 = (((r20 * x171)) + ((r10 * x173)) + ((r00 * x174)));
32329 new_r21 = (((r01 * x174)) + ((r11 * x173)) + ((r21 * x171)));
32330 new_r22 = (((r12 * x173)) + ((r22 * x171)) + ((r02 * x174)));
32331 {
32332 IkReal j11array[2], cj11array[2], sj11array[2];
32333 bool j11valid[2] = {false};
32334 _nj11 = 2;
32335 cj11array[0] = new_r22;
32336 if (cj11array[0] >= -1 - IKFAST_SINCOS_THRESH
32337 && cj11array[0] <= 1 + IKFAST_SINCOS_THRESH)
32338 {
32339 j11valid[0] = j11valid[1] = true;
32340 j11array[0] = IKacos(cj11array[0]);
32341 sj11array[0] = IKsin(j11array[0]);
32342 cj11array[1] = cj11array[0];
32343 j11array[1] = -j11array[0];
32344 sj11array[1] = -sj11array[0];
32345 }
32346 else if (std::isnan(cj11array[0]))
32347 {
32348 // probably any value will work
32349 j11valid[0] = true;
32350 cj11array[0] = 1;
32351 sj11array[0] = 0;
32352 j11array[0] = 0;
32353 }
32354 for (int ij11 = 0; ij11 < 2; ++ij11)
32355 {
32356 if (!j11valid[ij11])
32357 {
32358 continue;
32359 }
32360 _ij11[0] = ij11;
32361 _ij11[1] = -1;
32362 for (int iij11 = ij11 + 1; iij11 < 2; ++iij11)
32363 {
32364 if (j11valid[iij11]
32365 && IKabs(cj11array[ij11] - cj11array[iij11])
32366 < IKFAST_SOLUTION_THRESH
32367 && IKabs(sj11array[ij11] - sj11array[iij11])
32368 < IKFAST_SOLUTION_THRESH)
32369 {
32370 j11valid[iij11] = false;
32371 _ij11[1] = iij11;
32372 break;
32373 }
32374 }
32375 j11 = j11array[ij11];
32376 cj11 = cj11array[ij11];
32377 sj11 = sj11array[ij11];
32378
32379 {
32380 IkReal j10eval[2];
32381 IkReal x175 = ((1.0) * sj9);
32382 IkReal x176 = (cj9 * sj6);
32383 IkReal x177 = x160;
32384 IkReal x178 = (cj4 * sj8);
32385 IkReal x179 = (sj6 * x175);
32386 IkReal x180 = (cj6 * cj9);
32387 IkReal x181 = (((cj8 * x180)) + (((-1.0) * x179)));
32388 IkReal x182 = (((sj4 * x181)) + ((cj9 * x178)));
32389 IkReal x183 = ((1.0) * sj4 * sj8);
32390 IkReal x184 = (((cj4 * x181)) + (((-1.0) * cj9 * x183)));
32391 IkReal x185 = (sj6 * sj8);
32392 IkReal x186 = x169;
32393 IkReal x187 = x170;
32394 IkReal x188 = ((((-1.0) * cj8 * x179)) + x180);
32395 IkReal x189 = x172;
32396 IkReal x190 = (((sj9 * x178)) + ((sj4 * x189)));
32397 IkReal x191 = ((((-1.0) * sj4 * sj8 * x175)) + ((cj4 * x189)));
32398 new_r00 = (((r20 * x177)) + ((r00 * x184)) + ((r10 * x182)));
32399 new_r01 = (((r21 * x177)) + ((r11 * x182)) + ((r01 * x184)));
32400 new_r02 = (((r22 * x177)) + ((r12 * x182)) + ((r02 * x184)));
32401 new_r10 = (((r10 * x186)) + ((r20 * x185)) + ((r00 * x187)));
32402 new_r11 = (((r21 * x185)) + ((r11 * x186)) + ((r01 * x187)));
32403 new_r12 = (((r12 * x186)) + ((r22 * x185)) + ((r02 * x187)));
32404 new_r20 = (((r00 * x191)) + ((r20 * x188)) + ((r10 * x190)));
32405 new_r21 = (((r21 * x188)) + ((r11 * x190)) + ((r01 * x191)));
32406 new_r22 = (((r12 * x190)) + ((r22 * x188)) + ((r02 * x191)));
32407 j10eval[0] = sj11;
32408 j10eval[1] = IKsign(sj11);
32409 if (IKabs(j10eval[0]) < 0.0000010000000000
32410 || IKabs(j10eval[1]) < 0.0000010000000000)
32411 {
32412 {
32413 IkReal j10eval[1];
32414 IkReal x192 = ((1.0) * sj9);
32415 IkReal x193 = (cj9 * sj6);
32416 IkReal x194 = x160;
32417 IkReal x195 = (cj4 * sj8);
32418 IkReal x196 = (sj6 * x192);
32419 IkReal x197 = (cj6 * cj9);
32420 IkReal x198 = (((cj8 * x197)) + (((-1.0) * x196)));
32421 IkReal x199 = (((cj9 * x195)) + ((sj4 * x198)));
32422 IkReal x200 = ((1.0) * sj4 * sj8);
32423 IkReal x201 = ((((-1.0) * cj9 * x200)) + ((cj4 * x198)));
32424 IkReal x202 = (sj6 * sj8);
32425 IkReal x203 = x169;
32426 IkReal x204 = x170;
32427 IkReal x205 = ((((-1.0) * cj8 * x196)) + x197);
32428 IkReal x206 = x172;
32429 IkReal x207 = (((sj9 * x195)) + ((sj4 * x206)));
32430 IkReal x208 = (((cj4 * x206)) + (((-1.0) * sj4 * sj8 * x192)));
32431 new_r00 = (((r20 * x194)) + ((r00 * x201)) + ((r10 * x199)));
32432 new_r01 = (((r11 * x199)) + ((r01 * x201)) + ((r21 * x194)));
32433 new_r02 = (((r02 * x201)) + ((r22 * x194)) + ((r12 * x199)));
32434 new_r10 = (((r00 * x204)) + ((r10 * x203)) + ((r20 * x202)));
32435 new_r11 = (((r21 * x202)) + ((r11 * x203)) + ((r01 * x204)));
32436 new_r12 = (((r12 * x203)) + ((r22 * x202)) + ((r02 * x204)));
32437 new_r20 = (((r20 * x205)) + ((r10 * x207)) + ((r00 * x208)));
32438 new_r21 = (((r11 * x207)) + ((r01 * x208)) + ((r21 * x205)));
32439 new_r22 = (((r12 * x207)) + ((r02 * x208)) + ((r22 * x205)));
32440 j10eval[0] = sj11;
32441 if (IKabs(j10eval[0]) < 0.0000010000000000)
32442 {
32443 {
32444 IkReal evalcond[6];
32445 bool bgotonextstatement = true;
32446 do
32447 {
32448 evalcond[0]
32449 = ((-3.14159265358979)
32450 + (IKfmod(
32451 ((3.14159265358979) + (IKabs(j11))),
32452 6.28318530717959)));
32453 evalcond[1] = ((-1.0) + new_r22);
32454 evalcond[2] = new_r20;
32455 evalcond[3] = new_r02;
32456 evalcond[4] = new_r12;
32457 evalcond[5] = new_r21;
32458 if (IKabs(evalcond[0]) < 0.0000010000000000
32459 && IKabs(evalcond[1]) < 0.0000010000000000
32460 && IKabs(evalcond[2]) < 0.0000010000000000
32461 && IKabs(evalcond[3]) < 0.0000010000000000
32462 && IKabs(evalcond[4]) < 0.0000010000000000
32463 && IKabs(evalcond[5]) < 0.0000010000000000)
32464 {
32465 bgotonextstatement = false;
32466 {
32467 IkReal j10array[2], cj10array[2], sj10array[2];
32468 bool j10valid[2] = {false};
32469 _nj10 = 2;
32470 CheckValue<IkReal> x210 = IKatan2WithCheck(
32471 IkReal(new_r02), new_r12, IKFAST_ATAN2_MAGTHRESH);
32472 if (!x210.valid)
32473 {
32474 continue;
32475 }
32476 IkReal x209 = ((-1.0) * (((1.0) * (x210.value))));
32477 j10array[0] = x209;
32478 sj10array[0] = IKsin(j10array[0]);
32479 cj10array[0] = IKcos(j10array[0]);
32480 j10array[1] = ((3.14159265358979) + x209);
32481 sj10array[1] = IKsin(j10array[1]);
32482 cj10array[1] = IKcos(j10array[1]);
32483 if (j10array[0] > IKPI)
32484 {
32485 j10array[0] -= IK2PI;
32486 }
32487 else if (j10array[0] < -IKPI)
32488 {
32489 j10array[0] += IK2PI;
32490 }
32491 j10valid[0] = true;
32492 if (j10array[1] > IKPI)
32493 {
32494 j10array[1] -= IK2PI;
32495 }
32496 else if (j10array[1] < -IKPI)
32497 {
32498 j10array[1] += IK2PI;
32499 }
32500 j10valid[1] = true;
32501 for (int ij10 = 0; ij10 < 2; ++ij10)
32502 {
32503 if (!j10valid[ij10])
32504 {
32505 continue;
32506 }
32507 _ij10[0] = ij10;
32508 _ij10[1] = -1;
32509 for (int iij10 = ij10 + 1; iij10 < 2; ++iij10)
32510 {
32511 if (j10valid[iij10]
32512 && IKabs(cj10array[ij10] - cj10array[iij10])
32513 < IKFAST_SOLUTION_THRESH
32514 && IKabs(sj10array[ij10] - sj10array[iij10])
32515 < IKFAST_SOLUTION_THRESH)
32516 {
32517 j10valid[iij10] = false;
32518 _ij10[1] = iij10;
32519 break;
32520 }
32521 }
32522 j10 = j10array[ij10];
32523 cj10 = cj10array[ij10];
32524 sj10 = sj10array[ij10];
32525 {
32526 IkReal evalcond[1];
32527 evalcond[0]
32528 = (((new_r12 * (IKcos(j10))))
32529 + (((-1.0) * (1.0) * new_r02
32530 * (IKsin(j10)))));
32531 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
32532 {
32533 continue;
32534 }
32535 }
32536
32537 {
32538 IkReal j12array[1], cj12array[1], sj12array[1];
32539 bool j12valid[1] = {false};
32540 _nj12 = 1;
32541 IkReal x211 = ((1.0) * new_r01);
32542 if (IKabs(
32543 ((((-1.0) * cj10 * x211))
32544 + (((-1.0) * (1.0) * new_r00 * sj10))))
32545 < IKFAST_ATAN2_MAGTHRESH
32546 && IKabs(
32547 ((((-1.0) * sj10 * x211))
32548 + ((cj10 * new_r00))))
32549 < IKFAST_ATAN2_MAGTHRESH
32550 && IKabs(
32551 IKsqr(
32552 ((((-1.0) * cj10 * x211))
32553 + (((-1.0) * (1.0) * new_r00
32554 * sj10))))
32555 + IKsqr(
32556 ((((-1.0) * sj10 * x211))
32557 + ((cj10 * new_r00))))
32558 - 1)
32559 <= IKFAST_SINCOS_THRESH)
32560 continue;
32561 j12array[0] = IKatan2(
32562 ((((-1.0) * cj10 * x211))
32563 + (((-1.0) * (1.0) * new_r00 * sj10))),
32564 ((((-1.0) * sj10 * x211))
32565 + ((cj10 * new_r00))));
32566 sj12array[0] = IKsin(j12array[0]);
32567 cj12array[0] = IKcos(j12array[0]);
32568 if (j12array[0] > IKPI)
32569 {
32570 j12array[0] -= IK2PI;
32571 }
32572 else if (j12array[0] < -IKPI)
32573 {
32574 j12array[0] += IK2PI;
32575 }
32576 j12valid[0] = true;
32577 for (int ij12 = 0; ij12 < 1; ++ij12)
32578 {
32579 if (!j12valid[ij12])
32580 {
32581 continue;
32582 }
32583 _ij12[0] = ij12;
32584 _ij12[1] = -1;
32585 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
32586 {
32587 if (j12valid[iij12]
32588 && IKabs(
32589 cj12array[ij12] - cj12array[iij12])
32590 < IKFAST_SOLUTION_THRESH
32591 && IKabs(
32592 sj12array[ij12] - sj12array[iij12])
32593 < IKFAST_SOLUTION_THRESH)
32594 {
32595 j12valid[iij12] = false;
32596 _ij12[1] = iij12;
32597 break;
32598 }
32599 }
32600 j12 = j12array[ij12];
32601 cj12 = cj12array[ij12];
32602 sj12 = sj12array[ij12];
32603 {
32604 IkReal evalcond[8];
32605 IkReal x212 = IKsin(j12);
32606 IkReal x213 = (cj10 * x212);
32607 IkReal x214 = IKcos(j12);
32608 IkReal x215 = ((1.0) * x214);
32609 IkReal x216 = ((-1.0) * x215);
32610 IkReal x217 = ((1.0) * sj10);
32611 IkReal x218
32612 = ((((-1.0) * cj10 * x215))
32613 + ((sj10 * x212)));
32614 evalcond[0]
32615 = (((cj10 * new_r01)) + x212
32616 + ((new_r11 * sj10)));
32617 evalcond[1]
32618 = (((sj10 * x214)) + x213 + new_r01);
32619 evalcond[2]
32620 = (((cj10 * new_r00)) + ((new_r10 * sj10))
32621 + x216);
32622 evalcond[3]
32623 = ((((-1.0) * new_r00 * x217))
32624 + (((-1.0) * x212))
32625 + ((cj10 * new_r10)));
32626 evalcond[4]
32627 = (((cj10 * new_r11)) + x216
32628 + (((-1.0) * new_r01 * x217)));
32629 evalcond[5] = (x218 + new_r00);
32630 evalcond[6] = (x218 + new_r11);
32631 evalcond[7]
32632 = ((((-1.0) * x213))
32633 + (((-1.0) * x214 * x217)) + new_r10);
32634 if (IKabs(evalcond[0])
32635 > IKFAST_EVALCOND_THRESH
32636 || IKabs(evalcond[1])
32637 > IKFAST_EVALCOND_THRESH
32638 || IKabs(evalcond[2])
32639 > IKFAST_EVALCOND_THRESH
32640 || IKabs(evalcond[3])
32641 > IKFAST_EVALCOND_THRESH
32642 || IKabs(evalcond[4])
32643 > IKFAST_EVALCOND_THRESH
32644 || IKabs(evalcond[5])
32645 > IKFAST_EVALCOND_THRESH
32646 || IKabs(evalcond[6])
32647 > IKFAST_EVALCOND_THRESH
32648 || IKabs(evalcond[7])
32649 > IKFAST_EVALCOND_THRESH)
32650 {
32651 continue;
32652 }
32653 }
32654
32655 {
32656 std::vector<IkSingleDOFSolutionBase<IkReal> >
32657 vinfos(7);
32658 vinfos[0].jointtype = 1;
32659 vinfos[0].foffset = j4;
32660 vinfos[0].indices[0] = _ij4[0];
32661 vinfos[0].indices[1] = _ij4[1];
32662 vinfos[0].maxsolutions = _nj4;
32663 vinfos[1].jointtype = 1;
32664 vinfos[1].foffset = j6;
32665 vinfos[1].indices[0] = _ij6[0];
32666 vinfos[1].indices[1] = _ij6[1];
32667 vinfos[1].maxsolutions = _nj6;
32668 vinfos[2].jointtype = 1;
32669 vinfos[2].foffset = j8;
32670 vinfos[2].indices[0] = _ij8[0];
32671 vinfos[2].indices[1] = _ij8[1];
32672 vinfos[2].maxsolutions = _nj8;
32673 vinfos[3].jointtype = 1;
32674 vinfos[3].foffset = j9;
32675 vinfos[3].indices[0] = _ij9[0];
32676 vinfos[3].indices[1] = _ij9[1];
32677 vinfos[3].maxsolutions = _nj9;
32678 vinfos[4].jointtype = 1;
32679 vinfos[4].foffset = j10;
32680 vinfos[4].indices[0] = _ij10[0];
32681 vinfos[4].indices[1] = _ij10[1];
32682 vinfos[4].maxsolutions = _nj10;
32683 vinfos[5].jointtype = 1;
32684 vinfos[5].foffset = j11;
32685 vinfos[5].indices[0] = _ij11[0];
32686 vinfos[5].indices[1] = _ij11[1];
32687 vinfos[5].maxsolutions = _nj11;
32688 vinfos[6].jointtype = 1;
32689 vinfos[6].foffset = j12;
32690 vinfos[6].indices[0] = _ij12[0];
32691 vinfos[6].indices[1] = _ij12[1];
32692 vinfos[6].maxsolutions = _nj12;
32693 std::vector<int> vfree(0);
32694 solutions.AddSolution(vinfos, vfree);
32695 }
32696 }
32697 }
32698 }
32699 }
32700 }
32701 } while (0);
32702 if (bgotonextstatement)
32703 {
32704 bool bgotonextstatement = true;
32705 do
32706 {
32707 evalcond[0]
32708 = ((-3.14159265358979)
32709 + (IKfmod(
32710 ((3.14159265358979)
32711 + (IKabs(((-3.14159265358979) + j11)))),
32712 6.28318530717959)));
32713 evalcond[1] = ((1.0) + new_r22);
32714 evalcond[2] = new_r20;
32715 evalcond[3] = new_r02;
32716 evalcond[4] = new_r12;
32717 evalcond[5] = new_r21;
32718 if (IKabs(evalcond[0]) < 0.0000010000000000
32719 && IKabs(evalcond[1]) < 0.0000010000000000
32720 && IKabs(evalcond[2]) < 0.0000010000000000
32721 && IKabs(evalcond[3]) < 0.0000010000000000
32722 && IKabs(evalcond[4]) < 0.0000010000000000
32723 && IKabs(evalcond[5]) < 0.0000010000000000)
32724 {
32725 bgotonextstatement = false;
32726 {
32727 IkReal j10array[2], cj10array[2], sj10array[2];
32728 bool j10valid[2] = {false};
32729 _nj10 = 2;
32730 CheckValue<IkReal> x220 = IKatan2WithCheck(
32731 IkReal(new_r02),
32732 new_r12,
32733 IKFAST_ATAN2_MAGTHRESH);
32734 if (!x220.valid)
32735 {
32736 continue;
32737 }
32738 IkReal x219 = ((-1.0) * (((1.0) * (x220.value))));
32739 j10array[0] = x219;
32740 sj10array[0] = IKsin(j10array[0]);
32741 cj10array[0] = IKcos(j10array[0]);
32742 j10array[1] = ((3.14159265358979) + x219);
32743 sj10array[1] = IKsin(j10array[1]);
32744 cj10array[1] = IKcos(j10array[1]);
32745 if (j10array[0] > IKPI)
32746 {
32747 j10array[0] -= IK2PI;
32748 }
32749 else if (j10array[0] < -IKPI)
32750 {
32751 j10array[0] += IK2PI;
32752 }
32753 j10valid[0] = true;
32754 if (j10array[1] > IKPI)
32755 {
32756 j10array[1] -= IK2PI;
32757 }
32758 else if (j10array[1] < -IKPI)
32759 {
32760 j10array[1] += IK2PI;
32761 }
32762 j10valid[1] = true;
32763 for (int ij10 = 0; ij10 < 2; ++ij10)
32764 {
32765 if (!j10valid[ij10])
32766 {
32767 continue;
32768 }
32769 _ij10[0] = ij10;
32770 _ij10[1] = -1;
32771 for (int iij10 = ij10 + 1; iij10 < 2; ++iij10)
32772 {
32773 if (j10valid[iij10]
32774 && IKabs(cj10array[ij10] - cj10array[iij10])
32775 < IKFAST_SOLUTION_THRESH
32776 && IKabs(sj10array[ij10] - sj10array[iij10])
32777 < IKFAST_SOLUTION_THRESH)
32778 {
32779 j10valid[iij10] = false;
32780 _ij10[1] = iij10;
32781 break;
32782 }
32783 }
32784 j10 = j10array[ij10];
32785 cj10 = cj10array[ij10];
32786 sj10 = sj10array[ij10];
32787 {
32788 IkReal evalcond[1];
32789 evalcond[0]
32790 = (((new_r12 * (IKcos(j10))))
32791 + (((-1.0) * (1.0) * new_r02
32792 * (IKsin(j10)))));
32793 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
32794 {
32795 continue;
32796 }
32797 }
32798
32799 {
32800 IkReal j12array[1], cj12array[1], sj12array[1];
32801 bool j12valid[1] = {false};
32802 _nj12 = 1;
32803 IkReal x221 = ((1.0) * new_r00);
32804 if (IKabs(
32805 ((((-1.0) * sj10 * x221))
32806 + ((cj10 * new_r01))))
32807 < IKFAST_ATAN2_MAGTHRESH
32808 && IKabs(
32809 ((((-1.0) * cj10 * x221))
32810 + (((-1.0) * (1.0) * new_r01
32811 * sj10))))
32812 < IKFAST_ATAN2_MAGTHRESH
32813 && IKabs(
32814 IKsqr(
32815 ((((-1.0) * sj10 * x221))
32816 + ((cj10 * new_r01))))
32817 + IKsqr(
32818 ((((-1.0) * cj10 * x221))
32819 + (((-1.0) * (1.0) * new_r01
32820 * sj10))))
32821 - 1)
32822 <= IKFAST_SINCOS_THRESH)
32823 continue;
32824 j12array[0] = IKatan2(
32825 ((((-1.0) * sj10 * x221))
32826 + ((cj10 * new_r01))),
32827 ((((-1.0) * cj10 * x221))
32828 + (((-1.0) * (1.0) * new_r01 * sj10))));
32829 sj12array[0] = IKsin(j12array[0]);
32830 cj12array[0] = IKcos(j12array[0]);
32831 if (j12array[0] > IKPI)
32832 {
32833 j12array[0] -= IK2PI;
32834 }
32835 else if (j12array[0] < -IKPI)
32836 {
32837 j12array[0] += IK2PI;
32838 }
32839 j12valid[0] = true;
32840 for (int ij12 = 0; ij12 < 1; ++ij12)
32841 {
32842 if (!j12valid[ij12])
32843 {
32844 continue;
32845 }
32846 _ij12[0] = ij12;
32847 _ij12[1] = -1;
32848 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
32849 {
32850 if (j12valid[iij12]
32851 && IKabs(
32852 cj12array[ij12]
32853 - cj12array[iij12])
32854 < IKFAST_SOLUTION_THRESH
32855 && IKabs(
32856 sj12array[ij12]
32857 - sj12array[iij12])
32858 < IKFAST_SOLUTION_THRESH)
32859 {
32860 j12valid[iij12] = false;
32861 _ij12[1] = iij12;
32862 break;
32863 }
32864 }
32865 j12 = j12array[ij12];
32866 cj12 = cj12array[ij12];
32867 sj12 = sj12array[ij12];
32868 {
32869 IkReal evalcond[8];
32870 IkReal x222 = IKcos(j12);
32871 IkReal x223 = IKsin(j12);
32872 IkReal x224 = ((1.0) * x223);
32873 IkReal x225 = ((-1.0) * x224);
32874 IkReal x226 = (cj10 * x222);
32875 IkReal x227 = ((1.0) * sj10);
32876 IkReal x228
32877 = (((sj10 * x222))
32878 + (((-1.0) * cj10 * x224)));
32879 evalcond[0]
32880 = (((cj10 * new_r00))
32881 + ((new_r10 * sj10)) + x222);
32882 evalcond[1]
32883 = (((cj10 * new_r01))
32884 + ((new_r11 * sj10)) + x225);
32885 evalcond[2]
32886 = (((sj10 * x223)) + new_r00 + x226);
32887 evalcond[3]
32888 = (((cj10 * new_r10))
32889 + (((-1.0) * new_r00 * x227))
32890 + x225);
32891 evalcond[4]
32892 = (((cj10 * new_r11))
32893 + (((-1.0) * x222))
32894 + (((-1.0) * new_r01 * x227)));
32895 evalcond[5] = (new_r01 + x228);
32896 evalcond[6] = (new_r10 + x228);
32897 evalcond[7]
32898 = ((((-1.0) * x226)) + new_r11
32899 + (((-1.0) * x223 * x227)));
32900 if (IKabs(evalcond[0])
32901 > IKFAST_EVALCOND_THRESH
32902 || IKabs(evalcond[1])
32903 > IKFAST_EVALCOND_THRESH
32904 || IKabs(evalcond[2])
32905 > IKFAST_EVALCOND_THRESH
32906 || IKabs(evalcond[3])
32907 > IKFAST_EVALCOND_THRESH
32908 || IKabs(evalcond[4])
32909 > IKFAST_EVALCOND_THRESH
32910 || IKabs(evalcond[5])
32911 > IKFAST_EVALCOND_THRESH
32912 || IKabs(evalcond[6])
32913 > IKFAST_EVALCOND_THRESH
32914 || IKabs(evalcond[7])
32915 > IKFAST_EVALCOND_THRESH)
32916 {
32917 continue;
32918 }
32919 }
32920
32921 {
32922 std::vector<
32923 IkSingleDOFSolutionBase<IkReal> >
32924 vinfos(7);
32925 vinfos[0].jointtype = 1;
32926 vinfos[0].foffset = j4;
32927 vinfos[0].indices[0] = _ij4[0];
32928 vinfos[0].indices[1] = _ij4[1];
32929 vinfos[0].maxsolutions = _nj4;
32930 vinfos[1].jointtype = 1;
32931 vinfos[1].foffset = j6;
32932 vinfos[1].indices[0] = _ij6[0];
32933 vinfos[1].indices[1] = _ij6[1];
32934 vinfos[1].maxsolutions = _nj6;
32935 vinfos[2].jointtype = 1;
32936 vinfos[2].foffset = j8;
32937 vinfos[2].indices[0] = _ij8[0];
32938 vinfos[2].indices[1] = _ij8[1];
32939 vinfos[2].maxsolutions = _nj8;
32940 vinfos[3].jointtype = 1;
32941 vinfos[3].foffset = j9;
32942 vinfos[3].indices[0] = _ij9[0];
32943 vinfos[3].indices[1] = _ij9[1];
32944 vinfos[3].maxsolutions = _nj9;
32945 vinfos[4].jointtype = 1;
32946 vinfos[4].foffset = j10;
32947 vinfos[4].indices[0] = _ij10[0];
32948 vinfos[4].indices[1] = _ij10[1];
32949 vinfos[4].maxsolutions = _nj10;
32950 vinfos[5].jointtype = 1;
32951 vinfos[5].foffset = j11;
32952 vinfos[5].indices[0] = _ij11[0];
32953 vinfos[5].indices[1] = _ij11[1];
32954 vinfos[5].maxsolutions = _nj11;
32955 vinfos[6].jointtype = 1;
32956 vinfos[6].foffset = j12;
32957 vinfos[6].indices[0] = _ij12[0];
32958 vinfos[6].indices[1] = _ij12[1];
32959 vinfos[6].maxsolutions = _nj12;
32960 std::vector<int> vfree(0);
32961 solutions.AddSolution(vinfos, vfree);
32962 }
32963 }
32964 }
32965 }
32966 }
32967 }
32968 } while (0);
32969 if (bgotonextstatement)
32970 {
32971 bool bgotonextstatement = true;
32972 do
32973 {
32974 if (1)
32975 {
32976 bgotonextstatement = false;
32977 continue; // branch miss [j10, j12]
32978 }
32979 } while (0);
32980 if (bgotonextstatement)
32981 {
32982 }
32983 }
32984 }
32985 }
32986 }
32987 else
32988 {
32989 {
32990 IkReal j10array[1], cj10array[1], sj10array[1];
32991 bool j10valid[1] = {false};
32992 _nj10 = 1;
32993 CheckValue<IkReal> x230 = IKPowWithIntegerCheck(sj11, -1);
32994 if (!x230.valid)
32995 {
32996 continue;
32997 }
32998 IkReal x229 = x230.value;
32999 CheckValue<IkReal> x231
33000 = IKPowWithIntegerCheck(new_r12, -1);
33001 if (!x231.valid)
33002 {
33003 continue;
33004 }
33005 if (IKabs(
33006 (x229 * (x231.value)
33007 * (((1.0) + (((-1.0) * (1.0) * (cj11 * cj11)))
33008 + (((-1.0) * (1.0) * (new_r02 * new_r02)))))))
33009 < IKFAST_ATAN2_MAGTHRESH
33010 && IKabs((new_r02 * x229)) < IKFAST_ATAN2_MAGTHRESH
33011 && IKabs(
33012 IKsqr(
33013 (x229 * (x231.value)
33014 * (((1.0)
33015 + (((-1.0) * (1.0) * (cj11 * cj11)))
33016 + (((-1.0) * (1.0)
33017 * (new_r02 * new_r02)))))))
33018 + IKsqr((new_r02 * x229)) - 1)
33019 <= IKFAST_SINCOS_THRESH)
33020 continue;
33021 j10array[0] = IKatan2(
33022 (x229 * (x231.value)
33023 * (((1.0) + (((-1.0) * (1.0) * (cj11 * cj11)))
33024 + (((-1.0) * (1.0) * (new_r02 * new_r02)))))),
33025 (new_r02 * x229));
33026 sj10array[0] = IKsin(j10array[0]);
33027 cj10array[0] = IKcos(j10array[0]);
33028 if (j10array[0] > IKPI)
33029 {
33030 j10array[0] -= IK2PI;
33031 }
33032 else if (j10array[0] < -IKPI)
33033 {
33034 j10array[0] += IK2PI;
33035 }
33036 j10valid[0] = true;
33037 for (int ij10 = 0; ij10 < 1; ++ij10)
33038 {
33039 if (!j10valid[ij10])
33040 {
33041 continue;
33042 }
33043 _ij10[0] = ij10;
33044 _ij10[1] = -1;
33045 for (int iij10 = ij10 + 1; iij10 < 1; ++iij10)
33046 {
33047 if (j10valid[iij10]
33048 && IKabs(cj10array[ij10] - cj10array[iij10])
33049 < IKFAST_SOLUTION_THRESH
33050 && IKabs(sj10array[ij10] - sj10array[iij10])
33051 < IKFAST_SOLUTION_THRESH)
33052 {
33053 j10valid[iij10] = false;
33054 _ij10[1] = iij10;
33055 break;
33056 }
33057 }
33058 j10 = j10array[ij10];
33059 cj10 = cj10array[ij10];
33060 sj10 = sj10array[ij10];
33061 {
33062 IkReal evalcond[8];
33063 IkReal x232 = IKcos(j10);
33064 IkReal x233 = ((1.0) * sj11);
33065 IkReal x234 = (x232 * x233);
33066 IkReal x235 = IKsin(j10);
33067 IkReal x236 = (x233 * x235);
33068 IkReal x237 = (new_r02 * x232);
33069 IkReal x238 = (new_r12 * x235);
33070 IkReal x239 = ((1.0) * cj11);
33071 evalcond[0] = ((((-1.0) * x234)) + new_r02);
33072 evalcond[1] = (new_r12 + (((-1.0) * x236)));
33073 evalcond[2]
33074 = (((new_r12 * x232))
33075 + (((-1.0) * new_r02 * x235)));
33076 evalcond[3] = ((((-1.0) * x233)) + x237 + x238);
33077 evalcond[4]
33078 = (((cj11 * x238)) + (((-1.0) * new_r22 * x233))
33079 + ((cj11 * x237)));
33080 evalcond[5]
33081 = ((((-1.0) * new_r00 * x234))
33082 + (((-1.0) * new_r10 * x236))
33083 + (((-1.0) * new_r20 * x239)));
33084 evalcond[6]
33085 = ((((-1.0) * new_r01 * x234))
33086 + (((-1.0) * new_r21 * x239))
33087 + (((-1.0) * new_r11 * x236)));
33088 evalcond[7]
33089 = ((1.0) + (((-1.0) * new_r22 * x239))
33090 + (((-1.0) * x233 * x238))
33091 + (((-1.0) * x233 * x237)));
33092 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
33093 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
33094 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
33095 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
33096 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
33097 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
33098 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
33099 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
33100 {
33101 continue;
33102 }
33103 }
33104
33105 {
33106 IkReal j12eval[2];
33107 IkReal x240 = ((1.0) * sj9);
33108 IkReal x241 = (cj9 * sj6);
33109 IkReal x242 = x160;
33110 IkReal x243 = (cj4 * sj8);
33111 IkReal x244 = (sj6 * x240);
33112 IkReal x245 = (cj6 * cj9);
33113 IkReal x246 = (((cj8 * x245)) + (((-1.0) * x244)));
33114 IkReal x247 = (((cj9 * x243)) + ((sj4 * x246)));
33115 IkReal x248 = ((1.0) * sj4 * sj8);
33116 IkReal x249
33117 = ((((-1.0) * cj9 * x248)) + ((cj4 * x246)));
33118 IkReal x250 = (sj6 * sj8);
33119 IkReal x251 = x169;
33120 IkReal x252 = x170;
33121 IkReal x253 = (x245 + (((-1.0) * cj8 * x244)));
33122 IkReal x254 = x172;
33123 IkReal x255 = (((sj9 * x243)) + ((sj4 * x254)));
33124 IkReal x256
33125 = (((cj4 * x254)) + (((-1.0) * sj4 * sj8 * x240)));
33126 new_r00
33127 = (((r20 * x242)) + ((r00 * x249))
33128 + ((r10 * x247)));
33129 new_r01
33130 = (((r21 * x242)) + ((r01 * x249))
33131 + ((r11 * x247)));
33132 new_r02
33133 = (((r22 * x242)) + ((r12 * x247))
33134 + ((r02 * x249)));
33135 new_r10
33136 = (((r10 * x251)) + ((r20 * x250))
33137 + ((r00 * x252)));
33138 new_r11
33139 = (((r11 * x251)) + ((r21 * x250))
33140 + ((r01 * x252)));
33141 new_r12
33142 = (((r22 * x250)) + ((r12 * x251))
33143 + ((r02 * x252)));
33144 new_r20
33145 = (((r20 * x253)) + ((r00 * x256))
33146 + ((r10 * x255)));
33147 new_r21
33148 = (((r21 * x253)) + ((r11 * x255))
33149 + ((r01 * x256)));
33150 new_r22
33151 = (((r02 * x256)) + ((r12 * x255))
33152 + ((r22 * x253)));
33153 j12eval[0] = sj11;
33154 j12eval[1] = IKsign(sj11);
33155 if (IKabs(j12eval[0]) < 0.0000010000000000
33156 || IKabs(j12eval[1]) < 0.0000010000000000)
33157 {
33158 {
33159 IkReal j12eval[2];
33160 IkReal x257 = ((1.0) * sj9);
33161 IkReal x258 = (cj9 * sj6);
33162 IkReal x259 = x160;
33163 IkReal x260 = (cj4 * sj8);
33164 IkReal x261 = (sj6 * x257);
33165 IkReal x262 = (cj6 * cj9);
33166 IkReal x263 = (((cj8 * x262)) + (((-1.0) * x261)));
33167 IkReal x264 = (((sj4 * x263)) + ((cj9 * x260)));
33168 IkReal x265 = ((1.0) * sj4 * sj8);
33169 IkReal x266
33170 = ((((-1.0) * cj9 * x265)) + ((cj4 * x263)));
33171 IkReal x267 = (sj6 * sj8);
33172 IkReal x268 = x169;
33173 IkReal x269 = x170;
33174 IkReal x270 = (x262 + (((-1.0) * cj8 * x261)));
33175 IkReal x271 = x172;
33176 IkReal x272 = (((sj4 * x271)) + ((sj9 * x260)));
33177 IkReal x273
33178 = (((cj4 * x271))
33179 + (((-1.0) * sj4 * sj8 * x257)));
33180 new_r00
33181 = (((r00 * x266)) + ((r20 * x259))
33182 + ((r10 * x264)));
33183 new_r01
33184 = (((r21 * x259)) + ((r01 * x266))
33185 + ((r11 * x264)));
33186 new_r02
33187 = (((r02 * x266)) + ((r12 * x264))
33188 + ((r22 * x259)));
33189 new_r10
33190 = (((r20 * x267)) + ((r00 * x269))
33191 + ((r10 * x268)));
33192 new_r11
33193 = (((r01 * x269)) + ((r21 * x267))
33194 + ((r11 * x268)));
33195 new_r12
33196 = (((r22 * x267)) + ((r12 * x268))
33197 + ((r02 * x269)));
33198 new_r20
33199 = (((r00 * x273)) + ((r10 * x272))
33200 + ((r20 * x270)));
33201 new_r21
33202 = (((r11 * x272)) + ((r01 * x273))
33203 + ((r21 * x270)));
33204 new_r22
33205 = (((r12 * x272)) + ((r02 * x273))
33206 + ((r22 * x270)));
33207 j12eval[0] = sj10;
33208 j12eval[1] = sj11;
33209 if (IKabs(j12eval[0]) < 0.0000010000000000
33210 || IKabs(j12eval[1]) < 0.0000010000000000)
33211 {
33212 {
33213 IkReal j12eval[3];
33214 IkReal x274 = ((1.0) * sj9);
33215 IkReal x275 = (cj9 * sj6);
33216 IkReal x276 = x160;
33217 IkReal x277 = (cj4 * sj8);
33218 IkReal x278 = (sj6 * x274);
33219 IkReal x279 = (cj6 * cj9);
33220 IkReal x280
33221 = (((cj8 * x279)) + (((-1.0) * x278)));
33222 IkReal x281 = (((cj9 * x277)) + ((sj4 * x280)));
33223 IkReal x282 = ((1.0) * sj4 * sj8);
33224 IkReal x283
33225 = (((cj4 * x280))
33226 + (((-1.0) * cj9 * x282)));
33227 IkReal x284 = (sj6 * sj8);
33228 IkReal x285 = x169;
33229 IkReal x286 = x170;
33230 IkReal x287 = ((((-1.0) * cj8 * x278)) + x279);
33231 IkReal x288 = x172;
33232 IkReal x289 = (((sj9 * x277)) + ((sj4 * x288)));
33233 IkReal x290
33234 = (((cj4 * x288))
33235 + (((-1.0) * sj4 * sj8 * x274)));
33236 new_r00
33237 = (((r00 * x283)) + ((r10 * x281))
33238 + ((r20 * x276)));
33239 new_r01
33240 = (((r01 * x283)) + ((r11 * x281))
33241 + ((r21 * x276)));
33242 new_r02
33243 = (((r22 * x276)) + ((r02 * x283))
33244 + ((r12 * x281)));
33245 new_r10
33246 = (((r10 * x285)) + ((r00 * x286))
33247 + ((r20 * x284)));
33248 new_r11
33249 = (((r01 * x286)) + ((r21 * x284))
33250 + ((r11 * x285)));
33251 new_r12
33252 = (((r22 * x284)) + ((r02 * x286))
33253 + ((r12 * x285)));
33254 new_r20
33255 = (((r00 * x290)) + ((r20 * x287))
33256 + ((r10 * x289)));
33257 new_r21
33258 = (((r21 * x287)) + ((r11 * x289))
33259 + ((r01 * x290)));
33260 new_r22
33261 = (((r22 * x287)) + ((r12 * x289))
33262 + ((r02 * x290)));
33263 j12eval[0] = cj10;
33264 j12eval[1] = cj11;
33265 j12eval[2] = sj11;
33266 if (IKabs(j12eval[0]) < 0.0000010000000000
33267 || IKabs(j12eval[1]) < 0.0000010000000000
33268 || IKabs(j12eval[2]) < 0.0000010000000000)
33269 {
33270 {
33271 IkReal evalcond[12];
33272 bool bgotonextstatement = true;
33273 do
33274 {
33275 IkReal x291 = ((1.0) * cj11);
33276 IkReal x292
33277 = ((((-1.0) * x291)) + new_r22);
33278 IkReal x293 = ((1.0) * sj11);
33279 IkReal x294
33280 = ((((-1.0) * x293)) + new_r12);
33281 evalcond[0]
33282 = ((-3.14159265358979)
33283 + (IKfmod(
33284 ((3.14159265358979)
33285 + (IKabs(
33286 ((-1.5707963267949)
33287 + j10)))),
33288 6.28318530717959)));
33289 evalcond[1] = x292;
33290 evalcond[2] = x292;
33291 evalcond[3] = new_r02;
33292 evalcond[4] = x294;
33293 evalcond[5] = x294;
33294 evalcond[6]
33295 = ((((-1.0) * new_r22 * x293))
33296 + ((cj11 * new_r12)));
33297 evalcond[7]
33298 = ((((-1.0) * new_r10 * x293))
33299 + (((-1.0) * new_r20 * x291)));
33300 evalcond[8]
33301 = ((((-1.0) * new_r11 * x293))
33302 + (((-1.0) * new_r21 * x291)));
33303 evalcond[9]
33304 = ((1.0) + (((-1.0) * new_r12 * x293))
33305 + (((-1.0) * new_r22 * x291)));
33306 if (IKabs(evalcond[0])
33307 < 0.0000010000000000
33308 && IKabs(evalcond[1])
33309 < 0.0000010000000000
33310 && IKabs(evalcond[2])
33311 < 0.0000010000000000
33312 && IKabs(evalcond[3])
33313 < 0.0000010000000000
33314 && IKabs(evalcond[4])
33315 < 0.0000010000000000
33316 && IKabs(evalcond[5])
33317 < 0.0000010000000000
33318 && IKabs(evalcond[6])
33319 < 0.0000010000000000
33320 && IKabs(evalcond[7])
33321 < 0.0000010000000000
33322 && IKabs(evalcond[8])
33323 < 0.0000010000000000
33324 && IKabs(evalcond[9])
33325 < 0.0000010000000000)
33326 {
33327 bgotonextstatement = false;
33328 {
33329 IkReal j12array[1], cj12array[1],
33330 sj12array[1];
33331 bool j12valid[1] = {false};
33332 _nj12 = 1;
33333 CheckValue<IkReal> x295
33334 = IKPowWithIntegerCheck(
33335 IKsign(new_r12), -1);
33336 if (!x295.valid)
33337 {
33338 continue;
33339 }
33340 CheckValue<IkReal> x296
33341 = IKatan2WithCheck(
33342 IkReal(new_r21),
33343 ((-1.0)
33344 * (((1.0) * new_r20))),
33345 IKFAST_ATAN2_MAGTHRESH);
33346 if (!x296.valid)
33347 {
33348 continue;
33349 }
33350 j12array[0]
33351 = ((-1.5707963267949)
33352 + (((1.5707963267949)
33353 * (x295.value)))
33354 + (x296.value));
33355 sj12array[0] = IKsin(j12array[0]);
33356 cj12array[0] = IKcos(j12array[0]);
33357 if (j12array[0] > IKPI)
33358 {
33359 j12array[0] -= IK2PI;
33360 }
33361 else if (j12array[0] < -IKPI)
33362 {
33363 j12array[0] += IK2PI;
33364 }
33365 j12valid[0] = true;
33366 for (int ij12 = 0; ij12 < 1; ++ij12)
33367 {
33368 if (!j12valid[ij12])
33369 {
33370 continue;
33371 }
33372 _ij12[0] = ij12;
33373 _ij12[1] = -1;
33374 for (int iij12 = ij12 + 1;
33375 iij12 < 1;
33376 ++iij12)
33377 {
33378 if (j12valid[iij12]
33379 && IKabs(
33380 cj12array[ij12]
33381 - cj12array[iij12])
33382 < IKFAST_SOLUTION_THRESH
33383 && IKabs(
33384 sj12array[ij12]
33385 - sj12array[iij12])
33386 < IKFAST_SOLUTION_THRESH)
33387 {
33388 j12valid[iij12] = false;
33389 _ij12[1] = iij12;
33390 break;
33391 }
33392 }
33393 j12 = j12array[ij12];
33394 cj12 = cj12array[ij12];
33395 sj12 = sj12array[ij12];
33396 {
33397 IkReal evalcond[8];
33398 IkReal x297 = IKcos(j12);
33399 IkReal x298 = IKsin(j12);
33400 IkReal x299 = ((1.0) * new_r12);
33401 IkReal x300 = ((1.0) * x297);
33402 IkReal x301 = ((-1.0) * x300);
33403 evalcond[0]
33404 = (((new_r12 * x297))
33405 + new_r20);
33406 evalcond[1]
33407 = (((new_r22 * x298))
33408 + new_r11);
33409 evalcond[2]
33410 = ((((-1.0) * x298 * x299))
33411 + new_r21);
33412 evalcond[3]
33413 = ((((-1.0) * new_r22 * x300))
33414 + new_r10);
33415 evalcond[4]
33416 = ((((-1.0) * (1.0)
33417 * new_r00))
33418 + (((-1.0) * x298)));
33419 evalcond[5]
33420 = (x301
33421 + (((-1.0) * (1.0)
33422 * new_r01)));
33423 evalcond[6]
33424 = (((new_r11 * new_r22))
33425 + x298
33426 + (((-1.0) * new_r21
33427 * x299)));
33428 evalcond[7]
33429 = ((((-1.0) * new_r20 * x299))
33430 + x301
33431 + ((new_r10 * new_r22)));
33432 if (IKabs(evalcond[0])
33433 > IKFAST_EVALCOND_THRESH
33434 || IKabs(evalcond[1])
33435 > IKFAST_EVALCOND_THRESH
33436 || IKabs(evalcond[2])
33437 > IKFAST_EVALCOND_THRESH
33438 || IKabs(evalcond[3])
33439 > IKFAST_EVALCOND_THRESH
33440 || IKabs(evalcond[4])
33441 > IKFAST_EVALCOND_THRESH
33442 || IKabs(evalcond[5])
33443 > IKFAST_EVALCOND_THRESH
33444 || IKabs(evalcond[6])
33445 > IKFAST_EVALCOND_THRESH
33446 || IKabs(evalcond[7])
33447 > IKFAST_EVALCOND_THRESH)
33448 {
33449 continue;
33450 }
33451 }
33452
33453 {
33454 std::vector<
33455 IkSingleDOFSolutionBase<
33456 IkReal> >
33457 vinfos(7);
33458 vinfos[0].jointtype = 1;
33459 vinfos[0].foffset = j4;
33460 vinfos[0].indices[0] = _ij4[0];
33461 vinfos[0].indices[1] = _ij4[1];
33462 vinfos[0].maxsolutions = _nj4;
33463 vinfos[1].jointtype = 1;
33464 vinfos[1].foffset = j6;
33465 vinfos[1].indices[0] = _ij6[0];
33466 vinfos[1].indices[1] = _ij6[1];
33467 vinfos[1].maxsolutions = _nj6;
33468 vinfos[2].jointtype = 1;
33469 vinfos[2].foffset = j8;
33470 vinfos[2].indices[0] = _ij8[0];
33471 vinfos[2].indices[1] = _ij8[1];
33472 vinfos[2].maxsolutions = _nj8;
33473 vinfos[3].jointtype = 1;
33474 vinfos[3].foffset = j9;
33475 vinfos[3].indices[0] = _ij9[0];
33476 vinfos[3].indices[1] = _ij9[1];
33477 vinfos[3].maxsolutions = _nj9;
33478 vinfos[4].jointtype = 1;
33479 vinfos[4].foffset = j10;
33480 vinfos[4].indices[0] = _ij10[0];
33481 vinfos[4].indices[1] = _ij10[1];
33482 vinfos[4].maxsolutions = _nj10;
33483 vinfos[5].jointtype = 1;
33484 vinfos[5].foffset = j11;
33485 vinfos[5].indices[0] = _ij11[0];
33486 vinfos[5].indices[1] = _ij11[1];
33487 vinfos[5].maxsolutions = _nj11;
33488 vinfos[6].jointtype = 1;
33489 vinfos[6].foffset = j12;
33490 vinfos[6].indices[0] = _ij12[0];
33491 vinfos[6].indices[1] = _ij12[1];
33492 vinfos[6].maxsolutions = _nj12;
33493 std::vector<int> vfree(0);
33494 solutions.AddSolution(
33495 vinfos, vfree);
33496 }
33497 }
33498 }
33499 }
33500 } while (0);
33501 if (bgotonextstatement)
33502 {
33503 bool bgotonextstatement = true;
33504 do
33505 {
33506 IkReal x302 = ((1.0) * cj11);
33507 IkReal x303
33508 = ((((-1.0) * x302)) + new_r22);
33509 IkReal x304 = ((1.0) * sj11);
33510 evalcond[0]
33511 = ((-3.14159265358979)
33512 + (IKfmod(
33513 ((3.14159265358979)
33514 + (IKabs(
33515 ((1.5707963267949)
33516 + j10)))),
33517 6.28318530717959)));
33518 evalcond[1] = x303;
33519 evalcond[2] = x303;
33520 evalcond[3] = new_r02;
33521 evalcond[4] = (sj11 + new_r12);
33522 evalcond[5]
33523 = ((((-1.0) * (1.0) * new_r12))
33524 + (((-1.0) * x304)));
33525 evalcond[6]
33526 = ((((-1.0) * new_r22 * x304))
33527 + (((-1.0) * new_r12 * x302)));
33528 evalcond[7]
33529 = (((new_r10 * sj11))
33530 + (((-1.0) * new_r20 * x302)));
33531 evalcond[8]
33532 = ((((-1.0) * new_r21 * x302))
33533 + ((new_r11 * sj11)));
33534 evalcond[9]
33535 = ((1.0)
33536 + (((-1.0) * new_r22 * x302))
33537 + ((new_r12 * sj11)));
33538 if (IKabs(evalcond[0])
33539 < 0.0000010000000000
33540 && IKabs(evalcond[1])
33541 < 0.0000010000000000
33542 && IKabs(evalcond[2])
33543 < 0.0000010000000000
33544 && IKabs(evalcond[3])
33545 < 0.0000010000000000
33546 && IKabs(evalcond[4])
33547 < 0.0000010000000000
33548 && IKabs(evalcond[5])
33549 < 0.0000010000000000
33550 && IKabs(evalcond[6])
33551 < 0.0000010000000000
33552 && IKabs(evalcond[7])
33553 < 0.0000010000000000
33554 && IKabs(evalcond[8])
33555 < 0.0000010000000000
33556 && IKabs(evalcond[9])
33557 < 0.0000010000000000)
33558 {
33559 bgotonextstatement = false;
33560 {
33561 IkReal j12array[1], cj12array[1],
33562 sj12array[1];
33563 bool j12valid[1] = {false};
33564 _nj12 = 1;
33565 if (IKabs(new_r00)
33566 < IKFAST_ATAN2_MAGTHRESH
33567 && IKabs(new_r01)
33568 < IKFAST_ATAN2_MAGTHRESH
33569 && IKabs(
33570 IKsqr(new_r00)
33571 + IKsqr(new_r01) - 1)
33572 <= IKFAST_SINCOS_THRESH)
33573 continue;
33574 j12array[0]
33575 = IKatan2(new_r00, new_r01);
33576 sj12array[0] = IKsin(j12array[0]);
33577 cj12array[0] = IKcos(j12array[0]);
33578 if (j12array[0] > IKPI)
33579 {
33580 j12array[0] -= IK2PI;
33581 }
33582 else if (j12array[0] < -IKPI)
33583 {
33584 j12array[0] += IK2PI;
33585 }
33586 j12valid[0] = true;
33587 for (int ij12 = 0; ij12 < 1; ++ij12)
33588 {
33589 if (!j12valid[ij12])
33590 {
33591 continue;
33592 }
33593 _ij12[0] = ij12;
33594 _ij12[1] = -1;
33595 for (int iij12 = ij12 + 1;
33596 iij12 < 1;
33597 ++iij12)
33598 {
33599 if (j12valid[iij12]
33600 && IKabs(
33601 cj12array[ij12]
33602 - cj12array[iij12])
33603 < IKFAST_SOLUTION_THRESH
33604 && IKabs(
33605 sj12array[ij12]
33606 - sj12array[iij12])
33607 < IKFAST_SOLUTION_THRESH)
33608 {
33609 j12valid[iij12] = false;
33610 _ij12[1] = iij12;
33611 break;
33612 }
33613 }
33614 j12 = j12array[ij12];
33615 cj12 = cj12array[ij12];
33616 sj12 = sj12array[ij12];
33617 {
33618 IkReal evalcond[8];
33619 IkReal x305 = IKsin(j12);
33620 IkReal x306
33621 = ((1.0) * (IKcos(j12)));
33622 IkReal x307 = ((-1.0) * x306);
33623 IkReal x308 = ((1.0) * new_r11);
33624 IkReal x309 = ((1.0) * new_r10);
33625 evalcond[0]
33626 = (((new_r12 * x305))
33627 + new_r21);
33628 evalcond[1]
33629 = ((((-1.0) * x305))
33630 + new_r00);
33631 evalcond[2] = (x307 + new_r01);
33632 evalcond[3]
33633 = ((((-1.0) * new_r12
33634 * x306))
33635 + new_r20);
33636 evalcond[4]
33637 = ((((-1.0) * x308))
33638 + ((new_r22 * x305)));
33639 evalcond[5]
33640 = ((((-1.0) * x309))
33641 + (((-1.0) * new_r22
33642 * x306)));
33643 evalcond[6]
33644 = (x305
33645 + ((new_r12 * new_r21))
33646 + (((-1.0) * new_r22
33647 * x308)));
33648 evalcond[7]
33649 = (x307
33650 + ((new_r12 * new_r20))
33651 + (((-1.0) * new_r22
33652 * x309)));
33653 if (IKabs(evalcond[0])
33654 > IKFAST_EVALCOND_THRESH
33655 || IKabs(evalcond[1])
33656 > IKFAST_EVALCOND_THRESH
33657 || IKabs(evalcond[2])
33658 > IKFAST_EVALCOND_THRESH
33659 || IKabs(evalcond[3])
33660 > IKFAST_EVALCOND_THRESH
33661 || IKabs(evalcond[4])
33662 > IKFAST_EVALCOND_THRESH
33663 || IKabs(evalcond[5])
33664 > IKFAST_EVALCOND_THRESH
33665 || IKabs(evalcond[6])
33666 > IKFAST_EVALCOND_THRESH
33667 || IKabs(evalcond[7])
33668 > IKFAST_EVALCOND_THRESH)
33669 {
33670 continue;
33671 }
33672 }
33673
33674 {
33675 std::vector<
33676 IkSingleDOFSolutionBase<
33677 IkReal> >
33678 vinfos(7);
33679 vinfos[0].jointtype = 1;
33680 vinfos[0].foffset = j4;
33681 vinfos[0].indices[0] = _ij4[0];
33682 vinfos[0].indices[1] = _ij4[1];
33683 vinfos[0].maxsolutions = _nj4;
33684 vinfos[1].jointtype = 1;
33685 vinfos[1].foffset = j6;
33686 vinfos[1].indices[0] = _ij6[0];
33687 vinfos[1].indices[1] = _ij6[1];
33688 vinfos[1].maxsolutions = _nj6;
33689 vinfos[2].jointtype = 1;
33690 vinfos[2].foffset = j8;
33691 vinfos[2].indices[0] = _ij8[0];
33692 vinfos[2].indices[1] = _ij8[1];
33693 vinfos[2].maxsolutions = _nj8;
33694 vinfos[3].jointtype = 1;
33695 vinfos[3].foffset = j9;
33696 vinfos[3].indices[0] = _ij9[0];
33697 vinfos[3].indices[1] = _ij9[1];
33698 vinfos[3].maxsolutions = _nj9;
33699 vinfos[4].jointtype = 1;
33700 vinfos[4].foffset = j10;
33701 vinfos[4].indices[0] = _ij10[0];
33702 vinfos[4].indices[1] = _ij10[1];
33703 vinfos[4].maxsolutions = _nj10;
33704 vinfos[5].jointtype = 1;
33705 vinfos[5].foffset = j11;
33706 vinfos[5].indices[0] = _ij11[0];
33707 vinfos[5].indices[1] = _ij11[1];
33708 vinfos[5].maxsolutions = _nj11;
33709 vinfos[6].jointtype = 1;
33710 vinfos[6].foffset = j12;
33711 vinfos[6].indices[0] = _ij12[0];
33712 vinfos[6].indices[1] = _ij12[1];
33713 vinfos[6].maxsolutions = _nj12;
33714 std::vector<int> vfree(0);
33715 solutions.AddSolution(
33716 vinfos, vfree);
33717 }
33718 }
33719 }
33720 }
33721 } while (0);
33722 if (bgotonextstatement)
33723 {
33724 bool bgotonextstatement = true;
33725 do
33726 {
33727 IkReal x310 = ((1.0) * cj10);
33728 IkReal x311 = ((1.0) * sj10);
33729 IkReal x312
33730 = ((((-1.0) * new_r02 * x311))
33731 + ((cj10 * new_r12)));
33732 evalcond[0]
33733 = ((-3.14159265358979)
33734 + (IKfmod(
33735 ((3.14159265358979)
33736 + (IKabs(
33737 ((-1.5707963267949)
33738 + j11)))),
33739 6.28318530717959)));
33740 evalcond[1] = new_r22;
33741 evalcond[2]
33742 = ((((-1.0) * x310)) + new_r02);
33743 evalcond[3]
33744 = (new_r12 + (((-1.0) * x311)));
33745 evalcond[4] = x312;
33746 evalcond[5] = x312;
33747 evalcond[6]
33748 = ((-1.0) + ((cj10 * new_r02))
33749 + ((new_r12 * sj10)));
33750 evalcond[7]
33751 = (((cj10 * new_r01))
33752 + ((new_r11 * sj10)));
33753 evalcond[8]
33754 = (((cj10 * new_r00))
33755 + ((new_r10 * sj10)));
33756 evalcond[9]
33757 = ((((-1.0) * new_r10 * x311))
33758 + (((-1.0) * new_r00 * x310)));
33759 evalcond[10]
33760 = ((((-1.0) * new_r01 * x310))
33761 + (((-1.0) * new_r11 * x311)));
33762 evalcond[11]
33763 = ((1.0)
33764 + (((-1.0) * new_r12 * x311))
33765 + (((-1.0) * new_r02 * x310)));
33766 if (IKabs(evalcond[0])
33767 < 0.0000010000000000
33768 && IKabs(evalcond[1])
33769 < 0.0000010000000000
33770 && IKabs(evalcond[2])
33771 < 0.0000010000000000
33772 && IKabs(evalcond[3])
33773 < 0.0000010000000000
33774 && IKabs(evalcond[4])
33775 < 0.0000010000000000
33776 && IKabs(evalcond[5])
33777 < 0.0000010000000000
33778 && IKabs(evalcond[6])
33779 < 0.0000010000000000
33780 && IKabs(evalcond[7])
33781 < 0.0000010000000000
33782 && IKabs(evalcond[8])
33783 < 0.0000010000000000
33784 && IKabs(evalcond[9])
33785 < 0.0000010000000000
33786 && IKabs(evalcond[10])
33787 < 0.0000010000000000
33788 && IKabs(evalcond[11])
33789 < 0.0000010000000000)
33790 {
33791 bgotonextstatement = false;
33792 {
33793 IkReal j12array[1], cj12array[1],
33794 sj12array[1];
33795 bool j12valid[1] = {false};
33796 _nj12 = 1;
33797 if (IKabs(new_r21)
33798 < IKFAST_ATAN2_MAGTHRESH
33799 && IKabs((
33800 (-1.0)
33801 * (((1.0) * new_r20))))
33802 < IKFAST_ATAN2_MAGTHRESH
33803 && IKabs(
33804 IKsqr(new_r21)
33805 + IKsqr(
33806 ((-1.0)
33807 * (((1.0)
33808 * new_r20))))
33809 - 1)
33810 <= IKFAST_SINCOS_THRESH)
33811 continue;
33812 j12array[0] = IKatan2(
33813 new_r21,
33814 ((-1.0)
33815 * (((1.0) * new_r20))));
33816 sj12array[0] = IKsin(j12array[0]);
33817 cj12array[0] = IKcos(j12array[0]);
33818 if (j12array[0] > IKPI)
33819 {
33820 j12array[0] -= IK2PI;
33821 }
33822 else if (j12array[0] < -IKPI)
33823 {
33824 j12array[0] += IK2PI;
33825 }
33826 j12valid[0] = true;
33827 for (int ij12 = 0; ij12 < 1;
33828 ++ij12)
33829 {
33830 if (!j12valid[ij12])
33831 {
33832 continue;
33833 }
33834 _ij12[0] = ij12;
33835 _ij12[1] = -1;
33836 for (int iij12 = ij12 + 1;
33837 iij12 < 1;
33838 ++iij12)
33839 {
33840 if (j12valid[iij12]
33841 && IKabs(
33842 cj12array[ij12]
33843 - cj12array[iij12])
33844 < IKFAST_SOLUTION_THRESH
33845 && IKabs(
33846 sj12array[ij12]
33847 - sj12array[iij12])
33848 < IKFAST_SOLUTION_THRESH)
33849 {
33850 j12valid[iij12] = false;
33851 _ij12[1] = iij12;
33852 break;
33853 }
33854 }
33855 j12 = j12array[ij12];
33856 cj12 = cj12array[ij12];
33857 sj12 = sj12array[ij12];
33858 {
33859 IkReal evalcond[8];
33860 IkReal x313 = IKcos(j12);
33861 IkReal x314 = IKsin(j12);
33862 IkReal x315 = ((1.0) * x314);
33863 IkReal x316 = ((-1.0) * x315);
33864 IkReal x317 = ((1.0) * x313);
33865 IkReal x318
33866 = ((1.0) * new_r12);
33867 evalcond[0]
33868 = (x313 + new_r20);
33869 evalcond[1]
33870 = (x316 + new_r21);
33871 evalcond[2]
33872 = (((new_r12 * x313))
33873 + new_r01);
33874 evalcond[3]
33875 = (((new_r12 * x314))
33876 + new_r00);
33877 evalcond[4]
33878 = ((((-1.0) * new_r02
33879 * x317))
33880 + new_r11);
33881 evalcond[5]
33882 = (new_r10
33883 + (((-1.0) * new_r02
33884 * x315)));
33885 evalcond[6]
33886 = ((((-1.0) * new_r00
33887 * x318))
33888 + x316
33889 + ((new_r02
33890 * new_r10)));
33891 evalcond[7]
33892 = (((new_r02 * new_r11))
33893 + (((-1.0) * new_r01
33894 * x318))
33895 + (((-1.0) * x317)));
33896 if (IKabs(evalcond[0])
33897 > IKFAST_EVALCOND_THRESH
33898 || IKabs(evalcond[1])
33899 > IKFAST_EVALCOND_THRESH
33900 || IKabs(evalcond[2])
33901 > IKFAST_EVALCOND_THRESH
33902 || IKabs(evalcond[3])
33903 > IKFAST_EVALCOND_THRESH
33904 || IKabs(evalcond[4])
33905 > IKFAST_EVALCOND_THRESH
33906 || IKabs(evalcond[5])
33907 > IKFAST_EVALCOND_THRESH
33908 || IKabs(evalcond[6])
33909 > IKFAST_EVALCOND_THRESH
33910 || IKabs(evalcond[7])
33911 > IKFAST_EVALCOND_THRESH)
33912 {
33913 continue;
33914 }
33915 }
33916
33917 {
33918 std::vector<
33919 IkSingleDOFSolutionBase<
33920 IkReal> >
33921 vinfos(7);
33922 vinfos[0].jointtype = 1;
33923 vinfos[0].foffset = j4;
33924 vinfos[0].indices[0]
33925 = _ij4[0];
33926 vinfos[0].indices[1]
33927 = _ij4[1];
33928 vinfos[0].maxsolutions = _nj4;
33929 vinfos[1].jointtype = 1;
33930 vinfos[1].foffset = j6;
33931 vinfos[1].indices[0]
33932 = _ij6[0];
33933 vinfos[1].indices[1]
33934 = _ij6[1];
33935 vinfos[1].maxsolutions = _nj6;
33936 vinfos[2].jointtype = 1;
33937 vinfos[2].foffset = j8;
33938 vinfos[2].indices[0]
33939 = _ij8[0];
33940 vinfos[2].indices[1]
33941 = _ij8[1];
33942 vinfos[2].maxsolutions = _nj8;
33943 vinfos[3].jointtype = 1;
33944 vinfos[3].foffset = j9;
33945 vinfos[3].indices[0]
33946 = _ij9[0];
33947 vinfos[3].indices[1]
33948 = _ij9[1];
33949 vinfos[3].maxsolutions = _nj9;
33950 vinfos[4].jointtype = 1;
33951 vinfos[4].foffset = j10;
33952 vinfos[4].indices[0]
33953 = _ij10[0];
33954 vinfos[4].indices[1]
33955 = _ij10[1];
33956 vinfos[4].maxsolutions
33957 = _nj10;
33958 vinfos[5].jointtype = 1;
33959 vinfos[5].foffset = j11;
33960 vinfos[5].indices[0]
33961 = _ij11[0];
33962 vinfos[5].indices[1]
33963 = _ij11[1];
33964 vinfos[5].maxsolutions
33965 = _nj11;
33966 vinfos[6].jointtype = 1;
33967 vinfos[6].foffset = j12;
33968 vinfos[6].indices[0]
33969 = _ij12[0];
33970 vinfos[6].indices[1]
33971 = _ij12[1];
33972 vinfos[6].maxsolutions
33973 = _nj12;
33974 std::vector<int> vfree(0);
33975 solutions.AddSolution(
33976 vinfos, vfree);
33977 }
33978 }
33979 }
33980 }
33981 } while (0);
33982 if (bgotonextstatement)
33983 {
33984 bool bgotonextstatement = true;
33985 do
33986 {
33987 IkReal x319
33988 = ((((-1.0) * (1.0) * new_r02
33989 * sj10))
33990 + ((cj10 * new_r12)));
33991 IkReal x320
33992 = ((1.0) + ((cj10 * new_r02))
33993 + ((new_r12 * sj10)));
33994 IkReal x321
33995 = (((cj10 * new_r01))
33996 + ((new_r11 * sj10)));
33997 IkReal x322
33998 = (((cj10 * new_r00))
33999 + ((new_r10 * sj10)));
34000 evalcond[0]
34001 = ((-3.14159265358979)
34002 + (IKfmod(
34003 ((3.14159265358979)
34004 + (IKabs(
34005 ((1.5707963267949)
34006 + j11)))),
34007 6.28318530717959)));
34008 evalcond[1] = new_r22;
34009 evalcond[2] = (cj10 + new_r02);
34010 evalcond[3] = (sj10 + new_r12);
34011 evalcond[4] = x319;
34012 evalcond[5] = x319;
34013 evalcond[6] = x320;
34014 evalcond[7] = x321;
34015 evalcond[8] = x322;
34016 evalcond[9] = x322;
34017 evalcond[10] = x321;
34018 evalcond[11] = x320;
34019 if (IKabs(evalcond[0])
34020 < 0.0000010000000000
34021 && IKabs(evalcond[1])
34022 < 0.0000010000000000
34023 && IKabs(evalcond[2])
34024 < 0.0000010000000000
34025 && IKabs(evalcond[3])
34026 < 0.0000010000000000
34027 && IKabs(evalcond[4])
34028 < 0.0000010000000000
34029 && IKabs(evalcond[5])
34030 < 0.0000010000000000
34031 && IKabs(evalcond[6])
34032 < 0.0000010000000000
34033 && IKabs(evalcond[7])
34034 < 0.0000010000000000
34035 && IKabs(evalcond[8])
34036 < 0.0000010000000000
34037 && IKabs(evalcond[9])
34038 < 0.0000010000000000
34039 && IKabs(evalcond[10])
34040 < 0.0000010000000000
34041 && IKabs(evalcond[11])
34042 < 0.0000010000000000)
34043 {
34044 bgotonextstatement = false;
34045 {
34046 IkReal j12array[1],
34047 cj12array[1], sj12array[1];
34048 bool j12valid[1] = {false};
34049 _nj12 = 1;
34050 if (IKabs(
34051 ((-1.0)
34052 * (((1.0) * new_r21))))
34053 < IKFAST_ATAN2_MAGTHRESH
34054 && IKabs(new_r20)
34055 < IKFAST_ATAN2_MAGTHRESH
34056 && IKabs(
34057 IKsqr((
34058 (-1.0)
34059 * (((1.0)
34060 * new_r21))))
34061 + IKsqr(new_r20) - 1)
34062 <= IKFAST_SINCOS_THRESH)
34063 continue;
34064 j12array[0] = IKatan2(
34065 ((-1.0)
34066 * (((1.0) * new_r21))),
34067 new_r20);
34068 sj12array[0]
34069 = IKsin(j12array[0]);
34070 cj12array[0]
34071 = IKcos(j12array[0]);
34072 if (j12array[0] > IKPI)
34073 {
34074 j12array[0] -= IK2PI;
34075 }
34076 else if (j12array[0] < -IKPI)
34077 {
34078 j12array[0] += IK2PI;
34079 }
34080 j12valid[0] = true;
34081 for (int ij12 = 0; ij12 < 1;
34082 ++ij12)
34083 {
34084 if (!j12valid[ij12])
34085 {
34086 continue;
34087 }
34088 _ij12[0] = ij12;
34089 _ij12[1] = -1;
34090 for (int iij12 = ij12 + 1;
34091 iij12 < 1;
34092 ++iij12)
34093 {
34094 if (j12valid[iij12]
34095 && IKabs(
34096 cj12array[ij12]
34097 - cj12array
34098 [iij12])
34099 < IKFAST_SOLUTION_THRESH
34100 && IKabs(
34101 sj12array[ij12]
34102 - sj12array
34103 [iij12])
34104 < IKFAST_SOLUTION_THRESH)
34105 {
34106 j12valid[iij12] = false;
34107 _ij12[1] = iij12;
34108 break;
34109 }
34110 }
34111 j12 = j12array[ij12];
34112 cj12 = cj12array[ij12];
34113 sj12 = sj12array[ij12];
34114 {
34115 IkReal evalcond[8];
34116 IkReal x323 = IKsin(j12);
34117 IkReal x324 = IKcos(j12);
34118 IkReal x325
34119 = ((1.0) * x324);
34120 IkReal x326
34121 = ((-1.0) * x325);
34122 IkReal x327
34123 = ((1.0) * x323);
34124 IkReal x328
34125 = ((1.0) * new_r02);
34126 evalcond[0]
34127 = (x323 + new_r21);
34128 evalcond[1]
34129 = (x326 + new_r20);
34130 evalcond[2]
34131 = (((new_r02 * x324))
34132 + new_r11);
34133 evalcond[3]
34134 = (((new_r02 * x323))
34135 + new_r10);
34136 evalcond[4]
34137 = ((((-1.0) * new_r12
34138 * x325))
34139 + new_r01);
34140 evalcond[5]
34141 = ((((-1.0) * new_r12
34142 * x327))
34143 + new_r00);
34144 evalcond[6]
34145 = (((new_r00 * new_r12))
34146 + (((-1.0) * x327))
34147 + (((-1.0) * new_r10
34148 * x328)));
34149 evalcond[7]
34150 = (((new_r01 * new_r12))
34151 + x326
34152 + (((-1.0) * new_r11
34153 * x328)));
34154 if (IKabs(evalcond[0])
34155 > IKFAST_EVALCOND_THRESH
34156 || IKabs(evalcond[1])
34157 > IKFAST_EVALCOND_THRESH
34158 || IKabs(evalcond[2])
34159 > IKFAST_EVALCOND_THRESH
34160 || IKabs(evalcond[3])
34161 > IKFAST_EVALCOND_THRESH
34162 || IKabs(evalcond[4])
34163 > IKFAST_EVALCOND_THRESH
34164 || IKabs(evalcond[5])
34165 > IKFAST_EVALCOND_THRESH
34166 || IKabs(evalcond[6])
34167 > IKFAST_EVALCOND_THRESH
34168 || IKabs(evalcond[7])
34169 > IKFAST_EVALCOND_THRESH)
34170 {
34171 continue;
34172 }
34173 }
34174
34175 {
34176 std::vector<
34177 IkSingleDOFSolutionBase<
34178 IkReal> >
34179 vinfos(7);
34180 vinfos[0].jointtype = 1;
34181 vinfos[0].foffset = j4;
34182 vinfos[0].indices[0]
34183 = _ij4[0];
34184 vinfos[0].indices[1]
34185 = _ij4[1];
34186 vinfos[0].maxsolutions
34187 = _nj4;
34188 vinfos[1].jointtype = 1;
34189 vinfos[1].foffset = j6;
34190 vinfos[1].indices[0]
34191 = _ij6[0];
34192 vinfos[1].indices[1]
34193 = _ij6[1];
34194 vinfos[1].maxsolutions
34195 = _nj6;
34196 vinfos[2].jointtype = 1;
34197 vinfos[2].foffset = j8;
34198 vinfos[2].indices[0]
34199 = _ij8[0];
34200 vinfos[2].indices[1]
34201 = _ij8[1];
34202 vinfos[2].maxsolutions
34203 = _nj8;
34204 vinfos[3].jointtype = 1;
34205 vinfos[3].foffset = j9;
34206 vinfos[3].indices[0]
34207 = _ij9[0];
34208 vinfos[3].indices[1]
34209 = _ij9[1];
34210 vinfos[3].maxsolutions
34211 = _nj9;
34212 vinfos[4].jointtype = 1;
34213 vinfos[4].foffset = j10;
34214 vinfos[4].indices[0]
34215 = _ij10[0];
34216 vinfos[4].indices[1]
34217 = _ij10[1];
34218 vinfos[4].maxsolutions
34219 = _nj10;
34220 vinfos[5].jointtype = 1;
34221 vinfos[5].foffset = j11;
34222 vinfos[5].indices[0]
34223 = _ij11[0];
34224 vinfos[5].indices[1]
34225 = _ij11[1];
34226 vinfos[5].maxsolutions
34227 = _nj11;
34228 vinfos[6].jointtype = 1;
34229 vinfos[6].foffset = j12;
34230 vinfos[6].indices[0]
34231 = _ij12[0];
34232 vinfos[6].indices[1]
34233 = _ij12[1];
34234 vinfos[6].maxsolutions
34235 = _nj12;
34236 std::vector<int> vfree(0);
34237 solutions.AddSolution(
34238 vinfos, vfree);
34239 }
34240 }
34241 }
34242 }
34243 } while (0);
34244 if (bgotonextstatement)
34245 {
34246 bool bgotonextstatement = true;
34247 do
34248 {
34249 IkReal x329
34250 = ((((-1.0) * (1.0) * new_r02
34251 * sj10))
34252 + ((cj10 * new_r12)));
34253 IkReal x330
34254 = (((cj10 * new_r02))
34255 + ((new_r12 * sj10)));
34256 evalcond[0]
34257 = ((-3.14159265358979)
34258 + (IKfmod(
34259 ((3.14159265358979)
34260 + (IKabs(j11))),
34261 6.28318530717959)));
34262 evalcond[1] = ((-1.0) + new_r22);
34263 evalcond[2] = new_r20;
34264 evalcond[3] = new_r02;
34265 evalcond[4] = new_r12;
34266 evalcond[5] = new_r21;
34267 evalcond[6] = x329;
34268 evalcond[7] = x329;
34269 evalcond[8] = x330;
34270 evalcond[9] = x330;
34271 if (IKabs(evalcond[0])
34272 < 0.0000010000000000
34273 && IKabs(evalcond[1])
34274 < 0.0000010000000000
34275 && IKabs(evalcond[2])
34276 < 0.0000010000000000
34277 && IKabs(evalcond[3])
34278 < 0.0000010000000000
34279 && IKabs(evalcond[4])
34280 < 0.0000010000000000
34281 && IKabs(evalcond[5])
34282 < 0.0000010000000000
34283 && IKabs(evalcond[6])
34284 < 0.0000010000000000
34285 && IKabs(evalcond[7])
34286 < 0.0000010000000000
34287 && IKabs(evalcond[8])
34288 < 0.0000010000000000
34289 && IKabs(evalcond[9])
34290 < 0.0000010000000000)
34291 {
34292 bgotonextstatement = false;
34293 {
34294 IkReal j12array[1],
34295 cj12array[1],
34296 sj12array[1];
34297 bool j12valid[1] = {false};
34298 _nj12 = 1;
34299 IkReal x331
34300 = ((1.0) * new_r01);
34301 if (IKabs(
34302 ((((-1.0) * cj10
34303 * x331))
34304 + (((-1.0) * (1.0)
34305 * new_r00
34306 * sj10))))
34307 < IKFAST_ATAN2_MAGTHRESH
34308 && IKabs(
34309 (((cj10 * new_r00))
34310 + (((-1.0) * sj10
34311 * x331))))
34312 < IKFAST_ATAN2_MAGTHRESH
34313 && IKabs(
34314 IKsqr((
34315 (((-1.0) * cj10
34316 * x331))
34317 + (((-1.0)
34318 * (1.0)
34319 * new_r00
34320 * sj10))))
34321 + IKsqr(
34322 (((cj10
34323 * new_r00))
34324 + (((-1.0)
34325 * sj10
34326 * x331))))
34327 - 1)
34328 <= IKFAST_SINCOS_THRESH)
34329 continue;
34330 j12array[0] = IKatan2(
34331 ((((-1.0) * cj10 * x331))
34332 + (((-1.0) * (1.0)
34333 * new_r00 * sj10))),
34334 (((cj10 * new_r00))
34335 + (((-1.0) * sj10
34336 * x331))));
34337 sj12array[0]
34338 = IKsin(j12array[0]);
34339 cj12array[0]
34340 = IKcos(j12array[0]);
34341 if (j12array[0] > IKPI)
34342 {
34343 j12array[0] -= IK2PI;
34344 }
34345 else if (j12array[0] < -IKPI)
34346 {
34347 j12array[0] += IK2PI;
34348 }
34349 j12valid[0] = true;
34350 for (int ij12 = 0; ij12 < 1;
34351 ++ij12)
34352 {
34353 if (!j12valid[ij12])
34354 {
34355 continue;
34356 }
34357 _ij12[0] = ij12;
34358 _ij12[1] = -1;
34359 for (int iij12 = ij12 + 1;
34360 iij12 < 1;
34361 ++iij12)
34362 {
34363 if (j12valid[iij12]
34364 && IKabs(
34365 cj12array[ij12]
34366 - cj12array
34367 [iij12])
34368 < IKFAST_SOLUTION_THRESH
34369 && IKabs(
34370 sj12array[ij12]
34371 - sj12array
34372 [iij12])
34373 < IKFAST_SOLUTION_THRESH)
34374 {
34375 j12valid[iij12] = false;
34376 _ij12[1] = iij12;
34377 break;
34378 }
34379 }
34380 j12 = j12array[ij12];
34381 cj12 = cj12array[ij12];
34382 sj12 = sj12array[ij12];
34383 {
34384 IkReal evalcond[8];
34385 IkReal x332 = IKsin(j12);
34386 IkReal x333
34387 = (cj10 * x332);
34388 IkReal x334 = IKcos(j12);
34389 IkReal x335
34390 = ((1.0) * x334);
34391 IkReal x336
34392 = ((-1.0) * x335);
34393 IkReal x337
34394 = ((1.0) * sj10);
34395 IkReal x338
34396 = (((sj10 * x332))
34397 + (((-1.0) * cj10
34398 * x335)));
34399 evalcond[0]
34400 = (((cj10 * new_r01))
34401 + x332
34402 + ((new_r11
34403 * sj10)));
34404 evalcond[1]
34405 = (((sj10 * x334))
34406 + x333 + new_r01);
34407 evalcond[2]
34408 = (((cj10 * new_r00))
34409 + ((new_r10
34410 * sj10))
34411 + x336);
34412 evalcond[3]
34413 = (((cj10 * new_r10))
34414 + (((-1.0) * x332))
34415 + (((-1.0)
34416 * new_r00
34417 * x337)));
34418 evalcond[4]
34419 = (((cj10 * new_r11))
34420 + x336
34421 + (((-1.0)
34422 * new_r01
34423 * x337)));
34424 evalcond[5]
34425 = (x338 + new_r00);
34426 evalcond[6]
34427 = (x338 + new_r11);
34428 evalcond[7]
34429 = ((((-1.0) * x334
34430 * x337))
34431 + (((-1.0) * x333))
34432 + new_r10);
34433 if (IKabs(evalcond[0])
34434 > IKFAST_EVALCOND_THRESH
34435 || IKabs(evalcond[1])
34436 > IKFAST_EVALCOND_THRESH
34437 || IKabs(evalcond[2])
34438 > IKFAST_EVALCOND_THRESH
34439 || IKabs(evalcond[3])
34440 > IKFAST_EVALCOND_THRESH
34441 || IKabs(evalcond[4])
34442 > IKFAST_EVALCOND_THRESH
34443 || IKabs(evalcond[5])
34444 > IKFAST_EVALCOND_THRESH
34445 || IKabs(evalcond[6])
34446 > IKFAST_EVALCOND_THRESH
34447 || IKabs(evalcond[7])
34448 > IKFAST_EVALCOND_THRESH)
34449 {
34450 continue;
34451 }
34452 }
34453
34454 {
34455 std::vector<
34456 IkSingleDOFSolutionBase<
34457 IkReal> >
34458 vinfos(7);
34459 vinfos[0].jointtype = 1;
34460 vinfos[0].foffset = j4;
34461 vinfos[0].indices[0]
34462 = _ij4[0];
34463 vinfos[0].indices[1]
34464 = _ij4[1];
34465 vinfos[0].maxsolutions
34466 = _nj4;
34467 vinfos[1].jointtype = 1;
34468 vinfos[1].foffset = j6;
34469 vinfos[1].indices[0]
34470 = _ij6[0];
34471 vinfos[1].indices[1]
34472 = _ij6[1];
34473 vinfos[1].maxsolutions
34474 = _nj6;
34475 vinfos[2].jointtype = 1;
34476 vinfos[2].foffset = j8;
34477 vinfos[2].indices[0]
34478 = _ij8[0];
34479 vinfos[2].indices[1]
34480 = _ij8[1];
34481 vinfos[2].maxsolutions
34482 = _nj8;
34483 vinfos[3].jointtype = 1;
34484 vinfos[3].foffset = j9;
34485 vinfos[3].indices[0]
34486 = _ij9[0];
34487 vinfos[3].indices[1]
34488 = _ij9[1];
34489 vinfos[3].maxsolutions
34490 = _nj9;
34491 vinfos[4].jointtype = 1;
34492 vinfos[4].foffset = j10;
34493 vinfos[4].indices[0]
34494 = _ij10[0];
34495 vinfos[4].indices[1]
34496 = _ij10[1];
34497 vinfos[4].maxsolutions
34498 = _nj10;
34499 vinfos[5].jointtype = 1;
34500 vinfos[5].foffset = j11;
34501 vinfos[5].indices[0]
34502 = _ij11[0];
34503 vinfos[5].indices[1]
34504 = _ij11[1];
34505 vinfos[5].maxsolutions
34506 = _nj11;
34507 vinfos[6].jointtype = 1;
34508 vinfos[6].foffset = j12;
34509 vinfos[6].indices[0]
34510 = _ij12[0];
34511 vinfos[6].indices[1]
34512 = _ij12[1];
34513 vinfos[6].maxsolutions
34514 = _nj12;
34515 std::vector<int> vfree(0);
34516 solutions.AddSolution(
34517 vinfos, vfree);
34518 }
34519 }
34520 }
34521 }
34522 } while (0);
34523 if (bgotonextstatement)
34524 {
34525 bool bgotonextstatement = true;
34526 do
34527 {
34528 IkReal x339
34529 = ((((-1.0) * (1.0)
34530 * new_r02 * sj10))
34531 + ((cj10 * new_r12)));
34532 IkReal x340 = (cj10 * new_r02);
34533 IkReal x341 = (new_r12 * sj10);
34534 evalcond[0]
34535 = ((-3.14159265358979)
34536 + (IKfmod(
34537 ((3.14159265358979)
34538 + (IKabs((
34539 (-3.14159265358979)
34540 + j11)))),
34541 6.28318530717959)));
34542 evalcond[1] = ((1.0) + new_r22);
34543 evalcond[2] = new_r20;
34544 evalcond[3] = new_r02;
34545 evalcond[4] = new_r12;
34546 evalcond[5] = new_r21;
34547 evalcond[6] = x339;
34548 evalcond[7] = x339;
34549 evalcond[8] = (x340 + x341);
34550 evalcond[9]
34551 = ((((-1.0) * x341))
34552 + (((-1.0) * x340)));
34553 if (IKabs(evalcond[0])
34554 < 0.0000010000000000
34555 && IKabs(evalcond[1])
34556 < 0.0000010000000000
34557 && IKabs(evalcond[2])
34558 < 0.0000010000000000
34559 && IKabs(evalcond[3])
34560 < 0.0000010000000000
34561 && IKabs(evalcond[4])
34562 < 0.0000010000000000
34563 && IKabs(evalcond[5])
34564 < 0.0000010000000000
34565 && IKabs(evalcond[6])
34566 < 0.0000010000000000
34567 && IKabs(evalcond[7])
34568 < 0.0000010000000000
34569 && IKabs(evalcond[8])
34570 < 0.0000010000000000
34571 && IKabs(evalcond[9])
34572 < 0.0000010000000000)
34573 {
34574 bgotonextstatement = false;
34575 {
34576 IkReal j12array[1],
34577 cj12array[1],
34578 sj12array[1];
34579 bool j12valid[1] = {false};
34580 _nj12 = 1;
34581 IkReal x342
34582 = ((1.0) * new_r00);
34583 if (IKabs(
34584 (((cj10 * new_r01))
34585 + (((-1.0) * sj10
34586 * x342))))
34587 < IKFAST_ATAN2_MAGTHRESH
34588 && IKabs(
34589 ((((-1.0) * cj10
34590 * x342))
34591 + (((-1.0)
34592 * (1.0)
34593 * new_r01
34594 * sj10))))
34595 < IKFAST_ATAN2_MAGTHRESH
34596 && IKabs(
34597 IKsqr((
34598 ((cj10
34599 * new_r01))
34600 + (((-1.0)
34601 * sj10
34602 * x342))))
34603 + IKsqr((
34604 (((-1.0)
34605 * cj10
34606 * x342))
34607 + (((-1.0)
34608 * (1.0)
34609 * new_r01
34610 * sj10))))
34611 - 1)
34612 <= IKFAST_SINCOS_THRESH)
34613 continue;
34614 j12array[0] = IKatan2(
34615 (((cj10 * new_r01))
34616 + (((-1.0) * sj10
34617 * x342))),
34618 ((((-1.0) * cj10
34619 * x342))
34620 + (((-1.0) * (1.0)
34621 * new_r01
34622 * sj10))));
34623 sj12array[0]
34624 = IKsin(j12array[0]);
34625 cj12array[0]
34626 = IKcos(j12array[0]);
34627 if (j12array[0] > IKPI)
34628 {
34629 j12array[0] -= IK2PI;
34630 }
34631 else if (
34632 j12array[0] < -IKPI)
34633 {
34634 j12array[0] += IK2PI;
34635 }
34636 j12valid[0] = true;
34637 for (int ij12 = 0; ij12 < 1;
34638 ++ij12)
34639 {
34640 if (!j12valid[ij12])
34641 {
34642 continue;
34643 }
34644 _ij12[0] = ij12;
34645 _ij12[1] = -1;
34646 for (int iij12 = ij12 + 1;
34647 iij12 < 1;
34648 ++iij12)
34649 {
34650 if (j12valid[iij12]
34651 && IKabs(
34652 cj12array
34653 [ij12]
34654 - cj12array
34655 [iij12])
34656 < IKFAST_SOLUTION_THRESH
34657 && IKabs(
34658 sj12array
34659 [ij12]
34660 - sj12array
34661 [iij12])
34662 < IKFAST_SOLUTION_THRESH)
34663 {
34664 j12valid[iij12]
34665 = false;
34666 _ij12[1] = iij12;
34667 break;
34668 }
34669 }
34670 j12 = j12array[ij12];
34671 cj12 = cj12array[ij12];
34672 sj12 = sj12array[ij12];
34673 {
34674 IkReal evalcond[8];
34675 IkReal x343
34676 = IKcos(j12);
34677 IkReal x344
34678 = IKsin(j12);
34679 IkReal x345
34680 = ((1.0) * x344);
34681 IkReal x346
34682 = ((-1.0) * x345);
34683 IkReal x347
34684 = (cj10 * x343);
34685 IkReal x348
34686 = ((1.0) * sj10);
34687 IkReal x349
34688 = (((sj10 * x343))
34689 + (((-1.0) * cj10
34690 * x345)));
34691 evalcond[0]
34692 = (((cj10
34693 * new_r00))
34694 + ((new_r10
34695 * sj10))
34696 + x343);
34697 evalcond[1]
34698 = (((cj10
34699 * new_r01))
34700 + ((new_r11
34701 * sj10))
34702 + x346);
34703 evalcond[2]
34704 = (((sj10 * x344))
34705 + x347
34706 + new_r00);
34707 evalcond[3]
34708 = (((cj10
34709 * new_r10))
34710 + (((-1.0)
34711 * new_r00
34712 * x348))
34713 + x346);
34714 evalcond[4]
34715 = (((cj10
34716 * new_r11))
34717 + (((-1.0)
34718 * new_r01
34719 * x348))
34720 + (((-1.0)
34721 * x343)));
34722 evalcond[5]
34723 = (x349 + new_r01);
34724 evalcond[6]
34725 = (new_r10 + x349);
34726 evalcond[7]
34727 = ((((-1.0) * x347))
34728 + (((-1.0) * x344
34729 * x348))
34730 + new_r11);
34731 if (IKabs(evalcond[0])
34732 > IKFAST_EVALCOND_THRESH
34733 || IKabs(
34734 evalcond[1])
34735 > IKFAST_EVALCOND_THRESH
34736 || IKabs(
34737 evalcond[2])
34738 > IKFAST_EVALCOND_THRESH
34739 || IKabs(
34740 evalcond[3])
34741 > IKFAST_EVALCOND_THRESH
34742 || IKabs(
34743 evalcond[4])
34744 > IKFAST_EVALCOND_THRESH
34745 || IKabs(
34746 evalcond[5])
34747 > IKFAST_EVALCOND_THRESH
34748 || IKabs(
34749 evalcond[6])
34750 > IKFAST_EVALCOND_THRESH
34751 || IKabs(
34752 evalcond[7])
34753 > IKFAST_EVALCOND_THRESH)
34754 {
34755 continue;
34756 }
34757 }
34758
34759 {
34760 std::vector<
34761 IkSingleDOFSolutionBase<
34762 IkReal> >
34763 vinfos(7);
34764 vinfos[0].jointtype = 1;
34765 vinfos[0].foffset = j4;
34766 vinfos[0].indices[0]
34767 = _ij4[0];
34768 vinfos[0].indices[1]
34769 = _ij4[1];
34770 vinfos[0].maxsolutions
34771 = _nj4;
34772 vinfos[1].jointtype = 1;
34773 vinfos[1].foffset = j6;
34774 vinfos[1].indices[0]
34775 = _ij6[0];
34776 vinfos[1].indices[1]
34777 = _ij6[1];
34778 vinfos[1].maxsolutions
34779 = _nj6;
34780 vinfos[2].jointtype = 1;
34781 vinfos[2].foffset = j8;
34782 vinfos[2].indices[0]
34783 = _ij8[0];
34784 vinfos[2].indices[1]
34785 = _ij8[1];
34786 vinfos[2].maxsolutions
34787 = _nj8;
34788 vinfos[3].jointtype = 1;
34789 vinfos[3].foffset = j9;
34790 vinfos[3].indices[0]
34791 = _ij9[0];
34792 vinfos[3].indices[1]
34793 = _ij9[1];
34794 vinfos[3].maxsolutions
34795 = _nj9;
34796 vinfos[4].jointtype = 1;
34797 vinfos[4].foffset = j10;
34798 vinfos[4].indices[0]
34799 = _ij10[0];
34800 vinfos[4].indices[1]
34801 = _ij10[1];
34802 vinfos[4].maxsolutions
34803 = _nj10;
34804 vinfos[5].jointtype = 1;
34805 vinfos[5].foffset = j11;
34806 vinfos[5].indices[0]
34807 = _ij11[0];
34808 vinfos[5].indices[1]
34809 = _ij11[1];
34810 vinfos[5].maxsolutions
34811 = _nj11;
34812 vinfos[6].jointtype = 1;
34813 vinfos[6].foffset = j12;
34814 vinfos[6].indices[0]
34815 = _ij12[0];
34816 vinfos[6].indices[1]
34817 = _ij12[1];
34818 vinfos[6].maxsolutions
34819 = _nj12;
34820 std::vector<int> vfree(
34821 0);
34822 solutions.AddSolution(
34823 vinfos, vfree);
34824 }
34825 }
34826 }
34827 }
34828 } while (0);
34829 if (bgotonextstatement)
34830 {
34831 bool bgotonextstatement = true;
34832 do
34833 {
34834 IkReal x350 = ((1.0) * cj11);
34835 IkReal x351
34836 = ((((-1.0) * x350))
34837 + new_r22);
34838 IkReal x352 = ((1.0) * sj11);
34839 IkReal x353
34840 = ((((-1.0) * x352))
34841 + new_r02);
34842 evalcond[0]
34843 = ((-3.14159265358979)
34844 + (IKfmod(
34845 ((3.14159265358979)
34846 + (IKabs(j10))),
34847 6.28318530717959)));
34848 evalcond[1] = x351;
34849 evalcond[2] = x351;
34850 evalcond[3] = x353;
34851 evalcond[4] = new_r12;
34852 evalcond[5] = x353;
34853 evalcond[6]
34854 = ((((-1.0) * new_r22
34855 * x352))
34856 + ((cj11 * new_r02)));
34857 evalcond[7]
34858 = ((((-1.0) * new_r00
34859 * x352))
34860 + (((-1.0) * new_r20
34861 * x350)));
34862 evalcond[8]
34863 = ((((-1.0) * new_r01
34864 * x352))
34865 + (((-1.0) * new_r21
34866 * x350)));
34867 evalcond[9]
34868 = ((1.0)
34869 + (((-1.0) * new_r02
34870 * x352))
34871 + (((-1.0) * new_r22
34872 * x350)));
34873 if (IKabs(evalcond[0])
34874 < 0.0000010000000000
34875 && IKabs(evalcond[1])
34876 < 0.0000010000000000
34877 && IKabs(evalcond[2])
34878 < 0.0000010000000000
34879 && IKabs(evalcond[3])
34880 < 0.0000010000000000
34881 && IKabs(evalcond[4])
34882 < 0.0000010000000000
34883 && IKabs(evalcond[5])
34884 < 0.0000010000000000
34885 && IKabs(evalcond[6])
34886 < 0.0000010000000000
34887 && IKabs(evalcond[7])
34888 < 0.0000010000000000
34889 && IKabs(evalcond[8])
34890 < 0.0000010000000000
34891 && IKabs(evalcond[9])
34892 < 0.0000010000000000)
34893 {
34894 bgotonextstatement = false;
34895 {
34896 IkReal j12array[1],
34897 cj12array[1],
34898 sj12array[1];
34899 bool j12valid[1]
34900 = {false};
34901 _nj12 = 1;
34902 if (IKabs(new_r10)
34903 < IKFAST_ATAN2_MAGTHRESH
34904 && IKabs(new_r11)
34905 < IKFAST_ATAN2_MAGTHRESH
34906 && IKabs(
34907 IKsqr(new_r10)
34908 + IKsqr(
34909 new_r11)
34910 - 1)
34911 <= IKFAST_SINCOS_THRESH)
34912 continue;
34913 j12array[0] = IKatan2(
34914 new_r10, new_r11);
34915 sj12array[0]
34916 = IKsin(j12array[0]);
34917 cj12array[0]
34918 = IKcos(j12array[0]);
34919 if (j12array[0] > IKPI)
34920 {
34921 j12array[0] -= IK2PI;
34922 }
34923 else if (
34924 j12array[0] < -IKPI)
34925 {
34926 j12array[0] += IK2PI;
34927 }
34928 j12valid[0] = true;
34929 for (int ij12 = 0;
34930 ij12 < 1;
34931 ++ij12)
34932 {
34933 if (!j12valid[ij12])
34934 {
34935 continue;
34936 }
34937 _ij12[0] = ij12;
34938 _ij12[1] = -1;
34939 for (int iij12
34940 = ij12 + 1;
34941 iij12 < 1;
34942 ++iij12)
34943 {
34944 if (j12valid[iij12]
34945 && IKabs(
34946 cj12array
34947 [ij12]
34948 - cj12array
34949 [iij12])
34950 < IKFAST_SOLUTION_THRESH
34951 && IKabs(
34952 sj12array
34953 [ij12]
34954 - sj12array
34955 [iij12])
34956 < IKFAST_SOLUTION_THRESH)
34957 {
34958 j12valid[iij12]
34959 = false;
34960 _ij12[1] = iij12;
34961 break;
34962 }
34963 }
34964 j12 = j12array[ij12];
34965 cj12 = cj12array[ij12];
34966 sj12 = sj12array[ij12];
34967 {
34968 IkReal evalcond[8];
34969 IkReal x354
34970 = IKcos(j12);
34971 IkReal x355
34972 = IKsin(j12);
34973 IkReal x356
34974 = ((1.0) * x354);
34975 IkReal x357
34976 = ((-1.0) * x356);
34977 IkReal x358
34978 = ((1.0)
34979 * new_r02);
34980 evalcond[0]
34981 = (((new_r02
34982 * x354))
34983 + new_r20);
34984 evalcond[1]
34985 = ((((-1.0)
34986 * x355))
34987 + new_r10);
34988 evalcond[2]
34989 = (x357
34990 + new_r11);
34991 evalcond[3]
34992 = (((new_r22
34993 * x355))
34994 + new_r01);
34995 evalcond[4]
34996 = ((((-1.0) * x355
34997 * x358))
34998 + new_r21);
34999 evalcond[5]
35000 = ((((-1.0)
35001 * new_r22
35002 * x356))
35003 + new_r00);
35004 evalcond[6]
35005 = (x355
35006 + ((new_r01
35007 * new_r22))
35008 + (((-1.0)
35009 * new_r21
35010 * x358)));
35011 evalcond[7]
35012 = (x357
35013 + ((new_r00
35014 * new_r22))
35015 + (((-1.0)
35016 * new_r20
35017 * x358)));
35018 if (IKabs(evalcond[0])
35019 > IKFAST_EVALCOND_THRESH
35020 || IKabs(evalcond
35021 [1])
35022 > IKFAST_EVALCOND_THRESH
35023 || IKabs(evalcond
35024 [2])
35025 > IKFAST_EVALCOND_THRESH
35026 || IKabs(evalcond
35027 [3])
35028 > IKFAST_EVALCOND_THRESH
35029 || IKabs(evalcond
35030 [4])
35031 > IKFAST_EVALCOND_THRESH
35032 || IKabs(evalcond
35033 [5])
35034 > IKFAST_EVALCOND_THRESH
35035 || IKabs(evalcond
35036 [6])
35037 > IKFAST_EVALCOND_THRESH
35038 || IKabs(evalcond
35039 [7])
35040 > IKFAST_EVALCOND_THRESH)
35041 {
35042 continue;
35043 }
35044 }
35045
35046 {
35047 std::vector<
35048 IkSingleDOFSolutionBase<
35049 IkReal> >
35050 vinfos(7);
35051 vinfos[0].jointtype
35052 = 1;
35053 vinfos[0].foffset
35054 = j4;
35055 vinfos[0].indices[0]
35056 = _ij4[0];
35057 vinfos[0].indices[1]
35058 = _ij4[1];
35059 vinfos[0].maxsolutions
35060 = _nj4;
35061 vinfos[1].jointtype
35062 = 1;
35063 vinfos[1].foffset
35064 = j6;
35065 vinfos[1].indices[0]
35066 = _ij6[0];
35067 vinfos[1].indices[1]
35068 = _ij6[1];
35069 vinfos[1].maxsolutions
35070 = _nj6;
35071 vinfos[2].jointtype
35072 = 1;
35073 vinfos[2].foffset
35074 = j8;
35075 vinfos[2].indices[0]
35076 = _ij8[0];
35077 vinfos[2].indices[1]
35078 = _ij8[1];
35079 vinfos[2].maxsolutions
35080 = _nj8;
35081 vinfos[3].jointtype
35082 = 1;
35083 vinfos[3].foffset
35084 = j9;
35085 vinfos[3].indices[0]
35086 = _ij9[0];
35087 vinfos[3].indices[1]
35088 = _ij9[1];
35089 vinfos[3].maxsolutions
35090 = _nj9;
35091 vinfos[4].jointtype
35092 = 1;
35093 vinfos[4].foffset
35094 = j10;
35095 vinfos[4].indices[0]
35096 = _ij10[0];
35097 vinfos[4].indices[1]
35098 = _ij10[1];
35099 vinfos[4].maxsolutions
35100 = _nj10;
35101 vinfos[5].jointtype
35102 = 1;
35103 vinfos[5].foffset
35104 = j11;
35105 vinfos[5].indices[0]
35106 = _ij11[0];
35107 vinfos[5].indices[1]
35108 = _ij11[1];
35109 vinfos[5].maxsolutions
35110 = _nj11;
35111 vinfos[6].jointtype
35112 = 1;
35113 vinfos[6].foffset
35114 = j12;
35115 vinfos[6].indices[0]
35116 = _ij12[0];
35117 vinfos[6].indices[1]
35118 = _ij12[1];
35119 vinfos[6].maxsolutions
35120 = _nj12;
35121 std::vector<int>
35122 vfree(0);
35123 solutions.AddSolution(
35124 vinfos, vfree);
35125 }
35126 }
35127 }
35128 }
35129 } while (0);
35130 if (bgotonextstatement)
35131 {
35132 bool bgotonextstatement
35133 = true;
35134 do
35135 {
35136 IkReal x359
35137 = ((1.0) * cj11);
35138 IkReal x360
35139 = ((((-1.0) * x359))
35140 + new_r22);
35141 IkReal x361
35142 = ((1.0) * sj11);
35143 evalcond[0]
35144 = ((-3.14159265358979)
35145 + (IKfmod(
35146 ((3.14159265358979)
35147 + (IKabs((
35148 (-3.14159265358979)
35149 + j10)))),
35150 6.28318530717959)));
35151 evalcond[1] = x360;
35152 evalcond[2] = x360;
35153 evalcond[3]
35154 = (sj11 + new_r02);
35155 evalcond[4] = new_r12;
35156 evalcond[5]
35157 = ((((-1.0) * (1.0)
35158 * new_r02))
35159 + (((-1.0) * x361)));
35160 evalcond[6]
35161 = ((((-1.0) * new_r02
35162 * x359))
35163 + (((-1.0) * new_r22
35164 * x361)));
35165 evalcond[7]
35166 = ((((-1.0) * new_r20
35167 * x359))
35168 + ((new_r00
35169 * sj11)));
35170 evalcond[8]
35171 = (((new_r01 * sj11))
35172 + (((-1.0) * new_r21
35173 * x359)));
35174 evalcond[9]
35175 = ((1.0)
35176 + (((-1.0) * new_r22
35177 * x359))
35178 + ((new_r02
35179 * sj11)));
35180 if (IKabs(evalcond[0])
35181 < 0.0000010000000000
35182 && IKabs(evalcond[1])
35183 < 0.0000010000000000
35184 && IKabs(evalcond[2])
35185 < 0.0000010000000000
35186 && IKabs(evalcond[3])
35187 < 0.0000010000000000
35188 && IKabs(evalcond[4])
35189 < 0.0000010000000000
35190 && IKabs(evalcond[5])
35191 < 0.0000010000000000
35192 && IKabs(evalcond[6])
35193 < 0.0000010000000000
35194 && IKabs(evalcond[7])
35195 < 0.0000010000000000
35196 && IKabs(evalcond[8])
35197 < 0.0000010000000000
35198 && IKabs(evalcond[9])
35199 < 0.0000010000000000)
35200 {
35201 bgotonextstatement
35202 = false;
35203 {
35204 IkReal j12array[1],
35205 cj12array[1],
35206 sj12array[1];
35207 bool j12valid[1]
35208 = {false};
35209 _nj12 = 1;
35210 CheckValue<IkReal> x362
35211 = IKatan2WithCheck(
35212 IkReal((
35213 (-1.0)
35214 * (((1.0)
35215 * new_r21)))),
35216 new_r20,
35217 IKFAST_ATAN2_MAGTHRESH);
35218 if (!x362.valid)
35219 {
35220 continue;
35221 }
35222 CheckValue<IkReal> x363
35223 = IKPowWithIntegerCheck(
35224 IKsign(new_r02),
35225 -1);
35226 if (!x363.valid)
35227 {
35228 continue;
35229 }
35230 j12array[0]
35231 = ((-1.5707963267949)
35232 + (x362.value)
35233 + (((1.5707963267949)
35234 * (x363.value))));
35235 sj12array[0] = IKsin(
35236 j12array[0]);
35237 cj12array[0] = IKcos(
35238 j12array[0]);
35239 if (j12array[0] > IKPI)
35240 {
35241 j12array[0] -= IK2PI;
35242 }
35243 else if (
35244 j12array[0] < -IKPI)
35245 {
35246 j12array[0] += IK2PI;
35247 }
35248 j12valid[0] = true;
35249 for (int ij12 = 0;
35250 ij12 < 1;
35251 ++ij12)
35252 {
35253 if (!j12valid[ij12])
35254 {
35255 continue;
35256 }
35257 _ij12[0] = ij12;
35258 _ij12[1] = -1;
35259 for (int iij12
35260 = ij12 + 1;
35261 iij12 < 1;
35262 ++iij12)
35263 {
35264 if (j12valid[iij12]
35265 && IKabs(
35266 cj12array
35267 [ij12]
35268 - cj12array
35269 [iij12])
35270 < IKFAST_SOLUTION_THRESH
35271 && IKabs(
35272 sj12array
35273 [ij12]
35274 - sj12array
35275 [iij12])
35276 < IKFAST_SOLUTION_THRESH)
35277 {
35278 j12valid[iij12]
35279 = false;
35280 _ij12[1] = iij12;
35281 break;
35282 }
35283 }
35284 j12 = j12array[ij12];
35285 cj12
35286 = cj12array[ij12];
35287 sj12
35288 = sj12array[ij12];
35289 {
35290 IkReal evalcond[8];
35291 IkReal x364
35292 = IKsin(j12);
35293 IkReal x365
35294 = ((1.0)
35295 * (IKcos(
35296 j12)));
35297 IkReal x366
35298 = ((-1.0)
35299 * x365);
35300 IkReal x367
35301 = ((1.0)
35302 * new_r01);
35303 IkReal x368
35304 = ((1.0)
35305 * new_r00);
35306 evalcond[0]
35307 = (((new_r02
35308 * x364))
35309 + new_r21);
35310 evalcond[1]
35311 = ((((-1.0)
35312 * new_r02
35313 * x365))
35314 + new_r20);
35315 evalcond[2]
35316 = ((((-1.0)
35317 * x364))
35318 + (((-1.0)
35319 * (1.0)
35320 * new_r10)));
35321 evalcond[3]
35322 = (x366
35323 + (((-1.0)
35324 * (1.0)
35325 * new_r11)));
35326 evalcond[4]
35327 = ((((-1.0)
35328 * x367))
35329 + ((new_r22
35330 * x364)));
35331 evalcond[5]
35332 = ((((-1.0)
35333 * new_r22
35334 * x365))
35335 + (((-1.0)
35336 * x368)));
35337 evalcond[6]
35338 = (((new_r02
35339 * new_r21))
35340 + x364
35341 + (((-1.0)
35342 * new_r22
35343 * x367)));
35344 evalcond[7]
35345 = (((new_r02
35346 * new_r20))
35347 + x366
35348 + (((-1.0)
35349 * new_r22
35350 * x368)));
35351 if (IKabs(
35352 evalcond[0])
35353 > IKFAST_EVALCOND_THRESH
35354 || IKabs(
35355 evalcond
35356 [1])
35357 > IKFAST_EVALCOND_THRESH
35358 || IKabs(
35359 evalcond
35360 [2])
35361 > IKFAST_EVALCOND_THRESH
35362 || IKabs(
35363 evalcond
35364 [3])
35365 > IKFAST_EVALCOND_THRESH
35366 || IKabs(
35367 evalcond
35368 [4])
35369 > IKFAST_EVALCOND_THRESH
35370 || IKabs(
35371 evalcond
35372 [5])
35373 > IKFAST_EVALCOND_THRESH
35374 || IKabs(
35375 evalcond
35376 [6])
35377 > IKFAST_EVALCOND_THRESH
35378 || IKabs(
35379 evalcond
35380 [7])
35381 > IKFAST_EVALCOND_THRESH)
35382 {
35383 continue;
35384 }
35385 }
35386
35387 {
35388 std::vector<
35389 IkSingleDOFSolutionBase<
35390 IkReal> >
35391 vinfos(7);
35392 vinfos[0].jointtype
35393 = 1;
35394 vinfos[0].foffset
35395 = j4;
35396 vinfos[0].indices[0]
35397 = _ij4[0];
35398 vinfos[0].indices[1]
35399 = _ij4[1];
35400 vinfos[0]
35401 .maxsolutions
35402 = _nj4;
35403 vinfos[1].jointtype
35404 = 1;
35405 vinfos[1].foffset
35406 = j6;
35407 vinfos[1].indices[0]
35408 = _ij6[0];
35409 vinfos[1].indices[1]
35410 = _ij6[1];
35411 vinfos[1]
35412 .maxsolutions
35413 = _nj6;
35414 vinfos[2].jointtype
35415 = 1;
35416 vinfos[2].foffset
35417 = j8;
35418 vinfos[2].indices[0]
35419 = _ij8[0];
35420 vinfos[2].indices[1]
35421 = _ij8[1];
35422 vinfos[2]
35423 .maxsolutions
35424 = _nj8;
35425 vinfos[3].jointtype
35426 = 1;
35427 vinfos[3].foffset
35428 = j9;
35429 vinfos[3].indices[0]
35430 = _ij9[0];
35431 vinfos[3].indices[1]
35432 = _ij9[1];
35433 vinfos[3]
35434 .maxsolutions
35435 = _nj9;
35436 vinfos[4].jointtype
35437 = 1;
35438 vinfos[4].foffset
35439 = j10;
35440 vinfos[4].indices[0]
35441 = _ij10[0];
35442 vinfos[4].indices[1]
35443 = _ij10[1];
35444 vinfos[4]
35445 .maxsolutions
35446 = _nj10;
35447 vinfos[5].jointtype
35448 = 1;
35449 vinfos[5].foffset
35450 = j11;
35451 vinfos[5].indices[0]
35452 = _ij11[0];
35453 vinfos[5].indices[1]
35454 = _ij11[1];
35455 vinfos[5]
35456 .maxsolutions
35457 = _nj11;
35458 vinfos[6].jointtype
35459 = 1;
35460 vinfos[6].foffset
35461 = j12;
35462 vinfos[6].indices[0]
35463 = _ij12[0];
35464 vinfos[6].indices[1]
35465 = _ij12[1];
35466 vinfos[6]
35467 .maxsolutions
35468 = _nj12;
35469 std::vector<int>
35470 vfree(0);
35471 solutions
35472 .AddSolution(
35473 vinfos,
35474 vfree);
35475 }
35476 }
35477 }
35478 }
35479 } while (0);
35480 if (bgotonextstatement)
35481 {
35482 bool bgotonextstatement
35483 = true;
35484 do
35485 {
35486 if (1)
35487 {
35488 bgotonextstatement
35489 = false;
35490 continue; // branch miss
35491 // [j12]
35492 }
35493 } while (0);
35494 if (bgotonextstatement)
35495 {
35496 }
35497 }
35498 }
35499 }
35500 }
35501 }
35502 }
35503 }
35504 }
35505 }
35506 }
35507 else
35508 {
35509 {
35510 IkReal j12array[1], cj12array[1],
35511 sj12array[1];
35512 bool j12valid[1] = {false};
35513 _nj12 = 1;
35514 CheckValue<IkReal> x370
35515 = IKPowWithIntegerCheck(sj11, -1);
35516 if (!x370.valid)
35517 {
35518 continue;
35519 }
35520 IkReal x369 = x370.value;
35521 CheckValue<IkReal> x371
35522 = IKPowWithIntegerCheck(cj10, -1);
35523 if (!x371.valid)
35524 {
35525 continue;
35526 }
35527 CheckValue<IkReal> x372
35528 = IKPowWithIntegerCheck(cj11, -1);
35529 if (!x372.valid)
35530 {
35531 continue;
35532 }
35533 if (IKabs(
35534 (x369 * (x371.value) * (x372.value)
35535 * (((((-1.0) * (1.0) * new_r01
35536 * sj11))
35537 + ((new_r20 * sj10))))))
35538 < IKFAST_ATAN2_MAGTHRESH
35539 && IKabs(((-1.0) * new_r20 * x369))
35540 < IKFAST_ATAN2_MAGTHRESH
35541 && IKabs(
35542 IKsqr(
35543 (x369 * (x371.value)
35544 * (x372.value)
35545 * (((((-1.0) * (1.0)
35546 * new_r01 * sj11))
35547 + ((new_r20 * sj10))))))
35548 + IKsqr(
35549 ((-1.0) * new_r20 * x369))
35550 - 1)
35551 <= IKFAST_SINCOS_THRESH)
35552 continue;
35553 j12array[0] = IKatan2(
35554 (x369 * (x371.value) * (x372.value)
35555 * (((((-1.0) * (1.0) * new_r01 * sj11))
35556 + ((new_r20 * sj10))))),
35557 ((-1.0) * new_r20 * x369));
35558 sj12array[0] = IKsin(j12array[0]);
35559 cj12array[0] = IKcos(j12array[0]);
35560 if (j12array[0] > IKPI)
35561 {
35562 j12array[0] -= IK2PI;
35563 }
35564 else if (j12array[0] < -IKPI)
35565 {
35566 j12array[0] += IK2PI;
35567 }
35568 j12valid[0] = true;
35569 for (int ij12 = 0; ij12 < 1; ++ij12)
35570 {
35571 if (!j12valid[ij12])
35572 {
35573 continue;
35574 }
35575 _ij12[0] = ij12;
35576 _ij12[1] = -1;
35577 for (int iij12 = ij12 + 1; iij12 < 1;
35578 ++iij12)
35579 {
35580 if (j12valid[iij12]
35581 && IKabs(
35582 cj12array[ij12]
35583 - cj12array[iij12])
35584 < IKFAST_SOLUTION_THRESH
35585 && IKabs(
35586 sj12array[ij12]
35587 - sj12array[iij12])
35588 < IKFAST_SOLUTION_THRESH)
35589 {
35590 j12valid[iij12] = false;
35591 _ij12[1] = iij12;
35592 break;
35593 }
35594 }
35595 j12 = j12array[ij12];
35596 cj12 = cj12array[ij12];
35597 sj12 = sj12array[ij12];
35598 {
35599 IkReal evalcond[12];
35600 IkReal x373 = IKcos(j12);
35601 IkReal x374 = IKsin(j12);
35602 IkReal x375 = ((1.0) * sj11);
35603 IkReal x376 = (cj10 * new_r01);
35604 IkReal x377 = (new_r11 * sj10);
35605 IkReal x378 = (cj11 * x374);
35606 IkReal x379 = ((1.0) * sj10);
35607 IkReal x380 = ((1.0) * x374);
35608 IkReal x381 = ((1.0) * x373);
35609 IkReal x382 = ((-1.0) * x381);
35610 IkReal x383 = (cj10 * new_r00);
35611 IkReal x384 = (new_r10 * sj10);
35612 IkReal x385 = (cj10 * x381);
35613 evalcond[0]
35614 = (((sj11 * x373)) + new_r20);
35615 evalcond[1]
35616 = ((((-1.0) * x374 * x375))
35617 + new_r21);
35618 evalcond[2] = (x378 + x377 + x376);
35619 evalcond[3]
35620 = (((cj10 * new_r10))
35621 + (((-1.0) * x380))
35622 + (((-1.0) * new_r00 * x379)));
35623 evalcond[4]
35624 = (((cj10 * new_r11)) + x382
35625 + (((-1.0) * new_r01 * x379)));
35626 evalcond[5]
35627 = (((sj10 * x373)) + ((cj10 * x378))
35628 + new_r01);
35629 evalcond[6]
35630 = ((((-1.0) * cj11 * x381)) + x384
35631 + x383);
35632 evalcond[7]
35633 = (((sj10 * x374))
35634 + (((-1.0) * cj11 * x385))
35635 + new_r00);
35636 evalcond[8]
35637 = ((((-1.0) * x385)) + new_r11
35638 + ((sj10 * x378)));
35639 evalcond[9]
35640 = ((((-1.0) * cj10 * x380))
35641 + (((-1.0) * cj11 * x373 * x379))
35642 + new_r10);
35643 evalcond[10]
35644 = (((cj11 * x376)) + ((cj11 * x377))
35645 + (((-1.0) * new_r21 * x375))
35646 + x374);
35647 evalcond[11]
35648 = ((((-1.0) * new_r20 * x375))
35649 + x382 + ((cj11 * x384))
35650 + ((cj11 * x383)));
35651 if (IKabs(evalcond[0])
35652 > IKFAST_EVALCOND_THRESH
35653 || IKabs(evalcond[1])
35654 > IKFAST_EVALCOND_THRESH
35655 || IKabs(evalcond[2])
35656 > IKFAST_EVALCOND_THRESH
35657 || IKabs(evalcond[3])
35658 > IKFAST_EVALCOND_THRESH
35659 || IKabs(evalcond[4])
35660 > IKFAST_EVALCOND_THRESH
35661 || IKabs(evalcond[5])
35662 > IKFAST_EVALCOND_THRESH
35663 || IKabs(evalcond[6])
35664 > IKFAST_EVALCOND_THRESH
35665 || IKabs(evalcond[7])
35666 > IKFAST_EVALCOND_THRESH
35667 || IKabs(evalcond[8])
35668 > IKFAST_EVALCOND_THRESH
35669 || IKabs(evalcond[9])
35670 > IKFAST_EVALCOND_THRESH
35671 || IKabs(evalcond[10])
35672 > IKFAST_EVALCOND_THRESH
35673 || IKabs(evalcond[11])
35674 > IKFAST_EVALCOND_THRESH)
35675 {
35676 continue;
35677 }
35678 }
35679
35680 {
35681 std::vector<
35682 IkSingleDOFSolutionBase<IkReal> >
35683 vinfos(7);
35684 vinfos[0].jointtype = 1;
35685 vinfos[0].foffset = j4;
35686 vinfos[0].indices[0] = _ij4[0];
35687 vinfos[0].indices[1] = _ij4[1];
35688 vinfos[0].maxsolutions = _nj4;
35689 vinfos[1].jointtype = 1;
35690 vinfos[1].foffset = j6;
35691 vinfos[1].indices[0] = _ij6[0];
35692 vinfos[1].indices[1] = _ij6[1];
35693 vinfos[1].maxsolutions = _nj6;
35694 vinfos[2].jointtype = 1;
35695 vinfos[2].foffset = j8;
35696 vinfos[2].indices[0] = _ij8[0];
35697 vinfos[2].indices[1] = _ij8[1];
35698 vinfos[2].maxsolutions = _nj8;
35699 vinfos[3].jointtype = 1;
35700 vinfos[3].foffset = j9;
35701 vinfos[3].indices[0] = _ij9[0];
35702 vinfos[3].indices[1] = _ij9[1];
35703 vinfos[3].maxsolutions = _nj9;
35704 vinfos[4].jointtype = 1;
35705 vinfos[4].foffset = j10;
35706 vinfos[4].indices[0] = _ij10[0];
35707 vinfos[4].indices[1] = _ij10[1];
35708 vinfos[4].maxsolutions = _nj10;
35709 vinfos[5].jointtype = 1;
35710 vinfos[5].foffset = j11;
35711 vinfos[5].indices[0] = _ij11[0];
35712 vinfos[5].indices[1] = _ij11[1];
35713 vinfos[5].maxsolutions = _nj11;
35714 vinfos[6].jointtype = 1;
35715 vinfos[6].foffset = j12;
35716 vinfos[6].indices[0] = _ij12[0];
35717 vinfos[6].indices[1] = _ij12[1];
35718 vinfos[6].maxsolutions = _nj12;
35719 std::vector<int> vfree(0);
35720 solutions.AddSolution(vinfos, vfree);
35721 }
35722 }
35723 }
35724 }
35725 }
35726 }
35727 else
35728 {
35729 {
35730 IkReal j12array[1], cj12array[1], sj12array[1];
35731 bool j12valid[1] = {false};
35732 _nj12 = 1;
35733 CheckValue<IkReal> x388
35734 = IKPowWithIntegerCheck(sj11, -1);
35735 if (!x388.valid)
35736 {
35737 continue;
35738 }
35739 IkReal x386 = x388.value;
35740 IkReal x387 = ((1.0) * new_r20);
35741 CheckValue<IkReal> x389
35742 = IKPowWithIntegerCheck(sj10, -1);
35743 if (!x389.valid)
35744 {
35745 continue;
35746 }
35747 if (IKabs(
35748 (x386 * (x389.value)
35749 * (((((-1.0) * cj10 * cj11 * x387))
35750 + (((-1.0) * (1.0) * new_r00
35751 * sj11))))))
35752 < IKFAST_ATAN2_MAGTHRESH
35753 && IKabs(((-1.0) * x386 * x387))
35754 < IKFAST_ATAN2_MAGTHRESH
35755 && IKabs(
35756 IKsqr(
35757 (x386 * (x389.value)
35758 * (((((-1.0) * cj10 * cj11
35759 * x387))
35760 + (((-1.0) * (1.0) * new_r00
35761 * sj11))))))
35762 + IKsqr(((-1.0) * x386 * x387)) - 1)
35763 <= IKFAST_SINCOS_THRESH)
35764 continue;
35765 j12array[0] = IKatan2(
35766 (x386 * (x389.value)
35767 * (((((-1.0) * cj10 * cj11 * x387))
35768 + (((-1.0) * (1.0) * new_r00
35769 * sj11))))),
35770 ((-1.0) * x386 * x387));
35771 sj12array[0] = IKsin(j12array[0]);
35772 cj12array[0] = IKcos(j12array[0]);
35773 if (j12array[0] > IKPI)
35774 {
35775 j12array[0] -= IK2PI;
35776 }
35777 else if (j12array[0] < -IKPI)
35778 {
35779 j12array[0] += IK2PI;
35780 }
35781 j12valid[0] = true;
35782 for (int ij12 = 0; ij12 < 1; ++ij12)
35783 {
35784 if (!j12valid[ij12])
35785 {
35786 continue;
35787 }
35788 _ij12[0] = ij12;
35789 _ij12[1] = -1;
35790 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
35791 {
35792 if (j12valid[iij12]
35793 && IKabs(
35794 cj12array[ij12]
35795 - cj12array[iij12])
35796 < IKFAST_SOLUTION_THRESH
35797 && IKabs(
35798 sj12array[ij12]
35799 - sj12array[iij12])
35800 < IKFAST_SOLUTION_THRESH)
35801 {
35802 j12valid[iij12] = false;
35803 _ij12[1] = iij12;
35804 break;
35805 }
35806 }
35807 j12 = j12array[ij12];
35808 cj12 = cj12array[ij12];
35809 sj12 = sj12array[ij12];
35810 {
35811 IkReal evalcond[12];
35812 IkReal x390 = IKcos(j12);
35813 IkReal x391 = IKsin(j12);
35814 IkReal x392 = ((1.0) * sj11);
35815 IkReal x393 = (cj10 * new_r01);
35816 IkReal x394 = (new_r11 * sj10);
35817 IkReal x395 = (cj11 * x391);
35818 IkReal x396 = ((1.0) * sj10);
35819 IkReal x397 = ((1.0) * x391);
35820 IkReal x398 = ((1.0) * x390);
35821 IkReal x399 = ((-1.0) * x398);
35822 IkReal x400 = (cj10 * new_r00);
35823 IkReal x401 = (new_r10 * sj10);
35824 IkReal x402 = (cj10 * x398);
35825 evalcond[0] = (((sj11 * x390)) + new_r20);
35826 evalcond[1]
35827 = ((((-1.0) * x391 * x392)) + new_r21);
35828 evalcond[2] = (x394 + x395 + x393);
35829 evalcond[3]
35830 = (((cj10 * new_r10))
35831 + (((-1.0) * x397))
35832 + (((-1.0) * new_r00 * x396)));
35833 evalcond[4]
35834 = (((cj10 * new_r11)) + x399
35835 + (((-1.0) * new_r01 * x396)));
35836 evalcond[5]
35837 = (((cj10 * x395)) + ((sj10 * x390))
35838 + new_r01);
35839 evalcond[6]
35840 = (x401 + x400
35841 + (((-1.0) * cj11 * x398)));
35842 evalcond[7]
35843 = (new_r00 + (((-1.0) * cj11 * x402))
35844 + ((sj10 * x391)));
35845 evalcond[8]
35846 = ((((-1.0) * x402)) + ((sj10 * x395))
35847 + new_r11);
35848 evalcond[9]
35849 = ((((-1.0) * cj10 * x397)) + new_r10
35850 + (((-1.0) * cj11 * x390 * x396)));
35851 evalcond[10]
35852 = (x391 + ((cj11 * x393))
35853 + ((cj11 * x394))
35854 + (((-1.0) * new_r21 * x392)));
35855 evalcond[11]
35856 = (x399 + ((cj11 * x400))
35857 + (((-1.0) * new_r20 * x392))
35858 + ((cj11 * x401)));
35859 if (IKabs(evalcond[0])
35860 > IKFAST_EVALCOND_THRESH
35861 || IKabs(evalcond[1])
35862 > IKFAST_EVALCOND_THRESH
35863 || IKabs(evalcond[2])
35864 > IKFAST_EVALCOND_THRESH
35865 || IKabs(evalcond[3])
35866 > IKFAST_EVALCOND_THRESH
35867 || IKabs(evalcond[4])
35868 > IKFAST_EVALCOND_THRESH
35869 || IKabs(evalcond[5])
35870 > IKFAST_EVALCOND_THRESH
35871 || IKabs(evalcond[6])
35872 > IKFAST_EVALCOND_THRESH
35873 || IKabs(evalcond[7])
35874 > IKFAST_EVALCOND_THRESH
35875 || IKabs(evalcond[8])
35876 > IKFAST_EVALCOND_THRESH
35877 || IKabs(evalcond[9])
35878 > IKFAST_EVALCOND_THRESH
35879 || IKabs(evalcond[10])
35880 > IKFAST_EVALCOND_THRESH
35881 || IKabs(evalcond[11])
35882 > IKFAST_EVALCOND_THRESH)
35883 {
35884 continue;
35885 }
35886 }
35887
35888 {
35889 std::vector<
35890 IkSingleDOFSolutionBase<IkReal> >
35891 vinfos(7);
35892 vinfos[0].jointtype = 1;
35893 vinfos[0].foffset = j4;
35894 vinfos[0].indices[0] = _ij4[0];
35895 vinfos[0].indices[1] = _ij4[1];
35896 vinfos[0].maxsolutions = _nj4;
35897 vinfos[1].jointtype = 1;
35898 vinfos[1].foffset = j6;
35899 vinfos[1].indices[0] = _ij6[0];
35900 vinfos[1].indices[1] = _ij6[1];
35901 vinfos[1].maxsolutions = _nj6;
35902 vinfos[2].jointtype = 1;
35903 vinfos[2].foffset = j8;
35904 vinfos[2].indices[0] = _ij8[0];
35905 vinfos[2].indices[1] = _ij8[1];
35906 vinfos[2].maxsolutions = _nj8;
35907 vinfos[3].jointtype = 1;
35908 vinfos[3].foffset = j9;
35909 vinfos[3].indices[0] = _ij9[0];
35910 vinfos[3].indices[1] = _ij9[1];
35911 vinfos[3].maxsolutions = _nj9;
35912 vinfos[4].jointtype = 1;
35913 vinfos[4].foffset = j10;
35914 vinfos[4].indices[0] = _ij10[0];
35915 vinfos[4].indices[1] = _ij10[1];
35916 vinfos[4].maxsolutions = _nj10;
35917 vinfos[5].jointtype = 1;
35918 vinfos[5].foffset = j11;
35919 vinfos[5].indices[0] = _ij11[0];
35920 vinfos[5].indices[1] = _ij11[1];
35921 vinfos[5].maxsolutions = _nj11;
35922 vinfos[6].jointtype = 1;
35923 vinfos[6].foffset = j12;
35924 vinfos[6].indices[0] = _ij12[0];
35925 vinfos[6].indices[1] = _ij12[1];
35926 vinfos[6].maxsolutions = _nj12;
35927 std::vector<int> vfree(0);
35928 solutions.AddSolution(vinfos, vfree);
35929 }
35930 }
35931 }
35932 }
35933 }
35934 }
35935 else
35936 {
35937 {
35938 IkReal j12array[1], cj12array[1], sj12array[1];
35939 bool j12valid[1] = {false};
35940 _nj12 = 1;
35941 CheckValue<IkReal> x403
35942 = IKPowWithIntegerCheck(IKsign(sj11), -1);
35943 if (!x403.valid)
35944 {
35945 continue;
35946 }
35947 CheckValue<IkReal> x404 = IKatan2WithCheck(
35948 IkReal(new_r21),
35949 ((-1.0) * (((1.0) * new_r20))),
35950 IKFAST_ATAN2_MAGTHRESH);
35951 if (!x404.valid)
35952 {
35953 continue;
35954 }
35955 j12array[0]
35956 = ((-1.5707963267949)
35957 + (((1.5707963267949) * (x403.value)))
35958 + (x404.value));
35959 sj12array[0] = IKsin(j12array[0]);
35960 cj12array[0] = IKcos(j12array[0]);
35961 if (j12array[0] > IKPI)
35962 {
35963 j12array[0] -= IK2PI;
35964 }
35965 else if (j12array[0] < -IKPI)
35966 {
35967 j12array[0] += IK2PI;
35968 }
35969 j12valid[0] = true;
35970 for (int ij12 = 0; ij12 < 1; ++ij12)
35971 {
35972 if (!j12valid[ij12])
35973 {
35974 continue;
35975 }
35976 _ij12[0] = ij12;
35977 _ij12[1] = -1;
35978 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
35979 {
35980 if (j12valid[iij12]
35981 && IKabs(cj12array[ij12] - cj12array[iij12])
35982 < IKFAST_SOLUTION_THRESH
35983 && IKabs(sj12array[ij12] - sj12array[iij12])
35984 < IKFAST_SOLUTION_THRESH)
35985 {
35986 j12valid[iij12] = false;
35987 _ij12[1] = iij12;
35988 break;
35989 }
35990 }
35991 j12 = j12array[ij12];
35992 cj12 = cj12array[ij12];
35993 sj12 = sj12array[ij12];
35994 {
35995 IkReal evalcond[12];
35996 IkReal x405 = IKcos(j12);
35997 IkReal x406 = IKsin(j12);
35998 IkReal x407 = ((1.0) * sj11);
35999 IkReal x408 = (cj10 * new_r01);
36000 IkReal x409 = (new_r11 * sj10);
36001 IkReal x410 = (cj11 * x406);
36002 IkReal x411 = ((1.0) * sj10);
36003 IkReal x412 = ((1.0) * x406);
36004 IkReal x413 = ((1.0) * x405);
36005 IkReal x414 = ((-1.0) * x413);
36006 IkReal x415 = (cj10 * new_r00);
36007 IkReal x416 = (new_r10 * sj10);
36008 IkReal x417 = (cj10 * x413);
36009 evalcond[0] = (((sj11 * x405)) + new_r20);
36010 evalcond[1]
36011 = ((((-1.0) * x406 * x407)) + new_r21);
36012 evalcond[2] = (x410 + x409 + x408);
36013 evalcond[3]
36014 = ((((-1.0) * new_r00 * x411))
36015 + ((cj10 * new_r10))
36016 + (((-1.0) * x412)));
36017 evalcond[4]
36018 = (((cj10 * new_r11)) + x414
36019 + (((-1.0) * new_r01 * x411)));
36020 evalcond[5]
36021 = (((cj10 * x410)) + ((sj10 * x405))
36022 + new_r01);
36023 evalcond[6]
36024 = ((((-1.0) * cj11 * x413)) + x415 + x416);
36025 evalcond[7]
36026 = ((((-1.0) * cj11 * x417))
36027 + ((sj10 * x406)) + new_r00);
36028 evalcond[8]
36029 = (((sj10 * x410)) + (((-1.0) * x417))
36030 + new_r11);
36031 evalcond[9]
36032 = ((((-1.0) * cj10 * x412))
36033 + (((-1.0) * cj11 * x405 * x411))
36034 + new_r10);
36035 evalcond[10]
36036 = (((cj11 * x409))
36037 + (((-1.0) * new_r21 * x407)) + x406
36038 + ((cj11 * x408)));
36039 evalcond[11]
36040 = (x414 + ((cj11 * x416)) + ((cj11 * x415))
36041 + (((-1.0) * new_r20 * x407)));
36042 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
36043 || IKabs(evalcond[1])
36044 > IKFAST_EVALCOND_THRESH
36045 || IKabs(evalcond[2])
36046 > IKFAST_EVALCOND_THRESH
36047 || IKabs(evalcond[3])
36048 > IKFAST_EVALCOND_THRESH
36049 || IKabs(evalcond[4])
36050 > IKFAST_EVALCOND_THRESH
36051 || IKabs(evalcond[5])
36052 > IKFAST_EVALCOND_THRESH
36053 || IKabs(evalcond[6])
36054 > IKFAST_EVALCOND_THRESH
36055 || IKabs(evalcond[7])
36056 > IKFAST_EVALCOND_THRESH
36057 || IKabs(evalcond[8])
36058 > IKFAST_EVALCOND_THRESH
36059 || IKabs(evalcond[9])
36060 > IKFAST_EVALCOND_THRESH
36061 || IKabs(evalcond[10])
36062 > IKFAST_EVALCOND_THRESH
36063 || IKabs(evalcond[11])
36064 > IKFAST_EVALCOND_THRESH)
36065 {
36066 continue;
36067 }
36068 }
36069
36070 {
36071 std::vector<IkSingleDOFSolutionBase<IkReal> >
36072 vinfos(7);
36073 vinfos[0].jointtype = 1;
36074 vinfos[0].foffset = j4;
36075 vinfos[0].indices[0] = _ij4[0];
36076 vinfos[0].indices[1] = _ij4[1];
36077 vinfos[0].maxsolutions = _nj4;
36078 vinfos[1].jointtype = 1;
36079 vinfos[1].foffset = j6;
36080 vinfos[1].indices[0] = _ij6[0];
36081 vinfos[1].indices[1] = _ij6[1];
36082 vinfos[1].maxsolutions = _nj6;
36083 vinfos[2].jointtype = 1;
36084 vinfos[2].foffset = j8;
36085 vinfos[2].indices[0] = _ij8[0];
36086 vinfos[2].indices[1] = _ij8[1];
36087 vinfos[2].maxsolutions = _nj8;
36088 vinfos[3].jointtype = 1;
36089 vinfos[3].foffset = j9;
36090 vinfos[3].indices[0] = _ij9[0];
36091 vinfos[3].indices[1] = _ij9[1];
36092 vinfos[3].maxsolutions = _nj9;
36093 vinfos[4].jointtype = 1;
36094 vinfos[4].foffset = j10;
36095 vinfos[4].indices[0] = _ij10[0];
36096 vinfos[4].indices[1] = _ij10[1];
36097 vinfos[4].maxsolutions = _nj10;
36098 vinfos[5].jointtype = 1;
36099 vinfos[5].foffset = j11;
36100 vinfos[5].indices[0] = _ij11[0];
36101 vinfos[5].indices[1] = _ij11[1];
36102 vinfos[5].maxsolutions = _nj11;
36103 vinfos[6].jointtype = 1;
36104 vinfos[6].foffset = j12;
36105 vinfos[6].indices[0] = _ij12[0];
36106 vinfos[6].indices[1] = _ij12[1];
36107 vinfos[6].maxsolutions = _nj12;
36108 std::vector<int> vfree(0);
36109 solutions.AddSolution(vinfos, vfree);
36110 }
36111 }
36112 }
36113 }
36114 }
36115 }
36116 }
36117 }
36118 }
36119 }
36120 else
36121 {
36122 {
36123 IkReal j10array[1], cj10array[1], sj10array[1];
36124 bool j10valid[1] = {false};
36125 _nj10 = 1;
36126 CheckValue<IkReal> x418 = IKatan2WithCheck(
36127 IkReal(new_r12), new_r02, IKFAST_ATAN2_MAGTHRESH);
36128 if (!x418.valid)
36129 {
36130 continue;
36131 }
36132 CheckValue<IkReal> x419
36133 = IKPowWithIntegerCheck(IKsign(sj11), -1);
36134 if (!x419.valid)
36135 {
36136 continue;
36137 }
36138 j10array[0]
36139 = ((-1.5707963267949) + (x418.value)
36140 + (((1.5707963267949) * (x419.value))));
36141 sj10array[0] = IKsin(j10array[0]);
36142 cj10array[0] = IKcos(j10array[0]);
36143 if (j10array[0] > IKPI)
36144 {
36145 j10array[0] -= IK2PI;
36146 }
36147 else if (j10array[0] < -IKPI)
36148 {
36149 j10array[0] += IK2PI;
36150 }
36151 j10valid[0] = true;
36152 for (int ij10 = 0; ij10 < 1; ++ij10)
36153 {
36154 if (!j10valid[ij10])
36155 {
36156 continue;
36157 }
36158 _ij10[0] = ij10;
36159 _ij10[1] = -1;
36160 for (int iij10 = ij10 + 1; iij10 < 1; ++iij10)
36161 {
36162 if (j10valid[iij10]
36163 && IKabs(cj10array[ij10] - cj10array[iij10])
36164 < IKFAST_SOLUTION_THRESH
36165 && IKabs(sj10array[ij10] - sj10array[iij10])
36166 < IKFAST_SOLUTION_THRESH)
36167 {
36168 j10valid[iij10] = false;
36169 _ij10[1] = iij10;
36170 break;
36171 }
36172 }
36173 j10 = j10array[ij10];
36174 cj10 = cj10array[ij10];
36175 sj10 = sj10array[ij10];
36176 {
36177 IkReal evalcond[8];
36178 IkReal x420 = IKcos(j10);
36179 IkReal x421 = ((1.0) * sj11);
36180 IkReal x422 = (x420 * x421);
36181 IkReal x423 = IKsin(j10);
36182 IkReal x424 = (x421 * x423);
36183 IkReal x425 = (new_r02 * x420);
36184 IkReal x426 = (new_r12 * x423);
36185 IkReal x427 = ((1.0) * cj11);
36186 evalcond[0] = ((((-1.0) * x422)) + new_r02);
36187 evalcond[1] = ((((-1.0) * x424)) + new_r12);
36188 evalcond[2]
36189 = ((((-1.0) * new_r02 * x423)) + ((new_r12 * x420)));
36190 evalcond[3] = ((((-1.0) * x421)) + x426 + x425);
36191 evalcond[4]
36192 = (((cj11 * x426)) + ((cj11 * x425))
36193 + (((-1.0) * new_r22 * x421)));
36194 evalcond[5]
36195 = ((((-1.0) * new_r10 * x424))
36196 + (((-1.0) * new_r20 * x427))
36197 + (((-1.0) * new_r00 * x422)));
36198 evalcond[6]
36199 = ((((-1.0) * new_r21 * x427))
36200 + (((-1.0) * new_r01 * x422))
36201 + (((-1.0) * new_r11 * x424)));
36202 evalcond[7]
36203 = ((1.0) + (((-1.0) * new_r22 * x427))
36204 + (((-1.0) * x421 * x425))
36205 + (((-1.0) * x421 * x426)));
36206 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
36207 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
36208 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
36209 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
36210 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
36211 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
36212 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
36213 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
36214 {
36215 continue;
36216 }
36217 }
36218
36219 {
36220 IkReal j12eval[2];
36221 IkReal x428 = ((1.0) * sj9);
36222 IkReal x429 = (cj9 * sj6);
36223 IkReal x430 = x160;
36224 IkReal x431 = (cj4 * sj8);
36225 IkReal x432 = (sj6 * x428);
36226 IkReal x433 = (cj6 * cj9);
36227 IkReal x434 = ((((-1.0) * x432)) + ((cj8 * x433)));
36228 IkReal x435 = (((cj9 * x431)) + ((sj4 * x434)));
36229 IkReal x436 = ((1.0) * sj4 * sj8);
36230 IkReal x437 = ((((-1.0) * cj9 * x436)) + ((cj4 * x434)));
36231 IkReal x438 = (sj6 * sj8);
36232 IkReal x439 = x169;
36233 IkReal x440 = x170;
36234 IkReal x441 = ((((-1.0) * cj8 * x432)) + x433);
36235 IkReal x442 = x172;
36236 IkReal x443 = (((sj9 * x431)) + ((sj4 * x442)));
36237 IkReal x444
36238 = (((cj4 * x442)) + (((-1.0) * sj4 * sj8 * x428)));
36239 new_r00
36240 = (((r20 * x430)) + ((r10 * x435)) + ((r00 * x437)));
36241 new_r01
36242 = (((r11 * x435)) + ((r01 * x437)) + ((r21 * x430)));
36243 new_r02
36244 = (((r12 * x435)) + ((r02 * x437)) + ((r22 * x430)));
36245 new_r10
36246 = (((r20 * x438)) + ((r00 * x440)) + ((r10 * x439)));
36247 new_r11
36248 = (((r11 * x439)) + ((r21 * x438)) + ((r01 * x440)));
36249 new_r12
36250 = (((r22 * x438)) + ((r12 * x439)) + ((r02 * x440)));
36251 new_r20
36252 = (((r10 * x443)) + ((r20 * x441)) + ((r00 * x444)));
36253 new_r21
36254 = (((r01 * x444)) + ((r21 * x441)) + ((r11 * x443)));
36255 new_r22
36256 = (((r22 * x441)) + ((r12 * x443)) + ((r02 * x444)));
36257 j12eval[0] = sj11;
36258 j12eval[1] = IKsign(sj11);
36259 if (IKabs(j12eval[0]) < 0.0000010000000000
36260 || IKabs(j12eval[1]) < 0.0000010000000000)
36261 {
36262 {
36263 IkReal j12eval[2];
36264 IkReal x445 = ((1.0) * sj9);
36265 IkReal x446 = (cj9 * sj6);
36266 IkReal x447 = x160;
36267 IkReal x448 = (cj4 * sj8);
36268 IkReal x449 = (sj6 * x445);
36269 IkReal x450 = (cj6 * cj9);
36270 IkReal x451 = (((cj8 * x450)) + (((-1.0) * x449)));
36271 IkReal x452 = (((cj9 * x448)) + ((sj4 * x451)));
36272 IkReal x453 = ((1.0) * sj4 * sj8);
36273 IkReal x454
36274 = (((cj4 * x451)) + (((-1.0) * cj9 * x453)));
36275 IkReal x455 = (sj6 * sj8);
36276 IkReal x456 = x169;
36277 IkReal x457 = x170;
36278 IkReal x458 = (x450 + (((-1.0) * cj8 * x449)));
36279 IkReal x459 = x172;
36280 IkReal x460 = (((sj4 * x459)) + ((sj9 * x448)));
36281 IkReal x461
36282 = (((cj4 * x459)) + (((-1.0) * sj4 * sj8 * x445)));
36283 new_r00
36284 = (((r00 * x454)) + ((r20 * x447))
36285 + ((r10 * x452)));
36286 new_r01
36287 = (((r01 * x454)) + ((r21 * x447))
36288 + ((r11 * x452)));
36289 new_r02
36290 = (((r02 * x454)) + ((r12 * x452))
36291 + ((r22 * x447)));
36292 new_r10
36293 = (((r20 * x455)) + ((r00 * x457))
36294 + ((r10 * x456)));
36295 new_r11
36296 = (((r21 * x455)) + ((r11 * x456))
36297 + ((r01 * x457)));
36298 new_r12
36299 = (((r12 * x456)) + ((r22 * x455))
36300 + ((r02 * x457)));
36301 new_r20
36302 = (((r10 * x460)) + ((r20 * x458))
36303 + ((r00 * x461)));
36304 new_r21
36305 = (((r11 * x460)) + ((r21 * x458))
36306 + ((r01 * x461)));
36307 new_r22
36308 = (((r12 * x460)) + ((r22 * x458))
36309 + ((r02 * x461)));
36310 j12eval[0] = sj10;
36311 j12eval[1] = sj11;
36312 if (IKabs(j12eval[0]) < 0.0000010000000000
36313 || IKabs(j12eval[1]) < 0.0000010000000000)
36314 {
36315 {
36316 IkReal j12eval[3];
36317 IkReal x462 = ((1.0) * sj9);
36318 IkReal x463 = (cj9 * sj6);
36319 IkReal x464 = x160;
36320 IkReal x465 = (cj4 * sj8);
36321 IkReal x466 = (sj6 * x462);
36322 IkReal x467 = (cj6 * cj9);
36323 IkReal x468 = ((((-1.0) * x466)) + ((cj8 * x467)));
36324 IkReal x469 = (((cj9 * x465)) + ((sj4 * x468)));
36325 IkReal x470 = ((1.0) * sj4 * sj8);
36326 IkReal x471
36327 = ((((-1.0) * cj9 * x470)) + ((cj4 * x468)));
36328 IkReal x472 = (sj6 * sj8);
36329 IkReal x473 = x169;
36330 IkReal x474 = x170;
36331 IkReal x475 = ((((-1.0) * cj8 * x466)) + x467);
36332 IkReal x476 = x172;
36333 IkReal x477 = (((sj9 * x465)) + ((sj4 * x476)));
36334 IkReal x478
36335 = ((((-1.0) * sj4 * sj8 * x462))
36336 + ((cj4 * x476)));
36337 new_r00
36338 = (((r10 * x469)) + ((r00 * x471))
36339 + ((r20 * x464)));
36340 new_r01
36341 = (((r11 * x469)) + ((r01 * x471))
36342 + ((r21 * x464)));
36343 new_r02
36344 = (((r22 * x464)) + ((r02 * x471))
36345 + ((r12 * x469)));
36346 new_r10
36347 = (((r00 * x474)) + ((r10 * x473))
36348 + ((r20 * x472)));
36349 new_r11
36350 = (((r01 * x474)) + ((r11 * x473))
36351 + ((r21 * x472)));
36352 new_r12
36353 = (((r02 * x474)) + ((r12 * x473))
36354 + ((r22 * x472)));
36355 new_r20
36356 = (((r20 * x475)) + ((r10 * x477))
36357 + ((r00 * x478)));
36358 new_r21
36359 = (((r01 * x478)) + ((r21 * x475))
36360 + ((r11 * x477)));
36361 new_r22
36362 = (((r22 * x475)) + ((r02 * x478))
36363 + ((r12 * x477)));
36364 j12eval[0] = cj10;
36365 j12eval[1] = cj11;
36366 j12eval[2] = sj11;
36367 if (IKabs(j12eval[0]) < 0.0000010000000000
36368 || IKabs(j12eval[1]) < 0.0000010000000000
36369 || IKabs(j12eval[2]) < 0.0000010000000000)
36370 {
36371 {
36372 IkReal evalcond[12];
36373 bool bgotonextstatement = true;
36374 do
36375 {
36376 IkReal x479 = ((1.0) * cj11);
36377 IkReal x480 = ((((-1.0) * x479)) + new_r22);
36378 IkReal x481 = ((1.0) * sj11);
36379 IkReal x482 = ((((-1.0) * x481)) + new_r12);
36380 evalcond[0]
36381 = ((-3.14159265358979)
36382 + (IKfmod(
36383 ((3.14159265358979)
36384 + (IKabs(
36385 ((-1.5707963267949) + j10)))),
36386 6.28318530717959)));
36387 evalcond[1] = x480;
36388 evalcond[2] = x480;
36389 evalcond[3] = new_r02;
36390 evalcond[4] = x482;
36391 evalcond[5] = x482;
36392 evalcond[6]
36393 = ((((-1.0) * new_r22 * x481))
36394 + ((cj11 * new_r12)));
36395 evalcond[7]
36396 = ((((-1.0) * new_r10 * x481))
36397 + (((-1.0) * new_r20 * x479)));
36398 evalcond[8]
36399 = ((((-1.0) * new_r11 * x481))
36400 + (((-1.0) * new_r21 * x479)));
36401 evalcond[9]
36402 = ((1.0) + (((-1.0) * new_r12 * x481))
36403 + (((-1.0) * new_r22 * x479)));
36404 if (IKabs(evalcond[0]) < 0.0000010000000000
36405 && IKabs(evalcond[1]) < 0.0000010000000000
36406 && IKabs(evalcond[2]) < 0.0000010000000000
36407 && IKabs(evalcond[3]) < 0.0000010000000000
36408 && IKabs(evalcond[4]) < 0.0000010000000000
36409 && IKabs(evalcond[5]) < 0.0000010000000000
36410 && IKabs(evalcond[6]) < 0.0000010000000000
36411 && IKabs(evalcond[7]) < 0.0000010000000000
36412 && IKabs(evalcond[8]) < 0.0000010000000000
36413 && IKabs(evalcond[9])
36414 < 0.0000010000000000)
36415 {
36416 bgotonextstatement = false;
36417 {
36418 IkReal j12array[1], cj12array[1],
36419 sj12array[1];
36420 bool j12valid[1] = {false};
36421 _nj12 = 1;
36422 CheckValue<IkReal> x483
36423 = IKPowWithIntegerCheck(
36424 IKsign(new_r12), -1);
36425 if (!x483.valid)
36426 {
36427 continue;
36428 }
36429 CheckValue<IkReal> x484
36430 = IKatan2WithCheck(
36431 IkReal(new_r21),
36432 ((-1.0) * (((1.0) * new_r20))),
36433 IKFAST_ATAN2_MAGTHRESH);
36434 if (!x484.valid)
36435 {
36436 continue;
36437 }
36438 j12array[0]
36439 = ((-1.5707963267949)
36440 + (((1.5707963267949)
36441 * (x483.value)))
36442 + (x484.value));
36443 sj12array[0] = IKsin(j12array[0]);
36444 cj12array[0] = IKcos(j12array[0]);
36445 if (j12array[0] > IKPI)
36446 {
36447 j12array[0] -= IK2PI;
36448 }
36449 else if (j12array[0] < -IKPI)
36450 {
36451 j12array[0] += IK2PI;
36452 }
36453 j12valid[0] = true;
36454 for (int ij12 = 0; ij12 < 1; ++ij12)
36455 {
36456 if (!j12valid[ij12])
36457 {
36458 continue;
36459 }
36460 _ij12[0] = ij12;
36461 _ij12[1] = -1;
36462 for (int iij12 = ij12 + 1; iij12 < 1;
36463 ++iij12)
36464 {
36465 if (j12valid[iij12]
36466 && IKabs(
36467 cj12array[ij12]
36468 - cj12array[iij12])
36469 < IKFAST_SOLUTION_THRESH
36470 && IKabs(
36471 sj12array[ij12]
36472 - sj12array[iij12])
36473 < IKFAST_SOLUTION_THRESH)
36474 {
36475 j12valid[iij12] = false;
36476 _ij12[1] = iij12;
36477 break;
36478 }
36479 }
36480 j12 = j12array[ij12];
36481 cj12 = cj12array[ij12];
36482 sj12 = sj12array[ij12];
36483 {
36484 IkReal evalcond[8];
36485 IkReal x485 = IKcos(j12);
36486 IkReal x486 = IKsin(j12);
36487 IkReal x487 = ((1.0) * new_r12);
36488 IkReal x488 = ((1.0) * x485);
36489 IkReal x489 = ((-1.0) * x488);
36490 evalcond[0]
36491 = (((new_r12 * x485)) + new_r20);
36492 evalcond[1]
36493 = (((new_r22 * x486)) + new_r11);
36494 evalcond[2]
36495 = ((((-1.0) * x486 * x487))
36496 + new_r21);
36497 evalcond[3]
36498 = ((((-1.0) * new_r22 * x488))
36499 + new_r10);
36500 evalcond[4]
36501 = ((((-1.0) * (1.0) * new_r00))
36502 + (((-1.0) * x486)));
36503 evalcond[5]
36504 = ((((-1.0) * (1.0) * new_r01))
36505 + x489);
36506 evalcond[6]
36507 = (((new_r11 * new_r22))
36508 + (((-1.0) * new_r21 * x487))
36509 + x486);
36510 evalcond[7]
36511 = ((((-1.0) * new_r20 * x487))
36512 + ((new_r10 * new_r22))
36513 + x489);
36514 if (IKabs(evalcond[0])
36515 > IKFAST_EVALCOND_THRESH
36516 || IKabs(evalcond[1])
36517 > IKFAST_EVALCOND_THRESH
36518 || IKabs(evalcond[2])
36519 > IKFAST_EVALCOND_THRESH
36520 || IKabs(evalcond[3])
36521 > IKFAST_EVALCOND_THRESH
36522 || IKabs(evalcond[4])
36523 > IKFAST_EVALCOND_THRESH
36524 || IKabs(evalcond[5])
36525 > IKFAST_EVALCOND_THRESH
36526 || IKabs(evalcond[6])
36527 > IKFAST_EVALCOND_THRESH
36528 || IKabs(evalcond[7])
36529 > IKFAST_EVALCOND_THRESH)
36530 {
36531 continue;
36532 }
36533 }
36534
36535 {
36536 std::vector<
36537 IkSingleDOFSolutionBase<IkReal> >
36538 vinfos(7);
36539 vinfos[0].jointtype = 1;
36540 vinfos[0].foffset = j4;
36541 vinfos[0].indices[0] = _ij4[0];
36542 vinfos[0].indices[1] = _ij4[1];
36543 vinfos[0].maxsolutions = _nj4;
36544 vinfos[1].jointtype = 1;
36545 vinfos[1].foffset = j6;
36546 vinfos[1].indices[0] = _ij6[0];
36547 vinfos[1].indices[1] = _ij6[1];
36548 vinfos[1].maxsolutions = _nj6;
36549 vinfos[2].jointtype = 1;
36550 vinfos[2].foffset = j8;
36551 vinfos[2].indices[0] = _ij8[0];
36552 vinfos[2].indices[1] = _ij8[1];
36553 vinfos[2].maxsolutions = _nj8;
36554 vinfos[3].jointtype = 1;
36555 vinfos[3].foffset = j9;
36556 vinfos[3].indices[0] = _ij9[0];
36557 vinfos[3].indices[1] = _ij9[1];
36558 vinfos[3].maxsolutions = _nj9;
36559 vinfos[4].jointtype = 1;
36560 vinfos[4].foffset = j10;
36561 vinfos[4].indices[0] = _ij10[0];
36562 vinfos[4].indices[1] = _ij10[1];
36563 vinfos[4].maxsolutions = _nj10;
36564 vinfos[5].jointtype = 1;
36565 vinfos[5].foffset = j11;
36566 vinfos[5].indices[0] = _ij11[0];
36567 vinfos[5].indices[1] = _ij11[1];
36568 vinfos[5].maxsolutions = _nj11;
36569 vinfos[6].jointtype = 1;
36570 vinfos[6].foffset = j12;
36571 vinfos[6].indices[0] = _ij12[0];
36572 vinfos[6].indices[1] = _ij12[1];
36573 vinfos[6].maxsolutions = _nj12;
36574 std::vector<int> vfree(0);
36575 solutions.AddSolution(vinfos, vfree);
36576 }
36577 }
36578 }
36579 }
36580 } while (0);
36581 if (bgotonextstatement)
36582 {
36583 bool bgotonextstatement = true;
36584 do
36585 {
36586 IkReal x490 = ((1.0) * cj11);
36587 IkReal x491 = ((((-1.0) * x490)) + new_r22);
36588 IkReal x492 = ((1.0) * sj11);
36589 evalcond[0]
36590 = ((-3.14159265358979)
36591 + (IKfmod(
36592 ((3.14159265358979)
36593 + (IKabs((
36594 (1.5707963267949) + j10)))),
36595 6.28318530717959)));
36596 evalcond[1] = x491;
36597 evalcond[2] = x491;
36598 evalcond[3] = new_r02;
36599 evalcond[4] = (sj11 + new_r12);
36600 evalcond[5]
36601 = ((((-1.0) * (1.0) * new_r12))
36602 + (((-1.0) * x492)));
36603 evalcond[6]
36604 = ((((-1.0) * new_r22 * x492))
36605 + (((-1.0) * new_r12 * x490)));
36606 evalcond[7]
36607 = (((new_r10 * sj11))
36608 + (((-1.0) * new_r20 * x490)));
36609 evalcond[8]
36610 = ((((-1.0) * new_r21 * x490))
36611 + ((new_r11 * sj11)));
36612 evalcond[9]
36613 = ((1.0) + (((-1.0) * new_r22 * x490))
36614 + ((new_r12 * sj11)));
36615 if (IKabs(evalcond[0]) < 0.0000010000000000
36616 && IKabs(evalcond[1])
36617 < 0.0000010000000000
36618 && IKabs(evalcond[2])
36619 < 0.0000010000000000
36620 && IKabs(evalcond[3])
36621 < 0.0000010000000000
36622 && IKabs(evalcond[4])
36623 < 0.0000010000000000
36624 && IKabs(evalcond[5])
36625 < 0.0000010000000000
36626 && IKabs(evalcond[6])
36627 < 0.0000010000000000
36628 && IKabs(evalcond[7])
36629 < 0.0000010000000000
36630 && IKabs(evalcond[8])
36631 < 0.0000010000000000
36632 && IKabs(evalcond[9])
36633 < 0.0000010000000000)
36634 {
36635 bgotonextstatement = false;
36636 {
36637 IkReal j12array[1], cj12array[1],
36638 sj12array[1];
36639 bool j12valid[1] = {false};
36640 _nj12 = 1;
36641 if (IKabs(new_r00)
36642 < IKFAST_ATAN2_MAGTHRESH
36643 && IKabs(new_r01)
36644 < IKFAST_ATAN2_MAGTHRESH
36645 && IKabs(
36646 IKsqr(new_r00)
36647 + IKsqr(new_r01) - 1)
36648 <= IKFAST_SINCOS_THRESH)
36649 continue;
36650 j12array[0] = IKatan2(new_r00, new_r01);
36651 sj12array[0] = IKsin(j12array[0]);
36652 cj12array[0] = IKcos(j12array[0]);
36653 if (j12array[0] > IKPI)
36654 {
36655 j12array[0] -= IK2PI;
36656 }
36657 else if (j12array[0] < -IKPI)
36658 {
36659 j12array[0] += IK2PI;
36660 }
36661 j12valid[0] = true;
36662 for (int ij12 = 0; ij12 < 1; ++ij12)
36663 {
36664 if (!j12valid[ij12])
36665 {
36666 continue;
36667 }
36668 _ij12[0] = ij12;
36669 _ij12[1] = -1;
36670 for (int iij12 = ij12 + 1; iij12 < 1;
36671 ++iij12)
36672 {
36673 if (j12valid[iij12]
36674 && IKabs(
36675 cj12array[ij12]
36676 - cj12array[iij12])
36677 < IKFAST_SOLUTION_THRESH
36678 && IKabs(
36679 sj12array[ij12]
36680 - sj12array[iij12])
36681 < IKFAST_SOLUTION_THRESH)
36682 {
36683 j12valid[iij12] = false;
36684 _ij12[1] = iij12;
36685 break;
36686 }
36687 }
36688 j12 = j12array[ij12];
36689 cj12 = cj12array[ij12];
36690 sj12 = sj12array[ij12];
36691 {
36692 IkReal evalcond[8];
36693 IkReal x493 = IKsin(j12);
36694 IkReal x494
36695 = ((1.0) * (IKcos(j12)));
36696 IkReal x495 = ((-1.0) * x494);
36697 IkReal x496 = ((1.0) * new_r11);
36698 IkReal x497 = ((1.0) * new_r10);
36699 evalcond[0]
36700 = (((new_r12 * x493))
36701 + new_r21);
36702 evalcond[1]
36703 = (new_r00 + (((-1.0) * x493)));
36704 evalcond[2] = (new_r01 + x495);
36705 evalcond[3]
36706 = ((((-1.0) * new_r12 * x494))
36707 + new_r20);
36708 evalcond[4]
36709 = (((new_r22 * x493))
36710 + (((-1.0) * x496)));
36711 evalcond[5]
36712 = ((((-1.0) * new_r22 * x494))
36713 + (((-1.0) * x497)));
36714 evalcond[6]
36715 = (((new_r12 * new_r21))
36716 + (((-1.0) * new_r22 * x496))
36717 + x493);
36718 evalcond[7]
36719 = (((new_r12 * new_r20))
36720 + (((-1.0) * new_r22 * x497))
36721 + x495);
36722 if (IKabs(evalcond[0])
36723 > IKFAST_EVALCOND_THRESH
36724 || IKabs(evalcond[1])
36725 > IKFAST_EVALCOND_THRESH
36726 || IKabs(evalcond[2])
36727 > IKFAST_EVALCOND_THRESH
36728 || IKabs(evalcond[3])
36729 > IKFAST_EVALCOND_THRESH
36730 || IKabs(evalcond[4])
36731 > IKFAST_EVALCOND_THRESH
36732 || IKabs(evalcond[5])
36733 > IKFAST_EVALCOND_THRESH
36734 || IKabs(evalcond[6])
36735 > IKFAST_EVALCOND_THRESH
36736 || IKabs(evalcond[7])
36737 > IKFAST_EVALCOND_THRESH)
36738 {
36739 continue;
36740 }
36741 }
36742
36743 {
36744 std::vector<IkSingleDOFSolutionBase<
36745 IkReal> >
36746 vinfos(7);
36747 vinfos[0].jointtype = 1;
36748 vinfos[0].foffset = j4;
36749 vinfos[0].indices[0] = _ij4[0];
36750 vinfos[0].indices[1] = _ij4[1];
36751 vinfos[0].maxsolutions = _nj4;
36752 vinfos[1].jointtype = 1;
36753 vinfos[1].foffset = j6;
36754 vinfos[1].indices[0] = _ij6[0];
36755 vinfos[1].indices[1] = _ij6[1];
36756 vinfos[1].maxsolutions = _nj6;
36757 vinfos[2].jointtype = 1;
36758 vinfos[2].foffset = j8;
36759 vinfos[2].indices[0] = _ij8[0];
36760 vinfos[2].indices[1] = _ij8[1];
36761 vinfos[2].maxsolutions = _nj8;
36762 vinfos[3].jointtype = 1;
36763 vinfos[3].foffset = j9;
36764 vinfos[3].indices[0] = _ij9[0];
36765 vinfos[3].indices[1] = _ij9[1];
36766 vinfos[3].maxsolutions = _nj9;
36767 vinfos[4].jointtype = 1;
36768 vinfos[4].foffset = j10;
36769 vinfos[4].indices[0] = _ij10[0];
36770 vinfos[4].indices[1] = _ij10[1];
36771 vinfos[4].maxsolutions = _nj10;
36772 vinfos[5].jointtype = 1;
36773 vinfos[5].foffset = j11;
36774 vinfos[5].indices[0] = _ij11[0];
36775 vinfos[5].indices[1] = _ij11[1];
36776 vinfos[5].maxsolutions = _nj11;
36777 vinfos[6].jointtype = 1;
36778 vinfos[6].foffset = j12;
36779 vinfos[6].indices[0] = _ij12[0];
36780 vinfos[6].indices[1] = _ij12[1];
36781 vinfos[6].maxsolutions = _nj12;
36782 std::vector<int> vfree(0);
36783 solutions.AddSolution(
36784 vinfos, vfree);
36785 }
36786 }
36787 }
36788 }
36789 } while (0);
36790 if (bgotonextstatement)
36791 {
36792 bool bgotonextstatement = true;
36793 do
36794 {
36795 IkReal x498 = ((1.0) * cj10);
36796 IkReal x499 = ((1.0) * sj10);
36797 IkReal x500
36798 = ((((-1.0) * new_r02 * x499))
36799 + ((cj10 * new_r12)));
36800 evalcond[0]
36801 = ((-3.14159265358979)
36802 + (IKfmod(
36803 ((3.14159265358979)
36804 + (IKabs(
36805 ((-1.5707963267949)
36806 + j11)))),
36807 6.28318530717959)));
36808 evalcond[1] = new_r22;
36809 evalcond[2]
36810 = ((((-1.0) * x498)) + new_r02);
36811 evalcond[3]
36812 = ((((-1.0) * x499)) + new_r12);
36813 evalcond[4] = x500;
36814 evalcond[5] = x500;
36815 evalcond[6]
36816 = ((-1.0) + ((cj10 * new_r02))
36817 + ((new_r12 * sj10)));
36818 evalcond[7]
36819 = (((cj10 * new_r01))
36820 + ((new_r11 * sj10)));
36821 evalcond[8]
36822 = (((cj10 * new_r00))
36823 + ((new_r10 * sj10)));
36824 evalcond[9]
36825 = ((((-1.0) * new_r10 * x499))
36826 + (((-1.0) * new_r00 * x498)));
36827 evalcond[10]
36828 = ((((-1.0) * new_r11 * x499))
36829 + (((-1.0) * new_r01 * x498)));
36830 evalcond[11]
36831 = ((1.0) + (((-1.0) * new_r02 * x498))
36832 + (((-1.0) * new_r12 * x499)));
36833 if (IKabs(evalcond[0])
36834 < 0.0000010000000000
36835 && IKabs(evalcond[1])
36836 < 0.0000010000000000
36837 && IKabs(evalcond[2])
36838 < 0.0000010000000000
36839 && IKabs(evalcond[3])
36840 < 0.0000010000000000
36841 && IKabs(evalcond[4])
36842 < 0.0000010000000000
36843 && IKabs(evalcond[5])
36844 < 0.0000010000000000
36845 && IKabs(evalcond[6])
36846 < 0.0000010000000000
36847 && IKabs(evalcond[7])
36848 < 0.0000010000000000
36849 && IKabs(evalcond[8])
36850 < 0.0000010000000000
36851 && IKabs(evalcond[9])
36852 < 0.0000010000000000
36853 && IKabs(evalcond[10])
36854 < 0.0000010000000000
36855 && IKabs(evalcond[11])
36856 < 0.0000010000000000)
36857 {
36858 bgotonextstatement = false;
36859 {
36860 IkReal j12array[1], cj12array[1],
36861 sj12array[1];
36862 bool j12valid[1] = {false};
36863 _nj12 = 1;
36864 if (IKabs(new_r21)
36865 < IKFAST_ATAN2_MAGTHRESH
36866 && IKabs(
36867 ((-1.0)
36868 * (((1.0) * new_r20))))
36869 < IKFAST_ATAN2_MAGTHRESH
36870 && IKabs(
36871 IKsqr(new_r21)
36872 + IKsqr((
36873 (-1.0)
36874 * (((1.0) * new_r20))))
36875 - 1)
36876 <= IKFAST_SINCOS_THRESH)
36877 continue;
36878 j12array[0] = IKatan2(
36879 new_r21,
36880 ((-1.0) * (((1.0) * new_r20))));
36881 sj12array[0] = IKsin(j12array[0]);
36882 cj12array[0] = IKcos(j12array[0]);
36883 if (j12array[0] > IKPI)
36884 {
36885 j12array[0] -= IK2PI;
36886 }
36887 else if (j12array[0] < -IKPI)
36888 {
36889 j12array[0] += IK2PI;
36890 }
36891 j12valid[0] = true;
36892 for (int ij12 = 0; ij12 < 1; ++ij12)
36893 {
36894 if (!j12valid[ij12])
36895 {
36896 continue;
36897 }
36898 _ij12[0] = ij12;
36899 _ij12[1] = -1;
36900 for (int iij12 = ij12 + 1;
36901 iij12 < 1;
36902 ++iij12)
36903 {
36904 if (j12valid[iij12]
36905 && IKabs(
36906 cj12array[ij12]
36907 - cj12array[iij12])
36908 < IKFAST_SOLUTION_THRESH
36909 && IKabs(
36910 sj12array[ij12]
36911 - sj12array[iij12])
36912 < IKFAST_SOLUTION_THRESH)
36913 {
36914 j12valid[iij12] = false;
36915 _ij12[1] = iij12;
36916 break;
36917 }
36918 }
36919 j12 = j12array[ij12];
36920 cj12 = cj12array[ij12];
36921 sj12 = sj12array[ij12];
36922 {
36923 IkReal evalcond[8];
36924 IkReal x501 = IKcos(j12);
36925 IkReal x502 = IKsin(j12);
36926 IkReal x503 = ((1.0) * x502);
36927 IkReal x504 = ((-1.0) * x503);
36928 IkReal x505 = ((1.0) * x501);
36929 IkReal x506 = ((1.0) * new_r12);
36930 evalcond[0] = (x501 + new_r20);
36931 evalcond[1] = (x504 + new_r21);
36932 evalcond[2]
36933 = (((new_r12 * x501))
36934 + new_r01);
36935 evalcond[3]
36936 = (((new_r12 * x502))
36937 + new_r00);
36938 evalcond[4]
36939 = ((((-1.0) * new_r02 * x505))
36940 + new_r11);
36941 evalcond[5]
36942 = ((((-1.0) * new_r02 * x503))
36943 + new_r10);
36944 evalcond[6]
36945 = ((((-1.0) * new_r00 * x506))
36946 + x504
36947 + ((new_r02 * new_r10)));
36948 evalcond[7]
36949 = ((((-1.0) * new_r01 * x506))
36950 + ((new_r02 * new_r11))
36951 + (((-1.0) * x505)));
36952 if (IKabs(evalcond[0])
36953 > IKFAST_EVALCOND_THRESH
36954 || IKabs(evalcond[1])
36955 > IKFAST_EVALCOND_THRESH
36956 || IKabs(evalcond[2])
36957 > IKFAST_EVALCOND_THRESH
36958 || IKabs(evalcond[3])
36959 > IKFAST_EVALCOND_THRESH
36960 || IKabs(evalcond[4])
36961 > IKFAST_EVALCOND_THRESH
36962 || IKabs(evalcond[5])
36963 > IKFAST_EVALCOND_THRESH
36964 || IKabs(evalcond[6])
36965 > IKFAST_EVALCOND_THRESH
36966 || IKabs(evalcond[7])
36967 > IKFAST_EVALCOND_THRESH)
36968 {
36969 continue;
36970 }
36971 }
36972
36973 {
36974 std::vector<
36975 IkSingleDOFSolutionBase<
36976 IkReal> >
36977 vinfos(7);
36978 vinfos[0].jointtype = 1;
36979 vinfos[0].foffset = j4;
36980 vinfos[0].indices[0] = _ij4[0];
36981 vinfos[0].indices[1] = _ij4[1];
36982 vinfos[0].maxsolutions = _nj4;
36983 vinfos[1].jointtype = 1;
36984 vinfos[1].foffset = j6;
36985 vinfos[1].indices[0] = _ij6[0];
36986 vinfos[1].indices[1] = _ij6[1];
36987 vinfos[1].maxsolutions = _nj6;
36988 vinfos[2].jointtype = 1;
36989 vinfos[2].foffset = j8;
36990 vinfos[2].indices[0] = _ij8[0];
36991 vinfos[2].indices[1] = _ij8[1];
36992 vinfos[2].maxsolutions = _nj8;
36993 vinfos[3].jointtype = 1;
36994 vinfos[3].foffset = j9;
36995 vinfos[3].indices[0] = _ij9[0];
36996 vinfos[3].indices[1] = _ij9[1];
36997 vinfos[3].maxsolutions = _nj9;
36998 vinfos[4].jointtype = 1;
36999 vinfos[4].foffset = j10;
37000 vinfos[4].indices[0] = _ij10[0];
37001 vinfos[4].indices[1] = _ij10[1];
37002 vinfos[4].maxsolutions = _nj10;
37003 vinfos[5].jointtype = 1;
37004 vinfos[5].foffset = j11;
37005 vinfos[5].indices[0] = _ij11[0];
37006 vinfos[5].indices[1] = _ij11[1];
37007 vinfos[5].maxsolutions = _nj11;
37008 vinfos[6].jointtype = 1;
37009 vinfos[6].foffset = j12;
37010 vinfos[6].indices[0] = _ij12[0];
37011 vinfos[6].indices[1] = _ij12[1];
37012 vinfos[6].maxsolutions = _nj12;
37013 std::vector<int> vfree(0);
37014 solutions.AddSolution(
37015 vinfos, vfree);
37016 }
37017 }
37018 }
37019 }
37020 } while (0);
37021 if (bgotonextstatement)
37022 {
37023 bool bgotonextstatement = true;
37024 do
37025 {
37026 IkReal x507
37027 = ((((-1.0) * (1.0) * new_r02
37028 * sj10))
37029 + ((cj10 * new_r12)));
37030 IkReal x508
37031 = ((1.0) + ((cj10 * new_r02))
37032 + ((new_r12 * sj10)));
37033 IkReal x509
37034 = (((cj10 * new_r01))
37035 + ((new_r11 * sj10)));
37036 IkReal x510
37037 = (((cj10 * new_r00))
37038 + ((new_r10 * sj10)));
37039 evalcond[0]
37040 = ((-3.14159265358979)
37041 + (IKfmod(
37042 ((3.14159265358979)
37043 + (IKabs(
37044 ((1.5707963267949)
37045 + j11)))),
37046 6.28318530717959)));
37047 evalcond[1] = new_r22;
37048 evalcond[2] = (cj10 + new_r02);
37049 evalcond[3] = (sj10 + new_r12);
37050 evalcond[4] = x507;
37051 evalcond[5] = x507;
37052 evalcond[6] = x508;
37053 evalcond[7] = x509;
37054 evalcond[8] = x510;
37055 evalcond[9] = x510;
37056 evalcond[10] = x509;
37057 evalcond[11] = x508;
37058 if (IKabs(evalcond[0])
37059 < 0.0000010000000000
37060 && IKabs(evalcond[1])
37061 < 0.0000010000000000
37062 && IKabs(evalcond[2])
37063 < 0.0000010000000000
37064 && IKabs(evalcond[3])
37065 < 0.0000010000000000
37066 && IKabs(evalcond[4])
37067 < 0.0000010000000000
37068 && IKabs(evalcond[5])
37069 < 0.0000010000000000
37070 && IKabs(evalcond[6])
37071 < 0.0000010000000000
37072 && IKabs(evalcond[7])
37073 < 0.0000010000000000
37074 && IKabs(evalcond[8])
37075 < 0.0000010000000000
37076 && IKabs(evalcond[9])
37077 < 0.0000010000000000
37078 && IKabs(evalcond[10])
37079 < 0.0000010000000000
37080 && IKabs(evalcond[11])
37081 < 0.0000010000000000)
37082 {
37083 bgotonextstatement = false;
37084 {
37085 IkReal j12array[1], cj12array[1],
37086 sj12array[1];
37087 bool j12valid[1] = {false};
37088 _nj12 = 1;
37089 if (IKabs(
37090 ((-1.0)
37091 * (((1.0) * new_r21))))
37092 < IKFAST_ATAN2_MAGTHRESH
37093 && IKabs(new_r20)
37094 < IKFAST_ATAN2_MAGTHRESH
37095 && IKabs(
37096 IKsqr(
37097 ((-1.0)
37098 * (((1.0)
37099 * new_r21))))
37100 + IKsqr(new_r20) - 1)
37101 <= IKFAST_SINCOS_THRESH)
37102 continue;
37103 j12array[0] = IKatan2(
37104 ((-1.0) * (((1.0) * new_r21))),
37105 new_r20);
37106 sj12array[0] = IKsin(j12array[0]);
37107 cj12array[0] = IKcos(j12array[0]);
37108 if (j12array[0] > IKPI)
37109 {
37110 j12array[0] -= IK2PI;
37111 }
37112 else if (j12array[0] < -IKPI)
37113 {
37114 j12array[0] += IK2PI;
37115 }
37116 j12valid[0] = true;
37117 for (int ij12 = 0; ij12 < 1; ++ij12)
37118 {
37119 if (!j12valid[ij12])
37120 {
37121 continue;
37122 }
37123 _ij12[0] = ij12;
37124 _ij12[1] = -1;
37125 for (int iij12 = ij12 + 1;
37126 iij12 < 1;
37127 ++iij12)
37128 {
37129 if (j12valid[iij12]
37130 && IKabs(
37131 cj12array[ij12]
37132 - cj12array[iij12])
37133 < IKFAST_SOLUTION_THRESH
37134 && IKabs(
37135 sj12array[ij12]
37136 - sj12array[iij12])
37137 < IKFAST_SOLUTION_THRESH)
37138 {
37139 j12valid[iij12] = false;
37140 _ij12[1] = iij12;
37141 break;
37142 }
37143 }
37144 j12 = j12array[ij12];
37145 cj12 = cj12array[ij12];
37146 sj12 = sj12array[ij12];
37147 {
37148 IkReal evalcond[8];
37149 IkReal x511 = IKsin(j12);
37150 IkReal x512 = IKcos(j12);
37151 IkReal x513 = ((1.0) * x512);
37152 IkReal x514 = ((-1.0) * x513);
37153 IkReal x515 = ((1.0) * x511);
37154 IkReal x516 = ((1.0) * new_r02);
37155 evalcond[0] = (x511 + new_r21);
37156 evalcond[1] = (x514 + new_r20);
37157 evalcond[2]
37158 = (((new_r02 * x512))
37159 + new_r11);
37160 evalcond[3]
37161 = (((new_r02 * x511))
37162 + new_r10);
37163 evalcond[4]
37164 = ((((-1.0) * new_r12
37165 * x513))
37166 + new_r01);
37167 evalcond[5]
37168 = ((((-1.0) * new_r12
37169 * x515))
37170 + new_r00);
37171 evalcond[6]
37172 = ((((-1.0) * new_r10
37173 * x516))
37174 + ((new_r00 * new_r12))
37175 + (((-1.0) * x515)));
37176 evalcond[7]
37177 = ((((-1.0) * new_r11
37178 * x516))
37179 + ((new_r01 * new_r12))
37180 + x514);
37181 if (IKabs(evalcond[0])
37182 > IKFAST_EVALCOND_THRESH
37183 || IKabs(evalcond[1])
37184 > IKFAST_EVALCOND_THRESH
37185 || IKabs(evalcond[2])
37186 > IKFAST_EVALCOND_THRESH
37187 || IKabs(evalcond[3])
37188 > IKFAST_EVALCOND_THRESH
37189 || IKabs(evalcond[4])
37190 > IKFAST_EVALCOND_THRESH
37191 || IKabs(evalcond[5])
37192 > IKFAST_EVALCOND_THRESH
37193 || IKabs(evalcond[6])
37194 > IKFAST_EVALCOND_THRESH
37195 || IKabs(evalcond[7])
37196 > IKFAST_EVALCOND_THRESH)
37197 {
37198 continue;
37199 }
37200 }
37201
37202 {
37203 std::vector<
37204 IkSingleDOFSolutionBase<
37205 IkReal> >
37206 vinfos(7);
37207 vinfos[0].jointtype = 1;
37208 vinfos[0].foffset = j4;
37209 vinfos[0].indices[0] = _ij4[0];
37210 vinfos[0].indices[1] = _ij4[1];
37211 vinfos[0].maxsolutions = _nj4;
37212 vinfos[1].jointtype = 1;
37213 vinfos[1].foffset = j6;
37214 vinfos[1].indices[0] = _ij6[0];
37215 vinfos[1].indices[1] = _ij6[1];
37216 vinfos[1].maxsolutions = _nj6;
37217 vinfos[2].jointtype = 1;
37218 vinfos[2].foffset = j8;
37219 vinfos[2].indices[0] = _ij8[0];
37220 vinfos[2].indices[1] = _ij8[1];
37221 vinfos[2].maxsolutions = _nj8;
37222 vinfos[3].jointtype = 1;
37223 vinfos[3].foffset = j9;
37224 vinfos[3].indices[0] = _ij9[0];
37225 vinfos[3].indices[1] = _ij9[1];
37226 vinfos[3].maxsolutions = _nj9;
37227 vinfos[4].jointtype = 1;
37228 vinfos[4].foffset = j10;
37229 vinfos[4].indices[0] = _ij10[0];
37230 vinfos[4].indices[1] = _ij10[1];
37231 vinfos[4].maxsolutions = _nj10;
37232 vinfos[5].jointtype = 1;
37233 vinfos[5].foffset = j11;
37234 vinfos[5].indices[0] = _ij11[0];
37235 vinfos[5].indices[1] = _ij11[1];
37236 vinfos[5].maxsolutions = _nj11;
37237 vinfos[6].jointtype = 1;
37238 vinfos[6].foffset = j12;
37239 vinfos[6].indices[0] = _ij12[0];
37240 vinfos[6].indices[1] = _ij12[1];
37241 vinfos[6].maxsolutions = _nj12;
37242 std::vector<int> vfree(0);
37243 solutions.AddSolution(
37244 vinfos, vfree);
37245 }
37246 }
37247 }
37248 }
37249 } while (0);
37250 if (bgotonextstatement)
37251 {
37252 bool bgotonextstatement = true;
37253 do
37254 {
37255 IkReal x517
37256 = ((((-1.0) * (1.0) * new_r02
37257 * sj10))
37258 + ((cj10 * new_r12)));
37259 IkReal x518
37260 = (((cj10 * new_r02))
37261 + ((new_r12 * sj10)));
37262 evalcond[0]
37263 = ((-3.14159265358979)
37264 + (IKfmod(
37265 ((3.14159265358979)
37266 + (IKabs(j11))),
37267 6.28318530717959)));
37268 evalcond[1] = ((-1.0) + new_r22);
37269 evalcond[2] = new_r20;
37270 evalcond[3] = new_r02;
37271 evalcond[4] = new_r12;
37272 evalcond[5] = new_r21;
37273 evalcond[6] = x517;
37274 evalcond[7] = x517;
37275 evalcond[8] = x518;
37276 evalcond[9] = x518;
37277 if (IKabs(evalcond[0])
37278 < 0.0000010000000000
37279 && IKabs(evalcond[1])
37280 < 0.0000010000000000
37281 && IKabs(evalcond[2])
37282 < 0.0000010000000000
37283 && IKabs(evalcond[3])
37284 < 0.0000010000000000
37285 && IKabs(evalcond[4])
37286 < 0.0000010000000000
37287 && IKabs(evalcond[5])
37288 < 0.0000010000000000
37289 && IKabs(evalcond[6])
37290 < 0.0000010000000000
37291 && IKabs(evalcond[7])
37292 < 0.0000010000000000
37293 && IKabs(evalcond[8])
37294 < 0.0000010000000000
37295 && IKabs(evalcond[9])
37296 < 0.0000010000000000)
37297 {
37298 bgotonextstatement = false;
37299 {
37300 IkReal j12array[1], cj12array[1],
37301 sj12array[1];
37302 bool j12valid[1] = {false};
37303 _nj12 = 1;
37304 IkReal x519 = ((1.0) * new_r01);
37305 if (IKabs(
37306 ((((-1.0) * cj10 * x519))
37307 + (((-1.0) * (1.0)
37308 * new_r00 * sj10))))
37309 < IKFAST_ATAN2_MAGTHRESH
37310 && IKabs(
37311 (((cj10 * new_r00))
37312 + (((-1.0) * sj10
37313 * x519))))
37314 < IKFAST_ATAN2_MAGTHRESH
37315 && IKabs(
37316 IKsqr(
37317 ((((-1.0) * cj10
37318 * x519))
37319 + (((-1.0) * (1.0)
37320 * new_r00
37321 * sj10))))
37322 + IKsqr(
37323 (((cj10 * new_r00))
37324 + (((-1.0) * sj10
37325 * x519))))
37326 - 1)
37327 <= IKFAST_SINCOS_THRESH)
37328 continue;
37329 j12array[0] = IKatan2(
37330 ((((-1.0) * cj10 * x519))
37331 + (((-1.0) * (1.0) * new_r00
37332 * sj10))),
37333 (((cj10 * new_r00))
37334 + (((-1.0) * sj10 * x519))));
37335 sj12array[0] = IKsin(j12array[0]);
37336 cj12array[0] = IKcos(j12array[0]);
37337 if (j12array[0] > IKPI)
37338 {
37339 j12array[0] -= IK2PI;
37340 }
37341 else if (j12array[0] < -IKPI)
37342 {
37343 j12array[0] += IK2PI;
37344 }
37345 j12valid[0] = true;
37346 for (int ij12 = 0; ij12 < 1;
37347 ++ij12)
37348 {
37349 if (!j12valid[ij12])
37350 {
37351 continue;
37352 }
37353 _ij12[0] = ij12;
37354 _ij12[1] = -1;
37355 for (int iij12 = ij12 + 1;
37356 iij12 < 1;
37357 ++iij12)
37358 {
37359 if (j12valid[iij12]
37360 && IKabs(
37361 cj12array[ij12]
37362 - cj12array[iij12])
37363 < IKFAST_SOLUTION_THRESH
37364 && IKabs(
37365 sj12array[ij12]
37366 - sj12array[iij12])
37367 < IKFAST_SOLUTION_THRESH)
37368 {
37369 j12valid[iij12] = false;
37370 _ij12[1] = iij12;
37371 break;
37372 }
37373 }
37374 j12 = j12array[ij12];
37375 cj12 = cj12array[ij12];
37376 sj12 = sj12array[ij12];
37377 {
37378 IkReal evalcond[8];
37379 IkReal x520 = IKsin(j12);
37380 IkReal x521 = (cj10 * x520);
37381 IkReal x522 = IKcos(j12);
37382 IkReal x523 = ((1.0) * x522);
37383 IkReal x524 = ((-1.0) * x523);
37384 IkReal x525 = ((1.0) * sj10);
37385 IkReal x526
37386 = ((((-1.0) * cj10
37387 * x523))
37388 + ((sj10 * x520)));
37389 evalcond[0]
37390 = (((cj10 * new_r01))
37391 + ((new_r11 * sj10))
37392 + x520);
37393 evalcond[1]
37394 = (((sj10 * x522))
37395 + new_r01 + x521);
37396 evalcond[2]
37397 = (((cj10 * new_r00))
37398 + ((new_r10 * sj10))
37399 + x524);
37400 evalcond[3]
37401 = (((cj10 * new_r10))
37402 + (((-1.0) * new_r00
37403 * x525))
37404 + (((-1.0) * x520)));
37405 evalcond[4]
37406 = (((cj10 * new_r11))
37407 + (((-1.0) * new_r01
37408 * x525))
37409 + x524);
37410 evalcond[5]
37411 = (new_r00 + x526);
37412 evalcond[6]
37413 = (new_r11 + x526);
37414 evalcond[7]
37415 = ((((-1.0) * x521))
37416 + (((-1.0) * x522
37417 * x525))
37418 + new_r10);
37419 if (IKabs(evalcond[0])
37420 > IKFAST_EVALCOND_THRESH
37421 || IKabs(evalcond[1])
37422 > IKFAST_EVALCOND_THRESH
37423 || IKabs(evalcond[2])
37424 > IKFAST_EVALCOND_THRESH
37425 || IKabs(evalcond[3])
37426 > IKFAST_EVALCOND_THRESH
37427 || IKabs(evalcond[4])
37428 > IKFAST_EVALCOND_THRESH
37429 || IKabs(evalcond[5])
37430 > IKFAST_EVALCOND_THRESH
37431 || IKabs(evalcond[6])
37432 > IKFAST_EVALCOND_THRESH
37433 || IKabs(evalcond[7])
37434 > IKFAST_EVALCOND_THRESH)
37435 {
37436 continue;
37437 }
37438 }
37439
37440 {
37441 std::vector<
37442 IkSingleDOFSolutionBase<
37443 IkReal> >
37444 vinfos(7);
37445 vinfos[0].jointtype = 1;
37446 vinfos[0].foffset = j4;
37447 vinfos[0].indices[0]
37448 = _ij4[0];
37449 vinfos[0].indices[1]
37450 = _ij4[1];
37451 vinfos[0].maxsolutions = _nj4;
37452 vinfos[1].jointtype = 1;
37453 vinfos[1].foffset = j6;
37454 vinfos[1].indices[0]
37455 = _ij6[0];
37456 vinfos[1].indices[1]
37457 = _ij6[1];
37458 vinfos[1].maxsolutions = _nj6;
37459 vinfos[2].jointtype = 1;
37460 vinfos[2].foffset = j8;
37461 vinfos[2].indices[0]
37462 = _ij8[0];
37463 vinfos[2].indices[1]
37464 = _ij8[1];
37465 vinfos[2].maxsolutions = _nj8;
37466 vinfos[3].jointtype = 1;
37467 vinfos[3].foffset = j9;
37468 vinfos[3].indices[0]
37469 = _ij9[0];
37470 vinfos[3].indices[1]
37471 = _ij9[1];
37472 vinfos[3].maxsolutions = _nj9;
37473 vinfos[4].jointtype = 1;
37474 vinfos[4].foffset = j10;
37475 vinfos[4].indices[0]
37476 = _ij10[0];
37477 vinfos[4].indices[1]
37478 = _ij10[1];
37479 vinfos[4].maxsolutions
37480 = _nj10;
37481 vinfos[5].jointtype = 1;
37482 vinfos[5].foffset = j11;
37483 vinfos[5].indices[0]
37484 = _ij11[0];
37485 vinfos[5].indices[1]
37486 = _ij11[1];
37487 vinfos[5].maxsolutions
37488 = _nj11;
37489 vinfos[6].jointtype = 1;
37490 vinfos[6].foffset = j12;
37491 vinfos[6].indices[0]
37492 = _ij12[0];
37493 vinfos[6].indices[1]
37494 = _ij12[1];
37495 vinfos[6].maxsolutions
37496 = _nj12;
37497 std::vector<int> vfree(0);
37498 solutions.AddSolution(
37499 vinfos, vfree);
37500 }
37501 }
37502 }
37503 }
37504 } while (0);
37505 if (bgotonextstatement)
37506 {
37507 bool bgotonextstatement = true;
37508 do
37509 {
37510 IkReal x527
37511 = ((((-1.0) * (1.0) * new_r02
37512 * sj10))
37513 + ((cj10 * new_r12)));
37514 IkReal x528 = (cj10 * new_r02);
37515 IkReal x529 = (new_r12 * sj10);
37516 evalcond[0]
37517 = ((-3.14159265358979)
37518 + (IKfmod(
37519 ((3.14159265358979)
37520 + (IKabs(
37521 ((-3.14159265358979)
37522 + j11)))),
37523 6.28318530717959)));
37524 evalcond[1] = ((1.0) + new_r22);
37525 evalcond[2] = new_r20;
37526 evalcond[3] = new_r02;
37527 evalcond[4] = new_r12;
37528 evalcond[5] = new_r21;
37529 evalcond[6] = x527;
37530 evalcond[7] = x527;
37531 evalcond[8] = (x529 + x528);
37532 evalcond[9]
37533 = ((((-1.0) * x528))
37534 + (((-1.0) * x529)));
37535 if (IKabs(evalcond[0])
37536 < 0.0000010000000000
37537 && IKabs(evalcond[1])
37538 < 0.0000010000000000
37539 && IKabs(evalcond[2])
37540 < 0.0000010000000000
37541 && IKabs(evalcond[3])
37542 < 0.0000010000000000
37543 && IKabs(evalcond[4])
37544 < 0.0000010000000000
37545 && IKabs(evalcond[5])
37546 < 0.0000010000000000
37547 && IKabs(evalcond[6])
37548 < 0.0000010000000000
37549 && IKabs(evalcond[7])
37550 < 0.0000010000000000
37551 && IKabs(evalcond[8])
37552 < 0.0000010000000000
37553 && IKabs(evalcond[9])
37554 < 0.0000010000000000)
37555 {
37556 bgotonextstatement = false;
37557 {
37558 IkReal j12array[1],
37559 cj12array[1], sj12array[1];
37560 bool j12valid[1] = {false};
37561 _nj12 = 1;
37562 IkReal x530 = ((1.0) * new_r00);
37563 if (IKabs((
37564 (((-1.0) * sj10 * x530))
37565 + ((cj10 * new_r01))))
37566 < IKFAST_ATAN2_MAGTHRESH
37567 && IKabs(
37568 ((((-1.0) * cj10
37569 * x530))
37570 + (((-1.0) * (1.0)
37571 * new_r01
37572 * sj10))))
37573 < IKFAST_ATAN2_MAGTHRESH
37574 && IKabs(
37575 IKsqr((
37576 (((-1.0) * sj10
37577 * x530))
37578 + ((cj10
37579 * new_r01))))
37580 + IKsqr(
37581 ((((-1.0) * cj10
37582 * x530))
37583 + (((-1.0)
37584 * (1.0)
37585 * new_r01
37586 * sj10))))
37587 - 1)
37588 <= IKFAST_SINCOS_THRESH)
37589 continue;
37590 j12array[0] = IKatan2(
37591 ((((-1.0) * sj10 * x530))
37592 + ((cj10 * new_r01))),
37593 ((((-1.0) * cj10 * x530))
37594 + (((-1.0) * (1.0)
37595 * new_r01 * sj10))));
37596 sj12array[0]
37597 = IKsin(j12array[0]);
37598 cj12array[0]
37599 = IKcos(j12array[0]);
37600 if (j12array[0] > IKPI)
37601 {
37602 j12array[0] -= IK2PI;
37603 }
37604 else if (j12array[0] < -IKPI)
37605 {
37606 j12array[0] += IK2PI;
37607 }
37608 j12valid[0] = true;
37609 for (int ij12 = 0; ij12 < 1;
37610 ++ij12)
37611 {
37612 if (!j12valid[ij12])
37613 {
37614 continue;
37615 }
37616 _ij12[0] = ij12;
37617 _ij12[1] = -1;
37618 for (int iij12 = ij12 + 1;
37619 iij12 < 1;
37620 ++iij12)
37621 {
37622 if (j12valid[iij12]
37623 && IKabs(
37624 cj12array[ij12]
37625 - cj12array
37626 [iij12])
37627 < IKFAST_SOLUTION_THRESH
37628 && IKabs(
37629 sj12array[ij12]
37630 - sj12array
37631 [iij12])
37632 < IKFAST_SOLUTION_THRESH)
37633 {
37634 j12valid[iij12] = false;
37635 _ij12[1] = iij12;
37636 break;
37637 }
37638 }
37639 j12 = j12array[ij12];
37640 cj12 = cj12array[ij12];
37641 sj12 = sj12array[ij12];
37642 {
37643 IkReal evalcond[8];
37644 IkReal x531 = IKcos(j12);
37645 IkReal x532 = IKsin(j12);
37646 IkReal x533
37647 = ((1.0) * x532);
37648 IkReal x534
37649 = ((-1.0) * x533);
37650 IkReal x535 = (cj10 * x531);
37651 IkReal x536
37652 = ((1.0) * sj10);
37653 IkReal x537
37654 = ((((-1.0) * cj10
37655 * x533))
37656 + ((sj10 * x531)));
37657 evalcond[0]
37658 = (x531
37659 + ((cj10 * new_r00))
37660 + ((new_r10
37661 * sj10)));
37662 evalcond[1]
37663 = (x534
37664 + ((cj10 * new_r01))
37665 + ((new_r11
37666 * sj10)));
37667 evalcond[2]
37668 = (x535
37669 + ((sj10 * x532))
37670 + new_r00);
37671 evalcond[3]
37672 = (x534
37673 + ((cj10 * new_r10))
37674 + (((-1.0) * new_r00
37675 * x536)));
37676 evalcond[4]
37677 = (((cj10 * new_r11))
37678 + (((-1.0) * x531))
37679 + (((-1.0) * new_r01
37680 * x536)));
37681 evalcond[5]
37682 = (x537 + new_r01);
37683 evalcond[6]
37684 = (x537 + new_r10);
37685 evalcond[7]
37686 = ((((-1.0) * x532
37687 * x536))
37688 + new_r11
37689 + (((-1.0) * x535)));
37690 if (IKabs(evalcond[0])
37691 > IKFAST_EVALCOND_THRESH
37692 || IKabs(evalcond[1])
37693 > IKFAST_EVALCOND_THRESH
37694 || IKabs(evalcond[2])
37695 > IKFAST_EVALCOND_THRESH
37696 || IKabs(evalcond[3])
37697 > IKFAST_EVALCOND_THRESH
37698 || IKabs(evalcond[4])
37699 > IKFAST_EVALCOND_THRESH
37700 || IKabs(evalcond[5])
37701 > IKFAST_EVALCOND_THRESH
37702 || IKabs(evalcond[6])
37703 > IKFAST_EVALCOND_THRESH
37704 || IKabs(evalcond[7])
37705 > IKFAST_EVALCOND_THRESH)
37706 {
37707 continue;
37708 }
37709 }
37710
37711 {
37712 std::vector<
37713 IkSingleDOFSolutionBase<
37714 IkReal> >
37715 vinfos(7);
37716 vinfos[0].jointtype = 1;
37717 vinfos[0].foffset = j4;
37718 vinfos[0].indices[0]
37719 = _ij4[0];
37720 vinfos[0].indices[1]
37721 = _ij4[1];
37722 vinfos[0].maxsolutions
37723 = _nj4;
37724 vinfos[1].jointtype = 1;
37725 vinfos[1].foffset = j6;
37726 vinfos[1].indices[0]
37727 = _ij6[0];
37728 vinfos[1].indices[1]
37729 = _ij6[1];
37730 vinfos[1].maxsolutions
37731 = _nj6;
37732 vinfos[2].jointtype = 1;
37733 vinfos[2].foffset = j8;
37734 vinfos[2].indices[0]
37735 = _ij8[0];
37736 vinfos[2].indices[1]
37737 = _ij8[1];
37738 vinfos[2].maxsolutions
37739 = _nj8;
37740 vinfos[3].jointtype = 1;
37741 vinfos[3].foffset = j9;
37742 vinfos[3].indices[0]
37743 = _ij9[0];
37744 vinfos[3].indices[1]
37745 = _ij9[1];
37746 vinfos[3].maxsolutions
37747 = _nj9;
37748 vinfos[4].jointtype = 1;
37749 vinfos[4].foffset = j10;
37750 vinfos[4].indices[0]
37751 = _ij10[0];
37752 vinfos[4].indices[1]
37753 = _ij10[1];
37754 vinfos[4].maxsolutions
37755 = _nj10;
37756 vinfos[5].jointtype = 1;
37757 vinfos[5].foffset = j11;
37758 vinfos[5].indices[0]
37759 = _ij11[0];
37760 vinfos[5].indices[1]
37761 = _ij11[1];
37762 vinfos[5].maxsolutions
37763 = _nj11;
37764 vinfos[6].jointtype = 1;
37765 vinfos[6].foffset = j12;
37766 vinfos[6].indices[0]
37767 = _ij12[0];
37768 vinfos[6].indices[1]
37769 = _ij12[1];
37770 vinfos[6].maxsolutions
37771 = _nj12;
37772 std::vector<int> vfree(0);
37773 solutions.AddSolution(
37774 vinfos, vfree);
37775 }
37776 }
37777 }
37778 }
37779 } while (0);
37780 if (bgotonextstatement)
37781 {
37782 bool bgotonextstatement = true;
37783 do
37784 {
37785 IkReal x538 = ((1.0) * cj11);
37786 IkReal x539
37787 = ((((-1.0) * x538))
37788 + new_r22);
37789 IkReal x540 = ((1.0) * sj11);
37790 IkReal x541
37791 = ((((-1.0) * x540))
37792 + new_r02);
37793 evalcond[0]
37794 = ((-3.14159265358979)
37795 + (IKfmod(
37796 ((3.14159265358979)
37797 + (IKabs(j10))),
37798 6.28318530717959)));
37799 evalcond[1] = x539;
37800 evalcond[2] = x539;
37801 evalcond[3] = x541;
37802 evalcond[4] = new_r12;
37803 evalcond[5] = x541;
37804 evalcond[6]
37805 = ((((-1.0) * new_r22 * x540))
37806 + ((cj11 * new_r02)));
37807 evalcond[7]
37808 = ((((-1.0) * new_r20 * x538))
37809 + (((-1.0) * new_r00
37810 * x540)));
37811 evalcond[8]
37812 = ((((-1.0) * new_r21 * x538))
37813 + (((-1.0) * new_r01
37814 * x540)));
37815 evalcond[9]
37816 = ((1.0)
37817 + (((-1.0) * new_r02
37818 * x540))
37819 + (((-1.0) * new_r22
37820 * x538)));
37821 if (IKabs(evalcond[0])
37822 < 0.0000010000000000
37823 && IKabs(evalcond[1])
37824 < 0.0000010000000000
37825 && IKabs(evalcond[2])
37826 < 0.0000010000000000
37827 && IKabs(evalcond[3])
37828 < 0.0000010000000000
37829 && IKabs(evalcond[4])
37830 < 0.0000010000000000
37831 && IKabs(evalcond[5])
37832 < 0.0000010000000000
37833 && IKabs(evalcond[6])
37834 < 0.0000010000000000
37835 && IKabs(evalcond[7])
37836 < 0.0000010000000000
37837 && IKabs(evalcond[8])
37838 < 0.0000010000000000
37839 && IKabs(evalcond[9])
37840 < 0.0000010000000000)
37841 {
37842 bgotonextstatement = false;
37843 {
37844 IkReal j12array[1],
37845 cj12array[1],
37846 sj12array[1];
37847 bool j12valid[1] = {false};
37848 _nj12 = 1;
37849 if (IKabs(new_r10)
37850 < IKFAST_ATAN2_MAGTHRESH
37851 && IKabs(new_r11)
37852 < IKFAST_ATAN2_MAGTHRESH
37853 && IKabs(
37854 IKsqr(new_r10)
37855 + IKsqr(new_r11)
37856 - 1)
37857 <= IKFAST_SINCOS_THRESH)
37858 continue;
37859 j12array[0] = IKatan2(
37860 new_r10, new_r11);
37861 sj12array[0]
37862 = IKsin(j12array[0]);
37863 cj12array[0]
37864 = IKcos(j12array[0]);
37865 if (j12array[0] > IKPI)
37866 {
37867 j12array[0] -= IK2PI;
37868 }
37869 else if (j12array[0] < -IKPI)
37870 {
37871 j12array[0] += IK2PI;
37872 }
37873 j12valid[0] = true;
37874 for (int ij12 = 0; ij12 < 1;
37875 ++ij12)
37876 {
37877 if (!j12valid[ij12])
37878 {
37879 continue;
37880 }
37881 _ij12[0] = ij12;
37882 _ij12[1] = -1;
37883 for (int iij12 = ij12 + 1;
37884 iij12 < 1;
37885 ++iij12)
37886 {
37887 if (j12valid[iij12]
37888 && IKabs(
37889 cj12array[ij12]
37890 - cj12array
37891 [iij12])
37892 < IKFAST_SOLUTION_THRESH
37893 && IKabs(
37894 sj12array[ij12]
37895 - sj12array
37896 [iij12])
37897 < IKFAST_SOLUTION_THRESH)
37898 {
37899 j12valid[iij12] = false;
37900 _ij12[1] = iij12;
37901 break;
37902 }
37903 }
37904 j12 = j12array[ij12];
37905 cj12 = cj12array[ij12];
37906 sj12 = sj12array[ij12];
37907 {
37908 IkReal evalcond[8];
37909 IkReal x542 = IKcos(j12);
37910 IkReal x543 = IKsin(j12);
37911 IkReal x544
37912 = ((1.0) * x542);
37913 IkReal x545
37914 = ((-1.0) * x544);
37915 IkReal x546
37916 = ((1.0) * new_r02);
37917 evalcond[0]
37918 = (((new_r02 * x542))
37919 + new_r20);
37920 evalcond[1]
37921 = ((((-1.0) * x543))
37922 + new_r10);
37923 evalcond[2]
37924 = (new_r11 + x545);
37925 evalcond[3]
37926 = (((new_r22 * x543))
37927 + new_r01);
37928 evalcond[4]
37929 = ((((-1.0) * x543
37930 * x546))
37931 + new_r21);
37932 evalcond[5]
37933 = ((((-1.0) * new_r22
37934 * x544))
37935 + new_r00);
37936 evalcond[6]
37937 = (((new_r01
37938 * new_r22))
37939 + (((-1.0)
37940 * new_r21
37941 * x546))
37942 + x543);
37943 evalcond[7]
37944 = ((((-1.0) * new_r20
37945 * x546))
37946 + ((new_r00
37947 * new_r22))
37948 + x545);
37949 if (IKabs(evalcond[0])
37950 > IKFAST_EVALCOND_THRESH
37951 || IKabs(evalcond[1])
37952 > IKFAST_EVALCOND_THRESH
37953 || IKabs(evalcond[2])
37954 > IKFAST_EVALCOND_THRESH
37955 || IKabs(evalcond[3])
37956 > IKFAST_EVALCOND_THRESH
37957 || IKabs(evalcond[4])
37958 > IKFAST_EVALCOND_THRESH
37959 || IKabs(evalcond[5])
37960 > IKFAST_EVALCOND_THRESH
37961 || IKabs(evalcond[6])
37962 > IKFAST_EVALCOND_THRESH
37963 || IKabs(evalcond[7])
37964 > IKFAST_EVALCOND_THRESH)
37965 {
37966 continue;
37967 }
37968 }
37969
37970 {
37971 std::vector<
37972 IkSingleDOFSolutionBase<
37973 IkReal> >
37974 vinfos(7);
37975 vinfos[0].jointtype = 1;
37976 vinfos[0].foffset = j4;
37977 vinfos[0].indices[0]
37978 = _ij4[0];
37979 vinfos[0].indices[1]
37980 = _ij4[1];
37981 vinfos[0].maxsolutions
37982 = _nj4;
37983 vinfos[1].jointtype = 1;
37984 vinfos[1].foffset = j6;
37985 vinfos[1].indices[0]
37986 = _ij6[0];
37987 vinfos[1].indices[1]
37988 = _ij6[1];
37989 vinfos[1].maxsolutions
37990 = _nj6;
37991 vinfos[2].jointtype = 1;
37992 vinfos[2].foffset = j8;
37993 vinfos[2].indices[0]
37994 = _ij8[0];
37995 vinfos[2].indices[1]
37996 = _ij8[1];
37997 vinfos[2].maxsolutions
37998 = _nj8;
37999 vinfos[3].jointtype = 1;
38000 vinfos[3].foffset = j9;
38001 vinfos[3].indices[0]
38002 = _ij9[0];
38003 vinfos[3].indices[1]
38004 = _ij9[1];
38005 vinfos[3].maxsolutions
38006 = _nj9;
38007 vinfos[4].jointtype = 1;
38008 vinfos[4].foffset = j10;
38009 vinfos[4].indices[0]
38010 = _ij10[0];
38011 vinfos[4].indices[1]
38012 = _ij10[1];
38013 vinfos[4].maxsolutions
38014 = _nj10;
38015 vinfos[5].jointtype = 1;
38016 vinfos[5].foffset = j11;
38017 vinfos[5].indices[0]
38018 = _ij11[0];
38019 vinfos[5].indices[1]
38020 = _ij11[1];
38021 vinfos[5].maxsolutions
38022 = _nj11;
38023 vinfos[6].jointtype = 1;
38024 vinfos[6].foffset = j12;
38025 vinfos[6].indices[0]
38026 = _ij12[0];
38027 vinfos[6].indices[1]
38028 = _ij12[1];
38029 vinfos[6].maxsolutions
38030 = _nj12;
38031 std::vector<int> vfree(0);
38032 solutions.AddSolution(
38033 vinfos, vfree);
38034 }
38035 }
38036 }
38037 }
38038 } while (0);
38039 if (bgotonextstatement)
38040 {
38041 bool bgotonextstatement = true;
38042 do
38043 {
38044 IkReal x547 = ((1.0) * cj11);
38045 IkReal x548
38046 = ((((-1.0) * x547))
38047 + new_r22);
38048 IkReal x549 = ((1.0) * sj11);
38049 evalcond[0]
38050 = ((-3.14159265358979)
38051 + (IKfmod(
38052 ((3.14159265358979)
38053 + (IKabs((
38054 (-3.14159265358979)
38055 + j10)))),
38056 6.28318530717959)));
38057 evalcond[1] = x548;
38058 evalcond[2] = x548;
38059 evalcond[3] = (sj11 + new_r02);
38060 evalcond[4] = new_r12;
38061 evalcond[5]
38062 = ((((-1.0) * x549))
38063 + (((-1.0) * (1.0)
38064 * new_r02)));
38065 evalcond[6]
38066 = ((((-1.0) * new_r22
38067 * x549))
38068 + (((-1.0) * new_r02
38069 * x547)));
38070 evalcond[7]
38071 = ((((-1.0) * new_r20
38072 * x547))
38073 + ((new_r00 * sj11)));
38074 evalcond[8]
38075 = ((((-1.0) * new_r21
38076 * x547))
38077 + ((new_r01 * sj11)));
38078 evalcond[9]
38079 = ((1.0)
38080 + ((new_r02 * sj11))
38081 + (((-1.0) * new_r22
38082 * x547)));
38083 if (IKabs(evalcond[0])
38084 < 0.0000010000000000
38085 && IKabs(evalcond[1])
38086 < 0.0000010000000000
38087 && IKabs(evalcond[2])
38088 < 0.0000010000000000
38089 && IKabs(evalcond[3])
38090 < 0.0000010000000000
38091 && IKabs(evalcond[4])
38092 < 0.0000010000000000
38093 && IKabs(evalcond[5])
38094 < 0.0000010000000000
38095 && IKabs(evalcond[6])
38096 < 0.0000010000000000
38097 && IKabs(evalcond[7])
38098 < 0.0000010000000000
38099 && IKabs(evalcond[8])
38100 < 0.0000010000000000
38101 && IKabs(evalcond[9])
38102 < 0.0000010000000000)
38103 {
38104 bgotonextstatement = false;
38105 {
38106 IkReal j12array[1],
38107 cj12array[1],
38108 sj12array[1];
38109 bool j12valid[1] = {false};
38110 _nj12 = 1;
38111 CheckValue<IkReal> x550
38112 = IKatan2WithCheck(
38113 IkReal((
38114 (-1.0)
38115 * (((1.0)
38116 * new_r21)))),
38117 new_r20,
38118 IKFAST_ATAN2_MAGTHRESH);
38119 if (!x550.valid)
38120 {
38121 continue;
38122 }
38123 CheckValue<IkReal> x551
38124 = IKPowWithIntegerCheck(
38125 IKsign(new_r02),
38126 -1);
38127 if (!x551.valid)
38128 {
38129 continue;
38130 }
38131 j12array[0]
38132 = ((-1.5707963267949)
38133 + (x550.value)
38134 + (((1.5707963267949)
38135 * (x551.value))));
38136 sj12array[0]
38137 = IKsin(j12array[0]);
38138 cj12array[0]
38139 = IKcos(j12array[0]);
38140 if (j12array[0] > IKPI)
38141 {
38142 j12array[0] -= IK2PI;
38143 }
38144 else if (
38145 j12array[0] < -IKPI)
38146 {
38147 j12array[0] += IK2PI;
38148 }
38149 j12valid[0] = true;
38150 for (int ij12 = 0; ij12 < 1;
38151 ++ij12)
38152 {
38153 if (!j12valid[ij12])
38154 {
38155 continue;
38156 }
38157 _ij12[0] = ij12;
38158 _ij12[1] = -1;
38159 for (int iij12 = ij12 + 1;
38160 iij12 < 1;
38161 ++iij12)
38162 {
38163 if (j12valid[iij12]
38164 && IKabs(
38165 cj12array
38166 [ij12]
38167 - cj12array
38168 [iij12])
38169 < IKFAST_SOLUTION_THRESH
38170 && IKabs(
38171 sj12array
38172 [ij12]
38173 - sj12array
38174 [iij12])
38175 < IKFAST_SOLUTION_THRESH)
38176 {
38177 j12valid[iij12]
38178 = false;
38179 _ij12[1] = iij12;
38180 break;
38181 }
38182 }
38183 j12 = j12array[ij12];
38184 cj12 = cj12array[ij12];
38185 sj12 = sj12array[ij12];
38186 {
38187 IkReal evalcond[8];
38188 IkReal x552
38189 = IKsin(j12);
38190 IkReal x553
38191 = ((1.0)
38192 * (IKcos(j12)));
38193 IkReal x554
38194 = ((-1.0) * x553);
38195 IkReal x555
38196 = ((1.0) * new_r01);
38197 IkReal x556
38198 = ((1.0) * new_r00);
38199 evalcond[0]
38200 = (((new_r02
38201 * x552))
38202 + new_r21);
38203 evalcond[1]
38204 = ((((-1.0)
38205 * new_r02
38206 * x553))
38207 + new_r20);
38208 evalcond[2]
38209 = ((((-1.0) * (1.0)
38210 * new_r10))
38211 + (((-1.0)
38212 * x552)));
38213 evalcond[3]
38214 = (x554
38215 + (((-1.0)
38216 * (1.0)
38217 * new_r11)));
38218 evalcond[4]
38219 = (((new_r22
38220 * x552))
38221 + (((-1.0)
38222 * x555)));
38223 evalcond[5]
38224 = ((((-1.0) * x556))
38225 + (((-1.0)
38226 * new_r22
38227 * x553)));
38228 evalcond[6]
38229 = (((new_r02
38230 * new_r21))
38231 + (((-1.0)
38232 * new_r22
38233 * x555))
38234 + x552);
38235 evalcond[7]
38236 = (((new_r02
38237 * new_r20))
38238 + x554
38239 + (((-1.0)
38240 * new_r22
38241 * x556)));
38242 if (IKabs(evalcond[0])
38243 > IKFAST_EVALCOND_THRESH
38244 || IKabs(
38245 evalcond[1])
38246 > IKFAST_EVALCOND_THRESH
38247 || IKabs(
38248 evalcond[2])
38249 > IKFAST_EVALCOND_THRESH
38250 || IKabs(
38251 evalcond[3])
38252 > IKFAST_EVALCOND_THRESH
38253 || IKabs(
38254 evalcond[4])
38255 > IKFAST_EVALCOND_THRESH
38256 || IKabs(
38257 evalcond[5])
38258 > IKFAST_EVALCOND_THRESH
38259 || IKabs(
38260 evalcond[6])
38261 > IKFAST_EVALCOND_THRESH
38262 || IKabs(
38263 evalcond[7])
38264 > IKFAST_EVALCOND_THRESH)
38265 {
38266 continue;
38267 }
38268 }
38269
38270 {
38271 std::vector<
38272 IkSingleDOFSolutionBase<
38273 IkReal> >
38274 vinfos(7);
38275 vinfos[0].jointtype = 1;
38276 vinfos[0].foffset = j4;
38277 vinfos[0].indices[0]
38278 = _ij4[0];
38279 vinfos[0].indices[1]
38280 = _ij4[1];
38281 vinfos[0].maxsolutions
38282 = _nj4;
38283 vinfos[1].jointtype = 1;
38284 vinfos[1].foffset = j6;
38285 vinfos[1].indices[0]
38286 = _ij6[0];
38287 vinfos[1].indices[1]
38288 = _ij6[1];
38289 vinfos[1].maxsolutions
38290 = _nj6;
38291 vinfos[2].jointtype = 1;
38292 vinfos[2].foffset = j8;
38293 vinfos[2].indices[0]
38294 = _ij8[0];
38295 vinfos[2].indices[1]
38296 = _ij8[1];
38297 vinfos[2].maxsolutions
38298 = _nj8;
38299 vinfos[3].jointtype = 1;
38300 vinfos[3].foffset = j9;
38301 vinfos[3].indices[0]
38302 = _ij9[0];
38303 vinfos[3].indices[1]
38304 = _ij9[1];
38305 vinfos[3].maxsolutions
38306 = _nj9;
38307 vinfos[4].jointtype = 1;
38308 vinfos[4].foffset = j10;
38309 vinfos[4].indices[0]
38310 = _ij10[0];
38311 vinfos[4].indices[1]
38312 = _ij10[1];
38313 vinfos[4].maxsolutions
38314 = _nj10;
38315 vinfos[5].jointtype = 1;
38316 vinfos[5].foffset = j11;
38317 vinfos[5].indices[0]
38318 = _ij11[0];
38319 vinfos[5].indices[1]
38320 = _ij11[1];
38321 vinfos[5].maxsolutions
38322 = _nj11;
38323 vinfos[6].jointtype = 1;
38324 vinfos[6].foffset = j12;
38325 vinfos[6].indices[0]
38326 = _ij12[0];
38327 vinfos[6].indices[1]
38328 = _ij12[1];
38329 vinfos[6].maxsolutions
38330 = _nj12;
38331 std::vector<int> vfree(
38332 0);
38333 solutions.AddSolution(
38334 vinfos, vfree);
38335 }
38336 }
38337 }
38338 }
38339 } while (0);
38340 if (bgotonextstatement)
38341 {
38342 bool bgotonextstatement = true;
38343 do
38344 {
38345 if (1)
38346 {
38347 bgotonextstatement = false;
38348 continue; // branch miss
38349 // [j12]
38350 }
38351 } while (0);
38352 if (bgotonextstatement)
38353 {
38354 }
38355 }
38356 }
38357 }
38358 }
38359 }
38360 }
38361 }
38362 }
38363 }
38364 }
38365 else
38366 {
38367 {
38368 IkReal j12array[1], cj12array[1], sj12array[1];
38369 bool j12valid[1] = {false};
38370 _nj12 = 1;
38371 CheckValue<IkReal> x558
38372 = IKPowWithIntegerCheck(sj11, -1);
38373 if (!x558.valid)
38374 {
38375 continue;
38376 }
38377 IkReal x557 = x558.value;
38378 CheckValue<IkReal> x559
38379 = IKPowWithIntegerCheck(cj10, -1);
38380 if (!x559.valid)
38381 {
38382 continue;
38383 }
38384 CheckValue<IkReal> x560
38385 = IKPowWithIntegerCheck(cj11, -1);
38386 if (!x560.valid)
38387 {
38388 continue;
38389 }
38390 if (IKabs(
38391 (x557 * (x559.value) * (x560.value)
38392 * (((((-1.0) * (1.0) * new_r01 * sj11))
38393 + ((new_r20 * sj10))))))
38394 < IKFAST_ATAN2_MAGTHRESH
38395 && IKabs(((-1.0) * new_r20 * x557))
38396 < IKFAST_ATAN2_MAGTHRESH
38397 && IKabs(
38398 IKsqr(
38399 (x557 * (x559.value)
38400 * (x560.value)
38401 * (((((-1.0) * (1.0) * new_r01
38402 * sj11))
38403 + ((new_r20 * sj10))))))
38404 + IKsqr(((-1.0) * new_r20 * x557))
38405 - 1)
38406 <= IKFAST_SINCOS_THRESH)
38407 continue;
38408 j12array[0] = IKatan2(
38409 (x557 * (x559.value) * (x560.value)
38410 * (((((-1.0) * (1.0) * new_r01 * sj11))
38411 + ((new_r20 * sj10))))),
38412 ((-1.0) * new_r20 * x557));
38413 sj12array[0] = IKsin(j12array[0]);
38414 cj12array[0] = IKcos(j12array[0]);
38415 if (j12array[0] > IKPI)
38416 {
38417 j12array[0] -= IK2PI;
38418 }
38419 else if (j12array[0] < -IKPI)
38420 {
38421 j12array[0] += IK2PI;
38422 }
38423 j12valid[0] = true;
38424 for (int ij12 = 0; ij12 < 1; ++ij12)
38425 {
38426 if (!j12valid[ij12])
38427 {
38428 continue;
38429 }
38430 _ij12[0] = ij12;
38431 _ij12[1] = -1;
38432 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38433 {
38434 if (j12valid[iij12]
38435 && IKabs(
38436 cj12array[ij12]
38437 - cj12array[iij12])
38438 < IKFAST_SOLUTION_THRESH
38439 && IKabs(
38440 sj12array[ij12]
38441 - sj12array[iij12])
38442 < IKFAST_SOLUTION_THRESH)
38443 {
38444 j12valid[iij12] = false;
38445 _ij12[1] = iij12;
38446 break;
38447 }
38448 }
38449 j12 = j12array[ij12];
38450 cj12 = cj12array[ij12];
38451 sj12 = sj12array[ij12];
38452 {
38453 IkReal evalcond[12];
38454 IkReal x561 = IKcos(j12);
38455 IkReal x562 = IKsin(j12);
38456 IkReal x563 = ((1.0) * sj11);
38457 IkReal x564 = (cj10 * new_r01);
38458 IkReal x565 = (new_r11 * sj10);
38459 IkReal x566 = (cj11 * x562);
38460 IkReal x567 = ((1.0) * sj10);
38461 IkReal x568 = ((1.0) * x562);
38462 IkReal x569 = ((1.0) * x561);
38463 IkReal x570 = ((-1.0) * x569);
38464 IkReal x571 = (cj10 * new_r00);
38465 IkReal x572 = (new_r10 * sj10);
38466 IkReal x573 = (cj10 * x569);
38467 evalcond[0] = (((sj11 * x561)) + new_r20);
38468 evalcond[1]
38469 = ((((-1.0) * x562 * x563)) + new_r21);
38470 evalcond[2] = (x564 + x565 + x566);
38471 evalcond[3]
38472 = (((cj10 * new_r10))
38473 + (((-1.0) * x568))
38474 + (((-1.0) * new_r00 * x567)));
38475 evalcond[4]
38476 = (((cj10 * new_r11)) + x570
38477 + (((-1.0) * new_r01 * x567)));
38478 evalcond[5]
38479 = (((cj10 * x566)) + new_r01
38480 + ((sj10 * x561)));
38481 evalcond[6]
38482 = (x572 + x571
38483 + (((-1.0) * cj11 * x569)));
38484 evalcond[7]
38485 = (((sj10 * x562))
38486 + (((-1.0) * cj11 * x573))
38487 + new_r00);
38488 evalcond[8]
38489 = ((((-1.0) * x573)) + new_r11
38490 + ((sj10 * x566)));
38491 evalcond[9]
38492 = ((((-1.0) * cj10 * x568))
38493 + (((-1.0) * cj11 * x561 * x567))
38494 + new_r10);
38495 evalcond[10]
38496 = ((((-1.0) * new_r21 * x563))
38497 + ((cj11 * x565)) + ((cj11 * x564))
38498 + x562);
38499 evalcond[11]
38500 = (x570 + ((cj11 * x572))
38501 + ((cj11 * x571))
38502 + (((-1.0) * new_r20 * x563)));
38503 if (IKabs(evalcond[0])
38504 > IKFAST_EVALCOND_THRESH
38505 || IKabs(evalcond[1])
38506 > IKFAST_EVALCOND_THRESH
38507 || IKabs(evalcond[2])
38508 > IKFAST_EVALCOND_THRESH
38509 || IKabs(evalcond[3])
38510 > IKFAST_EVALCOND_THRESH
38511 || IKabs(evalcond[4])
38512 > IKFAST_EVALCOND_THRESH
38513 || IKabs(evalcond[5])
38514 > IKFAST_EVALCOND_THRESH
38515 || IKabs(evalcond[6])
38516 > IKFAST_EVALCOND_THRESH
38517 || IKabs(evalcond[7])
38518 > IKFAST_EVALCOND_THRESH
38519 || IKabs(evalcond[8])
38520 > IKFAST_EVALCOND_THRESH
38521 || IKabs(evalcond[9])
38522 > IKFAST_EVALCOND_THRESH
38523 || IKabs(evalcond[10])
38524 > IKFAST_EVALCOND_THRESH
38525 || IKabs(evalcond[11])
38526 > IKFAST_EVALCOND_THRESH)
38527 {
38528 continue;
38529 }
38530 }
38531
38532 {
38533 std::vector<
38534 IkSingleDOFSolutionBase<IkReal> >
38535 vinfos(7);
38536 vinfos[0].jointtype = 1;
38537 vinfos[0].foffset = j4;
38538 vinfos[0].indices[0] = _ij4[0];
38539 vinfos[0].indices[1] = _ij4[1];
38540 vinfos[0].maxsolutions = _nj4;
38541 vinfos[1].jointtype = 1;
38542 vinfos[1].foffset = j6;
38543 vinfos[1].indices[0] = _ij6[0];
38544 vinfos[1].indices[1] = _ij6[1];
38545 vinfos[1].maxsolutions = _nj6;
38546 vinfos[2].jointtype = 1;
38547 vinfos[2].foffset = j8;
38548 vinfos[2].indices[0] = _ij8[0];
38549 vinfos[2].indices[1] = _ij8[1];
38550 vinfos[2].maxsolutions = _nj8;
38551 vinfos[3].jointtype = 1;
38552 vinfos[3].foffset = j9;
38553 vinfos[3].indices[0] = _ij9[0];
38554 vinfos[3].indices[1] = _ij9[1];
38555 vinfos[3].maxsolutions = _nj9;
38556 vinfos[4].jointtype = 1;
38557 vinfos[4].foffset = j10;
38558 vinfos[4].indices[0] = _ij10[0];
38559 vinfos[4].indices[1] = _ij10[1];
38560 vinfos[4].maxsolutions = _nj10;
38561 vinfos[5].jointtype = 1;
38562 vinfos[5].foffset = j11;
38563 vinfos[5].indices[0] = _ij11[0];
38564 vinfos[5].indices[1] = _ij11[1];
38565 vinfos[5].maxsolutions = _nj11;
38566 vinfos[6].jointtype = 1;
38567 vinfos[6].foffset = j12;
38568 vinfos[6].indices[0] = _ij12[0];
38569 vinfos[6].indices[1] = _ij12[1];
38570 vinfos[6].maxsolutions = _nj12;
38571 std::vector<int> vfree(0);
38572 solutions.AddSolution(vinfos, vfree);
38573 }
38574 }
38575 }
38576 }
38577 }
38578 }
38579 else
38580 {
38581 {
38582 IkReal j12array[1], cj12array[1], sj12array[1];
38583 bool j12valid[1] = {false};
38584 _nj12 = 1;
38585 CheckValue<IkReal> x576
38586 = IKPowWithIntegerCheck(sj11, -1);
38587 if (!x576.valid)
38588 {
38589 continue;
38590 }
38591 IkReal x574 = x576.value;
38592 IkReal x575 = ((1.0) * new_r20);
38593 CheckValue<IkReal> x577
38594 = IKPowWithIntegerCheck(sj10, -1);
38595 if (!x577.valid)
38596 {
38597 continue;
38598 }
38599 if (IKabs(
38600 (x574 * (x577.value)
38601 * (((((-1.0) * cj10 * cj11 * x575))
38602 + (((-1.0) * (1.0) * new_r00
38603 * sj11))))))
38604 < IKFAST_ATAN2_MAGTHRESH
38605 && IKabs(((-1.0) * x574 * x575))
38606 < IKFAST_ATAN2_MAGTHRESH
38607 && IKabs(
38608 IKsqr(
38609 (x574 * (x577.value)
38610 * (((((-1.0) * cj10 * cj11 * x575))
38611 + (((-1.0) * (1.0) * new_r00
38612 * sj11))))))
38613 + IKsqr(((-1.0) * x574 * x575)) - 1)
38614 <= IKFAST_SINCOS_THRESH)
38615 continue;
38616 j12array[0] = IKatan2(
38617 (x574 * (x577.value)
38618 * (((((-1.0) * cj10 * cj11 * x575))
38619 + (((-1.0) * (1.0) * new_r00 * sj11))))),
38620 ((-1.0) * x574 * x575));
38621 sj12array[0] = IKsin(j12array[0]);
38622 cj12array[0] = IKcos(j12array[0]);
38623 if (j12array[0] > IKPI)
38624 {
38625 j12array[0] -= IK2PI;
38626 }
38627 else if (j12array[0] < -IKPI)
38628 {
38629 j12array[0] += IK2PI;
38630 }
38631 j12valid[0] = true;
38632 for (int ij12 = 0; ij12 < 1; ++ij12)
38633 {
38634 if (!j12valid[ij12])
38635 {
38636 continue;
38637 }
38638 _ij12[0] = ij12;
38639 _ij12[1] = -1;
38640 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38641 {
38642 if (j12valid[iij12]
38643 && IKabs(cj12array[ij12] - cj12array[iij12])
38644 < IKFAST_SOLUTION_THRESH
38645 && IKabs(sj12array[ij12] - sj12array[iij12])
38646 < IKFAST_SOLUTION_THRESH)
38647 {
38648 j12valid[iij12] = false;
38649 _ij12[1] = iij12;
38650 break;
38651 }
38652 }
38653 j12 = j12array[ij12];
38654 cj12 = cj12array[ij12];
38655 sj12 = sj12array[ij12];
38656 {
38657 IkReal evalcond[12];
38658 IkReal x578 = IKcos(j12);
38659 IkReal x579 = IKsin(j12);
38660 IkReal x580 = ((1.0) * sj11);
38661 IkReal x581 = (cj10 * new_r01);
38662 IkReal x582 = (new_r11 * sj10);
38663 IkReal x583 = (cj11 * x579);
38664 IkReal x584 = ((1.0) * sj10);
38665 IkReal x585 = ((1.0) * x579);
38666 IkReal x586 = ((1.0) * x578);
38667 IkReal x587 = ((-1.0) * x586);
38668 IkReal x588 = (cj10 * new_r00);
38669 IkReal x589 = (new_r10 * sj10);
38670 IkReal x590 = (cj10 * x586);
38671 evalcond[0] = (((sj11 * x578)) + new_r20);
38672 evalcond[1]
38673 = ((((-1.0) * x579 * x580)) + new_r21);
38674 evalcond[2] = (x581 + x582 + x583);
38675 evalcond[3]
38676 = (((cj10 * new_r10)) + (((-1.0) * x585))
38677 + (((-1.0) * new_r00 * x584)));
38678 evalcond[4]
38679 = (((cj10 * new_r11))
38680 + (((-1.0) * new_r01 * x584)) + x587);
38681 evalcond[5]
38682 = (((sj10 * x578)) + ((cj10 * x583))
38683 + new_r01);
38684 evalcond[6]
38685 = ((((-1.0) * cj11 * x586)) + x589 + x588);
38686 evalcond[7]
38687 = (new_r00 + ((sj10 * x579))
38688 + (((-1.0) * cj11 * x590)));
38689 evalcond[8]
38690 = ((((-1.0) * x590)) + ((sj10 * x583))
38691 + new_r11);
38692 evalcond[9]
38693 = ((((-1.0) * cj10 * x585))
38694 + (((-1.0) * cj11 * x578 * x584))
38695 + new_r10);
38696 evalcond[10]
38697 = (x579 + ((cj11 * x581)) + ((cj11 * x582))
38698 + (((-1.0) * new_r21 * x580)));
38699 evalcond[11]
38700 = (((cj11 * x589))
38701 + (((-1.0) * new_r20 * x580))
38702 + ((cj11 * x588)) + x587);
38703 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
38704 || IKabs(evalcond[1])
38705 > IKFAST_EVALCOND_THRESH
38706 || IKabs(evalcond[2])
38707 > IKFAST_EVALCOND_THRESH
38708 || IKabs(evalcond[3])
38709 > IKFAST_EVALCOND_THRESH
38710 || IKabs(evalcond[4])
38711 > IKFAST_EVALCOND_THRESH
38712 || IKabs(evalcond[5])
38713 > IKFAST_EVALCOND_THRESH
38714 || IKabs(evalcond[6])
38715 > IKFAST_EVALCOND_THRESH
38716 || IKabs(evalcond[7])
38717 > IKFAST_EVALCOND_THRESH
38718 || IKabs(evalcond[8])
38719 > IKFAST_EVALCOND_THRESH
38720 || IKabs(evalcond[9])
38721 > IKFAST_EVALCOND_THRESH
38722 || IKabs(evalcond[10])
38723 > IKFAST_EVALCOND_THRESH
38724 || IKabs(evalcond[11])
38725 > IKFAST_EVALCOND_THRESH)
38726 {
38727 continue;
38728 }
38729 }
38730
38731 {
38732 std::vector<IkSingleDOFSolutionBase<IkReal> >
38733 vinfos(7);
38734 vinfos[0].jointtype = 1;
38735 vinfos[0].foffset = j4;
38736 vinfos[0].indices[0] = _ij4[0];
38737 vinfos[0].indices[1] = _ij4[1];
38738 vinfos[0].maxsolutions = _nj4;
38739 vinfos[1].jointtype = 1;
38740 vinfos[1].foffset = j6;
38741 vinfos[1].indices[0] = _ij6[0];
38742 vinfos[1].indices[1] = _ij6[1];
38743 vinfos[1].maxsolutions = _nj6;
38744 vinfos[2].jointtype = 1;
38745 vinfos[2].foffset = j8;
38746 vinfos[2].indices[0] = _ij8[0];
38747 vinfos[2].indices[1] = _ij8[1];
38748 vinfos[2].maxsolutions = _nj8;
38749 vinfos[3].jointtype = 1;
38750 vinfos[3].foffset = j9;
38751 vinfos[3].indices[0] = _ij9[0];
38752 vinfos[3].indices[1] = _ij9[1];
38753 vinfos[3].maxsolutions = _nj9;
38754 vinfos[4].jointtype = 1;
38755 vinfos[4].foffset = j10;
38756 vinfos[4].indices[0] = _ij10[0];
38757 vinfos[4].indices[1] = _ij10[1];
38758 vinfos[4].maxsolutions = _nj10;
38759 vinfos[5].jointtype = 1;
38760 vinfos[5].foffset = j11;
38761 vinfos[5].indices[0] = _ij11[0];
38762 vinfos[5].indices[1] = _ij11[1];
38763 vinfos[5].maxsolutions = _nj11;
38764 vinfos[6].jointtype = 1;
38765 vinfos[6].foffset = j12;
38766 vinfos[6].indices[0] = _ij12[0];
38767 vinfos[6].indices[1] = _ij12[1];
38768 vinfos[6].maxsolutions = _nj12;
38769 std::vector<int> vfree(0);
38770 solutions.AddSolution(vinfos, vfree);
38771 }
38772 }
38773 }
38774 }
38775 }
38776 }
38777 else
38778 {
38779 {
38780 IkReal j12array[1], cj12array[1], sj12array[1];
38781 bool j12valid[1] = {false};
38782 _nj12 = 1;
38783 CheckValue<IkReal> x591
38784 = IKPowWithIntegerCheck(IKsign(sj11), -1);
38785 if (!x591.valid)
38786 {
38787 continue;
38788 }
38789 CheckValue<IkReal> x592 = IKatan2WithCheck(
38790 IkReal(new_r21),
38791 ((-1.0) * (((1.0) * new_r20))),
38792 IKFAST_ATAN2_MAGTHRESH);
38793 if (!x592.valid)
38794 {
38795 continue;
38796 }
38797 j12array[0]
38798 = ((-1.5707963267949)
38799 + (((1.5707963267949) * (x591.value)))
38800 + (x592.value));
38801 sj12array[0] = IKsin(j12array[0]);
38802 cj12array[0] = IKcos(j12array[0]);
38803 if (j12array[0] > IKPI)
38804 {
38805 j12array[0] -= IK2PI;
38806 }
38807 else if (j12array[0] < -IKPI)
38808 {
38809 j12array[0] += IK2PI;
38810 }
38811 j12valid[0] = true;
38812 for (int ij12 = 0; ij12 < 1; ++ij12)
38813 {
38814 if (!j12valid[ij12])
38815 {
38816 continue;
38817 }
38818 _ij12[0] = ij12;
38819 _ij12[1] = -1;
38820 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38821 {
38822 if (j12valid[iij12]
38823 && IKabs(cj12array[ij12] - cj12array[iij12])
38824 < IKFAST_SOLUTION_THRESH
38825 && IKabs(sj12array[ij12] - sj12array[iij12])
38826 < IKFAST_SOLUTION_THRESH)
38827 {
38828 j12valid[iij12] = false;
38829 _ij12[1] = iij12;
38830 break;
38831 }
38832 }
38833 j12 = j12array[ij12];
38834 cj12 = cj12array[ij12];
38835 sj12 = sj12array[ij12];
38836 {
38837 IkReal evalcond[12];
38838 IkReal x593 = IKcos(j12);
38839 IkReal x594 = IKsin(j12);
38840 IkReal x595 = ((1.0) * sj11);
38841 IkReal x596 = (cj10 * new_r01);
38842 IkReal x597 = (new_r11 * sj10);
38843 IkReal x598 = (cj11 * x594);
38844 IkReal x599 = ((1.0) * sj10);
38845 IkReal x600 = ((1.0) * x594);
38846 IkReal x601 = ((1.0) * x593);
38847 IkReal x602 = ((-1.0) * x601);
38848 IkReal x603 = (cj10 * new_r00);
38849 IkReal x604 = (new_r10 * sj10);
38850 IkReal x605 = (cj10 * x601);
38851 evalcond[0] = (new_r20 + ((sj11 * x593)));
38852 evalcond[1] = ((((-1.0) * x594 * x595)) + new_r21);
38853 evalcond[2] = (x598 + x596 + x597);
38854 evalcond[3]
38855 = (((cj10 * new_r10)) + (((-1.0) * x600))
38856 + (((-1.0) * new_r00 * x599)));
38857 evalcond[4]
38858 = (((cj10 * new_r11))
38859 + (((-1.0) * new_r01 * x599)) + x602);
38860 evalcond[5]
38861 = (((cj10 * x598)) + ((sj10 * x593)) + new_r01);
38862 evalcond[6]
38863 = ((((-1.0) * cj11 * x601)) + x603 + x604);
38864 evalcond[7]
38865 = ((((-1.0) * cj11 * x605)) + ((sj10 * x594))
38866 + new_r00);
38867 evalcond[8]
38868 = (((sj10 * x598)) + new_r11
38869 + (((-1.0) * x605)));
38870 evalcond[9]
38871 = ((((-1.0) * cj10 * x600))
38872 + (((-1.0) * cj11 * x593 * x599)) + new_r10);
38873 evalcond[10]
38874 = (((cj11 * x597)) + x594
38875 + (((-1.0) * new_r21 * x595))
38876 + ((cj11 * x596)));
38877 evalcond[11]
38878 = (((cj11 * x603)) + x602 + ((cj11 * x604))
38879 + (((-1.0) * new_r20 * x595)));
38880 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
38881 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
38882 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
38883 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
38884 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
38885 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
38886 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
38887 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH
38888 || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH
38889 || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH
38890 || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH
38891 || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
38892 {
38893 continue;
38894 }
38895 }
38896
38897 {
38898 std::vector<IkSingleDOFSolutionBase<IkReal> >
38899 vinfos(7);
38900 vinfos[0].jointtype = 1;
38901 vinfos[0].foffset = j4;
38902 vinfos[0].indices[0] = _ij4[0];
38903 vinfos[0].indices[1] = _ij4[1];
38904 vinfos[0].maxsolutions = _nj4;
38905 vinfos[1].jointtype = 1;
38906 vinfos[1].foffset = j6;
38907 vinfos[1].indices[0] = _ij6[0];
38908 vinfos[1].indices[1] = _ij6[1];
38909 vinfos[1].maxsolutions = _nj6;
38910 vinfos[2].jointtype = 1;
38911 vinfos[2].foffset = j8;
38912 vinfos[2].indices[0] = _ij8[0];
38913 vinfos[2].indices[1] = _ij8[1];
38914 vinfos[2].maxsolutions = _nj8;
38915 vinfos[3].jointtype = 1;
38916 vinfos[3].foffset = j9;
38917 vinfos[3].indices[0] = _ij9[0];
38918 vinfos[3].indices[1] = _ij9[1];
38919 vinfos[3].maxsolutions = _nj9;
38920 vinfos[4].jointtype = 1;
38921 vinfos[4].foffset = j10;
38922 vinfos[4].indices[0] = _ij10[0];
38923 vinfos[4].indices[1] = _ij10[1];
38924 vinfos[4].maxsolutions = _nj10;
38925 vinfos[5].jointtype = 1;
38926 vinfos[5].foffset = j11;
38927 vinfos[5].indices[0] = _ij11[0];
38928 vinfos[5].indices[1] = _ij11[1];
38929 vinfos[5].maxsolutions = _nj11;
38930 vinfos[6].jointtype = 1;
38931 vinfos[6].foffset = j12;
38932 vinfos[6].indices[0] = _ij12[0];
38933 vinfos[6].indices[1] = _ij12[1];
38934 vinfos[6].maxsolutions = _nj12;
38935 std::vector<int> vfree(0);
38936 solutions.AddSolution(vinfos, vfree);
38937 }
38938 }
38939 }
38940 }
38941 }
38942 }
38943 }
38944 }
38945 }
38946 }
38947 }
38948 }
38949 }
38950 };
38951
38952 /// solves the inverse kinematics equations.
38953 /// \param pfree is an array specifying the free joints of the chain.
38954 IKFAST_API bool ComputeIk(
38955 const IkReal* eetrans,
38956 const IkReal* eerot,
38957 const IkReal* pfree,
38958 IkSolutionListBase<IkReal>& solutions)
38959 {
38960 IKSolver solver;
38961 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
38962 }
38963
38964 IKFAST_API bool ComputeIk2(
38965 const IkReal* eetrans,
38966 const IkReal* eerot,
38967 const IkReal* pfree,
38968 IkSolutionListBase<IkReal>& solutions,
38969 void* pOpenRAVEManip)
38970 {
38971 IKSolver solver;
38972 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
38973 }
38974
38975 IKFAST_API const char* GetKinematicsHash()
38976 {
38977 return "268c2c509bc1bb657da055f0ef2eb7e1";
38978 }
38979
38980 IKFAST_API const char* GetIkFastVersion()
38981 {
38982 return IKFAST_STRINGIZE(IKFAST_VERSION);
38983 }
38984
38985 #ifdef IKFAST_NAMESPACE
38986 } // end namespace
38987 #endif
38988
38989 #ifndef IKFAST_NO_MAIN
38990 #include <stdio.h>
38991 #include <stdlib.h>
38992 #ifdef IKFAST_NAMESPACE
38993 using namespace IKFAST_NAMESPACE;
38994 #endif
38995 int main(int argc, char** argv)
38996 {
38997 if (argc != 12 + GetNumFreeParameters() + 1)
38998 {
38999 printf(
39000 "\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 "
39001 "...\n\n"
39002 "Returns the ik solutions given the transformation of the end effector "
39003 "specified by\n"
39004 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
39005 "There are %d free parameters that have to be specified.\n\n",
39006 GetNumFreeParameters());
39007 return 1;
39008 }
39009
39010 IkSolutionList<IkReal> solutions;
39011 std::vector<IkReal> vfree(GetNumFreeParameters());
39012 IkReal eerot[9], eetrans[3];
39013 eerot[0] = atof(argv[1]);
39014 eerot[1] = atof(argv[2]);
39015 eerot[2] = atof(argv[3]);
39016 eetrans[0] = atof(argv[4]);
39017 eerot[3] = atof(argv[5]);
39018 eerot[4] = atof(argv[6]);
39019 eerot[5] = atof(argv[7]);
39020 eetrans[1] = atof(argv[8]);
39021 eerot[6] = atof(argv[9]);
39022 eerot[7] = atof(argv[10]);
39023 eerot[8] = atof(argv[11]);
39024 eetrans[2] = atof(argv[12]);
39025 for (std::size_t i = 0; i < vfree.size(); ++i)
39026 vfree[i] = atof(argv[13 + i]);
39027 bool bSuccess = ComputeIk(
39028 eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
39029
39030 if (!bSuccess)
39031 {
39032 fprintf(stderr, "Failed to get ik solution\n");
39033 return -1;
39034 }
39035
39036 printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
39037 std::vector<IkReal> solvalues(GetNumJoints());
39038 for (std::size_t i = 0; i < solutions.GetNumSolutions(); ++i)
39039 {
39040 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
39041
39042 printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
39043
39044 std::vector<IkReal> vsolfree(sol.GetFree().size());
39045
39046 sol.GetSolution(&solvalues[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
39047
39048 for (std::size_t j = 0; j < solvalues.size(); ++j)
39049 printf("%.15f, ", solvalues[j]);
39050 printf("\n");
39051 }
39052 return 0;
39053 }
39054
39055 #endif