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
IKabs(float f)143 inline float IKabs(float f)
144 {
145 return fabsf(f);
146 }
IKabs(double f)147 inline double IKabs(double f)
148 {
149 return fabs(f);
150 }
151
IKsqr(float f)152 inline float IKsqr(float f)
153 {
154 return f * f;
155 }
IKsqr(double f)156 inline double IKsqr(double f)
157 {
158 return f * f;
159 }
160
IKlog(float f)161 inline float IKlog(float f)
162 {
163 return logf(f);
164 }
IKlog(double f)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
IKasin(float f)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 }
IKasin(double f)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)
IKfmod(float x,float 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)
IKfmod(double x,double 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
IKacos(float f)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 }
IKacos(double f)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 }
IKsin(float f)260 inline float IKsin(float f)
261 {
262 return sinf(f);
263 }
IKsin(double f)264 inline double IKsin(double f)
265 {
266 return sin(f);
267 }
IKcos(float f)268 inline float IKcos(float f)
269 {
270 return cosf(f);
271 }
IKcos(double f)272 inline double IKcos(double f)
273 {
274 return cos(f);
275 }
IKtan(float f)276 inline float IKtan(float f)
277 {
278 return tanf(f);
279 }
IKtan(double f)280 inline double IKtan(double f)
281 {
282 return tan(f);
283 }
IKsqrt(float f)284 inline float IKsqrt(float f)
285 {
286 if (f <= 0.0f)
287 return 0.0f;
288 return sqrtf(f);
289 }
IKsqrt(double f)290 inline double IKsqrt(double f)
291 {
292 if (f <= 0.0)
293 return 0.0;
294 return sqrt(f);
295 }
IKatan2Simple(float fy,float fx)296 inline float IKatan2Simple(float fy, float fx)
297 {
298 return atan2f(fy, fx);
299 }
IKatan2(float fy,float fx)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 }
IKatan2Simple(double fy,double fx)314 inline double IKatan2Simple(double fy, double fx)
315 {
316 return atan2(fy, fx);
317 }
IKatan2(double fy,double fx)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>
IKatan2WithCheck(T fy,T fx,T epsilon)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
IKsign(float f)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
IKsign(double f)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>
IKPowWithIntegerCheck(T f,int n)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.
ComputeFk(const IkReal * j,IkReal * eetrans,IkReal * eerot)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
GetNumFreeParameters()549 IKFAST_API int GetNumFreeParameters()
550 {
551 return 1;
552 }
GetFreeParameters()553 IKFAST_API int* GetFreeParameters()
554 {
555 static int freeparams[] = {2};
556 return freeparams;
557 }
GetNumJoints()558 IKFAST_API int GetNumJoints()
559 {
560 return 7;
561 }
562
GetIkRealSize()563 IKFAST_API int GetIkRealSize()
564 {
565 return sizeof(IkReal);
566 }
567
GetIkType()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;
ComputeIk(const IkReal * eetrans,const IkReal * eerot,const IkReal * pfree,IkSolutionListBase<IkReal> & solutions)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 * ((
3285 (((-1.0)
3286 * (78.4313725490196)
3287 * sj4
3288 * (py
3289 * py
3290 * py)))
3291 + (((-78.4313725490196)
3292 * x641
3293 * x644))
3294 + (((-78.4313725490196) * x642 * x643)) + (((-1.0) * (78.4313725490196) * cj4 * (px * px * px))) + (((56.6666666666667) * x642))
3295 + (((56.6666666666667)
3296 * x641))))))
3297 - 1)
3298 <= IKFAST_SINCOS_THRESH)
3299 continue;
3300 j6array[0] = IKatan2(
3301 ((((1.17647058823529)
3302 * x642))
3303 + (((1.17647058823529)
3304 * x641))),
3305 ((x645.value)
3306 * (((((-1.0)
3307 * (78.4313725490196)
3308 * sj4
3309 * (py
3310 * py
3311 * py)))
3312 + (((-78.4313725490196)
3313 * x641
3314 * x644))
3315 + (((-78.4313725490196)
3316 * x642
3317 * x643))
3318 + (((-1.0)
3319 * (78.4313725490196)
3320 * cj4
3321 * (px
3322 * px
3323 * px)))
3324 + (((56.6666666666667)
3325 * x642))
3326 + (((56.6666666666667)
3327 * x641))))));
3328 sj6array
3329 [0]
3330 = IKsin(
3331 j6array
3332 [0]);
3333 cj6array
3334 [0]
3335 = IKcos(
3336 j6array
3337 [0]);
3338 if (j6array
3339 [0]
3340 > IKPI)
3341 {
3342 j6array
3343 [0]
3344 -= IK2PI;
3345 }
3346 else if (
3347 j6array
3348 [0]
3349 < -IKPI)
3350 {
3351 j6array
3352 [0]
3353 += IK2PI;
3354 }
3355 j6valid
3356 [0]
3357 = true;
3358 for (
3359 int ij6
3360 = 0;
3361 ij6
3362 < 1;
3363 ++ij6)
3364 {
3365 if (!j6valid
3366 [ij6])
3367 {
3368 continue;
3369 }
3370 _ij6[0]
3371 = ij6;
3372 _ij6[1]
3373 = -1;
3374 for (
3375 int iij6
3376 = ij6
3377 + 1;
3378 iij6
3379 < 1;
3380 ++iij6)
3381 {
3382 if (j6valid
3383 [iij6]
3384 && IKabs(
3385 cj6array
3386 [ij6]
3387 - cj6array
3388 [iij6])
3389 < IKFAST_SOLUTION_THRESH
3390 && IKabs(
3391 sj6array
3392 [ij6]
3393 - sj6array
3394 [iij6])
3395 < IKFAST_SOLUTION_THRESH)
3396 {
3397 j6valid
3398 [iij6]
3399 = false;
3400 _ij6[1]
3401 = iij6;
3402 break;
3403 }
3404 }
3405 j6 = j6array
3406 [ij6];
3407 cj6 = cj6array
3408 [ij6];
3409 sj6 = sj6array
3410 [ij6];
3411 {
3412 IkReal evalcond
3413 [5];
3414 IkReal
3415 x646
3416 = IKsin(
3417 j6);
3418 IkReal
3419 x647
3420 = (cj4
3421 * px);
3422 IkReal
3423 x648
3424 = (x646
3425 * x647);
3426 IkReal
3427 x649
3428 = (py
3429 * sj4);
3430 IkReal
3431 x650
3432 = (x646
3433 * x649);
3434 IkReal
3435 x651
3436 = ((1.0)
3437 * x647);
3438 IkReal
3439 x652
3440 = ((1.0)
3441 * x649);
3442 IkReal
3443 x653
3444 = IKcos(
3445 j6);
3446 IkReal
3447 x654
3448 = px
3449 * px;
3450 IkReal
3451 x655
3452 = ((3.92156862745098)
3453 * x646);
3454 IkReal
3455 x656
3456 = ((0.588235294117647)
3457 * x653);
3458 IkReal
3459 x657
3460 = py
3461 * py;
3462 IkReal
3463 x658
3464 = ((0.09)
3465 * x653);
3466 evalcond
3467 [0]
3468 = ((-0.85)
3469 + x650
3470 + x648);
3471 evalcond
3472 [1]
3473 = ((((-1.0)
3474 * x652))
3475 + (((-1.0)
3476 * x651))
3477 + (((0.85)
3478 * x646)));
3479 evalcond
3480 [2]
3481 = ((((-1.0)
3482 * x652
3483 * x653))
3484 + (((-1.0)
3485 * x651
3486 * x653)));
3487 evalcond
3488 [3]
3489 = (((x654
3490 * x655))
3491 + ((x655
3492 * x657))
3493 + (((-1.0)
3494 * x654
3495 * x656))
3496 + (((-0.425)
3497 * x653))
3498 + (((-1.0)
3499 * x656
3500 * x657))
3501 + (((-2.83333333333333)
3502 * x646)));
3503 evalcond
3504 [4]
3505 = ((-0.2125)
3506 + (((1.1)
3507 * x648))
3508 + (((1.1)
3509 * x650))
3510 + ((x649
3511 * x658))
3512 + (((-1.0)
3513 * x657))
3514 + ((x647
3515 * x658))
3516 + (((-1.0)
3517 * x654)));
3518 if (IKabs(
3519 evalcond
3520 [0])
3521 > IKFAST_EVALCOND_THRESH
3522 || IKabs(
3523 evalcond
3524 [1])
3525 > IKFAST_EVALCOND_THRESH
3526 || IKabs(
3527 evalcond
3528 [2])
3529 > IKFAST_EVALCOND_THRESH
3530 || IKabs(
3531 evalcond
3532 [3])
3533 > IKFAST_EVALCOND_THRESH
3534 || IKabs(
3535 evalcond
3536 [4])
3537 > IKFAST_EVALCOND_THRESH)
3538 {
3539 continue;
3540 }
3541 }
3542
3543 rotationfunction0(
3544 solutions);
3545 }
3546 }
3547 }
3548 }
3549 }
3550 else
3551 {
3552 {
3553 IkReal
3554 j6array
3555 [1],
3556 cj6array
3557 [1],
3558 sj6array
3559 [1];
3560 bool j6valid
3561 [1]
3562 = {false};
3563 _nj6 = 1;
3564 IkReal x659
3565 = (cj4
3566 * px);
3567 IkReal x660
3568 = (py
3569 * sj4);
3570 IkReal x661
3571 = px
3572 * px;
3573 IkReal x662
3574 = py
3575 * py;
3576 IkReal x663
3577 = ((1.29411764705882)
3578 * (cj4
3579 * cj4));
3580 CheckValue<
3581 IkReal>
3582 x664
3583 = IKPowWithIntegerCheck(
3584 ((((0.09)
3585 * x659))
3586 + (((0.09)
3587 * x660))),
3588 -1);
3589 if (!x664.valid)
3590 {
3591 continue;
3592 }
3593 if (IKabs((
3594 (((1.17647058823529)
3595 * x660))
3596 + (((1.17647058823529)
3597 * x659))))
3598 < IKFAST_ATAN2_MAGTHRESH
3599 && IKabs((
3600 (x664.value)
3601 * (((0.2125)
3602 + (((-2.58823529411765)
3603 * cj4
3604 * px
3605 * x660))
3606 + (((-0.294117647058824)
3607 * x662))
3608 + (((-1.0)
3609 * x661
3610 * x663))
3611 + ((x662
3612 * x663))
3613 + x661))))
3614 < IKFAST_ATAN2_MAGTHRESH
3615 && IKabs(
3616 IKsqr((
3617 (((1.17647058823529)
3618 * x660))
3619 + (((1.17647058823529)
3620 * x659))))
3621 + IKsqr((
3622 (x664.value)
3623 * (((0.2125)
3624 + (((-2.58823529411765)
3625 * cj4
3626 * px
3627 * x660))
3628 + (((-0.294117647058824)
3629 * x662))
3630 + (((-1.0)
3631 * x661
3632 * x663))
3633 + ((x662
3634 * x663))
3635 + x661))))
3636 - 1)
3637 <= IKFAST_SINCOS_THRESH)
3638 continue;
3639 j6array[0] = IKatan2(
3640 ((((1.17647058823529)
3641 * x660))
3642 + (((1.17647058823529)
3643 * x659))),
3644 ((x664.value)
3645 * (((0.2125)
3646 + (((-2.58823529411765)
3647 * cj4
3648 * px
3649 * x660))
3650 + (((-0.294117647058824)
3651 * x662))
3652 + (((-1.0)
3653 * x661
3654 * x663))
3655 + ((x662
3656 * x663))
3657 + x661))));
3658 sj6array[0] = IKsin(
3659 j6array
3660 [0]);
3661 cj6array[0] = IKcos(
3662 j6array
3663 [0]);
3664 if (j6array
3665 [0]
3666 > IKPI)
3667 {
3668 j6array[0]
3669 -= IK2PI;
3670 }
3671 else if (
3672 j6array
3673 [0]
3674 < -IKPI)
3675 {
3676 j6array[0]
3677 += IK2PI;
3678 }
3679 j6valid[0]
3680 = true;
3681 for (int ij6
3682 = 0;
3683 ij6
3684 < 1;
3685 ++ij6)
3686 {
3687 if (!j6valid
3688 [ij6])
3689 {
3690 continue;
3691 }
3692 _ij6[0]
3693 = ij6;
3694 _ij6[1]
3695 = -1;
3696 for (
3697 int iij6
3698 = ij6
3699 + 1;
3700 iij6
3701 < 1;
3702 ++iij6)
3703 {
3704 if (j6valid
3705 [iij6]
3706 && IKabs(
3707 cj6array
3708 [ij6]
3709 - cj6array
3710 [iij6])
3711 < IKFAST_SOLUTION_THRESH
3712 && IKabs(
3713 sj6array
3714 [ij6]
3715 - sj6array
3716 [iij6])
3717 < IKFAST_SOLUTION_THRESH)
3718 {
3719 j6valid
3720 [iij6]
3721 = false;
3722 _ij6[1]
3723 = iij6;
3724 break;
3725 }
3726 }
3727 j6 = j6array
3728 [ij6];
3729 cj6 = cj6array
3730 [ij6];
3731 sj6 = sj6array
3732 [ij6];
3733 {
3734 IkReal evalcond
3735 [5];
3736 IkReal
3737 x665
3738 = IKsin(
3739 j6);
3740 IkReal
3741 x666
3742 = (cj4
3743 * px);
3744 IkReal
3745 x667
3746 = (x665
3747 * x666);
3748 IkReal
3749 x668
3750 = (py
3751 * sj4);
3752 IkReal
3753 x669
3754 = (x665
3755 * x668);
3756 IkReal
3757 x670
3758 = ((1.0)
3759 * x666);
3760 IkReal
3761 x671
3762 = ((1.0)
3763 * x668);
3764 IkReal
3765 x672
3766 = IKcos(
3767 j6);
3768 IkReal
3769 x673
3770 = px
3771 * px;
3772 IkReal
3773 x674
3774 = ((3.92156862745098)
3775 * x665);
3776 IkReal
3777 x675
3778 = ((0.588235294117647)
3779 * x672);
3780 IkReal
3781 x676
3782 = py
3783 * py;
3784 IkReal
3785 x677
3786 = ((0.09)
3787 * x672);
3788 evalcond
3789 [0]
3790 = ((-0.85)
3791 + x667
3792 + x669);
3793 evalcond
3794 [1]
3795 = ((((0.85)
3796 * x665))
3797 + (((-1.0)
3798 * x670))
3799 + (((-1.0)
3800 * x671)));
3801 evalcond
3802 [2]
3803 = ((((-1.0)
3804 * x671
3805 * x672))
3806 + (((-1.0)
3807 * x670
3808 * x672)));
3809 evalcond
3810 [3]
3811 = ((((-2.83333333333333)
3812 * x665))
3813 + (((-0.425)
3814 * x672))
3815 + ((x673
3816 * x674))
3817 + (((-1.0)
3818 * x675
3819 * x676))
3820 + ((x674
3821 * x676))
3822 + (((-1.0)
3823 * x673
3824 * x675)));
3825 evalcond
3826 [4]
3827 = ((-0.2125)
3828 + ((x668
3829 * x677))
3830 + (((-1.0)
3831 * x676))
3832 + ((x666
3833 * x677))
3834 + (((1.1)
3835 * x667))
3836 + (((-1.0)
3837 * x673))
3838 + (((1.1)
3839 * x669)));
3840 if (IKabs(
3841 evalcond
3842 [0])
3843 > IKFAST_EVALCOND_THRESH
3844 || IKabs(
3845 evalcond
3846 [1])
3847 > IKFAST_EVALCOND_THRESH
3848 || IKabs(
3849 evalcond
3850 [2])
3851 > IKFAST_EVALCOND_THRESH
3852 || IKabs(
3853 evalcond
3854 [3])
3855 > IKFAST_EVALCOND_THRESH
3856 || IKabs(
3857 evalcond
3858 [4])
3859 > IKFAST_EVALCOND_THRESH)
3860 {
3861 continue;
3862 }
3863 }
3864
3865 rotationfunction0(
3866 solutions);
3867 }
3868 }
3869 }
3870 }
3871 }
3872 } while (0);
3873 if (bgotonextstatement)
3874 {
3875 bool
3876 bgotonextstatement
3877 = true;
3878 do
3879 {
3880 if (1)
3881 {
3882 bgotonextstatement
3883 = false;
3884 continue; // branch
3885 // miss
3886 // [j6]
3887 }
3888 } while (0);
3889 if (bgotonextstatement)
3890 {
3891 }
3892 }
3893 }
3894 }
3895 else
3896 {
3897 {
3898 IkReal j6array[1],
3899 cj6array[1],
3900 sj6array[1];
3901 bool j6valid[1]
3902 = {false};
3903 _nj6 = 1;
3904 IkReal x678
3905 = (cj4 * px);
3906 IkReal x679
3907 = (py * sj4);
3908 IkReal x680
3909 = ((0.108264705882353)
3910 * cj9);
3911 IkReal x681
3912 = ((0.588235294117647)
3913 * pp);
3914 IkReal x682
3915 = (cj9 * pp);
3916 IkReal x683
3917 = (cj9 * sj9);
3918 IkReal x684
3919 = (pp * sj9);
3920 IkReal x685
3921 = cj9 * cj9;
3922 IkReal x686
3923 = ((1.0) * pz);
3924 CheckValue<IkReal> x687 = IKPowWithIntegerCheck(
3925 IKsign((
3926 (((-1.0)
3927 * x678
3928 * x681))
3929 + (((-1.0)
3930 * x679
3931 * x680))
3932 + (((1.51009803921569)
3933 * pz))
3934 + (((1.32323529411765)
3935 * cj9
3936 * pz))
3937 + (((-1.0)
3938 * (3.92156862745098)
3939 * pp
3940 * pz))
3941 + (((-1.0)
3942 * x678
3943 * x680))
3944 + (((-0.316735294117647)
3945 * x679))
3946 + (((-0.316735294117647)
3947 * x678))
3948 + (((-1.0)
3949 * x679
3950 * x681)))),
3951 -1);
3952 if (!x687.valid)
3953 {
3954 continue;
3955 }
3956 CheckValue<IkReal> x688 = IKatan2WithCheck(
3957 IkReal((
3958 (-0.174204411764706)
3959 + (((-0.0264705882352941)
3960 * x684))
3961 + (((-0.00487191176470588)
3962 * x683))
3963 + (pz * pz)
3964 + (((-0.0324794117647059)
3965 * x685))
3966 + (((-1.0)
3967 * (0.154566176470588)
3968 * cj9))
3969 + (((-0.176470588235294)
3970 * x682))
3971 + (((-1.0)
3972 * (0.323529411764706)
3973 * pp))
3974 + (((-1.0)
3975 * (0.0142530882352941)
3976 * sj9)))),
3977 ((0.830553921568627)
3978 + (((-1.17647058823529)
3979 * x682))
3980 + (((0.396970588235294)
3981 * x685))
3982 + (((1.18080882352941)
3983 * cj9))
3984 + (((0.0595455882352941)
3985 * x683))
3986 + (((-1.0)
3987 * (2.15686274509804)
3988 * pp))
3989 + (((-1.0) * x678
3990 * x686))
3991 + (((-1.0) * x679
3992 * x686))
3993 + (((-0.176470588235294)
3994 * x684))
3995 + (((0.0679544117647059)
3996 * sj9))),
3997 IKFAST_ATAN2_MAGTHRESH);
3998 if (!x688.valid)
3999 {
4000 continue;
4001 }
4002 j6array[0]
4003 = ((-1.5707963267949)
4004 + (((1.5707963267949)
4005 * (x687.value)))
4006 + (x688.value));
4007 sj6array[0] = IKsin(
4008 j6array[0]);
4009 cj6array[0] = IKcos(
4010 j6array[0]);
4011 if (j6array[0] > IKPI)
4012 {
4013 j6array[0] -= IK2PI;
4014 }
4015 else if (
4016 j6array[0]
4017 < -IKPI)
4018 {
4019 j6array[0] += IK2PI;
4020 }
4021 j6valid[0] = true;
4022 for (int ij6 = 0;
4023 ij6 < 1;
4024 ++ij6)
4025 {
4026 if (!j6valid[ij6])
4027 {
4028 continue;
4029 }
4030 _ij6[0] = ij6;
4031 _ij6[1] = -1;
4032 for (int iij6
4033 = ij6 + 1;
4034 iij6 < 1;
4035 ++iij6)
4036 {
4037 if (j6valid[iij6]
4038 && IKabs(
4039 cj6array
4040 [ij6]
4041 - cj6array
4042 [iij6])
4043 < IKFAST_SOLUTION_THRESH
4044 && IKabs(
4045 sj6array
4046 [ij6]
4047 - sj6array
4048 [iij6])
4049 < IKFAST_SOLUTION_THRESH)
4050 {
4051 j6valid[iij6]
4052 = false;
4053 _ij6[1] = iij6;
4054 break;
4055 }
4056 }
4057 j6 = j6array[ij6];
4058 cj6 = cj6array[ij6];
4059 sj6 = sj6array[ij6];
4060 {
4061 IkReal
4062 evalcond[5];
4063 IkReal x689
4064 = ((0.3)
4065 * cj9);
4066 IkReal x690
4067 = ((0.045)
4068 * sj9);
4069 IkReal x691
4070 = IKcos(j6);
4071 IkReal x692
4072 = (pz * x691);
4073 IkReal x693
4074 = IKsin(j6);
4075 IkReal x694
4076 = (cj4 * px);
4077 IkReal x695
4078 = (x693
4079 * x694);
4080 IkReal x696
4081 = (py * sj4);
4082 IkReal x697
4083 = (x693
4084 * x696);
4085 IkReal x698
4086 = ((0.045)
4087 * cj9);
4088 IkReal x699
4089 = ((0.3)
4090 * sj9);
4091 IkReal x700
4092 = (pz * x693);
4093 IkReal x701
4094 = ((1.0)
4095 * x694);
4096 IkReal x702
4097 = ((1.0)
4098 * x696);
4099 IkReal x703
4100 = ((0.09)
4101 * x691);
4102 evalcond[0]
4103 = ((-0.55)
4104 + (((-1.0)
4105 * x689))
4106 + (((-1.0)
4107 * x690))
4108 + x692
4109 + x695
4110 + x697);
4111 evalcond[1]
4112 = ((0.045)
4113 + x700
4114 + (((-1.0)
4115 * x691
4116 * x701))
4117 + (((-1.0)
4118 * x698))
4119 + x699
4120 + (((-1.0)
4121 * x691
4122 * x702)));
4123 evalcond[2]
4124 = ((((-1.51009803921569)
4125 * x693))
4126 + (((-0.588235294117647)
4127 * pp
4128 * x691))
4129 + (((3.92156862745098)
4130 * pp
4131 * x693))
4132 + pz
4133 + (((-1.32323529411765)
4134 * cj9
4135 * x693))
4136 + (((-0.316735294117647)
4137 * x691))
4138 + (((-0.108264705882353)
4139 * cj9
4140 * x691)));
4141 evalcond[3]
4142 = ((((-1.0)
4143 * x691
4144 * x698))
4145 + (((-1.0)
4146 * x702))
4147 + ((x690
4148 * x693))
4149 + ((x691
4150 * x699))
4151 + (((0.55)
4152 * x693))
4153 + (((0.045)
4154 * x691))
4155 + ((x689
4156 * x693))
4157 + (((-1.0)
4158 * x701)));
4159 evalcond[4]
4160 = ((-0.2125)
4161 + ((x696
4162 * x703))
4163 + (((1.1)
4164 * x695))
4165 + (((-0.09)
4166 * x700))
4167 + (((1.1)
4168 * x692))
4169 + (((1.1)
4170 * x697))
4171 + ((x694
4172 * x703))
4173 + (((-1.0)
4174 * (1.0)
4175 * pp)));
4176 if (IKabs(evalcond
4177 [0])
4178 > IKFAST_EVALCOND_THRESH
4179 || IKabs(
4180 evalcond
4181 [1])
4182 > IKFAST_EVALCOND_THRESH
4183 || IKabs(
4184 evalcond
4185 [2])
4186 > IKFAST_EVALCOND_THRESH
4187 || IKabs(
4188 evalcond
4189 [3])
4190 > IKFAST_EVALCOND_THRESH
4191 || IKabs(
4192 evalcond
4193 [4])
4194 > IKFAST_EVALCOND_THRESH)
4195 {
4196 continue;
4197 }
4198 }
4199
4200 rotationfunction0(
4201 solutions);
4202 }
4203 }
4204 }
4205 }
4206 }
4207 else
4208 {
4209 {
4210 IkReal j6array[1],
4211 cj6array[1],
4212 sj6array[1];
4213 bool j6valid[1] = {false};
4214 _nj6 = 1;
4215 IkReal x704
4216 = ((0.045) * cj4
4217 * px);
4218 IkReal x705
4219 = ((0.045) * py
4220 * sj4);
4221 IkReal x706
4222 = ((0.3) * sj9);
4223 IkReal x707 = (cj4 * px);
4224 IkReal x708 = (py * sj4);
4225 IkReal x709 = (cj9 * sj9);
4226 IkReal x710 = cj9 * cj9;
4227 IkReal x711
4228 = ((1.0) * pz);
4229 IkReal x712 = py * py;
4230 IkReal x713 = cj4 * cj4;
4231 CheckValue<IkReal> x714
4232 = IKatan2WithCheck(
4233 IkReal((
4234 (0.03825)
4235 + (((0.087975)
4236 * x709))
4237 + (((-0.027)
4238 * x710))
4239 + (((-1.0)
4240 * x707
4241 * x711))
4242 + (((0.167025)
4243 * sj9))
4244 + (((-1.0)
4245 * (0.01125)
4246 * cj9))
4247 + (((-1.0)
4248 * x708
4249 * x711)))),
4250 ((-0.304525)
4251 + (((-1.0)
4252 * (0.0495)
4253 * sj9))
4254 + (((-0.087975)
4255 * x710))
4256 + (((2.0) * cj4
4257 * px * x708))
4258 + x712
4259 + (((-1.0) * x712
4260 * x713))
4261 + (((-1.0)
4262 * (0.33)
4263 * cj9))
4264 + ((x713
4265 * (px * px)))
4266 + (((-0.027)
4267 * x709))),
4268 IKFAST_ATAN2_MAGTHRESH);
4269 if (!x714.valid)
4270 {
4271 continue;
4272 }
4273 CheckValue<IkReal> x715
4274 = IKPowWithIntegerCheck(
4275 IKsign((
4276 (((-1.0) * cj9
4277 * x704))
4278 + (((-1.0)
4279 * (0.55)
4280 * pz))
4281 + ((x706
4282 * x708))
4283 + ((x706
4284 * x707))
4285 + x705 + x704
4286 + (((-1.0)
4287 * cj9
4288 * x705))
4289 + (((-1.0)
4290 * (0.045)
4291 * pz
4292 * sj9))
4293 + (((-1.0)
4294 * (0.3)
4295 * cj9
4296 * pz)))),
4297 -1);
4298 if (!x715.valid)
4299 {
4300 continue;
4301 }
4302 j6array[0]
4303 = ((-1.5707963267949)
4304 + (x714.value)
4305 + (((1.5707963267949)
4306 * (x715.value))));
4307 sj6array[0]
4308 = IKsin(j6array[0]);
4309 cj6array[0]
4310 = IKcos(j6array[0]);
4311 if (j6array[0] > IKPI)
4312 {
4313 j6array[0] -= IK2PI;
4314 }
4315 else if (
4316 j6array[0] < -IKPI)
4317 {
4318 j6array[0] += IK2PI;
4319 }
4320 j6valid[0] = true;
4321 for (int ij6 = 0; ij6 < 1;
4322 ++ij6)
4323 {
4324 if (!j6valid[ij6])
4325 {
4326 continue;
4327 }
4328 _ij6[0] = ij6;
4329 _ij6[1] = -1;
4330 for (int iij6 = ij6 + 1;
4331 iij6 < 1;
4332 ++iij6)
4333 {
4334 if (j6valid[iij6]
4335 && IKabs(
4336 cj6array
4337 [ij6]
4338 - cj6array
4339 [iij6])
4340 < IKFAST_SOLUTION_THRESH
4341 && IKabs(
4342 sj6array
4343 [ij6]
4344 - sj6array
4345 [iij6])
4346 < IKFAST_SOLUTION_THRESH)
4347 {
4348 j6valid[iij6]
4349 = false;
4350 _ij6[1] = iij6;
4351 break;
4352 }
4353 }
4354 j6 = j6array[ij6];
4355 cj6 = cj6array[ij6];
4356 sj6 = sj6array[ij6];
4357 {
4358 IkReal evalcond[5];
4359 IkReal x716
4360 = ((0.3) * cj9);
4361 IkReal x717
4362 = ((0.045) * sj9);
4363 IkReal x718
4364 = IKcos(j6);
4365 IkReal x719
4366 = (pz * x718);
4367 IkReal x720
4368 = IKsin(j6);
4369 IkReal x721
4370 = (cj4 * px);
4371 IkReal x722
4372 = (x720 * x721);
4373 IkReal x723
4374 = (py * sj4);
4375 IkReal x724
4376 = (x720 * x723);
4377 IkReal x725
4378 = ((0.045) * cj9);
4379 IkReal x726
4380 = ((0.3) * sj9);
4381 IkReal x727
4382 = (pz * x720);
4383 IkReal x728
4384 = ((1.0) * x721);
4385 IkReal x729
4386 = ((1.0) * x723);
4387 IkReal x730
4388 = ((0.09) * x718);
4389 evalcond[0]
4390 = ((-0.55) + x719
4391 + (((-1.0)
4392 * x716))
4393 + (((-1.0)
4394 * x717))
4395 + x724 + x722);
4396 evalcond[1]
4397 = ((0.045)
4398 + (((-1.0)
4399 * x718
4400 * x729))
4401 + (((-1.0)
4402 * x725))
4403 + (((-1.0)
4404 * x718
4405 * x728))
4406 + x727 + x726);
4407 evalcond[2]
4408 = ((((3.92156862745098)
4409 * pp * x720))
4410 + (((-1.32323529411765)
4411 * cj9
4412 * x720))
4413 + (((-0.108264705882353)
4414 * cj9
4415 * x718))
4416 + (((-1.51009803921569)
4417 * x720))
4418 + pz
4419 + (((-0.588235294117647)
4420 * pp
4421 * x718))
4422 + (((-0.316735294117647)
4423 * x718)));
4424 evalcond[3]
4425 = (((x717 * x720))
4426 + (((-1.0)
4427 * x718
4428 * x725))
4429 + (((0.045)
4430 * x718))
4431 + ((x716
4432 * x720))
4433 + (((-1.0)
4434 * x728))
4435 + (((0.55)
4436 * x720))
4437 + (((-1.0)
4438 * x729))
4439 + ((x718
4440 * x726)));
4441 evalcond[4]
4442 = ((-0.2125)
4443 + (((1.1)
4444 * x722))
4445 + (((-0.09)
4446 * x727))
4447 + (((1.1)
4448 * x719))
4449 + ((x723
4450 * x730))
4451 + (((1.1)
4452 * x724))
4453 + (((-1.0)
4454 * (1.0)
4455 * pp))
4456 + ((x721
4457 * x730)));
4458 if (IKabs(evalcond[0])
4459 > IKFAST_EVALCOND_THRESH
4460 || IKabs(evalcond
4461 [1])
4462 > IKFAST_EVALCOND_THRESH
4463 || IKabs(evalcond
4464 [2])
4465 > IKFAST_EVALCOND_THRESH
4466 || IKabs(evalcond
4467 [3])
4468 > IKFAST_EVALCOND_THRESH
4469 || IKabs(evalcond
4470 [4])
4471 > IKFAST_EVALCOND_THRESH)
4472 {
4473 continue;
4474 }
4475 }
4476
4477 rotationfunction0(
4478 solutions);
4479 }
4480 }
4481 }
4482 }
4483 }
4484 else
4485 {
4486 {
4487 IkReal j6array[1],
4488 cj6array[1], sj6array[1];
4489 bool j6valid[1] = {false};
4490 _nj6 = 1;
4491 IkReal x731 = (cj4 * px);
4492 IkReal x732 = (py * sj4);
4493 IkReal x733
4494 = ((1.32323529411765)
4495 * cj9);
4496 IkReal x734
4497 = ((3.92156862745098)
4498 * pp);
4499 IkReal x735
4500 = ((0.0264705882352941)
4501 * pp);
4502 IkReal x736 = (cj9 * sj9);
4503 IkReal x737
4504 = ((0.176470588235294)
4505 * pp);
4506 IkReal x738 = cj9 * cj9;
4507 CheckValue<IkReal> x739 = IKatan2WithCheck(
4508 IkReal((
4509 (-0.0142530882352941)
4510 + (((-0.0324794117647059)
4511 * x736))
4512 + (((0.00938117647058823)
4513 * cj9))
4514 + ((pz * x732))
4515 + ((pz * x731))
4516 + (((-1.0) * x735))
4517 + (((0.00487191176470588)
4518 * x738))
4519 + (((-1.0)
4520 * (0.0950205882352941)
4521 * sj9))
4522 + ((cj9 * x735))
4523 + (((-1.0) * sj9
4524 * x737)))),
4525 ((0.0679544117647059)
4526 + (((-1.0) * x737))
4527 + ((cj9 * x737))
4528 + (pz * pz)
4529 + (((-1.0)
4530 * (0.00840882352941177)
4531 * cj9))
4532 + (((-0.0595455882352941)
4533 * x738))
4534 + (((0.453029411764706)
4535 * sj9))
4536 + (((0.396970588235294)
4537 * x736))
4538 + (((-1.0)
4539 * (1.17647058823529)
4540 * pp * sj9))),
4541 IKFAST_ATAN2_MAGTHRESH);
4542 if (!x739.valid)
4543 {
4544 continue;
4545 }
4546 CheckValue<IkReal> x740 = IKPowWithIntegerCheck(
4547 IKsign((
4548 (((0.316735294117647)
4549 * pz))
4550 + (((0.108264705882353)
4551 * cj9 * pz))
4552 + (((0.588235294117647)
4553 * pp * pz))
4554 + ((x732 * x733))
4555 + (((-1.0) * x732
4556 * x734))
4557 + (((1.51009803921569)
4558 * x731))
4559 + (((1.51009803921569)
4560 * x732))
4561 + ((x731 * x733))
4562 + (((-1.0) * x731
4563 * x734)))),
4564 -1);
4565 if (!x740.valid)
4566 {
4567 continue;
4568 }
4569 j6array[0]
4570 = ((-1.5707963267949)
4571 + (x739.value)
4572 + (((1.5707963267949)
4573 * (x740.value))));
4574 sj6array[0]
4575 = IKsin(j6array[0]);
4576 cj6array[0]
4577 = IKcos(j6array[0]);
4578 if (j6array[0] > IKPI)
4579 {
4580 j6array[0] -= IK2PI;
4581 }
4582 else if (j6array[0] < -IKPI)
4583 {
4584 j6array[0] += IK2PI;
4585 }
4586 j6valid[0] = true;
4587 for (int ij6 = 0; ij6 < 1;
4588 ++ij6)
4589 {
4590 if (!j6valid[ij6])
4591 {
4592 continue;
4593 }
4594 _ij6[0] = ij6;
4595 _ij6[1] = -1;
4596 for (int iij6 = ij6 + 1;
4597 iij6 < 1;
4598 ++iij6)
4599 {
4600 if (j6valid[iij6]
4601 && IKabs(
4602 cj6array[ij6]
4603 - cj6array
4604 [iij6])
4605 < IKFAST_SOLUTION_THRESH
4606 && IKabs(
4607 sj6array[ij6]
4608 - sj6array
4609 [iij6])
4610 < IKFAST_SOLUTION_THRESH)
4611 {
4612 j6valid[iij6] = false;
4613 _ij6[1] = iij6;
4614 break;
4615 }
4616 }
4617 j6 = j6array[ij6];
4618 cj6 = cj6array[ij6];
4619 sj6 = sj6array[ij6];
4620 {
4621 IkReal evalcond[5];
4622 IkReal x741
4623 = ((0.3) * cj9);
4624 IkReal x742
4625 = ((0.045) * sj9);
4626 IkReal x743 = IKcos(j6);
4627 IkReal x744 = (pz * x743);
4628 IkReal x745 = IKsin(j6);
4629 IkReal x746 = (cj4 * px);
4630 IkReal x747
4631 = (x745 * x746);
4632 IkReal x748 = (py * sj4);
4633 IkReal x749
4634 = (x745 * x748);
4635 IkReal x750
4636 = ((0.045) * cj9);
4637 IkReal x751
4638 = ((0.3) * sj9);
4639 IkReal x752 = (pz * x745);
4640 IkReal x753
4641 = ((1.0) * x746);
4642 IkReal x754
4643 = ((1.0) * x748);
4644 IkReal x755
4645 = ((0.09) * x743);
4646 evalcond[0]
4647 = ((-0.55)
4648 + (((-1.0) * x741))
4649 + x749 + x744
4650 + x747
4651 + (((-1.0)
4652 * x742)));
4653 evalcond[1]
4654 = ((0.045)
4655 + (((-1.0) * x743
4656 * x753))
4657 + (((-1.0) * x743
4658 * x754))
4659 + x752 + x751
4660 + (((-1.0)
4661 * x750)));
4662 evalcond[2]
4663 = ((((-0.316735294117647)
4664 * x743))
4665 + (((-1.32323529411765)
4666 * cj9 * x745))
4667 + pz
4668 + (((-0.108264705882353)
4669 * cj9 * x743))
4670 + (((-1.51009803921569)
4671 * x745))
4672 + (((-0.588235294117647)
4673 * pp * x743))
4674 + (((3.92156862745098)
4675 * pp * x745)));
4676 evalcond[3]
4677 = ((((-1.0) * x753))
4678 + (((0.55) * x745))
4679 + (((0.045)
4680 * x743))
4681 + ((x741 * x745))
4682 + ((x743 * x751))
4683 + (((-1.0) * x743
4684 * x750))
4685 + (((-1.0) * x754))
4686 + ((x742 * x745)));
4687 evalcond[4]
4688 = ((-0.2125)
4689 + (((1.1) * x749))
4690 + (((-0.09)
4691 * x752))
4692 + ((x748 * x755))
4693 + (((1.1) * x744))
4694 + (((-1.0) * (1.0)
4695 * pp))
4696 + ((x746 * x755))
4697 + (((1.1)
4698 * x747)));
4699 if (IKabs(evalcond[0])
4700 > IKFAST_EVALCOND_THRESH
4701 || IKabs(evalcond[1])
4702 > IKFAST_EVALCOND_THRESH
4703 || IKabs(evalcond[2])
4704 > IKFAST_EVALCOND_THRESH
4705 || IKabs(evalcond[3])
4706 > IKFAST_EVALCOND_THRESH
4707 || IKabs(evalcond[4])
4708 > IKFAST_EVALCOND_THRESH)
4709 {
4710 continue;
4711 }
4712 }
4713
4714 rotationfunction0(
4715 solutions);
4716 }
4717 }
4718 }
4719 }
4720 }
4721 } while (0);
4722 if (bgotonextstatement)
4723 {
4724 bool bgotonextstatement = true;
4725 do
4726 {
4727 IkReal x756 = (px * sj4);
4728 IkReal x757 = (cj4 * py);
4729 evalcond[0]
4730 = ((-3.14159265358979)
4731 + (IKfmod(
4732 ((3.14159265358979)
4733 + (IKabs((
4734 (-3.14159265358979)
4735 + j8)))),
4736 6.28318530717959)));
4737 evalcond[1]
4738 = ((0.39655)
4739 + (((0.0765) * sj9))
4740 + (((0.32595) * cj9))
4741 + (((-1.0) * (1.0) * pp)));
4742 evalcond[2]
4743 = ((((-1.0) * x757)) + x756);
4744 evalcond[3]
4745 = ((((-1.0) * x756)) + x757);
4746 if (IKabs(evalcond[0])
4747 < 0.0000010000000000
4748 && IKabs(evalcond[1])
4749 < 0.0000010000000000
4750 && IKabs(evalcond[2])
4751 < 0.0000010000000000
4752 && IKabs(evalcond[3])
4753 < 0.0000010000000000)
4754 {
4755 bgotonextstatement = false;
4756 {
4757 IkReal j6eval[2];
4758 sj8 = 0;
4759 cj8 = -1.0;
4760 j8 = 3.14159265358979;
4761 IkReal x758 = py * py;
4762 IkReal x759 = cj4 * cj4;
4763 IkReal x760
4764 = ((pz * pz)
4765 + ((x759 * (px * px)))
4766 + (((2.0) * cj4 * px * py
4767 * sj4))
4768 + x758
4769 + (((-1.0) * x758
4770 * x759)));
4771 j6eval[0] = x760;
4772 j6eval[1] = IKsign(x760);
4773 if (IKabs(j6eval[0])
4774 < 0.0000010000000000
4775 || IKabs(j6eval[1])
4776 < 0.0000010000000000)
4777 {
4778 {
4779 IkReal j6eval[2];
4780 sj8 = 0;
4781 cj8 = -1.0;
4782 j8 = 3.14159265358979;
4783 IkReal x761 = (cj4 * px);
4784 IkReal x762 = (cj9 * pz);
4785 IkReal x763 = (py * sj4);
4786 IkReal x764 = (pz * sj9);
4787 IkReal x765
4788 = (cj4 * px * sj9);
4789 IkReal x766
4790 = (py * sj4 * sj9);
4791 IkReal x767
4792 = ((0.045) * x761);
4793 IkReal x768
4794 = ((0.045) * x763);
4795 j6eval[0]
4796 = (((cj9 * x761))
4797 + (((-1.0)
4798 * (12.2222222222222)
4799 * pz))
4800 + (((-1.0) * x764))
4801 + (((-6.66666666666667)
4802 * x765))
4803 + (((-1.0) * x763))
4804 + (((-6.66666666666667)
4805 * x766))
4806 + ((cj9 * x763))
4807 + (((-6.66666666666667)
4808 * x762))
4809 + (((-1.0) * x761)));
4810 j6eval[1] = IKsign(
4811 ((((-0.3) * x766))
4812 + (((-1.0) * x767))
4813 + (((-1.0) * (0.55)
4814 * pz))
4815 + ((cj9 * x768))
4816 + (((-0.3) * x765))
4817 + (((-1.0) * x768))
4818 + (((-0.3) * x762))
4819 + (((-0.045) * x764))
4820 + ((cj9 * x767))));
4821 if (IKabs(j6eval[0])
4822 < 0.0000010000000000
4823 || IKabs(j6eval[1])
4824 < 0.0000010000000000)
4825 {
4826 {
4827 IkReal j6eval[2];
4828 sj8 = 0;
4829 cj8 = -1.0;
4830 j8 = 3.14159265358979;
4831 IkReal x769
4832 = (cj4 * px);
4833 IkReal x770
4834 = (cj9 * pz);
4835 IkReal x771 = (pp * pz);
4836 IkReal x772
4837 = (py * sj4);
4838 IkReal x773
4839 = (cj4 * cj9 * px);
4840 IkReal x774
4841 = (cj4 * pp * px);
4842 IkReal x775
4843 = (cj9 * py * sj4);
4844 IkReal x776
4845 = (pp * py * sj4);
4846 j6eval[0]
4847 = ((((-1.0) * x775))
4848 + (((-1.0)
4849 * (13.9482024812098)
4850 * pz))
4851 + (((-2.92556370551481)
4852 * x772))
4853 + (((-12.2222222222222)
4854 * x770))
4855 + (((-5.4333061668025)
4856 * x776))
4857 + (((-1.0)
4858 * x773))
4859 + (((-5.4333061668025)
4860 * x774))
4861 + (((-2.92556370551481)
4862 * x769))
4863 + (((36.2220411120167)
4864 * x771)));
4865 j6eval[1] = IKsign((
4866 (((-0.588235294117647)
4867 * x776))
4868 + (((-0.108264705882353)
4869 * x773))
4870 + (((3.92156862745098)
4871 * x771))
4872 + (((-0.316735294117647)
4873 * x772))
4874 + (((-1.0)
4875 * (1.51009803921569)
4876 * pz))
4877 + (((-0.316735294117647)
4878 * x769))
4879 + (((-1.32323529411765)
4880 * x770))
4881 + (((-0.588235294117647)
4882 * x774))
4883 + (((-0.108264705882353)
4884 * x775))));
4885 if (IKabs(j6eval[0])
4886 < 0.0000010000000000
4887 || IKabs(j6eval[1])
4888 < 0.0000010000000000)
4889 {
4890 {
4891 IkReal evalcond[1];
4892 bool
4893 bgotonextstatement
4894 = true;
4895 do
4896 {
4897 evalcond[0]
4898 = ((IKabs((
4899 (-3.14159265358979)
4900 + (IKfmod(
4901 ((3.14159265358979)
4902 + j9),
4903 6.28318530717959)))))
4904 + (IKabs(
4905 pz)));
4906 if (IKabs(evalcond
4907 [0])
4908 < 0.0000010000000000)
4909 {
4910 bgotonextstatement
4911 = false;
4912 {
4913 IkReal
4914 j6eval[1];
4915 IkReal x777
4916 = ((1.0)
4917 * py);
4918 sj8 = 0;
4919 cj8 = -1.0;
4920 j8 = 3.14159265358979;
4921 pz = 0;
4922 j9 = 0;
4923 sj9 = 0;
4924 cj9 = 1.0;
4925 pp
4926 = ((py
4927 * py)
4928 + (px
4929 * px));
4930 npx
4931 = (((py
4932 * r10))
4933 + ((px
4934 * r00)));
4935 npy
4936 = (((py
4937 * r11))
4938 + ((px
4939 * r01)));
4940 npz
4941 = (((px
4942 * r02))
4943 + ((py
4944 * r12)));
4945 rxp0_0
4946 = ((-1.0)
4947 * r20
4948 * x777);
4949 rxp0_1
4950 = (px
4951 * r20);
4952 rxp1_0
4953 = ((-1.0)
4954 * r21
4955 * x777);
4956 rxp1_1
4957 = (px
4958 * r21);
4959 rxp2_0
4960 = ((-1.0)
4961 * r22
4962 * x777);
4963 rxp2_1
4964 = (px
4965 * r22);
4966 j6eval[0]
4967 = ((((-1.0)
4968 * (1.0)
4969 * cj4
4970 * px))
4971 + (((-1.0)
4972 * (1.0)
4973 * py
4974 * sj4)));
4975 if (IKabs(
4976 j6eval
4977 [0])
4978 < 0.0000010000000000)
4979 {
4980 {
4981 IkReal j6eval
4982 [1];
4983 IkReal
4984 x778
4985 = ((1.0)
4986 * py);
4987 sj8 = 0;
4988 cj8 = -1.0;
4989 j8 = 3.14159265358979;
4990 pz = 0;
4991 j9 = 0;
4992 sj9 = 0;
4993 cj9 = 1.0;
4994 pp
4995 = ((py
4996 * py)
4997 + (px
4998 * px));
4999 npx
5000 = (((py
5001 * r10))
5002 + ((px
5003 * r00)));
5004 npy
5005 = (((py
5006 * r11))
5007 + ((px
5008 * r01)));
5009 npz
5010 = (((px
5011 * r02))
5012 + ((py
5013 * r12)));
5014 rxp0_0
5015 = ((-1.0)
5016 * r20
5017 * x778);
5018 rxp0_1
5019 = (px
5020 * r20);
5021 rxp1_0
5022 = ((-1.0)
5023 * r21
5024 * x778);
5025 rxp1_1
5026 = (px
5027 * r21);
5028 rxp2_0
5029 = ((-1.0)
5030 * r22
5031 * x778);
5032 rxp2_1
5033 = (px
5034 * r22);
5035 j6eval[0]
5036 = ((-1.0)
5037 + (((-1.0)
5038 * (1.3840830449827)
5039 * (px
5040 * px)))
5041 + (((-1.0)
5042 * (1.3840830449827)
5043 * (py
5044 * py))));
5045 if (IKabs(
5046 j6eval
5047 [0])
5048 < 0.0000010000000000)
5049 {
5050 {
5051 IkReal j6eval
5052 [2];
5053 IkReal
5054 x779
5055 = ((1.0)
5056 * py);
5057 sj8 = 0;
5058 cj8 = -1.0;
5059 j8 = 3.14159265358979;
5060 pz = 0;
5061 j9 = 0;
5062 sj9 = 0;
5063 cj9 = 1.0;
5064 pp
5065 = ((py
5066 * py)
5067 + (px
5068 * px));
5069 npx
5070 = (((py
5071 * r10))
5072 + ((px
5073 * r00)));
5074 npy
5075 = (((py
5076 * r11))
5077 + ((px
5078 * r01)));
5079 npz
5080 = (((px
5081 * r02))
5082 + ((py
5083 * r12)));
5084 rxp0_0
5085 = ((-1.0)
5086 * r20
5087 * x779);
5088 rxp0_1
5089 = (px
5090 * r20);
5091 rxp1_0
5092 = ((-1.0)
5093 * r21
5094 * x779);
5095 rxp1_1
5096 = (px
5097 * r21);
5098 rxp2_0
5099 = ((-1.0)
5100 * r22
5101 * x779);
5102 rxp2_1
5103 = (px
5104 * r22);
5105 IkReal
5106 x780
5107 = (cj4
5108 * px);
5109 IkReal
5110 x781
5111 = (py
5112 * sj4);
5113 j6eval
5114 [0]
5115 = (x780
5116 + x781);
5117 j6eval
5118 [1]
5119 = ((((-1.0)
5120 * (1.3840830449827)
5121 * cj4
5122 * (px
5123 * px
5124 * px)))
5125 + (((-1.0)
5126 * x780))
5127 + (((-1.3840830449827)
5128 * x780
5129 * (py
5130 * py)))
5131 + (((-1.0)
5132 * x781))
5133 + (((-1.0)
5134 * (1.3840830449827)
5135 * sj4
5136 * (py
5137 * py
5138 * py)))
5139 + (((-1.3840830449827)
5140 * x781
5141 * (px
5142 * px))));
5143 if (IKabs(
5144 j6eval
5145 [0])
5146 < 0.0000010000000000
5147 || IKabs(
5148 j6eval
5149 [1])
5150 < 0.0000010000000000)
5151 {
5152 {
5153 IkReal evalcond
5154 [4];
5155 bool
5156 bgotonextstatement
5157 = true;
5158 do
5159 {
5160 evalcond
5161 [0]
5162 = ((IKabs(
5163 py))
5164 + (IKabs(
5165 px)));
5166 evalcond
5167 [1]
5168 = -0.85;
5169 evalcond
5170 [2]
5171 = 0;
5172 evalcond
5173 [3]
5174 = -0.2125;
5175 if (IKabs(
5176 evalcond
5177 [0])
5178 < 0.0000010000000000
5179 && IKabs(
5180 evalcond
5181 [1])
5182 < 0.0000010000000000
5183 && IKabs(
5184 evalcond
5185 [2])
5186 < 0.0000010000000000
5187 && IKabs(
5188 evalcond
5189 [3])
5190 < 0.0000010000000000)
5191 {
5192 bgotonextstatement
5193 = false;
5194 {
5195 IkReal j6array
5196 [2],
5197 cj6array
5198 [2],
5199 sj6array
5200 [2];
5201 bool j6valid
5202 [2]
5203 = {false};
5204 _nj6
5205 = 2;
5206 j6array
5207 [0]
5208 = 0.148889947609497;
5209 sj6array
5210 [0]
5211 = IKsin(
5212 j6array
5213 [0]);
5214 cj6array
5215 [0]
5216 = IKcos(
5217 j6array
5218 [0]);
5219 j6array
5220 [1]
5221 = 3.29048260119929;
5222 sj6array
5223 [1]
5224 = IKsin(
5225 j6array
5226 [1]);
5227 cj6array
5228 [1]
5229 = IKcos(
5230 j6array
5231 [1]);
5232 if (j6array
5233 [0]
5234 > IKPI)
5235 {
5236 j6array
5237 [0]
5238 -= IK2PI;
5239 }
5240 else if (
5241 j6array
5242 [0]
5243 < -IKPI)
5244 {
5245 j6array
5246 [0]
5247 += IK2PI;
5248 }
5249 j6valid
5250 [0]
5251 = true;
5252 if (j6array
5253 [1]
5254 > IKPI)
5255 {
5256 j6array
5257 [1]
5258 -= IK2PI;
5259 }
5260 else if (
5261 j6array
5262 [1]
5263 < -IKPI)
5264 {
5265 j6array
5266 [1]
5267 += IK2PI;
5268 }
5269 j6valid
5270 [1]
5271 = true;
5272 for (
5273 int ij6
5274 = 0;
5275 ij6
5276 < 2;
5277 ++ij6)
5278 {
5279 if (!j6valid
5280 [ij6])
5281 {
5282 continue;
5283 }
5284 _ij6[0]
5285 = ij6;
5286 _ij6[1]
5287 = -1;
5288 for (
5289 int iij6
5290 = ij6
5291 + 1;
5292 iij6
5293 < 2;
5294 ++iij6)
5295 {
5296 if (j6valid
5297 [iij6]
5298 && IKabs(
5299 cj6array
5300 [ij6]
5301 - cj6array
5302 [iij6])
5303 < IKFAST_SOLUTION_THRESH
5304 && IKabs(
5305 sj6array
5306 [ij6]
5307 - sj6array
5308 [iij6])
5309 < IKFAST_SOLUTION_THRESH)
5310 {
5311 j6valid
5312 [iij6]
5313 = false;
5314 _ij6[1]
5315 = iij6;
5316 break;
5317 }
5318 }
5319 j6 = j6array
5320 [ij6];
5321 cj6 = cj6array
5322 [ij6];
5323 sj6 = sj6array
5324 [ij6];
5325 {
5326 IkReal evalcond
5327 [1];
5328 evalcond
5329 [0]
5330 = ((0.85)
5331 * (IKsin(
5332 j6)));
5333 if (IKabs(
5334 evalcond
5335 [0])
5336 > IKFAST_EVALCOND_THRESH)
5337 {
5338 continue;
5339 }
5340 }
5341
5342 rotationfunction0(
5343 solutions);
5344 }
5345 }
5346 }
5347 } while (
5348 0);
5349 if (bgotonextstatement)
5350 {
5351 bool
5352 bgotonextstatement
5353 = true;
5354 do
5355 {
5356 evalcond
5357 [0]
5358 = ((IKabs((
5359 (-3.14159265358979)
5360 + (IKfmod(
5361 ((3.14159265358979)
5362 + j4),
5363 6.28318530717959)))))
5364 + (IKabs(
5365 px)));
5366 evalcond
5367 [1]
5368 = -0.85;
5369 evalcond
5370 [2]
5371 = 0;
5372 evalcond
5373 [3]
5374 = ((-0.2125)
5375 + (((-1.0)
5376 * (1.0)
5377 * (py
5378 * py))));
5379 if (IKabs(
5380 evalcond
5381 [0])
5382 < 0.0000010000000000
5383 && IKabs(
5384 evalcond
5385 [1])
5386 < 0.0000010000000000
5387 && IKabs(
5388 evalcond
5389 [2])
5390 < 0.0000010000000000
5391 && IKabs(
5392 evalcond
5393 [3])
5394 < 0.0000010000000000)
5395 {
5396 bgotonextstatement
5397 = false;
5398 {
5399 IkReal j6eval
5400 [1];
5401 IkReal
5402 x782
5403 = ((1.0)
5404 * py);
5405 sj8 = 0;
5406 cj8 = -1.0;
5407 j8 = 3.14159265358979;
5408 pz = 0;
5409 j9 = 0;
5410 sj9 = 0;
5411 cj9 = 1.0;
5412 pp = py
5413 * py;
5414 npx
5415 = (py
5416 * r10);
5417 npy
5418 = (py
5419 * r11);
5420 npz
5421 = (py
5422 * r12);
5423 rxp0_0
5424 = ((-1.0)
5425 * r20
5426 * x782);
5427 rxp0_1
5428 = 0;
5429 rxp1_0
5430 = ((-1.0)
5431 * r21
5432 * x782);
5433 rxp1_1
5434 = 0;
5435 rxp2_0
5436 = ((-1.0)
5437 * r22
5438 * x782);
5439 rxp2_1
5440 = 0;
5441 px = 0;
5442 j4 = 0;
5443 sj4 = 0;
5444 cj4 = 1.0;
5445 rxp0_2
5446 = (py
5447 * r00);
5448 rxp1_2
5449 = (py
5450 * r01);
5451 rxp2_2
5452 = (py
5453 * r02);
5454 j6eval
5455 [0]
5456 = ((1.0)
5457 + (((1.91568587540858)
5458 * (py
5459 * py
5460 * py
5461 * py)))
5462 + (((-1.0)
5463 * (2.64633970947792)
5464 * (py
5465 * py))));
5466 if (IKabs(
5467 j6eval
5468 [0])
5469 < 0.0000010000000000)
5470 {
5471 continue; // no branches [j6]
5472 }
5473 else
5474 {
5475 {
5476 IkReal j6array
5477 [2],
5478 cj6array
5479 [2],
5480 sj6array
5481 [2];
5482 bool j6valid
5483 [2]
5484 = {false};
5485 _nj6
5486 = 2;
5487 IkReal
5488 x783
5489 = py
5490 * py;
5491 CheckValue<IkReal> x785 = IKatan2WithCheck(
5492 IkReal((
5493 (-0.425)
5494 + (((-0.588235294117647)
5495 * x783)))),
5496 ((2.83333333333333)
5497 + (((-3.92156862745098)
5498 * x783))),
5499 IKFAST_ATAN2_MAGTHRESH);
5500 if (!x785.valid)
5501 {
5502 continue;
5503 }
5504 IkReal
5505 x784
5506 = ((-1.0)
5507 * (x785.value));
5508 j6array
5509 [0]
5510 = x784;
5511 sj6array
5512 [0]
5513 = IKsin(
5514 j6array
5515 [0]);
5516 cj6array
5517 [0]
5518 = IKcos(
5519 j6array
5520 [0]);
5521 j6array
5522 [1]
5523 = ((3.14159265358979)
5524 + x784);
5525 sj6array
5526 [1]
5527 = IKsin(
5528 j6array
5529 [1]);
5530 cj6array
5531 [1]
5532 = IKcos(
5533 j6array
5534 [1]);
5535 if (j6array
5536 [0]
5537 > IKPI)
5538 {
5539 j6array
5540 [0]
5541 -= IK2PI;
5542 }
5543 else if (
5544 j6array
5545 [0]
5546 < -IKPI)
5547 {
5548 j6array
5549 [0]
5550 += IK2PI;
5551 }
5552 j6valid
5553 [0]
5554 = true;
5555 if (j6array
5556 [1]
5557 > IKPI)
5558 {
5559 j6array
5560 [1]
5561 -= IK2PI;
5562 }
5563 else if (
5564 j6array
5565 [1]
5566 < -IKPI)
5567 {
5568 j6array
5569 [1]
5570 += IK2PI;
5571 }
5572 j6valid
5573 [1]
5574 = true;
5575 for (
5576 int ij6
5577 = 0;
5578 ij6
5579 < 2;
5580 ++ij6)
5581 {
5582 if (!j6valid
5583 [ij6])
5584 {
5585 continue;
5586 }
5587 _ij6[0]
5588 = ij6;
5589 _ij6[1]
5590 = -1;
5591 for (
5592 int iij6
5593 = ij6
5594 + 1;
5595 iij6
5596 < 2;
5597 ++iij6)
5598 {
5599 if (j6valid
5600 [iij6]
5601 && IKabs(
5602 cj6array
5603 [ij6]
5604 - cj6array
5605 [iij6])
5606 < IKFAST_SOLUTION_THRESH
5607 && IKabs(
5608 sj6array
5609 [ij6]
5610 - sj6array
5611 [iij6])
5612 < IKFAST_SOLUTION_THRESH)
5613 {
5614 j6valid
5615 [iij6]
5616 = false;
5617 _ij6[1]
5618 = iij6;
5619 break;
5620 }
5621 }
5622 j6 = j6array
5623 [ij6];
5624 cj6 = cj6array
5625 [ij6];
5626 sj6 = sj6array
5627 [ij6];
5628 {
5629 IkReal evalcond
5630 [1];
5631 evalcond
5632 [0]
5633 = ((0.85)
5634 * (IKsin(
5635 j6)));
5636 if (IKabs(
5637 evalcond
5638 [0])
5639 > IKFAST_EVALCOND_THRESH)
5640 {
5641 continue;
5642 }
5643 }
5644
5645 rotationfunction0(
5646 solutions);
5647 }
5648 }
5649 }
5650 }
5651 }
5652 } while (
5653 0);
5654 if (bgotonextstatement)
5655 {
5656 bool
5657 bgotonextstatement
5658 = true;
5659 do
5660 {
5661 evalcond
5662 [0]
5663 = ((IKabs((
5664 (-3.14159265358979)
5665 + (IKfmod(
5666 j4,
5667 6.28318530717959)))))
5668 + (IKabs(
5669 px)));
5670 evalcond
5671 [1]
5672 = -0.85;
5673 evalcond
5674 [2]
5675 = 0;
5676 evalcond
5677 [3]
5678 = ((-0.2125)
5679 + (((-1.0)
5680 * (1.0)
5681 * (py
5682 * py))));
5683 if (IKabs(
5684 evalcond
5685 [0])
5686 < 0.0000010000000000
5687 && IKabs(
5688 evalcond
5689 [1])
5690 < 0.0000010000000000
5691 && IKabs(
5692 evalcond
5693 [2])
5694 < 0.0000010000000000
5695 && IKabs(
5696 evalcond
5697 [3])
5698 < 0.0000010000000000)
5699 {
5700 bgotonextstatement
5701 = false;
5702 {
5703 IkReal j6eval
5704 [1];
5705 IkReal
5706 x786
5707 = ((1.0)
5708 * py);
5709 sj8 = 0;
5710 cj8 = -1.0;
5711 j8 = 3.14159265358979;
5712 pz = 0;
5713 j9 = 0;
5714 sj9 = 0;
5715 cj9 = 1.0;
5716 pp = py
5717 * py;
5718 npx
5719 = (py
5720 * r10);
5721 npy
5722 = (py
5723 * r11);
5724 npz
5725 = (py
5726 * r12);
5727 rxp0_0
5728 = ((-1.0)
5729 * r20
5730 * x786);
5731 rxp0_1
5732 = 0;
5733 rxp1_0
5734 = ((-1.0)
5735 * r21
5736 * x786);
5737 rxp1_1
5738 = 0;
5739 rxp2_0
5740 = ((-1.0)
5741 * r22
5742 * x786);
5743 rxp2_1
5744 = 0;
5745 px = 0;
5746 j4 = 3.14159265358979;
5747 sj4 = 0;
5748 cj4 = -1.0;
5749 rxp0_2
5750 = (py
5751 * r00);
5752 rxp1_2
5753 = (py
5754 * r01);
5755 rxp2_2
5756 = (py
5757 * r02);
5758 j6eval
5759 [0]
5760 = ((1.0)
5761 + (((1.91568587540858)
5762 * (py
5763 * py
5764 * py
5765 * py)))
5766 + (((-1.0)
5767 * (2.64633970947792)
5768 * (py
5769 * py))));
5770 if (IKabs(
5771 j6eval
5772 [0])
5773 < 0.0000010000000000)
5774 {
5775 continue; // no branches [j6]
5776 }
5777 else
5778 {
5779 {
5780 IkReal j6array
5781 [2],
5782 cj6array
5783 [2],
5784 sj6array
5785 [2];
5786 bool j6valid
5787 [2]
5788 = {false};
5789 _nj6
5790 = 2;
5791 IkReal
5792 x787
5793 = py
5794 * py;
5795 CheckValue<IkReal> x789 = IKatan2WithCheck(
5796 IkReal((
5797 (-0.425)
5798 + (((-0.588235294117647)
5799 * x787)))),
5800 ((2.83333333333333)
5801 + (((-3.92156862745098)
5802 * x787))),
5803 IKFAST_ATAN2_MAGTHRESH);
5804 if (!x789.valid)
5805 {
5806 continue;
5807 }
5808 IkReal
5809 x788
5810 = ((-1.0)
5811 * (x789.value));
5812 j6array
5813 [0]
5814 = x788;
5815 sj6array
5816 [0]
5817 = IKsin(
5818 j6array
5819 [0]);
5820 cj6array
5821 [0]
5822 = IKcos(
5823 j6array
5824 [0]);
5825 j6array
5826 [1]
5827 = ((3.14159265358979)
5828 + x788);
5829 sj6array
5830 [1]
5831 = IKsin(
5832 j6array
5833 [1]);
5834 cj6array
5835 [1]
5836 = IKcos(
5837 j6array
5838 [1]);
5839 if (j6array
5840 [0]
5841 > IKPI)
5842 {
5843 j6array
5844 [0]
5845 -= IK2PI;
5846 }
5847 else if (
5848 j6array
5849 [0]
5850 < -IKPI)
5851 {
5852 j6array
5853 [0]
5854 += IK2PI;
5855 }
5856 j6valid
5857 [0]
5858 = true;
5859 if (j6array
5860 [1]
5861 > IKPI)
5862 {
5863 j6array
5864 [1]
5865 -= IK2PI;
5866 }
5867 else if (
5868 j6array
5869 [1]
5870 < -IKPI)
5871 {
5872 j6array
5873 [1]
5874 += IK2PI;
5875 }
5876 j6valid
5877 [1]
5878 = true;
5879 for (
5880 int ij6
5881 = 0;
5882 ij6
5883 < 2;
5884 ++ij6)
5885 {
5886 if (!j6valid
5887 [ij6])
5888 {
5889 continue;
5890 }
5891 _ij6[0]
5892 = ij6;
5893 _ij6[1]
5894 = -1;
5895 for (
5896 int iij6
5897 = ij6
5898 + 1;
5899 iij6
5900 < 2;
5901 ++iij6)
5902 {
5903 if (j6valid
5904 [iij6]
5905 && IKabs(
5906 cj6array
5907 [ij6]
5908 - cj6array
5909 [iij6])
5910 < IKFAST_SOLUTION_THRESH
5911 && IKabs(
5912 sj6array
5913 [ij6]
5914 - sj6array
5915 [iij6])
5916 < IKFAST_SOLUTION_THRESH)
5917 {
5918 j6valid
5919 [iij6]
5920 = false;
5921 _ij6[1]
5922 = iij6;
5923 break;
5924 }
5925 }
5926 j6 = j6array
5927 [ij6];
5928 cj6 = cj6array
5929 [ij6];
5930 sj6 = sj6array
5931 [ij6];
5932 {
5933 IkReal evalcond
5934 [1];
5935 evalcond
5936 [0]
5937 = ((0.85)
5938 * (IKsin(
5939 j6)));
5940 if (IKabs(
5941 evalcond
5942 [0])
5943 > IKFAST_EVALCOND_THRESH)
5944 {
5945 continue;
5946 }
5947 }
5948
5949 rotationfunction0(
5950 solutions);
5951 }
5952 }
5953 }
5954 }
5955 }
5956 } while (
5957 0);
5958 if (bgotonextstatement)
5959 {
5960 bool
5961 bgotonextstatement
5962 = true;
5963 do
5964 {
5965 evalcond
5966 [0]
5967 = ((IKabs((
5968 (-3.14159265358979)
5969 + (IKfmod(
5970 ((1.5707963267949)
5971 + j4),
5972 6.28318530717959)))))
5973 + (IKabs(
5974 py)));
5975 evalcond
5976 [1]
5977 = -0.85;
5978 evalcond
5979 [2]
5980 = 0;
5981 evalcond
5982 [3]
5983 = ((-0.2125)
5984 + (((-1.0)
5985 * (1.0)
5986 * (px
5987 * px))));
5988 if (IKabs(
5989 evalcond
5990 [0])
5991 < 0.0000010000000000
5992 && IKabs(
5993 evalcond
5994 [1])
5995 < 0.0000010000000000
5996 && IKabs(
5997 evalcond
5998 [2])
5999 < 0.0000010000000000
6000 && IKabs(
6001 evalcond
6002 [3])
6003 < 0.0000010000000000)
6004 {
6005 bgotonextstatement
6006 = false;
6007 {
6008 IkReal j6eval
6009 [1];
6010 IkReal
6011 x790
6012 = ((1.0)
6013 * px);
6014 sj8 = 0;
6015 cj8 = -1.0;
6016 j8 = 3.14159265358979;
6017 pz = 0;
6018 j9 = 0;
6019 sj9 = 0;
6020 cj9 = 1.0;
6021 pp = px
6022 * px;
6023 npx
6024 = (px
6025 * r00);
6026 npy
6027 = (px
6028 * r01);
6029 npz
6030 = (px
6031 * r02);
6032 rxp0_0
6033 = 0;
6034 rxp0_1
6035 = (px
6036 * r20);
6037 rxp1_0
6038 = 0;
6039 rxp1_1
6040 = (px
6041 * r21);
6042 rxp2_0
6043 = 0;
6044 rxp2_1
6045 = (px
6046 * r22);
6047 py = 0;
6048 j4 = 1.5707963267949;
6049 sj4 = 1.0;
6050 cj4 = 0;
6051 rxp0_2
6052 = ((-1.0)
6053 * r10
6054 * x790);
6055 rxp1_2
6056 = ((-1.0)
6057 * r11
6058 * x790);
6059 rxp2_2
6060 = ((-1.0)
6061 * r12
6062 * x790);
6063 j6eval
6064 [0]
6065 = ((1.0)
6066 + (((1.91568587540858)
6067 * (px
6068 * px
6069 * px
6070 * px)))
6071 + (((-1.0)
6072 * (2.64633970947792)
6073 * (px
6074 * px))));
6075 if (IKabs(
6076 j6eval
6077 [0])
6078 < 0.0000010000000000)
6079 {
6080 continue; // no branches [j6]
6081 }
6082 else
6083 {
6084 {
6085 IkReal j6array
6086 [2],
6087 cj6array
6088 [2],
6089 sj6array
6090 [2];
6091 bool j6valid
6092 [2]
6093 = {false};
6094 _nj6
6095 = 2;
6096 IkReal
6097 x791
6098 = px
6099 * px;
6100 CheckValue<IkReal> x793 = IKatan2WithCheck(
6101 IkReal((
6102 (-0.425)
6103 + (((-0.588235294117647)
6104 * x791)))),
6105 ((2.83333333333333)
6106 + (((-3.92156862745098)
6107 * x791))),
6108 IKFAST_ATAN2_MAGTHRESH);
6109 if (!x793.valid)
6110 {
6111 continue;
6112 }
6113 IkReal
6114 x792
6115 = ((-1.0)
6116 * (x793.value));
6117 j6array
6118 [0]
6119 = x792;
6120 sj6array
6121 [0]
6122 = IKsin(
6123 j6array
6124 [0]);
6125 cj6array
6126 [0]
6127 = IKcos(
6128 j6array
6129 [0]);
6130 j6array
6131 [1]
6132 = ((3.14159265358979)
6133 + x792);
6134 sj6array
6135 [1]
6136 = IKsin(
6137 j6array
6138 [1]);
6139 cj6array
6140 [1]
6141 = IKcos(
6142 j6array
6143 [1]);
6144 if (j6array
6145 [0]
6146 > IKPI)
6147 {
6148 j6array
6149 [0]
6150 -= IK2PI;
6151 }
6152 else if (
6153 j6array
6154 [0]
6155 < -IKPI)
6156 {
6157 j6array
6158 [0]
6159 += IK2PI;
6160 }
6161 j6valid
6162 [0]
6163 = true;
6164 if (j6array
6165 [1]
6166 > IKPI)
6167 {
6168 j6array
6169 [1]
6170 -= IK2PI;
6171 }
6172 else if (
6173 j6array
6174 [1]
6175 < -IKPI)
6176 {
6177 j6array
6178 [1]
6179 += IK2PI;
6180 }
6181 j6valid
6182 [1]
6183 = true;
6184 for (
6185 int ij6
6186 = 0;
6187 ij6
6188 < 2;
6189 ++ij6)
6190 {
6191 if (!j6valid
6192 [ij6])
6193 {
6194 continue;
6195 }
6196 _ij6[0]
6197 = ij6;
6198 _ij6[1]
6199 = -1;
6200 for (
6201 int iij6
6202 = ij6
6203 + 1;
6204 iij6
6205 < 2;
6206 ++iij6)
6207 {
6208 if (j6valid
6209 [iij6]
6210 && IKabs(
6211 cj6array
6212 [ij6]
6213 - cj6array
6214 [iij6])
6215 < IKFAST_SOLUTION_THRESH
6216 && IKabs(
6217 sj6array
6218 [ij6]
6219 - sj6array
6220 [iij6])
6221 < IKFAST_SOLUTION_THRESH)
6222 {
6223 j6valid
6224 [iij6]
6225 = false;
6226 _ij6[1]
6227 = iij6;
6228 break;
6229 }
6230 }
6231 j6 = j6array
6232 [ij6];
6233 cj6 = cj6array
6234 [ij6];
6235 sj6 = sj6array
6236 [ij6];
6237 {
6238 IkReal evalcond
6239 [1];
6240 evalcond
6241 [0]
6242 = ((0.85)
6243 * (IKsin(
6244 j6)));
6245 if (IKabs(
6246 evalcond
6247 [0])
6248 > IKFAST_EVALCOND_THRESH)
6249 {
6250 continue;
6251 }
6252 }
6253
6254 rotationfunction0(
6255 solutions);
6256 }
6257 }
6258 }
6259 }
6260 }
6261 } while (
6262 0);
6263 if (bgotonextstatement)
6264 {
6265 bool
6266 bgotonextstatement
6267 = true;
6268 do
6269 {
6270 evalcond
6271 [0]
6272 = ((IKabs(
6273 py))
6274 + (IKabs((
6275 (-3.14159265358979)
6276 + (IKfmod(
6277 ((4.71238898038469)
6278 + j4),
6279 6.28318530717959))))));
6280 evalcond
6281 [1]
6282 = -0.85;
6283 evalcond
6284 [2]
6285 = 0;
6286 evalcond
6287 [3]
6288 = ((-0.2125)
6289 + (((-1.0)
6290 * (1.0)
6291 * (px
6292 * px))));
6293 if (IKabs(
6294 evalcond
6295 [0])
6296 < 0.0000010000000000
6297 && IKabs(
6298 evalcond
6299 [1])
6300 < 0.0000010000000000
6301 && IKabs(
6302 evalcond
6303 [2])
6304 < 0.0000010000000000
6305 && IKabs(
6306 evalcond
6307 [3])
6308 < 0.0000010000000000)
6309 {
6310 bgotonextstatement
6311 = false;
6312 {
6313 IkReal j6eval
6314 [1];
6315 IkReal
6316 x794
6317 = ((1.0)
6318 * px);
6319 sj8 = 0;
6320 cj8 = -1.0;
6321 j8 = 3.14159265358979;
6322 pz = 0;
6323 j9 = 0;
6324 sj9 = 0;
6325 cj9 = 1.0;
6326 pp = px
6327 * px;
6328 npx
6329 = (px
6330 * r00);
6331 npy
6332 = (px
6333 * r01);
6334 npz
6335 = (px
6336 * r02);
6337 rxp0_0
6338 = 0;
6339 rxp0_1
6340 = (px
6341 * r20);
6342 rxp1_0
6343 = 0;
6344 rxp1_1
6345 = (px
6346 * r21);
6347 rxp2_0
6348 = 0;
6349 rxp2_1
6350 = (px
6351 * r22);
6352 py = 0;
6353 j4 = -1.5707963267949;
6354 sj4 = -1.0;
6355 cj4 = 0;
6356 rxp0_2
6357 = ((-1.0)
6358 * r10
6359 * x794);
6360 rxp1_2
6361 = ((-1.0)
6362 * r11
6363 * x794);
6364 rxp2_2
6365 = ((-1.0)
6366 * r12
6367 * x794);
6368 j6eval
6369 [0]
6370 = ((1.0)
6371 + (((1.91568587540858)
6372 * (px
6373 * px
6374 * px
6375 * px)))
6376 + (((-1.0)
6377 * (2.64633970947792)
6378 * (px
6379 * px))));
6380 if (IKabs(
6381 j6eval
6382 [0])
6383 < 0.0000010000000000)
6384 {
6385 continue; // no branches [j6]
6386 }
6387 else
6388 {
6389 {
6390 IkReal j6array
6391 [2],
6392 cj6array
6393 [2],
6394 sj6array
6395 [2];
6396 bool j6valid
6397 [2]
6398 = {false};
6399 _nj6
6400 = 2;
6401 IkReal
6402 x795
6403 = px
6404 * px;
6405 CheckValue<IkReal> x797 = IKatan2WithCheck(
6406 IkReal((
6407 (-0.425)
6408 + (((-0.588235294117647)
6409 * x795)))),
6410 ((2.83333333333333)
6411 + (((-3.92156862745098)
6412 * x795))),
6413 IKFAST_ATAN2_MAGTHRESH);
6414 if (!x797.valid)
6415 {
6416 continue;
6417 }
6418 IkReal
6419 x796
6420 = ((-1.0)
6421 * (x797.value));
6422 j6array
6423 [0]
6424 = x796;
6425 sj6array
6426 [0]
6427 = IKsin(
6428 j6array
6429 [0]);
6430 cj6array
6431 [0]
6432 = IKcos(
6433 j6array
6434 [0]);
6435 j6array
6436 [1]
6437 = ((3.14159265358979)
6438 + x796);
6439 sj6array
6440 [1]
6441 = IKsin(
6442 j6array
6443 [1]);
6444 cj6array
6445 [1]
6446 = IKcos(
6447 j6array
6448 [1]);
6449 if (j6array
6450 [0]
6451 > IKPI)
6452 {
6453 j6array
6454 [0]
6455 -= IK2PI;
6456 }
6457 else if (
6458 j6array
6459 [0]
6460 < -IKPI)
6461 {
6462 j6array
6463 [0]
6464 += IK2PI;
6465 }
6466 j6valid
6467 [0]
6468 = true;
6469 if (j6array
6470 [1]
6471 > IKPI)
6472 {
6473 j6array
6474 [1]
6475 -= IK2PI;
6476 }
6477 else if (
6478 j6array
6479 [1]
6480 < -IKPI)
6481 {
6482 j6array
6483 [1]
6484 += IK2PI;
6485 }
6486 j6valid
6487 [1]
6488 = true;
6489 for (
6490 int ij6
6491 = 0;
6492 ij6
6493 < 2;
6494 ++ij6)
6495 {
6496 if (!j6valid
6497 [ij6])
6498 {
6499 continue;
6500 }
6501 _ij6[0]
6502 = ij6;
6503 _ij6[1]
6504 = -1;
6505 for (
6506 int iij6
6507 = ij6
6508 + 1;
6509 iij6
6510 < 2;
6511 ++iij6)
6512 {
6513 if (j6valid
6514 [iij6]
6515 && IKabs(
6516 cj6array
6517 [ij6]
6518 - cj6array
6519 [iij6])
6520 < IKFAST_SOLUTION_THRESH
6521 && IKabs(
6522 sj6array
6523 [ij6]
6524 - sj6array
6525 [iij6])
6526 < IKFAST_SOLUTION_THRESH)
6527 {
6528 j6valid
6529 [iij6]
6530 = false;
6531 _ij6[1]
6532 = iij6;
6533 break;
6534 }
6535 }
6536 j6 = j6array
6537 [ij6];
6538 cj6 = cj6array
6539 [ij6];
6540 sj6 = sj6array
6541 [ij6];
6542 {
6543 IkReal evalcond
6544 [1];
6545 evalcond
6546 [0]
6547 = ((0.85)
6548 * (IKsin(
6549 j6)));
6550 if (IKabs(
6551 evalcond
6552 [0])
6553 > IKFAST_EVALCOND_THRESH)
6554 {
6555 continue;
6556 }
6557 }
6558
6559 rotationfunction0(
6560 solutions);
6561 }
6562 }
6563 }
6564 }
6565 }
6566 } while (
6567 0);
6568 if (bgotonextstatement)
6569 {
6570 bool
6571 bgotonextstatement
6572 = true;
6573 do
6574 {
6575 if (1)
6576 {
6577 bgotonextstatement
6578 = false;
6579 continue; // branch miss [j6]
6580 }
6581 } while (
6582 0);
6583 if (bgotonextstatement)
6584 {
6585 }
6586 }
6587 }
6588 }
6589 }
6590 }
6591 }
6592 }
6593 else
6594 {
6595 {
6596 IkReal j6array
6597 [1],
6598 cj6array
6599 [1],
6600 sj6array
6601 [1];
6602 bool j6valid
6603 [1]
6604 = {false};
6605 _nj6
6606 = 1;
6607 IkReal
6608 x798
6609 = (cj4
6610 * px);
6611 IkReal
6612 x799
6613 = (py
6614 * sj4);
6615 IkReal
6616 x800
6617 = px
6618 * px;
6619 IkReal
6620 x801
6621 = py
6622 * py;
6623 CheckValue<IkReal> x802 = IKPowWithIntegerCheck(
6624 ((((20.0)
6625 * x799))
6626 + (((20.0)
6627 * x798))),
6628 -1);
6629 if (!x802.valid)
6630 {
6631 continue;
6632 }
6633 CheckValue<IkReal> x803 = IKPowWithIntegerCheck(
6634 ((((-8.5)
6635 * x798))
6636 + (((-1.0)
6637 * (11.7647058823529)
6638 * cj4
6639 * (px
6640 * px
6641 * px)))
6642 + (((-11.7647058823529)
6643 * x798
6644 * x801))
6645 + (((-11.7647058823529)
6646 * x799
6647 * x800))
6648 + (((-8.5)
6649 * x799))
6650 + (((-1.0)
6651 * (11.7647058823529)
6652 * sj4
6653 * (py
6654 * py
6655 * py)))),
6656 -1);
6657 if (!x803.valid)
6658 {
6659 continue;
6660 }
6661 if (IKabs((
6662 (17.0)
6663 * (x802.value)))
6664 < IKFAST_ATAN2_MAGTHRESH
6665 && IKabs((
6666 (x803.value)
6667 * (((-48.1666666666667)
6668 + (((66.6666666666667)
6669 * x800))
6670 + (((66.6666666666667)
6671 * x801))))))
6672 < IKFAST_ATAN2_MAGTHRESH
6673 && IKabs(
6674 IKsqr((
6675 (17.0)
6676 * (x802.value)))
6677 + IKsqr((
6678 (x803.value)
6679 * (((-48.1666666666667)
6680 + (((66.6666666666667)
6681 * x800))
6682 + (((66.6666666666667)
6683 * x801))))))
6684 - 1)
6685 <= IKFAST_SINCOS_THRESH)
6686 continue;
6687 j6array[0] = IKatan2(
6688 ((17.0)
6689 * (x802.value)),
6690 ((x803.value)
6691 * (((-48.1666666666667)
6692 + (((66.6666666666667)
6693 * x800))
6694 + (((66.6666666666667)
6695 * x801))))));
6696 sj6array
6697 [0]
6698 = IKsin(
6699 j6array
6700 [0]);
6701 cj6array
6702 [0]
6703 = IKcos(
6704 j6array
6705 [0]);
6706 if (j6array
6707 [0]
6708 > IKPI)
6709 {
6710 j6array
6711 [0]
6712 -= IK2PI;
6713 }
6714 else if (
6715 j6array
6716 [0]
6717 < -IKPI)
6718 {
6719 j6array
6720 [0]
6721 += IK2PI;
6722 }
6723 j6valid
6724 [0]
6725 = true;
6726 for (
6727 int ij6
6728 = 0;
6729 ij6
6730 < 1;
6731 ++ij6)
6732 {
6733 if (!j6valid
6734 [ij6])
6735 {
6736 continue;
6737 }
6738 _ij6[0]
6739 = ij6;
6740 _ij6[1]
6741 = -1;
6742 for (
6743 int iij6
6744 = ij6
6745 + 1;
6746 iij6
6747 < 1;
6748 ++iij6)
6749 {
6750 if (j6valid
6751 [iij6]
6752 && IKabs(
6753 cj6array
6754 [ij6]
6755 - cj6array
6756 [iij6])
6757 < IKFAST_SOLUTION_THRESH
6758 && IKabs(
6759 sj6array
6760 [ij6]
6761 - sj6array
6762 [iij6])
6763 < IKFAST_SOLUTION_THRESH)
6764 {
6765 j6valid
6766 [iij6]
6767 = false;
6768 _ij6[1]
6769 = iij6;
6770 break;
6771 }
6772 }
6773 j6 = j6array
6774 [ij6];
6775 cj6 = cj6array
6776 [ij6];
6777 sj6 = sj6array
6778 [ij6];
6779 {
6780 IkReal evalcond
6781 [5];
6782 IkReal
6783 x804
6784 = IKcos(
6785 j6);
6786 IkReal
6787 x805
6788 = (cj4
6789 * px);
6790 IkReal
6791 x806
6792 = (x804
6793 * x805);
6794 IkReal
6795 x807
6796 = (py
6797 * sj4);
6798 IkReal
6799 x808
6800 = (x804
6801 * x807);
6802 IkReal
6803 x809
6804 = IKsin(
6805 j6);
6806 IkReal
6807 x810
6808 = (x805
6809 * x809);
6810 IkReal
6811 x811
6812 = (x807
6813 * x809);
6814 IkReal
6815 x812
6816 = px
6817 * px;
6818 IkReal
6819 x813
6820 = ((3.92156862745098)
6821 * x809);
6822 IkReal
6823 x814
6824 = ((0.588235294117647)
6825 * x804);
6826 IkReal
6827 x815
6828 = py
6829 * py;
6830 evalcond
6831 [0]
6832 = (x806
6833 + x808);
6834 evalcond
6835 [1]
6836 = ((-0.85)
6837 + x811
6838 + x810);
6839 evalcond
6840 [2]
6841 = ((((-1.0)
6842 * x807))
6843 + (((0.85)
6844 * x809))
6845 + (((-1.0)
6846 * x805)));
6847 evalcond
6848 [3]
6849 = ((((-1.0)
6850 * x812
6851 * x814))
6852 + (((-1.0)
6853 * x814
6854 * x815))
6855 + (((-1.0)
6856 * x812
6857 * x813))
6858 + (((2.83333333333333)
6859 * x809))
6860 + (((-0.425)
6861 * x804))
6862 + (((-1.0)
6863 * x813
6864 * x815)));
6865 evalcond
6866 [4]
6867 = ((-0.2125)
6868 + (((1.1)
6869 * x810))
6870 + (((-0.09)
6871 * x808))
6872 + (((-1.0)
6873 * x815))
6874 + (((1.1)
6875 * x811))
6876 + (((-1.0)
6877 * x812))
6878 + (((-0.09)
6879 * x806)));
6880 if (IKabs(
6881 evalcond
6882 [0])
6883 > IKFAST_EVALCOND_THRESH
6884 || IKabs(
6885 evalcond
6886 [1])
6887 > IKFAST_EVALCOND_THRESH
6888 || IKabs(
6889 evalcond
6890 [2])
6891 > IKFAST_EVALCOND_THRESH
6892 || IKabs(
6893 evalcond
6894 [3])
6895 > IKFAST_EVALCOND_THRESH
6896 || IKabs(
6897 evalcond
6898 [4])
6899 > IKFAST_EVALCOND_THRESH)
6900 {
6901 continue;
6902 }
6903 }
6904
6905 rotationfunction0(
6906 solutions);
6907 }
6908 }
6909 }
6910 }
6911 }
6912 else
6913 {
6914 {
6915 IkReal j6array
6916 [1],
6917 cj6array
6918 [1],
6919 sj6array
6920 [1];
6921 bool j6valid
6922 [1]
6923 = {false};
6924 _nj6
6925 = 1;
6926 IkReal
6927 x816
6928 = (cj4
6929 * px);
6930 IkReal
6931 x817
6932 = (py
6933 * sj4);
6934 IkReal
6935 x818
6936 = px
6937 * px;
6938 IkReal
6939 x819
6940 = py
6941 * py;
6942 CheckValue<IkReal> x820 = IKPowWithIntegerCheck(
6943 ((-7.225)
6944 + (((-10.0)
6945 * x818))
6946 + (((-10.0)
6947 * x819))),
6948 -1);
6949 if (!x820.valid)
6950 {
6951 continue;
6952 }
6953 if (IKabs((
6954 (((1.17647058823529)
6955 * x816))
6956 + (((1.17647058823529)
6957 * x817))))
6958 < IKFAST_ATAN2_MAGTHRESH
6959 && IKabs((
6960 (x820.value)
6961 * (((((78.4313725490196)
6962 * x817
6963 * x818))
6964 + (((78.4313725490196)
6965 * sj4
6966 * (py
6967 * py
6968 * py)))
6969 + (((78.4313725490196)
6970 * cj4
6971 * (px
6972 * px
6973 * px)))
6974 + (((-56.6666666666667)
6975 * x816))
6976 + (((-56.6666666666667)
6977 * x817))
6978 + (((78.4313725490196)
6979 * x816
6980 * x819))))))
6981 < IKFAST_ATAN2_MAGTHRESH
6982 && IKabs(
6983 IKsqr((
6984 (((1.17647058823529)
6985 * x816))
6986 + (((1.17647058823529)
6987 * x817))))
6988 + IKsqr(
6989 (
6990 (x820.value)
6991 * (((((78.4313725490196)
6992 * x817
6993 * x818))
6994 + (((78.4313725490196) * sj4 * (py * py * py))) + (((78.4313725490196) * cj4 * (px * px * px))) + (((-56.6666666666667) * x816))
6995 + (((-56.6666666666667)
6996 * x817))
6997 + (((78.4313725490196)
6998 * x816
6999 * x819))))))
7000 - 1)
7001 <= IKFAST_SINCOS_THRESH)
7002 continue;
7003 j6array[0] = IKatan2(
7004 ((((1.17647058823529)
7005 * x816))
7006 + (((1.17647058823529)
7007 * x817))),
7008 ((x820.value)
7009 * (((((78.4313725490196)
7010 * x817
7011 * x818))
7012 + (((78.4313725490196)
7013 * sj4
7014 * (py
7015 * py
7016 * py)))
7017 + (((78.4313725490196)
7018 * cj4
7019 * (px
7020 * px
7021 * px)))
7022 + (((-56.6666666666667)
7023 * x816))
7024 + (((-56.6666666666667)
7025 * x817))
7026 + (((78.4313725490196)
7027 * x816
7028 * x819))))));
7029 sj6array
7030 [0]
7031 = IKsin(
7032 j6array
7033 [0]);
7034 cj6array
7035 [0]
7036 = IKcos(
7037 j6array
7038 [0]);
7039 if (j6array
7040 [0]
7041 > IKPI)
7042 {
7043 j6array
7044 [0]
7045 -= IK2PI;
7046 }
7047 else if (
7048 j6array
7049 [0]
7050 < -IKPI)
7051 {
7052 j6array
7053 [0]
7054 += IK2PI;
7055 }
7056 j6valid
7057 [0]
7058 = true;
7059 for (
7060 int ij6
7061 = 0;
7062 ij6
7063 < 1;
7064 ++ij6)
7065 {
7066 if (!j6valid
7067 [ij6])
7068 {
7069 continue;
7070 }
7071 _ij6[0]
7072 = ij6;
7073 _ij6[1]
7074 = -1;
7075 for (
7076 int iij6
7077 = ij6
7078 + 1;
7079 iij6
7080 < 1;
7081 ++iij6)
7082 {
7083 if (j6valid
7084 [iij6]
7085 && IKabs(
7086 cj6array
7087 [ij6]
7088 - cj6array
7089 [iij6])
7090 < IKFAST_SOLUTION_THRESH
7091 && IKabs(
7092 sj6array
7093 [ij6]
7094 - sj6array
7095 [iij6])
7096 < IKFAST_SOLUTION_THRESH)
7097 {
7098 j6valid
7099 [iij6]
7100 = false;
7101 _ij6[1]
7102 = iij6;
7103 break;
7104 }
7105 }
7106 j6 = j6array
7107 [ij6];
7108 cj6 = cj6array
7109 [ij6];
7110 sj6 = sj6array
7111 [ij6];
7112 {
7113 IkReal evalcond
7114 [5];
7115 IkReal
7116 x821
7117 = IKcos(
7118 j6);
7119 IkReal
7120 x822
7121 = (cj4
7122 * px);
7123 IkReal
7124 x823
7125 = (x821
7126 * x822);
7127 IkReal
7128 x824
7129 = (py
7130 * sj4);
7131 IkReal
7132 x825
7133 = (x821
7134 * x824);
7135 IkReal
7136 x826
7137 = IKsin(
7138 j6);
7139 IkReal
7140 x827
7141 = (x822
7142 * x826);
7143 IkReal
7144 x828
7145 = (x824
7146 * x826);
7147 IkReal
7148 x829
7149 = px
7150 * px;
7151 IkReal
7152 x830
7153 = ((3.92156862745098)
7154 * x826);
7155 IkReal
7156 x831
7157 = ((0.588235294117647)
7158 * x821);
7159 IkReal
7160 x832
7161 = py
7162 * py;
7163 evalcond
7164 [0]
7165 = (x825
7166 + x823);
7167 evalcond
7168 [1]
7169 = ((-0.85)
7170 + x827
7171 + x828);
7172 evalcond
7173 [2]
7174 = ((((0.85)
7175 * x826))
7176 + (((-1.0)
7177 * x822))
7178 + (((-1.0)
7179 * x824)));
7180 evalcond
7181 [3]
7182 = ((((-1.0)
7183 * x830
7184 * x832))
7185 + (((2.83333333333333)
7186 * x826))
7187 + (((-1.0)
7188 * x829
7189 * x831))
7190 + (((-1.0)
7191 * x831
7192 * x832))
7193 + (((-1.0)
7194 * x829
7195 * x830))
7196 + (((-0.425)
7197 * x821)));
7198 evalcond
7199 [4]
7200 = ((-0.2125)
7201 + (((1.1)
7202 * x828))
7203 + (((-0.09)
7204 * x825))
7205 + (((1.1)
7206 * x827))
7207 + (((-0.09)
7208 * x823))
7209 + (((-1.0)
7210 * x832))
7211 + (((-1.0)
7212 * x829)));
7213 if (IKabs(
7214 evalcond
7215 [0])
7216 > IKFAST_EVALCOND_THRESH
7217 || IKabs(
7218 evalcond
7219 [1])
7220 > IKFAST_EVALCOND_THRESH
7221 || IKabs(
7222 evalcond
7223 [2])
7224 > IKFAST_EVALCOND_THRESH
7225 || IKabs(
7226 evalcond
7227 [3])
7228 > IKFAST_EVALCOND_THRESH
7229 || IKabs(
7230 evalcond
7231 [4])
7232 > IKFAST_EVALCOND_THRESH)
7233 {
7234 continue;
7235 }
7236 }
7237
7238 rotationfunction0(
7239 solutions);
7240 }
7241 }
7242 }
7243 }
7244 }
7245 else
7246 {
7247 {
7248 IkReal j6array
7249 [1],
7250 cj6array
7251 [1],
7252 sj6array
7253 [1];
7254 bool j6valid
7255 [1]
7256 = {false};
7257 _nj6 = 1;
7258 IkReal
7259 x833
7260 = (cj4
7261 * px);
7262 IkReal
7263 x834
7264 = (py
7265 * sj4);
7266 IkReal
7267 x835
7268 = px
7269 * px;
7270 IkReal
7271 x836
7272 = py
7273 * py;
7274 IkReal
7275 x837
7276 = ((1.29411764705882)
7277 * (cj4
7278 * cj4));
7279 CheckValue<
7280 IkReal>
7281 x838
7282 = IKPowWithIntegerCheck(
7283 ((((-0.09)
7284 * x834))
7285 + (((-0.09)
7286 * x833))),
7287 -1);
7288 if (!x838.valid)
7289 {
7290 continue;
7291 }
7292 if (IKabs((
7293 (((1.17647058823529)
7294 * x833))
7295 + (((1.17647058823529)
7296 * x834))))
7297 < IKFAST_ATAN2_MAGTHRESH
7298 && IKabs((
7299 (x838.value)
7300 * (((0.2125)
7301 + (((-0.294117647058824)
7302 * x836))
7303 + x835
7304 + (((-2.58823529411765)
7305 * cj4
7306 * px
7307 * x834))
7308 + ((x836
7309 * x837))
7310 + (((-1.0)
7311 * x835
7312 * x837))))))
7313 < IKFAST_ATAN2_MAGTHRESH
7314 && IKabs(
7315 IKsqr((
7316 (((1.17647058823529)
7317 * x833))
7318 + (((1.17647058823529)
7319 * x834))))
7320 + IKsqr((
7321 (x838.value)
7322 * (((0.2125)
7323 + (((-0.294117647058824)
7324 * x836))
7325 + x835
7326 + (((-2.58823529411765)
7327 * cj4
7328 * px
7329 * x834))
7330 + ((x836
7331 * x837))
7332 + (((-1.0)
7333 * x835
7334 * x837))))))
7335 - 1)
7336 <= IKFAST_SINCOS_THRESH)
7337 continue;
7338 j6array[0] = IKatan2(
7339 ((((1.17647058823529)
7340 * x833))
7341 + (((1.17647058823529)
7342 * x834))),
7343 ((x838.value)
7344 * (((0.2125)
7345 + (((-0.294117647058824)
7346 * x836))
7347 + x835
7348 + (((-2.58823529411765)
7349 * cj4
7350 * px
7351 * x834))
7352 + ((x836
7353 * x837))
7354 + (((-1.0)
7355 * x835
7356 * x837))))));
7357 sj6array[0] = IKsin(
7358 j6array
7359 [0]);
7360 cj6array[0] = IKcos(
7361 j6array
7362 [0]);
7363 if (j6array
7364 [0]
7365 > IKPI)
7366 {
7367 j6array
7368 [0]
7369 -= IK2PI;
7370 }
7371 else if (
7372 j6array
7373 [0]
7374 < -IKPI)
7375 {
7376 j6array
7377 [0]
7378 += IK2PI;
7379 }
7380 j6valid[0]
7381 = true;
7382 for (
7383 int ij6
7384 = 0;
7385 ij6
7386 < 1;
7387 ++ij6)
7388 {
7389 if (!j6valid
7390 [ij6])
7391 {
7392 continue;
7393 }
7394 _ij6[0]
7395 = ij6;
7396 _ij6[1]
7397 = -1;
7398 for (
7399 int iij6
7400 = ij6
7401 + 1;
7402 iij6
7403 < 1;
7404 ++iij6)
7405 {
7406 if (j6valid
7407 [iij6]
7408 && IKabs(
7409 cj6array
7410 [ij6]
7411 - cj6array
7412 [iij6])
7413 < IKFAST_SOLUTION_THRESH
7414 && IKabs(
7415 sj6array
7416 [ij6]
7417 - sj6array
7418 [iij6])
7419 < IKFAST_SOLUTION_THRESH)
7420 {
7421 j6valid
7422 [iij6]
7423 = false;
7424 _ij6[1]
7425 = iij6;
7426 break;
7427 }
7428 }
7429 j6 = j6array
7430 [ij6];
7431 cj6 = cj6array
7432 [ij6];
7433 sj6 = sj6array
7434 [ij6];
7435 {
7436 IkReal evalcond
7437 [5];
7438 IkReal
7439 x839
7440 = IKcos(
7441 j6);
7442 IkReal
7443 x840
7444 = (cj4
7445 * px);
7446 IkReal
7447 x841
7448 = (x839
7449 * x840);
7450 IkReal
7451 x842
7452 = (py
7453 * sj4);
7454 IkReal
7455 x843
7456 = (x839
7457 * x842);
7458 IkReal
7459 x844
7460 = IKsin(
7461 j6);
7462 IkReal
7463 x845
7464 = (x840
7465 * x844);
7466 IkReal
7467 x846
7468 = (x842
7469 * x844);
7470 IkReal
7471 x847
7472 = px
7473 * px;
7474 IkReal
7475 x848
7476 = ((3.92156862745098)
7477 * x844);
7478 IkReal
7479 x849
7480 = ((0.588235294117647)
7481 * x839);
7482 IkReal
7483 x850
7484 = py
7485 * py;
7486 evalcond
7487 [0]
7488 = (x843
7489 + x841);
7490 evalcond
7491 [1]
7492 = ((-0.85)
7493 + x846
7494 + x845);
7495 evalcond
7496 [2]
7497 = ((((0.85)
7498 * x844))
7499 + (((-1.0)
7500 * x842))
7501 + (((-1.0)
7502 * x840)));
7503 evalcond
7504 [3]
7505 = ((((-1.0)
7506 * x847
7507 * x849))
7508 + (((-1.0)
7509 * x847
7510 * x848))
7511 + (((-0.425)
7512 * x839))
7513 + (((-1.0)
7514 * x848
7515 * x850))
7516 + (((-1.0)
7517 * x849
7518 * x850))
7519 + (((2.83333333333333)
7520 * x844)));
7521 evalcond
7522 [4]
7523 = ((-0.2125)
7524 + (((-1.0)
7525 * x850))
7526 + (((-0.09)
7527 * x843))
7528 + (((1.1)
7529 * x846))
7530 + (((1.1)
7531 * x845))
7532 + (((-1.0)
7533 * x847))
7534 + (((-0.09)
7535 * x841)));
7536 if (IKabs(
7537 evalcond
7538 [0])
7539 > IKFAST_EVALCOND_THRESH
7540 || IKabs(
7541 evalcond
7542 [1])
7543 > IKFAST_EVALCOND_THRESH
7544 || IKabs(
7545 evalcond
7546 [2])
7547 > IKFAST_EVALCOND_THRESH
7548 || IKabs(
7549 evalcond
7550 [3])
7551 > IKFAST_EVALCOND_THRESH
7552 || IKabs(
7553 evalcond
7554 [4])
7555 > IKFAST_EVALCOND_THRESH)
7556 {
7557 continue;
7558 }
7559 }
7560
7561 rotationfunction0(
7562 solutions);
7563 }
7564 }
7565 }
7566 }
7567 }
7568 } while (0);
7569 if (bgotonextstatement)
7570 {
7571 bool
7572 bgotonextstatement
7573 = true;
7574 do
7575 {
7576 if (1)
7577 {
7578 bgotonextstatement
7579 = false;
7580 continue; // branch miss [j6]
7581 }
7582 } while (0);
7583 if (bgotonextstatement)
7584 {
7585 }
7586 }
7587 }
7588 }
7589 else
7590 {
7591 {
7592 IkReal j6array[1],
7593 cj6array[1],
7594 sj6array[1];
7595 bool j6valid[1]
7596 = {false};
7597 _nj6 = 1;
7598 IkReal x851
7599 = (cj4 * px);
7600 IkReal x852
7601 = (py * sj4);
7602 IkReal x853
7603 = ((0.108264705882353)
7604 * cj9);
7605 IkReal x854
7606 = ((0.588235294117647)
7607 * pp);
7608 IkReal x855
7609 = (cj9 * pp);
7610 IkReal x856
7611 = (cj9 * sj9);
7612 IkReal x857
7613 = (pp * sj9);
7614 IkReal x858
7615 = cj9 * cj9;
7616 IkReal x859
7617 = ((1.0) * pz);
7618 CheckValue<IkReal> x860 = IKatan2WithCheck(
7619 IkReal((
7620 (-0.174204411764706)
7621 + (pz * pz)
7622 + (((-0.176470588235294)
7623 * x855))
7624 + (((-1.0)
7625 * (0.154566176470588)
7626 * cj9))
7627 + (((-1.0)
7628 * (0.323529411764706)
7629 * pp))
7630 + (((-0.0264705882352941)
7631 * x857))
7632 + (((-0.00487191176470588)
7633 * x856))
7634 + (((-1.0)
7635 * (0.0142530882352941)
7636 * sj9))
7637 + (((-0.0324794117647059)
7638 * x858)))),
7639 ((-0.830553921568627)
7640 + (((-0.396970588235294)
7641 * x858))
7642 + (((-1.0)
7643 * (0.0679544117647059)
7644 * sj9))
7645 + (((-1.0)
7646 * (1.18080882352941)
7647 * cj9))
7648 + (((0.176470588235294)
7649 * x857))
7650 + (((-1.0)
7651 * x852
7652 * x859))
7653 + (((2.15686274509804)
7654 * pp))
7655 + (((1.17647058823529)
7656 * x855))
7657 + (((-1.0)
7658 * x851
7659 * x859))
7660 + (((-0.0595455882352941)
7661 * x856))),
7662 IKFAST_ATAN2_MAGTHRESH);
7663 if (!x860.valid)
7664 {
7665 continue;
7666 }
7667 CheckValue<IkReal> x861 = IKPowWithIntegerCheck(
7668 IKsign((
7669 (((-1.0)
7670 * x851
7671 * x854))
7672 + (((-1.0)
7673 * (1.32323529411765)
7674 * cj9
7675 * pz))
7676 + (((3.92156862745098)
7677 * pp
7678 * pz))
7679 + (((-1.0)
7680 * (1.51009803921569)
7681 * pz))
7682 + (((-0.316735294117647)
7683 * x852))
7684 + (((-1.0)
7685 * x852
7686 * x853))
7687 + (((-0.316735294117647)
7688 * x851))
7689 + (((-1.0)
7690 * x851
7691 * x853))
7692 + (((-1.0)
7693 * x852
7694 * x854)))),
7695 -1);
7696 if (!x861.valid)
7697 {
7698 continue;
7699 }
7700 j6array[0]
7701 = ((-1.5707963267949)
7702 + (x860.value)
7703 + (((1.5707963267949)
7704 * (x861.value))));
7705 sj6array[0] = IKsin(
7706 j6array[0]);
7707 cj6array[0] = IKcos(
7708 j6array[0]);
7709 if (j6array[0]
7710 > IKPI)
7711 {
7712 j6array[0]
7713 -= IK2PI;
7714 }
7715 else if (
7716 j6array[0]
7717 < -IKPI)
7718 {
7719 j6array[0]
7720 += IK2PI;
7721 }
7722 j6valid[0] = true;
7723 for (int ij6 = 0;
7724 ij6 < 1;
7725 ++ij6)
7726 {
7727 if (!j6valid[ij6])
7728 {
7729 continue;
7730 }
7731 _ij6[0] = ij6;
7732 _ij6[1] = -1;
7733 for (int iij6
7734 = ij6 + 1;
7735 iij6 < 1;
7736 ++iij6)
7737 {
7738 if (j6valid
7739 [iij6]
7740 && IKabs(
7741 cj6array
7742 [ij6]
7743 - cj6array
7744 [iij6])
7745 < IKFAST_SOLUTION_THRESH
7746 && IKabs(
7747 sj6array
7748 [ij6]
7749 - sj6array
7750 [iij6])
7751 < IKFAST_SOLUTION_THRESH)
7752 {
7753 j6valid[iij6]
7754 = false;
7755 _ij6[1]
7756 = iij6;
7757 break;
7758 }
7759 }
7760 j6 = j6array[ij6];
7761 cj6 = cj6array
7762 [ij6];
7763 sj6 = sj6array
7764 [ij6];
7765 {
7766 IkReal
7767 evalcond[5];
7768 IkReal x862
7769 = ((0.3)
7770 * cj9);
7771 IkReal x863
7772 = ((0.045)
7773 * sj9);
7774 IkReal x864
7775 = IKcos(j6);
7776 IkReal x865
7777 = (pz
7778 * x864);
7779 IkReal x866
7780 = IKsin(j6);
7781 IkReal x867
7782 = (cj4
7783 * px);
7784 IkReal x868
7785 = (x866
7786 * x867);
7787 IkReal x869
7788 = (py
7789 * sj4);
7790 IkReal x870
7791 = (x866
7792 * x869);
7793 IkReal x871
7794 = ((0.045)
7795 * cj9);
7796 IkReal x872
7797 = ((0.3)
7798 * sj9);
7799 IkReal x873
7800 = (pz
7801 * x866);
7802 IkReal x874
7803 = (x864
7804 * x867);
7805 IkReal x875
7806 = (x864
7807 * x869);
7808 evalcond[0]
7809 = ((-0.55)
7810 + (((-1.0)
7811 * x862))
7812 + x868
7813 + x865
7814 + x870
7815 + (((-1.0)
7816 * x863)));
7817 evalcond[1]
7818 = ((0.045)
7819 + (((-1.0)
7820 * x873))
7821 + x872
7822 + x874
7823 + x875
7824 + (((-1.0)
7825 * x871)));
7826 evalcond[2]
7827 = ((((1.51009803921569)
7828 * x866))
7829 + (((-0.316735294117647)
7830 * x864))
7831 + pz
7832 + (((-3.92156862745098)
7833 * pp
7834 * x866))
7835 + (((-0.108264705882353)
7836 * cj9
7837 * x864))
7838 + (((-0.588235294117647)
7839 * pp
7840 * x864))
7841 + (((1.32323529411765)
7842 * cj9
7843 * x866)));
7844 evalcond[3]
7845 = ((((0.55)
7846 * x866))
7847 + (((-1.0)
7848 * x869))
7849 + (((-0.045)
7850 * x864))
7851 + (((-1.0)
7852 * x867))
7853 + (((-1.0)
7854 * x864
7855 * x872))
7856 + ((x862
7857 * x866))
7858 + ((x864
7859 * x871))
7860 + ((x863
7861 * x866)));
7862 evalcond[4]
7863 = ((-0.2125)
7864 + (((-0.09)
7865 * x875))
7866 + (((1.1)
7867 * x865))
7868 + (((0.09)
7869 * x873))
7870 + (((-0.09)
7871 * x874))
7872 + (((1.1)
7873 * x870))
7874 + (((1.1)
7875 * x868))
7876 + (((-1.0)
7877 * (1.0)
7878 * pp)));
7879 if (IKabs(
7880 evalcond
7881 [0])
7882 > IKFAST_EVALCOND_THRESH
7883 || IKabs(
7884 evalcond
7885 [1])
7886 > IKFAST_EVALCOND_THRESH
7887 || IKabs(
7888 evalcond
7889 [2])
7890 > IKFAST_EVALCOND_THRESH
7891 || IKabs(
7892 evalcond
7893 [3])
7894 > IKFAST_EVALCOND_THRESH
7895 || IKabs(
7896 evalcond
7897 [4])
7898 > IKFAST_EVALCOND_THRESH)
7899 {
7900 continue;
7901 }
7902 }
7903
7904 rotationfunction0(
7905 solutions);
7906 }
7907 }
7908 }
7909 }
7910 }
7911 else
7912 {
7913 {
7914 IkReal j6array[1],
7915 cj6array[1],
7916 sj6array[1];
7917 bool j6valid[1]
7918 = {false};
7919 _nj6 = 1;
7920 IkReal x876
7921 = ((0.045) * cj4
7922 * px);
7923 IkReal x877
7924 = ((0.045) * py
7925 * sj4);
7926 IkReal x878
7927 = ((0.3) * sj9);
7928 IkReal x879
7929 = (cj4 * px);
7930 IkReal x880
7931 = (py * sj4);
7932 IkReal x881
7933 = (cj9 * sj9);
7934 IkReal x882 = cj9 * cj9;
7935 IkReal x883
7936 = ((1.0) * pz);
7937 IkReal x884 = py * py;
7938 IkReal x885 = cj4 * cj4;
7939 CheckValue<IkReal> x886
7940 = IKatan2WithCheck(
7941 IkReal((
7942 (-0.03825)
7943 + (((0.027)
7944 * x882))
7945 + (((-0.087975)
7946 * x881))
7947 + (((-1.0)
7948 * (0.167025)
7949 * sj9))
7950 + (((-1.0)
7951 * x879
7952 * x883))
7953 + (((-1.0)
7954 * x880
7955 * x883))
7956 + (((0.01125)
7957 * cj9)))),
7958 ((-0.304525)
7959 + (((-1.0)
7960 * x884
7961 * x885))
7962 + (((2.0) * cj4
7963 * px
7964 * x880))
7965 + (((-1.0)
7966 * (0.0495)
7967 * sj9))
7968 + (((-0.087975)
7969 * x882))
7970 + ((x885
7971 * (px
7972 * px)))
7973 + x884
7974 + (((-1.0)
7975 * (0.33)
7976 * cj9))
7977 + (((-0.027)
7978 * x881))),
7979 IKFAST_ATAN2_MAGTHRESH);
7980 if (!x886.valid)
7981 {
7982 continue;
7983 }
7984 CheckValue<IkReal> x887
7985 = IKPowWithIntegerCheck(
7986 IKsign((
7987 (((-1.0)
7988 * x878
7989 * x879))
7990 + (((-1.0)
7991 * (0.55)
7992 * pz))
7993 + (((-1.0)
7994 * x877))
7995 + (((-1.0)
7996 * x876))
7997 + (((-1.0)
7998 * x878
7999 * x880))
8000 + ((cj9
8001 * x877))
8002 + ((cj9
8003 * x876))
8004 + (((-1.0)
8005 * (0.045)
8006 * pz
8007 * sj9))
8008 + (((-1.0)
8009 * (0.3)
8010 * cj9
8011 * pz)))),
8012 -1);
8013 if (!x887.valid)
8014 {
8015 continue;
8016 }
8017 j6array[0]
8018 = ((-1.5707963267949)
8019 + (x886.value)
8020 + (((1.5707963267949)
8021 * (x887.value))));
8022 sj6array[0]
8023 = IKsin(j6array[0]);
8024 cj6array[0]
8025 = IKcos(j6array[0]);
8026 if (j6array[0] > IKPI)
8027 {
8028 j6array[0] -= IK2PI;
8029 }
8030 else if (
8031 j6array[0] < -IKPI)
8032 {
8033 j6array[0] += IK2PI;
8034 }
8035 j6valid[0] = true;
8036 for (int ij6 = 0;
8037 ij6 < 1;
8038 ++ij6)
8039 {
8040 if (!j6valid[ij6])
8041 {
8042 continue;
8043 }
8044 _ij6[0] = ij6;
8045 _ij6[1] = -1;
8046 for (int iij6
8047 = ij6 + 1;
8048 iij6 < 1;
8049 ++iij6)
8050 {
8051 if (j6valid[iij6]
8052 && IKabs(
8053 cj6array
8054 [ij6]
8055 - cj6array
8056 [iij6])
8057 < IKFAST_SOLUTION_THRESH
8058 && IKabs(
8059 sj6array
8060 [ij6]
8061 - sj6array
8062 [iij6])
8063 < IKFAST_SOLUTION_THRESH)
8064 {
8065 j6valid[iij6]
8066 = false;
8067 _ij6[1] = iij6;
8068 break;
8069 }
8070 }
8071 j6 = j6array[ij6];
8072 cj6 = cj6array[ij6];
8073 sj6 = sj6array[ij6];
8074 {
8075 IkReal evalcond[5];
8076 IkReal x888
8077 = ((0.3) * cj9);
8078 IkReal x889
8079 = ((0.045)
8080 * sj9);
8081 IkReal x890
8082 = IKcos(j6);
8083 IkReal x891
8084 = (pz * x890);
8085 IkReal x892
8086 = IKsin(j6);
8087 IkReal x893
8088 = (cj4 * px);
8089 IkReal x894
8090 = (x892 * x893);
8091 IkReal x895
8092 = (py * sj4);
8093 IkReal x896
8094 = (x892 * x895);
8095 IkReal x897
8096 = ((0.045)
8097 * cj9);
8098 IkReal x898
8099 = ((0.3) * sj9);
8100 IkReal x899
8101 = (pz * x892);
8102 IkReal x900
8103 = (x890 * x893);
8104 IkReal x901
8105 = (x890 * x895);
8106 evalcond[0]
8107 = ((-0.55)
8108 + (((-1.0)
8109 * x888))
8110 + x894 + x896
8111 + x891
8112 + (((-1.0)
8113 * x889)));
8114 evalcond[1]
8115 = ((0.045)
8116 + (((-1.0)
8117 * x897))
8118 + x900 + x901
8119 + x898
8120 + (((-1.0)
8121 * x899)));
8122 evalcond[2]
8123 = ((((-0.588235294117647)
8124 * pp
8125 * x890))
8126 + (((-0.316735294117647)
8127 * x890))
8128 + (((1.32323529411765)
8129 * cj9
8130 * x892))
8131 + pz
8132 + (((-0.108264705882353)
8133 * cj9
8134 * x890))
8135 + (((-3.92156862745098)
8136 * pp
8137 * x892))
8138 + (((1.51009803921569)
8139 * x892)));
8140 evalcond[3]
8141 = ((((-1.0)
8142 * x893))
8143 + ((x890
8144 * x897))
8145 + (((0.55)
8146 * x892))
8147 + ((x888
8148 * x892))
8149 + (((-1.0)
8150 * x890
8151 * x898))
8152 + ((x889
8153 * x892))
8154 + (((-1.0)
8155 * x895))
8156 + (((-0.045)
8157 * x890)));
8158 evalcond[4]
8159 = ((-0.2125)
8160 + (((-0.09)
8161 * x900))
8162 + (((1.1)
8163 * x891))
8164 + (((0.09)
8165 * x899))
8166 + (((-0.09)
8167 * x901))
8168 + (((1.1)
8169 * x896))
8170 + (((-1.0)
8171 * (1.0)
8172 * pp))
8173 + (((1.1)
8174 * x894)));
8175 if (IKabs(
8176 evalcond[0])
8177 > IKFAST_EVALCOND_THRESH
8178 || IKabs(
8179 evalcond
8180 [1])
8181 > IKFAST_EVALCOND_THRESH
8182 || IKabs(
8183 evalcond
8184 [2])
8185 > IKFAST_EVALCOND_THRESH
8186 || IKabs(
8187 evalcond
8188 [3])
8189 > IKFAST_EVALCOND_THRESH
8190 || IKabs(
8191 evalcond
8192 [4])
8193 > IKFAST_EVALCOND_THRESH)
8194 {
8195 continue;
8196 }
8197 }
8198
8199 rotationfunction0(
8200 solutions);
8201 }
8202 }
8203 }
8204 }
8205 }
8206 else
8207 {
8208 {
8209 IkReal j6array[1],
8210 cj6array[1],
8211 sj6array[1];
8212 bool j6valid[1] = {false};
8213 _nj6 = 1;
8214 IkReal x902 = py * py;
8215 IkReal x903 = (py * sj4);
8216 IkReal x904 = cj4 * cj4;
8217 IkReal x905
8218 = ((0.045) * pz);
8219 IkReal x906 = (cj4 * px);
8220 IkReal x907 = ((0.3) * pz);
8221 IkReal x908
8222 = ((0.3) * cj4 * px);
8223 IkReal x909
8224 = ((0.045) * x906);
8225 IkReal x910
8226 = ((0.3) * py * sj4);
8227 IkReal x911
8228 = ((0.045) * x903);
8229 CheckValue<IkReal> x912
8230 = IKatan2WithCheck(
8231 IkReal((
8232 ((sj9 * x911))
8233 + ((sj9 * x909))
8234 + ((cj9 * x910))
8235 + ((sj9 * x907))
8236 + x905
8237 + (((0.55)
8238 * x906))
8239 + (((0.55)
8240 * x903))
8241 + ((cj9 * x908))
8242 + (((-1.0) * cj9
8243 * x905)))),
8244 ((((-1.0) * x911))
8245 + ((cj9 * x907))
8246 + ((cj9 * x911))
8247 + ((cj9 * x909))
8248 + (((-1.0) * sj9
8249 * x910))
8250 + (((0.55) * pz))
8251 + (((-1.0) * sj9
8252 * x908))
8253 + (((-1.0) * x909))
8254 + ((sj9 * x905))),
8255 IKFAST_ATAN2_MAGTHRESH);
8256 if (!x912.valid)
8257 {
8258 continue;
8259 }
8260 CheckValue<IkReal> x913
8261 = IKPowWithIntegerCheck(
8262 IKsign(
8263 ((((2.0) * cj4
8264 * px * x903))
8265 + (pz * pz)
8266 + x902
8267 + ((x904
8268 * (px
8269 * px)))
8270 + (((-1.0)
8271 * x902
8272 * x904)))),
8273 -1);
8274 if (!x913.valid)
8275 {
8276 continue;
8277 }
8278 j6array[0]
8279 = ((-1.5707963267949)
8280 + (x912.value)
8281 + (((1.5707963267949)
8282 * (x913.value))));
8283 sj6array[0]
8284 = IKsin(j6array[0]);
8285 cj6array[0]
8286 = IKcos(j6array[0]);
8287 if (j6array[0] > IKPI)
8288 {
8289 j6array[0] -= IK2PI;
8290 }
8291 else if (j6array[0] < -IKPI)
8292 {
8293 j6array[0] += IK2PI;
8294 }
8295 j6valid[0] = true;
8296 for (int ij6 = 0; ij6 < 1;
8297 ++ij6)
8298 {
8299 if (!j6valid[ij6])
8300 {
8301 continue;
8302 }
8303 _ij6[0] = ij6;
8304 _ij6[1] = -1;
8305 for (int iij6 = ij6 + 1;
8306 iij6 < 1;
8307 ++iij6)
8308 {
8309 if (j6valid[iij6]
8310 && IKabs(
8311 cj6array[ij6]
8312 - cj6array
8313 [iij6])
8314 < IKFAST_SOLUTION_THRESH
8315 && IKabs(
8316 sj6array[ij6]
8317 - sj6array
8318 [iij6])
8319 < IKFAST_SOLUTION_THRESH)
8320 {
8321 j6valid[iij6] = false;
8322 _ij6[1] = iij6;
8323 break;
8324 }
8325 }
8326 j6 = j6array[ij6];
8327 cj6 = cj6array[ij6];
8328 sj6 = sj6array[ij6];
8329 {
8330 IkReal evalcond[5];
8331 IkReal x914
8332 = ((0.3) * cj9);
8333 IkReal x915
8334 = ((0.045) * sj9);
8335 IkReal x916 = IKcos(j6);
8336 IkReal x917
8337 = (pz * x916);
8338 IkReal x918 = IKsin(j6);
8339 IkReal x919
8340 = (cj4 * px);
8341 IkReal x920
8342 = (x918 * x919);
8343 IkReal x921
8344 = (py * sj4);
8345 IkReal x922
8346 = (x918 * x921);
8347 IkReal x923
8348 = ((0.045) * cj9);
8349 IkReal x924
8350 = ((0.3) * sj9);
8351 IkReal x925
8352 = (pz * x918);
8353 IkReal x926
8354 = (x916 * x919);
8355 IkReal x927
8356 = (x916 * x921);
8357 evalcond[0]
8358 = ((-0.55)
8359 + (((-1.0)
8360 * x914))
8361 + x917
8362 + (((-1.0)
8363 * x915))
8364 + x922 + x920);
8365 evalcond[1]
8366 = ((0.045)
8367 + (((-1.0)
8368 * x925))
8369 + (((-1.0)
8370 * x923))
8371 + x927 + x924
8372 + x926);
8373 evalcond[2]
8374 = ((((-0.316735294117647)
8375 * x916))
8376 + (((-3.92156862745098)
8377 * pp * x918))
8378 + (((-0.588235294117647)
8379 * pp * x916))
8380 + pz
8381 + (((-0.108264705882353)
8382 * cj9
8383 * x916))
8384 + (((1.32323529411765)
8385 * cj9
8386 * x918))
8387 + (((1.51009803921569)
8388 * x918)));
8389 evalcond[3]
8390 = ((((0.55) * x918))
8391 + (((-0.045)
8392 * x916))
8393 + ((x914 * x918))
8394 + ((x915 * x918))
8395 + (((-1.0)
8396 * x921))
8397 + ((x916 * x923))
8398 + (((-1.0)
8399 * x919))
8400 + (((-1.0) * x916
8401 * x924)));
8402 evalcond[4]
8403 = ((-0.2125)
8404 + (((-0.09)
8405 * x927))
8406 + (((1.1)
8407 * x920))
8408 + (((0.09)
8409 * x925))
8410 + (((-0.09)
8411 * x926))
8412 + (((1.1)
8413 * x917))
8414 + (((1.1)
8415 * x922))
8416 + (((-1.0)
8417 * (1.0)
8418 * pp)));
8419 if (IKabs(evalcond[0])
8420 > IKFAST_EVALCOND_THRESH
8421 || IKabs(
8422 evalcond[1])
8423 > IKFAST_EVALCOND_THRESH
8424 || IKabs(
8425 evalcond[2])
8426 > IKFAST_EVALCOND_THRESH
8427 || IKabs(
8428 evalcond[3])
8429 > IKFAST_EVALCOND_THRESH
8430 || IKabs(
8431 evalcond[4])
8432 > IKFAST_EVALCOND_THRESH)
8433 {
8434 continue;
8435 }
8436 }
8437
8438 rotationfunction0(
8439 solutions);
8440 }
8441 }
8442 }
8443 }
8444 }
8445 } while (0);
8446 if (bgotonextstatement)
8447 {
8448 bool bgotonextstatement = true;
8449 do
8450 {
8451 if (1)
8452 {
8453 bgotonextstatement = false;
8454 continue; // branch miss [j6]
8455 }
8456 } while (0);
8457 if (bgotonextstatement)
8458 {
8459 }
8460 }
8461 }
8462 }
8463 }
8464 else
8465 {
8466 {
8467 IkReal j6array[1], cj6array[1],
8468 sj6array[1];
8469 bool j6valid[1] = {false};
8470 _nj6 = 1;
8471 IkReal x928 = (pz * sj8);
8472 IkReal x929 = ((0.3) * cj9);
8473 IkReal x930 = ((0.045) * sj9);
8474 IkReal x931 = (cj4 * px);
8475 IkReal x932 = ((0.045) * cj8 * sj8);
8476 IkReal x933 = (x931 * x932);
8477 IkReal x934 = (py * sj4);
8478 IkReal x935 = (x932 * x934);
8479 IkReal x936 = ((0.3) * cj8 * sj8 * sj9);
8480 IkReal x937 = ((0.55) * cj8);
8481 IkReal x938 = (cj4 * py);
8482 IkReal x939 = (px * sj4);
8483 IkReal x940 = (cj4 * cj8 * py);
8484 IkReal x941 = ((1.0) * pz * sj8);
8485 IkReal x942 = (cj8 * px * sj4);
8486 IkReal x943 = cj8 * cj8;
8487 IkReal x944 = ((0.045) * x943);
8488 IkReal x945 = (x938 * x944);
8489 IkReal x946 = (x939 * x944);
8490 IkReal x947 = ((0.3) * sj9 * x943);
8491 CheckValue<IkReal> x948
8492 = IKPowWithIntegerCheck(
8493 IKsign(
8494 ((((-0.55) * x928))
8495 + (((-1.0) * cj9 * x933))
8496 + (((-1.0) * cj9 * x935))
8497 + (((-1.0) * x928 * x929))
8498 + ((x931 * x936))
8499 + (((-1.0) * x928 * x930))
8500 + x933 + x935
8501 + ((x934 * x936)))),
8502 -1);
8503 if (!x948.valid)
8504 {
8505 continue;
8506 }
8507 CheckValue<IkReal> x949
8508 = IKatan2WithCheck(
8509 IkReal(
8510 ((((-1.0) * x937 * x939))
8511 + ((x937 * x938))
8512 + ((x930 * x940))
8513 + (((-1.0) * x930 * x942))
8514 + (((-1.0) * x931 * x941))
8515 + (((-1.0) * x934 * x941))
8516 + (((-1.0) * x929 * x942))
8517 + ((x929 * x940)))),
8518 (((cj9 * x946))
8519 + (((-1.0) * (1.0) * sj8
8520 * (pz * pz)))
8521 + ((x938 * x947))
8522 + (((-1.0) * x939 * x947))
8523 + (((-1.0) * cj9 * x945))
8524 + (((-1.0) * x946)) + x945),
8525 IKFAST_ATAN2_MAGTHRESH);
8526 if (!x949.valid)
8527 {
8528 continue;
8529 }
8530 j6array[0]
8531 = ((-1.5707963267949)
8532 + (((1.5707963267949)
8533 * (x948.value)))
8534 + (x949.value));
8535 sj6array[0] = IKsin(j6array[0]);
8536 cj6array[0] = IKcos(j6array[0]);
8537 if (j6array[0] > IKPI)
8538 {
8539 j6array[0] -= IK2PI;
8540 }
8541 else if (j6array[0] < -IKPI)
8542 {
8543 j6array[0] += IK2PI;
8544 }
8545 j6valid[0] = true;
8546 for (int ij6 = 0; ij6 < 1; ++ij6)
8547 {
8548 if (!j6valid[ij6])
8549 {
8550 continue;
8551 }
8552 _ij6[0] = ij6;
8553 _ij6[1] = -1;
8554 for (int iij6 = ij6 + 1; iij6 < 1;
8555 ++iij6)
8556 {
8557 if (j6valid[iij6]
8558 && IKabs(
8559 cj6array[ij6]
8560 - cj6array[iij6])
8561 < IKFAST_SOLUTION_THRESH
8562 && IKabs(
8563 sj6array[ij6]
8564 - sj6array[iij6])
8565 < IKFAST_SOLUTION_THRESH)
8566 {
8567 j6valid[iij6] = false;
8568 _ij6[1] = iij6;
8569 break;
8570 }
8571 }
8572 j6 = j6array[ij6];
8573 cj6 = cj6array[ij6];
8574 sj6 = sj6array[ij6];
8575 {
8576 IkReal evalcond[6];
8577 IkReal x950 = ((0.3) * cj9);
8578 IkReal x951 = ((0.045) * sj9);
8579 IkReal x952 = IKcos(j6);
8580 IkReal x953 = (pz * x952);
8581 IkReal x954 = IKsin(j6);
8582 IkReal x955 = (cj4 * px * x954);
8583 IkReal x956 = (py * sj4);
8584 IkReal x957 = (x954 * x956);
8585 IkReal x958 = (px * sj4);
8586 IkReal x959 = ((1.0) * cj4 * py);
8587 IkReal x960 = (cj4 * sj8);
8588 IkReal x961 = ((0.045) * cj8);
8589 IkReal x962 = ((0.045) * cj9);
8590 IkReal x963 = (cj8 * x954);
8591 IkReal x964 = ((0.3) * sj9);
8592 IkReal x965 = (sj8 * x958);
8593 IkReal x966 = (pz * x963);
8594 IkReal x967
8595 = (px * (((1.0) * cj4)));
8596 IkReal x968 = (cj8 * x952);
8597 IkReal x969 = ((1.0) * x956);
8598 IkReal x970 = ((0.09) * cj8 * x952);
8599 evalcond[0]
8600 = ((-0.55) + x953 + x955 + x957
8601 + (((-1.0) * x951))
8602 + (((-1.0) * x950)));
8603 evalcond[1]
8604 = ((((-1.0) * cj8 * x959))
8605 + ((cj8 * x958))
8606 + (((-1.0) * pz * sj8
8607 * x954))
8608 + ((sj8 * x952 * x956))
8609 + ((px * x952 * x960)));
8610 evalcond[2]
8611 = ((((-0.55) * x952)) + pz
8612 + (((-1.0) * x962 * x963))
8613 + (((-1.0) * x951 * x952))
8614 + ((x954 * x961))
8615 + ((x963 * x964))
8616 + (((-1.0) * x950 * x952)));
8617 evalcond[3]
8618 = ((0.045)
8619 + (((-1.0) * x968 * x969))
8620 + (((-1.0) * sj8 * x959))
8621 + x966 + x964 + x965
8622 + (((-1.0) * x962))
8623 + (((-1.0) * x967 * x968)));
8624 evalcond[4]
8625 = (((x952 * x961))
8626 + ((x964 * x968))
8627 + (((0.55) * x954))
8628 + (((-1.0) * x962 * x968))
8629 + (((-1.0) * x969))
8630 + ((x951 * x954))
8631 + ((x950 * x954))
8632 + (((-1.0) * x967)));
8633 evalcond[5]
8634 = ((-0.2125) + (((1.1) * x957))
8635 + (((1.1) * x953))
8636 + (((-0.09) * x965))
8637 + (((1.1) * x955))
8638 + (((0.09) * py * x960))
8639 + ((cj4 * px * x970))
8640 + ((x956 * x970))
8641 + (((-0.09) * x966))
8642 + (((-1.0) * (1.0) * pp)));
8643 if (IKabs(evalcond[0])
8644 > IKFAST_EVALCOND_THRESH
8645 || IKabs(evalcond[1])
8646 > IKFAST_EVALCOND_THRESH
8647 || IKabs(evalcond[2])
8648 > IKFAST_EVALCOND_THRESH
8649 || IKabs(evalcond[3])
8650 > IKFAST_EVALCOND_THRESH
8651 || IKabs(evalcond[4])
8652 > IKFAST_EVALCOND_THRESH
8653 || IKabs(evalcond[5])
8654 > IKFAST_EVALCOND_THRESH)
8655 {
8656 continue;
8657 }
8658 }
8659
8660 rotationfunction0(solutions);
8661 }
8662 }
8663 }
8664 }
8665 }
8666 else
8667 {
8668 {
8669 IkReal j6array[1], cj6array[1], sj6array[1];
8670 bool j6valid[1] = {false};
8671 _nj6 = 1;
8672 IkReal x971 = py * py;
8673 IkReal x972 = (sj8 * x971);
8674 IkReal x973 = (cj4 * px * sj8);
8675 IkReal x974 = cj4 * cj4;
8676 IkReal x975 = px * px;
8677 IkReal x976 = ((0.55) * sj8);
8678 IkReal x977 = (cj8 * px);
8679 IkReal x978 = ((0.3) * cj9);
8680 IkReal x979 = ((0.045) * sj9);
8681 IkReal x980 = (py * sj4 * sj8);
8682 IkReal x981 = (pz * sj8);
8683 IkReal x982 = (cj4 * cj8 * sj4);
8684 CheckValue<IkReal> x983 = IKatan2WithCheck(
8685 IkReal(
8686 (((x973 * x978)) + ((x973 * x979))
8687 + ((pz * sj4 * x977))
8688 + (((-1.0) * cj4 * cj8 * py * pz))
8689 + ((cj4 * px * x976))
8690 + ((x979 * x980)) + ((x978 * x980))
8691 + ((py * sj4 * x976)))),
8692 (((pz * x976))
8693 + (((2.0) * py * x974 * x977))
8694 + ((x979 * x981)) + ((x971 * x982))
8695 + (((-1.0) * py * x977))
8696 + ((x978 * x981))
8697 + (((-1.0) * x975 * x982))),
8698 IKFAST_ATAN2_MAGTHRESH);
8699 if (!x983.valid)
8700 {
8701 continue;
8702 }
8703 CheckValue<IkReal> x984
8704 = IKPowWithIntegerCheck(
8705 IKsign(
8706 ((((2.0) * py * sj4 * x973))
8707 + ((sj8 * (pz * pz))) + x972
8708 + (((-1.0) * x972 * x974))
8709 + ((sj8 * x974 * x975)))),
8710 -1);
8711 if (!x984.valid)
8712 {
8713 continue;
8714 }
8715 j6array[0]
8716 = ((-1.5707963267949) + (x983.value)
8717 + (((1.5707963267949)
8718 * (x984.value))));
8719 sj6array[0] = IKsin(j6array[0]);
8720 cj6array[0] = IKcos(j6array[0]);
8721 if (j6array[0] > IKPI)
8722 {
8723 j6array[0] -= IK2PI;
8724 }
8725 else if (j6array[0] < -IKPI)
8726 {
8727 j6array[0] += IK2PI;
8728 }
8729 j6valid[0] = true;
8730 for (int ij6 = 0; ij6 < 1; ++ij6)
8731 {
8732 if (!j6valid[ij6])
8733 {
8734 continue;
8735 }
8736 _ij6[0] = ij6;
8737 _ij6[1] = -1;
8738 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
8739 {
8740 if (j6valid[iij6]
8741 && IKabs(
8742 cj6array[ij6]
8743 - cj6array[iij6])
8744 < IKFAST_SOLUTION_THRESH
8745 && IKabs(
8746 sj6array[ij6]
8747 - sj6array[iij6])
8748 < IKFAST_SOLUTION_THRESH)
8749 {
8750 j6valid[iij6] = false;
8751 _ij6[1] = iij6;
8752 break;
8753 }
8754 }
8755 j6 = j6array[ij6];
8756 cj6 = cj6array[ij6];
8757 sj6 = sj6array[ij6];
8758 {
8759 IkReal evalcond[6];
8760 IkReal x985 = ((0.3) * cj9);
8761 IkReal x986 = ((0.045) * sj9);
8762 IkReal x987 = IKcos(j6);
8763 IkReal x988 = (pz * x987);
8764 IkReal x989 = IKsin(j6);
8765 IkReal x990 = (cj4 * px * x989);
8766 IkReal x991 = (py * sj4);
8767 IkReal x992 = (x989 * x991);
8768 IkReal x993 = (px * sj4);
8769 IkReal x994 = ((1.0) * cj4 * py);
8770 IkReal x995 = (cj4 * sj8);
8771 IkReal x996 = ((0.045) * cj8);
8772 IkReal x997 = ((0.045) * cj9);
8773 IkReal x998 = (cj8 * x989);
8774 IkReal x999 = ((0.3) * sj9);
8775 IkReal x1000 = (sj8 * x993);
8776 IkReal x1001 = (pz * x998);
8777 IkReal x1002 = (px * (((1.0) * cj4)));
8778 IkReal x1003 = (cj8 * x987);
8779 IkReal x1004 = ((1.0) * x991);
8780 IkReal x1005 = ((0.09) * cj8 * x987);
8781 evalcond[0]
8782 = ((-0.55) + (((-1.0) * x985))
8783 + (((-1.0) * x986)) + x988 + x990
8784 + x992);
8785 evalcond[1]
8786 = ((((-1.0) * cj8 * x994))
8787 + (((-1.0) * pz * sj8 * x989))
8788 + ((px * x987 * x995))
8789 + ((sj8 * x987 * x991))
8790 + ((cj8 * x993)));
8791 evalcond[2]
8792 = ((((-1.0) * x986 * x987)) + pz
8793 + (((-0.55) * x987))
8794 + (((-1.0) * x997 * x998))
8795 + ((x998 * x999))
8796 + (((-1.0) * x985 * x987))
8797 + ((x989 * x996)));
8798 evalcond[3]
8799 = ((0.045) + (((-1.0) * sj8 * x994))
8800 + (((-1.0) * x997)) + x1000
8801 + x1001
8802 + (((-1.0) * x1002 * x1003))
8803 + (((-1.0) * x1003 * x1004))
8804 + x999);
8805 evalcond[4]
8806 = ((((-1.0) * x1003 * x997))
8807 + ((x985 * x989))
8808 + ((x1003 * x999))
8809 + (((0.55) * x989))
8810 + (((-1.0) * x1004))
8811 + (((-1.0) * x1002))
8812 + ((x986 * x989))
8813 + ((x987 * x996)));
8814 evalcond[5]
8815 = ((-0.2125) + (((1.1) * x990))
8816 + ((cj4 * px * x1005))
8817 + ((x1005 * x991))
8818 + (((-0.09) * x1001))
8819 + (((0.09) * py * x995))
8820 + (((-0.09) * x1000))
8821 + (((-1.0) * (1.0) * pp))
8822 + (((1.1) * x988))
8823 + (((1.1) * x992)));
8824 if (IKabs(evalcond[0])
8825 > IKFAST_EVALCOND_THRESH
8826 || IKabs(evalcond[1])
8827 > IKFAST_EVALCOND_THRESH
8828 || IKabs(evalcond[2])
8829 > IKFAST_EVALCOND_THRESH
8830 || IKabs(evalcond[3])
8831 > IKFAST_EVALCOND_THRESH
8832 || IKabs(evalcond[4])
8833 > IKFAST_EVALCOND_THRESH
8834 || IKabs(evalcond[5])
8835 > IKFAST_EVALCOND_THRESH)
8836 {
8837 continue;
8838 }
8839 }
8840
8841 rotationfunction0(solutions);
8842 }
8843 }
8844 }
8845 }
8846 }
8847 else
8848 {
8849 {
8850 IkReal j6array[1], cj6array[1], sj6array[1];
8851 bool j6valid[1] = {false};
8852 _nj6 = 1;
8853 IkReal x1006 = (cj4 * px);
8854 IkReal x1007 = ((0.045) * pz);
8855 IkReal x1008 = (py * sj4);
8856 IkReal x1009 = ((0.3) * cj9);
8857 IkReal x1010 = ((0.045) * sj9);
8858 IkReal x1011 = (cj8 * cj9);
8859 IkReal x1012 = (cj8 * sj9);
8860 IkReal x1013 = cj9 * cj9;
8861 IkReal x1014 = ((1.0) * pz);
8862 CheckValue<IkReal> x1015 = IKatan2WithCheck(
8863 IkReal(
8864 ((-0.304525)
8865 + (((-1.0) * (0.027) * cj9 * sj9))
8866 + (((-1.0) * (0.0495) * sj9))
8867 + (pz * pz) + (((-0.087975) * x1013))
8868 + (((-1.0) * (0.33) * cj9)))),
8869 ((((-0.087975) * sj9 * x1011))
8870 + (((-1.0) * x1008 * x1014))
8871 + (((-1.0) * x1006 * x1014))
8872 + (((0.027) * cj8 * x1013))
8873 + (((0.01125) * x1011))
8874 + (((-0.167025) * x1012))
8875 + (((-1.0) * (0.03825) * cj8))),
8876 IKFAST_ATAN2_MAGTHRESH);
8877 if (!x1015.valid)
8878 {
8879 continue;
8880 }
8881 CheckValue<IkReal> x1016
8882 = IKPowWithIntegerCheck(
8883 IKsign(
8884 ((((-1.0) * cj8 * x1007))
8885 + ((x1007 * x1011))
8886 + (((-0.3) * pz * x1012))
8887 + (((-0.55) * x1008))
8888 + (((-1.0) * x1008 * x1010))
8889 + (((-0.55) * x1006))
8890 + (((-1.0) * x1008 * x1009))
8891 + (((-1.0) * x1006 * x1009))
8892 + (((-1.0) * x1006 * x1010)))),
8893 -1);
8894 if (!x1016.valid)
8895 {
8896 continue;
8897 }
8898 j6array[0]
8899 = ((-1.5707963267949) + (x1015.value)
8900 + (((1.5707963267949) * (x1016.value))));
8901 sj6array[0] = IKsin(j6array[0]);
8902 cj6array[0] = IKcos(j6array[0]);
8903 if (j6array[0] > IKPI)
8904 {
8905 j6array[0] -= IK2PI;
8906 }
8907 else if (j6array[0] < -IKPI)
8908 {
8909 j6array[0] += IK2PI;
8910 }
8911 j6valid[0] = true;
8912 for (int ij6 = 0; ij6 < 1; ++ij6)
8913 {
8914 if (!j6valid[ij6])
8915 {
8916 continue;
8917 }
8918 _ij6[0] = ij6;
8919 _ij6[1] = -1;
8920 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
8921 {
8922 if (j6valid[iij6]
8923 && IKabs(cj6array[ij6] - cj6array[iij6])
8924 < IKFAST_SOLUTION_THRESH
8925 && IKabs(sj6array[ij6] - sj6array[iij6])
8926 < IKFAST_SOLUTION_THRESH)
8927 {
8928 j6valid[iij6] = false;
8929 _ij6[1] = iij6;
8930 break;
8931 }
8932 }
8933 j6 = j6array[ij6];
8934 cj6 = cj6array[ij6];
8935 sj6 = sj6array[ij6];
8936 {
8937 IkReal evalcond[6];
8938 IkReal x1017 = ((0.3) * cj9);
8939 IkReal x1018 = ((0.045) * sj9);
8940 IkReal x1019 = IKcos(j6);
8941 IkReal x1020 = (pz * x1019);
8942 IkReal x1021 = IKsin(j6);
8943 IkReal x1022 = (cj4 * px * x1021);
8944 IkReal x1023 = (py * sj4);
8945 IkReal x1024 = (x1021 * x1023);
8946 IkReal x1025 = (px * sj4);
8947 IkReal x1026 = ((1.0) * cj4 * py);
8948 IkReal x1027 = (cj4 * sj8);
8949 IkReal x1028 = ((0.045) * cj8);
8950 IkReal x1029 = ((0.045) * cj9);
8951 IkReal x1030 = (cj8 * x1021);
8952 IkReal x1031 = ((0.3) * sj9);
8953 IkReal x1032 = (sj8 * x1025);
8954 IkReal x1033 = (pz * x1030);
8955 IkReal x1034 = (px * (((1.0) * cj4)));
8956 IkReal x1035 = (cj8 * x1019);
8957 IkReal x1036 = ((1.0) * x1023);
8958 IkReal x1037 = ((0.09) * cj8 * x1019);
8959 evalcond[0]
8960 = ((-0.55) + x1020 + x1022 + x1024
8961 + (((-1.0) * x1018))
8962 + (((-1.0) * x1017)));
8963 evalcond[1]
8964 = ((((-1.0) * cj8 * x1026))
8965 + (((-1.0) * pz * sj8 * x1021))
8966 + ((px * x1019 * x1027))
8967 + ((cj8 * x1025))
8968 + ((sj8 * x1019 * x1023)));
8969 evalcond[2]
8970 = ((((-1.0) * x1018 * x1019))
8971 + (((-1.0) * x1029 * x1030))
8972 + ((x1030 * x1031))
8973 + ((x1021 * x1028))
8974 + (((-0.55) * x1019)) + pz
8975 + (((-1.0) * x1017 * x1019)));
8976 evalcond[3]
8977 = ((0.045) + (((-1.0) * x1029))
8978 + (((-1.0) * x1035 * x1036))
8979 + (((-1.0) * x1034 * x1035)) + x1031
8980 + x1033 + x1032
8981 + (((-1.0) * sj8 * x1026)));
8982 evalcond[4]
8983 = (((x1031 * x1035))
8984 + (((0.55) * x1021))
8985 + (((-1.0) * x1036))
8986 + ((x1019 * x1028))
8987 + ((x1017 * x1021))
8988 + ((x1018 * x1021))
8989 + (((-1.0) * x1029 * x1035))
8990 + (((-1.0) * x1034)));
8991 evalcond[5]
8992 = ((-0.2125) + ((cj4 * px * x1037))
8993 + ((x1023 * x1037))
8994 + (((1.1) * x1024))
8995 + (((-0.09) * x1032))
8996 + (((0.09) * py * x1027))
8997 + (((1.1) * x1020))
8998 + (((-1.0) * (1.0) * pp))
8999 + (((1.1) * x1022))
9000 + (((-0.09) * x1033)));
9001 if (IKabs(evalcond[0])
9002 > IKFAST_EVALCOND_THRESH
9003 || IKabs(evalcond[1])
9004 > IKFAST_EVALCOND_THRESH
9005 || IKabs(evalcond[2])
9006 > IKFAST_EVALCOND_THRESH
9007 || IKabs(evalcond[3])
9008 > IKFAST_EVALCOND_THRESH
9009 || IKabs(evalcond[4])
9010 > IKFAST_EVALCOND_THRESH
9011 || IKabs(evalcond[5])
9012 > IKFAST_EVALCOND_THRESH)
9013 {
9014 continue;
9015 }
9016 }
9017
9018 rotationfunction0(solutions);
9019 }
9020 }
9021 }
9022 }
9023 }
9024 }
9025 }
9026 }
9027 }
9028 else
9029 {
9030 {
9031 IkReal j6array[2], cj6array[2], sj6array[2];
9032 bool j6valid[2] = {false};
9033 _nj6 = 2;
9034 IkReal x1038
9035 = ((-0.55) + (((-1.0) * (0.3) * cj9))
9036 + (((-1.0) * (0.045) * sj9)));
9037 IkReal x1039 = ((0.045) * cj8);
9038 IkReal x1040
9039 = ((((-1.0) * cj9 * x1039)) + x1039
9040 + (((0.3) * cj8 * sj9)));
9041 CheckValue<IkReal> x1043 = IKatan2WithCheck(
9042 IkReal(x1038), x1040, IKFAST_ATAN2_MAGTHRESH);
9043 if (!x1043.valid)
9044 {
9045 continue;
9046 }
9047 IkReal x1041 = ((-1.0) * (x1043.value));
9048 if ((((x1040 * x1040) + (x1038 * x1038))) < -0.00001)
9049 continue;
9050 CheckValue<IkReal> x1044 = IKPowWithIntegerCheck(
9051 IKabs(IKsqrt(((x1040 * x1040) + (x1038 * x1038)))), -1);
9052 if (!x1044.valid)
9053 {
9054 continue;
9055 }
9056 if (((pz * (x1044.value))) < -1 - IKFAST_SINCOS_THRESH
9057 || ((pz * (x1044.value))) > 1 + IKFAST_SINCOS_THRESH)
9058 continue;
9059 IkReal x1042 = IKasin((pz * (x1044.value)));
9060 j6array[0] = ((((-1.0) * x1042)) + x1041);
9061 sj6array[0] = IKsin(j6array[0]);
9062 cj6array[0] = IKcos(j6array[0]);
9063 j6array[1] = ((3.14159265358979) + x1042 + x1041);
9064 sj6array[1] = IKsin(j6array[1]);
9065 cj6array[1] = IKcos(j6array[1]);
9066 if (j6array[0] > IKPI)
9067 {
9068 j6array[0] -= IK2PI;
9069 }
9070 else if (j6array[0] < -IKPI)
9071 {
9072 j6array[0] += IK2PI;
9073 }
9074 j6valid[0] = true;
9075 if (j6array[1] > IKPI)
9076 {
9077 j6array[1] -= IK2PI;
9078 }
9079 else if (j6array[1] < -IKPI)
9080 {
9081 j6array[1] += IK2PI;
9082 }
9083 j6valid[1] = true;
9084 for (int ij6 = 0; ij6 < 2; ++ij6)
9085 {
9086 if (!j6valid[ij6])
9087 {
9088 continue;
9089 }
9090 _ij6[0] = ij6;
9091 _ij6[1] = -1;
9092 for (int iij6 = ij6 + 1; iij6 < 2; ++iij6)
9093 {
9094 if (j6valid[iij6]
9095 && IKabs(cj6array[ij6] - cj6array[iij6])
9096 < IKFAST_SOLUTION_THRESH
9097 && IKabs(sj6array[ij6] - sj6array[iij6])
9098 < IKFAST_SOLUTION_THRESH)
9099 {
9100 j6valid[iij6] = false;
9101 _ij6[1] = iij6;
9102 break;
9103 }
9104 }
9105 j6 = j6array[ij6];
9106 cj6 = cj6array[ij6];
9107 sj6 = sj6array[ij6];
9108
9109 {
9110 IkReal j4eval[2];
9111 IkReal x1045
9112 = ((((-1.0) * (1.0) * sj6 * (pz * pz)))
9113 + ((pp * sj6)));
9114 j4eval[0] = x1045;
9115 j4eval[1] = IKsign(x1045);
9116 if (IKabs(j4eval[0]) < 0.0000010000000000
9117 || IKabs(j4eval[1]) < 0.0000010000000000)
9118 {
9119 {
9120 IkReal j4eval[2];
9121 IkReal x1046 = (cj8 * sj6);
9122 IkReal x1047
9123 = (((x1046 * (pz * pz)))
9124 + (((-1.0) * pp * x1046)));
9125 j4eval[0] = x1047;
9126 j4eval[1] = IKsign(x1047);
9127 if (IKabs(j4eval[0]) < 0.0000010000000000
9128 || IKabs(j4eval[1]) < 0.0000010000000000)
9129 {
9130 {
9131 IkReal j4eval[2];
9132 IkReal x1048
9133 = (pp + (((-1.0) * (1.0) * (pz * pz))));
9134 j4eval[0] = x1048;
9135 j4eval[1] = IKsign(x1048);
9136 if (IKabs(j4eval[0]) < 0.0000010000000000
9137 || IKabs(j4eval[1]) < 0.0000010000000000)
9138 {
9139 {
9140 IkReal evalcond[4];
9141 bool bgotonextstatement = true;
9142 do
9143 {
9144 evalcond[0]
9145 = ((-3.14159265358979)
9146 + (IKfmod(
9147 ((3.14159265358979)
9148 + (IKabs(
9149 ((-1.5707963267949)
9150 + j8)))),
9151 6.28318530717959)));
9152 evalcond[1]
9153 = ((0.39655) + (((0.0765) * sj9))
9154 + (((0.32595) * cj9))
9155 + (((-1.0) * (1.0) * pp)));
9156 evalcond[2]
9157 = ((((-1.0) * (0.3) * cj6 * cj9))
9158 + (((-1.0) * (0.55) * cj6)) + pz
9159 + (((-1.0) * (0.045) * cj6
9160 * sj9)));
9161 if (IKabs(evalcond[0])
9162 < 0.0000010000000000
9163 && IKabs(evalcond[1])
9164 < 0.0000010000000000
9165 && IKabs(evalcond[2])
9166 < 0.0000010000000000)
9167 {
9168 bgotonextstatement = false;
9169 {
9170 IkReal j4eval[3];
9171 sj8 = 1.0;
9172 cj8 = 0;
9173 j8 = 1.5707963267949;
9174 IkReal x1049
9175 = ((((-1.0) * (1.0) * cj6
9176 * (pz * pz)))
9177 + ((cj6 * pp)));
9178 IkReal x1050
9179 = ((1.51009803921569) * cj6);
9180 IkReal x1051 = (pz * sj6);
9181 IkReal x1052
9182 = ((1.32323529411765) * cj6
9183 * cj9);
9184 IkReal x1053
9185 = ((3.92156862745098) * cj6 * pp);
9186 j4eval[0] = x1049;
9187 j4eval[1] = IKsign(x1049);
9188 j4eval[2]
9189 = ((IKabs(
9190 (((py * x1053))
9191 + (((-1.0) * py * x1052))
9192 + (((-1.0) * py * x1050))
9193 + ((px * x1051)))))
9194 + (IKabs(
9195 (((px * x1052))
9196 + ((px * x1050))
9197 + ((py * x1051))
9198 + (((-1.0) * px
9199 * x1053))))));
9200 if (IKabs(j4eval[0])
9201 < 0.0000010000000000
9202 || IKabs(j4eval[1])
9203 < 0.0000010000000000
9204 || IKabs(j4eval[2])
9205 < 0.0000010000000000)
9206 {
9207 {
9208 IkReal j4eval[3];
9209 sj8 = 1.0;
9210 cj8 = 0;
9211 j8 = 1.5707963267949;
9212 IkReal x1054 = (cj6 * pp);
9213 IkReal x1055 = (cj6 * (pz * pz));
9214 IkReal x1056 = ((0.2125) * cj6);
9215 IkReal x1057 = ((1.1) * pz);
9216 IkReal x1058
9217 = ((0.09) * pz * sj6);
9218 j4eval[0]
9219 = ((((-1.0) * x1055))
9220 + x1054);
9221 j4eval[1] = IKsign(
9222 ((((-0.09) * x1055))
9223 + (((0.09) * x1054))));
9224 j4eval[2]
9225 = ((IKabs(
9226 (((py * x1058))
9227 + ((px * x1057))
9228 + (((-1.0) * px
9229 * x1054))
9230 + (((-1.0) * px
9231 * x1056)))))
9232 + (IKabs((
9233 ((py * x1056))
9234 + ((py * x1054))
9235 + (((-1.0) * py
9236 * x1057))
9237 + ((px * x1058))))));
9238 if (IKabs(j4eval[0])
9239 < 0.0000010000000000
9240 || IKabs(j4eval[1])
9241 < 0.0000010000000000
9242 || IKabs(j4eval[2])
9243 < 0.0000010000000000)
9244 {
9245 {
9246 IkReal j4eval[3];
9247 sj8 = 1.0;
9248 cj8 = 0;
9249 j8 = 1.5707963267949;
9250 IkReal x1059
9251 = (pp
9252 + (((-1.0) * (1.0)
9253 * (pz * pz))));
9254 IkReal x1060
9255 = ((1.32323529411765)
9256 * cj9);
9257 IkReal x1061
9258 = ((3.92156862745098)
9259 * pp);
9260 IkReal x1062
9261 = ((0.316735294117647)
9262 * sj6);
9263 IkReal x1063
9264 = ((0.108264705882353)
9265 * cj9 * sj6);
9266 IkReal x1064
9267 = ((0.588235294117647)
9268 * pp * sj6);
9269 j4eval[0] = x1059;
9270 j4eval[1]
9271 = ((IKabs((
9272 (((-1.0)
9273 * (1.51009803921569)
9274 * py))
9275 + ((px * x1064))
9276 + (((-1.0) * py
9277 * x1060))
9278 + ((px * x1063))
9279 + ((py * x1061))
9280 + ((px * x1062)))))
9281 + (IKabs((
9282 ((px * x1060))
9283 + (((-1.0) * px
9284 * x1061))
9285 + (((1.51009803921569)
9286 * px))
9287 + ((py * x1063))
9288 + ((py * x1062))
9289 + ((py
9290 * x1064))))));
9291 j4eval[2] = IKsign(x1059);
9292 if (IKabs(j4eval[0])
9293 < 0.0000010000000000
9294 || IKabs(j4eval[1])
9295 < 0.0000010000000000
9296 || IKabs(j4eval[2])
9297 < 0.0000010000000000)
9298 {
9299 {
9300 IkReal evalcond[7];
9301 bool bgotonextstatement
9302 = true;
9303 do
9304 {
9305 evalcond[0]
9306 = ((-3.14159265358979)
9307 + (IKfmod(
9308 ((3.14159265358979)
9309 + (IKabs((
9310 (-1.5707963267949)
9311 + j6)))),
9312 6.28318530717959)));
9313 evalcond[1]
9314 = ((-1.0)
9315 * (((1.0)
9316 * pz)));
9317 if (IKabs(evalcond[0])
9318 < 0.0000010000000000
9319 && IKabs(
9320 evalcond[1])
9321 < 0.0000010000000000)
9322 {
9323 bgotonextstatement
9324 = false;
9325 {
9326 IkReal j4eval[3];
9327 sj8 = 1.0;
9328 cj8 = 0;
9329 j8 = 1.5707963267949;
9330 sj6 = 1.0;
9331 cj6 = 0;
9332 j6 = 1.5707963267949;
9333 IkReal x1065
9334 = (pp
9335 + (((-1.0)
9336 * (1.0)
9337 * (pz
9338 * pz))));
9339 IkReal x1066
9340 = (cj9 * px);
9341 IkReal x1067
9342 = (cj9 * py);
9343 IkReal x1068
9344 = ((3.92156862745098)
9345 * pp);
9346 IkReal x1069
9347 = ((0.045)
9348 * sj9);
9349 j4eval[0] = x1065;
9350 j4eval[1]
9351 = ((IKabs((
9352 (((1.51009803921569)
9353 * px))
9354 + ((py
9355 * x1069))
9356 + (((0.55)
9357 * py))
9358 + (((0.3)
9359 * x1067))
9360 + (((1.32323529411765)
9361 * x1066))
9362 + (((-1.0)
9363 * px
9364 * x1068)))))
9365 + (IKabs((
9366 (((0.3)
9367 * x1066))
9368 + (((-1.0)
9369 * (1.51009803921569)
9370 * py))
9371 + (((-1.32323529411765)
9372 * x1067))
9373 + (((0.55)
9374 * px))
9375 + ((py
9376 * x1068))
9377 + ((px
9378 * x1069))))));
9379 j4eval[2]
9380 = IKsign(x1065);
9381 if (IKabs(j4eval[0])
9382 < 0.0000010000000000
9383 || IKabs(
9384 j4eval
9385 [1])
9386 < 0.0000010000000000
9387 || IKabs(
9388 j4eval
9389 [2])
9390 < 0.0000010000000000)
9391 {
9392 {
9393 IkReal
9394 j4eval[3];
9395 sj8 = 1.0;
9396 cj8 = 0;
9397 j8 = 1.5707963267949;
9398 sj6 = 1.0;
9399 cj6 = 0;
9400 j6 = 1.5707963267949;
9401 IkReal x1070
9402 = (pp
9403 + (((-1.0)
9404 * (1.0)
9405 * (pz
9406 * pz))));
9407 IkReal x1071
9408 = (cj9
9409 * px);
9410 IkReal x1072
9411 = (cj9
9412 * py);
9413 IkReal x1073
9414 = (pp * px);
9415 IkReal x1074
9416 = (pp * py);
9417 j4eval[0]
9418 = x1070;
9419 j4eval[1]
9420 = ((IKabs((
9421 (((0.108264705882353)
9422 * x1072))
9423 + (((0.588235294117647)
9424 * x1074))
9425 + (((1.32323529411765)
9426 * x1071))
9427 + (((0.316735294117647)
9428 * py))
9429 + (((1.51009803921569)
9430 * px))
9431 + (((-3.92156862745098)
9432 * x1073)))))
9433 + (IKabs((
9434 (((-1.32323529411765)
9435 * x1072))
9436 + (((0.588235294117647)
9437 * x1073))
9438 + (((3.92156862745098)
9439 * x1074))
9440 + (((0.316735294117647)
9441 * px))
9442 + (((0.108264705882353)
9443 * x1071))
9444 + (((-1.0)
9445 * (1.51009803921569)
9446 * py))))));
9447 j4eval[2]
9448 = IKsign(
9449 x1070);
9450 if (IKabs(
9451 j4eval
9452 [0])
9453 < 0.0000010000000000
9454 || IKabs(
9455 j4eval
9456 [1])
9457 < 0.0000010000000000
9458 || IKabs(
9459 j4eval
9460 [2])
9461 < 0.0000010000000000)
9462 {
9463 {
9464 IkReal
9465 j4eval
9466 [3];
9467 sj8 = 1.0;
9468 cj8 = 0;
9469 j8 = 1.5707963267949;
9470 sj6 = 1.0;
9471 cj6 = 0;
9472 j6 = 1.5707963267949;
9473 IkReal x1075
9474 = pz
9475 * pz;
9476 IkReal x1076
9477 = (cj9
9478 * px);
9479 IkReal x1077
9480 = (cj9
9481 * py);
9482 IkReal x1078
9483 = (pp
9484 * px);
9485 IkReal x1079
9486 = (pp
9487 * py);
9488 j4eval[0]
9489 = ((((-1.0)
9490 * x1075))
9491 + pp);
9492 j4eval[1]
9493 = ((IKabs((
9494 (((0.348408823529412)
9495 * py))
9496 + (((1.66110784313725)
9497 * px))
9498 + (((1.45555882352941)
9499 * x1076))
9500 + (((0.647058823529412)
9501 * x1079))
9502 + (((-4.31372549019608)
9503 * x1078))
9504 + (((0.119091176470588)
9505 * x1077)))))
9506 + (IKabs((
9507 (((0.348408823529412)
9508 * px))
9509 + (((-1.45555882352941)
9510 * x1077))
9511 + (((4.31372549019608)
9512 * x1079))
9513 + (((0.647058823529412)
9514 * x1078))
9515 + (((-1.0)
9516 * (1.66110784313725)
9517 * py))
9518 + (((0.119091176470588)
9519 * x1076))))));
9520 j4eval[2] = IKsign((
9521 (((1.1)
9522 * pp))
9523 + (((-1.1)
9524 * x1075))));
9525 if (IKabs(
9526 j4eval
9527 [0])
9528 < 0.0000010000000000
9529 || IKabs(
9530 j4eval
9531 [1])
9532 < 0.0000010000000000
9533 || IKabs(
9534 j4eval
9535 [2])
9536 < 0.0000010000000000)
9537 {
9538 {
9539 IkReal evalcond
9540 [6];
9541 bool
9542 bgotonextstatement
9543 = true;
9544 do
9545 {
9546 IkReal
9547 x1080
9548 = ((-1.51009803921569)
9549 + (((-1.0)
9550 * (1.32323529411765)
9551 * cj9))
9552 + (((3.92156862745098)
9553 * pp)));
9554 evalcond
9555 [0]
9556 = ((IKabs(
9557 py))
9558 + (IKabs(
9559 px)));
9560 evalcond
9561 [1]
9562 = ((-0.55)
9563 + (((-1.0)
9564 * (0.3)
9565 * cj9))
9566 + (((-1.0)
9567 * (0.045)
9568 * sj9)));
9569 evalcond
9570 [2]
9571 = x1080;
9572 evalcond
9573 [3]
9574 = x1080;
9575 evalcond
9576 [4]
9577 = ((0.316735294117647)
9578 + (((0.108264705882353)
9579 * cj9))
9580 + (((0.588235294117647)
9581 * pp)));
9582 evalcond
9583 [5]
9584 = ((-0.2125)
9585 + (((-1.0)
9586 * (1.0)
9587 * pp)));
9588 if (IKabs(
9589 evalcond
9590 [0])
9591 < 0.0000010000000000
9592 && IKabs(
9593 evalcond
9594 [1])
9595 < 0.0000010000000000
9596 && IKabs(
9597 evalcond
9598 [2])
9599 < 0.0000010000000000
9600 && IKabs(
9601 evalcond
9602 [3])
9603 < 0.0000010000000000
9604 && IKabs(
9605 evalcond
9606 [4])
9607 < 0.0000010000000000
9608 && IKabs(
9609 evalcond
9610 [5])
9611 < 0.0000010000000000)
9612 {
9613 bgotonextstatement
9614 = false;
9615 {
9616 IkReal j4array
9617 [4],
9618 cj4array
9619 [4],
9620 sj4array
9621 [4];
9622 bool j4valid
9623 [4]
9624 = {false};
9625 _nj4
9626 = 4;
9627 j4array
9628 [0]
9629 = 0;
9630 sj4array
9631 [0]
9632 = IKsin(
9633 j4array
9634 [0]);
9635 cj4array
9636 [0]
9637 = IKcos(
9638 j4array
9639 [0]);
9640 j4array
9641 [1]
9642 = 1.5707963267949;
9643 sj4array
9644 [1]
9645 = IKsin(
9646 j4array
9647 [1]);
9648 cj4array
9649 [1]
9650 = IKcos(
9651 j4array
9652 [1]);
9653 j4array
9654 [2]
9655 = 3.14159265358979;
9656 sj4array
9657 [2]
9658 = IKsin(
9659 j4array
9660 [2]);
9661 cj4array
9662 [2]
9663 = IKcos(
9664 j4array
9665 [2]);
9666 j4array
9667 [3]
9668 = -1.5707963267949;
9669 sj4array
9670 [3]
9671 = IKsin(
9672 j4array
9673 [3]);
9674 cj4array
9675 [3]
9676 = IKcos(
9677 j4array
9678 [3]);
9679 if (j4array
9680 [0]
9681 > IKPI)
9682 {
9683 j4array
9684 [0]
9685 -= IK2PI;
9686 }
9687 else if (
9688 j4array
9689 [0]
9690 < -IKPI)
9691 {
9692 j4array
9693 [0]
9694 += IK2PI;
9695 }
9696 j4valid
9697 [0]
9698 = true;
9699 if (j4array
9700 [1]
9701 > IKPI)
9702 {
9703 j4array
9704 [1]
9705 -= IK2PI;
9706 }
9707 else if (
9708 j4array
9709 [1]
9710 < -IKPI)
9711 {
9712 j4array
9713 [1]
9714 += IK2PI;
9715 }
9716 j4valid
9717 [1]
9718 = true;
9719 if (j4array
9720 [2]
9721 > IKPI)
9722 {
9723 j4array
9724 [2]
9725 -= IK2PI;
9726 }
9727 else if (
9728 j4array
9729 [2]
9730 < -IKPI)
9731 {
9732 j4array
9733 [2]
9734 += IK2PI;
9735 }
9736 j4valid
9737 [2]
9738 = true;
9739 if (j4array
9740 [3]
9741 > IKPI)
9742 {
9743 j4array
9744 [3]
9745 -= IK2PI;
9746 }
9747 else if (
9748 j4array
9749 [3]
9750 < -IKPI)
9751 {
9752 j4array
9753 [3]
9754 += IK2PI;
9755 }
9756 j4valid
9757 [3]
9758 = true;
9759 for (
9760 int ij4
9761 = 0;
9762 ij4
9763 < 4;
9764 ++ij4)
9765 {
9766 if (!j4valid
9767 [ij4])
9768 {
9769 continue;
9770 }
9771 _ij4[0]
9772 = ij4;
9773 _ij4[1]
9774 = -1;
9775 for (
9776 int iij4
9777 = ij4
9778 + 1;
9779 iij4
9780 < 4;
9781 ++iij4)
9782 {
9783 if (j4valid
9784 [iij4]
9785 && IKabs(
9786 cj4array
9787 [ij4]
9788 - cj4array
9789 [iij4])
9790 < IKFAST_SOLUTION_THRESH
9791 && IKabs(
9792 sj4array
9793 [ij4]
9794 - sj4array
9795 [iij4])
9796 < IKFAST_SOLUTION_THRESH)
9797 {
9798 j4valid
9799 [iij4]
9800 = false;
9801 _ij4[1]
9802 = iij4;
9803 break;
9804 }
9805 }
9806 j4 = j4array
9807 [ij4];
9808 cj4 = cj4array
9809 [ij4];
9810 sj4 = sj4array
9811 [ij4];
9812
9813 rotationfunction0(
9814 solutions);
9815 }
9816 }
9817 }
9818 } while (
9819 0);
9820 if (bgotonextstatement)
9821 {
9822 bool
9823 bgotonextstatement
9824 = true;
9825 do
9826 {
9827 if (1)
9828 {
9829 bgotonextstatement
9830 = false;
9831 continue; // branch miss [j4]
9832 }
9833 } while (
9834 0);
9835 if (bgotonextstatement)
9836 {
9837 }
9838 }
9839 }
9840 }
9841 else
9842 {
9843 {
9844 IkReal j4array
9845 [1],
9846 cj4array
9847 [1],
9848 sj4array
9849 [1];
9850 bool j4valid
9851 [1]
9852 = {false};
9853 _nj4
9854 = 1;
9855 IkReal
9856 x1081
9857 = (cj9
9858 * px);
9859 IkReal
9860 x1082
9861 = (cj9
9862 * py);
9863 IkReal
9864 x1083
9865 = (pp
9866 * px);
9867 IkReal
9868 x1084
9869 = (pp
9870 * py);
9871 CheckValue<IkReal> x1085 = IKatan2WithCheck(
9872 IkReal((
9873 (((0.119091176470588)
9874 * x1082))
9875 + (((0.348408823529412)
9876 * py))
9877 + (((1.66110784313725)
9878 * px))
9879 + (((0.647058823529412)
9880 * x1084))
9881 + (((-4.31372549019608)
9882 * x1083))
9883 + (((1.45555882352941)
9884 * x1081)))),
9885 ((((4.31372549019608)
9886 * x1084))
9887 + (((0.119091176470588)
9888 * x1081))
9889 + (((0.348408823529412)
9890 * px))
9891 + (((0.647058823529412)
9892 * x1083))
9893 + (((-1.45555882352941)
9894 * x1082))
9895 + (((-1.0)
9896 * (1.66110784313725)
9897 * py))),
9898 IKFAST_ATAN2_MAGTHRESH);
9899 if (!x1085
9900 .valid)
9901 {
9902 continue;
9903 }
9904 CheckValue<IkReal> x1086 = IKPowWithIntegerCheck(
9905 IKsign((
9906 (((1.1)
9907 * pp))
9908 + (((-1.0)
9909 * (1.1)
9910 * (pz
9911 * pz))))),
9912 -1);
9913 if (!x1086
9914 .valid)
9915 {
9916 continue;
9917 }
9918 j4array
9919 [0]
9920 = ((-1.5707963267949)
9921 + (x1085
9922 .value)
9923 + (((1.5707963267949)
9924 * (x1086
9925 .value))));
9926 sj4array
9927 [0]
9928 = IKsin(
9929 j4array
9930 [0]);
9931 cj4array
9932 [0]
9933 = IKcos(
9934 j4array
9935 [0]);
9936 if (j4array
9937 [0]
9938 > IKPI)
9939 {
9940 j4array
9941 [0]
9942 -= IK2PI;
9943 }
9944 else if (
9945 j4array
9946 [0]
9947 < -IKPI)
9948 {
9949 j4array
9950 [0]
9951 += IK2PI;
9952 }
9953 j4valid
9954 [0]
9955 = true;
9956 for (
9957 int ij4
9958 = 0;
9959 ij4
9960 < 1;
9961 ++ij4)
9962 {
9963 if (!j4valid
9964 [ij4])
9965 {
9966 continue;
9967 }
9968 _ij4[0]
9969 = ij4;
9970 _ij4[1]
9971 = -1;
9972 for (
9973 int iij4
9974 = ij4
9975 + 1;
9976 iij4
9977 < 1;
9978 ++iij4)
9979 {
9980 if (j4valid
9981 [iij4]
9982 && IKabs(
9983 cj4array
9984 [ij4]
9985 - cj4array
9986 [iij4])
9987 < IKFAST_SOLUTION_THRESH
9988 && IKabs(
9989 sj4array
9990 [ij4]
9991 - sj4array
9992 [iij4])
9993 < IKFAST_SOLUTION_THRESH)
9994 {
9995 j4valid
9996 [iij4]
9997 = false;
9998 _ij4[1]
9999 = iij4;
10000 break;
10001 }
10002 }
10003 j4 = j4array
10004 [ij4];
10005 cj4 = cj4array
10006 [ij4];
10007 sj4 = sj4array
10008 [ij4];
10009 {
10010 IkReal evalcond
10011 [4];
10012 IkReal
10013 x1087
10014 = IKcos(
10015 j4);
10016 IkReal
10017 x1088
10018 = (px
10019 * x1087);
10020 IkReal
10021 x1089
10022 = IKsin(
10023 j4);
10024 IkReal
10025 x1090
10026 = (py
10027 * x1089);
10028 IkReal
10029 x1091
10030 = (px
10031 * x1089);
10032 IkReal
10033 x1092
10034 = (py
10035 * x1087);
10036 evalcond
10037 [0]
10038 = ((-0.55)
10039 + x1088
10040 + (((-1.0)
10041 * (0.3)
10042 * cj9))
10043 + x1090
10044 + (((-1.0)
10045 * (0.045)
10046 * sj9)));
10047 evalcond
10048 [1]
10049 = ((-1.51009803921569)
10050 + (((-1.0)
10051 * x1092))
10052 + (((-1.0)
10053 * (1.32323529411765)
10054 * cj9))
10055 + (((3.92156862745098)
10056 * pp))
10057 + x1091);
10058 evalcond
10059 [2]
10060 = ((0.316735294117647)
10061 + (((-1.0)
10062 * x1090))
10063 + (((-1.0)
10064 * x1088))
10065 + (((0.108264705882353)
10066 * cj9))
10067 + (((0.588235294117647)
10068 * pp)));
10069 evalcond
10070 [3]
10071 = ((-0.2125)
10072 + (((1.1)
10073 * x1090))
10074 + (((0.09)
10075 * x1092))
10076 + (((-0.09)
10077 * x1091))
10078 + (((1.1)
10079 * x1088))
10080 + (((-1.0)
10081 * (1.0)
10082 * pp)));
10083 if (IKabs(
10084 evalcond
10085 [0])
10086 > IKFAST_EVALCOND_THRESH
10087 || IKabs(
10088 evalcond
10089 [1])
10090 > IKFAST_EVALCOND_THRESH
10091 || IKabs(
10092 evalcond
10093 [2])
10094 > IKFAST_EVALCOND_THRESH
10095 || IKabs(
10096 evalcond
10097 [3])
10098 > IKFAST_EVALCOND_THRESH)
10099 {
10100 continue;
10101 }
10102 }
10103
10104 rotationfunction0(
10105 solutions);
10106 }
10107 }
10108 }
10109 }
10110 }
10111 else
10112 {
10113 {
10114 IkReal
10115 j4array
10116 [1],
10117 cj4array
10118 [1],
10119 sj4array
10120 [1];
10121 bool j4valid
10122 [1]
10123 = {false};
10124 _nj4 = 1;
10125 IkReal x1093
10126 = (cj9
10127 * px);
10128 IkReal x1094
10129 = (cj9
10130 * py);
10131 IkReal x1095
10132 = (pp
10133 * px);
10134 IkReal x1096
10135 = (pp
10136 * py);
10137 CheckValue<IkReal> x1097 = IKatan2WithCheck(
10138 IkReal((
10139 (((-3.92156862745098)
10140 * x1095))
10141 + (((0.316735294117647)
10142 * py))
10143 + (((1.51009803921569)
10144 * px))
10145 + (((0.108264705882353)
10146 * x1094))
10147 + (((1.32323529411765)
10148 * x1093))
10149 + (((0.588235294117647)
10150 * x1096)))),
10151 ((((0.108264705882353)
10152 * x1093))
10153 + (((0.588235294117647)
10154 * x1095))
10155 + (((-1.32323529411765)
10156 * x1094))
10157 + (((0.316735294117647)
10158 * px))
10159 + (((-1.0)
10160 * (1.51009803921569)
10161 * py))
10162 + (((3.92156862745098)
10163 * x1096))),
10164 IKFAST_ATAN2_MAGTHRESH);
10165 if (!x1097
10166 .valid)
10167 {
10168 continue;
10169 }
10170 CheckValue<IkReal> x1098 = IKPowWithIntegerCheck(
10171 IKsign((
10172 pp
10173 + (((-1.0)
10174 * (1.0)
10175 * (pz
10176 * pz))))),
10177 -1);
10178 if (!x1098
10179 .valid)
10180 {
10181 continue;
10182 }
10183 j4array[0]
10184 = ((-1.5707963267949)
10185 + (x1097
10186 .value)
10187 + (((1.5707963267949)
10188 * (x1098
10189 .value))));
10190 sj4array[0] = IKsin(
10191 j4array
10192 [0]);
10193 cj4array[0] = IKcos(
10194 j4array
10195 [0]);
10196 if (j4array
10197 [0]
10198 > IKPI)
10199 {
10200 j4array[0]
10201 -= IK2PI;
10202 }
10203 else if (
10204 j4array
10205 [0]
10206 < -IKPI)
10207 {
10208 j4array[0]
10209 += IK2PI;
10210 }
10211 j4valid[0]
10212 = true;
10213 for (int ij4
10214 = 0;
10215 ij4
10216 < 1;
10217 ++ij4)
10218 {
10219 if (!j4valid
10220 [ij4])
10221 {
10222 continue;
10223 }
10224 _ij4[0]
10225 = ij4;
10226 _ij4[1]
10227 = -1;
10228 for (
10229 int iij4
10230 = ij4
10231 + 1;
10232 iij4
10233 < 1;
10234 ++iij4)
10235 {
10236 if (j4valid
10237 [iij4]
10238 && IKabs(
10239 cj4array
10240 [ij4]
10241 - cj4array
10242 [iij4])
10243 < IKFAST_SOLUTION_THRESH
10244 && IKabs(
10245 sj4array
10246 [ij4]
10247 - sj4array
10248 [iij4])
10249 < IKFAST_SOLUTION_THRESH)
10250 {
10251 j4valid
10252 [iij4]
10253 = false;
10254 _ij4[1]
10255 = iij4;
10256 break;
10257 }
10258 }
10259 j4 = j4array
10260 [ij4];
10261 cj4 = cj4array
10262 [ij4];
10263 sj4 = sj4array
10264 [ij4];
10265 {
10266 IkReal evalcond
10267 [4];
10268 IkReal
10269 x1099
10270 = IKcos(
10271 j4);
10272 IkReal
10273 x1100
10274 = (px
10275 * x1099);
10276 IkReal
10277 x1101
10278 = IKsin(
10279 j4);
10280 IkReal
10281 x1102
10282 = (py
10283 * x1101);
10284 IkReal
10285 x1103
10286 = (px
10287 * x1101);
10288 IkReal
10289 x1104
10290 = (py
10291 * x1099);
10292 evalcond
10293 [0]
10294 = ((-0.55)
10295 + x1102
10296 + x1100
10297 + (((-1.0)
10298 * (0.3)
10299 * cj9))
10300 + (((-1.0)
10301 * (0.045)
10302 * sj9)));
10303 evalcond
10304 [1]
10305 = ((-1.51009803921569)
10306 + (((-1.0)
10307 * x1104))
10308 + (((-1.0)
10309 * (1.32323529411765)
10310 * cj9))
10311 + x1103
10312 + (((3.92156862745098)
10313 * pp)));
10314 evalcond
10315 [2]
10316 = ((0.316735294117647)
10317 + (((-1.0)
10318 * x1102))
10319 + (((-1.0)
10320 * x1100))
10321 + (((0.108264705882353)
10322 * cj9))
10323 + (((0.588235294117647)
10324 * pp)));
10325 evalcond
10326 [3]
10327 = ((-0.2125)
10328 + (((-0.09)
10329 * x1103))
10330 + (((1.1)
10331 * x1100))
10332 + (((1.1)
10333 * x1102))
10334 + (((-1.0)
10335 * (1.0)
10336 * pp))
10337 + (((0.09)
10338 * x1104)));
10339 if (IKabs(
10340 evalcond
10341 [0])
10342 > IKFAST_EVALCOND_THRESH
10343 || IKabs(
10344 evalcond
10345 [1])
10346 > IKFAST_EVALCOND_THRESH
10347 || IKabs(
10348 evalcond
10349 [2])
10350 > IKFAST_EVALCOND_THRESH
10351 || IKabs(
10352 evalcond
10353 [3])
10354 > IKFAST_EVALCOND_THRESH)
10355 {
10356 continue;
10357 }
10358 }
10359
10360 rotationfunction0(
10361 solutions);
10362 }
10363 }
10364 }
10365 }
10366 }
10367 else
10368 {
10369 {
10370 IkReal
10371 j4array[1],
10372 cj4array[1],
10373 sj4array[1];
10374 bool j4valid[1]
10375 = {false};
10376 _nj4 = 1;
10377 IkReal x1105
10378 = (cj9
10379 * px);
10380 IkReal x1106
10381 = (cj9
10382 * py);
10383 IkReal x1107
10384 = ((3.92156862745098)
10385 * pp);
10386 IkReal x1108
10387 = ((0.045)
10388 * sj9);
10389 CheckValue<IkReal> x1109 = IKPowWithIntegerCheck(
10390 IKsign((
10391 pp
10392 + (((-1.0)
10393 * (1.0)
10394 * (pz
10395 * pz))))),
10396 -1);
10397 if (!x1109
10398 .valid)
10399 {
10400 continue;
10401 }
10402 CheckValue<IkReal> x1110 = IKatan2WithCheck(
10403 IkReal((
10404 (((-1.0)
10405 * px
10406 * x1107))
10407 + ((py
10408 * x1108))
10409 + (((1.51009803921569)
10410 * px))
10411 + (((0.55)
10412 * py))
10413 + (((1.32323529411765)
10414 * x1105))
10415 + (((0.3)
10416 * x1106)))),
10417 ((((-1.32323529411765)
10418 * x1106))
10419 + (((-1.0)
10420 * (1.51009803921569)
10421 * py))
10422 + ((py
10423 * x1107))
10424 + (((0.55)
10425 * px))
10426 + ((px
10427 * x1108))
10428 + (((0.3)
10429 * x1105))),
10430 IKFAST_ATAN2_MAGTHRESH);
10431 if (!x1110
10432 .valid)
10433 {
10434 continue;
10435 }
10436 j4array[0]
10437 = ((-1.5707963267949)
10438 + (((1.5707963267949)
10439 * (x1109
10440 .value)))
10441 + (x1110
10442 .value));
10443 sj4array[0]
10444 = IKsin(
10445 j4array
10446 [0]);
10447 cj4array[0]
10448 = IKcos(
10449 j4array
10450 [0]);
10451 if (j4array[0]
10452 > IKPI)
10453 {
10454 j4array[0]
10455 -= IK2PI;
10456 }
10457 else if (
10458 j4array[0]
10459 < -IKPI)
10460 {
10461 j4array[0]
10462 += IK2PI;
10463 }
10464 j4valid[0]
10465 = true;
10466 for (int ij4
10467 = 0;
10468 ij4 < 1;
10469 ++ij4)
10470 {
10471 if (!j4valid
10472 [ij4])
10473 {
10474 continue;
10475 }
10476 _ij4[0] = ij4;
10477 _ij4[1] = -1;
10478 for (int iij4
10479 = ij4
10480 + 1;
10481 iij4 < 1;
10482 ++iij4)
10483 {
10484 if (j4valid
10485 [iij4]
10486 && IKabs(
10487 cj4array
10488 [ij4]
10489 - cj4array
10490 [iij4])
10491 < IKFAST_SOLUTION_THRESH
10492 && IKabs(
10493 sj4array
10494 [ij4]
10495 - sj4array
10496 [iij4])
10497 < IKFAST_SOLUTION_THRESH)
10498 {
10499 j4valid
10500 [iij4]
10501 = false;
10502 _ij4[1]
10503 = iij4;
10504 break;
10505 }
10506 }
10507 j4 = j4array
10508 [ij4];
10509 cj4 = cj4array
10510 [ij4];
10511 sj4 = sj4array
10512 [ij4];
10513 {
10514 IkReal
10515 evalcond
10516 [4];
10517 IkReal x1111
10518 = IKcos(
10519 j4);
10520 IkReal x1112
10521 = (px
10522 * x1111);
10523 IkReal x1113
10524 = IKsin(
10525 j4);
10526 IkReal x1114
10527 = (py
10528 * x1113);
10529 IkReal x1115
10530 = (px
10531 * x1113);
10532 IkReal x1116
10533 = (py
10534 * x1111);
10535 evalcond[0]
10536 = ((-0.55)
10537 + x1112
10538 + x1114
10539 + (((-1.0)
10540 * (0.3)
10541 * cj9))
10542 + (((-1.0)
10543 * (0.045)
10544 * sj9)));
10545 evalcond[1]
10546 = ((-1.51009803921569)
10547 + x1115
10548 + (((-1.0)
10549 * (1.32323529411765)
10550 * cj9))
10551 + (((-1.0)
10552 * x1116))
10553 + (((3.92156862745098)
10554 * pp)));
10555 evalcond[2]
10556 = ((0.316735294117647)
10557 + (((-1.0)
10558 * x1114))
10559 + (((0.108264705882353)
10560 * cj9))
10561 + (((0.588235294117647)
10562 * pp))
10563 + (((-1.0)
10564 * x1112)));
10565 evalcond[3]
10566 = ((-0.2125)
10567 + (((-0.09)
10568 * x1115))
10569 + (((1.1)
10570 * x1114))
10571 + (((1.1)
10572 * x1112))
10573 + (((-1.0)
10574 * (1.0)
10575 * pp))
10576 + (((0.09)
10577 * x1116)));
10578 if (IKabs(
10579 evalcond
10580 [0])
10581 > IKFAST_EVALCOND_THRESH
10582 || IKabs(
10583 evalcond
10584 [1])
10585 > IKFAST_EVALCOND_THRESH
10586 || IKabs(
10587 evalcond
10588 [2])
10589 > IKFAST_EVALCOND_THRESH
10590 || IKabs(
10591 evalcond
10592 [3])
10593 > IKFAST_EVALCOND_THRESH)
10594 {
10595 continue;
10596 }
10597 }
10598
10599 rotationfunction0(
10600 solutions);
10601 }
10602 }
10603 }
10604 }
10605 }
10606 } while (0);
10607 if (bgotonextstatement)
10608 {
10609 bool bgotonextstatement
10610 = true;
10611 do
10612 {
10613 evalcond[0]
10614 = ((-3.14159265358979)
10615 + (IKfmod(
10616 ((3.14159265358979)
10617 + (IKabs((
10618 (1.5707963267949)
10619 + j6)))),
10620 6.28318530717959)));
10621 evalcond[1] = pz;
10622 if (IKabs(evalcond[0])
10623 < 0.0000010000000000
10624 && IKabs(evalcond
10625 [1])
10626 < 0.0000010000000000)
10627 {
10628 bgotonextstatement
10629 = false;
10630 {
10631 IkReal j4eval[3];
10632 sj8 = 1.0;
10633 cj8 = 0;
10634 j8 = 1.5707963267949;
10635 sj6 = -1.0;
10636 cj6 = 0;
10637 j6 = -1.5707963267949;
10638 IkReal x1117
10639 = (pp
10640 + (((-1.0)
10641 * (1.0)
10642 * (pz
10643 * pz))));
10644 IkReal x1118
10645 = (cj9 * px);
10646 IkReal x1119
10647 = (cj9 * py);
10648 IkReal x1120
10649 = ((3.92156862745098)
10650 * pp);
10651 IkReal x1121
10652 = ((0.045)
10653 * sj9);
10654 j4eval[0] = x1117;
10655 j4eval[1]
10656 = ((IKabs((
10657 (((-0.3)
10658 * x1119))
10659 + (((1.51009803921569)
10660 * px))
10661 + (((1.32323529411765)
10662 * x1118))
10663 + (((-1.0)
10664 * px
10665 * x1120))
10666 + (((-1.0)
10667 * py
10668 * x1121))
10669 + (((-1.0)
10670 * (0.55)
10671 * py)))))
10672 + (IKabs((
10673 (((-1.0)
10674 * px
10675 * x1121))
10676 + (((-1.0)
10677 * (0.55)
10678 * px))
10679 + (((-0.3)
10680 * x1118))
10681 + (((-1.0)
10682 * (1.51009803921569)
10683 * py))
10684 + ((py
10685 * x1120))
10686 + (((-1.32323529411765)
10687 * x1119))))));
10688 j4eval[2]
10689 = IKsign(
10690 x1117);
10691 if (IKabs(
10692 j4eval[0])
10693 < 0.0000010000000000
10694 || IKabs(
10695 j4eval
10696 [1])
10697 < 0.0000010000000000
10698 || IKabs(
10699 j4eval
10700 [2])
10701 < 0.0000010000000000)
10702 {
10703 {
10704 IkReal
10705 j4eval[3];
10706 sj8 = 1.0;
10707 cj8 = 0;
10708 j8 = 1.5707963267949;
10709 sj6 = -1.0;
10710 cj6 = 0;
10711 j6 = -1.5707963267949;
10712 IkReal x1122
10713 = (pp
10714 + (((-1.0)
10715 * (1.0)
10716 * (pz
10717 * pz))));
10718 IkReal x1123
10719 = (cj9
10720 * px);
10721 IkReal x1124
10722 = (cj9
10723 * py);
10724 IkReal x1125
10725 = (pp
10726 * px);
10727 IkReal x1126
10728 = (pp
10729 * py);
10730 j4eval[0]
10731 = x1122;
10732 j4eval[1]
10733 = ((IKabs((
10734 (((-0.108264705882353)
10735 * x1124))
10736 + (((1.51009803921569)
10737 * px))
10738 + (((-1.0)
10739 * (0.316735294117647)
10740 * py))
10741 + (((-3.92156862745098)
10742 * x1125))
10743 + (((-0.588235294117647)
10744 * x1126))
10745 + (((1.32323529411765)
10746 * x1123)))))
10747 + (IKabs((
10748 (((-0.108264705882353)
10749 * x1123))
10750 + (((-1.0)
10751 * (1.51009803921569)
10752 * py))
10753 + (((-1.0)
10754 * (0.316735294117647)
10755 * px))
10756 + (((-0.588235294117647)
10757 * x1125))
10758 + (((3.92156862745098)
10759 * x1126))
10760 + (((-1.32323529411765)
10761 * x1124))))));
10762 j4eval[2]
10763 = IKsign(
10764 x1122);
10765 if (IKabs(
10766 j4eval
10767 [0])
10768 < 0.0000010000000000
10769 || IKabs(
10770 j4eval
10771 [1])
10772 < 0.0000010000000000
10773 || IKabs(
10774 j4eval
10775 [2])
10776 < 0.0000010000000000)
10777 {
10778 {
10779 IkReal j4eval
10780 [3];
10781 sj8 = 1.0;
10782 cj8 = 0;
10783 j8 = 1.5707963267949;
10784 sj6 = -1.0;
10785 cj6 = 0;
10786 j6 = -1.5707963267949;
10787 IkReal
10788 x1127
10789 = pz
10790 * pz;
10791 IkReal
10792 x1128
10793 = (cj9
10794 * px);
10795 IkReal
10796 x1129
10797 = (cj9
10798 * py);
10799 IkReal
10800 x1130
10801 = (pp
10802 * px);
10803 IkReal
10804 x1131
10805 = (pp
10806 * py);
10807 j4eval[0]
10808 = (x1127
10809 + (((-1.0)
10810 * (1.0)
10811 * pp)));
10812 j4eval[1] = IKsign((
10813 (((1.1)
10814 * x1127))
10815 + (((-1.0)
10816 * (1.1)
10817 * pp))));
10818 j4eval[2]
10819 = ((IKabs((
10820 (((0.348408823529412)
10821 * px))
10822 + (((1.66110784313725)
10823 * py))
10824 + (((0.119091176470588)
10825 * x1128))
10826 + (((0.647058823529412)
10827 * x1130))
10828 + (((1.45555882352941)
10829 * x1129))
10830 + (((-4.31372549019608)
10831 * x1131)))))
10832 + (IKabs((
10833 (((0.647058823529412)
10834 * x1131))
10835 + (((0.348408823529412)
10836 * py))
10837 + (((0.119091176470588)
10838 * x1129))
10839 + (((4.31372549019608)
10840 * x1130))
10841 + (((-1.0)
10842 * (1.66110784313725)
10843 * px))
10844 + (((-1.45555882352941)
10845 * x1128))))));
10846 if (IKabs(
10847 j4eval
10848 [0])
10849 < 0.0000010000000000
10850 || IKabs(
10851 j4eval
10852 [1])
10853 < 0.0000010000000000
10854 || IKabs(
10855 j4eval
10856 [2])
10857 < 0.0000010000000000)
10858 {
10859 {
10860 IkReal evalcond
10861 [6];
10862 bool
10863 bgotonextstatement
10864 = true;
10865 do
10866 {
10867 IkReal
10868 x1132
10869 = ((-1.51009803921569)
10870 + (((-1.0)
10871 * (1.32323529411765)
10872 * cj9))
10873 + (((3.92156862745098)
10874 * pp)));
10875 evalcond
10876 [0]
10877 = ((IKabs(
10878 py))
10879 + (IKabs(
10880 px)));
10881 evalcond
10882 [1]
10883 = ((-0.55)
10884 + (((-1.0)
10885 * (0.3)
10886 * cj9))
10887 + (((-1.0)
10888 * (0.045)
10889 * sj9)));
10890 evalcond
10891 [2]
10892 = x1132;
10893 evalcond
10894 [3]
10895 = x1132;
10896 evalcond
10897 [4]
10898 = ((-0.316735294117647)
10899 + (((-1.0)
10900 * (0.588235294117647)
10901 * pp))
10902 + (((-1.0)
10903 * (0.108264705882353)
10904 * cj9)));
10905 evalcond
10906 [5]
10907 = ((-0.2125)
10908 + (((-1.0)
10909 * (1.0)
10910 * pp)));
10911 if (IKabs(
10912 evalcond
10913 [0])
10914 < 0.0000010000000000
10915 && IKabs(
10916 evalcond
10917 [1])
10918 < 0.0000010000000000
10919 && IKabs(
10920 evalcond
10921 [2])
10922 < 0.0000010000000000
10923 && IKabs(
10924 evalcond
10925 [3])
10926 < 0.0000010000000000
10927 && IKabs(
10928 evalcond
10929 [4])
10930 < 0.0000010000000000
10931 && IKabs(
10932 evalcond
10933 [5])
10934 < 0.0000010000000000)
10935 {
10936 bgotonextstatement
10937 = false;
10938 {
10939 IkReal j4array
10940 [4],
10941 cj4array
10942 [4],
10943 sj4array
10944 [4];
10945 bool j4valid
10946 [4]
10947 = {false};
10948 _nj4
10949 = 4;
10950 j4array
10951 [0]
10952 = 0;
10953 sj4array
10954 [0]
10955 = IKsin(
10956 j4array
10957 [0]);
10958 cj4array
10959 [0]
10960 = IKcos(
10961 j4array
10962 [0]);
10963 j4array
10964 [1]
10965 = 1.5707963267949;
10966 sj4array
10967 [1]
10968 = IKsin(
10969 j4array
10970 [1]);
10971 cj4array
10972 [1]
10973 = IKcos(
10974 j4array
10975 [1]);
10976 j4array
10977 [2]
10978 = 3.14159265358979;
10979 sj4array
10980 [2]
10981 = IKsin(
10982 j4array
10983 [2]);
10984 cj4array
10985 [2]
10986 = IKcos(
10987 j4array
10988 [2]);
10989 j4array
10990 [3]
10991 = -1.5707963267949;
10992 sj4array
10993 [3]
10994 = IKsin(
10995 j4array
10996 [3]);
10997 cj4array
10998 [3]
10999 = IKcos(
11000 j4array
11001 [3]);
11002 if (j4array
11003 [0]
11004 > IKPI)
11005 {
11006 j4array
11007 [0]
11008 -= IK2PI;
11009 }
11010 else if (
11011 j4array
11012 [0]
11013 < -IKPI)
11014 {
11015 j4array
11016 [0]
11017 += IK2PI;
11018 }
11019 j4valid
11020 [0]
11021 = true;
11022 if (j4array
11023 [1]
11024 > IKPI)
11025 {
11026 j4array
11027 [1]
11028 -= IK2PI;
11029 }
11030 else if (
11031 j4array
11032 [1]
11033 < -IKPI)
11034 {
11035 j4array
11036 [1]
11037 += IK2PI;
11038 }
11039 j4valid
11040 [1]
11041 = true;
11042 if (j4array
11043 [2]
11044 > IKPI)
11045 {
11046 j4array
11047 [2]
11048 -= IK2PI;
11049 }
11050 else if (
11051 j4array
11052 [2]
11053 < -IKPI)
11054 {
11055 j4array
11056 [2]
11057 += IK2PI;
11058 }
11059 j4valid
11060 [2]
11061 = true;
11062 if (j4array
11063 [3]
11064 > IKPI)
11065 {
11066 j4array
11067 [3]
11068 -= IK2PI;
11069 }
11070 else if (
11071 j4array
11072 [3]
11073 < -IKPI)
11074 {
11075 j4array
11076 [3]
11077 += IK2PI;
11078 }
11079 j4valid
11080 [3]
11081 = true;
11082 for (
11083 int ij4
11084 = 0;
11085 ij4
11086 < 4;
11087 ++ij4)
11088 {
11089 if (!j4valid
11090 [ij4])
11091 {
11092 continue;
11093 }
11094 _ij4[0]
11095 = ij4;
11096 _ij4[1]
11097 = -1;
11098 for (
11099 int iij4
11100 = ij4
11101 + 1;
11102 iij4
11103 < 4;
11104 ++iij4)
11105 {
11106 if (j4valid
11107 [iij4]
11108 && IKabs(
11109 cj4array
11110 [ij4]
11111 - cj4array
11112 [iij4])
11113 < IKFAST_SOLUTION_THRESH
11114 && IKabs(
11115 sj4array
11116 [ij4]
11117 - sj4array
11118 [iij4])
11119 < IKFAST_SOLUTION_THRESH)
11120 {
11121 j4valid
11122 [iij4]
11123 = false;
11124 _ij4[1]
11125 = iij4;
11126 break;
11127 }
11128 }
11129 j4 = j4array
11130 [ij4];
11131 cj4 = cj4array
11132 [ij4];
11133 sj4 = sj4array
11134 [ij4];
11135
11136 rotationfunction0(
11137 solutions);
11138 }
11139 }
11140 }
11141 } while (
11142 0);
11143 if (bgotonextstatement)
11144 {
11145 bool
11146 bgotonextstatement
11147 = true;
11148 do
11149 {
11150 if (1)
11151 {
11152 bgotonextstatement
11153 = false;
11154 continue; // branch miss [j4]
11155 }
11156 } while (
11157 0);
11158 if (bgotonextstatement)
11159 {
11160 }
11161 }
11162 }
11163 }
11164 else
11165 {
11166 {
11167 IkReal j4array
11168 [1],
11169 cj4array
11170 [1],
11171 sj4array
11172 [1];
11173 bool j4valid
11174 [1]
11175 = {false};
11176 _nj4
11177 = 1;
11178 IkReal
11179 x1133
11180 = (cj9
11181 * px);
11182 IkReal
11183 x1134
11184 = (cj9
11185 * py);
11186 IkReal
11187 x1135
11188 = (pp
11189 * px);
11190 IkReal
11191 x1136
11192 = (pp
11193 * py);
11194 CheckValue<IkReal> x1137 = IKatan2WithCheck(
11195 IkReal((
11196 (((4.31372549019608)
11197 * x1135))
11198 + (((-1.45555882352941)
11199 * x1133))
11200 + (((0.348408823529412)
11201 * py))
11202 + (((0.119091176470588)
11203 * x1134))
11204 + (((-1.0)
11205 * (1.66110784313725)
11206 * px))
11207 + (((0.647058823529412)
11208 * x1136)))),
11209 ((((0.348408823529412)
11210 * px))
11211 + (((0.119091176470588)
11212 * x1133))
11213 + (((1.66110784313725)
11214 * py))
11215 + (((1.45555882352941)
11216 * x1134))
11217 + (((-4.31372549019608)
11218 * x1136))
11219 + (((0.647058823529412)
11220 * x1135))),
11221 IKFAST_ATAN2_MAGTHRESH);
11222 if (!x1137
11223 .valid)
11224 {
11225 continue;
11226 }
11227 CheckValue<IkReal> x1138 = IKPowWithIntegerCheck(
11228 IKsign((
11229 (((1.1)
11230 * (pz
11231 * pz)))
11232 + (((-1.0)
11233 * (1.1)
11234 * pp)))),
11235 -1);
11236 if (!x1138
11237 .valid)
11238 {
11239 continue;
11240 }
11241 j4array
11242 [0]
11243 = ((-1.5707963267949)
11244 + (x1137
11245 .value)
11246 + (((1.5707963267949)
11247 * (x1138
11248 .value))));
11249 sj4array
11250 [0]
11251 = IKsin(
11252 j4array
11253 [0]);
11254 cj4array
11255 [0]
11256 = IKcos(
11257 j4array
11258 [0]);
11259 if (j4array
11260 [0]
11261 > IKPI)
11262 {
11263 j4array
11264 [0]
11265 -= IK2PI;
11266 }
11267 else if (
11268 j4array
11269 [0]
11270 < -IKPI)
11271 {
11272 j4array
11273 [0]
11274 += IK2PI;
11275 }
11276 j4valid
11277 [0]
11278 = true;
11279 for (
11280 int ij4
11281 = 0;
11282 ij4
11283 < 1;
11284 ++ij4)
11285 {
11286 if (!j4valid
11287 [ij4])
11288 {
11289 continue;
11290 }
11291 _ij4[0]
11292 = ij4;
11293 _ij4[1]
11294 = -1;
11295 for (
11296 int iij4
11297 = ij4
11298 + 1;
11299 iij4
11300 < 1;
11301 ++iij4)
11302 {
11303 if (j4valid
11304 [iij4]
11305 && IKabs(
11306 cj4array
11307 [ij4]
11308 - cj4array
11309 [iij4])
11310 < IKFAST_SOLUTION_THRESH
11311 && IKabs(
11312 sj4array
11313 [ij4]
11314 - sj4array
11315 [iij4])
11316 < IKFAST_SOLUTION_THRESH)
11317 {
11318 j4valid
11319 [iij4]
11320 = false;
11321 _ij4[1]
11322 = iij4;
11323 break;
11324 }
11325 }
11326 j4 = j4array
11327 [ij4];
11328 cj4 = cj4array
11329 [ij4];
11330 sj4 = sj4array
11331 [ij4];
11332 {
11333 IkReal evalcond
11334 [4];
11335 IkReal
11336 x1139
11337 = IKsin(
11338 j4);
11339 IkReal
11340 x1140
11341 = (px
11342 * x1139);
11343 IkReal
11344 x1141
11345 = IKcos(
11346 j4);
11347 IkReal
11348 x1142
11349 = (py
11350 * x1141);
11351 IkReal
11352 x1143
11353 = (px
11354 * x1141);
11355 IkReal
11356 x1144
11357 = (py
11358 * x1139);
11359 IkReal
11360 x1145
11361 = ((((-1.0)
11362 * x1144))
11363 + (((-1.0)
11364 * x1143)));
11365 evalcond
11366 [0]
11367 = ((-1.51009803921569)
11368 + (((-1.0)
11369 * (1.32323529411765)
11370 * cj9))
11371 + x1140
11372 + (((3.92156862745098)
11373 * pp))
11374 + (((-1.0)
11375 * x1142)));
11376 evalcond
11377 [1]
11378 = ((-0.55)
11379 + x1145
11380 + (((-1.0)
11381 * (0.3)
11382 * cj9))
11383 + (((-1.0)
11384 * (0.045)
11385 * sj9)));
11386 evalcond
11387 [2]
11388 = ((-0.316735294117647)
11389 + x1145
11390 + (((-1.0)
11391 * (0.588235294117647)
11392 * pp))
11393 + (((-1.0)
11394 * (0.108264705882353)
11395 * cj9)));
11396 evalcond
11397 [3]
11398 = ((-0.2125)
11399 + (((-0.09)
11400 * x1140))
11401 + (((-1.1)
11402 * x1144))
11403 + (((-1.1)
11404 * x1143))
11405 + (((0.09)
11406 * x1142))
11407 + (((-1.0)
11408 * (1.0)
11409 * pp)));
11410 if (IKabs(
11411 evalcond
11412 [0])
11413 > IKFAST_EVALCOND_THRESH
11414 || IKabs(
11415 evalcond
11416 [1])
11417 > IKFAST_EVALCOND_THRESH
11418 || IKabs(
11419 evalcond
11420 [2])
11421 > IKFAST_EVALCOND_THRESH
11422 || IKabs(
11423 evalcond
11424 [3])
11425 > IKFAST_EVALCOND_THRESH)
11426 {
11427 continue;
11428 }
11429 }
11430
11431 rotationfunction0(
11432 solutions);
11433 }
11434 }
11435 }
11436 }
11437 }
11438 else
11439 {
11440 {
11441 IkReal j4array
11442 [1],
11443 cj4array
11444 [1],
11445 sj4array
11446 [1];
11447 bool j4valid
11448 [1]
11449 = {false};
11450 _nj4 = 1;
11451 IkReal
11452 x1146
11453 = (cj9
11454 * px);
11455 IkReal
11456 x1147
11457 = (cj9
11458 * py);
11459 IkReal
11460 x1148
11461 = (pp
11462 * px);
11463 IkReal
11464 x1149
11465 = (pp
11466 * py);
11467 CheckValue<IkReal> x1150 = IKatan2WithCheck(
11468 IkReal((
11469 (((1.51009803921569)
11470 * px))
11471 + (((-1.0)
11472 * (0.316735294117647)
11473 * py))
11474 + (((-0.108264705882353)
11475 * x1147))
11476 + (((-0.588235294117647)
11477 * x1149))
11478 + (((1.32323529411765)
11479 * x1146))
11480 + (((-3.92156862745098)
11481 * x1148)))),
11482 ((((-0.108264705882353)
11483 * x1146))
11484 + (((-1.32323529411765)
11485 * x1147))
11486 + (((3.92156862745098)
11487 * x1149))
11488 + (((-1.0)
11489 * (1.51009803921569)
11490 * py))
11491 + (((-1.0)
11492 * (0.316735294117647)
11493 * px))
11494 + (((-0.588235294117647)
11495 * x1148))),
11496 IKFAST_ATAN2_MAGTHRESH);
11497 if (!x1150
11498 .valid)
11499 {
11500 continue;
11501 }
11502 CheckValue<IkReal> x1151 = IKPowWithIntegerCheck(
11503 IKsign((
11504 pp
11505 + (((-1.0)
11506 * (1.0)
11507 * (pz
11508 * pz))))),
11509 -1);
11510 if (!x1151
11511 .valid)
11512 {
11513 continue;
11514 }
11515 j4array[0]
11516 = ((-1.5707963267949)
11517 + (x1150
11518 .value)
11519 + (((1.5707963267949)
11520 * (x1151
11521 .value))));
11522 sj4array[0] = IKsin(
11523 j4array
11524 [0]);
11525 cj4array[0] = IKcos(
11526 j4array
11527 [0]);
11528 if (j4array
11529 [0]
11530 > IKPI)
11531 {
11532 j4array
11533 [0]
11534 -= IK2PI;
11535 }
11536 else if (
11537 j4array
11538 [0]
11539 < -IKPI)
11540 {
11541 j4array
11542 [0]
11543 += IK2PI;
11544 }
11545 j4valid[0]
11546 = true;
11547 for (
11548 int ij4
11549 = 0;
11550 ij4
11551 < 1;
11552 ++ij4)
11553 {
11554 if (!j4valid
11555 [ij4])
11556 {
11557 continue;
11558 }
11559 _ij4[0]
11560 = ij4;
11561 _ij4[1]
11562 = -1;
11563 for (
11564 int iij4
11565 = ij4
11566 + 1;
11567 iij4
11568 < 1;
11569 ++iij4)
11570 {
11571 if (j4valid
11572 [iij4]
11573 && IKabs(
11574 cj4array
11575 [ij4]
11576 - cj4array
11577 [iij4])
11578 < IKFAST_SOLUTION_THRESH
11579 && IKabs(
11580 sj4array
11581 [ij4]
11582 - sj4array
11583 [iij4])
11584 < IKFAST_SOLUTION_THRESH)
11585 {
11586 j4valid
11587 [iij4]
11588 = false;
11589 _ij4[1]
11590 = iij4;
11591 break;
11592 }
11593 }
11594 j4 = j4array
11595 [ij4];
11596 cj4 = cj4array
11597 [ij4];
11598 sj4 = sj4array
11599 [ij4];
11600 {
11601 IkReal evalcond
11602 [4];
11603 IkReal
11604 x1152
11605 = IKsin(
11606 j4);
11607 IkReal
11608 x1153
11609 = (px
11610 * x1152);
11611 IkReal
11612 x1154
11613 = IKcos(
11614 j4);
11615 IkReal
11616 x1155
11617 = (py
11618 * x1154);
11619 IkReal
11620 x1156
11621 = (px
11622 * x1154);
11623 IkReal
11624 x1157
11625 = (py
11626 * x1152);
11627 IkReal
11628 x1158
11629 = ((((-1.0)
11630 * x1156))
11631 + (((-1.0)
11632 * x1157)));
11633 evalcond
11634 [0]
11635 = ((-1.51009803921569)
11636 + (((-1.0)
11637 * x1155))
11638 + x1153
11639 + (((-1.0)
11640 * (1.32323529411765)
11641 * cj9))
11642 + (((3.92156862745098)
11643 * pp)));
11644 evalcond
11645 [1]
11646 = ((-0.55)
11647 + x1158
11648 + (((-1.0)
11649 * (0.3)
11650 * cj9))
11651 + (((-1.0)
11652 * (0.045)
11653 * sj9)));
11654 evalcond
11655 [2]
11656 = ((-0.316735294117647)
11657 + x1158
11658 + (((-1.0)
11659 * (0.588235294117647)
11660 * pp))
11661 + (((-1.0)
11662 * (0.108264705882353)
11663 * cj9)));
11664 evalcond
11665 [3]
11666 = ((-0.2125)
11667 + (((-1.1)
11668 * x1156))
11669 + (((-1.1)
11670 * x1157))
11671 + (((-0.09)
11672 * x1153))
11673 + (((-1.0)
11674 * (1.0)
11675 * pp))
11676 + (((0.09)
11677 * x1155)));
11678 if (IKabs(
11679 evalcond
11680 [0])
11681 > IKFAST_EVALCOND_THRESH
11682 || IKabs(
11683 evalcond
11684 [1])
11685 > IKFAST_EVALCOND_THRESH
11686 || IKabs(
11687 evalcond
11688 [2])
11689 > IKFAST_EVALCOND_THRESH
11690 || IKabs(
11691 evalcond
11692 [3])
11693 > IKFAST_EVALCOND_THRESH)
11694 {
11695 continue;
11696 }
11697 }
11698
11699 rotationfunction0(
11700 solutions);
11701 }
11702 }
11703 }
11704 }
11705 }
11706 else
11707 {
11708 {
11709 IkReal j4array
11710 [1],
11711 cj4array
11712 [1],
11713 sj4array
11714 [1];
11715 bool
11716 j4valid[1]
11717 = {false};
11718 _nj4 = 1;
11719 IkReal x1159
11720 = (cj9
11721 * px);
11722 IkReal x1160
11723 = (cj9
11724 * py);
11725 IkReal x1161
11726 = ((3.92156862745098)
11727 * pp);
11728 IkReal x1162
11729 = ((0.045)
11730 * sj9);
11731 CheckValue<IkReal> x1163 = IKPowWithIntegerCheck(
11732 IKsign((
11733 pp
11734 + (((-1.0)
11735 * (1.0)
11736 * (pz
11737 * pz))))),
11738 -1);
11739 if (!x1163
11740 .valid)
11741 {
11742 continue;
11743 }
11744 CheckValue<IkReal> x1164 = IKatan2WithCheck(
11745 IkReal((
11746 (((-0.3)
11747 * x1160))
11748 + (((1.51009803921569)
11749 * px))
11750 + (((-1.0)
11751 * py
11752 * x1162))
11753 + (((-1.0)
11754 * px
11755 * x1161))
11756 + (((1.32323529411765)
11757 * x1159))
11758 + (((-1.0)
11759 * (0.55)
11760 * py)))),
11761 ((((-1.0)
11762 * (0.55)
11763 * px))
11764 + (((-1.32323529411765)
11765 * x1160))
11766 + (((-1.0)
11767 * (1.51009803921569)
11768 * py))
11769 + (((-0.3)
11770 * x1159))
11771 + ((py
11772 * x1161))
11773 + (((-1.0)
11774 * px
11775 * x1162))),
11776 IKFAST_ATAN2_MAGTHRESH);
11777 if (!x1164
11778 .valid)
11779 {
11780 continue;
11781 }
11782 j4array[0]
11783 = ((-1.5707963267949)
11784 + (((1.5707963267949)
11785 * (x1163
11786 .value)))
11787 + (x1164
11788 .value));
11789 sj4array[0]
11790 = IKsin(
11791 j4array
11792 [0]);
11793 cj4array[0]
11794 = IKcos(
11795 j4array
11796 [0]);
11797 if (j4array[0]
11798 > IKPI)
11799 {
11800 j4array[0]
11801 -= IK2PI;
11802 }
11803 else if (
11804 j4array[0]
11805 < -IKPI)
11806 {
11807 j4array[0]
11808 += IK2PI;
11809 }
11810 j4valid[0]
11811 = true;
11812 for (int ij4
11813 = 0;
11814 ij4 < 1;
11815 ++ij4)
11816 {
11817 if (!j4valid
11818 [ij4])
11819 {
11820 continue;
11821 }
11822 _ij4[0]
11823 = ij4;
11824 _ij4[1]
11825 = -1;
11826 for (
11827 int iij4
11828 = ij4
11829 + 1;
11830 iij4
11831 < 1;
11832 ++iij4)
11833 {
11834 if (j4valid
11835 [iij4]
11836 && IKabs(
11837 cj4array
11838 [ij4]
11839 - cj4array
11840 [iij4])
11841 < IKFAST_SOLUTION_THRESH
11842 && IKabs(
11843 sj4array
11844 [ij4]
11845 - sj4array
11846 [iij4])
11847 < IKFAST_SOLUTION_THRESH)
11848 {
11849 j4valid
11850 [iij4]
11851 = false;
11852 _ij4[1]
11853 = iij4;
11854 break;
11855 }
11856 }
11857 j4 = j4array
11858 [ij4];
11859 cj4 = cj4array
11860 [ij4];
11861 sj4 = sj4array
11862 [ij4];
11863 {
11864 IkReal evalcond
11865 [4];
11866 IkReal
11867 x1165
11868 = IKsin(
11869 j4);
11870 IkReal
11871 x1166
11872 = (px
11873 * x1165);
11874 IkReal
11875 x1167
11876 = IKcos(
11877 j4);
11878 IkReal
11879 x1168
11880 = (py
11881 * x1167);
11882 IkReal
11883 x1169
11884 = (px
11885 * x1167);
11886 IkReal
11887 x1170
11888 = (py
11889 * x1165);
11890 IkReal
11891 x1171
11892 = ((((-1.0)
11893 * x1169))
11894 + (((-1.0)
11895 * x1170)));
11896 evalcond
11897 [0]
11898 = ((-1.51009803921569)
11899 + (((-1.0)
11900 * (1.32323529411765)
11901 * cj9))
11902 + (((-1.0)
11903 * x1168))
11904 + (((3.92156862745098)
11905 * pp))
11906 + x1166);
11907 evalcond
11908 [1]
11909 = ((-0.55)
11910 + (((-1.0)
11911 * (0.3)
11912 * cj9))
11913 + x1171
11914 + (((-1.0)
11915 * (0.045)
11916 * sj9)));
11917 evalcond
11918 [2]
11919 = ((-0.316735294117647)
11920 + (((-1.0)
11921 * (0.588235294117647)
11922 * pp))
11923 + (((-1.0)
11924 * (0.108264705882353)
11925 * cj9))
11926 + x1171);
11927 evalcond
11928 [3]
11929 = ((-0.2125)
11930 + (((-1.1)
11931 * x1170))
11932 + (((-1.1)
11933 * x1169))
11934 + (((0.09)
11935 * x1168))
11936 + (((-0.09)
11937 * x1166))
11938 + (((-1.0)
11939 * (1.0)
11940 * pp)));
11941 if (IKabs(
11942 evalcond
11943 [0])
11944 > IKFAST_EVALCOND_THRESH
11945 || IKabs(
11946 evalcond
11947 [1])
11948 > IKFAST_EVALCOND_THRESH
11949 || IKabs(
11950 evalcond
11951 [2])
11952 > IKFAST_EVALCOND_THRESH
11953 || IKabs(
11954 evalcond
11955 [3])
11956 > IKFAST_EVALCOND_THRESH)
11957 {
11958 continue;
11959 }
11960 }
11961
11962 rotationfunction0(
11963 solutions);
11964 }
11965 }
11966 }
11967 }
11968 }
11969 } while (0);
11970 if (bgotonextstatement)
11971 {
11972 bool
11973 bgotonextstatement
11974 = true;
11975 do
11976 {
11977 IkReal x1172
11978 = (cj6 * pz);
11979 IkReal x1173
11980 = ((-1.51009803921569)
11981 + (((-1.0)
11982 * (1.32323529411765)
11983 * cj9))
11984 + (((3.92156862745098)
11985 * pp)));
11986 evalcond[0]
11987 = ((IKabs(py))
11988 + (IKabs(
11989 px)));
11990 evalcond[1]
11991 = ((-0.55)
11992 + (((-1.0)
11993 * (0.3)
11994 * cj9))
11995 + x1172
11996 + (((-1.0)
11997 * (0.045)
11998 * sj9)));
11999 evalcond[2] = x1173;
12000 evalcond[3]
12001 = ((-1.0)
12002 * (((1.0)
12003 * pz
12004 * sj6)));
12005 evalcond[4] = x1173;
12006 evalcond[5]
12007 = ((((0.108264705882353)
12008 * cj9
12009 * sj6))
12010 + (((0.588235294117647)
12011 * pp
12012 * sj6))
12013 + (((0.316735294117647)
12014 * sj6)));
12015 evalcond[6]
12016 = ((-0.2125)
12017 + (((1.1)
12018 * x1172))
12019 + (((-1.0)
12020 * (1.0)
12021 * pp)));
12022 if (IKabs(
12023 evalcond[0])
12024 < 0.0000010000000000
12025 && IKabs(
12026 evalcond
12027 [1])
12028 < 0.0000010000000000
12029 && IKabs(
12030 evalcond
12031 [2])
12032 < 0.0000010000000000
12033 && IKabs(
12034 evalcond
12035 [3])
12036 < 0.0000010000000000
12037 && IKabs(
12038 evalcond
12039 [4])
12040 < 0.0000010000000000
12041 && IKabs(
12042 evalcond
12043 [5])
12044 < 0.0000010000000000
12045 && IKabs(
12046 evalcond
12047 [6])
12048 < 0.0000010000000000)
12049 {
12050 bgotonextstatement
12051 = false;
12052 {
12053 IkReal
12054 j4array[4],
12055 cj4array[4],
12056 sj4array[4];
12057 bool j4valid[4]
12058 = {false};
12059 _nj4 = 4;
12060 j4array[0] = 0;
12061 sj4array[0]
12062 = IKsin(
12063 j4array
12064 [0]);
12065 cj4array[0]
12066 = IKcos(
12067 j4array
12068 [0]);
12069 j4array[1]
12070 = 1.5707963267949;
12071 sj4array[1]
12072 = IKsin(
12073 j4array
12074 [1]);
12075 cj4array[1]
12076 = IKcos(
12077 j4array
12078 [1]);
12079 j4array[2]
12080 = 3.14159265358979;
12081 sj4array[2]
12082 = IKsin(
12083 j4array
12084 [2]);
12085 cj4array[2]
12086 = IKcos(
12087 j4array
12088 [2]);
12089 j4array[3]
12090 = -1.5707963267949;
12091 sj4array[3]
12092 = IKsin(
12093 j4array
12094 [3]);
12095 cj4array[3]
12096 = IKcos(
12097 j4array
12098 [3]);
12099 if (j4array[0]
12100 > IKPI)
12101 {
12102 j4array[0]
12103 -= IK2PI;
12104 }
12105 else if (
12106 j4array[0]
12107 < -IKPI)
12108 {
12109 j4array[0]
12110 += IK2PI;
12111 }
12112 j4valid[0]
12113 = true;
12114 if (j4array[1]
12115 > IKPI)
12116 {
12117 j4array[1]
12118 -= IK2PI;
12119 }
12120 else if (
12121 j4array[1]
12122 < -IKPI)
12123 {
12124 j4array[1]
12125 += IK2PI;
12126 }
12127 j4valid[1]
12128 = true;
12129 if (j4array[2]
12130 > IKPI)
12131 {
12132 j4array[2]
12133 -= IK2PI;
12134 }
12135 else if (
12136 j4array[2]
12137 < -IKPI)
12138 {
12139 j4array[2]
12140 += IK2PI;
12141 }
12142 j4valid[2]
12143 = true;
12144 if (j4array[3]
12145 > IKPI)
12146 {
12147 j4array[3]
12148 -= IK2PI;
12149 }
12150 else if (
12151 j4array[3]
12152 < -IKPI)
12153 {
12154 j4array[3]
12155 += IK2PI;
12156 }
12157 j4valid[3]
12158 = true;
12159 for (int ij4
12160 = 0;
12161 ij4 < 4;
12162 ++ij4)
12163 {
12164 if (!j4valid
12165 [ij4])
12166 {
12167 continue;
12168 }
12169 _ij4[0] = ij4;
12170 _ij4[1] = -1;
12171 for (int iij4
12172 = ij4
12173 + 1;
12174 iij4 < 4;
12175 ++iij4)
12176 {
12177 if (j4valid
12178 [iij4]
12179 && IKabs(
12180 cj4array
12181 [ij4]
12182 - cj4array
12183 [iij4])
12184 < IKFAST_SOLUTION_THRESH
12185 && IKabs(
12186 sj4array
12187 [ij4]
12188 - sj4array
12189 [iij4])
12190 < IKFAST_SOLUTION_THRESH)
12191 {
12192 j4valid
12193 [iij4]
12194 = false;
12195 _ij4[1]
12196 = iij4;
12197 break;
12198 }
12199 }
12200 j4 = j4array
12201 [ij4];
12202 cj4 = cj4array
12203 [ij4];
12204 sj4 = sj4array
12205 [ij4];
12206
12207 rotationfunction0(
12208 solutions);
12209 }
12210 }
12211 }
12212 } while (0);
12213 if (bgotonextstatement)
12214 {
12215 bool
12216 bgotonextstatement
12217 = true;
12218 do
12219 {
12220 if (1)
12221 {
12222 bgotonextstatement
12223 = false;
12224 continue; // branch
12225 // miss
12226 // [j4]
12227 }
12228 } while (0);
12229 if (bgotonextstatement)
12230 {
12231 }
12232 }
12233 }
12234 }
12235 }
12236 }
12237 else
12238 {
12239 {
12240 IkReal j4array[1],
12241 cj4array[1],
12242 sj4array[1];
12243 bool j4valid[1] = {false};
12244 _nj4 = 1;
12245 IkReal x1174
12246 = ((1.32323529411765)
12247 * cj9);
12248 IkReal x1175
12249 = ((3.92156862745098)
12250 * pp);
12251 IkReal x1176
12252 = ((0.316735294117647)
12253 * sj6);
12254 IkReal x1177
12255 = ((0.108264705882353)
12256 * cj9 * sj6);
12257 IkReal x1178
12258 = ((0.588235294117647)
12259 * pp * sj6);
12260 CheckValue<IkReal> x1179
12261 = IKatan2WithCheck(
12262 IkReal((
12263 (((-1.0) * px
12264 * x1175))
12265 + ((px
12266 * x1174))
12267 + (((1.51009803921569)
12268 * px))
12269 + ((py
12270 * x1178))
12271 + ((py
12272 * x1177))
12273 + ((py
12274 * x1176)))),
12275 (((px * x1176))
12276 + (((-1.0)
12277 * (1.51009803921569)
12278 * py))
12279 + ((px * x1178))
12280 + ((py * x1175))
12281 + (((-1.0) * py
12282 * x1174))
12283 + ((px
12284 * x1177))),
12285 IKFAST_ATAN2_MAGTHRESH);
12286 if (!x1179.valid)
12287 {
12288 continue;
12289 }
12290 CheckValue<IkReal> x1180
12291 = IKPowWithIntegerCheck(
12292 IKsign((
12293 pp
12294 + (((-1.0)
12295 * (1.0)
12296 * (pz
12297 * pz))))),
12298 -1);
12299 if (!x1180.valid)
12300 {
12301 continue;
12302 }
12303 j4array[0]
12304 = ((-1.5707963267949)
12305 + (x1179.value)
12306 + (((1.5707963267949)
12307 * (x1180
12308 .value))));
12309 sj4array[0]
12310 = IKsin(j4array[0]);
12311 cj4array[0]
12312 = IKcos(j4array[0]);
12313 if (j4array[0] > IKPI)
12314 {
12315 j4array[0] -= IK2PI;
12316 }
12317 else if (
12318 j4array[0] < -IKPI)
12319 {
12320 j4array[0] += IK2PI;
12321 }
12322 j4valid[0] = true;
12323 for (int ij4 = 0; ij4 < 1;
12324 ++ij4)
12325 {
12326 if (!j4valid[ij4])
12327 {
12328 continue;
12329 }
12330 _ij4[0] = ij4;
12331 _ij4[1] = -1;
12332 for (int iij4 = ij4 + 1;
12333 iij4 < 1;
12334 ++iij4)
12335 {
12336 if (j4valid[iij4]
12337 && IKabs(
12338 cj4array
12339 [ij4]
12340 - cj4array
12341 [iij4])
12342 < IKFAST_SOLUTION_THRESH
12343 && IKabs(
12344 sj4array
12345 [ij4]
12346 - sj4array
12347 [iij4])
12348 < IKFAST_SOLUTION_THRESH)
12349 {
12350 j4valid[iij4]
12351 = false;
12352 _ij4[1] = iij4;
12353 break;
12354 }
12355 }
12356 j4 = j4array[ij4];
12357 cj4 = cj4array[ij4];
12358 sj4 = sj4array[ij4];
12359 {
12360 IkReal evalcond[5];
12361 IkReal x1181
12362 = IKcos(j4);
12363 IkReal x1182
12364 = (px * x1181);
12365 IkReal x1183
12366 = IKsin(j4);
12367 IkReal x1184
12368 = (py * x1183);
12369 IkReal x1185
12370 = (px * x1183);
12371 IkReal x1186
12372 = ((1.0) * x1181);
12373 IkReal x1187
12374 = (cj6 * pz);
12375 IkReal x1188
12376 = (sj6 * x1182);
12377 IkReal x1189
12378 = (sj6 * x1184);
12379 evalcond[0]
12380 = (((cj6 * x1184))
12381 + ((cj6
12382 * x1182))
12383 + (((-1.0)
12384 * (1.0)
12385 * pz
12386 * sj6)));
12387 evalcond[1]
12388 = ((-1.51009803921569)
12389 + (((-1.0) * py
12390 * x1186))
12391 + (((-1.0)
12392 * (1.32323529411765)
12393 * cj9))
12394 + (((3.92156862745098)
12395 * pp))
12396 + x1185);
12397 evalcond[2]
12398 = ((-0.55)
12399 + (((-1.0)
12400 * (0.3)
12401 * cj9))
12402 + x1187 + x1188
12403 + x1189
12404 + (((-1.0)
12405 * (0.045)
12406 * sj9)));
12407 evalcond[3]
12408 = ((((0.108264705882353)
12409 * cj9 * sj6))
12410 + (((-1.0)
12411 * x1184))
12412 + (((-1.0) * px
12413 * x1186))
12414 + (((0.588235294117647)
12415 * pp
12416 * sj6))
12417 + (((0.316735294117647)
12418 * sj6)));
12419 evalcond[4]
12420 = ((-0.2125)
12421 + (((1.1)
12422 * x1188))
12423 + (((-0.09)
12424 * x1185))
12425 + (((0.09) * py
12426 * x1181))
12427 + (((-1.0)
12428 * (1.0)
12429 * pp))
12430 + (((1.1)
12431 * x1187))
12432 + (((1.1)
12433 * x1189)));
12434 if (IKabs(evalcond[0])
12435 > IKFAST_EVALCOND_THRESH
12436 || IKabs(evalcond
12437 [1])
12438 > IKFAST_EVALCOND_THRESH
12439 || IKabs(evalcond
12440 [2])
12441 > IKFAST_EVALCOND_THRESH
12442 || IKabs(evalcond
12443 [3])
12444 > IKFAST_EVALCOND_THRESH
12445 || IKabs(evalcond
12446 [4])
12447 > IKFAST_EVALCOND_THRESH)
12448 {
12449 continue;
12450 }
12451 }
12452
12453 rotationfunction0(
12454 solutions);
12455 }
12456 }
12457 }
12458 }
12459 }
12460 else
12461 {
12462 {
12463 IkReal j4array[1],
12464 cj4array[1], sj4array[1];
12465 bool j4valid[1] = {false};
12466 _nj4 = 1;
12467 IkReal x1190 = (cj6 * pp);
12468 IkReal x1191
12469 = ((0.2125) * cj6);
12470 IkReal x1192 = ((1.1) * pz);
12471 IkReal x1193
12472 = ((0.09) * pz * sj6);
12473 CheckValue<IkReal> x1194
12474 = IKatan2WithCheck(
12475 IkReal(
12476 (((px * x1192))
12477 + (((-1.0) * px
12478 * x1190))
12479 + ((py * x1193))
12480 + (((-1.0) * px
12481 * x1191)))),
12482 (((py * x1190))
12483 + (((-1.0) * py
12484 * x1192))
12485 + ((py * x1191))
12486 + ((px * x1193))),
12487 IKFAST_ATAN2_MAGTHRESH);
12488 if (!x1194.valid)
12489 {
12490 continue;
12491 }
12492 CheckValue<IkReal> x1195
12493 = IKPowWithIntegerCheck(
12494 IKsign((
12495 (((0.09) * x1190))
12496 + (((-1.0)
12497 * (0.09) * cj6
12498 * (pz
12499 * pz))))),
12500 -1);
12501 if (!x1195.valid)
12502 {
12503 continue;
12504 }
12505 j4array[0]
12506 = ((-1.5707963267949)
12507 + (x1194.value)
12508 + (((1.5707963267949)
12509 * (x1195.value))));
12510 sj4array[0]
12511 = IKsin(j4array[0]);
12512 cj4array[0]
12513 = IKcos(j4array[0]);
12514 if (j4array[0] > IKPI)
12515 {
12516 j4array[0] -= IK2PI;
12517 }
12518 else if (j4array[0] < -IKPI)
12519 {
12520 j4array[0] += IK2PI;
12521 }
12522 j4valid[0] = true;
12523 for (int ij4 = 0; ij4 < 1;
12524 ++ij4)
12525 {
12526 if (!j4valid[ij4])
12527 {
12528 continue;
12529 }
12530 _ij4[0] = ij4;
12531 _ij4[1] = -1;
12532 for (int iij4 = ij4 + 1;
12533 iij4 < 1;
12534 ++iij4)
12535 {
12536 if (j4valid[iij4]
12537 && IKabs(
12538 cj4array[ij4]
12539 - cj4array
12540 [iij4])
12541 < IKFAST_SOLUTION_THRESH
12542 && IKabs(
12543 sj4array[ij4]
12544 - sj4array
12545 [iij4])
12546 < IKFAST_SOLUTION_THRESH)
12547 {
12548 j4valid[iij4] = false;
12549 _ij4[1] = iij4;
12550 break;
12551 }
12552 }
12553 j4 = j4array[ij4];
12554 cj4 = cj4array[ij4];
12555 sj4 = sj4array[ij4];
12556 {
12557 IkReal evalcond[5];
12558 IkReal x1196 = IKcos(j4);
12559 IkReal x1197
12560 = (px * x1196);
12561 IkReal x1198 = IKsin(j4);
12562 IkReal x1199
12563 = (py * x1198);
12564 IkReal x1200
12565 = (px * x1198);
12566 IkReal x1201
12567 = ((1.0) * x1196);
12568 IkReal x1202 = (cj6 * pz);
12569 IkReal x1203
12570 = (sj6 * x1197);
12571 IkReal x1204
12572 = (sj6 * x1199);
12573 evalcond[0]
12574 = (((cj6 * x1199))
12575 + ((cj6 * x1197))
12576 + (((-1.0) * (1.0)
12577 * pz * sj6)));
12578 evalcond[1]
12579 = ((-1.51009803921569)
12580 + x1200
12581 + (((-1.0)
12582 * (1.32323529411765)
12583 * cj9))
12584 + (((-1.0) * py
12585 * x1201))
12586 + (((3.92156862745098)
12587 * pp)));
12588 evalcond[2]
12589 = ((-0.55) + x1202
12590 + x1203 + x1204
12591 + (((-1.0) * (0.3)
12592 * cj9))
12593 + (((-1.0)
12594 * (0.045)
12595 * sj9)));
12596 evalcond[3]
12597 = ((((0.108264705882353)
12598 * cj9 * sj6))
12599 + (((0.588235294117647)
12600 * pp * sj6))
12601 + (((-1.0) * px
12602 * x1201))
12603 + (((0.316735294117647)
12604 * sj6))
12605 + (((-1.0)
12606 * x1199)));
12607 evalcond[4]
12608 = ((-0.2125)
12609 + (((1.1) * x1204))
12610 + (((1.1) * x1203))
12611 + (((-0.09)
12612 * x1200))
12613 + (((0.09) * py
12614 * x1196))
12615 + (((1.1) * x1202))
12616 + (((-1.0) * (1.0)
12617 * pp)));
12618 if (IKabs(evalcond[0])
12619 > IKFAST_EVALCOND_THRESH
12620 || IKabs(evalcond[1])
12621 > IKFAST_EVALCOND_THRESH
12622 || IKabs(evalcond[2])
12623 > IKFAST_EVALCOND_THRESH
12624 || IKabs(evalcond[3])
12625 > IKFAST_EVALCOND_THRESH
12626 || IKabs(evalcond[4])
12627 > IKFAST_EVALCOND_THRESH)
12628 {
12629 continue;
12630 }
12631 }
12632
12633 rotationfunction0(
12634 solutions);
12635 }
12636 }
12637 }
12638 }
12639 }
12640 else
12641 {
12642 {
12643 IkReal j4array[1], cj4array[1],
12644 sj4array[1];
12645 bool j4valid[1] = {false};
12646 _nj4 = 1;
12647 IkReal x1205
12648 = ((1.51009803921569) * cj6);
12649 IkReal x1206 = (pz * sj6);
12650 IkReal x1207
12651 = ((1.32323529411765) * cj6
12652 * cj9);
12653 IkReal x1208
12654 = ((3.92156862745098) * cj6
12655 * pp);
12656 CheckValue<IkReal> x1209
12657 = IKatan2WithCheck(
12658 IkReal(
12659 (((px * x1205))
12660 + ((px * x1207))
12661 + ((py * x1206))
12662 + (((-1.0) * px
12663 * x1208)))),
12664 ((((-1.0) * py * x1205))
12665 + ((py * x1208))
12666 + ((px * x1206))
12667 + (((-1.0) * py
12668 * x1207))),
12669 IKFAST_ATAN2_MAGTHRESH);
12670 if (!x1209.valid)
12671 {
12672 continue;
12673 }
12674 CheckValue<IkReal> x1210
12675 = IKPowWithIntegerCheck(
12676 IKsign(
12677 ((((-1.0) * (1.0)
12678 * cj6 * (pz * pz)))
12679 + ((cj6 * pp)))),
12680 -1);
12681 if (!x1210.valid)
12682 {
12683 continue;
12684 }
12685 j4array[0]
12686 = ((-1.5707963267949)
12687 + (x1209.value)
12688 + (((1.5707963267949)
12689 * (x1210.value))));
12690 sj4array[0] = IKsin(j4array[0]);
12691 cj4array[0] = IKcos(j4array[0]);
12692 if (j4array[0] > IKPI)
12693 {
12694 j4array[0] -= IK2PI;
12695 }
12696 else if (j4array[0] < -IKPI)
12697 {
12698 j4array[0] += IK2PI;
12699 }
12700 j4valid[0] = true;
12701 for (int ij4 = 0; ij4 < 1; ++ij4)
12702 {
12703 if (!j4valid[ij4])
12704 {
12705 continue;
12706 }
12707 _ij4[0] = ij4;
12708 _ij4[1] = -1;
12709 for (int iij4 = ij4 + 1;
12710 iij4 < 1;
12711 ++iij4)
12712 {
12713 if (j4valid[iij4]
12714 && IKabs(
12715 cj4array[ij4]
12716 - cj4array[iij4])
12717 < IKFAST_SOLUTION_THRESH
12718 && IKabs(
12719 sj4array[ij4]
12720 - sj4array[iij4])
12721 < IKFAST_SOLUTION_THRESH)
12722 {
12723 j4valid[iij4] = false;
12724 _ij4[1] = iij4;
12725 break;
12726 }
12727 }
12728 j4 = j4array[ij4];
12729 cj4 = cj4array[ij4];
12730 sj4 = sj4array[ij4];
12731 {
12732 IkReal evalcond[5];
12733 IkReal x1211 = IKcos(j4);
12734 IkReal x1212 = (px * x1211);
12735 IkReal x1213 = IKsin(j4);
12736 IkReal x1214 = (py * x1213);
12737 IkReal x1215 = (px * x1213);
12738 IkReal x1216
12739 = ((1.0) * x1211);
12740 IkReal x1217 = (cj6 * pz);
12741 IkReal x1218 = (sj6 * x1212);
12742 IkReal x1219 = (sj6 * x1214);
12743 evalcond[0]
12744 = (((cj6 * x1212))
12745 + ((cj6 * x1214))
12746 + (((-1.0) * (1.0) * pz
12747 * sj6)));
12748 evalcond[1]
12749 = ((-1.51009803921569)
12750 + (((-1.0)
12751 * (1.32323529411765)
12752 * cj9))
12753 + (((3.92156862745098)
12754 * pp))
12755 + x1215
12756 + (((-1.0) * py
12757 * x1216)));
12758 evalcond[2]
12759 = ((-0.55)
12760 + (((-1.0) * (0.3)
12761 * cj9))
12762 + x1218 + x1219 + x1217
12763 + (((-1.0) * (0.045)
12764 * sj9)));
12765 evalcond[3]
12766 = ((((0.108264705882353)
12767 * cj9 * sj6))
12768 + (((-1.0) * x1214))
12769 + (((0.588235294117647)
12770 * pp * sj6))
12771 + (((-1.0) * px
12772 * x1216))
12773 + (((0.316735294117647)
12774 * sj6)));
12775 evalcond[4]
12776 = ((-0.2125)
12777 + (((1.1) * x1217))
12778 + (((1.1) * x1218))
12779 + (((-0.09) * x1215))
12780 + (((1.1) * x1219))
12781 + (((-1.0) * (1.0)
12782 * pp))
12783 + (((0.09) * py
12784 * x1211)));
12785 if (IKabs(evalcond[0])
12786 > IKFAST_EVALCOND_THRESH
12787 || IKabs(evalcond[1])
12788 > IKFAST_EVALCOND_THRESH
12789 || IKabs(evalcond[2])
12790 > IKFAST_EVALCOND_THRESH
12791 || IKabs(evalcond[3])
12792 > IKFAST_EVALCOND_THRESH
12793 || IKabs(evalcond[4])
12794 > IKFAST_EVALCOND_THRESH)
12795 {
12796 continue;
12797 }
12798 }
12799
12800 rotationfunction0(solutions);
12801 }
12802 }
12803 }
12804 }
12805 }
12806 } while (0);
12807 if (bgotonextstatement)
12808 {
12809 bool bgotonextstatement = true;
12810 do
12811 {
12812 evalcond[0]
12813 = ((-3.14159265358979)
12814 + (IKfmod(
12815 ((3.14159265358979)
12816 + (IKabs(
12817 ((1.5707963267949)
12818 + j8)))),
12819 6.28318530717959)));
12820 evalcond[1]
12821 = ((0.39655) + (((0.0765) * sj9))
12822 + (((0.32595) * cj9))
12823 + (((-1.0) * (1.0) * pp)));
12824 evalcond[2]
12825 = ((((-1.0) * (0.3) * cj6 * cj9))
12826 + (((-1.0) * (0.55) * cj6)) + pz
12827 + (((-1.0) * (0.045) * cj6
12828 * sj9)));
12829 if (IKabs(evalcond[0])
12830 < 0.0000010000000000
12831 && IKabs(evalcond[1])
12832 < 0.0000010000000000
12833 && IKabs(evalcond[2])
12834 < 0.0000010000000000)
12835 {
12836 bgotonextstatement = false;
12837 {
12838 IkReal j4eval[3];
12839 sj8 = -1.0;
12840 cj8 = 0;
12841 j8 = -1.5707963267949;
12842 IkReal x1220
12843 = ((((-1.0) * (1.0) * cj6
12844 * (pz * pz)))
12845 + ((cj6 * pp)));
12846 IkReal x1221
12847 = ((1.51009803921569) * cj6);
12848 IkReal x1222 = (pz * sj6);
12849 IkReal x1223
12850 = ((1.32323529411765) * cj6
12851 * cj9);
12852 IkReal x1224
12853 = ((3.92156862745098) * cj6
12854 * pp);
12855 j4eval[0] = x1220;
12856 j4eval[1]
12857 = ((IKabs(
12858 (((py * x1221))
12859 + ((py * x1223))
12860 + ((px * x1222))
12861 + (((-1.0) * py
12862 * x1224)))))
12863 + (IKabs((
12864 (((-1.0) * px * x1221))
12865 + ((py * x1222))
12866 + (((-1.0) * px
12867 * x1223))
12868 + ((px * x1224))))));
12869 j4eval[2] = IKsign(x1220);
12870 if (IKabs(j4eval[0])
12871 < 0.0000010000000000
12872 || IKabs(j4eval[1])
12873 < 0.0000010000000000
12874 || IKabs(j4eval[2])
12875 < 0.0000010000000000)
12876 {
12877 {
12878 IkReal j4eval[3];
12879 sj8 = -1.0;
12880 cj8 = 0;
12881 j8 = -1.5707963267949;
12882 IkReal x1225 = (cj6 * pp);
12883 IkReal x1226 = ((1.0) * x1225);
12884 IkReal x1227
12885 = (cj6 * (pz * pz));
12886 IkReal x1228 = ((0.2125) * cj6);
12887 IkReal x1229 = ((1.1) * pz);
12888 IkReal x1230
12889 = ((0.09) * pz * sj6);
12890 j4eval[0]
12891 = ((((-1.0) * x1226))
12892 + x1227);
12893 j4eval[1] = IKsign(
12894 ((((-0.09) * x1225))
12895 + (((0.09) * x1227))));
12896 j4eval[2]
12897 = ((IKabs(
12898 (((py * x1228))
12899 + ((py * x1225))
12900 + (((-1.0) * py
12901 * x1229))
12902 + (((-1.0) * px
12903 * x1230)))))
12904 + (IKabs(
12905 ((((-1.0) * px
12906 * x1228))
12907 + (((-1.0) * px
12908 * x1226))
12909 + ((px * x1229))
12910 + (((-1.0) * py
12911 * x1230))))));
12912 if (IKabs(j4eval[0])
12913 < 0.0000010000000000
12914 || IKabs(j4eval[1])
12915 < 0.0000010000000000
12916 || IKabs(j4eval[2])
12917 < 0.0000010000000000)
12918 {
12919 {
12920 IkReal j4eval[3];
12921 sj8 = -1.0;
12922 cj8 = 0;
12923 j8 = -1.5707963267949;
12924 IkReal x1231
12925 = (pp
12926 + (((-1.0) * (1.0)
12927 * (pz * pz))));
12928 IkReal x1232
12929 = ((1.32323529411765)
12930 * cj9);
12931 IkReal x1233
12932 = ((3.92156862745098)
12933 * pp);
12934 IkReal x1234
12935 = ((0.316735294117647)
12936 * sj6);
12937 IkReal x1235
12938 = ((0.108264705882353)
12939 * cj9 * sj6);
12940 IkReal x1236
12941 = ((0.588235294117647)
12942 * pp * sj6);
12943 j4eval[0] = x1231;
12944 j4eval[1]
12945 = ((IKabs((
12946 ((px * x1233))
12947 + (((-1.0)
12948 * (1.51009803921569)
12949 * px))
12950 + (((-1.0) * px
12951 * x1232))
12952 + ((py * x1234))
12953 + ((py * x1236))
12954 + ((py
12955 * x1235)))))
12956 + (IKabs((
12957 (((-1.0) * py
12958 * x1233))
12959 + ((py
12960 * x1232))
12961 + ((px
12962 * x1236))
12963 + ((px
12964 * x1235))
12965 + (((1.51009803921569)
12966 * py))
12967 + ((px
12968 * x1234))))));
12969 j4eval[2] = IKsign(x1231);
12970 if (IKabs(j4eval[0])
12971 < 0.0000010000000000
12972 || IKabs(j4eval[1])
12973 < 0.0000010000000000
12974 || IKabs(j4eval[2])
12975 < 0.0000010000000000)
12976 {
12977 {
12978 IkReal evalcond[7];
12979 bool bgotonextstatement
12980 = true;
12981 do
12982 {
12983 evalcond[0]
12984 = ((-3.14159265358979)
12985 + (IKfmod(
12986 ((3.14159265358979)
12987 + (IKabs((
12988 (-1.5707963267949)
12989 + j6)))),
12990 6.28318530717959)));
12991 evalcond[1] = pz;
12992 if (IKabs(evalcond[0])
12993 < 0.0000010000000000
12994 && IKabs(evalcond
12995 [1])
12996 < 0.0000010000000000)
12997 {
12998 bgotonextstatement
12999 = false;
13000 {
13001 IkReal j4eval[3];
13002 sj8 = -1.0;
13003 cj8 = 0;
13004 j8 = -1.5707963267949;
13005 sj6 = 1.0;
13006 cj6 = 0;
13007 j6 = 1.5707963267949;
13008 IkReal x1237
13009 = (pp
13010 + (((-1.0)
13011 * (1.0)
13012 * (pz
13013 * pz))));
13014 IkReal x1238
13015 = (cj9 * px);
13016 IkReal x1239
13017 = (cj9 * py);
13018 IkReal x1240
13019 = ((3.92156862745098)
13020 * pp);
13021 IkReal x1241
13022 = ((0.045)
13023 * sj9);
13024 j4eval[0] = x1237;
13025 j4eval[1]
13026 = ((IKabs((
13027 ((px
13028 * x1240))
13029 + (((-1.0)
13030 * (1.51009803921569)
13031 * px))
13032 + (((0.55)
13033 * py))
13034 + (((0.3)
13035 * x1239))
13036 + ((py
13037 * x1241))
13038 + (((-1.32323529411765)
13039 * x1238)))))
13040 + (IKabs((
13041 ((px
13042 * x1241))
13043 + (((1.51009803921569)
13044 * py))
13045 + (((1.32323529411765)
13046 * x1239))
13047 + (((-1.0)
13048 * py
13049 * x1240))
13050 + (((0.55)
13051 * px))
13052 + (((0.3)
13053 * x1238))))));
13054 j4eval[2]
13055 = IKsign(
13056 x1237);
13057 if (IKabs(
13058 j4eval[0])
13059 < 0.0000010000000000
13060 || IKabs(
13061 j4eval
13062 [1])
13063 < 0.0000010000000000
13064 || IKabs(
13065 j4eval
13066 [2])
13067 < 0.0000010000000000)
13068 {
13069 {
13070 IkReal
13071 j4eval[3];
13072 sj8 = -1.0;
13073 cj8 = 0;
13074 j8 = -1.5707963267949;
13075 sj6 = 1.0;
13076 cj6 = 0;
13077 j6 = 1.5707963267949;
13078 IkReal x1242
13079 = (pp
13080 + (((-1.0)
13081 * (1.0)
13082 * (pz
13083 * pz))));
13084 IkReal x1243
13085 = (cj9
13086 * px);
13087 IkReal x1244
13088 = (cj9
13089 * py);
13090 IkReal x1245
13091 = (pp
13092 * px);
13093 IkReal x1246
13094 = (pp
13095 * py);
13096 j4eval[0]
13097 = x1242;
13098 j4eval[1]
13099 = ((IKabs((
13100 (((-1.32323529411765)
13101 * x1243))
13102 + (((0.316735294117647)
13103 * py))
13104 + (((-1.0)
13105 * (1.51009803921569)
13106 * px))
13107 + (((0.108264705882353)
13108 * x1244))
13109 + (((0.588235294117647)
13110 * x1246))
13111 + (((3.92156862745098)
13112 * x1245)))))
13113 + (IKabs((
13114 (((0.588235294117647)
13115 * x1245))
13116 + (((1.32323529411765)
13117 * x1244))
13118 + (((-3.92156862745098)
13119 * x1246))
13120 + (((0.316735294117647)
13121 * px))
13122 + (((1.51009803921569)
13123 * py))
13124 + (((0.108264705882353)
13125 * x1243))))));
13126 j4eval[2]
13127 = IKsign(
13128 x1242);
13129 if (IKabs(
13130 j4eval
13131 [0])
13132 < 0.0000010000000000
13133 || IKabs(
13134 j4eval
13135 [1])
13136 < 0.0000010000000000
13137 || IKabs(
13138 j4eval
13139 [2])
13140 < 0.0000010000000000)
13141 {
13142 {
13143 IkReal j4eval
13144 [3];
13145 sj8 = -1.0;
13146 cj8 = 0;
13147 j8 = -1.5707963267949;
13148 sj6 = 1.0;
13149 cj6 = 0;
13150 j6 = 1.5707963267949;
13151 IkReal
13152 x1247
13153 = pz
13154 * pz;
13155 IkReal
13156 x1248
13157 = (cj9
13158 * px);
13159 IkReal
13160 x1249
13161 = (cj9
13162 * py);
13163 IkReal
13164 x1250
13165 = (pp
13166 * px);
13167 IkReal
13168 x1251
13169 = (pp
13170 * py);
13171 j4eval[0]
13172 = (pp
13173 + (((-1.0)
13174 * x1247)));
13175 j4eval[1]
13176 = ((IKabs((
13177 (((0.348408823529412)
13178 * py))
13179 + (((0.119091176470588)
13180 * x1249))
13181 + (((0.647058823529412)
13182 * x1251))
13183 + (((-1.45555882352941)
13184 * x1248))
13185 + (((4.31372549019608)
13186 * x1250))
13187 + (((-1.0)
13188 * (1.66110784313725)
13189 * px)))))
13190 + (IKabs((
13191 (((1.45555882352941)
13192 * x1249))
13193 + (((0.348408823529412)
13194 * px))
13195 + (((1.66110784313725)
13196 * py))
13197 + (((0.647058823529412)
13198 * x1250))
13199 + (((0.119091176470588)
13200 * x1248))
13201 + (((-4.31372549019608)
13202 * x1251))))));
13203 j4eval[2] = IKsign((
13204 (((1.1)
13205 * pp))
13206 + (((-1.1)
13207 * x1247))));
13208 if (IKabs(
13209 j4eval
13210 [0])
13211 < 0.0000010000000000
13212 || IKabs(
13213 j4eval
13214 [1])
13215 < 0.0000010000000000
13216 || IKabs(
13217 j4eval
13218 [2])
13219 < 0.0000010000000000)
13220 {
13221 {
13222 IkReal evalcond
13223 [6];
13224 bool
13225 bgotonextstatement
13226 = true;
13227 do
13228 {
13229 IkReal
13230 x1252
13231 = ((1.32323529411765)
13232 * cj9);
13233 IkReal
13234 x1253
13235 = ((3.92156862745098)
13236 * pp);
13237 evalcond
13238 [0]
13239 = ((IKabs(
13240 py))
13241 + (IKabs(
13242 px)));
13243 evalcond
13244 [1]
13245 = ((-0.55)
13246 + (((-1.0)
13247 * (0.3)
13248 * cj9))
13249 + (((-1.0)
13250 * (0.045)
13251 * sj9)));
13252 evalcond
13253 [2]
13254 = ((1.51009803921569)
13255 + (((-1.0)
13256 * x1253))
13257 + x1252);
13258 evalcond
13259 [3]
13260 = ((-1.51009803921569)
13261 + (((-1.0)
13262 * x1252))
13263 + x1253);
13264 evalcond
13265 [4]
13266 = ((0.316735294117647)
13267 + (((0.108264705882353)
13268 * cj9))
13269 + (((0.588235294117647)
13270 * pp)));
13271 evalcond
13272 [5]
13273 = ((-0.2125)
13274 + (((-1.0)
13275 * (1.0)
13276 * pp)));
13277 if (IKabs(
13278 evalcond
13279 [0])
13280 < 0.0000010000000000
13281 && IKabs(
13282 evalcond
13283 [1])
13284 < 0.0000010000000000
13285 && IKabs(
13286 evalcond
13287 [2])
13288 < 0.0000010000000000
13289 && IKabs(
13290 evalcond
13291 [3])
13292 < 0.0000010000000000
13293 && IKabs(
13294 evalcond
13295 [4])
13296 < 0.0000010000000000
13297 && IKabs(
13298 evalcond
13299 [5])
13300 < 0.0000010000000000)
13301 {
13302 bgotonextstatement
13303 = false;
13304 {
13305 IkReal j4array
13306 [4],
13307 cj4array
13308 [4],
13309 sj4array
13310 [4];
13311 bool j4valid
13312 [4]
13313 = {false};
13314 _nj4
13315 = 4;
13316 j4array
13317 [0]
13318 = 0;
13319 sj4array
13320 [0]
13321 = IKsin(
13322 j4array
13323 [0]);
13324 cj4array
13325 [0]
13326 = IKcos(
13327 j4array
13328 [0]);
13329 j4array
13330 [1]
13331 = 1.5707963267949;
13332 sj4array
13333 [1]
13334 = IKsin(
13335 j4array
13336 [1]);
13337 cj4array
13338 [1]
13339 = IKcos(
13340 j4array
13341 [1]);
13342 j4array
13343 [2]
13344 = 3.14159265358979;
13345 sj4array
13346 [2]
13347 = IKsin(
13348 j4array
13349 [2]);
13350 cj4array
13351 [2]
13352 = IKcos(
13353 j4array
13354 [2]);
13355 j4array
13356 [3]
13357 = -1.5707963267949;
13358 sj4array
13359 [3]
13360 = IKsin(
13361 j4array
13362 [3]);
13363 cj4array
13364 [3]
13365 = IKcos(
13366 j4array
13367 [3]);
13368 if (j4array
13369 [0]
13370 > IKPI)
13371 {
13372 j4array
13373 [0]
13374 -= IK2PI;
13375 }
13376 else if (
13377 j4array
13378 [0]
13379 < -IKPI)
13380 {
13381 j4array
13382 [0]
13383 += IK2PI;
13384 }
13385 j4valid
13386 [0]
13387 = true;
13388 if (j4array
13389 [1]
13390 > IKPI)
13391 {
13392 j4array
13393 [1]
13394 -= IK2PI;
13395 }
13396 else if (
13397 j4array
13398 [1]
13399 < -IKPI)
13400 {
13401 j4array
13402 [1]
13403 += IK2PI;
13404 }
13405 j4valid
13406 [1]
13407 = true;
13408 if (j4array
13409 [2]
13410 > IKPI)
13411 {
13412 j4array
13413 [2]
13414 -= IK2PI;
13415 }
13416 else if (
13417 j4array
13418 [2]
13419 < -IKPI)
13420 {
13421 j4array
13422 [2]
13423 += IK2PI;
13424 }
13425 j4valid
13426 [2]
13427 = true;
13428 if (j4array
13429 [3]
13430 > IKPI)
13431 {
13432 j4array
13433 [3]
13434 -= IK2PI;
13435 }
13436 else if (
13437 j4array
13438 [3]
13439 < -IKPI)
13440 {
13441 j4array
13442 [3]
13443 += IK2PI;
13444 }
13445 j4valid
13446 [3]
13447 = true;
13448 for (
13449 int ij4
13450 = 0;
13451 ij4
13452 < 4;
13453 ++ij4)
13454 {
13455 if (!j4valid
13456 [ij4])
13457 {
13458 continue;
13459 }
13460 _ij4[0]
13461 = ij4;
13462 _ij4[1]
13463 = -1;
13464 for (
13465 int iij4
13466 = ij4
13467 + 1;
13468 iij4
13469 < 4;
13470 ++iij4)
13471 {
13472 if (j4valid
13473 [iij4]
13474 && IKabs(
13475 cj4array
13476 [ij4]
13477 - cj4array
13478 [iij4])
13479 < IKFAST_SOLUTION_THRESH
13480 && IKabs(
13481 sj4array
13482 [ij4]
13483 - sj4array
13484 [iij4])
13485 < IKFAST_SOLUTION_THRESH)
13486 {
13487 j4valid
13488 [iij4]
13489 = false;
13490 _ij4[1]
13491 = iij4;
13492 break;
13493 }
13494 }
13495 j4 = j4array
13496 [ij4];
13497 cj4 = cj4array
13498 [ij4];
13499 sj4 = sj4array
13500 [ij4];
13501
13502 rotationfunction0(
13503 solutions);
13504 }
13505 }
13506 }
13507 } while (
13508 0);
13509 if (bgotonextstatement)
13510 {
13511 bool
13512 bgotonextstatement
13513 = true;
13514 do
13515 {
13516 if (1)
13517 {
13518 bgotonextstatement
13519 = false;
13520 continue; // branch miss [j4]
13521 }
13522 } while (
13523 0);
13524 if (bgotonextstatement)
13525 {
13526 }
13527 }
13528 }
13529 }
13530 else
13531 {
13532 {
13533 IkReal j4array
13534 [1],
13535 cj4array
13536 [1],
13537 sj4array
13538 [1];
13539 bool j4valid
13540 [1]
13541 = {false};
13542 _nj4
13543 = 1;
13544 IkReal
13545 x1254
13546 = (cj9
13547 * px);
13548 IkReal
13549 x1255
13550 = (cj9
13551 * py);
13552 IkReal
13553 x1256
13554 = (pp
13555 * px);
13556 IkReal
13557 x1257
13558 = (pp
13559 * py);
13560 CheckValue<IkReal> x1258 = IKatan2WithCheck(
13561 IkReal((
13562 (((0.348408823529412)
13563 * py))
13564 + (((0.647058823529412)
13565 * x1257))
13566 + (((4.31372549019608)
13567 * x1256))
13568 + (((0.119091176470588)
13569 * x1255))
13570 + (((-1.0)
13571 * (1.66110784313725)
13572 * px))
13573 + (((-1.45555882352941)
13574 * x1254)))),
13575 ((((0.348408823529412)
13576 * px))
13577 + (((1.66110784313725)
13578 * py))
13579 + (((0.119091176470588)
13580 * x1254))
13581 + (((-4.31372549019608)
13582 * x1257))
13583 + (((0.647058823529412)
13584 * x1256))
13585 + (((1.45555882352941)
13586 * x1255))),
13587 IKFAST_ATAN2_MAGTHRESH);
13588 if (!x1258
13589 .valid)
13590 {
13591 continue;
13592 }
13593 CheckValue<IkReal> x1259 = IKPowWithIntegerCheck(
13594 IKsign((
13595 (((1.1)
13596 * pp))
13597 + (((-1.0)
13598 * (1.1)
13599 * (pz
13600 * pz))))),
13601 -1);
13602 if (!x1259
13603 .valid)
13604 {
13605 continue;
13606 }
13607 j4array
13608 [0]
13609 = ((-1.5707963267949)
13610 + (x1258
13611 .value)
13612 + (((1.5707963267949)
13613 * (x1259
13614 .value))));
13615 sj4array
13616 [0]
13617 = IKsin(
13618 j4array
13619 [0]);
13620 cj4array
13621 [0]
13622 = IKcos(
13623 j4array
13624 [0]);
13625 if (j4array
13626 [0]
13627 > IKPI)
13628 {
13629 j4array
13630 [0]
13631 -= IK2PI;
13632 }
13633 else if (
13634 j4array
13635 [0]
13636 < -IKPI)
13637 {
13638 j4array
13639 [0]
13640 += IK2PI;
13641 }
13642 j4valid
13643 [0]
13644 = true;
13645 for (
13646 int ij4
13647 = 0;
13648 ij4
13649 < 1;
13650 ++ij4)
13651 {
13652 if (!j4valid
13653 [ij4])
13654 {
13655 continue;
13656 }
13657 _ij4[0]
13658 = ij4;
13659 _ij4[1]
13660 = -1;
13661 for (
13662 int iij4
13663 = ij4
13664 + 1;
13665 iij4
13666 < 1;
13667 ++iij4)
13668 {
13669 if (j4valid
13670 [iij4]
13671 && IKabs(
13672 cj4array
13673 [ij4]
13674 - cj4array
13675 [iij4])
13676 < IKFAST_SOLUTION_THRESH
13677 && IKabs(
13678 sj4array
13679 [ij4]
13680 - sj4array
13681 [iij4])
13682 < IKFAST_SOLUTION_THRESH)
13683 {
13684 j4valid
13685 [iij4]
13686 = false;
13687 _ij4[1]
13688 = iij4;
13689 break;
13690 }
13691 }
13692 j4 = j4array
13693 [ij4];
13694 cj4 = cj4array
13695 [ij4];
13696 sj4 = sj4array
13697 [ij4];
13698 {
13699 IkReal evalcond
13700 [4];
13701 IkReal
13702 x1260
13703 = IKcos(
13704 j4);
13705 IkReal
13706 x1261
13707 = (px
13708 * x1260);
13709 IkReal
13710 x1262
13711 = IKsin(
13712 j4);
13713 IkReal
13714 x1263
13715 = (py
13716 * x1262);
13717 IkReal
13718 x1264
13719 = (px
13720 * x1262);
13721 IkReal
13722 x1265
13723 = (py
13724 * x1260);
13725 evalcond
13726 [0]
13727 = ((-0.55)
13728 + x1261
13729 + x1263
13730 + (((-1.0)
13731 * (0.3)
13732 * cj9))
13733 + (((-1.0)
13734 * (0.045)
13735 * sj9)));
13736 evalcond
13737 [1]
13738 = ((1.51009803921569)
13739 + (((-1.0)
13740 * (3.92156862745098)
13741 * pp))
13742 + (((-1.0)
13743 * x1265))
13744 + x1264
13745 + (((1.32323529411765)
13746 * cj9)));
13747 evalcond
13748 [2]
13749 = ((0.316735294117647)
13750 + (((-1.0)
13751 * x1261))
13752 + (((-1.0)
13753 * x1263))
13754 + (((0.108264705882353)
13755 * cj9))
13756 + (((0.588235294117647)
13757 * pp)));
13758 evalcond
13759 [3]
13760 = ((-0.2125)
13761 + (((0.09)
13762 * x1264))
13763 + (((-0.09)
13764 * x1265))
13765 + (((-1.0)
13766 * (1.0)
13767 * pp))
13768 + (((1.1)
13769 * x1263))
13770 + (((1.1)
13771 * x1261)));
13772 if (IKabs(
13773 evalcond
13774 [0])
13775 > IKFAST_EVALCOND_THRESH
13776 || IKabs(
13777 evalcond
13778 [1])
13779 > IKFAST_EVALCOND_THRESH
13780 || IKabs(
13781 evalcond
13782 [2])
13783 > IKFAST_EVALCOND_THRESH
13784 || IKabs(
13785 evalcond
13786 [3])
13787 > IKFAST_EVALCOND_THRESH)
13788 {
13789 continue;
13790 }
13791 }
13792
13793 rotationfunction0(
13794 solutions);
13795 }
13796 }
13797 }
13798 }
13799 }
13800 else
13801 {
13802 {
13803 IkReal j4array
13804 [1],
13805 cj4array
13806 [1],
13807 sj4array
13808 [1];
13809 bool j4valid
13810 [1]
13811 = {false};
13812 _nj4 = 1;
13813 IkReal
13814 x1266
13815 = (cj9
13816 * px);
13817 IkReal
13818 x1267
13819 = (cj9
13820 * py);
13821 IkReal
13822 x1268
13823 = (pp
13824 * px);
13825 IkReal
13826 x1269
13827 = (pp
13828 * py);
13829 CheckValue<IkReal> x1270 = IKatan2WithCheck(
13830 IkReal((
13831 (((0.316735294117647)
13832 * py))
13833 + (((0.108264705882353)
13834 * x1267))
13835 + (((-1.32323529411765)
13836 * x1266))
13837 + (((3.92156862745098)
13838 * x1268))
13839 + (((-1.0)
13840 * (1.51009803921569)
13841 * px))
13842 + (((0.588235294117647)
13843 * x1269)))),
13844 ((((0.588235294117647)
13845 * x1268))
13846 + (((1.32323529411765)
13847 * x1267))
13848 + (((0.316735294117647)
13849 * px))
13850 + (((1.51009803921569)
13851 * py))
13852 + (((-3.92156862745098)
13853 * x1269))
13854 + (((0.108264705882353)
13855 * x1266))),
13856 IKFAST_ATAN2_MAGTHRESH);
13857 if (!x1270
13858 .valid)
13859 {
13860 continue;
13861 }
13862 CheckValue<IkReal> x1271 = IKPowWithIntegerCheck(
13863 IKsign((
13864 pp
13865 + (((-1.0)
13866 * (1.0)
13867 * (pz
13868 * pz))))),
13869 -1);
13870 if (!x1271
13871 .valid)
13872 {
13873 continue;
13874 }
13875 j4array[0]
13876 = ((-1.5707963267949)
13877 + (x1270
13878 .value)
13879 + (((1.5707963267949)
13880 * (x1271
13881 .value))));
13882 sj4array[0] = IKsin(
13883 j4array
13884 [0]);
13885 cj4array[0] = IKcos(
13886 j4array
13887 [0]);
13888 if (j4array
13889 [0]
13890 > IKPI)
13891 {
13892 j4array
13893 [0]
13894 -= IK2PI;
13895 }
13896 else if (
13897 j4array
13898 [0]
13899 < -IKPI)
13900 {
13901 j4array
13902 [0]
13903 += IK2PI;
13904 }
13905 j4valid[0]
13906 = true;
13907 for (
13908 int ij4
13909 = 0;
13910 ij4
13911 < 1;
13912 ++ij4)
13913 {
13914 if (!j4valid
13915 [ij4])
13916 {
13917 continue;
13918 }
13919 _ij4[0]
13920 = ij4;
13921 _ij4[1]
13922 = -1;
13923 for (
13924 int iij4
13925 = ij4
13926 + 1;
13927 iij4
13928 < 1;
13929 ++iij4)
13930 {
13931 if (j4valid
13932 [iij4]
13933 && IKabs(
13934 cj4array
13935 [ij4]
13936 - cj4array
13937 [iij4])
13938 < IKFAST_SOLUTION_THRESH
13939 && IKabs(
13940 sj4array
13941 [ij4]
13942 - sj4array
13943 [iij4])
13944 < IKFAST_SOLUTION_THRESH)
13945 {
13946 j4valid
13947 [iij4]
13948 = false;
13949 _ij4[1]
13950 = iij4;
13951 break;
13952 }
13953 }
13954 j4 = j4array
13955 [ij4];
13956 cj4 = cj4array
13957 [ij4];
13958 sj4 = sj4array
13959 [ij4];
13960 {
13961 IkReal evalcond
13962 [4];
13963 IkReal
13964 x1272
13965 = IKcos(
13966 j4);
13967 IkReal
13968 x1273
13969 = (px
13970 * x1272);
13971 IkReal
13972 x1274
13973 = IKsin(
13974 j4);
13975 IkReal
13976 x1275
13977 = (py
13978 * x1274);
13979 IkReal
13980 x1276
13981 = (px
13982 * x1274);
13983 IkReal
13984 x1277
13985 = (py
13986 * x1272);
13987 evalcond
13988 [0]
13989 = ((-0.55)
13990 + x1275
13991 + x1273
13992 + (((-1.0)
13993 * (0.3)
13994 * cj9))
13995 + (((-1.0)
13996 * (0.045)
13997 * sj9)));
13998 evalcond
13999 [1]
14000 = ((1.51009803921569)
14001 + (((-1.0)
14002 * (3.92156862745098)
14003 * pp))
14004 + x1276
14005 + (((1.32323529411765)
14006 * cj9))
14007 + (((-1.0)
14008 * x1277)));
14009 evalcond
14010 [2]
14011 = ((0.316735294117647)
14012 + (((0.108264705882353)
14013 * cj9))
14014 + (((-1.0)
14015 * x1273))
14016 + (((0.588235294117647)
14017 * pp))
14018 + (((-1.0)
14019 * x1275)));
14020 evalcond
14021 [3]
14022 = ((-0.2125)
14023 + (((0.09)
14024 * x1276))
14025 + (((1.1)
14026 * x1275))
14027 + (((1.1)
14028 * x1273))
14029 + (((-1.0)
14030 * (1.0)
14031 * pp))
14032 + (((-0.09)
14033 * x1277)));
14034 if (IKabs(
14035 evalcond
14036 [0])
14037 > IKFAST_EVALCOND_THRESH
14038 || IKabs(
14039 evalcond
14040 [1])
14041 > IKFAST_EVALCOND_THRESH
14042 || IKabs(
14043 evalcond
14044 [2])
14045 > IKFAST_EVALCOND_THRESH
14046 || IKabs(
14047 evalcond
14048 [3])
14049 > IKFAST_EVALCOND_THRESH)
14050 {
14051 continue;
14052 }
14053 }
14054
14055 rotationfunction0(
14056 solutions);
14057 }
14058 }
14059 }
14060 }
14061 }
14062 else
14063 {
14064 {
14065 IkReal j4array
14066 [1],
14067 cj4array
14068 [1],
14069 sj4array
14070 [1];
14071 bool
14072 j4valid[1]
14073 = {false};
14074 _nj4 = 1;
14075 IkReal x1278
14076 = (cj9
14077 * px);
14078 IkReal x1279
14079 = (cj9
14080 * py);
14081 IkReal x1280
14082 = ((3.92156862745098)
14083 * pp);
14084 IkReal x1281
14085 = ((0.045)
14086 * sj9);
14087 CheckValue<IkReal> x1282 = IKPowWithIntegerCheck(
14088 IKsign((
14089 pp
14090 + (((-1.0)
14091 * (1.0)
14092 * (pz
14093 * pz))))),
14094 -1);
14095 if (!x1282
14096 .valid)
14097 {
14098 continue;
14099 }
14100 CheckValue<IkReal> x1283 = IKatan2WithCheck(
14101 IkReal((
14102 (((-1.0)
14103 * (1.51009803921569)
14104 * px))
14105 + (((-1.32323529411765)
14106 * x1278))
14107 + (((0.55)
14108 * py))
14109 + ((px
14110 * x1280))
14111 + ((py
14112 * x1281))
14113 + (((0.3)
14114 * x1279)))),
14115 ((((-1.0)
14116 * py
14117 * x1280))
14118 + (((1.32323529411765)
14119 * x1279))
14120 + (((1.51009803921569)
14121 * py))
14122 + (((0.3)
14123 * x1278))
14124 + (((0.55)
14125 * px))
14126 + ((px
14127 * x1281))),
14128 IKFAST_ATAN2_MAGTHRESH);
14129 if (!x1283
14130 .valid)
14131 {
14132 continue;
14133 }
14134 j4array[0]
14135 = ((-1.5707963267949)
14136 + (((1.5707963267949)
14137 * (x1282
14138 .value)))
14139 + (x1283
14140 .value));
14141 sj4array[0]
14142 = IKsin(
14143 j4array
14144 [0]);
14145 cj4array[0]
14146 = IKcos(
14147 j4array
14148 [0]);
14149 if (j4array[0]
14150 > IKPI)
14151 {
14152 j4array[0]
14153 -= IK2PI;
14154 }
14155 else if (
14156 j4array[0]
14157 < -IKPI)
14158 {
14159 j4array[0]
14160 += IK2PI;
14161 }
14162 j4valid[0]
14163 = true;
14164 for (int ij4
14165 = 0;
14166 ij4 < 1;
14167 ++ij4)
14168 {
14169 if (!j4valid
14170 [ij4])
14171 {
14172 continue;
14173 }
14174 _ij4[0]
14175 = ij4;
14176 _ij4[1]
14177 = -1;
14178 for (
14179 int iij4
14180 = ij4
14181 + 1;
14182 iij4
14183 < 1;
14184 ++iij4)
14185 {
14186 if (j4valid
14187 [iij4]
14188 && IKabs(
14189 cj4array
14190 [ij4]
14191 - cj4array
14192 [iij4])
14193 < IKFAST_SOLUTION_THRESH
14194 && IKabs(
14195 sj4array
14196 [ij4]
14197 - sj4array
14198 [iij4])
14199 < IKFAST_SOLUTION_THRESH)
14200 {
14201 j4valid
14202 [iij4]
14203 = false;
14204 _ij4[1]
14205 = iij4;
14206 break;
14207 }
14208 }
14209 j4 = j4array
14210 [ij4];
14211 cj4 = cj4array
14212 [ij4];
14213 sj4 = sj4array
14214 [ij4];
14215 {
14216 IkReal evalcond
14217 [4];
14218 IkReal
14219 x1284
14220 = IKcos(
14221 j4);
14222 IkReal
14223 x1285
14224 = (px
14225 * x1284);
14226 IkReal
14227 x1286
14228 = IKsin(
14229 j4);
14230 IkReal
14231 x1287
14232 = (py
14233 * x1286);
14234 IkReal
14235 x1288
14236 = (px
14237 * x1286);
14238 IkReal
14239 x1289
14240 = (py
14241 * x1284);
14242 evalcond
14243 [0]
14244 = ((-0.55)
14245 + x1287
14246 + x1285
14247 + (((-1.0)
14248 * (0.3)
14249 * cj9))
14250 + (((-1.0)
14251 * (0.045)
14252 * sj9)));
14253 evalcond
14254 [1]
14255 = ((1.51009803921569)
14256 + (((-1.0)
14257 * x1289))
14258 + (((-1.0)
14259 * (3.92156862745098)
14260 * pp))
14261 + x1288
14262 + (((1.32323529411765)
14263 * cj9)));
14264 evalcond
14265 [2]
14266 = ((0.316735294117647)
14267 + (((-1.0)
14268 * x1285))
14269 + (((0.108264705882353)
14270 * cj9))
14271 + (((0.588235294117647)
14272 * pp))
14273 + (((-1.0)
14274 * x1287)));
14275 evalcond
14276 [3]
14277 = ((-0.2125)
14278 + (((0.09)
14279 * x1288))
14280 + (((-0.09)
14281 * x1289))
14282 + (((1.1)
14283 * x1287))
14284 + (((-1.0)
14285 * (1.0)
14286 * pp))
14287 + (((1.1)
14288 * x1285)));
14289 if (IKabs(
14290 evalcond
14291 [0])
14292 > IKFAST_EVALCOND_THRESH
14293 || IKabs(
14294 evalcond
14295 [1])
14296 > IKFAST_EVALCOND_THRESH
14297 || IKabs(
14298 evalcond
14299 [2])
14300 > IKFAST_EVALCOND_THRESH
14301 || IKabs(
14302 evalcond
14303 [3])
14304 > IKFAST_EVALCOND_THRESH)
14305 {
14306 continue;
14307 }
14308 }
14309
14310 rotationfunction0(
14311 solutions);
14312 }
14313 }
14314 }
14315 }
14316 }
14317 } while (0);
14318 if (bgotonextstatement)
14319 {
14320 bool
14321 bgotonextstatement
14322 = true;
14323 do
14324 {
14325 evalcond[0]
14326 = ((-3.14159265358979)
14327 + (IKfmod(
14328 ((3.14159265358979)
14329 + (IKabs((
14330 (1.5707963267949)
14331 + j6)))),
14332 6.28318530717959)));
14333 evalcond[1]
14334 = ((-1.0)
14335 * (((1.0)
14336 * pz)));
14337 if (IKabs(
14338 evalcond[0])
14339 < 0.0000010000000000
14340 && IKabs(
14341 evalcond
14342 [1])
14343 < 0.0000010000000000)
14344 {
14345 bgotonextstatement
14346 = false;
14347 {
14348 IkReal
14349 j4eval[3];
14350 sj8 = -1.0;
14351 cj8 = 0;
14352 j8 = -1.5707963267949;
14353 sj6 = -1.0;
14354 cj6 = 0;
14355 j6 = -1.5707963267949;
14356 IkReal x1290
14357 = (pp
14358 + (((-1.0)
14359 * (1.0)
14360 * (pz
14361 * pz))));
14362 IkReal x1291
14363 = (cj9
14364 * px);
14365 IkReal x1292
14366 = (cj9
14367 * py);
14368 IkReal x1293
14369 = ((3.92156862745098)
14370 * pp);
14371 IkReal x1294
14372 = ((0.045)
14373 * sj9);
14374 j4eval[0]
14375 = x1290;
14376 j4eval[1]
14377 = ((IKabs((
14378 (((-1.32323529411765)
14379 * x1291))
14380 + ((px
14381 * x1293))
14382 + (((-1.0)
14383 * (1.51009803921569)
14384 * px))
14385 + (((-1.0)
14386 * py
14387 * x1294))
14388 + (((-0.3)
14389 * x1292))
14390 + (((-1.0)
14391 * (0.55)
14392 * py)))))
14393 + (IKabs((
14394 (((-1.0)
14395 * (0.55)
14396 * px))
14397 + (((-0.3)
14398 * x1291))
14399 + (((-1.0)
14400 * px
14401 * x1294))
14402 + (((1.51009803921569)
14403 * py))
14404 + (((-1.0)
14405 * py
14406 * x1293))
14407 + (((1.32323529411765)
14408 * x1292))))));
14409 j4eval[2]
14410 = IKsign(
14411 x1290);
14412 if (IKabs(
14413 j4eval
14414 [0])
14415 < 0.0000010000000000
14416 || IKabs(
14417 j4eval
14418 [1])
14419 < 0.0000010000000000
14420 || IKabs(
14421 j4eval
14422 [2])
14423 < 0.0000010000000000)
14424 {
14425 {
14426 IkReal
14427 j4eval
14428 [3];
14429 sj8 = -1.0;
14430 cj8 = 0;
14431 j8 = -1.5707963267949;
14432 sj6 = -1.0;
14433 cj6 = 0;
14434 j6 = -1.5707963267949;
14435 IkReal x1295
14436 = (pp
14437 + (((-1.0)
14438 * (1.0)
14439 * (pz
14440 * pz))));
14441 IkReal x1296
14442 = (cj9
14443 * px);
14444 IkReal x1297
14445 = (cj9
14446 * py);
14447 IkReal x1298
14448 = (pp
14449 * px);
14450 IkReal x1299
14451 = (pp
14452 * py);
14453 j4eval[0]
14454 = x1295;
14455 j4eval[1]
14456 = ((IKabs((
14457 (((-3.92156862745098)
14458 * x1299))
14459 + (((1.51009803921569)
14460 * py))
14461 + (((-0.108264705882353)
14462 * x1296))
14463 + (((-1.0)
14464 * (0.316735294117647)
14465 * px))
14466 + (((1.32323529411765)
14467 * x1297))
14468 + (((-0.588235294117647)
14469 * x1298)))))
14470 + (IKabs((
14471 (((-0.588235294117647)
14472 * x1299))
14473 + (((-1.32323529411765)
14474 * x1296))
14475 + (((-1.0)
14476 * (1.51009803921569)
14477 * px))
14478 + (((-1.0)
14479 * (0.316735294117647)
14480 * py))
14481 + (((-0.108264705882353)
14482 * x1297))
14483 + (((3.92156862745098)
14484 * x1298))))));
14485 j4eval[2]
14486 = IKsign(
14487 x1295);
14488 if (IKabs(
14489 j4eval
14490 [0])
14491 < 0.0000010000000000
14492 || IKabs(
14493 j4eval
14494 [1])
14495 < 0.0000010000000000
14496 || IKabs(
14497 j4eval
14498 [2])
14499 < 0.0000010000000000)
14500 {
14501 {
14502 IkReal j4eval
14503 [3];
14504 sj8 = -1.0;
14505 cj8 = 0;
14506 j8 = -1.5707963267949;
14507 sj6 = -1.0;
14508 cj6 = 0;
14509 j6 = -1.5707963267949;
14510 IkReal
14511 x1300
14512 = pz
14513 * pz;
14514 IkReal
14515 x1301
14516 = (cj9
14517 * px);
14518 IkReal
14519 x1302
14520 = (cj9
14521 * py);
14522 IkReal
14523 x1303
14524 = (pp
14525 * px);
14526 IkReal
14527 x1304
14528 = (pp
14529 * py);
14530 j4eval
14531 [0]
14532 = (x1300
14533 + (((-1.0)
14534 * (1.0)
14535 * pp)));
14536 j4eval[1] = IKsign((
14537 (((1.1)
14538 * x1300))
14539 + (((-1.0)
14540 * (1.1)
14541 * pp))));
14542 j4eval
14543 [2]
14544 = ((IKabs((
14545 (((4.31372549019608)
14546 * x1304))
14547 + (((0.348408823529412)
14548 * px))
14549 + (((0.119091176470588)
14550 * x1301))
14551 + (((-1.0)
14552 * (1.66110784313725)
14553 * py))
14554 + (((0.647058823529412)
14555 * x1303))
14556 + (((-1.45555882352941)
14557 * x1302)))))
14558 + (IKabs((
14559 (((0.348408823529412)
14560 * py))
14561 + (((1.66110784313725)
14562 * px))
14563 + (((1.45555882352941)
14564 * x1301))
14565 + (((-4.31372549019608)
14566 * x1303))
14567 + (((0.647058823529412)
14568 * x1304))
14569 + (((0.119091176470588)
14570 * x1302))))));
14571 if (IKabs(
14572 j4eval
14573 [0])
14574 < 0.0000010000000000
14575 || IKabs(
14576 j4eval
14577 [1])
14578 < 0.0000010000000000
14579 || IKabs(
14580 j4eval
14581 [2])
14582 < 0.0000010000000000)
14583 {
14584 {
14585 IkReal evalcond
14586 [6];
14587 bool
14588 bgotonextstatement
14589 = true;
14590 do
14591 {
14592 IkReal
14593 x1305
14594 = ((1.32323529411765)
14595 * cj9);
14596 IkReal
14597 x1306
14598 = ((3.92156862745098)
14599 * pp);
14600 evalcond
14601 [0]
14602 = ((IKabs(
14603 py))
14604 + (IKabs(
14605 px)));
14606 evalcond
14607 [1]
14608 = ((-0.55)
14609 + (((-1.0)
14610 * (0.3)
14611 * cj9))
14612 + (((-1.0)
14613 * (0.045)
14614 * sj9)));
14615 evalcond
14616 [2]
14617 = ((1.51009803921569)
14618 + (((-1.0)
14619 * x1306))
14620 + x1305);
14621 evalcond
14622 [3]
14623 = ((-1.51009803921569)
14624 + x1306
14625 + (((-1.0)
14626 * x1305)));
14627 evalcond
14628 [4]
14629 = ((-0.316735294117647)
14630 + (((-1.0)
14631 * (0.588235294117647)
14632 * pp))
14633 + (((-1.0)
14634 * (0.108264705882353)
14635 * cj9)));
14636 evalcond
14637 [5]
14638 = ((-0.2125)
14639 + (((-1.0)
14640 * (1.0)
14641 * pp)));
14642 if (IKabs(
14643 evalcond
14644 [0])
14645 < 0.0000010000000000
14646 && IKabs(
14647 evalcond
14648 [1])
14649 < 0.0000010000000000
14650 && IKabs(
14651 evalcond
14652 [2])
14653 < 0.0000010000000000
14654 && IKabs(
14655 evalcond
14656 [3])
14657 < 0.0000010000000000
14658 && IKabs(
14659 evalcond
14660 [4])
14661 < 0.0000010000000000
14662 && IKabs(
14663 evalcond
14664 [5])
14665 < 0.0000010000000000)
14666 {
14667 bgotonextstatement
14668 = false;
14669 {
14670 IkReal j4array
14671 [4],
14672 cj4array
14673 [4],
14674 sj4array
14675 [4];
14676 bool j4valid
14677 [4]
14678 = {false};
14679 _nj4
14680 = 4;
14681 j4array
14682 [0]
14683 = 0;
14684 sj4array
14685 [0]
14686 = IKsin(
14687 j4array
14688 [0]);
14689 cj4array
14690 [0]
14691 = IKcos(
14692 j4array
14693 [0]);
14694 j4array
14695 [1]
14696 = 1.5707963267949;
14697 sj4array
14698 [1]
14699 = IKsin(
14700 j4array
14701 [1]);
14702 cj4array
14703 [1]
14704 = IKcos(
14705 j4array
14706 [1]);
14707 j4array
14708 [2]
14709 = 3.14159265358979;
14710 sj4array
14711 [2]
14712 = IKsin(
14713 j4array
14714 [2]);
14715 cj4array
14716 [2]
14717 = IKcos(
14718 j4array
14719 [2]);
14720 j4array
14721 [3]
14722 = -1.5707963267949;
14723 sj4array
14724 [3]
14725 = IKsin(
14726 j4array
14727 [3]);
14728 cj4array
14729 [3]
14730 = IKcos(
14731 j4array
14732 [3]);
14733 if (j4array
14734 [0]
14735 > IKPI)
14736 {
14737 j4array
14738 [0]
14739 -= IK2PI;
14740 }
14741 else if (
14742 j4array
14743 [0]
14744 < -IKPI)
14745 {
14746 j4array
14747 [0]
14748 += IK2PI;
14749 }
14750 j4valid
14751 [0]
14752 = true;
14753 if (j4array
14754 [1]
14755 > IKPI)
14756 {
14757 j4array
14758 [1]
14759 -= IK2PI;
14760 }
14761 else if (
14762 j4array
14763 [1]
14764 < -IKPI)
14765 {
14766 j4array
14767 [1]
14768 += IK2PI;
14769 }
14770 j4valid
14771 [1]
14772 = true;
14773 if (j4array
14774 [2]
14775 > IKPI)
14776 {
14777 j4array
14778 [2]
14779 -= IK2PI;
14780 }
14781 else if (
14782 j4array
14783 [2]
14784 < -IKPI)
14785 {
14786 j4array
14787 [2]
14788 += IK2PI;
14789 }
14790 j4valid
14791 [2]
14792 = true;
14793 if (j4array
14794 [3]
14795 > IKPI)
14796 {
14797 j4array
14798 [3]
14799 -= IK2PI;
14800 }
14801 else if (
14802 j4array
14803 [3]
14804 < -IKPI)
14805 {
14806 j4array
14807 [3]
14808 += IK2PI;
14809 }
14810 j4valid
14811 [3]
14812 = true;
14813 for (
14814 int ij4
14815 = 0;
14816 ij4
14817 < 4;
14818 ++ij4)
14819 {
14820 if (!j4valid
14821 [ij4])
14822 {
14823 continue;
14824 }
14825 _ij4[0]
14826 = ij4;
14827 _ij4[1]
14828 = -1;
14829 for (
14830 int iij4
14831 = ij4
14832 + 1;
14833 iij4
14834 < 4;
14835 ++iij4)
14836 {
14837 if (j4valid
14838 [iij4]
14839 && IKabs(
14840 cj4array
14841 [ij4]
14842 - cj4array
14843 [iij4])
14844 < IKFAST_SOLUTION_THRESH
14845 && IKabs(
14846 sj4array
14847 [ij4]
14848 - sj4array
14849 [iij4])
14850 < IKFAST_SOLUTION_THRESH)
14851 {
14852 j4valid
14853 [iij4]
14854 = false;
14855 _ij4[1]
14856 = iij4;
14857 break;
14858 }
14859 }
14860 j4 = j4array
14861 [ij4];
14862 cj4 = cj4array
14863 [ij4];
14864 sj4 = sj4array
14865 [ij4];
14866
14867 rotationfunction0(
14868 solutions);
14869 }
14870 }
14871 }
14872 } while (
14873 0);
14874 if (bgotonextstatement)
14875 {
14876 bool
14877 bgotonextstatement
14878 = true;
14879 do
14880 {
14881 if (1)
14882 {
14883 bgotonextstatement
14884 = false;
14885 continue; // branch miss [j4]
14886 }
14887 } while (
14888 0);
14889 if (bgotonextstatement)
14890 {
14891 }
14892 }
14893 }
14894 }
14895 else
14896 {
14897 {
14898 IkReal j4array
14899 [1],
14900 cj4array
14901 [1],
14902 sj4array
14903 [1];
14904 bool j4valid
14905 [1]
14906 = {false};
14907 _nj4
14908 = 1;
14909 IkReal
14910 x1307
14911 = (cj9
14912 * px);
14913 IkReal
14914 x1308
14915 = (cj9
14916 * py);
14917 IkReal
14918 x1309
14919 = (pp
14920 * px);
14921 IkReal
14922 x1310
14923 = (pp
14924 * py);
14925 CheckValue<IkReal> x1311 = IKPowWithIntegerCheck(
14926 IKsign((
14927 (((1.1)
14928 * (pz
14929 * pz)))
14930 + (((-1.0)
14931 * (1.1)
14932 * pp)))),
14933 -1);
14934 if (!x1311
14935 .valid)
14936 {
14937 continue;
14938 }
14939 CheckValue<IkReal> x1312 = IKatan2WithCheck(
14940 IkReal((
14941 (((1.45555882352941)
14942 * x1307))
14943 + (((0.348408823529412)
14944 * py))
14945 + (((1.66110784313725)
14946 * px))
14947 + (((0.119091176470588)
14948 * x1308))
14949 + (((-4.31372549019608)
14950 * x1309))
14951 + (((0.647058823529412)
14952 * x1310)))),
14953 ((((0.647058823529412)
14954 * x1309))
14955 + (((0.348408823529412)
14956 * px))
14957 + (((4.31372549019608)
14958 * x1310))
14959 + (((-1.45555882352941)
14960 * x1308))
14961 + (((0.119091176470588)
14962 * x1307))
14963 + (((-1.0)
14964 * (1.66110784313725)
14965 * py))),
14966 IKFAST_ATAN2_MAGTHRESH);
14967 if (!x1312
14968 .valid)
14969 {
14970 continue;
14971 }
14972 j4array
14973 [0]
14974 = ((-1.5707963267949)
14975 + (((1.5707963267949)
14976 * (x1311
14977 .value)))
14978 + (x1312
14979 .value));
14980 sj4array
14981 [0]
14982 = IKsin(
14983 j4array
14984 [0]);
14985 cj4array
14986 [0]
14987 = IKcos(
14988 j4array
14989 [0]);
14990 if (j4array
14991 [0]
14992 > IKPI)
14993 {
14994 j4array
14995 [0]
14996 -= IK2PI;
14997 }
14998 else if (
14999 j4array
15000 [0]
15001 < -IKPI)
15002 {
15003 j4array
15004 [0]
15005 += IK2PI;
15006 }
15007 j4valid
15008 [0]
15009 = true;
15010 for (
15011 int ij4
15012 = 0;
15013 ij4
15014 < 1;
15015 ++ij4)
15016 {
15017 if (!j4valid
15018 [ij4])
15019 {
15020 continue;
15021 }
15022 _ij4[0]
15023 = ij4;
15024 _ij4[1]
15025 = -1;
15026 for (
15027 int iij4
15028 = ij4
15029 + 1;
15030 iij4
15031 < 1;
15032 ++iij4)
15033 {
15034 if (j4valid
15035 [iij4]
15036 && IKabs(
15037 cj4array
15038 [ij4]
15039 - cj4array
15040 [iij4])
15041 < IKFAST_SOLUTION_THRESH
15042 && IKabs(
15043 sj4array
15044 [ij4]
15045 - sj4array
15046 [iij4])
15047 < IKFAST_SOLUTION_THRESH)
15048 {
15049 j4valid
15050 [iij4]
15051 = false;
15052 _ij4[1]
15053 = iij4;
15054 break;
15055 }
15056 }
15057 j4 = j4array
15058 [ij4];
15059 cj4 = cj4array
15060 [ij4];
15061 sj4 = sj4array
15062 [ij4];
15063 {
15064 IkReal evalcond
15065 [4];
15066 IkReal
15067 x1313
15068 = IKsin(
15069 j4);
15070 IkReal
15071 x1314
15072 = (px
15073 * x1313);
15074 IkReal
15075 x1315
15076 = IKcos(
15077 j4);
15078 IkReal
15079 x1316
15080 = (py
15081 * x1315);
15082 IkReal
15083 x1317
15084 = (px
15085 * x1315);
15086 IkReal
15087 x1318
15088 = (py
15089 * x1313);
15090 IkReal
15091 x1319
15092 = ((((-1.0)
15093 * x1318))
15094 + (((-1.0)
15095 * x1317)));
15096 evalcond
15097 [0]
15098 = ((1.51009803921569)
15099 + (((-1.0)
15100 * (3.92156862745098)
15101 * pp))
15102 + (((-1.0)
15103 * x1316))
15104 + (((1.32323529411765)
15105 * cj9))
15106 + x1314);
15107 evalcond
15108 [1]
15109 = ((-0.55)
15110 + (((-1.0)
15111 * (0.3)
15112 * cj9))
15113 + (((-1.0)
15114 * (0.045)
15115 * sj9))
15116 + x1319);
15117 evalcond
15118 [2]
15119 = ((-0.316735294117647)
15120 + (((-1.0)
15121 * (0.588235294117647)
15122 * pp))
15123 + (((-1.0)
15124 * (0.108264705882353)
15125 * cj9))
15126 + x1319);
15127 evalcond
15128 [3]
15129 = ((-0.2125)
15130 + (((-1.1)
15131 * x1317))
15132 + (((-1.0)
15133 * (1.0)
15134 * pp))
15135 + (((0.09)
15136 * x1314))
15137 + (((-0.09)
15138 * x1316))
15139 + (((-1.1)
15140 * x1318)));
15141 if (IKabs(
15142 evalcond
15143 [0])
15144 > IKFAST_EVALCOND_THRESH
15145 || IKabs(
15146 evalcond
15147 [1])
15148 > IKFAST_EVALCOND_THRESH
15149 || IKabs(
15150 evalcond
15151 [2])
15152 > IKFAST_EVALCOND_THRESH
15153 || IKabs(
15154 evalcond
15155 [3])
15156 > IKFAST_EVALCOND_THRESH)
15157 {
15158 continue;
15159 }
15160 }
15161
15162 rotationfunction0(
15163 solutions);
15164 }
15165 }
15166 }
15167 }
15168 }
15169 else
15170 {
15171 {
15172 IkReal j4array
15173 [1],
15174 cj4array
15175 [1],
15176 sj4array
15177 [1];
15178 bool j4valid
15179 [1]
15180 = {false};
15181 _nj4
15182 = 1;
15183 IkReal
15184 x1320
15185 = (cj9
15186 * px);
15187 IkReal
15188 x1321
15189 = (cj9
15190 * py);
15191 IkReal
15192 x1322
15193 = (pp
15194 * px);
15195 IkReal
15196 x1323
15197 = (pp
15198 * py);
15199 CheckValue<IkReal> x1324 = IKPowWithIntegerCheck(
15200 IKsign((
15201 pp
15202 + (((-1.0)
15203 * (1.0)
15204 * (pz
15205 * pz))))),
15206 -1);
15207 if (!x1324
15208 .valid)
15209 {
15210 continue;
15211 }
15212 CheckValue<IkReal> x1325 = IKatan2WithCheck(
15213 IkReal((
15214 (((-0.108264705882353)
15215 * x1321))
15216 + (((-1.0)
15217 * (1.51009803921569)
15218 * px))
15219 + (((-1.32323529411765)
15220 * x1320))
15221 + (((-1.0)
15222 * (0.316735294117647)
15223 * py))
15224 + (((3.92156862745098)
15225 * x1322))
15226 + (((-0.588235294117647)
15227 * x1323)))),
15228 ((((-3.92156862745098)
15229 * x1323))
15230 + (((-0.588235294117647)
15231 * x1322))
15232 + (((-0.108264705882353)
15233 * x1320))
15234 + (((1.51009803921569)
15235 * py))
15236 + (((1.32323529411765)
15237 * x1321))
15238 + (((-1.0)
15239 * (0.316735294117647)
15240 * px))),
15241 IKFAST_ATAN2_MAGTHRESH);
15242 if (!x1325
15243 .valid)
15244 {
15245 continue;
15246 }
15247 j4array
15248 [0]
15249 = ((-1.5707963267949)
15250 + (((1.5707963267949)
15251 * (x1324
15252 .value)))
15253 + (x1325
15254 .value));
15255 sj4array
15256 [0]
15257 = IKsin(
15258 j4array
15259 [0]);
15260 cj4array
15261 [0]
15262 = IKcos(
15263 j4array
15264 [0]);
15265 if (j4array
15266 [0]
15267 > IKPI)
15268 {
15269 j4array
15270 [0]
15271 -= IK2PI;
15272 }
15273 else if (
15274 j4array
15275 [0]
15276 < -IKPI)
15277 {
15278 j4array
15279 [0]
15280 += IK2PI;
15281 }
15282 j4valid
15283 [0]
15284 = true;
15285 for (
15286 int ij4
15287 = 0;
15288 ij4
15289 < 1;
15290 ++ij4)
15291 {
15292 if (!j4valid
15293 [ij4])
15294 {
15295 continue;
15296 }
15297 _ij4[0]
15298 = ij4;
15299 _ij4[1]
15300 = -1;
15301 for (
15302 int iij4
15303 = ij4
15304 + 1;
15305 iij4
15306 < 1;
15307 ++iij4)
15308 {
15309 if (j4valid
15310 [iij4]
15311 && IKabs(
15312 cj4array
15313 [ij4]
15314 - cj4array
15315 [iij4])
15316 < IKFAST_SOLUTION_THRESH
15317 && IKabs(
15318 sj4array
15319 [ij4]
15320 - sj4array
15321 [iij4])
15322 < IKFAST_SOLUTION_THRESH)
15323 {
15324 j4valid
15325 [iij4]
15326 = false;
15327 _ij4[1]
15328 = iij4;
15329 break;
15330 }
15331 }
15332 j4 = j4array
15333 [ij4];
15334 cj4 = cj4array
15335 [ij4];
15336 sj4 = sj4array
15337 [ij4];
15338 {
15339 IkReal evalcond
15340 [4];
15341 IkReal
15342 x1326
15343 = IKsin(
15344 j4);
15345 IkReal
15346 x1327
15347 = (px
15348 * x1326);
15349 IkReal
15350 x1328
15351 = IKcos(
15352 j4);
15353 IkReal
15354 x1329
15355 = (py
15356 * x1328);
15357 IkReal
15358 x1330
15359 = (px
15360 * x1328);
15361 IkReal
15362 x1331
15363 = (py
15364 * x1326);
15365 IkReal
15366 x1332
15367 = ((((-1.0)
15368 * x1330))
15369 + (((-1.0)
15370 * x1331)));
15371 evalcond
15372 [0]
15373 = ((1.51009803921569)
15374 + x1327
15375 + (((-1.0)
15376 * x1329))
15377 + (((-1.0)
15378 * (3.92156862745098)
15379 * pp))
15380 + (((1.32323529411765)
15381 * cj9)));
15382 evalcond
15383 [1]
15384 = ((-0.55)
15385 + x1332
15386 + (((-1.0)
15387 * (0.3)
15388 * cj9))
15389 + (((-1.0)
15390 * (0.045)
15391 * sj9)));
15392 evalcond
15393 [2]
15394 = ((-0.316735294117647)
15395 + x1332
15396 + (((-1.0)
15397 * (0.588235294117647)
15398 * pp))
15399 + (((-1.0)
15400 * (0.108264705882353)
15401 * cj9)));
15402 evalcond
15403 [3]
15404 = ((-0.2125)
15405 + (((-1.1)
15406 * x1330))
15407 + (((0.09)
15408 * x1327))
15409 + (((-0.09)
15410 * x1329))
15411 + (((-1.0)
15412 * (1.0)
15413 * pp))
15414 + (((-1.1)
15415 * x1331)));
15416 if (IKabs(
15417 evalcond
15418 [0])
15419 > IKFAST_EVALCOND_THRESH
15420 || IKabs(
15421 evalcond
15422 [1])
15423 > IKFAST_EVALCOND_THRESH
15424 || IKabs(
15425 evalcond
15426 [2])
15427 > IKFAST_EVALCOND_THRESH
15428 || IKabs(
15429 evalcond
15430 [3])
15431 > IKFAST_EVALCOND_THRESH)
15432 {
15433 continue;
15434 }
15435 }
15436
15437 rotationfunction0(
15438 solutions);
15439 }
15440 }
15441 }
15442 }
15443 }
15444 else
15445 {
15446 {
15447 IkReal
15448 j4array
15449 [1],
15450 cj4array
15451 [1],
15452 sj4array
15453 [1];
15454 bool j4valid
15455 [1]
15456 = {false};
15457 _nj4 = 1;
15458 IkReal x1333
15459 = (cj9
15460 * px);
15461 IkReal x1334
15462 = (cj9
15463 * py);
15464 IkReal x1335
15465 = ((3.92156862745098)
15466 * pp);
15467 IkReal x1336
15468 = ((0.045)
15469 * sj9);
15470 CheckValue<IkReal> x1337 = IKatan2WithCheck(
15471 IkReal((
15472 (((-1.32323529411765)
15473 * x1333))
15474 + (((-0.3)
15475 * x1334))
15476 + ((px
15477 * x1335))
15478 + (((-1.0)
15479 * (1.51009803921569)
15480 * px))
15481 + (((-1.0)
15482 * py
15483 * x1336))
15484 + (((-1.0)
15485 * (0.55)
15486 * py)))),
15487 ((((-1.0)
15488 * (0.55)
15489 * px))
15490 + (((-1.0)
15491 * py
15492 * x1335))
15493 + (((-1.0)
15494 * px
15495 * x1336))
15496 + (((1.51009803921569)
15497 * py))
15498 + (((-0.3)
15499 * x1333))
15500 + (((1.32323529411765)
15501 * x1334))),
15502 IKFAST_ATAN2_MAGTHRESH);
15503 if (!x1337
15504 .valid)
15505 {
15506 continue;
15507 }
15508 CheckValue<IkReal> x1338 = IKPowWithIntegerCheck(
15509 IKsign((
15510 pp
15511 + (((-1.0)
15512 * (1.0)
15513 * (pz
15514 * pz))))),
15515 -1);
15516 if (!x1338
15517 .valid)
15518 {
15519 continue;
15520 }
15521 j4array[0]
15522 = ((-1.5707963267949)
15523 + (x1337
15524 .value)
15525 + (((1.5707963267949)
15526 * (x1338
15527 .value))));
15528 sj4array[0] = IKsin(
15529 j4array
15530 [0]);
15531 cj4array[0] = IKcos(
15532 j4array
15533 [0]);
15534 if (j4array
15535 [0]
15536 > IKPI)
15537 {
15538 j4array[0]
15539 -= IK2PI;
15540 }
15541 else if (
15542 j4array
15543 [0]
15544 < -IKPI)
15545 {
15546 j4array[0]
15547 += IK2PI;
15548 }
15549 j4valid[0]
15550 = true;
15551 for (int ij4
15552 = 0;
15553 ij4
15554 < 1;
15555 ++ij4)
15556 {
15557 if (!j4valid
15558 [ij4])
15559 {
15560 continue;
15561 }
15562 _ij4[0]
15563 = ij4;
15564 _ij4[1]
15565 = -1;
15566 for (
15567 int iij4
15568 = ij4
15569 + 1;
15570 iij4
15571 < 1;
15572 ++iij4)
15573 {
15574 if (j4valid
15575 [iij4]
15576 && IKabs(
15577 cj4array
15578 [ij4]
15579 - cj4array
15580 [iij4])
15581 < IKFAST_SOLUTION_THRESH
15582 && IKabs(
15583 sj4array
15584 [ij4]
15585 - sj4array
15586 [iij4])
15587 < IKFAST_SOLUTION_THRESH)
15588 {
15589 j4valid
15590 [iij4]
15591 = false;
15592 _ij4[1]
15593 = iij4;
15594 break;
15595 }
15596 }
15597 j4 = j4array
15598 [ij4];
15599 cj4 = cj4array
15600 [ij4];
15601 sj4 = sj4array
15602 [ij4];
15603 {
15604 IkReal evalcond
15605 [4];
15606 IkReal
15607 x1339
15608 = IKsin(
15609 j4);
15610 IkReal
15611 x1340
15612 = (px
15613 * x1339);
15614 IkReal
15615 x1341
15616 = IKcos(
15617 j4);
15618 IkReal
15619 x1342
15620 = (py
15621 * x1341);
15622 IkReal
15623 x1343
15624 = (px
15625 * x1341);
15626 IkReal
15627 x1344
15628 = (py
15629 * x1339);
15630 IkReal
15631 x1345
15632 = ((((-1.0)
15633 * x1343))
15634 + (((-1.0)
15635 * x1344)));
15636 evalcond
15637 [0]
15638 = ((1.51009803921569)
15639 + (((-1.0)
15640 * (3.92156862745098)
15641 * pp))
15642 + (((-1.0)
15643 * x1342))
15644 + x1340
15645 + (((1.32323529411765)
15646 * cj9)));
15647 evalcond
15648 [1]
15649 = ((-0.55)
15650 + (((-1.0)
15651 * (0.3)
15652 * cj9))
15653 + (((-1.0)
15654 * (0.045)
15655 * sj9))
15656 + x1345);
15657 evalcond
15658 [2]
15659 = ((-0.316735294117647)
15660 + (((-1.0)
15661 * (0.588235294117647)
15662 * pp))
15663 + (((-1.0)
15664 * (0.108264705882353)
15665 * cj9))
15666 + x1345);
15667 evalcond
15668 [3]
15669 = ((-0.2125)
15670 + (((-1.1)
15671 * x1344))
15672 + (((-1.1)
15673 * x1343))
15674 + (((0.09)
15675 * x1340))
15676 + (((-0.09)
15677 * x1342))
15678 + (((-1.0)
15679 * (1.0)
15680 * pp)));
15681 if (IKabs(
15682 evalcond
15683 [0])
15684 > IKFAST_EVALCOND_THRESH
15685 || IKabs(
15686 evalcond
15687 [1])
15688 > IKFAST_EVALCOND_THRESH
15689 || IKabs(
15690 evalcond
15691 [2])
15692 > IKFAST_EVALCOND_THRESH
15693 || IKabs(
15694 evalcond
15695 [3])
15696 > IKFAST_EVALCOND_THRESH)
15697 {
15698 continue;
15699 }
15700 }
15701
15702 rotationfunction0(
15703 solutions);
15704 }
15705 }
15706 }
15707 }
15708 }
15709 } while (0);
15710 if (bgotonextstatement)
15711 {
15712 bool
15713 bgotonextstatement
15714 = true;
15715 do
15716 {
15717 IkReal x1346
15718 = (cj6 * pz);
15719 IkReal x1347
15720 = ((1.32323529411765)
15721 * cj9);
15722 IkReal x1348
15723 = ((3.92156862745098)
15724 * pp);
15725 evalcond[0]
15726 = ((IKabs(py))
15727 + (IKabs(
15728 px)));
15729 evalcond[1]
15730 = ((-0.55)
15731 + (((-1.0)
15732 * (0.3)
15733 * cj9))
15734 + (((-1.0)
15735 * (0.045)
15736 * sj9))
15737 + x1346);
15738 evalcond[2]
15739 = ((1.51009803921569)
15740 + (((-1.0)
15741 * x1348))
15742 + x1347);
15743 evalcond[3]
15744 = (pz * sj6);
15745 evalcond[4]
15746 = ((-1.51009803921569)
15747 + (((-1.0)
15748 * x1347))
15749 + x1348);
15750 evalcond[5]
15751 = ((((0.108264705882353)
15752 * cj9
15753 * sj6))
15754 + (((0.588235294117647)
15755 * pp
15756 * sj6))
15757 + (((0.316735294117647)
15758 * sj6)));
15759 evalcond[6]
15760 = ((-0.2125)
15761 + (((1.1)
15762 * x1346))
15763 + (((-1.0)
15764 * (1.0)
15765 * pp)));
15766 if (IKabs(evalcond
15767 [0])
15768 < 0.0000010000000000
15769 && IKabs(
15770 evalcond
15771 [1])
15772 < 0.0000010000000000
15773 && IKabs(
15774 evalcond
15775 [2])
15776 < 0.0000010000000000
15777 && IKabs(
15778 evalcond
15779 [3])
15780 < 0.0000010000000000
15781 && IKabs(
15782 evalcond
15783 [4])
15784 < 0.0000010000000000
15785 && IKabs(
15786 evalcond
15787 [5])
15788 < 0.0000010000000000
15789 && IKabs(
15790 evalcond
15791 [6])
15792 < 0.0000010000000000)
15793 {
15794 bgotonextstatement
15795 = false;
15796 {
15797 IkReal j4array
15798 [4],
15799 cj4array
15800 [4],
15801 sj4array
15802 [4];
15803 bool
15804 j4valid[4]
15805 = {false};
15806 _nj4 = 4;
15807 j4array[0]
15808 = 0;
15809 sj4array[0]
15810 = IKsin(
15811 j4array
15812 [0]);
15813 cj4array[0]
15814 = IKcos(
15815 j4array
15816 [0]);
15817 j4array[1]
15818 = 1.5707963267949;
15819 sj4array[1]
15820 = IKsin(
15821 j4array
15822 [1]);
15823 cj4array[1]
15824 = IKcos(
15825 j4array
15826 [1]);
15827 j4array[2]
15828 = 3.14159265358979;
15829 sj4array[2]
15830 = IKsin(
15831 j4array
15832 [2]);
15833 cj4array[2]
15834 = IKcos(
15835 j4array
15836 [2]);
15837 j4array[3]
15838 = -1.5707963267949;
15839 sj4array[3]
15840 = IKsin(
15841 j4array
15842 [3]);
15843 cj4array[3]
15844 = IKcos(
15845 j4array
15846 [3]);
15847 if (j4array[0]
15848 > IKPI)
15849 {
15850 j4array[0]
15851 -= IK2PI;
15852 }
15853 else if (
15854 j4array[0]
15855 < -IKPI)
15856 {
15857 j4array[0]
15858 += IK2PI;
15859 }
15860 j4valid[0]
15861 = true;
15862 if (j4array[1]
15863 > IKPI)
15864 {
15865 j4array[1]
15866 -= IK2PI;
15867 }
15868 else if (
15869 j4array[1]
15870 < -IKPI)
15871 {
15872 j4array[1]
15873 += IK2PI;
15874 }
15875 j4valid[1]
15876 = true;
15877 if (j4array[2]
15878 > IKPI)
15879 {
15880 j4array[2]
15881 -= IK2PI;
15882 }
15883 else if (
15884 j4array[2]
15885 < -IKPI)
15886 {
15887 j4array[2]
15888 += IK2PI;
15889 }
15890 j4valid[2]
15891 = true;
15892 if (j4array[3]
15893 > IKPI)
15894 {
15895 j4array[3]
15896 -= IK2PI;
15897 }
15898 else if (
15899 j4array[3]
15900 < -IKPI)
15901 {
15902 j4array[3]
15903 += IK2PI;
15904 }
15905 j4valid[3]
15906 = true;
15907 for (int ij4
15908 = 0;
15909 ij4 < 4;
15910 ++ij4)
15911 {
15912 if (!j4valid
15913 [ij4])
15914 {
15915 continue;
15916 }
15917 _ij4[0]
15918 = ij4;
15919 _ij4[1]
15920 = -1;
15921 for (
15922 int iij4
15923 = ij4
15924 + 1;
15925 iij4
15926 < 4;
15927 ++iij4)
15928 {
15929 if (j4valid
15930 [iij4]
15931 && IKabs(
15932 cj4array
15933 [ij4]
15934 - cj4array
15935 [iij4])
15936 < IKFAST_SOLUTION_THRESH
15937 && IKabs(
15938 sj4array
15939 [ij4]
15940 - sj4array
15941 [iij4])
15942 < IKFAST_SOLUTION_THRESH)
15943 {
15944 j4valid
15945 [iij4]
15946 = false;
15947 _ij4[1]
15948 = iij4;
15949 break;
15950 }
15951 }
15952 j4 = j4array
15953 [ij4];
15954 cj4 = cj4array
15955 [ij4];
15956 sj4 = sj4array
15957 [ij4];
15958
15959 rotationfunction0(
15960 solutions);
15961 }
15962 }
15963 }
15964 } while (0);
15965 if (bgotonextstatement)
15966 {
15967 bool
15968 bgotonextstatement
15969 = true;
15970 do
15971 {
15972 if (1)
15973 {
15974 bgotonextstatement
15975 = false;
15976 continue; // branch miss [j4]
15977 }
15978 } while (0);
15979 if (bgotonextstatement)
15980 {
15981 }
15982 }
15983 }
15984 }
15985 }
15986 }
15987 else
15988 {
15989 {
15990 IkReal j4array[1],
15991 cj4array[1],
15992 sj4array[1];
15993 bool j4valid[1]
15994 = {false};
15995 _nj4 = 1;
15996 IkReal x1349
15997 = ((1.32323529411765)
15998 * cj9);
15999 IkReal x1350
16000 = ((3.92156862745098)
16001 * pp);
16002 IkReal x1351
16003 = ((0.316735294117647)
16004 * sj6);
16005 IkReal x1352
16006 = ((0.108264705882353)
16007 * cj9 * sj6);
16008 IkReal x1353
16009 = ((0.588235294117647)
16010 * pp * sj6);
16011 CheckValue<IkReal> x1354 = IKatan2WithCheck(
16012 IkReal((
16013 ((py * x1353))
16014 + (((-1.0)
16015 * (1.51009803921569)
16016 * px))
16017 + (((-1.0) * px
16018 * x1349))
16019 + ((py * x1352))
16020 + ((px * x1350))
16021 + ((py
16022 * x1351)))),
16023 (((px * x1351))
16024 + ((px * x1353))
16025 + (((1.51009803921569)
16026 * py))
16027 + ((py * x1349))
16028 + ((px * x1352))
16029 + (((-1.0) * py
16030 * x1350))),
16031 IKFAST_ATAN2_MAGTHRESH);
16032 if (!x1354.valid)
16033 {
16034 continue;
16035 }
16036 CheckValue<IkReal> x1355
16037 = IKPowWithIntegerCheck(
16038 IKsign((
16039 pp
16040 + (((-1.0)
16041 * (1.0)
16042 * (pz
16043 * pz))))),
16044 -1);
16045 if (!x1355.valid)
16046 {
16047 continue;
16048 }
16049 j4array[0]
16050 = ((-1.5707963267949)
16051 + (x1354.value)
16052 + (((1.5707963267949)
16053 * (x1355
16054 .value))));
16055 sj4array[0]
16056 = IKsin(j4array[0]);
16057 cj4array[0]
16058 = IKcos(j4array[0]);
16059 if (j4array[0] > IKPI)
16060 {
16061 j4array[0] -= IK2PI;
16062 }
16063 else if (
16064 j4array[0] < -IKPI)
16065 {
16066 j4array[0] += IK2PI;
16067 }
16068 j4valid[0] = true;
16069 for (int ij4 = 0;
16070 ij4 < 1;
16071 ++ij4)
16072 {
16073 if (!j4valid[ij4])
16074 {
16075 continue;
16076 }
16077 _ij4[0] = ij4;
16078 _ij4[1] = -1;
16079 for (int iij4
16080 = ij4 + 1;
16081 iij4 < 1;
16082 ++iij4)
16083 {
16084 if (j4valid[iij4]
16085 && IKabs(
16086 cj4array
16087 [ij4]
16088 - cj4array
16089 [iij4])
16090 < IKFAST_SOLUTION_THRESH
16091 && IKabs(
16092 sj4array
16093 [ij4]
16094 - sj4array
16095 [iij4])
16096 < IKFAST_SOLUTION_THRESH)
16097 {
16098 j4valid[iij4]
16099 = false;
16100 _ij4[1] = iij4;
16101 break;
16102 }
16103 }
16104 j4 = j4array[ij4];
16105 cj4 = cj4array[ij4];
16106 sj4 = sj4array[ij4];
16107 {
16108 IkReal evalcond[5];
16109 IkReal x1356
16110 = IKcos(j4);
16111 IkReal x1357
16112 = ((1.0)
16113 * x1356);
16114 IkReal x1358
16115 = (px * x1357);
16116 IkReal x1359
16117 = IKsin(j4);
16118 IkReal x1360
16119 = (py * x1359);
16120 IkReal x1361
16121 = ((1.0)
16122 * x1360);
16123 IkReal x1362
16124 = (px * x1359);
16125 IkReal x1363
16126 = (cj6 * pz);
16127 IkReal x1364
16128 = (px * sj6
16129 * x1356);
16130 IkReal x1365
16131 = (sj6 * x1360);
16132 evalcond[0]
16133 = ((((-1.0)
16134 * cj6
16135 * x1361))
16136 + (((-1.0)
16137 * cj6
16138 * x1358))
16139 + ((pz
16140 * sj6)));
16141 evalcond[1]
16142 = ((1.51009803921569)
16143 + (((-1.0)
16144 * py
16145 * x1357))
16146 + (((-1.0)
16147 * (3.92156862745098)
16148 * pp))
16149 + x1362
16150 + (((1.32323529411765)
16151 * cj9)));
16152 evalcond[2]
16153 = ((-0.55)
16154 + (((-1.0)
16155 * (0.3)
16156 * cj9))
16157 + x1363
16158 + x1365
16159 + x1364
16160 + (((-1.0)
16161 * (0.045)
16162 * sj9)));
16163 evalcond[3]
16164 = ((((0.108264705882353)
16165 * cj9
16166 * sj6))
16167 + (((0.588235294117647)
16168 * pp
16169 * sj6))
16170 + (((-1.0)
16171 * x1361))
16172 + (((-1.0)
16173 * x1358))
16174 + (((0.316735294117647)
16175 * sj6)));
16176 evalcond[4]
16177 = ((-0.2125)
16178 + (((1.1)
16179 * x1364))
16180 + (((1.1)
16181 * x1365))
16182 + (((-0.09)
16183 * py
16184 * x1356))
16185 + (((-1.0)
16186 * (1.0)
16187 * pp))
16188 + (((1.1)
16189 * x1363))
16190 + (((0.09)
16191 * x1362)));
16192 if (IKabs(
16193 evalcond[0])
16194 > IKFAST_EVALCOND_THRESH
16195 || IKabs(
16196 evalcond
16197 [1])
16198 > IKFAST_EVALCOND_THRESH
16199 || IKabs(
16200 evalcond
16201 [2])
16202 > IKFAST_EVALCOND_THRESH
16203 || IKabs(
16204 evalcond
16205 [3])
16206 > IKFAST_EVALCOND_THRESH
16207 || IKabs(
16208 evalcond
16209 [4])
16210 > IKFAST_EVALCOND_THRESH)
16211 {
16212 continue;
16213 }
16214 }
16215
16216 rotationfunction0(
16217 solutions);
16218 }
16219 }
16220 }
16221 }
16222 }
16223 else
16224 {
16225 {
16226 IkReal j4array[1],
16227 cj4array[1],
16228 sj4array[1];
16229 bool j4valid[1] = {false};
16230 _nj4 = 1;
16231 IkReal x1366 = (cj6 * pp);
16232 IkReal x1367
16233 = ((0.2125) * cj6);
16234 IkReal x1368 = ((1.1) * pz);
16235 IkReal x1369
16236 = ((0.09) * pz * sj6);
16237 CheckValue<IkReal> x1370
16238 = IKatan2WithCheck(
16239 IkReal((
16240 ((px * x1368))
16241 + (((-1.0) * px
16242 * x1367))
16243 + (((-1.0) * px
16244 * x1366))
16245 + (((-1.0) * py
16246 * x1369)))),
16247 ((((-1.0) * px
16248 * x1369))
16249 + (((-1.0) * py
16250 * x1368))
16251 + ((py * x1367))
16252 + ((py * x1366))),
16253 IKFAST_ATAN2_MAGTHRESH);
16254 if (!x1370.valid)
16255 {
16256 continue;
16257 }
16258 CheckValue<IkReal> x1371
16259 = IKPowWithIntegerCheck(
16260 IKsign((
16261 (((0.09) * cj6
16262 * (pz * pz)))
16263 + (((-0.09)
16264 * x1366)))),
16265 -1);
16266 if (!x1371.valid)
16267 {
16268 continue;
16269 }
16270 j4array[0]
16271 = ((-1.5707963267949)
16272 + (x1370.value)
16273 + (((1.5707963267949)
16274 * (x1371
16275 .value))));
16276 sj4array[0]
16277 = IKsin(j4array[0]);
16278 cj4array[0]
16279 = IKcos(j4array[0]);
16280 if (j4array[0] > IKPI)
16281 {
16282 j4array[0] -= IK2PI;
16283 }
16284 else if (j4array[0] < -IKPI)
16285 {
16286 j4array[0] += IK2PI;
16287 }
16288 j4valid[0] = true;
16289 for (int ij4 = 0; ij4 < 1;
16290 ++ij4)
16291 {
16292 if (!j4valid[ij4])
16293 {
16294 continue;
16295 }
16296 _ij4[0] = ij4;
16297 _ij4[1] = -1;
16298 for (int iij4 = ij4 + 1;
16299 iij4 < 1;
16300 ++iij4)
16301 {
16302 if (j4valid[iij4]
16303 && IKabs(
16304 cj4array[ij4]
16305 - cj4array
16306 [iij4])
16307 < IKFAST_SOLUTION_THRESH
16308 && IKabs(
16309 sj4array[ij4]
16310 - sj4array
16311 [iij4])
16312 < IKFAST_SOLUTION_THRESH)
16313 {
16314 j4valid[iij4] = false;
16315 _ij4[1] = iij4;
16316 break;
16317 }
16318 }
16319 j4 = j4array[ij4];
16320 cj4 = cj4array[ij4];
16321 sj4 = sj4array[ij4];
16322 {
16323 IkReal evalcond[5];
16324 IkReal x1372
16325 = IKcos(j4);
16326 IkReal x1373
16327 = ((1.0) * x1372);
16328 IkReal x1374
16329 = (px * x1373);
16330 IkReal x1375
16331 = IKsin(j4);
16332 IkReal x1376
16333 = (py * x1375);
16334 IkReal x1377
16335 = ((1.0) * x1376);
16336 IkReal x1378
16337 = (px * x1375);
16338 IkReal x1379
16339 = (cj6 * pz);
16340 IkReal x1380
16341 = (px * sj6
16342 * x1372);
16343 IkReal x1381
16344 = (sj6 * x1376);
16345 evalcond[0]
16346 = ((((-1.0) * cj6
16347 * x1374))
16348 + ((pz * sj6))
16349 + (((-1.0) * cj6
16350 * x1377)));
16351 evalcond[1]
16352 = ((1.51009803921569)
16353 + x1378
16354 + (((-1.0)
16355 * (3.92156862745098)
16356 * pp))
16357 + (((-1.0) * py
16358 * x1373))
16359 + (((1.32323529411765)
16360 * cj9)));
16361 evalcond[2]
16362 = ((-0.55) + x1379
16363 + x1380 + x1381
16364 + (((-1.0)
16365 * (0.3)
16366 * cj9))
16367 + (((-1.0)
16368 * (0.045)
16369 * sj9)));
16370 evalcond[3]
16371 = ((((0.108264705882353)
16372 * cj9 * sj6))
16373 + (((-1.0)
16374 * x1374))
16375 + (((0.588235294117647)
16376 * pp * sj6))
16377 + (((0.316735294117647)
16378 * sj6))
16379 + (((-1.0)
16380 * x1377)));
16381 evalcond[4]
16382 = ((-0.2125)
16383 + (((-0.09) * py
16384 * x1372))
16385 + (((0.09)
16386 * x1378))
16387 + (((1.1)
16388 * x1379))
16389 + (((1.1)
16390 * x1381))
16391 + (((1.1)
16392 * x1380))
16393 + (((-1.0)
16394 * (1.0)
16395 * pp)));
16396 if (IKabs(evalcond[0])
16397 > IKFAST_EVALCOND_THRESH
16398 || IKabs(
16399 evalcond[1])
16400 > IKFAST_EVALCOND_THRESH
16401 || IKabs(
16402 evalcond[2])
16403 > IKFAST_EVALCOND_THRESH
16404 || IKabs(
16405 evalcond[3])
16406 > IKFAST_EVALCOND_THRESH
16407 || IKabs(
16408 evalcond[4])
16409 > IKFAST_EVALCOND_THRESH)
16410 {
16411 continue;
16412 }
16413 }
16414
16415 rotationfunction0(
16416 solutions);
16417 }
16418 }
16419 }
16420 }
16421 }
16422 else
16423 {
16424 {
16425 IkReal j4array[1], cj4array[1],
16426 sj4array[1];
16427 bool j4valid[1] = {false};
16428 _nj4 = 1;
16429 IkReal x1382
16430 = ((1.51009803921569)
16431 * cj6);
16432 IkReal x1383 = (pz * sj6);
16433 IkReal x1384
16434 = ((1.32323529411765) * cj6
16435 * cj9);
16436 IkReal x1385
16437 = ((3.92156862745098) * cj6
16438 * pp);
16439 CheckValue<IkReal> x1386
16440 = IKPowWithIntegerCheck(
16441 IKsign(
16442 ((((-1.0) * (1.0)
16443 * cj6
16444 * (pz * pz)))
16445 + ((cj6 * pp)))),
16446 -1);
16447 if (!x1386.valid)
16448 {
16449 continue;
16450 }
16451 CheckValue<IkReal> x1387
16452 = IKatan2WithCheck(
16453 IkReal(
16454 ((((-1.0) * px
16455 * x1384))
16456 + (((-1.0) * px
16457 * x1382))
16458 + ((py * x1383))
16459 + ((px * x1385)))),
16460 ((((-1.0) * py * x1385))
16461 + ((py * x1384))
16462 + ((px * x1383))
16463 + ((py * x1382))),
16464 IKFAST_ATAN2_MAGTHRESH);
16465 if (!x1387.valid)
16466 {
16467 continue;
16468 }
16469 j4array[0]
16470 = ((-1.5707963267949)
16471 + (((1.5707963267949)
16472 * (x1386.value)))
16473 + (x1387.value));
16474 sj4array[0] = IKsin(j4array[0]);
16475 cj4array[0] = IKcos(j4array[0]);
16476 if (j4array[0] > IKPI)
16477 {
16478 j4array[0] -= IK2PI;
16479 }
16480 else if (j4array[0] < -IKPI)
16481 {
16482 j4array[0] += IK2PI;
16483 }
16484 j4valid[0] = true;
16485 for (int ij4 = 0; ij4 < 1;
16486 ++ij4)
16487 {
16488 if (!j4valid[ij4])
16489 {
16490 continue;
16491 }
16492 _ij4[0] = ij4;
16493 _ij4[1] = -1;
16494 for (int iij4 = ij4 + 1;
16495 iij4 < 1;
16496 ++iij4)
16497 {
16498 if (j4valid[iij4]
16499 && IKabs(
16500 cj4array[ij4]
16501 - cj4array[iij4])
16502 < IKFAST_SOLUTION_THRESH
16503 && IKabs(
16504 sj4array[ij4]
16505 - sj4array[iij4])
16506 < IKFAST_SOLUTION_THRESH)
16507 {
16508 j4valid[iij4] = false;
16509 _ij4[1] = iij4;
16510 break;
16511 }
16512 }
16513 j4 = j4array[ij4];
16514 cj4 = cj4array[ij4];
16515 sj4 = sj4array[ij4];
16516 {
16517 IkReal evalcond[5];
16518 IkReal x1388 = IKcos(j4);
16519 IkReal x1389
16520 = ((1.0) * x1388);
16521 IkReal x1390 = (px * x1389);
16522 IkReal x1391 = IKsin(j4);
16523 IkReal x1392 = (py * x1391);
16524 IkReal x1393
16525 = ((1.0) * x1392);
16526 IkReal x1394 = (px * x1391);
16527 IkReal x1395 = (cj6 * pz);
16528 IkReal x1396
16529 = (px * sj6 * x1388);
16530 IkReal x1397
16531 = (sj6 * x1392);
16532 evalcond[0]
16533 = ((((-1.0) * cj6
16534 * x1393))
16535 + ((pz * sj6))
16536 + (((-1.0) * cj6
16537 * x1390)));
16538 evalcond[1]
16539 = ((1.51009803921569)
16540 + (((-1.0) * py
16541 * x1389))
16542 + (((-1.0)
16543 * (3.92156862745098)
16544 * pp))
16545 + x1394
16546 + (((1.32323529411765)
16547 * cj9)));
16548 evalcond[2]
16549 = ((-0.55)
16550 + (((-1.0) * (0.3)
16551 * cj9))
16552 + x1397 + x1396
16553 + x1395
16554 + (((-1.0) * (0.045)
16555 * sj9)));
16556 evalcond[3]
16557 = ((((-1.0) * x1393))
16558 + (((0.108264705882353)
16559 * cj9 * sj6))
16560 + (((0.588235294117647)
16561 * pp * sj6))
16562 + (((0.316735294117647)
16563 * sj6))
16564 + (((-1.0)
16565 * x1390)));
16566 evalcond[4]
16567 = ((-0.2125)
16568 + (((1.1) * x1395))
16569 + (((0.09) * x1394))
16570 + (((-0.09) * py
16571 * x1388))
16572 + (((1.1) * x1396))
16573 + (((1.1) * x1397))
16574 + (((-1.0) * (1.0)
16575 * pp)));
16576 if (IKabs(evalcond[0])
16577 > IKFAST_EVALCOND_THRESH
16578 || IKabs(evalcond[1])
16579 > IKFAST_EVALCOND_THRESH
16580 || IKabs(evalcond[2])
16581 > IKFAST_EVALCOND_THRESH
16582 || IKabs(evalcond[3])
16583 > IKFAST_EVALCOND_THRESH
16584 || IKabs(evalcond[4])
16585 > IKFAST_EVALCOND_THRESH)
16586 {
16587 continue;
16588 }
16589 }
16590
16591 rotationfunction0(solutions);
16592 }
16593 }
16594 }
16595 }
16596 }
16597 } while (0);
16598 if (bgotonextstatement)
16599 {
16600 bool bgotonextstatement = true;
16601 do
16602 {
16603 IkReal x1398
16604 = ((-0.55) + pz
16605 + (((-1.0) * (0.3) * cj9))
16606 + (((-1.0) * (0.045) * sj9)));
16607 evalcond[0]
16608 = ((-3.14159265358979)
16609 + (IKfmod(
16610 ((3.14159265358979)
16611 + (IKabs(j6))),
16612 6.28318530717959)));
16613 evalcond[1]
16614 = ((0.39655) + (((0.0765) * sj9))
16615 + (((0.32595) * cj9))
16616 + (((-1.0) * (1.0) * pp)));
16617 evalcond[2] = x1398;
16618 evalcond[3] = x1398;
16619 if (IKabs(evalcond[0])
16620 < 0.0000010000000000
16621 && IKabs(evalcond[1])
16622 < 0.0000010000000000
16623 && IKabs(evalcond[2])
16624 < 0.0000010000000000
16625 && IKabs(evalcond[3])
16626 < 0.0000010000000000)
16627 {
16628 bgotonextstatement = false;
16629 {
16630 IkReal j4eval[3];
16631 sj6 = 0;
16632 cj6 = 1.0;
16633 j6 = 0;
16634 IkReal x1399
16635 = (pp
16636 + (((-1.0) * (1.0)
16637 * (pz * pz))));
16638 IkReal x1400
16639 = ((1.51009803921569) * cj8);
16640 IkReal x1401
16641 = ((1.51009803921569) * sj8);
16642 IkReal x1402
16643 = ((1.32323529411765) * cj8
16644 * cj9);
16645 IkReal x1403
16646 = ((3.92156862745098) * cj8
16647 * pp);
16648 IkReal x1404
16649 = ((1.32323529411765) * cj9
16650 * sj8);
16651 IkReal x1405
16652 = ((3.92156862745098) * pp
16653 * sj8);
16654 j4eval[0] = x1399;
16655 j4eval[1]
16656 = ((IKabs(
16657 (((py * x1403))
16658 + ((px * x1401))
16659 + (((-1.0) * py
16660 * x1402))
16661 + ((px * x1404))
16662 + (((-1.0) * px
16663 * x1405))
16664 + (((-1.0) * py
16665 * x1400)))))
16666 + (IKabs((
16667 (((-1.0) * px
16668 * x1400))
16669 + (((-1.0) * py
16670 * x1404))
16671 + (((-1.0) * px
16672 * x1402))
16673 + (((-1.0) * py
16674 * x1401))
16675 + ((py * x1405))
16676 + ((px * x1403))))));
16677 j4eval[2] = IKsign(x1399);
16678 if (IKabs(j4eval[0])
16679 < 0.0000010000000000
16680 || IKabs(j4eval[1])
16681 < 0.0000010000000000
16682 || IKabs(j4eval[2])
16683 < 0.0000010000000000)
16684 {
16685 {
16686 IkReal j4eval[2];
16687 sj6 = 0;
16688 cj6 = 1.0;
16689 j6 = 0;
16690 IkReal x1406
16691 = (((cj8 * (pz * pz)))
16692 + (((-1.0) * (1.0)
16693 * cj8 * pp)));
16694 j4eval[0] = x1406;
16695 j4eval[1] = IKsign(x1406);
16696 if (IKabs(j4eval[0])
16697 < 0.0000010000000000
16698 || IKabs(j4eval[1])
16699 < 0.0000010000000000)
16700 {
16701 {
16702 IkReal j4eval[2];
16703 sj6 = 0;
16704 cj6 = 1.0;
16705 j6 = 0;
16706 IkReal x1407
16707 = ((((-1.0) * (1.0)
16708 * sj8
16709 * (pz * pz)))
16710 + ((pp * sj8)));
16711 j4eval[0] = x1407;
16712 j4eval[1] = IKsign(x1407);
16713 if (IKabs(j4eval[0])
16714 < 0.0000010000000000
16715 || IKabs(j4eval[1])
16716 < 0.0000010000000000)
16717 {
16718 {
16719 IkReal evalcond[6];
16720 bool
16721 bgotonextstatement
16722 = true;
16723 do
16724 {
16725 evalcond[0]
16726 = ((-3.14159265358979)
16727 + (IKfmod(
16728 ((3.14159265358979)
16729 + (IKabs(
16730 j8))),
16731 6.28318530717959)));
16732 if (IKabs(
16733 evalcond[0])
16734 < 0.0000010000000000)
16735 {
16736 bgotonextstatement
16737 = false;
16738 {
16739 IkReal
16740 j4eval[3];
16741 sj6 = 0;
16742 cj6 = 1.0;
16743 j6 = 0;
16744 sj8 = 0;
16745 cj8 = 1.0;
16746 j8 = 0;
16747 IkReal x1408
16748 = (pp
16749 + (((-1.0)
16750 * (1.0)
16751 * (pz
16752 * pz))));
16753 IkReal x1409
16754 = ((13497.0)
16755 * cj9);
16756 IkReal x1410
16757 = ((40000.0)
16758 * pp);
16759 j4eval[0]
16760 = x1408;
16761 j4eval[1]
16762 = ((IKabs((
16763 ((py
16764 * x1410))
16765 + (((-1.0)
16766 * (15403.0)
16767 * py))
16768 + (((-1.0)
16769 * py
16770 * x1409)))))
16771 + (IKabs((
16772 (((-1.0)
16773 * px
16774 * x1409))
16775 + (((-1.0)
16776 * (15403.0)
16777 * px))
16778 + ((px
16779 * x1410))))));
16780 j4eval[2]
16781 = IKsign(
16782 x1408);
16783 if (IKabs(
16784 j4eval
16785 [0])
16786 < 0.0000010000000000
16787 || IKabs(
16788 j4eval
16789 [1])
16790 < 0.0000010000000000
16791 || IKabs(
16792 j4eval
16793 [2])
16794 < 0.0000010000000000)
16795 {
16796 {
16797 IkReal
16798 j4eval
16799 [3];
16800 sj6 = 0;
16801 cj6 = 1.0;
16802 j6 = 0;
16803 sj8 = 0;
16804 cj8 = 1.0;
16805 j8 = 0;
16806 IkReal x1411
16807 = pz
16808 * pz;
16809 IkReal x1412
16810 = ((80.0)
16811 * pp);
16812 IkReal x1413
16813 = ((88.0)
16814 * pz);
16815 j4eval[0]
16816 = ((((-1.0)
16817 * x1411))
16818 + pp);
16819 j4eval[1]
16820 = ((IKabs((
16821 (((-1.0)
16822 * px
16823 * x1413))
16824 + ((px
16825 * x1412))
16826 + (((17.0)
16827 * px)))))
16828 + (IKabs((
16829 ((py
16830 * x1412))
16831 + (((17.0)
16832 * py))
16833 + (((-1.0)
16834 * py
16835 * x1413))))));
16836 j4eval[2] = IKsign((
16837 (((9.0)
16838 * pp))
16839 + (((-9.0)
16840 * x1411))));
16841 if (IKabs(
16842 j4eval
16843 [0])
16844 < 0.0000010000000000
16845 || IKabs(
16846 j4eval
16847 [1])
16848 < 0.0000010000000000
16849 || IKabs(
16850 j4eval
16851 [2])
16852 < 0.0000010000000000)
16853 {
16854 {
16855 IkReal evalcond
16856 [5];
16857 bool
16858 bgotonextstatement
16859 = true;
16860 do
16861 {
16862 IkReal
16863 x1414
16864 = ((-1.51009803921569)
16865 + (((-1.0)
16866 * (1.32323529411765)
16867 * cj9))
16868 + (((3.92156862745098)
16869 * pp)));
16870 evalcond
16871 [0]
16872 = ((IKabs(
16873 py))
16874 + (IKabs(
16875 px)));
16876 evalcond
16877 [1]
16878 = 0;
16879 evalcond
16880 [2]
16881 = x1414;
16882 evalcond
16883 [3]
16884 = x1414;
16885 evalcond
16886 [4]
16887 = ((-0.2125)
16888 + (((1.1)
16889 * pz))
16890 + (((-1.0)
16891 * (1.0)
16892 * pp)));
16893 if (IKabs(
16894 evalcond
16895 [0])
16896 < 0.0000010000000000
16897 && IKabs(
16898 evalcond
16899 [1])
16900 < 0.0000010000000000
16901 && IKabs(
16902 evalcond
16903 [2])
16904 < 0.0000010000000000
16905 && IKabs(
16906 evalcond
16907 [3])
16908 < 0.0000010000000000
16909 && IKabs(
16910 evalcond
16911 [4])
16912 < 0.0000010000000000)
16913 {
16914 bgotonextstatement
16915 = false;
16916 {
16917 IkReal j4array
16918 [4],
16919 cj4array
16920 [4],
16921 sj4array
16922 [4];
16923 bool j4valid
16924 [4]
16925 = {false};
16926 _nj4
16927 = 4;
16928 j4array
16929 [0]
16930 = 0;
16931 sj4array
16932 [0]
16933 = IKsin(
16934 j4array
16935 [0]);
16936 cj4array
16937 [0]
16938 = IKcos(
16939 j4array
16940 [0]);
16941 j4array
16942 [1]
16943 = 1.5707963267949;
16944 sj4array
16945 [1]
16946 = IKsin(
16947 j4array
16948 [1]);
16949 cj4array
16950 [1]
16951 = IKcos(
16952 j4array
16953 [1]);
16954 j4array
16955 [2]
16956 = 3.14159265358979;
16957 sj4array
16958 [2]
16959 = IKsin(
16960 j4array
16961 [2]);
16962 cj4array
16963 [2]
16964 = IKcos(
16965 j4array
16966 [2]);
16967 j4array
16968 [3]
16969 = -1.5707963267949;
16970 sj4array
16971 [3]
16972 = IKsin(
16973 j4array
16974 [3]);
16975 cj4array
16976 [3]
16977 = IKcos(
16978 j4array
16979 [3]);
16980 if (j4array
16981 [0]
16982 > IKPI)
16983 {
16984 j4array
16985 [0]
16986 -= IK2PI;
16987 }
16988 else if (
16989 j4array
16990 [0]
16991 < -IKPI)
16992 {
16993 j4array
16994 [0]
16995 += IK2PI;
16996 }
16997 j4valid
16998 [0]
16999 = true;
17000 if (j4array
17001 [1]
17002 > IKPI)
17003 {
17004 j4array
17005 [1]
17006 -= IK2PI;
17007 }
17008 else if (
17009 j4array
17010 [1]
17011 < -IKPI)
17012 {
17013 j4array
17014 [1]
17015 += IK2PI;
17016 }
17017 j4valid
17018 [1]
17019 = true;
17020 if (j4array
17021 [2]
17022 > IKPI)
17023 {
17024 j4array
17025 [2]
17026 -= IK2PI;
17027 }
17028 else if (
17029 j4array
17030 [2]
17031 < -IKPI)
17032 {
17033 j4array
17034 [2]
17035 += IK2PI;
17036 }
17037 j4valid
17038 [2]
17039 = true;
17040 if (j4array
17041 [3]
17042 > IKPI)
17043 {
17044 j4array
17045 [3]
17046 -= IK2PI;
17047 }
17048 else if (
17049 j4array
17050 [3]
17051 < -IKPI)
17052 {
17053 j4array
17054 [3]
17055 += IK2PI;
17056 }
17057 j4valid
17058 [3]
17059 = true;
17060 for (
17061 int ij4
17062 = 0;
17063 ij4
17064 < 4;
17065 ++ij4)
17066 {
17067 if (!j4valid
17068 [ij4])
17069 {
17070 continue;
17071 }
17072 _ij4[0]
17073 = ij4;
17074 _ij4[1]
17075 = -1;
17076 for (
17077 int iij4
17078 = ij4
17079 + 1;
17080 iij4
17081 < 4;
17082 ++iij4)
17083 {
17084 if (j4valid
17085 [iij4]
17086 && IKabs(
17087 cj4array
17088 [ij4]
17089 - cj4array
17090 [iij4])
17091 < IKFAST_SOLUTION_THRESH
17092 && IKabs(
17093 sj4array
17094 [ij4]
17095 - sj4array
17096 [iij4])
17097 < IKFAST_SOLUTION_THRESH)
17098 {
17099 j4valid
17100 [iij4]
17101 = false;
17102 _ij4[1]
17103 = iij4;
17104 break;
17105 }
17106 }
17107 j4 = j4array
17108 [ij4];
17109 cj4 = cj4array
17110 [ij4];
17111 sj4 = sj4array
17112 [ij4];
17113
17114 rotationfunction0(
17115 solutions);
17116 }
17117 }
17118 }
17119 } while (
17120 0);
17121 if (bgotonextstatement)
17122 {
17123 bool
17124 bgotonextstatement
17125 = true;
17126 do
17127 {
17128 if (1)
17129 {
17130 bgotonextstatement
17131 = false;
17132 continue; // branch miss [j4]
17133 }
17134 } while (
17135 0);
17136 if (bgotonextstatement)
17137 {
17138 }
17139 }
17140 }
17141 }
17142 else
17143 {
17144 {
17145 IkReal j4array
17146 [1],
17147 cj4array
17148 [1],
17149 sj4array
17150 [1];
17151 bool j4valid
17152 [1]
17153 = {false};
17154 _nj4
17155 = 1;
17156 IkReal
17157 x1415
17158 = ((100.0)
17159 * pp);
17160 IkReal
17161 x1416
17162 = ((110.0)
17163 * pz);
17164 CheckValue<IkReal> x1417 = IKPowWithIntegerCheck(
17165 IKsign((
17166 (((-1.0)
17167 * (9.0)
17168 * (pz
17169 * pz)))
17170 + (((9.0)
17171 * pp)))),
17172 -1);
17173 if (!x1417
17174 .valid)
17175 {
17176 continue;
17177 }
17178 CheckValue<IkReal> x1418 = IKatan2WithCheck(
17179 IkReal((
17180 ((py
17181 * x1415))
17182 + (((21.25)
17183 * py))
17184 + (((-1.0)
17185 * py
17186 * x1416)))),
17187 (((px
17188 * x1415))
17189 + (((-1.0)
17190 * px
17191 * x1416))
17192 + (((21.25)
17193 * px))),
17194 IKFAST_ATAN2_MAGTHRESH);
17195 if (!x1418
17196 .valid)
17197 {
17198 continue;
17199 }
17200 j4array
17201 [0]
17202 = ((-1.5707963267949)
17203 + (((1.5707963267949)
17204 * (x1417
17205 .value)))
17206 + (x1418
17207 .value));
17208 sj4array
17209 [0]
17210 = IKsin(
17211 j4array
17212 [0]);
17213 cj4array
17214 [0]
17215 = IKcos(
17216 j4array
17217 [0]);
17218 if (j4array
17219 [0]
17220 > IKPI)
17221 {
17222 j4array
17223 [0]
17224 -= IK2PI;
17225 }
17226 else if (
17227 j4array
17228 [0]
17229 < -IKPI)
17230 {
17231 j4array
17232 [0]
17233 += IK2PI;
17234 }
17235 j4valid
17236 [0]
17237 = true;
17238 for (
17239 int ij4
17240 = 0;
17241 ij4
17242 < 1;
17243 ++ij4)
17244 {
17245 if (!j4valid
17246 [ij4])
17247 {
17248 continue;
17249 }
17250 _ij4[0]
17251 = ij4;
17252 _ij4[1]
17253 = -1;
17254 for (
17255 int iij4
17256 = ij4
17257 + 1;
17258 iij4
17259 < 1;
17260 ++iij4)
17261 {
17262 if (j4valid
17263 [iij4]
17264 && IKabs(
17265 cj4array
17266 [ij4]
17267 - cj4array
17268 [iij4])
17269 < IKFAST_SOLUTION_THRESH
17270 && IKabs(
17271 sj4array
17272 [ij4]
17273 - sj4array
17274 [iij4])
17275 < IKFAST_SOLUTION_THRESH)
17276 {
17277 j4valid
17278 [iij4]
17279 = false;
17280 _ij4[1]
17281 = iij4;
17282 break;
17283 }
17284 }
17285 j4 = j4array
17286 [ij4];
17287 cj4 = cj4array
17288 [ij4];
17289 sj4 = sj4array
17290 [ij4];
17291 {
17292 IkReal evalcond
17293 [3];
17294 IkReal
17295 x1419
17296 = IKsin(
17297 j4);
17298 IkReal
17299 x1420
17300 = IKcos(
17301 j4);
17302 IkReal
17303 x1421
17304 = (px
17305 * x1420);
17306 IkReal
17307 x1422
17308 = (py
17309 * x1419);
17310 evalcond
17311 [0]
17312 = ((((-1.0)
17313 * py
17314 * x1420))
17315 + ((px
17316 * x1419)));
17317 evalcond
17318 [1]
17319 = ((-1.51009803921569)
17320 + (((-1.0)
17321 * (1.32323529411765)
17322 * cj9))
17323 + (((-1.0)
17324 * x1421))
17325 + (((-1.0)
17326 * x1422))
17327 + (((3.92156862745098)
17328 * pp)));
17329 evalcond
17330 [2]
17331 = ((-0.2125)
17332 + (((1.1)
17333 * pz))
17334 + (((0.09)
17335 * x1421))
17336 + (((0.09)
17337 * x1422))
17338 + (((-1.0)
17339 * (1.0)
17340 * pp)));
17341 if (IKabs(
17342 evalcond
17343 [0])
17344 > IKFAST_EVALCOND_THRESH
17345 || IKabs(
17346 evalcond
17347 [1])
17348 > IKFAST_EVALCOND_THRESH
17349 || IKabs(
17350 evalcond
17351 [2])
17352 > IKFAST_EVALCOND_THRESH)
17353 {
17354 continue;
17355 }
17356 }
17357
17358 rotationfunction0(
17359 solutions);
17360 }
17361 }
17362 }
17363 }
17364 }
17365 else
17366 {
17367 {
17368 IkReal
17369 j4array
17370 [1],
17371 cj4array
17372 [1],
17373 sj4array
17374 [1];
17375 bool j4valid
17376 [1]
17377 = {false};
17378 _nj4 = 1;
17379 IkReal x1423
17380 = ((1.32323529411765)
17381 * cj9);
17382 IkReal x1424
17383 = ((3.92156862745098)
17384 * pp);
17385 CheckValue<IkReal> x1425 = IKatan2WithCheck(
17386 IkReal((
17387 ((py
17388 * x1424))
17389 + (((-1.0)
17390 * (1.51009803921569)
17391 * py))
17392 + (((-1.0)
17393 * py
17394 * x1423)))),
17395 ((((-1.0)
17396 * px
17397 * x1423))
17398 + (((-1.0)
17399 * (1.51009803921569)
17400 * px))
17401 + ((px
17402 * x1424))),
17403 IKFAST_ATAN2_MAGTHRESH);
17404 if (!x1425
17405 .valid)
17406 {
17407 continue;
17408 }
17409 CheckValue<IkReal> x1426 = IKPowWithIntegerCheck(
17410 IKsign((
17411 pp
17412 + (((-1.0)
17413 * (1.0)
17414 * (pz
17415 * pz))))),
17416 -1);
17417 if (!x1426
17418 .valid)
17419 {
17420 continue;
17421 }
17422 j4array[0]
17423 = ((-1.5707963267949)
17424 + (x1425
17425 .value)
17426 + (((1.5707963267949)
17427 * (x1426
17428 .value))));
17429 sj4array[0] = IKsin(
17430 j4array
17431 [0]);
17432 cj4array[0] = IKcos(
17433 j4array
17434 [0]);
17435 if (j4array
17436 [0]
17437 > IKPI)
17438 {
17439 j4array[0]
17440 -= IK2PI;
17441 }
17442 else if (
17443 j4array
17444 [0]
17445 < -IKPI)
17446 {
17447 j4array[0]
17448 += IK2PI;
17449 }
17450 j4valid[0]
17451 = true;
17452 for (int ij4
17453 = 0;
17454 ij4
17455 < 1;
17456 ++ij4)
17457 {
17458 if (!j4valid
17459 [ij4])
17460 {
17461 continue;
17462 }
17463 _ij4[0]
17464 = ij4;
17465 _ij4[1]
17466 = -1;
17467 for (
17468 int iij4
17469 = ij4
17470 + 1;
17471 iij4
17472 < 1;
17473 ++iij4)
17474 {
17475 if (j4valid
17476 [iij4]
17477 && IKabs(
17478 cj4array
17479 [ij4]
17480 - cj4array
17481 [iij4])
17482 < IKFAST_SOLUTION_THRESH
17483 && IKabs(
17484 sj4array
17485 [ij4]
17486 - sj4array
17487 [iij4])
17488 < IKFAST_SOLUTION_THRESH)
17489 {
17490 j4valid
17491 [iij4]
17492 = false;
17493 _ij4[1]
17494 = iij4;
17495 break;
17496 }
17497 }
17498 j4 = j4array
17499 [ij4];
17500 cj4 = cj4array
17501 [ij4];
17502 sj4 = sj4array
17503 [ij4];
17504 {
17505 IkReal evalcond
17506 [3];
17507 IkReal
17508 x1427
17509 = IKsin(
17510 j4);
17511 IkReal
17512 x1428
17513 = IKcos(
17514 j4);
17515 IkReal
17516 x1429
17517 = (px
17518 * x1428);
17519 IkReal
17520 x1430
17521 = (py
17522 * x1427);
17523 evalcond
17524 [0]
17525 = ((((-1.0)
17526 * py
17527 * x1428))
17528 + ((px
17529 * x1427)));
17530 evalcond
17531 [1]
17532 = ((-1.51009803921569)
17533 + (((-1.0)
17534 * (1.32323529411765)
17535 * cj9))
17536 + (((3.92156862745098)
17537 * pp))
17538 + (((-1.0)
17539 * x1430))
17540 + (((-1.0)
17541 * x1429)));
17542 evalcond
17543 [2]
17544 = ((-0.2125)
17545 + (((1.1)
17546 * pz))
17547 + (((0.09)
17548 * x1430))
17549 + (((-1.0)
17550 * (1.0)
17551 * pp))
17552 + (((0.09)
17553 * x1429)));
17554 if (IKabs(
17555 evalcond
17556 [0])
17557 > IKFAST_EVALCOND_THRESH
17558 || IKabs(
17559 evalcond
17560 [1])
17561 > IKFAST_EVALCOND_THRESH
17562 || IKabs(
17563 evalcond
17564 [2])
17565 > IKFAST_EVALCOND_THRESH)
17566 {
17567 continue;
17568 }
17569 }
17570
17571 rotationfunction0(
17572 solutions);
17573 }
17574 }
17575 }
17576 }
17577 }
17578 } while (0);
17579 if (bgotonextstatement)
17580 {
17581 bool
17582 bgotonextstatement
17583 = true;
17584 do
17585 {
17586 evalcond[0]
17587 = ((-3.14159265358979)
17588 + (IKfmod(
17589 ((3.14159265358979)
17590 + (IKabs((
17591 (-3.14159265358979)
17592 + j8)))),
17593 6.28318530717959)));
17594 if (IKabs(evalcond
17595 [0])
17596 < 0.0000010000000000)
17597 {
17598 bgotonextstatement
17599 = false;
17600 {
17601 IkReal
17602 j4eval[3];
17603 sj6 = 0;
17604 cj6 = 1.0;
17605 j6 = 0;
17606 sj8 = 0;
17607 cj8 = -1.0;
17608 j8 = 3.14159265358979;
17609 IkReal x1431
17610 = (pp
17611 + (((-1.0)
17612 * (1.0)
17613 * (pz
17614 * pz))));
17615 IkReal x1432
17616 = ((13497.0)
17617 * cj9);
17618 IkReal x1433
17619 = ((40000.0)
17620 * pp);
17621 j4eval[0]
17622 = x1431;
17623 j4eval[1]
17624 = ((IKabs((
17625 (((15403.0)
17626 * px))
17627 + ((px
17628 * x1432))
17629 + (((-1.0)
17630 * px
17631 * x1433)))))
17632 + (IKabs((
17633 (((15403.0)
17634 * py))
17635 + ((py
17636 * x1432))
17637 + (((-1.0)
17638 * py
17639 * x1433))))));
17640 j4eval[2]
17641 = IKsign(
17642 x1431);
17643 if (IKabs(
17644 j4eval
17645 [0])
17646 < 0.0000010000000000
17647 || IKabs(
17648 j4eval
17649 [1])
17650 < 0.0000010000000000
17651 || IKabs(
17652 j4eval
17653 [2])
17654 < 0.0000010000000000)
17655 {
17656 {
17657 IkReal j4eval
17658 [3];
17659 sj6 = 0;
17660 cj6 = 1.0;
17661 j6 = 0;
17662 sj8 = 0;
17663 cj8 = -1.0;
17664 j8 = 3.14159265358979;
17665 IkReal
17666 x1434
17667 = pz
17668 * pz;
17669 IkReal
17670 x1435
17671 = ((80.0)
17672 * pp);
17673 IkReal
17674 x1436
17675 = ((88.0)
17676 * pz);
17677 j4eval[0]
17678 = (pp
17679 + (((-1.0)
17680 * x1434)));
17681 j4eval[1] = IKsign((
17682 (((9.0)
17683 * pp))
17684 + (((-9.0)
17685 * x1434))));
17686 j4eval[2]
17687 = ((IKabs((
17688 (((-1.0)
17689 * (17.0)
17690 * px))
17691 + ((px
17692 * x1436))
17693 + (((-1.0)
17694 * px
17695 * x1435)))))
17696 + (IKabs((
17697 ((py
17698 * x1436))
17699 + (((-1.0)
17700 * py
17701 * x1435))
17702 + (((-1.0)
17703 * (17.0)
17704 * py))))));
17705 if (IKabs(
17706 j4eval
17707 [0])
17708 < 0.0000010000000000
17709 || IKabs(
17710 j4eval
17711 [1])
17712 < 0.0000010000000000
17713 || IKabs(
17714 j4eval
17715 [2])
17716 < 0.0000010000000000)
17717 {
17718 {
17719 IkReal evalcond
17720 [5];
17721 bool
17722 bgotonextstatement
17723 = true;
17724 do
17725 {
17726 IkReal
17727 x1437
17728 = ((1.32323529411765)
17729 * cj9);
17730 IkReal
17731 x1438
17732 = ((3.92156862745098)
17733 * pp);
17734 evalcond
17735 [0]
17736 = ((IKabs(
17737 py))
17738 + (IKabs(
17739 px)));
17740 evalcond
17741 [1]
17742 = 0;
17743 evalcond
17744 [2]
17745 = ((-1.51009803921569)
17746 + x1438
17747 + (((-1.0)
17748 * x1437)));
17749 evalcond
17750 [3]
17751 = ((1.51009803921569)
17752 + (((-1.0)
17753 * x1438))
17754 + x1437);
17755 evalcond
17756 [4]
17757 = ((-0.2125)
17758 + (((1.1)
17759 * pz))
17760 + (((-1.0)
17761 * (1.0)
17762 * pp)));
17763 if (IKabs(
17764 evalcond
17765 [0])
17766 < 0.0000010000000000
17767 && IKabs(
17768 evalcond
17769 [1])
17770 < 0.0000010000000000
17771 && IKabs(
17772 evalcond
17773 [2])
17774 < 0.0000010000000000
17775 && IKabs(
17776 evalcond
17777 [3])
17778 < 0.0000010000000000
17779 && IKabs(
17780 evalcond
17781 [4])
17782 < 0.0000010000000000)
17783 {
17784 bgotonextstatement
17785 = false;
17786 {
17787 IkReal j4array
17788 [4],
17789 cj4array
17790 [4],
17791 sj4array
17792 [4];
17793 bool j4valid
17794 [4]
17795 = {false};
17796 _nj4
17797 = 4;
17798 j4array
17799 [0]
17800 = 0;
17801 sj4array
17802 [0]
17803 = IKsin(
17804 j4array
17805 [0]);
17806 cj4array
17807 [0]
17808 = IKcos(
17809 j4array
17810 [0]);
17811 j4array
17812 [1]
17813 = 1.5707963267949;
17814 sj4array
17815 [1]
17816 = IKsin(
17817 j4array
17818 [1]);
17819 cj4array
17820 [1]
17821 = IKcos(
17822 j4array
17823 [1]);
17824 j4array
17825 [2]
17826 = 3.14159265358979;
17827 sj4array
17828 [2]
17829 = IKsin(
17830 j4array
17831 [2]);
17832 cj4array
17833 [2]
17834 = IKcos(
17835 j4array
17836 [2]);
17837 j4array
17838 [3]
17839 = -1.5707963267949;
17840 sj4array
17841 [3]
17842 = IKsin(
17843 j4array
17844 [3]);
17845 cj4array
17846 [3]
17847 = IKcos(
17848 j4array
17849 [3]);
17850 if (j4array
17851 [0]
17852 > IKPI)
17853 {
17854 j4array
17855 [0]
17856 -= IK2PI;
17857 }
17858 else if (
17859 j4array
17860 [0]
17861 < -IKPI)
17862 {
17863 j4array
17864 [0]
17865 += IK2PI;
17866 }
17867 j4valid
17868 [0]
17869 = true;
17870 if (j4array
17871 [1]
17872 > IKPI)
17873 {
17874 j4array
17875 [1]
17876 -= IK2PI;
17877 }
17878 else if (
17879 j4array
17880 [1]
17881 < -IKPI)
17882 {
17883 j4array
17884 [1]
17885 += IK2PI;
17886 }
17887 j4valid
17888 [1]
17889 = true;
17890 if (j4array
17891 [2]
17892 > IKPI)
17893 {
17894 j4array
17895 [2]
17896 -= IK2PI;
17897 }
17898 else if (
17899 j4array
17900 [2]
17901 < -IKPI)
17902 {
17903 j4array
17904 [2]
17905 += IK2PI;
17906 }
17907 j4valid
17908 [2]
17909 = true;
17910 if (j4array
17911 [3]
17912 > IKPI)
17913 {
17914 j4array
17915 [3]
17916 -= IK2PI;
17917 }
17918 else if (
17919 j4array
17920 [3]
17921 < -IKPI)
17922 {
17923 j4array
17924 [3]
17925 += IK2PI;
17926 }
17927 j4valid
17928 [3]
17929 = true;
17930 for (
17931 int ij4
17932 = 0;
17933 ij4
17934 < 4;
17935 ++ij4)
17936 {
17937 if (!j4valid
17938 [ij4])
17939 {
17940 continue;
17941 }
17942 _ij4[0]
17943 = ij4;
17944 _ij4[1]
17945 = -1;
17946 for (
17947 int iij4
17948 = ij4
17949 + 1;
17950 iij4
17951 < 4;
17952 ++iij4)
17953 {
17954 if (j4valid
17955 [iij4]
17956 && IKabs(
17957 cj4array
17958 [ij4]
17959 - cj4array
17960 [iij4])
17961 < IKFAST_SOLUTION_THRESH
17962 && IKabs(
17963 sj4array
17964 [ij4]
17965 - sj4array
17966 [iij4])
17967 < IKFAST_SOLUTION_THRESH)
17968 {
17969 j4valid
17970 [iij4]
17971 = false;
17972 _ij4[1]
17973 = iij4;
17974 break;
17975 }
17976 }
17977 j4 = j4array
17978 [ij4];
17979 cj4 = cj4array
17980 [ij4];
17981 sj4 = sj4array
17982 [ij4];
17983
17984 rotationfunction0(
17985 solutions);
17986 }
17987 }
17988 }
17989 } while (
17990 0);
17991 if (bgotonextstatement)
17992 {
17993 bool
17994 bgotonextstatement
17995 = true;
17996 do
17997 {
17998 if (1)
17999 {
18000 bgotonextstatement
18001 = false;
18002 continue; // branch miss [j4]
18003 }
18004 } while (
18005 0);
18006 if (bgotonextstatement)
18007 {
18008 }
18009 }
18010 }
18011 }
18012 else
18013 {
18014 {
18015 IkReal j4array
18016 [1],
18017 cj4array
18018 [1],
18019 sj4array
18020 [1];
18021 bool j4valid
18022 [1]
18023 = {false};
18024 _nj4
18025 = 1;
18026 IkReal
18027 x1439
18028 = ((100.0)
18029 * pp);
18030 IkReal
18031 x1440
18032 = ((110.0)
18033 * pz);
18034 CheckValue<IkReal> x1441 = IKPowWithIntegerCheck(
18035 IKsign((
18036 (((-1.0)
18037 * (9.0)
18038 * (pz
18039 * pz)))
18040 + (((9.0)
18041 * pp)))),
18042 -1);
18043 if (!x1441
18044 .valid)
18045 {
18046 continue;
18047 }
18048 CheckValue<IkReal> x1442 = IKatan2WithCheck(
18049 IkReal((
18050 ((py
18051 * x1440))
18052 + (((-1.0)
18053 * (21.25)
18054 * py))
18055 + (((-1.0)
18056 * py
18057 * x1439)))),
18058 (((px
18059 * x1440))
18060 + (((-1.0)
18061 * (21.25)
18062 * px))
18063 + (((-1.0)
18064 * px
18065 * x1439))),
18066 IKFAST_ATAN2_MAGTHRESH);
18067 if (!x1442
18068 .valid)
18069 {
18070 continue;
18071 }
18072 j4array
18073 [0]
18074 = ((-1.5707963267949)
18075 + (((1.5707963267949)
18076 * (x1441
18077 .value)))
18078 + (x1442
18079 .value));
18080 sj4array
18081 [0]
18082 = IKsin(
18083 j4array
18084 [0]);
18085 cj4array
18086 [0]
18087 = IKcos(
18088 j4array
18089 [0]);
18090 if (j4array
18091 [0]
18092 > IKPI)
18093 {
18094 j4array
18095 [0]
18096 -= IK2PI;
18097 }
18098 else if (
18099 j4array
18100 [0]
18101 < -IKPI)
18102 {
18103 j4array
18104 [0]
18105 += IK2PI;
18106 }
18107 j4valid
18108 [0]
18109 = true;
18110 for (
18111 int ij4
18112 = 0;
18113 ij4
18114 < 1;
18115 ++ij4)
18116 {
18117 if (!j4valid
18118 [ij4])
18119 {
18120 continue;
18121 }
18122 _ij4[0]
18123 = ij4;
18124 _ij4[1]
18125 = -1;
18126 for (
18127 int iij4
18128 = ij4
18129 + 1;
18130 iij4
18131 < 1;
18132 ++iij4)
18133 {
18134 if (j4valid
18135 [iij4]
18136 && IKabs(
18137 cj4array
18138 [ij4]
18139 - cj4array
18140 [iij4])
18141 < IKFAST_SOLUTION_THRESH
18142 && IKabs(
18143 sj4array
18144 [ij4]
18145 - sj4array
18146 [iij4])
18147 < IKFAST_SOLUTION_THRESH)
18148 {
18149 j4valid
18150 [iij4]
18151 = false;
18152 _ij4[1]
18153 = iij4;
18154 break;
18155 }
18156 }
18157 j4 = j4array
18158 [ij4];
18159 cj4 = cj4array
18160 [ij4];
18161 sj4 = sj4array
18162 [ij4];
18163 {
18164 IkReal evalcond
18165 [3];
18166 IkReal
18167 x1443
18168 = IKsin(
18169 j4);
18170 IkReal
18171 x1444
18172 = IKcos(
18173 j4);
18174 IkReal
18175 x1445
18176 = (px
18177 * x1444);
18178 IkReal
18179 x1446
18180 = (py
18181 * x1443);
18182 evalcond
18183 [0]
18184 = ((((-1.0)
18185 * py
18186 * x1444))
18187 + ((px
18188 * x1443)));
18189 evalcond
18190 [1]
18191 = ((-1.51009803921569)
18192 + (((-1.0)
18193 * (1.32323529411765)
18194 * cj9))
18195 + (((3.92156862745098)
18196 * pp))
18197 + x1446
18198 + x1445);
18199 evalcond
18200 [2]
18201 = ((-0.2125)
18202 + (((1.1)
18203 * pz))
18204 + (((-0.09)
18205 * x1446))
18206 + (((-1.0)
18207 * (1.0)
18208 * pp))
18209 + (((-0.09)
18210 * x1445)));
18211 if (IKabs(
18212 evalcond
18213 [0])
18214 > IKFAST_EVALCOND_THRESH
18215 || IKabs(
18216 evalcond
18217 [1])
18218 > IKFAST_EVALCOND_THRESH
18219 || IKabs(
18220 evalcond
18221 [2])
18222 > IKFAST_EVALCOND_THRESH)
18223 {
18224 continue;
18225 }
18226 }
18227
18228 rotationfunction0(
18229 solutions);
18230 }
18231 }
18232 }
18233 }
18234 }
18235 else
18236 {
18237 {
18238 IkReal j4array
18239 [1],
18240 cj4array
18241 [1],
18242 sj4array
18243 [1];
18244 bool j4valid
18245 [1]
18246 = {false};
18247 _nj4 = 1;
18248 IkReal
18249 x1447
18250 = ((1.32323529411765)
18251 * cj9);
18252 IkReal
18253 x1448
18254 = ((3.92156862745098)
18255 * pp);
18256 CheckValue<IkReal> x1449 = IKPowWithIntegerCheck(
18257 IKsign((
18258 pp
18259 + (((-1.0)
18260 * (1.0)
18261 * (pz
18262 * pz))))),
18263 -1);
18264 if (!x1449
18265 .valid)
18266 {
18267 continue;
18268 }
18269 CheckValue<IkReal> x1450 = IKatan2WithCheck(
18270 IkReal((
18271 (((-1.0)
18272 * py
18273 * x1448))
18274 + (((1.51009803921569)
18275 * py))
18276 + ((py
18277 * x1447)))),
18278 ((((-1.0)
18279 * px
18280 * x1448))
18281 + (((1.51009803921569)
18282 * px))
18283 + ((px
18284 * x1447))),
18285 IKFAST_ATAN2_MAGTHRESH);
18286 if (!x1450
18287 .valid)
18288 {
18289 continue;
18290 }
18291 j4array[0]
18292 = ((-1.5707963267949)
18293 + (((1.5707963267949)
18294 * (x1449
18295 .value)))
18296 + (x1450
18297 .value));
18298 sj4array[0] = IKsin(
18299 j4array
18300 [0]);
18301 cj4array[0] = IKcos(
18302 j4array
18303 [0]);
18304 if (j4array
18305 [0]
18306 > IKPI)
18307 {
18308 j4array
18309 [0]
18310 -= IK2PI;
18311 }
18312 else if (
18313 j4array
18314 [0]
18315 < -IKPI)
18316 {
18317 j4array
18318 [0]
18319 += IK2PI;
18320 }
18321 j4valid[0]
18322 = true;
18323 for (
18324 int ij4
18325 = 0;
18326 ij4
18327 < 1;
18328 ++ij4)
18329 {
18330 if (!j4valid
18331 [ij4])
18332 {
18333 continue;
18334 }
18335 _ij4[0]
18336 = ij4;
18337 _ij4[1]
18338 = -1;
18339 for (
18340 int iij4
18341 = ij4
18342 + 1;
18343 iij4
18344 < 1;
18345 ++iij4)
18346 {
18347 if (j4valid
18348 [iij4]
18349 && IKabs(
18350 cj4array
18351 [ij4]
18352 - cj4array
18353 [iij4])
18354 < IKFAST_SOLUTION_THRESH
18355 && IKabs(
18356 sj4array
18357 [ij4]
18358 - sj4array
18359 [iij4])
18360 < IKFAST_SOLUTION_THRESH)
18361 {
18362 j4valid
18363 [iij4]
18364 = false;
18365 _ij4[1]
18366 = iij4;
18367 break;
18368 }
18369 }
18370 j4 = j4array
18371 [ij4];
18372 cj4 = cj4array
18373 [ij4];
18374 sj4 = sj4array
18375 [ij4];
18376 {
18377 IkReal evalcond
18378 [3];
18379 IkReal
18380 x1451
18381 = IKsin(
18382 j4);
18383 IkReal
18384 x1452
18385 = IKcos(
18386 j4);
18387 IkReal
18388 x1453
18389 = (px
18390 * x1452);
18391 IkReal
18392 x1454
18393 = (py
18394 * x1451);
18395 evalcond
18396 [0]
18397 = (((px
18398 * x1451))
18399 + (((-1.0)
18400 * py
18401 * x1452)));
18402 evalcond
18403 [1]
18404 = ((-1.51009803921569)
18405 + (((-1.0)
18406 * (1.32323529411765)
18407 * cj9))
18408 + (((3.92156862745098)
18409 * pp))
18410 + x1453
18411 + x1454);
18412 evalcond
18413 [2]
18414 = ((-0.2125)
18415 + (((-0.09)
18416 * x1453))
18417 + (((1.1)
18418 * pz))
18419 + (((-1.0)
18420 * (1.0)
18421 * pp))
18422 + (((-0.09)
18423 * x1454)));
18424 if (IKabs(
18425 evalcond
18426 [0])
18427 > IKFAST_EVALCOND_THRESH
18428 || IKabs(
18429 evalcond
18430 [1])
18431 > IKFAST_EVALCOND_THRESH
18432 || IKabs(
18433 evalcond
18434 [2])
18435 > IKFAST_EVALCOND_THRESH)
18436 {
18437 continue;
18438 }
18439 }
18440
18441 rotationfunction0(
18442 solutions);
18443 }
18444 }
18445 }
18446 }
18447 }
18448 } while (0);
18449 if (bgotonextstatement)
18450 {
18451 bool
18452 bgotonextstatement
18453 = true;
18454 do
18455 {
18456 evalcond[0]
18457 = ((-3.14159265358979)
18458 + (IKfmod(
18459 ((3.14159265358979)
18460 + (IKabs((
18461 (-1.5707963267949)
18462 + j8)))),
18463 6.28318530717959)));
18464 if (IKabs(
18465 evalcond
18466 [0])
18467 < 0.0000010000000000)
18468 {
18469 bgotonextstatement
18470 = false;
18471 {
18472 IkReal
18473 j4eval
18474 [3];
18475 sj6 = 0;
18476 cj6 = 1.0;
18477 j6 = 0;
18478 sj8 = 1.0;
18479 cj8 = 0;
18480 j8 = 1.5707963267949;
18481 IkReal x1455
18482 = (pp
18483 + (((-1.0)
18484 * (1.0)
18485 * (pz
18486 * pz))));
18487 IkReal x1456
18488 = ((13497.0)
18489 * cj9);
18490 IkReal x1457
18491 = ((40000.0)
18492 * pp);
18493 j4eval[0]
18494 = x1455;
18495 j4eval[1]
18496 = ((IKabs((
18497 (((15403.0)
18498 * px))
18499 + (((-1.0)
18500 * px
18501 * x1457))
18502 + ((px
18503 * x1456)))))
18504 + (IKabs((
18505 (((-1.0)
18506 * py
18507 * x1456))
18508 + ((py
18509 * x1457))
18510 + (((-1.0)
18511 * (15403.0)
18512 * py))))));
18513 j4eval[2]
18514 = IKsign(
18515 x1455);
18516 if (IKabs(
18517 j4eval
18518 [0])
18519 < 0.0000010000000000
18520 || IKabs(
18521 j4eval
18522 [1])
18523 < 0.0000010000000000
18524 || IKabs(
18525 j4eval
18526 [2])
18527 < 0.0000010000000000)
18528 {
18529 {
18530 IkReal j4eval
18531 [3];
18532 sj6 = 0;
18533 cj6 = 1.0;
18534 j6 = 0;
18535 sj8 = 1.0;
18536 cj8 = 0;
18537 j8 = 1.5707963267949;
18538 IkReal
18539 x1458
18540 = pz
18541 * pz;
18542 IkReal
18543 x1459
18544 = ((80.0)
18545 * pp);
18546 IkReal
18547 x1460
18548 = ((88.0)
18549 * pz);
18550 j4eval
18551 [0]
18552 = (pp
18553 + (((-1.0)
18554 * x1458)));
18555 j4eval[1] = IKsign((
18556 (((-9.0)
18557 * x1458))
18558 + (((9.0)
18559 * pp))));
18560 j4eval
18561 [2]
18562 = ((IKabs((
18563 (((-1.0)
18564 * py
18565 * x1460))
18566 + ((py
18567 * x1459))
18568 + (((17.0)
18569 * py)))))
18570 + (IKabs((
18571 (((-1.0)
18572 * (17.0)
18573 * px))
18574 + (((-1.0)
18575 * px
18576 * x1459))
18577 + ((px
18578 * x1460))))));
18579 if (IKabs(
18580 j4eval
18581 [0])
18582 < 0.0000010000000000
18583 || IKabs(
18584 j4eval
18585 [1])
18586 < 0.0000010000000000
18587 || IKabs(
18588 j4eval
18589 [2])
18590 < 0.0000010000000000)
18591 {
18592 {
18593 IkReal evalcond
18594 [5];
18595 bool
18596 bgotonextstatement
18597 = true;
18598 do
18599 {
18600 IkReal
18601 x1461
18602 = ((-1.51009803921569)
18603 + (((-1.0)
18604 * (1.32323529411765)
18605 * cj9))
18606 + (((3.92156862745098)
18607 * pp)));
18608 evalcond
18609 [0]
18610 = ((IKabs(
18611 py))
18612 + (IKabs(
18613 px)));
18614 evalcond
18615 [1]
18616 = x1461;
18617 evalcond
18618 [2]
18619 = 0;
18620 evalcond
18621 [3]
18622 = x1461;
18623 evalcond
18624 [4]
18625 = ((-0.2125)
18626 + (((1.1)
18627 * pz))
18628 + (((-1.0)
18629 * (1.0)
18630 * pp)));
18631 if (IKabs(
18632 evalcond
18633 [0])
18634 < 0.0000010000000000
18635 && IKabs(
18636 evalcond
18637 [1])
18638 < 0.0000010000000000
18639 && IKabs(
18640 evalcond
18641 [2])
18642 < 0.0000010000000000
18643 && IKabs(
18644 evalcond
18645 [3])
18646 < 0.0000010000000000
18647 && IKabs(
18648 evalcond
18649 [4])
18650 < 0.0000010000000000)
18651 {
18652 bgotonextstatement
18653 = false;
18654 {
18655 IkReal j4array
18656 [4],
18657 cj4array
18658 [4],
18659 sj4array
18660 [4];
18661 bool j4valid
18662 [4]
18663 = {false};
18664 _nj4
18665 = 4;
18666 j4array
18667 [0]
18668 = 0;
18669 sj4array
18670 [0]
18671 = IKsin(
18672 j4array
18673 [0]);
18674 cj4array
18675 [0]
18676 = IKcos(
18677 j4array
18678 [0]);
18679 j4array
18680 [1]
18681 = 1.5707963267949;
18682 sj4array
18683 [1]
18684 = IKsin(
18685 j4array
18686 [1]);
18687 cj4array
18688 [1]
18689 = IKcos(
18690 j4array
18691 [1]);
18692 j4array
18693 [2]
18694 = 3.14159265358979;
18695 sj4array
18696 [2]
18697 = IKsin(
18698 j4array
18699 [2]);
18700 cj4array
18701 [2]
18702 = IKcos(
18703 j4array
18704 [2]);
18705 j4array
18706 [3]
18707 = -1.5707963267949;
18708 sj4array
18709 [3]
18710 = IKsin(
18711 j4array
18712 [3]);
18713 cj4array
18714 [3]
18715 = IKcos(
18716 j4array
18717 [3]);
18718 if (j4array
18719 [0]
18720 > IKPI)
18721 {
18722 j4array
18723 [0]
18724 -= IK2PI;
18725 }
18726 else if (
18727 j4array
18728 [0]
18729 < -IKPI)
18730 {
18731 j4array
18732 [0]
18733 += IK2PI;
18734 }
18735 j4valid
18736 [0]
18737 = true;
18738 if (j4array
18739 [1]
18740 > IKPI)
18741 {
18742 j4array
18743 [1]
18744 -= IK2PI;
18745 }
18746 else if (
18747 j4array
18748 [1]
18749 < -IKPI)
18750 {
18751 j4array
18752 [1]
18753 += IK2PI;
18754 }
18755 j4valid
18756 [1]
18757 = true;
18758 if (j4array
18759 [2]
18760 > IKPI)
18761 {
18762 j4array
18763 [2]
18764 -= IK2PI;
18765 }
18766 else if (
18767 j4array
18768 [2]
18769 < -IKPI)
18770 {
18771 j4array
18772 [2]
18773 += IK2PI;
18774 }
18775 j4valid
18776 [2]
18777 = true;
18778 if (j4array
18779 [3]
18780 > IKPI)
18781 {
18782 j4array
18783 [3]
18784 -= IK2PI;
18785 }
18786 else if (
18787 j4array
18788 [3]
18789 < -IKPI)
18790 {
18791 j4array
18792 [3]
18793 += IK2PI;
18794 }
18795 j4valid
18796 [3]
18797 = true;
18798 for (
18799 int ij4
18800 = 0;
18801 ij4
18802 < 4;
18803 ++ij4)
18804 {
18805 if (!j4valid
18806 [ij4])
18807 {
18808 continue;
18809 }
18810 _ij4[0]
18811 = ij4;
18812 _ij4[1]
18813 = -1;
18814 for (
18815 int iij4
18816 = ij4
18817 + 1;
18818 iij4
18819 < 4;
18820 ++iij4)
18821 {
18822 if (j4valid
18823 [iij4]
18824 && IKabs(
18825 cj4array
18826 [ij4]
18827 - cj4array
18828 [iij4])
18829 < IKFAST_SOLUTION_THRESH
18830 && IKabs(
18831 sj4array
18832 [ij4]
18833 - sj4array
18834 [iij4])
18835 < IKFAST_SOLUTION_THRESH)
18836 {
18837 j4valid
18838 [iij4]
18839 = false;
18840 _ij4[1]
18841 = iij4;
18842 break;
18843 }
18844 }
18845 j4 = j4array
18846 [ij4];
18847 cj4 = cj4array
18848 [ij4];
18849 sj4 = sj4array
18850 [ij4];
18851
18852 rotationfunction0(
18853 solutions);
18854 }
18855 }
18856 }
18857 } while (
18858 0);
18859 if (bgotonextstatement)
18860 {
18861 bool
18862 bgotonextstatement
18863 = true;
18864 do
18865 {
18866 if (1)
18867 {
18868 bgotonextstatement
18869 = false;
18870 continue; // branch miss [j4]
18871 }
18872 } while (
18873 0);
18874 if (bgotonextstatement)
18875 {
18876 }
18877 }
18878 }
18879 }
18880 else
18881 {
18882 {
18883 IkReal j4array
18884 [1],
18885 cj4array
18886 [1],
18887 sj4array
18888 [1];
18889 bool j4valid
18890 [1]
18891 = {false};
18892 _nj4
18893 = 1;
18894 IkReal
18895 x1462
18896 = ((100.0)
18897 * pp);
18898 IkReal
18899 x1463
18900 = ((110.0)
18901 * pz);
18902 CheckValue<IkReal> x1464 = IKPowWithIntegerCheck(
18903 IKsign((
18904 (((-1.0)
18905 * (9.0)
18906 * (pz
18907 * pz)))
18908 + (((9.0)
18909 * pp)))),
18910 -1);
18911 if (!x1464
18912 .valid)
18913 {
18914 continue;
18915 }
18916 CheckValue<IkReal> x1465 = IKatan2WithCheck(
18917 IkReal((
18918 (((-1.0)
18919 * px
18920 * x1462))
18921 + (((-1.0)
18922 * (21.25)
18923 * px))
18924 + ((px
18925 * x1463)))),
18926 (((py
18927 * x1462))
18928 + (((21.25)
18929 * py))
18930 + (((-1.0)
18931 * py
18932 * x1463))),
18933 IKFAST_ATAN2_MAGTHRESH);
18934 if (!x1465
18935 .valid)
18936 {
18937 continue;
18938 }
18939 j4array
18940 [0]
18941 = ((-1.5707963267949)
18942 + (((1.5707963267949)
18943 * (x1464
18944 .value)))
18945 + (x1465
18946 .value));
18947 sj4array
18948 [0]
18949 = IKsin(
18950 j4array
18951 [0]);
18952 cj4array
18953 [0]
18954 = IKcos(
18955 j4array
18956 [0]);
18957 if (j4array
18958 [0]
18959 > IKPI)
18960 {
18961 j4array
18962 [0]
18963 -= IK2PI;
18964 }
18965 else if (
18966 j4array
18967 [0]
18968 < -IKPI)
18969 {
18970 j4array
18971 [0]
18972 += IK2PI;
18973 }
18974 j4valid
18975 [0]
18976 = true;
18977 for (
18978 int ij4
18979 = 0;
18980 ij4
18981 < 1;
18982 ++ij4)
18983 {
18984 if (!j4valid
18985 [ij4])
18986 {
18987 continue;
18988 }
18989 _ij4[0]
18990 = ij4;
18991 _ij4[1]
18992 = -1;
18993 for (
18994 int iij4
18995 = ij4
18996 + 1;
18997 iij4
18998 < 1;
18999 ++iij4)
19000 {
19001 if (j4valid
19002 [iij4]
19003 && IKabs(
19004 cj4array
19005 [ij4]
19006 - cj4array
19007 [iij4])
19008 < IKFAST_SOLUTION_THRESH
19009 && IKabs(
19010 sj4array
19011 [ij4]
19012 - sj4array
19013 [iij4])
19014 < IKFAST_SOLUTION_THRESH)
19015 {
19016 j4valid
19017 [iij4]
19018 = false;
19019 _ij4[1]
19020 = iij4;
19021 break;
19022 }
19023 }
19024 j4 = j4array
19025 [ij4];
19026 cj4 = cj4array
19027 [ij4];
19028 sj4 = sj4array
19029 [ij4];
19030 {
19031 IkReal evalcond
19032 [3];
19033 IkReal
19034 x1466
19035 = IKcos(
19036 j4);
19037 IkReal
19038 x1467
19039 = IKsin(
19040 j4);
19041 IkReal
19042 x1468
19043 = (px
19044 * x1467);
19045 IkReal
19046 x1469
19047 = (py
19048 * x1466);
19049 evalcond
19050 [0]
19051 = (((py
19052 * x1467))
19053 + ((px
19054 * x1466)));
19055 evalcond
19056 [1]
19057 = ((-1.51009803921569)
19058 + (((-1.0)
19059 * x1469))
19060 + (((-1.0)
19061 * (1.32323529411765)
19062 * cj9))
19063 + (((3.92156862745098)
19064 * pp))
19065 + x1468);
19066 evalcond
19067 [2]
19068 = ((-0.2125)
19069 + (((1.1)
19070 * pz))
19071 + (((0.09)
19072 * x1469))
19073 + (((-0.09)
19074 * x1468))
19075 + (((-1.0)
19076 * (1.0)
19077 * pp)));
19078 if (IKabs(
19079 evalcond
19080 [0])
19081 > IKFAST_EVALCOND_THRESH
19082 || IKabs(
19083 evalcond
19084 [1])
19085 > IKFAST_EVALCOND_THRESH
19086 || IKabs(
19087 evalcond
19088 [2])
19089 > IKFAST_EVALCOND_THRESH)
19090 {
19091 continue;
19092 }
19093 }
19094
19095 rotationfunction0(
19096 solutions);
19097 }
19098 }
19099 }
19100 }
19101 }
19102 else
19103 {
19104 {
19105 IkReal j4array
19106 [1],
19107 cj4array
19108 [1],
19109 sj4array
19110 [1];
19111 bool j4valid
19112 [1]
19113 = {false};
19114 _nj4
19115 = 1;
19116 IkReal
19117 x1470
19118 = ((1.32323529411765)
19119 * cj9);
19120 IkReal
19121 x1471
19122 = ((3.92156862745098)
19123 * pp);
19124 CheckValue<IkReal> x1472 = IKPowWithIntegerCheck(
19125 IKsign((
19126 pp
19127 + (((-1.0)
19128 * (1.0)
19129 * (pz
19130 * pz))))),
19131 -1);
19132 if (!x1472
19133 .valid)
19134 {
19135 continue;
19136 }
19137 CheckValue<IkReal> x1473 = IKatan2WithCheck(
19138 IkReal((
19139 ((px
19140 * x1470))
19141 + (((-1.0)
19142 * px
19143 * x1471))
19144 + (((1.51009803921569)
19145 * px)))),
19146 (((py
19147 * x1471))
19148 + (((-1.0)
19149 * (1.51009803921569)
19150 * py))
19151 + (((-1.0)
19152 * py
19153 * x1470))),
19154 IKFAST_ATAN2_MAGTHRESH);
19155 if (!x1473
19156 .valid)
19157 {
19158 continue;
19159 }
19160 j4array
19161 [0]
19162 = ((-1.5707963267949)
19163 + (((1.5707963267949)
19164 * (x1472
19165 .value)))
19166 + (x1473
19167 .value));
19168 sj4array
19169 [0]
19170 = IKsin(
19171 j4array
19172 [0]);
19173 cj4array
19174 [0]
19175 = IKcos(
19176 j4array
19177 [0]);
19178 if (j4array
19179 [0]
19180 > IKPI)
19181 {
19182 j4array
19183 [0]
19184 -= IK2PI;
19185 }
19186 else if (
19187 j4array
19188 [0]
19189 < -IKPI)
19190 {
19191 j4array
19192 [0]
19193 += IK2PI;
19194 }
19195 j4valid
19196 [0]
19197 = true;
19198 for (
19199 int ij4
19200 = 0;
19201 ij4
19202 < 1;
19203 ++ij4)
19204 {
19205 if (!j4valid
19206 [ij4])
19207 {
19208 continue;
19209 }
19210 _ij4[0]
19211 = ij4;
19212 _ij4[1]
19213 = -1;
19214 for (
19215 int iij4
19216 = ij4
19217 + 1;
19218 iij4
19219 < 1;
19220 ++iij4)
19221 {
19222 if (j4valid
19223 [iij4]
19224 && IKabs(
19225 cj4array
19226 [ij4]
19227 - cj4array
19228 [iij4])
19229 < IKFAST_SOLUTION_THRESH
19230 && IKabs(
19231 sj4array
19232 [ij4]
19233 - sj4array
19234 [iij4])
19235 < IKFAST_SOLUTION_THRESH)
19236 {
19237 j4valid
19238 [iij4]
19239 = false;
19240 _ij4[1]
19241 = iij4;
19242 break;
19243 }
19244 }
19245 j4 = j4array
19246 [ij4];
19247 cj4 = cj4array
19248 [ij4];
19249 sj4 = sj4array
19250 [ij4];
19251 {
19252 IkReal evalcond
19253 [3];
19254 IkReal
19255 x1474
19256 = IKcos(
19257 j4);
19258 IkReal
19259 x1475
19260 = IKsin(
19261 j4);
19262 IkReal
19263 x1476
19264 = (px
19265 * x1475);
19266 IkReal
19267 x1477
19268 = (py
19269 * x1474);
19270 evalcond
19271 [0]
19272 = (((px
19273 * x1474))
19274 + ((py
19275 * x1475)));
19276 evalcond
19277 [1]
19278 = ((-1.51009803921569)
19279 + (((-1.0)
19280 * (1.32323529411765)
19281 * cj9))
19282 + (((3.92156862745098)
19283 * pp))
19284 + (((-1.0)
19285 * x1477))
19286 + x1476);
19287 evalcond
19288 [2]
19289 = ((-0.2125)
19290 + (((-0.09)
19291 * x1476))
19292 + (((0.09)
19293 * x1477))
19294 + (((1.1)
19295 * pz))
19296 + (((-1.0)
19297 * (1.0)
19298 * pp)));
19299 if (IKabs(
19300 evalcond
19301 [0])
19302 > IKFAST_EVALCOND_THRESH
19303 || IKabs(
19304 evalcond
19305 [1])
19306 > IKFAST_EVALCOND_THRESH
19307 || IKabs(
19308 evalcond
19309 [2])
19310 > IKFAST_EVALCOND_THRESH)
19311 {
19312 continue;
19313 }
19314 }
19315
19316 rotationfunction0(
19317 solutions);
19318 }
19319 }
19320 }
19321 }
19322 }
19323 } while (0);
19324 if (bgotonextstatement)
19325 {
19326 bool
19327 bgotonextstatement
19328 = true;
19329 do
19330 {
19331 evalcond[0]
19332 = ((-3.14159265358979)
19333 + (IKfmod(
19334 ((3.14159265358979)
19335 + (IKabs((
19336 (1.5707963267949)
19337 + j8)))),
19338 6.28318530717959)));
19339 if (IKabs(
19340 evalcond
19341 [0])
19342 < 0.0000010000000000)
19343 {
19344 bgotonextstatement
19345 = false;
19346 {
19347 IkReal j4eval
19348 [3];
19349 sj6 = 0;
19350 cj6 = 1.0;
19351 j6 = 0;
19352 sj8 = -1.0;
19353 cj8 = 0;
19354 j8 = -1.5707963267949;
19355 IkReal
19356 x1478
19357 = (pp
19358 + (((-1.0)
19359 * (1.0)
19360 * (pz
19361 * pz))));
19362 IkReal
19363 x1479
19364 = ((13497.0)
19365 * cj9);
19366 IkReal
19367 x1480
19368 = ((40000.0)
19369 * pp);
19370 j4eval[0]
19371 = x1478;
19372 j4eval[1]
19373 = ((IKabs((
19374 (((-1.0)
19375 * py
19376 * x1480))
19377 + (((15403.0)
19378 * py))
19379 + ((py
19380 * x1479)))))
19381 + (IKabs((
19382 (((-1.0)
19383 * px
19384 * x1479))
19385 + ((px
19386 * x1480))
19387 + (((-1.0)
19388 * (15403.0)
19389 * px))))));
19390 j4eval[2]
19391 = IKsign(
19392 x1478);
19393 if (IKabs(
19394 j4eval
19395 [0])
19396 < 0.0000010000000000
19397 || IKabs(
19398 j4eval
19399 [1])
19400 < 0.0000010000000000
19401 || IKabs(
19402 j4eval
19403 [2])
19404 < 0.0000010000000000)
19405 {
19406 {
19407 IkReal j4eval
19408 [3];
19409 sj6 = 0;
19410 cj6 = 1.0;
19411 j6 = 0;
19412 sj8 = -1.0;
19413 cj8 = 0;
19414 j8 = -1.5707963267949;
19415 IkReal
19416 x1481
19417 = pz
19418 * pz;
19419 IkReal
19420 x1482
19421 = ((80.0)
19422 * pp);
19423 IkReal
19424 x1483
19425 = ((88.0)
19426 * pz);
19427 j4eval
19428 [0]
19429 = ((((-1.0)
19430 * x1481))
19431 + pp);
19432 j4eval
19433 [1]
19434 = ((IKabs((
19435 (((-1.0)
19436 * py
19437 * x1482))
19438 + ((py
19439 * x1483))
19440 + (((-1.0)
19441 * (17.0)
19442 * py)))))
19443 + (IKabs((
19444 (((17.0)
19445 * px))
19446 + ((px
19447 * x1482))
19448 + (((-1.0)
19449 * px
19450 * x1483))))));
19451 j4eval[2] = IKsign((
19452 (((-9.0)
19453 * x1481))
19454 + (((9.0)
19455 * pp))));
19456 if (IKabs(
19457 j4eval
19458 [0])
19459 < 0.0000010000000000
19460 || IKabs(
19461 j4eval
19462 [1])
19463 < 0.0000010000000000
19464 || IKabs(
19465 j4eval
19466 [2])
19467 < 0.0000010000000000)
19468 {
19469 {
19470 IkReal evalcond
19471 [5];
19472 bool
19473 bgotonextstatement
19474 = true;
19475 do
19476 {
19477 IkReal
19478 x1484
19479 = ((1.32323529411765)
19480 * cj9);
19481 IkReal
19482 x1485
19483 = ((3.92156862745098)
19484 * pp);
19485 evalcond
19486 [0]
19487 = ((IKabs(
19488 py))
19489 + (IKabs(
19490 px)));
19491 evalcond
19492 [1]
19493 = ((1.51009803921569)
19494 + (((-1.0)
19495 * x1485))
19496 + x1484);
19497 evalcond
19498 [2]
19499 = 0;
19500 evalcond
19501 [3]
19502 = ((-1.51009803921569)
19503 + x1485
19504 + (((-1.0)
19505 * x1484)));
19506 evalcond
19507 [4]
19508 = ((-0.2125)
19509 + (((1.1)
19510 * pz))
19511 + (((-1.0)
19512 * (1.0)
19513 * pp)));
19514 if (IKabs(
19515 evalcond
19516 [0])
19517 < 0.0000010000000000
19518 && IKabs(
19519 evalcond
19520 [1])
19521 < 0.0000010000000000
19522 && IKabs(
19523 evalcond
19524 [2])
19525 < 0.0000010000000000
19526 && IKabs(
19527 evalcond
19528 [3])
19529 < 0.0000010000000000
19530 && IKabs(
19531 evalcond
19532 [4])
19533 < 0.0000010000000000)
19534 {
19535 bgotonextstatement
19536 = false;
19537 {
19538 IkReal j4array
19539 [4],
19540 cj4array
19541 [4],
19542 sj4array
19543 [4];
19544 bool j4valid
19545 [4]
19546 = {false};
19547 _nj4
19548 = 4;
19549 j4array
19550 [0]
19551 = 0;
19552 sj4array
19553 [0]
19554 = IKsin(
19555 j4array
19556 [0]);
19557 cj4array
19558 [0]
19559 = IKcos(
19560 j4array
19561 [0]);
19562 j4array
19563 [1]
19564 = 1.5707963267949;
19565 sj4array
19566 [1]
19567 = IKsin(
19568 j4array
19569 [1]);
19570 cj4array
19571 [1]
19572 = IKcos(
19573 j4array
19574 [1]);
19575 j4array
19576 [2]
19577 = 3.14159265358979;
19578 sj4array
19579 [2]
19580 = IKsin(
19581 j4array
19582 [2]);
19583 cj4array
19584 [2]
19585 = IKcos(
19586 j4array
19587 [2]);
19588 j4array
19589 [3]
19590 = -1.5707963267949;
19591 sj4array
19592 [3]
19593 = IKsin(
19594 j4array
19595 [3]);
19596 cj4array
19597 [3]
19598 = IKcos(
19599 j4array
19600 [3]);
19601 if (j4array
19602 [0]
19603 > IKPI)
19604 {
19605 j4array
19606 [0]
19607 -= IK2PI;
19608 }
19609 else if (
19610 j4array
19611 [0]
19612 < -IKPI)
19613 {
19614 j4array
19615 [0]
19616 += IK2PI;
19617 }
19618 j4valid
19619 [0]
19620 = true;
19621 if (j4array
19622 [1]
19623 > IKPI)
19624 {
19625 j4array
19626 [1]
19627 -= IK2PI;
19628 }
19629 else if (
19630 j4array
19631 [1]
19632 < -IKPI)
19633 {
19634 j4array
19635 [1]
19636 += IK2PI;
19637 }
19638 j4valid
19639 [1]
19640 = true;
19641 if (j4array
19642 [2]
19643 > IKPI)
19644 {
19645 j4array
19646 [2]
19647 -= IK2PI;
19648 }
19649 else if (
19650 j4array
19651 [2]
19652 < -IKPI)
19653 {
19654 j4array
19655 [2]
19656 += IK2PI;
19657 }
19658 j4valid
19659 [2]
19660 = true;
19661 if (j4array
19662 [3]
19663 > IKPI)
19664 {
19665 j4array
19666 [3]
19667 -= IK2PI;
19668 }
19669 else if (
19670 j4array
19671 [3]
19672 < -IKPI)
19673 {
19674 j4array
19675 [3]
19676 += IK2PI;
19677 }
19678 j4valid
19679 [3]
19680 = true;
19681 for (
19682 int ij4
19683 = 0;
19684 ij4
19685 < 4;
19686 ++ij4)
19687 {
19688 if (!j4valid
19689 [ij4])
19690 {
19691 continue;
19692 }
19693 _ij4[0]
19694 = ij4;
19695 _ij4[1]
19696 = -1;
19697 for (
19698 int iij4
19699 = ij4
19700 + 1;
19701 iij4
19702 < 4;
19703 ++iij4)
19704 {
19705 if (j4valid
19706 [iij4]
19707 && IKabs(
19708 cj4array
19709 [ij4]
19710 - cj4array
19711 [iij4])
19712 < IKFAST_SOLUTION_THRESH
19713 && IKabs(
19714 sj4array
19715 [ij4]
19716 - sj4array
19717 [iij4])
19718 < IKFAST_SOLUTION_THRESH)
19719 {
19720 j4valid
19721 [iij4]
19722 = false;
19723 _ij4[1]
19724 = iij4;
19725 break;
19726 }
19727 }
19728 j4 = j4array
19729 [ij4];
19730 cj4 = cj4array
19731 [ij4];
19732 sj4 = sj4array
19733 [ij4];
19734
19735 rotationfunction0(
19736 solutions);
19737 }
19738 }
19739 }
19740 } while (
19741 0);
19742 if (bgotonextstatement)
19743 {
19744 bool
19745 bgotonextstatement
19746 = true;
19747 do
19748 {
19749 if (1)
19750 {
19751 bgotonextstatement
19752 = false;
19753 continue; // branch miss [j4]
19754 }
19755 } while (
19756 0);
19757 if (bgotonextstatement)
19758 {
19759 }
19760 }
19761 }
19762 }
19763 else
19764 {
19765 {
19766 IkReal j4array
19767 [1],
19768 cj4array
19769 [1],
19770 sj4array
19771 [1];
19772 bool j4valid
19773 [1]
19774 = {false};
19775 _nj4
19776 = 1;
19777 IkReal
19778 x1486
19779 = ((100.0)
19780 * pp);
19781 IkReal
19782 x1487
19783 = ((110.0)
19784 * pz);
19785 CheckValue<IkReal> x1488 = IKatan2WithCheck(
19786 IkReal((
19787 (((-1.0)
19788 * px
19789 * x1487))
19790 + ((px
19791 * x1486))
19792 + (((21.25)
19793 * px)))),
19794 ((((-1.0)
19795 * py
19796 * x1486))
19797 + (((-1.0)
19798 * (21.25)
19799 * py))
19800 + ((py
19801 * x1487))),
19802 IKFAST_ATAN2_MAGTHRESH);
19803 if (!x1488
19804 .valid)
19805 {
19806 continue;
19807 }
19808 CheckValue<IkReal> x1489 = IKPowWithIntegerCheck(
19809 IKsign((
19810 (((-1.0)
19811 * (9.0)
19812 * (pz
19813 * pz)))
19814 + (((9.0)
19815 * pp)))),
19816 -1);
19817 if (!x1489
19818 .valid)
19819 {
19820 continue;
19821 }
19822 j4array
19823 [0]
19824 = ((-1.5707963267949)
19825 + (x1488
19826 .value)
19827 + (((1.5707963267949)
19828 * (x1489
19829 .value))));
19830 sj4array
19831 [0]
19832 = IKsin(
19833 j4array
19834 [0]);
19835 cj4array
19836 [0]
19837 = IKcos(
19838 j4array
19839 [0]);
19840 if (j4array
19841 [0]
19842 > IKPI)
19843 {
19844 j4array
19845 [0]
19846 -= IK2PI;
19847 }
19848 else if (
19849 j4array
19850 [0]
19851 < -IKPI)
19852 {
19853 j4array
19854 [0]
19855 += IK2PI;
19856 }
19857 j4valid
19858 [0]
19859 = true;
19860 for (
19861 int ij4
19862 = 0;
19863 ij4
19864 < 1;
19865 ++ij4)
19866 {
19867 if (!j4valid
19868 [ij4])
19869 {
19870 continue;
19871 }
19872 _ij4[0]
19873 = ij4;
19874 _ij4[1]
19875 = -1;
19876 for (
19877 int iij4
19878 = ij4
19879 + 1;
19880 iij4
19881 < 1;
19882 ++iij4)
19883 {
19884 if (j4valid
19885 [iij4]
19886 && IKabs(
19887 cj4array
19888 [ij4]
19889 - cj4array
19890 [iij4])
19891 < IKFAST_SOLUTION_THRESH
19892 && IKabs(
19893 sj4array
19894 [ij4]
19895 - sj4array
19896 [iij4])
19897 < IKFAST_SOLUTION_THRESH)
19898 {
19899 j4valid
19900 [iij4]
19901 = false;
19902 _ij4[1]
19903 = iij4;
19904 break;
19905 }
19906 }
19907 j4 = j4array
19908 [ij4];
19909 cj4 = cj4array
19910 [ij4];
19911 sj4 = sj4array
19912 [ij4];
19913 {
19914 IkReal evalcond
19915 [3];
19916 IkReal
19917 x1490
19918 = IKcos(
19919 j4);
19920 IkReal
19921 x1491
19922 = ((1.0)
19923 * x1490);
19924 IkReal
19925 x1492
19926 = IKsin(
19927 j4);
19928 IkReal
19929 x1493
19930 = (px
19931 * x1492);
19932 evalcond
19933 [0]
19934 = ((((-1.0)
19935 * py
19936 * x1492))
19937 + (((-1.0)
19938 * px
19939 * x1491)));
19940 evalcond
19941 [1]
19942 = ((1.51009803921569)
19943 + x1493
19944 + (((-1.0)
19945 * py
19946 * x1491))
19947 + (((-1.0)
19948 * (3.92156862745098)
19949 * pp))
19950 + (((1.32323529411765)
19951 * cj9)));
19952 evalcond
19953 [2]
19954 = ((-0.2125)
19955 + (((1.1)
19956 * pz))
19957 + (((-1.0)
19958 * (1.0)
19959 * pp))
19960 + (((-0.09)
19961 * py
19962 * x1490))
19963 + (((0.09)
19964 * x1493)));
19965 if (IKabs(
19966 evalcond
19967 [0])
19968 > IKFAST_EVALCOND_THRESH
19969 || IKabs(
19970 evalcond
19971 [1])
19972 > IKFAST_EVALCOND_THRESH
19973 || IKabs(
19974 evalcond
19975 [2])
19976 > IKFAST_EVALCOND_THRESH)
19977 {
19978 continue;
19979 }
19980 }
19981
19982 rotationfunction0(
19983 solutions);
19984 }
19985 }
19986 }
19987 }
19988 }
19989 else
19990 {
19991 {
19992 IkReal j4array
19993 [1],
19994 cj4array
19995 [1],
19996 sj4array
19997 [1];
19998 bool j4valid
19999 [1]
20000 = {false};
20001 _nj4
20002 = 1;
20003 IkReal
20004 x1494
20005 = ((1.32323529411765)
20006 * cj9);
20007 IkReal
20008 x1495
20009 = ((3.92156862745098)
20010 * pp);
20011 CheckValue<IkReal> x1496 = IKatan2WithCheck(
20012 IkReal((
20013 (((-1.0)
20014 * (1.51009803921569)
20015 * px))
20016 + (((-1.0)
20017 * px
20018 * x1494))
20019 + ((px
20020 * x1495)))),
20021 ((((-1.0)
20022 * py
20023 * x1495))
20024 + (((1.51009803921569)
20025 * py))
20026 + ((py
20027 * x1494))),
20028 IKFAST_ATAN2_MAGTHRESH);
20029 if (!x1496
20030 .valid)
20031 {
20032 continue;
20033 }
20034 CheckValue<IkReal> x1497 = IKPowWithIntegerCheck(
20035 IKsign((
20036 pp
20037 + (((-1.0)
20038 * (1.0)
20039 * (pz
20040 * pz))))),
20041 -1);
20042 if (!x1497
20043 .valid)
20044 {
20045 continue;
20046 }
20047 j4array
20048 [0]
20049 = ((-1.5707963267949)
20050 + (x1496
20051 .value)
20052 + (((1.5707963267949)
20053 * (x1497
20054 .value))));
20055 sj4array
20056 [0]
20057 = IKsin(
20058 j4array
20059 [0]);
20060 cj4array
20061 [0]
20062 = IKcos(
20063 j4array
20064 [0]);
20065 if (j4array
20066 [0]
20067 > IKPI)
20068 {
20069 j4array
20070 [0]
20071 -= IK2PI;
20072 }
20073 else if (
20074 j4array
20075 [0]
20076 < -IKPI)
20077 {
20078 j4array
20079 [0]
20080 += IK2PI;
20081 }
20082 j4valid
20083 [0]
20084 = true;
20085 for (
20086 int ij4
20087 = 0;
20088 ij4
20089 < 1;
20090 ++ij4)
20091 {
20092 if (!j4valid
20093 [ij4])
20094 {
20095 continue;
20096 }
20097 _ij4[0]
20098 = ij4;
20099 _ij4[1]
20100 = -1;
20101 for (
20102 int iij4
20103 = ij4
20104 + 1;
20105 iij4
20106 < 1;
20107 ++iij4)
20108 {
20109 if (j4valid
20110 [iij4]
20111 && IKabs(
20112 cj4array
20113 [ij4]
20114 - cj4array
20115 [iij4])
20116 < IKFAST_SOLUTION_THRESH
20117 && IKabs(
20118 sj4array
20119 [ij4]
20120 - sj4array
20121 [iij4])
20122 < IKFAST_SOLUTION_THRESH)
20123 {
20124 j4valid
20125 [iij4]
20126 = false;
20127 _ij4[1]
20128 = iij4;
20129 break;
20130 }
20131 }
20132 j4 = j4array
20133 [ij4];
20134 cj4 = cj4array
20135 [ij4];
20136 sj4 = sj4array
20137 [ij4];
20138 {
20139 IkReal evalcond
20140 [3];
20141 IkReal
20142 x1498
20143 = IKcos(
20144 j4);
20145 IkReal
20146 x1499
20147 = ((1.0)
20148 * x1498);
20149 IkReal
20150 x1500
20151 = IKsin(
20152 j4);
20153 IkReal
20154 x1501
20155 = (px
20156 * x1500);
20157 evalcond
20158 [0]
20159 = ((((-1.0)
20160 * px
20161 * x1499))
20162 + (((-1.0)
20163 * py
20164 * x1500)));
20165 evalcond
20166 [1]
20167 = ((1.51009803921569)
20168 + (((-1.0)
20169 * (3.92156862745098)
20170 * pp))
20171 + (((-1.0)
20172 * py
20173 * x1499))
20174 + x1501
20175 + (((1.32323529411765)
20176 * cj9)));
20177 evalcond
20178 [2]
20179 = ((-0.2125)
20180 + (((-0.09)
20181 * py
20182 * x1498))
20183 + (((1.1)
20184 * pz))
20185 + (((-1.0)
20186 * (1.0)
20187 * pp))
20188 + (((0.09)
20189 * x1501)));
20190 if (IKabs(
20191 evalcond
20192 [0])
20193 > IKFAST_EVALCOND_THRESH
20194 || IKabs(
20195 evalcond
20196 [1])
20197 > IKFAST_EVALCOND_THRESH
20198 || IKabs(
20199 evalcond
20200 [2])
20201 > IKFAST_EVALCOND_THRESH)
20202 {
20203 continue;
20204 }
20205 }
20206
20207 rotationfunction0(
20208 solutions);
20209 }
20210 }
20211 }
20212 }
20213 }
20214 } while (0);
20215 if (bgotonextstatement)
20216 {
20217 bool
20218 bgotonextstatement
20219 = true;
20220 do
20221 {
20222 IkReal x1502
20223 = ((1.32323529411765)
20224 * cj9);
20225 IkReal x1503
20226 = ((3.92156862745098)
20227 * pp);
20228 evalcond[0]
20229 = ((IKabs(
20230 py))
20231 + (IKabs(
20232 px)));
20233 evalcond[1]
20234 = (((sj8
20235 * x1503))
20236 + (((-1.0)
20237 * (1.51009803921569)
20238 * sj8))
20239 + (((-1.0)
20240 * sj8
20241 * x1502)));
20242 evalcond[2]
20243 = 0;
20244 evalcond[3]
20245 = ((-1.51009803921569)
20246 + (((-1.0)
20247 * x1502))
20248 + x1503);
20249 evalcond[4]
20250 = ((((-1.0)
20251 * (1.51009803921569)
20252 * cj8))
20253 + ((cj8
20254 * x1503))
20255 + (((-1.0)
20256 * cj8
20257 * x1502)));
20258 evalcond[5]
20259 = ((-0.2125)
20260 + (((1.1)
20261 * pz))
20262 + (((-1.0)
20263 * (1.0)
20264 * pp)));
20265 if (IKabs(
20266 evalcond
20267 [0])
20268 < 0.0000010000000000
20269 && IKabs(
20270 evalcond
20271 [1])
20272 < 0.0000010000000000
20273 && IKabs(
20274 evalcond
20275 [2])
20276 < 0.0000010000000000
20277 && IKabs(
20278 evalcond
20279 [3])
20280 < 0.0000010000000000
20281 && IKabs(
20282 evalcond
20283 [4])
20284 < 0.0000010000000000
20285 && IKabs(
20286 evalcond
20287 [5])
20288 < 0.0000010000000000)
20289 {
20290 bgotonextstatement
20291 = false;
20292 {
20293 IkReal j4array
20294 [4],
20295 cj4array
20296 [4],
20297 sj4array
20298 [4];
20299 bool j4valid
20300 [4]
20301 = {false};
20302 _nj4
20303 = 4;
20304 j4array
20305 [0]
20306 = 0;
20307 sj4array
20308 [0]
20309 = IKsin(
20310 j4array
20311 [0]);
20312 cj4array
20313 [0]
20314 = IKcos(
20315 j4array
20316 [0]);
20317 j4array
20318 [1]
20319 = 1.5707963267949;
20320 sj4array
20321 [1]
20322 = IKsin(
20323 j4array
20324 [1]);
20325 cj4array
20326 [1]
20327 = IKcos(
20328 j4array
20329 [1]);
20330 j4array
20331 [2]
20332 = 3.14159265358979;
20333 sj4array
20334 [2]
20335 = IKsin(
20336 j4array
20337 [2]);
20338 cj4array
20339 [2]
20340 = IKcos(
20341 j4array
20342 [2]);
20343 j4array
20344 [3]
20345 = -1.5707963267949;
20346 sj4array
20347 [3]
20348 = IKsin(
20349 j4array
20350 [3]);
20351 cj4array
20352 [3]
20353 = IKcos(
20354 j4array
20355 [3]);
20356 if (j4array
20357 [0]
20358 > IKPI)
20359 {
20360 j4array
20361 [0]
20362 -= IK2PI;
20363 }
20364 else if (
20365 j4array
20366 [0]
20367 < -IKPI)
20368 {
20369 j4array
20370 [0]
20371 += IK2PI;
20372 }
20373 j4valid
20374 [0]
20375 = true;
20376 if (j4array
20377 [1]
20378 > IKPI)
20379 {
20380 j4array
20381 [1]
20382 -= IK2PI;
20383 }
20384 else if (
20385 j4array
20386 [1]
20387 < -IKPI)
20388 {
20389 j4array
20390 [1]
20391 += IK2PI;
20392 }
20393 j4valid
20394 [1]
20395 = true;
20396 if (j4array
20397 [2]
20398 > IKPI)
20399 {
20400 j4array
20401 [2]
20402 -= IK2PI;
20403 }
20404 else if (
20405 j4array
20406 [2]
20407 < -IKPI)
20408 {
20409 j4array
20410 [2]
20411 += IK2PI;
20412 }
20413 j4valid
20414 [2]
20415 = true;
20416 if (j4array
20417 [3]
20418 > IKPI)
20419 {
20420 j4array
20421 [3]
20422 -= IK2PI;
20423 }
20424 else if (
20425 j4array
20426 [3]
20427 < -IKPI)
20428 {
20429 j4array
20430 [3]
20431 += IK2PI;
20432 }
20433 j4valid
20434 [3]
20435 = true;
20436 for (
20437 int ij4
20438 = 0;
20439 ij4
20440 < 4;
20441 ++ij4)
20442 {
20443 if (!j4valid
20444 [ij4])
20445 {
20446 continue;
20447 }
20448 _ij4[0]
20449 = ij4;
20450 _ij4[1]
20451 = -1;
20452 for (
20453 int iij4
20454 = ij4
20455 + 1;
20456 iij4
20457 < 4;
20458 ++iij4)
20459 {
20460 if (j4valid
20461 [iij4]
20462 && IKabs(
20463 cj4array
20464 [ij4]
20465 - cj4array
20466 [iij4])
20467 < IKFAST_SOLUTION_THRESH
20468 && IKabs(
20469 sj4array
20470 [ij4]
20471 - sj4array
20472 [iij4])
20473 < IKFAST_SOLUTION_THRESH)
20474 {
20475 j4valid
20476 [iij4]
20477 = false;
20478 _ij4[1]
20479 = iij4;
20480 break;
20481 }
20482 }
20483 j4 = j4array
20484 [ij4];
20485 cj4 = cj4array
20486 [ij4];
20487 sj4 = sj4array
20488 [ij4];
20489
20490 rotationfunction0(
20491 solutions);
20492 }
20493 }
20494 }
20495 } while (0);
20496 if (bgotonextstatement)
20497 {
20498 bool
20499 bgotonextstatement
20500 = true;
20501 do
20502 {
20503 if (1)
20504 {
20505 bgotonextstatement
20506 = false;
20507 continue; // branch miss [j4]
20508 }
20509 } while (0);
20510 if (bgotonextstatement)
20511 {
20512 }
20513 }
20514 }
20515 }
20516 }
20517 }
20518 }
20519 }
20520 else
20521 {
20522 {
20523 IkReal j4array[1],
20524 cj4array[1],
20525 sj4array[1];
20526 bool j4valid[1]
20527 = {false};
20528 _nj4 = 1;
20529 IkReal x1504
20530 = ((1.51009803921569)
20531 * px);
20532 IkReal x1505
20533 = ((1.32323529411765)
20534 * cj9);
20535 IkReal x1506
20536 = (px * x1505);
20537 IkReal x1507
20538 = ((3.92156862745098)
20539 * pp);
20540 IkReal x1508
20541 = (px * x1507);
20542 IkReal x1509
20543 = ((1.51009803921569)
20544 * py);
20545 IkReal x1510
20546 = (cj8 * sj8);
20547 IkReal x1511
20548 = cj8 * cj8;
20549 IkReal x1512
20550 = (py * x1505);
20551 IkReal x1513
20552 = ((3.92156862745098)
20553 * cj8 * pp
20554 * sj8);
20555 IkReal x1514
20556 = (py * x1507);
20557 CheckValue<IkReal>
20558 x1515
20559 = IKPowWithIntegerCheck(
20560 IKsign((
20561 (((-1.0)
20562 * (1.0)
20563 * sj8
20564 * (pz
20565 * pz)))
20566 + ((pp
20567 * sj8)))),
20568 -1);
20569 if (!x1515.valid)
20570 {
20571 continue;
20572 }
20573 CheckValue<IkReal> x1516 = IKatan2WithCheck(
20574 IkReal(
20575 ((((-1.0)
20576 * x1506
20577 * x1511))
20578 + (((-1.0)
20579 * x1510
20580 * x1512))
20581 + (((-1.0)
20582 * x1508))
20583 + (((-1.0)
20584 * x1504
20585 * x1511))
20586 + ((x1508
20587 * x1511))
20588 + ((py
20589 * x1513))
20590 + (((-1.0)
20591 * x1509
20592 * x1510))
20593 + x1504
20594 + x1506)),
20595 ((((-1.0)
20596 * x1512))
20597 + ((px * x1513))
20598 + (((-1.0)
20599 * x1506
20600 * x1510))
20601 + x1514
20602 + (((-1.0)
20603 * x1511
20604 * x1514))
20605 + ((x1509
20606 * x1511))
20607 + (((-1.0)
20608 * x1504
20609 * x1510))
20610 + (((-1.0)
20611 * x1509))
20612 + ((x1511
20613 * x1512))),
20614 IKFAST_ATAN2_MAGTHRESH);
20615 if (!x1516.valid)
20616 {
20617 continue;
20618 }
20619 j4array[0]
20620 = ((-1.5707963267949)
20621 + (((1.5707963267949)
20622 * (x1515
20623 .value)))
20624 + (x1516
20625 .value));
20626 sj4array[0] = IKsin(
20627 j4array[0]);
20628 cj4array[0] = IKcos(
20629 j4array[0]);
20630 if (j4array[0] > IKPI)
20631 {
20632 j4array[0] -= IK2PI;
20633 }
20634 else if (
20635 j4array[0]
20636 < -IKPI)
20637 {
20638 j4array[0] += IK2PI;
20639 }
20640 j4valid[0] = true;
20641 for (int ij4 = 0;
20642 ij4 < 1;
20643 ++ij4)
20644 {
20645 if (!j4valid[ij4])
20646 {
20647 continue;
20648 }
20649 _ij4[0] = ij4;
20650 _ij4[1] = -1;
20651 for (int iij4
20652 = ij4 + 1;
20653 iij4 < 1;
20654 ++iij4)
20655 {
20656 if (j4valid[iij4]
20657 && IKabs(
20658 cj4array
20659 [ij4]
20660 - cj4array
20661 [iij4])
20662 < IKFAST_SOLUTION_THRESH
20663 && IKabs(
20664 sj4array
20665 [ij4]
20666 - sj4array
20667 [iij4])
20668 < IKFAST_SOLUTION_THRESH)
20669 {
20670 j4valid[iij4]
20671 = false;
20672 _ij4[1] = iij4;
20673 break;
20674 }
20675 }
20676 j4 = j4array[ij4];
20677 cj4 = cj4array[ij4];
20678 sj4 = sj4array[ij4];
20679 {
20680 IkReal
20681 evalcond[5];
20682 IkReal x1517
20683 = ((1.32323529411765)
20684 * cj9);
20685 IkReal x1518
20686 = ((3.92156862745098)
20687 * pp);
20688 IkReal x1519
20689 = IKsin(j4);
20690 IkReal x1520
20691 = (px
20692 * x1519);
20693 IkReal x1521
20694 = IKcos(j4);
20695 IkReal x1522
20696 = ((1.0)
20697 * x1521);
20698 IkReal x1523
20699 = (py
20700 * x1522);
20701 IkReal x1524
20702 = (px
20703 * x1522);
20704 IkReal x1525
20705 = (py
20706 * x1519);
20707 IkReal x1526
20708 = ((1.0)
20709 * x1525);
20710 IkReal x1527
20711 = (px
20712 * x1521);
20713 IkReal x1528
20714 = (sj8
20715 * x1520);
20716 IkReal x1529
20717 = ((0.09)
20718 * cj8);
20719 evalcond[0]
20720 = ((((-1.0)
20721 * sj8
20722 * x1517))
20723 + (((-1.0)
20724 * x1523))
20725 + ((sj8
20726 * x1518))
20727 + x1520
20728 + (((-1.0)
20729 * (1.51009803921569)
20730 * sj8)));
20731 evalcond[1]
20732 = ((((-1.0)
20733 * (1.51009803921569)
20734 * cj8))
20735 + (((-1.0)
20736 * x1526))
20737 + ((cj8
20738 * x1518))
20739 + (((-1.0)
20740 * x1524))
20741 + (((-1.0)
20742 * cj8
20743 * x1517)));
20744 evalcond[2]
20745 = (((sj8
20746 * x1527))
20747 + (((-1.0)
20748 * cj8
20749 * x1523))
20750 + ((cj8
20751 * x1520))
20752 + ((sj8
20753 * x1525)));
20754 evalcond[3]
20755 = ((-1.51009803921569)
20756 + x1518
20757 + (((-1.0)
20758 * cj8
20759 * x1526))
20760 + (((-1.0)
20761 * sj8
20762 * x1523))
20763 + (((-1.0)
20764 * x1517))
20765 + (((-1.0)
20766 * cj8
20767 * x1524))
20768 + x1528);
20769 evalcond[4]
20770 = ((-0.2125)
20771 + ((x1527
20772 * x1529))
20773 + (((0.09)
20774 * py
20775 * sj8
20776 * x1521))
20777 + ((x1525
20778 * x1529))
20779 + (((1.1)
20780 * pz))
20781 + (((-1.0)
20782 * (1.0)
20783 * pp))
20784 + (((-0.09)
20785 * x1528)));
20786 if (IKabs(evalcond
20787 [0])
20788 > IKFAST_EVALCOND_THRESH
20789 || IKabs(
20790 evalcond
20791 [1])
20792 > IKFAST_EVALCOND_THRESH
20793 || IKabs(
20794 evalcond
20795 [2])
20796 > IKFAST_EVALCOND_THRESH
20797 || IKabs(
20798 evalcond
20799 [3])
20800 > IKFAST_EVALCOND_THRESH
20801 || IKabs(
20802 evalcond
20803 [4])
20804 > IKFAST_EVALCOND_THRESH)
20805 {
20806 continue;
20807 }
20808 }
20809
20810 rotationfunction0(
20811 solutions);
20812 }
20813 }
20814 }
20815 }
20816 }
20817 else
20818 {
20819 {
20820 IkReal j4array[1],
20821 cj4array[1],
20822 sj4array[1];
20823 bool j4valid[1] = {false};
20824 _nj4 = 1;
20825 IkReal x1530
20826 = ((1.51009803921569)
20827 * cj8 * sj8);
20828 IkReal x1531 = cj8 * cj8;
20829 IkReal x1532
20830 = ((1.51009803921569)
20831 * x1531);
20832 IkReal x1533
20833 = ((1.32323529411765)
20834 * cj8 * cj9 * sj8);
20835 IkReal x1534
20836 = ((3.92156862745098)
20837 * cj8 * pp * sj8);
20838 IkReal x1535
20839 = ((1.32323529411765)
20840 * cj9 * x1531);
20841 IkReal x1536
20842 = ((3.92156862745098)
20843 * pp * x1531);
20844 CheckValue<IkReal> x1537
20845 = IKatan2WithCheck(
20846 IkReal((
20847 (((-1.0) * py
20848 * x1536))
20849 + ((px
20850 * x1534))
20851 + (((-1.0)
20852 * px
20853 * x1530))
20854 + (((-1.0)
20855 * px
20856 * x1533))
20857 + ((py
20858 * x1535))
20859 + ((py
20860 * x1532)))),
20861 (((px * x1532))
20862 + (((-1.0) * py
20863 * x1534))
20864 + (((-1.0) * px
20865 * x1536))
20866 + ((px * x1535))
20867 + ((py * x1533))
20868 + ((py
20869 * x1530))),
20870 IKFAST_ATAN2_MAGTHRESH);
20871 if (!x1537.valid)
20872 {
20873 continue;
20874 }
20875 CheckValue<IkReal> x1538
20876 = IKPowWithIntegerCheck(
20877 IKsign(
20878 (((cj8
20879 * (pz
20880 * pz)))
20881 + (((-1.0)
20882 * cj8
20883 * pp)))),
20884 -1);
20885 if (!x1538.valid)
20886 {
20887 continue;
20888 }
20889 j4array[0]
20890 = ((-1.5707963267949)
20891 + (x1537.value)
20892 + (((1.5707963267949)
20893 * (x1538
20894 .value))));
20895 sj4array[0]
20896 = IKsin(j4array[0]);
20897 cj4array[0]
20898 = IKcos(j4array[0]);
20899 if (j4array[0] > IKPI)
20900 {
20901 j4array[0] -= IK2PI;
20902 }
20903 else if (
20904 j4array[0] < -IKPI)
20905 {
20906 j4array[0] += IK2PI;
20907 }
20908 j4valid[0] = true;
20909 for (int ij4 = 0; ij4 < 1;
20910 ++ij4)
20911 {
20912 if (!j4valid[ij4])
20913 {
20914 continue;
20915 }
20916 _ij4[0] = ij4;
20917 _ij4[1] = -1;
20918 for (int iij4 = ij4 + 1;
20919 iij4 < 1;
20920 ++iij4)
20921 {
20922 if (j4valid[iij4]
20923 && IKabs(
20924 cj4array
20925 [ij4]
20926 - cj4array
20927 [iij4])
20928 < IKFAST_SOLUTION_THRESH
20929 && IKabs(
20930 sj4array
20931 [ij4]
20932 - sj4array
20933 [iij4])
20934 < IKFAST_SOLUTION_THRESH)
20935 {
20936 j4valid[iij4]
20937 = false;
20938 _ij4[1] = iij4;
20939 break;
20940 }
20941 }
20942 j4 = j4array[ij4];
20943 cj4 = cj4array[ij4];
20944 sj4 = sj4array[ij4];
20945 {
20946 IkReal evalcond[5];
20947 IkReal x1539
20948 = ((1.32323529411765)
20949 * cj9);
20950 IkReal x1540
20951 = ((3.92156862745098)
20952 * pp);
20953 IkReal x1541
20954 = IKsin(j4);
20955 IkReal x1542
20956 = (px * x1541);
20957 IkReal x1543
20958 = IKcos(j4);
20959 IkReal x1544
20960 = ((1.0) * x1543);
20961 IkReal x1545
20962 = (py * x1544);
20963 IkReal x1546
20964 = (px * x1544);
20965 IkReal x1547
20966 = (py * x1541);
20967 IkReal x1548
20968 = ((1.0) * x1547);
20969 IkReal x1549
20970 = (px * x1543);
20971 IkReal x1550
20972 = (sj8 * x1542);
20973 IkReal x1551
20974 = ((0.09) * cj8);
20975 evalcond[0]
20976 = (x1542
20977 + (((-1.0)
20978 * sj8
20979 * x1539))
20980 + ((sj8
20981 * x1540))
20982 + (((-1.0)
20983 * x1545))
20984 + (((-1.0)
20985 * (1.51009803921569)
20986 * sj8)));
20987 evalcond[1]
20988 = ((((-1.0)
20989 * (1.51009803921569)
20990 * cj8))
20991 + ((cj8
20992 * x1540))
20993 + (((-1.0)
20994 * x1546))
20995 + (((-1.0)
20996 * cj8
20997 * x1539))
20998 + (((-1.0)
20999 * x1548)));
21000 evalcond[2]
21001 = (((sj8 * x1547))
21002 + (((-1.0)
21003 * cj8
21004 * x1545))
21005 + ((cj8
21006 * x1542))
21007 + ((sj8
21008 * x1549)));
21009 evalcond[3]
21010 = ((-1.51009803921569)
21011 + (((-1.0)
21012 * cj8
21013 * x1546))
21014 + x1540 + x1550
21015 + (((-1.0)
21016 * x1539))
21017 + (((-1.0)
21018 * cj8
21019 * x1548))
21020 + (((-1.0)
21021 * sj8
21022 * x1545)));
21023 evalcond[4]
21024 = ((-0.2125)
21025 + (((1.1)
21026 * pz))
21027 + ((x1549
21028 * x1551))
21029 + (((-1.0)
21030 * (1.0)
21031 * pp))
21032 + (((-0.09)
21033 * x1550))
21034 + (((0.09) * py
21035 * sj8
21036 * x1543))
21037 + ((x1547
21038 * x1551)));
21039 if (IKabs(evalcond[0])
21040 > IKFAST_EVALCOND_THRESH
21041 || IKabs(evalcond
21042 [1])
21043 > IKFAST_EVALCOND_THRESH
21044 || IKabs(evalcond
21045 [2])
21046 > IKFAST_EVALCOND_THRESH
21047 || IKabs(evalcond
21048 [3])
21049 > IKFAST_EVALCOND_THRESH
21050 || IKabs(evalcond
21051 [4])
21052 > IKFAST_EVALCOND_THRESH)
21053 {
21054 continue;
21055 }
21056 }
21057
21058 rotationfunction0(
21059 solutions);
21060 }
21061 }
21062 }
21063 }
21064 }
21065 else
21066 {
21067 {
21068 IkReal j4array[1],
21069 cj4array[1], sj4array[1];
21070 bool j4valid[1] = {false};
21071 _nj4 = 1;
21072 IkReal x1552
21073 = ((1.51009803921569)
21074 * cj8);
21075 IkReal x1553
21076 = ((1.51009803921569)
21077 * sj8);
21078 IkReal x1554
21079 = ((1.32323529411765)
21080 * cj8 * cj9);
21081 IkReal x1555
21082 = ((3.92156862745098)
21083 * cj8 * pp);
21084 IkReal x1556
21085 = ((1.32323529411765)
21086 * cj9 * sj8);
21087 IkReal x1557
21088 = ((3.92156862745098) * pp
21089 * sj8);
21090 CheckValue<IkReal> x1558
21091 = IKatan2WithCheck(
21092 IkReal(
21093 ((((-1.0) * py
21094 * x1554))
21095 + (((-1.0) * py
21096 * x1552))
21097 + (((-1.0) * px
21098 * x1557))
21099 + ((py * x1555))
21100 + ((px * x1556))
21101 + ((px
21102 * x1553)))),
21103 ((((-1.0) * px
21104 * x1552))
21105 + ((py * x1557))
21106 + (((-1.0) * py
21107 * x1553))
21108 + (((-1.0) * py
21109 * x1556))
21110 + (((-1.0) * px
21111 * x1554))
21112 + ((px * x1555))),
21113 IKFAST_ATAN2_MAGTHRESH);
21114 if (!x1558.valid)
21115 {
21116 continue;
21117 }
21118 CheckValue<IkReal> x1559
21119 = IKPowWithIntegerCheck(
21120 IKsign((
21121 pp
21122 + (((-1.0) * (1.0)
21123 * (pz
21124 * pz))))),
21125 -1);
21126 if (!x1559.valid)
21127 {
21128 continue;
21129 }
21130 j4array[0]
21131 = ((-1.5707963267949)
21132 + (x1558.value)
21133 + (((1.5707963267949)
21134 * (x1559.value))));
21135 sj4array[0]
21136 = IKsin(j4array[0]);
21137 cj4array[0]
21138 = IKcos(j4array[0]);
21139 if (j4array[0] > IKPI)
21140 {
21141 j4array[0] -= IK2PI;
21142 }
21143 else if (j4array[0] < -IKPI)
21144 {
21145 j4array[0] += IK2PI;
21146 }
21147 j4valid[0] = true;
21148 for (int ij4 = 0; ij4 < 1;
21149 ++ij4)
21150 {
21151 if (!j4valid[ij4])
21152 {
21153 continue;
21154 }
21155 _ij4[0] = ij4;
21156 _ij4[1] = -1;
21157 for (int iij4 = ij4 + 1;
21158 iij4 < 1;
21159 ++iij4)
21160 {
21161 if (j4valid[iij4]
21162 && IKabs(
21163 cj4array[ij4]
21164 - cj4array
21165 [iij4])
21166 < IKFAST_SOLUTION_THRESH
21167 && IKabs(
21168 sj4array[ij4]
21169 - sj4array
21170 [iij4])
21171 < IKFAST_SOLUTION_THRESH)
21172 {
21173 j4valid[iij4] = false;
21174 _ij4[1] = iij4;
21175 break;
21176 }
21177 }
21178 j4 = j4array[ij4];
21179 cj4 = cj4array[ij4];
21180 sj4 = sj4array[ij4];
21181 {
21182 IkReal evalcond[5];
21183 IkReal x1560
21184 = ((1.32323529411765)
21185 * cj9);
21186 IkReal x1561
21187 = ((3.92156862745098)
21188 * pp);
21189 IkReal x1562 = IKsin(j4);
21190 IkReal x1563
21191 = (px * x1562);
21192 IkReal x1564 = IKcos(j4);
21193 IkReal x1565
21194 = ((1.0) * x1564);
21195 IkReal x1566
21196 = (py * x1565);
21197 IkReal x1567
21198 = (px * x1565);
21199 IkReal x1568
21200 = (py * x1562);
21201 IkReal x1569
21202 = ((1.0) * x1568);
21203 IkReal x1570
21204 = (px * x1564);
21205 IkReal x1571
21206 = (sj8 * x1563);
21207 IkReal x1572
21208 = ((0.09) * cj8);
21209 evalcond[0]
21210 = (((sj8 * x1561))
21211 + x1563
21212 + (((-1.0)
21213 * (1.51009803921569)
21214 * sj8))
21215 + (((-1.0) * sj8
21216 * x1560))
21217 + (((-1.0)
21218 * x1566)));
21219 evalcond[1]
21220 = ((((-1.0)
21221 * (1.51009803921569)
21222 * cj8))
21223 + (((-1.0) * cj8
21224 * x1560))
21225 + (((-1.0)
21226 * x1569))
21227 + ((cj8 * x1561))
21228 + (((-1.0)
21229 * x1567)));
21230 evalcond[2]
21231 = ((((-1.0) * cj8
21232 * x1566))
21233 + ((sj8 * x1570))
21234 + ((cj8 * x1563))
21235 + ((sj8 * x1568)));
21236 evalcond[3]
21237 = ((-1.51009803921569)
21238 + (((-1.0)
21239 * x1560))
21240 + (((-1.0) * sj8
21241 * x1566))
21242 + x1571 + x1561
21243 + (((-1.0) * cj8
21244 * x1569))
21245 + (((-1.0) * cj8
21246 * x1567)));
21247 evalcond[4]
21248 = ((-0.2125)
21249 + (((-0.09)
21250 * x1571))
21251 + ((x1570 * x1572))
21252 + (((0.09) * py
21253 * sj8 * x1564))
21254 + (((1.1) * pz))
21255 + (((-1.0) * (1.0)
21256 * pp))
21257 + ((x1568
21258 * x1572)));
21259 if (IKabs(evalcond[0])
21260 > IKFAST_EVALCOND_THRESH
21261 || IKabs(evalcond[1])
21262 > IKFAST_EVALCOND_THRESH
21263 || IKabs(evalcond[2])
21264 > IKFAST_EVALCOND_THRESH
21265 || IKabs(evalcond[3])
21266 > IKFAST_EVALCOND_THRESH
21267 || IKabs(evalcond[4])
21268 > IKFAST_EVALCOND_THRESH)
21269 {
21270 continue;
21271 }
21272 }
21273
21274 rotationfunction0(
21275 solutions);
21276 }
21277 }
21278 }
21279 }
21280 }
21281 } while (0);
21282 if (bgotonextstatement)
21283 {
21284 bool bgotonextstatement = true;
21285 do
21286 {
21287 IkReal x1573 = ((0.3) * cj9);
21288 IkReal x1574 = ((0.045) * sj9);
21289 evalcond[0]
21290 = ((-3.14159265358979)
21291 + (IKfmod(
21292 ((3.14159265358979)
21293 + (IKabs((
21294 (-3.14159265358979)
21295 + j6)))),
21296 6.28318530717959)));
21297 evalcond[1]
21298 = ((0.39655)
21299 + (((0.0765) * sj9))
21300 + (((0.32595) * cj9))
21301 + (((-1.0) * (1.0) * pp)));
21302 evalcond[2]
21303 = ((-0.55)
21304 + (((-1.0) * (1.0) * pz))
21305 + (((-1.0) * x1573))
21306 + (((-1.0) * x1574)));
21307 evalcond[3]
21308 = ((0.55) + x1574 + x1573 + pz);
21309 if (IKabs(evalcond[0])
21310 < 0.0000010000000000
21311 && IKabs(evalcond[1])
21312 < 0.0000010000000000
21313 && IKabs(evalcond[2])
21314 < 0.0000010000000000
21315 && IKabs(evalcond[3])
21316 < 0.0000010000000000)
21317 {
21318 bgotonextstatement = false;
21319 {
21320 IkReal j4eval[3];
21321 sj6 = 0;
21322 cj6 = -1.0;
21323 j6 = 3.14159265358979;
21324 IkReal x1575
21325 = (pp
21326 + (((-1.0) * (1.0)
21327 * (pz * pz))));
21328 IkReal x1576
21329 = ((1.51009803921569)
21330 * cj8);
21331 IkReal x1577
21332 = ((1.51009803921569)
21333 * sj8);
21334 IkReal x1578
21335 = ((1.32323529411765) * cj8
21336 * cj9);
21337 IkReal x1579
21338 = ((3.92156862745098) * cj8
21339 * pp);
21340 IkReal x1580
21341 = ((1.32323529411765) * cj9
21342 * sj8);
21343 IkReal x1581
21344 = ((3.92156862745098) * pp
21345 * sj8);
21346 j4eval[0] = x1575;
21347 j4eval[1]
21348 = ((IKabs(
21349 ((((-1.0) * py
21350 * x1579))
21351 + ((px * x1577))
21352 + ((py * x1578))
21353 + (((-1.0) * px
21354 * x1581))
21355 + ((py * x1576))
21356 + ((px * x1580)))))
21357 + (IKabs(
21358 (((px * x1578))
21359 + (((-1.0) * py
21360 * x1580))
21361 + ((px * x1576))
21362 + (((-1.0) * px
21363 * x1579))
21364 + ((py * x1581))
21365 + (((-1.0) * py
21366 * x1577))))));
21367 j4eval[2] = IKsign(x1575);
21368 if (IKabs(j4eval[0])
21369 < 0.0000010000000000
21370 || IKabs(j4eval[1])
21371 < 0.0000010000000000
21372 || IKabs(j4eval[2])
21373 < 0.0000010000000000)
21374 {
21375 {
21376 IkReal j4eval[2];
21377 sj6 = 0;
21378 cj6 = -1.0;
21379 j6 = 3.14159265358979;
21380 IkReal x1582
21381 = (((cj8 * (pz * pz)))
21382 + (((-1.0) * (1.0)
21383 * cj8 * pp)));
21384 j4eval[0] = x1582;
21385 j4eval[1] = IKsign(x1582);
21386 if (IKabs(j4eval[0])
21387 < 0.0000010000000000
21388 || IKabs(j4eval[1])
21389 < 0.0000010000000000)
21390 {
21391 {
21392 IkReal j4eval[2];
21393 sj6 = 0;
21394 cj6 = -1.0;
21395 j6 = 3.14159265358979;
21396 IkReal x1583
21397 = ((((-1.0) * (1.0)
21398 * cj8
21399 * (pz * pz)))
21400 + ((cj8 * pp)));
21401 j4eval[0] = x1583;
21402 j4eval[1]
21403 = IKsign(x1583);
21404 if (IKabs(j4eval[0])
21405 < 0.0000010000000000
21406 || IKabs(j4eval[1])
21407 < 0.0000010000000000)
21408 {
21409 {
21410 IkReal evalcond[6];
21411 bool
21412 bgotonextstatement
21413 = true;
21414 do
21415 {
21416 evalcond[0]
21417 = ((-3.14159265358979)
21418 + (IKfmod(
21419 ((3.14159265358979)
21420 + (IKabs((
21421 (-1.5707963267949)
21422 + j8)))),
21423 6.28318530717959)));
21424 if (IKabs(evalcond
21425 [0])
21426 < 0.0000010000000000)
21427 {
21428 bgotonextstatement
21429 = false;
21430 {
21431 IkReal
21432 j4eval[3];
21433 sj6 = 0;
21434 cj6 = -1.0;
21435 j6 = 3.14159265358979;
21436 sj8 = 1.0;
21437 cj8 = 0;
21438 j8 = 1.5707963267949;
21439 IkReal x1584
21440 = (pp
21441 + (((-1.0)
21442 * (1.0)
21443 * (pz
21444 * pz))));
21445 IkReal x1585
21446 = ((13497.0)
21447 * cj9);
21448 IkReal x1586
21449 = ((40000.0)
21450 * pp);
21451 j4eval[0]
21452 = x1584;
21453 j4eval[1]
21454 = ((IKabs((
21455 (((15403.0)
21456 * px))
21457 + ((px
21458 * x1585))
21459 + (((-1.0)
21460 * px
21461 * x1586)))))
21462 + (IKabs((
21463 (((-1.0)
21464 * py
21465 * x1585))
21466 + ((py
21467 * x1586))
21468 + (((-1.0)
21469 * (15403.0)
21470 * py))))));
21471 j4eval[2]
21472 = IKsign(
21473 x1584);
21474 if (IKabs(
21475 j4eval
21476 [0])
21477 < 0.0000010000000000
21478 || IKabs(
21479 j4eval
21480 [1])
21481 < 0.0000010000000000
21482 || IKabs(
21483 j4eval
21484 [2])
21485 < 0.0000010000000000)
21486 {
21487 {
21488 IkReal j4eval
21489 [3];
21490 sj6 = 0;
21491 cj6 = -1.0;
21492 j6 = 3.14159265358979;
21493 sj8 = 1.0;
21494 cj8 = 0;
21495 j8 = 1.5707963267949;
21496 IkReal
21497 x1587
21498 = pz
21499 * pz;
21500 IkReal
21501 x1588
21502 = ((80.0)
21503 * pp);
21504 IkReal
21505 x1589
21506 = ((88.0)
21507 * pz);
21508 j4eval[0]
21509 = (pp
21510 + (((-1.0)
21511 * x1587)));
21512 j4eval[1]
21513 = ((IKabs((
21514 ((py
21515 * x1589))
21516 + (((17.0)
21517 * py))
21518 + ((py
21519 * x1588)))))
21520 + (IKabs((
21521 ((px
21522 * x1588))
21523 + (((17.0)
21524 * px))
21525 + ((px
21526 * x1589))))));
21527 j4eval[2] = IKsign((
21528 (((9.0)
21529 * pp))
21530 + (((-9.0)
21531 * x1587))));
21532 if (IKabs(
21533 j4eval
21534 [0])
21535 < 0.0000010000000000
21536 || IKabs(
21537 j4eval
21538 [1])
21539 < 0.0000010000000000
21540 || IKabs(
21541 j4eval
21542 [2])
21543 < 0.0000010000000000)
21544 {
21545 {
21546 IkReal evalcond
21547 [5];
21548 bool
21549 bgotonextstatement
21550 = true;
21551 do
21552 {
21553 IkReal
21554 x1590
21555 = ((-1.51009803921569)
21556 + (((-1.0)
21557 * (1.32323529411765)
21558 * cj9))
21559 + (((3.92156862745098)
21560 * pp)));
21561 evalcond
21562 [0]
21563 = ((IKabs(
21564 py))
21565 + (IKabs(
21566 px)));
21567 evalcond
21568 [1]
21569 = x1590;
21570 evalcond
21571 [2]
21572 = 0;
21573 evalcond
21574 [3]
21575 = x1590;
21576 evalcond
21577 [4]
21578 = ((-0.2125)
21579 + (((-1.0)
21580 * (1.1)
21581 * pz))
21582 + (((-1.0)
21583 * (1.0)
21584 * pp)));
21585 if (IKabs(
21586 evalcond
21587 [0])
21588 < 0.0000010000000000
21589 && IKabs(
21590 evalcond
21591 [1])
21592 < 0.0000010000000000
21593 && IKabs(
21594 evalcond
21595 [2])
21596 < 0.0000010000000000
21597 && IKabs(
21598 evalcond
21599 [3])
21600 < 0.0000010000000000
21601 && IKabs(
21602 evalcond
21603 [4])
21604 < 0.0000010000000000)
21605 {
21606 bgotonextstatement
21607 = false;
21608 {
21609 IkReal j4array
21610 [4],
21611 cj4array
21612 [4],
21613 sj4array
21614 [4];
21615 bool j4valid
21616 [4]
21617 = {false};
21618 _nj4
21619 = 4;
21620 j4array
21621 [0]
21622 = 0;
21623 sj4array
21624 [0]
21625 = IKsin(
21626 j4array
21627 [0]);
21628 cj4array
21629 [0]
21630 = IKcos(
21631 j4array
21632 [0]);
21633 j4array
21634 [1]
21635 = 1.5707963267949;
21636 sj4array
21637 [1]
21638 = IKsin(
21639 j4array
21640 [1]);
21641 cj4array
21642 [1]
21643 = IKcos(
21644 j4array
21645 [1]);
21646 j4array
21647 [2]
21648 = 3.14159265358979;
21649 sj4array
21650 [2]
21651 = IKsin(
21652 j4array
21653 [2]);
21654 cj4array
21655 [2]
21656 = IKcos(
21657 j4array
21658 [2]);
21659 j4array
21660 [3]
21661 = -1.5707963267949;
21662 sj4array
21663 [3]
21664 = IKsin(
21665 j4array
21666 [3]);
21667 cj4array
21668 [3]
21669 = IKcos(
21670 j4array
21671 [3]);
21672 if (j4array
21673 [0]
21674 > IKPI)
21675 {
21676 j4array
21677 [0]
21678 -= IK2PI;
21679 }
21680 else if (
21681 j4array
21682 [0]
21683 < -IKPI)
21684 {
21685 j4array
21686 [0]
21687 += IK2PI;
21688 }
21689 j4valid
21690 [0]
21691 = true;
21692 if (j4array
21693 [1]
21694 > IKPI)
21695 {
21696 j4array
21697 [1]
21698 -= IK2PI;
21699 }
21700 else if (
21701 j4array
21702 [1]
21703 < -IKPI)
21704 {
21705 j4array
21706 [1]
21707 += IK2PI;
21708 }
21709 j4valid
21710 [1]
21711 = true;
21712 if (j4array
21713 [2]
21714 > IKPI)
21715 {
21716 j4array
21717 [2]
21718 -= IK2PI;
21719 }
21720 else if (
21721 j4array
21722 [2]
21723 < -IKPI)
21724 {
21725 j4array
21726 [2]
21727 += IK2PI;
21728 }
21729 j4valid
21730 [2]
21731 = true;
21732 if (j4array
21733 [3]
21734 > IKPI)
21735 {
21736 j4array
21737 [3]
21738 -= IK2PI;
21739 }
21740 else if (
21741 j4array
21742 [3]
21743 < -IKPI)
21744 {
21745 j4array
21746 [3]
21747 += IK2PI;
21748 }
21749 j4valid
21750 [3]
21751 = true;
21752 for (
21753 int ij4
21754 = 0;
21755 ij4
21756 < 4;
21757 ++ij4)
21758 {
21759 if (!j4valid
21760 [ij4])
21761 {
21762 continue;
21763 }
21764 _ij4[0]
21765 = ij4;
21766 _ij4[1]
21767 = -1;
21768 for (
21769 int iij4
21770 = ij4
21771 + 1;
21772 iij4
21773 < 4;
21774 ++iij4)
21775 {
21776 if (j4valid
21777 [iij4]
21778 && IKabs(
21779 cj4array
21780 [ij4]
21781 - cj4array
21782 [iij4])
21783 < IKFAST_SOLUTION_THRESH
21784 && IKabs(
21785 sj4array
21786 [ij4]
21787 - sj4array
21788 [iij4])
21789 < IKFAST_SOLUTION_THRESH)
21790 {
21791 j4valid
21792 [iij4]
21793 = false;
21794 _ij4[1]
21795 = iij4;
21796 break;
21797 }
21798 }
21799 j4 = j4array
21800 [ij4];
21801 cj4 = cj4array
21802 [ij4];
21803 sj4 = sj4array
21804 [ij4];
21805
21806 rotationfunction0(
21807 solutions);
21808 }
21809 }
21810 }
21811 } while (
21812 0);
21813 if (bgotonextstatement)
21814 {
21815 bool
21816 bgotonextstatement
21817 = true;
21818 do
21819 {
21820 if (1)
21821 {
21822 bgotonextstatement
21823 = false;
21824 continue; // branch miss [j4]
21825 }
21826 } while (
21827 0);
21828 if (bgotonextstatement)
21829 {
21830 }
21831 }
21832 }
21833 }
21834 else
21835 {
21836 {
21837 IkReal j4array
21838 [1],
21839 cj4array
21840 [1],
21841 sj4array
21842 [1];
21843 bool j4valid
21844 [1]
21845 = {false};
21846 _nj4
21847 = 1;
21848 IkReal
21849 x1591
21850 = ((100.0)
21851 * pp);
21852 IkReal
21853 x1592
21854 = ((110.0)
21855 * pz);
21856 CheckValue<IkReal> x1593 = IKatan2WithCheck(
21857 IkReal((
21858 (((-1.0)
21859 * px
21860 * x1591))
21861 + (((-1.0)
21862 * (21.25)
21863 * px))
21864 + (((-1.0)
21865 * px
21866 * x1592)))),
21867 (((py
21868 * x1591))
21869 + ((py
21870 * x1592))
21871 + (((21.25)
21872 * py))),
21873 IKFAST_ATAN2_MAGTHRESH);
21874 if (!x1593
21875 .valid)
21876 {
21877 continue;
21878 }
21879 CheckValue<IkReal> x1594 = IKPowWithIntegerCheck(
21880 IKsign((
21881 (((-1.0)
21882 * (9.0)
21883 * (pz
21884 * pz)))
21885 + (((9.0)
21886 * pp)))),
21887 -1);
21888 if (!x1594
21889 .valid)
21890 {
21891 continue;
21892 }
21893 j4array
21894 [0]
21895 = ((-1.5707963267949)
21896 + (x1593
21897 .value)
21898 + (((1.5707963267949)
21899 * (x1594
21900 .value))));
21901 sj4array
21902 [0]
21903 = IKsin(
21904 j4array
21905 [0]);
21906 cj4array
21907 [0]
21908 = IKcos(
21909 j4array
21910 [0]);
21911 if (j4array
21912 [0]
21913 > IKPI)
21914 {
21915 j4array
21916 [0]
21917 -= IK2PI;
21918 }
21919 else if (
21920 j4array
21921 [0]
21922 < -IKPI)
21923 {
21924 j4array
21925 [0]
21926 += IK2PI;
21927 }
21928 j4valid
21929 [0]
21930 = true;
21931 for (
21932 int ij4
21933 = 0;
21934 ij4
21935 < 1;
21936 ++ij4)
21937 {
21938 if (!j4valid
21939 [ij4])
21940 {
21941 continue;
21942 }
21943 _ij4[0]
21944 = ij4;
21945 _ij4[1]
21946 = -1;
21947 for (
21948 int iij4
21949 = ij4
21950 + 1;
21951 iij4
21952 < 1;
21953 ++iij4)
21954 {
21955 if (j4valid
21956 [iij4]
21957 && IKabs(
21958 cj4array
21959 [ij4]
21960 - cj4array
21961 [iij4])
21962 < IKFAST_SOLUTION_THRESH
21963 && IKabs(
21964 sj4array
21965 [ij4]
21966 - sj4array
21967 [iij4])
21968 < IKFAST_SOLUTION_THRESH)
21969 {
21970 j4valid
21971 [iij4]
21972 = false;
21973 _ij4[1]
21974 = iij4;
21975 break;
21976 }
21977 }
21978 j4 = j4array
21979 [ij4];
21980 cj4 = cj4array
21981 [ij4];
21982 sj4 = sj4array
21983 [ij4];
21984 {
21985 IkReal evalcond
21986 [3];
21987 IkReal
21988 x1595
21989 = IKcos(
21990 j4);
21991 IkReal
21992 x1596
21993 = ((1.0)
21994 * x1595);
21995 IkReal
21996 x1597
21997 = IKsin(
21998 j4);
21999 IkReal
22000 x1598
22001 = (px
22002 * x1597);
22003 evalcond
22004 [0]
22005 = ((((-1.0)
22006 * px
22007 * x1596))
22008 + (((-1.0)
22009 * py
22010 * x1597)));
22011 evalcond
22012 [1]
22013 = ((-1.51009803921569)
22014 + (((-1.0)
22015 * py
22016 * x1596))
22017 + (((-1.0)
22018 * (1.32323529411765)
22019 * cj9))
22020 + (((3.92156862745098)
22021 * pp))
22022 + x1598);
22023 evalcond
22024 [2]
22025 = ((-0.2125)
22026 + (((-1.0)
22027 * (1.1)
22028 * pz))
22029 + (((-0.09)
22030 * x1598))
22031 + (((0.09)
22032 * py
22033 * x1595))
22034 + (((-1.0)
22035 * (1.0)
22036 * pp)));
22037 if (IKabs(
22038 evalcond
22039 [0])
22040 > IKFAST_EVALCOND_THRESH
22041 || IKabs(
22042 evalcond
22043 [1])
22044 > IKFAST_EVALCOND_THRESH
22045 || IKabs(
22046 evalcond
22047 [2])
22048 > IKFAST_EVALCOND_THRESH)
22049 {
22050 continue;
22051 }
22052 }
22053
22054 rotationfunction0(
22055 solutions);
22056 }
22057 }
22058 }
22059 }
22060 }
22061 else
22062 {
22063 {
22064 IkReal j4array
22065 [1],
22066 cj4array
22067 [1],
22068 sj4array
22069 [1];
22070 bool j4valid
22071 [1]
22072 = {false};
22073 _nj4 = 1;
22074 IkReal
22075 x1599
22076 = ((1.32323529411765)
22077 * cj9);
22078 IkReal
22079 x1600
22080 = ((3.92156862745098)
22081 * pp);
22082 CheckValue<IkReal> x1601 = IKPowWithIntegerCheck(
22083 IKsign((
22084 pp
22085 + (((-1.0)
22086 * (1.0)
22087 * (pz
22088 * pz))))),
22089 -1);
22090 if (!x1601
22091 .valid)
22092 {
22093 continue;
22094 }
22095 CheckValue<IkReal> x1602 = IKatan2WithCheck(
22096 IkReal((
22097 ((px
22098 * x1599))
22099 + (((1.51009803921569)
22100 * px))
22101 + (((-1.0)
22102 * px
22103 * x1600)))),
22104 ((((-1.0)
22105 * (1.51009803921569)
22106 * py))
22107 + (((-1.0)
22108 * py
22109 * x1599))
22110 + ((py
22111 * x1600))),
22112 IKFAST_ATAN2_MAGTHRESH);
22113 if (!x1602
22114 .valid)
22115 {
22116 continue;
22117 }
22118 j4array[0]
22119 = ((-1.5707963267949)
22120 + (((1.5707963267949)
22121 * (x1601
22122 .value)))
22123 + (x1602
22124 .value));
22125 sj4array[0] = IKsin(
22126 j4array
22127 [0]);
22128 cj4array[0] = IKcos(
22129 j4array
22130 [0]);
22131 if (j4array
22132 [0]
22133 > IKPI)
22134 {
22135 j4array
22136 [0]
22137 -= IK2PI;
22138 }
22139 else if (
22140 j4array
22141 [0]
22142 < -IKPI)
22143 {
22144 j4array
22145 [0]
22146 += IK2PI;
22147 }
22148 j4valid[0]
22149 = true;
22150 for (
22151 int ij4
22152 = 0;
22153 ij4
22154 < 1;
22155 ++ij4)
22156 {
22157 if (!j4valid
22158 [ij4])
22159 {
22160 continue;
22161 }
22162 _ij4[0]
22163 = ij4;
22164 _ij4[1]
22165 = -1;
22166 for (
22167 int iij4
22168 = ij4
22169 + 1;
22170 iij4
22171 < 1;
22172 ++iij4)
22173 {
22174 if (j4valid
22175 [iij4]
22176 && IKabs(
22177 cj4array
22178 [ij4]
22179 - cj4array
22180 [iij4])
22181 < IKFAST_SOLUTION_THRESH
22182 && IKabs(
22183 sj4array
22184 [ij4]
22185 - sj4array
22186 [iij4])
22187 < IKFAST_SOLUTION_THRESH)
22188 {
22189 j4valid
22190 [iij4]
22191 = false;
22192 _ij4[1]
22193 = iij4;
22194 break;
22195 }
22196 }
22197 j4 = j4array
22198 [ij4];
22199 cj4 = cj4array
22200 [ij4];
22201 sj4 = sj4array
22202 [ij4];
22203 {
22204 IkReal evalcond
22205 [3];
22206 IkReal
22207 x1603
22208 = IKcos(
22209 j4);
22210 IkReal
22211 x1604
22212 = ((1.0)
22213 * x1603);
22214 IkReal
22215 x1605
22216 = IKsin(
22217 j4);
22218 IkReal
22219 x1606
22220 = (px
22221 * x1605);
22222 evalcond
22223 [0]
22224 = ((((-1.0)
22225 * px
22226 * x1604))
22227 + (((-1.0)
22228 * py
22229 * x1605)));
22230 evalcond
22231 [1]
22232 = ((-1.51009803921569)
22233 + (((-1.0)
22234 * (1.32323529411765)
22235 * cj9))
22236 + (((3.92156862745098)
22237 * pp))
22238 + x1606
22239 + (((-1.0)
22240 * py
22241 * x1604)));
22242 evalcond
22243 [2]
22244 = ((-0.2125)
22245 + (((-1.0)
22246 * (1.1)
22247 * pz))
22248 + (((-0.09)
22249 * x1606))
22250 + (((-1.0)
22251 * (1.0)
22252 * pp))
22253 + (((0.09)
22254 * py
22255 * x1603)));
22256 if (IKabs(
22257 evalcond
22258 [0])
22259 > IKFAST_EVALCOND_THRESH
22260 || IKabs(
22261 evalcond
22262 [1])
22263 > IKFAST_EVALCOND_THRESH
22264 || IKabs(
22265 evalcond
22266 [2])
22267 > IKFAST_EVALCOND_THRESH)
22268 {
22269 continue;
22270 }
22271 }
22272
22273 rotationfunction0(
22274 solutions);
22275 }
22276 }
22277 }
22278 }
22279 }
22280 } while (0);
22281 if (bgotonextstatement)
22282 {
22283 bool
22284 bgotonextstatement
22285 = true;
22286 do
22287 {
22288 evalcond[0]
22289 = ((-3.14159265358979)
22290 + (IKfmod(
22291 ((3.14159265358979)
22292 + (IKabs((
22293 (1.5707963267949)
22294 + j8)))),
22295 6.28318530717959)));
22296 if (IKabs(
22297 evalcond
22298 [0])
22299 < 0.0000010000000000)
22300 {
22301 bgotonextstatement
22302 = false;
22303 {
22304 IkReal
22305 j4eval
22306 [3];
22307 sj6 = 0;
22308 cj6 = -1.0;
22309 j6 = 3.14159265358979;
22310 sj8 = -1.0;
22311 cj8 = 0;
22312 j8 = -1.5707963267949;
22313 IkReal x1607
22314 = (pp
22315 + (((-1.0)
22316 * (1.0)
22317 * (pz
22318 * pz))));
22319 IkReal x1608
22320 = ((13497.0)
22321 * cj9);
22322 IkReal x1609
22323 = ((40000.0)
22324 * pp);
22325 j4eval[0]
22326 = x1607;
22327 j4eval[1]
22328 = ((IKabs((
22329 (((-1.0)
22330 * py
22331 * x1609))
22332 + (((15403.0)
22333 * py))
22334 + ((py
22335 * x1608)))))
22336 + (IKabs((
22337 (((-1.0)
22338 * px
22339 * x1608))
22340 + (((-1.0)
22341 * (15403.0)
22342 * px))
22343 + ((px
22344 * x1609))))));
22345 j4eval[2]
22346 = IKsign(
22347 x1607);
22348 if (IKabs(
22349 j4eval
22350 [0])
22351 < 0.0000010000000000
22352 || IKabs(
22353 j4eval
22354 [1])
22355 < 0.0000010000000000
22356 || IKabs(
22357 j4eval
22358 [2])
22359 < 0.0000010000000000)
22360 {
22361 {
22362 IkReal j4eval
22363 [3];
22364 sj6 = 0;
22365 cj6 = -1.0;
22366 j6 = 3.14159265358979;
22367 sj8 = -1.0;
22368 cj8 = 0;
22369 j8 = -1.5707963267949;
22370 IkReal
22371 x1610
22372 = pz
22373 * pz;
22374 IkReal
22375 x1611
22376 = ((80.0)
22377 * pp);
22378 IkReal
22379 x1612
22380 = ((88.0)
22381 * pz);
22382 j4eval
22383 [0]
22384 = (pp
22385 + (((-1.0)
22386 * x1610)));
22387 j4eval
22388 [1]
22389 = ((IKabs((
22390 (((17.0)
22391 * py))
22392 + ((py
22393 * x1612))
22394 + ((py
22395 * x1611)))))
22396 + (IKabs((
22397 (((17.0)
22398 * px))
22399 + ((px
22400 * x1612))
22401 + ((px
22402 * x1611))))));
22403 j4eval[2] = IKsign((
22404 (((-9.0)
22405 * x1610))
22406 + (((9.0)
22407 * pp))));
22408 if (IKabs(
22409 j4eval
22410 [0])
22411 < 0.0000010000000000
22412 || IKabs(
22413 j4eval
22414 [1])
22415 < 0.0000010000000000
22416 || IKabs(
22417 j4eval
22418 [2])
22419 < 0.0000010000000000)
22420 {
22421 {
22422 IkReal evalcond
22423 [5];
22424 bool
22425 bgotonextstatement
22426 = true;
22427 do
22428 {
22429 IkReal
22430 x1613
22431 = ((1.32323529411765)
22432 * cj9);
22433 IkReal
22434 x1614
22435 = ((3.92156862745098)
22436 * pp);
22437 evalcond
22438 [0]
22439 = ((IKabs(
22440 py))
22441 + (IKabs(
22442 px)));
22443 evalcond
22444 [1]
22445 = ((1.51009803921569)
22446 + (((-1.0)
22447 * x1614))
22448 + x1613);
22449 evalcond
22450 [2]
22451 = 0;
22452 evalcond
22453 [3]
22454 = ((-1.51009803921569)
22455 + (((-1.0)
22456 * x1613))
22457 + x1614);
22458 evalcond
22459 [4]
22460 = ((-0.2125)
22461 + (((-1.0)
22462 * (1.1)
22463 * pz))
22464 + (((-1.0)
22465 * (1.0)
22466 * pp)));
22467 if (IKabs(
22468 evalcond
22469 [0])
22470 < 0.0000010000000000
22471 && IKabs(
22472 evalcond
22473 [1])
22474 < 0.0000010000000000
22475 && IKabs(
22476 evalcond
22477 [2])
22478 < 0.0000010000000000
22479 && IKabs(
22480 evalcond
22481 [3])
22482 < 0.0000010000000000
22483 && IKabs(
22484 evalcond
22485 [4])
22486 < 0.0000010000000000)
22487 {
22488 bgotonextstatement
22489 = false;
22490 {
22491 IkReal j4array
22492 [4],
22493 cj4array
22494 [4],
22495 sj4array
22496 [4];
22497 bool j4valid
22498 [4]
22499 = {false};
22500 _nj4
22501 = 4;
22502 j4array
22503 [0]
22504 = 0;
22505 sj4array
22506 [0]
22507 = IKsin(
22508 j4array
22509 [0]);
22510 cj4array
22511 [0]
22512 = IKcos(
22513 j4array
22514 [0]);
22515 j4array
22516 [1]
22517 = 1.5707963267949;
22518 sj4array
22519 [1]
22520 = IKsin(
22521 j4array
22522 [1]);
22523 cj4array
22524 [1]
22525 = IKcos(
22526 j4array
22527 [1]);
22528 j4array
22529 [2]
22530 = 3.14159265358979;
22531 sj4array
22532 [2]
22533 = IKsin(
22534 j4array
22535 [2]);
22536 cj4array
22537 [2]
22538 = IKcos(
22539 j4array
22540 [2]);
22541 j4array
22542 [3]
22543 = -1.5707963267949;
22544 sj4array
22545 [3]
22546 = IKsin(
22547 j4array
22548 [3]);
22549 cj4array
22550 [3]
22551 = IKcos(
22552 j4array
22553 [3]);
22554 if (j4array
22555 [0]
22556 > IKPI)
22557 {
22558 j4array
22559 [0]
22560 -= IK2PI;
22561 }
22562 else if (
22563 j4array
22564 [0]
22565 < -IKPI)
22566 {
22567 j4array
22568 [0]
22569 += IK2PI;
22570 }
22571 j4valid
22572 [0]
22573 = true;
22574 if (j4array
22575 [1]
22576 > IKPI)
22577 {
22578 j4array
22579 [1]
22580 -= IK2PI;
22581 }
22582 else if (
22583 j4array
22584 [1]
22585 < -IKPI)
22586 {
22587 j4array
22588 [1]
22589 += IK2PI;
22590 }
22591 j4valid
22592 [1]
22593 = true;
22594 if (j4array
22595 [2]
22596 > IKPI)
22597 {
22598 j4array
22599 [2]
22600 -= IK2PI;
22601 }
22602 else if (
22603 j4array
22604 [2]
22605 < -IKPI)
22606 {
22607 j4array
22608 [2]
22609 += IK2PI;
22610 }
22611 j4valid
22612 [2]
22613 = true;
22614 if (j4array
22615 [3]
22616 > IKPI)
22617 {
22618 j4array
22619 [3]
22620 -= IK2PI;
22621 }
22622 else if (
22623 j4array
22624 [3]
22625 < -IKPI)
22626 {
22627 j4array
22628 [3]
22629 += IK2PI;
22630 }
22631 j4valid
22632 [3]
22633 = true;
22634 for (
22635 int ij4
22636 = 0;
22637 ij4
22638 < 4;
22639 ++ij4)
22640 {
22641 if (!j4valid
22642 [ij4])
22643 {
22644 continue;
22645 }
22646 _ij4[0]
22647 = ij4;
22648 _ij4[1]
22649 = -1;
22650 for (
22651 int iij4
22652 = ij4
22653 + 1;
22654 iij4
22655 < 4;
22656 ++iij4)
22657 {
22658 if (j4valid
22659 [iij4]
22660 && IKabs(
22661 cj4array
22662 [ij4]
22663 - cj4array
22664 [iij4])
22665 < IKFAST_SOLUTION_THRESH
22666 && IKabs(
22667 sj4array
22668 [ij4]
22669 - sj4array
22670 [iij4])
22671 < IKFAST_SOLUTION_THRESH)
22672 {
22673 j4valid
22674 [iij4]
22675 = false;
22676 _ij4[1]
22677 = iij4;
22678 break;
22679 }
22680 }
22681 j4 = j4array
22682 [ij4];
22683 cj4 = cj4array
22684 [ij4];
22685 sj4 = sj4array
22686 [ij4];
22687
22688 rotationfunction0(
22689 solutions);
22690 }
22691 }
22692 }
22693 } while (
22694 0);
22695 if (bgotonextstatement)
22696 {
22697 bool
22698 bgotonextstatement
22699 = true;
22700 do
22701 {
22702 if (1)
22703 {
22704 bgotonextstatement
22705 = false;
22706 continue; // branch miss [j4]
22707 }
22708 } while (
22709 0);
22710 if (bgotonextstatement)
22711 {
22712 }
22713 }
22714 }
22715 }
22716 else
22717 {
22718 {
22719 IkReal j4array
22720 [1],
22721 cj4array
22722 [1],
22723 sj4array
22724 [1];
22725 bool j4valid
22726 [1]
22727 = {false};
22728 _nj4
22729 = 1;
22730 IkReal
22731 x1615
22732 = ((100.0)
22733 * pp);
22734 IkReal
22735 x1616
22736 = ((110.0)
22737 * pz);
22738 CheckValue<IkReal> x1617 = IKatan2WithCheck(
22739 IkReal((
22740 ((px
22741 * x1616))
22742 + (((21.25)
22743 * px))
22744 + ((px
22745 * x1615)))),
22746 ((((-1.0)
22747 * py
22748 * x1616))
22749 + (((-1.0)
22750 * (21.25)
22751 * py))
22752 + (((-1.0)
22753 * py
22754 * x1615))),
22755 IKFAST_ATAN2_MAGTHRESH);
22756 if (!x1617
22757 .valid)
22758 {
22759 continue;
22760 }
22761 CheckValue<IkReal> x1618 = IKPowWithIntegerCheck(
22762 IKsign((
22763 (((-1.0)
22764 * (9.0)
22765 * (pz
22766 * pz)))
22767 + (((9.0)
22768 * pp)))),
22769 -1);
22770 if (!x1618
22771 .valid)
22772 {
22773 continue;
22774 }
22775 j4array
22776 [0]
22777 = ((-1.5707963267949)
22778 + (x1617
22779 .value)
22780 + (((1.5707963267949)
22781 * (x1618
22782 .value))));
22783 sj4array
22784 [0]
22785 = IKsin(
22786 j4array
22787 [0]);
22788 cj4array
22789 [0]
22790 = IKcos(
22791 j4array
22792 [0]);
22793 if (j4array
22794 [0]
22795 > IKPI)
22796 {
22797 j4array
22798 [0]
22799 -= IK2PI;
22800 }
22801 else if (
22802 j4array
22803 [0]
22804 < -IKPI)
22805 {
22806 j4array
22807 [0]
22808 += IK2PI;
22809 }
22810 j4valid
22811 [0]
22812 = true;
22813 for (
22814 int ij4
22815 = 0;
22816 ij4
22817 < 1;
22818 ++ij4)
22819 {
22820 if (!j4valid
22821 [ij4])
22822 {
22823 continue;
22824 }
22825 _ij4[0]
22826 = ij4;
22827 _ij4[1]
22828 = -1;
22829 for (
22830 int iij4
22831 = ij4
22832 + 1;
22833 iij4
22834 < 1;
22835 ++iij4)
22836 {
22837 if (j4valid
22838 [iij4]
22839 && IKabs(
22840 cj4array
22841 [ij4]
22842 - cj4array
22843 [iij4])
22844 < IKFAST_SOLUTION_THRESH
22845 && IKabs(
22846 sj4array
22847 [ij4]
22848 - sj4array
22849 [iij4])
22850 < IKFAST_SOLUTION_THRESH)
22851 {
22852 j4valid
22853 [iij4]
22854 = false;
22855 _ij4[1]
22856 = iij4;
22857 break;
22858 }
22859 }
22860 j4 = j4array
22861 [ij4];
22862 cj4 = cj4array
22863 [ij4];
22864 sj4 = sj4array
22865 [ij4];
22866 {
22867 IkReal evalcond
22868 [3];
22869 IkReal
22870 x1619
22871 = IKcos(
22872 j4);
22873 IkReal
22874 x1620
22875 = IKsin(
22876 j4);
22877 IkReal
22878 x1621
22879 = (px
22880 * x1620);
22881 IkReal
22882 x1622
22883 = (py
22884 * x1619);
22885 evalcond
22886 [0]
22887 = (((px
22888 * x1619))
22889 + ((py
22890 * x1620)));
22891 evalcond
22892 [1]
22893 = ((1.51009803921569)
22894 + (((-1.0)
22895 * x1622))
22896 + (((-1.0)
22897 * (3.92156862745098)
22898 * pp))
22899 + x1621
22900 + (((1.32323529411765)
22901 * cj9)));
22902 evalcond
22903 [2]
22904 = ((-0.2125)
22905 + (((-1.0)
22906 * (1.1)
22907 * pz))
22908 + (((0.09)
22909 * x1621))
22910 + (((-0.09)
22911 * x1622))
22912 + (((-1.0)
22913 * (1.0)
22914 * pp)));
22915 if (IKabs(
22916 evalcond
22917 [0])
22918 > IKFAST_EVALCOND_THRESH
22919 || IKabs(
22920 evalcond
22921 [1])
22922 > IKFAST_EVALCOND_THRESH
22923 || IKabs(
22924 evalcond
22925 [2])
22926 > IKFAST_EVALCOND_THRESH)
22927 {
22928 continue;
22929 }
22930 }
22931
22932 rotationfunction0(
22933 solutions);
22934 }
22935 }
22936 }
22937 }
22938 }
22939 else
22940 {
22941 {
22942 IkReal j4array
22943 [1],
22944 cj4array
22945 [1],
22946 sj4array
22947 [1];
22948 bool j4valid
22949 [1]
22950 = {false};
22951 _nj4
22952 = 1;
22953 IkReal
22954 x1623
22955 = ((1.32323529411765)
22956 * cj9);
22957 IkReal
22958 x1624
22959 = ((3.92156862745098)
22960 * pp);
22961 CheckValue<IkReal> x1625 = IKatan2WithCheck(
22962 IkReal((
22963 (((-1.0)
22964 * (1.51009803921569)
22965 * px))
22966 + (((-1.0)
22967 * px
22968 * x1623))
22969 + ((px
22970 * x1624)))),
22971 (((py
22972 * x1623))
22973 + (((1.51009803921569)
22974 * py))
22975 + (((-1.0)
22976 * py
22977 * x1624))),
22978 IKFAST_ATAN2_MAGTHRESH);
22979 if (!x1625
22980 .valid)
22981 {
22982 continue;
22983 }
22984 CheckValue<IkReal> x1626 = IKPowWithIntegerCheck(
22985 IKsign((
22986 pp
22987 + (((-1.0)
22988 * (1.0)
22989 * (pz
22990 * pz))))),
22991 -1);
22992 if (!x1626
22993 .valid)
22994 {
22995 continue;
22996 }
22997 j4array
22998 [0]
22999 = ((-1.5707963267949)
23000 + (x1625
23001 .value)
23002 + (((1.5707963267949)
23003 * (x1626
23004 .value))));
23005 sj4array
23006 [0]
23007 = IKsin(
23008 j4array
23009 [0]);
23010 cj4array
23011 [0]
23012 = IKcos(
23013 j4array
23014 [0]);
23015 if (j4array
23016 [0]
23017 > IKPI)
23018 {
23019 j4array
23020 [0]
23021 -= IK2PI;
23022 }
23023 else if (
23024 j4array
23025 [0]
23026 < -IKPI)
23027 {
23028 j4array
23029 [0]
23030 += IK2PI;
23031 }
23032 j4valid
23033 [0]
23034 = true;
23035 for (
23036 int ij4
23037 = 0;
23038 ij4
23039 < 1;
23040 ++ij4)
23041 {
23042 if (!j4valid
23043 [ij4])
23044 {
23045 continue;
23046 }
23047 _ij4[0]
23048 = ij4;
23049 _ij4[1]
23050 = -1;
23051 for (
23052 int iij4
23053 = ij4
23054 + 1;
23055 iij4
23056 < 1;
23057 ++iij4)
23058 {
23059 if (j4valid
23060 [iij4]
23061 && IKabs(
23062 cj4array
23063 [ij4]
23064 - cj4array
23065 [iij4])
23066 < IKFAST_SOLUTION_THRESH
23067 && IKabs(
23068 sj4array
23069 [ij4]
23070 - sj4array
23071 [iij4])
23072 < IKFAST_SOLUTION_THRESH)
23073 {
23074 j4valid
23075 [iij4]
23076 = false;
23077 _ij4[1]
23078 = iij4;
23079 break;
23080 }
23081 }
23082 j4 = j4array
23083 [ij4];
23084 cj4 = cj4array
23085 [ij4];
23086 sj4 = sj4array
23087 [ij4];
23088 {
23089 IkReal evalcond
23090 [3];
23091 IkReal
23092 x1627
23093 = IKcos(
23094 j4);
23095 IkReal
23096 x1628
23097 = IKsin(
23098 j4);
23099 IkReal
23100 x1629
23101 = (px
23102 * x1628);
23103 IkReal
23104 x1630
23105 = (py
23106 * x1627);
23107 evalcond
23108 [0]
23109 = (((py
23110 * x1628))
23111 + ((px
23112 * x1627)));
23113 evalcond
23114 [1]
23115 = ((1.51009803921569)
23116 + (((-1.0)
23117 * (3.92156862745098)
23118 * pp))
23119 + (((-1.0)
23120 * x1630))
23121 + x1629
23122 + (((1.32323529411765)
23123 * cj9)));
23124 evalcond
23125 [2]
23126 = ((-0.2125)
23127 + (((-1.0)
23128 * (1.1)
23129 * pz))
23130 + (((0.09)
23131 * x1629))
23132 + (((-0.09)
23133 * x1630))
23134 + (((-1.0)
23135 * (1.0)
23136 * pp)));
23137 if (IKabs(
23138 evalcond
23139 [0])
23140 > IKFAST_EVALCOND_THRESH
23141 || IKabs(
23142 evalcond
23143 [1])
23144 > IKFAST_EVALCOND_THRESH
23145 || IKabs(
23146 evalcond
23147 [2])
23148 > IKFAST_EVALCOND_THRESH)
23149 {
23150 continue;
23151 }
23152 }
23153
23154 rotationfunction0(
23155 solutions);
23156 }
23157 }
23158 }
23159 }
23160 }
23161 } while (0);
23162 if (bgotonextstatement)
23163 {
23164 bool
23165 bgotonextstatement
23166 = true;
23167 do
23168 {
23169 IkReal x1631
23170 = ((1.32323529411765)
23171 * cj9);
23172 IkReal x1632
23173 = ((3.92156862745098)
23174 * pp);
23175 evalcond[0]
23176 = ((IKabs(
23177 py))
23178 + (IKabs(
23179 px)));
23180 evalcond[1]
23181 = ((((-1.0)
23182 * sj8
23183 * x1631))
23184 + ((sj8
23185 * x1632))
23186 + (((-1.0)
23187 * (1.51009803921569)
23188 * sj8)));
23189 evalcond[2]
23190 = 0;
23191 evalcond[3]
23192 = ((-1.51009803921569)
23193 + (((-1.0)
23194 * x1631))
23195 + x1632);
23196 evalcond[4]
23197 = ((((1.51009803921569)
23198 * cj8))
23199 + (((-1.0)
23200 * cj8
23201 * x1632))
23202 + ((cj8
23203 * x1631)));
23204 evalcond[5]
23205 = ((-0.2125)
23206 + (((-1.0)
23207 * (1.1)
23208 * pz))
23209 + (((-1.0)
23210 * (1.0)
23211 * pp)));
23212 if (IKabs(
23213 evalcond
23214 [0])
23215 < 0.0000010000000000
23216 && IKabs(
23217 evalcond
23218 [1])
23219 < 0.0000010000000000
23220 && IKabs(
23221 evalcond
23222 [2])
23223 < 0.0000010000000000
23224 && IKabs(
23225 evalcond
23226 [3])
23227 < 0.0000010000000000
23228 && IKabs(
23229 evalcond
23230 [4])
23231 < 0.0000010000000000
23232 && IKabs(
23233 evalcond
23234 [5])
23235 < 0.0000010000000000)
23236 {
23237 bgotonextstatement
23238 = false;
23239 {
23240 IkReal j4array
23241 [4],
23242 cj4array
23243 [4],
23244 sj4array
23245 [4];
23246 bool j4valid
23247 [4]
23248 = {false};
23249 _nj4 = 4;
23250 j4array[0]
23251 = 0;
23252 sj4array[0] = IKsin(
23253 j4array
23254 [0]);
23255 cj4array[0] = IKcos(
23256 j4array
23257 [0]);
23258 j4array[1]
23259 = 1.5707963267949;
23260 sj4array[1] = IKsin(
23261 j4array
23262 [1]);
23263 cj4array[1] = IKcos(
23264 j4array
23265 [1]);
23266 j4array[2]
23267 = 3.14159265358979;
23268 sj4array[2] = IKsin(
23269 j4array
23270 [2]);
23271 cj4array[2] = IKcos(
23272 j4array
23273 [2]);
23274 j4array[3]
23275 = -1.5707963267949;
23276 sj4array[3] = IKsin(
23277 j4array
23278 [3]);
23279 cj4array[3] = IKcos(
23280 j4array
23281 [3]);
23282 if (j4array
23283 [0]
23284 > IKPI)
23285 {
23286 j4array
23287 [0]
23288 -= IK2PI;
23289 }
23290 else if (
23291 j4array
23292 [0]
23293 < -IKPI)
23294 {
23295 j4array
23296 [0]
23297 += IK2PI;
23298 }
23299 j4valid[0]
23300 = true;
23301 if (j4array
23302 [1]
23303 > IKPI)
23304 {
23305 j4array
23306 [1]
23307 -= IK2PI;
23308 }
23309 else if (
23310 j4array
23311 [1]
23312 < -IKPI)
23313 {
23314 j4array
23315 [1]
23316 += IK2PI;
23317 }
23318 j4valid[1]
23319 = true;
23320 if (j4array
23321 [2]
23322 > IKPI)
23323 {
23324 j4array
23325 [2]
23326 -= IK2PI;
23327 }
23328 else if (
23329 j4array
23330 [2]
23331 < -IKPI)
23332 {
23333 j4array
23334 [2]
23335 += IK2PI;
23336 }
23337 j4valid[2]
23338 = true;
23339 if (j4array
23340 [3]
23341 > IKPI)
23342 {
23343 j4array
23344 [3]
23345 -= IK2PI;
23346 }
23347 else if (
23348 j4array
23349 [3]
23350 < -IKPI)
23351 {
23352 j4array
23353 [3]
23354 += IK2PI;
23355 }
23356 j4valid[3]
23357 = true;
23358 for (
23359 int ij4
23360 = 0;
23361 ij4
23362 < 4;
23363 ++ij4)
23364 {
23365 if (!j4valid
23366 [ij4])
23367 {
23368 continue;
23369 }
23370 _ij4[0]
23371 = ij4;
23372 _ij4[1]
23373 = -1;
23374 for (
23375 int iij4
23376 = ij4
23377 + 1;
23378 iij4
23379 < 4;
23380 ++iij4)
23381 {
23382 if (j4valid
23383 [iij4]
23384 && IKabs(
23385 cj4array
23386 [ij4]
23387 - cj4array
23388 [iij4])
23389 < IKFAST_SOLUTION_THRESH
23390 && IKabs(
23391 sj4array
23392 [ij4]
23393 - sj4array
23394 [iij4])
23395 < IKFAST_SOLUTION_THRESH)
23396 {
23397 j4valid
23398 [iij4]
23399 = false;
23400 _ij4[1]
23401 = iij4;
23402 break;
23403 }
23404 }
23405 j4 = j4array
23406 [ij4];
23407 cj4 = cj4array
23408 [ij4];
23409 sj4 = sj4array
23410 [ij4];
23411
23412 rotationfunction0(
23413 solutions);
23414 }
23415 }
23416 }
23417 } while (0);
23418 if (bgotonextstatement)
23419 {
23420 bool
23421 bgotonextstatement
23422 = true;
23423 do
23424 {
23425 if (1)
23426 {
23427 bgotonextstatement
23428 = false;
23429 continue; // branch miss [j4]
23430 }
23431 } while (0);
23432 if (bgotonextstatement)
23433 {
23434 }
23435 }
23436 }
23437 }
23438 }
23439 }
23440 else
23441 {
23442 {
23443 IkReal j4array[1],
23444 cj4array[1],
23445 sj4array[1];
23446 bool j4valid[1]
23447 = {false};
23448 _nj4 = 1;
23449 IkReal x1633
23450 = ((1.51009803921569)
23451 * cj8 * sj8);
23452 IkReal x1634
23453 = cj8 * cj8;
23454 IkReal x1635
23455 = ((1.51009803921569)
23456 * x1634);
23457 IkReal x1636
23458 = ((1.32323529411765)
23459 * cj8 * cj9
23460 * sj8);
23461 IkReal x1637
23462 = ((3.92156862745098)
23463 * cj8 * pp
23464 * sj8);
23465 IkReal x1638
23466 = ((1.32323529411765)
23467 * cj9
23468 * x1634);
23469 IkReal x1639
23470 = ((3.92156862745098)
23471 * pp
23472 * x1634);
23473 CheckValue<IkReal> x1640 = IKatan2WithCheck(
23474 IkReal((
23475 (((-1.0)
23476 * px
23477 * x1637))
23478 + ((py
23479 * x1635))
23480 + ((py
23481 * x1638))
23482 + ((px
23483 * x1633))
23484 + ((px
23485 * x1636))
23486 + (((-1.0)
23487 * py
23488 * x1639)))),
23489 (((px * x1638))
23490 + ((py
23491 * x1637))
23492 + (((-1.0) * px
23493 * x1639))
23494 + (((-1.0) * py
23495 * x1633))
23496 + ((px
23497 * x1635))
23498 + (((-1.0) * py
23499 * x1636))),
23500 IKFAST_ATAN2_MAGTHRESH);
23501 if (!x1640.valid)
23502 {
23503 continue;
23504 }
23505 CheckValue<IkReal>
23506 x1641
23507 = IKPowWithIntegerCheck(
23508 IKsign((
23509 (((-1.0)
23510 * (1.0)
23511 * cj8
23512 * (pz
23513 * pz)))
23514 + ((cj8
23515 * pp)))),
23516 -1);
23517 if (!x1641.valid)
23518 {
23519 continue;
23520 }
23521 j4array[0]
23522 = ((-1.5707963267949)
23523 + (x1640
23524 .value)
23525 + (((1.5707963267949)
23526 * (x1641
23527 .value))));
23528 sj4array[0] = IKsin(
23529 j4array[0]);
23530 cj4array[0] = IKcos(
23531 j4array[0]);
23532 if (j4array[0]
23533 > IKPI)
23534 {
23535 j4array[0]
23536 -= IK2PI;
23537 }
23538 else if (
23539 j4array[0]
23540 < -IKPI)
23541 {
23542 j4array[0]
23543 += IK2PI;
23544 }
23545 j4valid[0] = true;
23546 for (int ij4 = 0;
23547 ij4 < 1;
23548 ++ij4)
23549 {
23550 if (!j4valid[ij4])
23551 {
23552 continue;
23553 }
23554 _ij4[0] = ij4;
23555 _ij4[1] = -1;
23556 for (int iij4
23557 = ij4 + 1;
23558 iij4 < 1;
23559 ++iij4)
23560 {
23561 if (j4valid
23562 [iij4]
23563 && IKabs(
23564 cj4array
23565 [ij4]
23566 - cj4array
23567 [iij4])
23568 < IKFAST_SOLUTION_THRESH
23569 && IKabs(
23570 sj4array
23571 [ij4]
23572 - sj4array
23573 [iij4])
23574 < IKFAST_SOLUTION_THRESH)
23575 {
23576 j4valid[iij4]
23577 = false;
23578 _ij4[1]
23579 = iij4;
23580 break;
23581 }
23582 }
23583 j4 = j4array[ij4];
23584 cj4 = cj4array
23585 [ij4];
23586 sj4 = sj4array
23587 [ij4];
23588 {
23589 IkReal
23590 evalcond[5];
23591 IkReal x1642
23592 = ((1.32323529411765)
23593 * cj9);
23594 IkReal x1643
23595 = ((3.92156862745098)
23596 * pp);
23597 IkReal x1644
23598 = IKsin(j4);
23599 IkReal x1645
23600 = (px
23601 * x1644);
23602 IkReal x1646
23603 = IKcos(j4);
23604 IkReal x1647
23605 = ((1.0)
23606 * x1646);
23607 IkReal x1648
23608 = (py
23609 * x1647);
23610 IkReal x1649
23611 = (px
23612 * x1647);
23613 IkReal x1650
23614 = (py
23615 * x1644);
23616 IkReal x1651
23617 = ((1.0)
23618 * x1650);
23619 IkReal x1652
23620 = (cj8 * px
23621 * x1646);
23622 IkReal x1653
23623 = (cj8
23624 * x1650);
23625 IkReal x1654
23626 = (sj8
23627 * x1645);
23628 evalcond[0]
23629 = (((sj8
23630 * x1643))
23631 + (((-1.0)
23632 * sj8
23633 * x1642))
23634 + (((-1.0)
23635 * x1648))
23636 + (((-1.0)
23637 * (1.51009803921569)
23638 * sj8))
23639 + x1645);
23640 evalcond[1]
23641 = (((cj8
23642 * x1642))
23643 + (((1.51009803921569)
23644 * cj8))
23645 + (((-1.0)
23646 * cj8
23647 * x1643))
23648 + (((-1.0)
23649 * x1651))
23650 + (((-1.0)
23651 * x1649)));
23652 evalcond[2]
23653 = ((((-1.0)
23654 * sj8
23655 * x1649))
23656 + (((-1.0)
23657 * sj8
23658 * x1651))
23659 + ((cj8
23660 * x1645))
23661 + (((-1.0)
23662 * cj8
23663 * x1648)));
23664 evalcond[3]
23665 = ((-1.51009803921569)
23666 + (((-1.0)
23667 * sj8
23668 * x1648))
23669 + x1654
23670 + x1652
23671 + x1653
23672 + (((-1.0)
23673 * x1642))
23674 + x1643);
23675 evalcond[4]
23676 = ((-0.2125)
23677 + (((-1.0)
23678 * (1.1)
23679 * pz))
23680 + (((-0.09)
23681 * x1654))
23682 + (((0.09)
23683 * py
23684 * sj8
23685 * x1646))
23686 + (((-1.0)
23687 * (1.0)
23688 * pp))
23689 + (((-0.09)
23690 * x1652))
23691 + (((-0.09)
23692 * x1653)));
23693 if (IKabs(
23694 evalcond
23695 [0])
23696 > IKFAST_EVALCOND_THRESH
23697 || IKabs(
23698 evalcond
23699 [1])
23700 > IKFAST_EVALCOND_THRESH
23701 || IKabs(
23702 evalcond
23703 [2])
23704 > IKFAST_EVALCOND_THRESH
23705 || IKabs(
23706 evalcond
23707 [3])
23708 > IKFAST_EVALCOND_THRESH
23709 || IKabs(
23710 evalcond
23711 [4])
23712 > IKFAST_EVALCOND_THRESH)
23713 {
23714 continue;
23715 }
23716 }
23717
23718 rotationfunction0(
23719 solutions);
23720 }
23721 }
23722 }
23723 }
23724 }
23725 else
23726 {
23727 {
23728 IkReal j4array[1],
23729 cj4array[1],
23730 sj4array[1];
23731 bool j4valid[1]
23732 = {false};
23733 _nj4 = 1;
23734 IkReal x1655
23735 = ((1.51009803921569)
23736 * cj8 * sj8);
23737 IkReal x1656
23738 = cj8 * cj8;
23739 IkReal x1657
23740 = ((1.51009803921569)
23741 * x1656);
23742 IkReal x1658
23743 = ((1.32323529411765)
23744 * cj8 * cj9
23745 * sj8);
23746 IkReal x1659
23747 = ((3.92156862745098)
23748 * cj8 * pp
23749 * sj8);
23750 IkReal x1660
23751 = ((1.32323529411765)
23752 * cj9 * x1656);
23753 IkReal x1661
23754 = ((3.92156862745098)
23755 * pp * x1656);
23756 CheckValue<IkReal> x1662
23757 = IKPowWithIntegerCheck(
23758 IKsign((
23759 ((cj8
23760 * (pz
23761 * pz)))
23762 + (((-1.0)
23763 * cj8
23764 * pp)))),
23765 -1);
23766 if (!x1662.valid)
23767 {
23768 continue;
23769 }
23770 CheckValue<IkReal> x1663
23771 = IKatan2WithCheck(
23772 IkReal((
23773 (((-1.0)
23774 * px
23775 * x1658))
23776 + ((py
23777 * x1661))
23778 + ((px
23779 * x1659))
23780 + (((-1.0)
23781 * py
23782 * x1660))
23783 + (((-1.0)
23784 * px
23785 * x1655))
23786 + (((-1.0)
23787 * py
23788 * x1657)))),
23789 ((((-1.0) * px
23790 * x1660))
23791 + ((py
23792 * x1658))
23793 + (((-1.0) * py
23794 * x1659))
23795 + ((px
23796 * x1661))
23797 + (((-1.0) * px
23798 * x1657))
23799 + ((py
23800 * x1655))),
23801 IKFAST_ATAN2_MAGTHRESH);
23802 if (!x1663.valid)
23803 {
23804 continue;
23805 }
23806 j4array[0]
23807 = ((-1.5707963267949)
23808 + (((1.5707963267949)
23809 * (x1662
23810 .value)))
23811 + (x1663.value));
23812 sj4array[0]
23813 = IKsin(j4array[0]);
23814 cj4array[0]
23815 = IKcos(j4array[0]);
23816 if (j4array[0] > IKPI)
23817 {
23818 j4array[0] -= IK2PI;
23819 }
23820 else if (
23821 j4array[0] < -IKPI)
23822 {
23823 j4array[0] += IK2PI;
23824 }
23825 j4valid[0] = true;
23826 for (int ij4 = 0;
23827 ij4 < 1;
23828 ++ij4)
23829 {
23830 if (!j4valid[ij4])
23831 {
23832 continue;
23833 }
23834 _ij4[0] = ij4;
23835 _ij4[1] = -1;
23836 for (int iij4
23837 = ij4 + 1;
23838 iij4 < 1;
23839 ++iij4)
23840 {
23841 if (j4valid[iij4]
23842 && IKabs(
23843 cj4array
23844 [ij4]
23845 - cj4array
23846 [iij4])
23847 < IKFAST_SOLUTION_THRESH
23848 && IKabs(
23849 sj4array
23850 [ij4]
23851 - sj4array
23852 [iij4])
23853 < IKFAST_SOLUTION_THRESH)
23854 {
23855 j4valid[iij4]
23856 = false;
23857 _ij4[1] = iij4;
23858 break;
23859 }
23860 }
23861 j4 = j4array[ij4];
23862 cj4 = cj4array[ij4];
23863 sj4 = sj4array[ij4];
23864 {
23865 IkReal evalcond[5];
23866 IkReal x1664
23867 = ((1.32323529411765)
23868 * cj9);
23869 IkReal x1665
23870 = ((3.92156862745098)
23871 * pp);
23872 IkReal x1666
23873 = IKsin(j4);
23874 IkReal x1667
23875 = (px * x1666);
23876 IkReal x1668
23877 = IKcos(j4);
23878 IkReal x1669
23879 = ((1.0)
23880 * x1668);
23881 IkReal x1670
23882 = (py * x1669);
23883 IkReal x1671
23884 = (px * x1669);
23885 IkReal x1672
23886 = (py * x1666);
23887 IkReal x1673
23888 = ((1.0)
23889 * x1672);
23890 IkReal x1674
23891 = (cj8 * px
23892 * x1668);
23893 IkReal x1675
23894 = (cj8 * x1672);
23895 IkReal x1676
23896 = (sj8 * x1667);
23897 evalcond[0]
23898 = (((sj8
23899 * x1665))
23900 + (((-1.0)
23901 * sj8
23902 * x1664))
23903 + (((-1.0)
23904 * x1670))
23905 + (((-1.0)
23906 * (1.51009803921569)
23907 * sj8))
23908 + x1667);
23909 evalcond[1]
23910 = ((((-1.0)
23911 * cj8
23912 * x1665))
23913 + (((1.51009803921569)
23914 * cj8))
23915 + (((-1.0)
23916 * x1673))
23917 + ((cj8
23918 * x1664))
23919 + (((-1.0)
23920 * x1671)));
23921 evalcond[2]
23922 = (((cj8
23923 * x1667))
23924 + (((-1.0)
23925 * sj8
23926 * x1673))
23927 + (((-1.0)
23928 * cj8
23929 * x1670))
23930 + (((-1.0)
23931 * sj8
23932 * x1671)));
23933 evalcond[3]
23934 = ((-1.51009803921569)
23935 + x1676
23936 + x1674
23937 + x1675
23938 + (((-1.0)
23939 * x1664))
23940 + (((-1.0)
23941 * sj8
23942 * x1670))
23943 + x1665);
23944 evalcond[4]
23945 = ((-0.2125)
23946 + (((-1.0)
23947 * (1.1)
23948 * pz))
23949 + (((0.09)
23950 * py
23951 * sj8
23952 * x1668))
23953 + (((-0.09)
23954 * x1674))
23955 + (((-0.09)
23956 * x1676))
23957 + (((-1.0)
23958 * (1.0)
23959 * pp))
23960 + (((-0.09)
23961 * x1675)));
23962 if (IKabs(
23963 evalcond[0])
23964 > IKFAST_EVALCOND_THRESH
23965 || IKabs(
23966 evalcond
23967 [1])
23968 > IKFAST_EVALCOND_THRESH
23969 || IKabs(
23970 evalcond
23971 [2])
23972 > IKFAST_EVALCOND_THRESH
23973 || IKabs(
23974 evalcond
23975 [3])
23976 > IKFAST_EVALCOND_THRESH
23977 || IKabs(
23978 evalcond
23979 [4])
23980 > IKFAST_EVALCOND_THRESH)
23981 {
23982 continue;
23983 }
23984 }
23985
23986 rotationfunction0(
23987 solutions);
23988 }
23989 }
23990 }
23991 }
23992 }
23993 else
23994 {
23995 {
23996 IkReal j4array[1],
23997 cj4array[1],
23998 sj4array[1];
23999 bool j4valid[1] = {false};
24000 _nj4 = 1;
24001 IkReal x1677
24002 = ((1.51009803921569)
24003 * cj8);
24004 IkReal x1678
24005 = ((1.51009803921569)
24006 * sj8);
24007 IkReal x1679
24008 = ((1.32323529411765)
24009 * cj8 * cj9);
24010 IkReal x1680
24011 = ((3.92156862745098)
24012 * cj8 * pp);
24013 IkReal x1681
24014 = ((1.32323529411765)
24015 * cj9 * sj8);
24016 IkReal x1682
24017 = ((3.92156862745098)
24018 * pp * sj8);
24019 CheckValue<IkReal> x1683
24020 = IKPowWithIntegerCheck(
24021 IKsign((
24022 pp
24023 + (((-1.0)
24024 * (1.0)
24025 * (pz
24026 * pz))))),
24027 -1);
24028 if (!x1683.valid)
24029 {
24030 continue;
24031 }
24032 CheckValue<IkReal> x1684
24033 = IKatan2WithCheck(
24034 IkReal((
24035 ((py * x1677))
24036 + (((-1.0) * px
24037 * x1682))
24038 + ((px * x1678))
24039 + ((py * x1679))
24040 + ((px * x1681))
24041 + (((-1.0) * py
24042 * x1680)))),
24043 ((((-1.0) * py
24044 * x1678))
24045 + ((py * x1682))
24046 + (((-1.0) * px
24047 * x1680))
24048 + ((px * x1679))
24049 + ((px * x1677))
24050 + (((-1.0) * py
24051 * x1681))),
24052 IKFAST_ATAN2_MAGTHRESH);
24053 if (!x1684.valid)
24054 {
24055 continue;
24056 }
24057 j4array[0]
24058 = ((-1.5707963267949)
24059 + (((1.5707963267949)
24060 * (x1683.value)))
24061 + (x1684.value));
24062 sj4array[0]
24063 = IKsin(j4array[0]);
24064 cj4array[0]
24065 = IKcos(j4array[0]);
24066 if (j4array[0] > IKPI)
24067 {
24068 j4array[0] -= IK2PI;
24069 }
24070 else if (j4array[0] < -IKPI)
24071 {
24072 j4array[0] += IK2PI;
24073 }
24074 j4valid[0] = true;
24075 for (int ij4 = 0; ij4 < 1;
24076 ++ij4)
24077 {
24078 if (!j4valid[ij4])
24079 {
24080 continue;
24081 }
24082 _ij4[0] = ij4;
24083 _ij4[1] = -1;
24084 for (int iij4 = ij4 + 1;
24085 iij4 < 1;
24086 ++iij4)
24087 {
24088 if (j4valid[iij4]
24089 && IKabs(
24090 cj4array[ij4]
24091 - cj4array
24092 [iij4])
24093 < IKFAST_SOLUTION_THRESH
24094 && IKabs(
24095 sj4array[ij4]
24096 - sj4array
24097 [iij4])
24098 < IKFAST_SOLUTION_THRESH)
24099 {
24100 j4valid[iij4] = false;
24101 _ij4[1] = iij4;
24102 break;
24103 }
24104 }
24105 j4 = j4array[ij4];
24106 cj4 = cj4array[ij4];
24107 sj4 = sj4array[ij4];
24108 {
24109 IkReal evalcond[5];
24110 IkReal x1685
24111 = ((1.32323529411765)
24112 * cj9);
24113 IkReal x1686
24114 = ((3.92156862745098)
24115 * pp);
24116 IkReal x1687
24117 = IKsin(j4);
24118 IkReal x1688
24119 = (px * x1687);
24120 IkReal x1689
24121 = IKcos(j4);
24122 IkReal x1690
24123 = ((1.0) * x1689);
24124 IkReal x1691
24125 = (py * x1690);
24126 IkReal x1692
24127 = (px * x1690);
24128 IkReal x1693
24129 = (py * x1687);
24130 IkReal x1694
24131 = ((1.0) * x1693);
24132 IkReal x1695
24133 = (cj8 * px
24134 * x1689);
24135 IkReal x1696
24136 = (cj8 * x1693);
24137 IkReal x1697
24138 = (sj8 * x1688);
24139 evalcond[0]
24140 = (x1688
24141 + (((-1.0)
24142 * x1691))
24143 + (((-1.0) * sj8
24144 * x1685))
24145 + ((sj8 * x1686))
24146 + (((-1.0)
24147 * (1.51009803921569)
24148 * sj8)));
24149 evalcond[1]
24150 = (((cj8 * x1685))
24151 + (((-1.0) * cj8
24152 * x1686))
24153 + (((-1.0)
24154 * x1692))
24155 + (((1.51009803921569)
24156 * cj8))
24157 + (((-1.0)
24158 * x1694)));
24159 evalcond[2]
24160 = ((((-1.0) * cj8
24161 * x1691))
24162 + (((-1.0) * sj8
24163 * x1692))
24164 + ((cj8 * x1688))
24165 + (((-1.0) * sj8
24166 * x1694)));
24167 evalcond[3]
24168 = ((-1.51009803921569)
24169 + x1695 + x1696
24170 + x1697 + x1686
24171 + (((-1.0) * sj8
24172 * x1691))
24173 + (((-1.0)
24174 * x1685)));
24175 evalcond[4]
24176 = ((-0.2125)
24177 + (((-1.0)
24178 * (1.1)
24179 * pz))
24180 + (((-0.09)
24181 * x1697))
24182 + (((-0.09)
24183 * x1696))
24184 + (((-0.09)
24185 * x1695))
24186 + (((0.09) * py
24187 * sj8
24188 * x1689))
24189 + (((-1.0)
24190 * (1.0)
24191 * pp)));
24192 if (IKabs(evalcond[0])
24193 > IKFAST_EVALCOND_THRESH
24194 || IKabs(
24195 evalcond[1])
24196 > IKFAST_EVALCOND_THRESH
24197 || IKabs(
24198 evalcond[2])
24199 > IKFAST_EVALCOND_THRESH
24200 || IKabs(
24201 evalcond[3])
24202 > IKFAST_EVALCOND_THRESH
24203 || IKabs(
24204 evalcond[4])
24205 > IKFAST_EVALCOND_THRESH)
24206 {
24207 continue;
24208 }
24209 }
24210
24211 rotationfunction0(
24212 solutions);
24213 }
24214 }
24215 }
24216 }
24217 }
24218 } while (0);
24219 if (bgotonextstatement)
24220 {
24221 bool bgotonextstatement = true;
24222 do
24223 {
24224 if (1)
24225 {
24226 bgotonextstatement = false;
24227 continue; // branch miss [j4]
24228 }
24229 } while (0);
24230 if (bgotonextstatement)
24231 {
24232 }
24233 }
24234 }
24235 }
24236 }
24237 }
24238 }
24239 else
24240 {
24241 {
24242 IkReal j4array[1], cj4array[1], sj4array[1];
24243 bool j4valid[1] = {false};
24244 _nj4 = 1;
24245 IkReal x1698 = ((0.045) * sj8);
24246 IkReal x1699 = (px * x1698);
24247 IkReal x1700 = ((0.55) * sj6);
24248 IkReal x1701 = ((0.045) * cj6 * cj8);
24249 IkReal x1702 = (py * x1701);
24250 IkReal x1703 = ((0.3) * cj9 * sj6);
24251 IkReal x1704 = ((0.3) * sj8);
24252 IkReal x1705 = (px * sj9);
24253 IkReal x1706 = ((0.045) * sj6);
24254 IkReal x1707 = (py * sj9);
24255 IkReal x1708 = ((0.3) * cj6 * cj8);
24256 IkReal x1709 = (py * x1698);
24257 IkReal x1710 = (px * x1701);
24258 CheckValue<IkReal> x1711 = IKatan2WithCheck(
24259 IkReal(
24260 ((((-1.0) * x1699))
24261 + (((-1.0) * x1704 * x1705))
24262 + (((-1.0) * cj9 * x1702))
24263 + ((py * x1700)) + x1702
24264 + ((x1706 * x1707))
24265 + ((py * x1703)) + ((cj9 * x1699))
24266 + ((x1707 * x1708)))),
24267 (x1709 + (((-1.0) * cj9 * x1709))
24268 + x1710 + ((px * x1700))
24269 + ((px * x1703)) + ((x1705 * x1708))
24270 + ((x1704 * x1707))
24271 + (((-1.0) * cj9 * x1710))
24272 + ((x1705 * x1706))),
24273 IKFAST_ATAN2_MAGTHRESH);
24274 if (!x1711.valid)
24275 {
24276 continue;
24277 }
24278 CheckValue<IkReal> x1712
24279 = IKPowWithIntegerCheck(
24280 IKsign(
24281 (pp
24282 + (((-1.0) * (1.0)
24283 * (pz * pz))))),
24284 -1);
24285 if (!x1712.valid)
24286 {
24287 continue;
24288 }
24289 j4array[0]
24290 = ((-1.5707963267949) + (x1711.value)
24291 + (((1.5707963267949)
24292 * (x1712.value))));
24293 sj4array[0] = IKsin(j4array[0]);
24294 cj4array[0] = IKcos(j4array[0]);
24295 if (j4array[0] > IKPI)
24296 {
24297 j4array[0] -= IK2PI;
24298 }
24299 else if (j4array[0] < -IKPI)
24300 {
24301 j4array[0] += IK2PI;
24302 }
24303 j4valid[0] = true;
24304 for (int ij4 = 0; ij4 < 1; ++ij4)
24305 {
24306 if (!j4valid[ij4])
24307 {
24308 continue;
24309 }
24310 _ij4[0] = ij4;
24311 _ij4[1] = -1;
24312 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24313 {
24314 if (j4valid[iij4]
24315 && IKabs(
24316 cj4array[ij4]
24317 - cj4array[iij4])
24318 < IKFAST_SOLUTION_THRESH
24319 && IKabs(
24320 sj4array[ij4]
24321 - sj4array[iij4])
24322 < IKFAST_SOLUTION_THRESH)
24323 {
24324 j4valid[iij4] = false;
24325 _ij4[1] = iij4;
24326 break;
24327 }
24328 }
24329 j4 = j4array[ij4];
24330 cj4 = cj4array[ij4];
24331 sj4 = sj4array[ij4];
24332 {
24333 IkReal evalcond[6];
24334 IkReal x1713 = ((0.3) * cj9);
24335 IkReal x1714 = ((0.045) * sj9);
24336 IkReal x1715 = (cj6 * pz);
24337 IkReal x1716 = IKcos(j4);
24338 IkReal x1717 = (px * sj6 * x1716);
24339 IkReal x1718 = IKsin(j4);
24340 IkReal x1719 = (py * x1718);
24341 IkReal x1720 = (sj6 * x1719);
24342 IkReal x1721 = ((0.045) * cj9);
24343 IkReal x1722 = (px * x1718);
24344 IkReal x1723 = ((0.3) * sj9);
24345 IkReal x1724 = ((1.0) * x1716);
24346 IkReal x1725 = (py * x1724);
24347 IkReal x1726 = (sj8 * x1716);
24348 IkReal x1727 = (cj6 * cj8);
24349 IkReal x1728 = (px * x1724);
24350 IkReal x1729 = ((1.0) * x1719);
24351 IkReal x1730 = (cj8 * pz * sj6);
24352 IkReal x1731 = (sj8 * x1722);
24353 IkReal x1732 = ((0.09) * cj6 * cj8);
24354 evalcond[0]
24355 = ((-0.55) + (((-1.0) * x1714))
24356 + x1715 + x1717
24357 + (((-1.0) * x1713)) + x1720);
24358 evalcond[1]
24359 = ((((-1.0) * sj8 * x1721))
24360 + (((-1.0) * x1725))
24361 + (((0.045) * sj8)) + x1722
24362 + ((sj8 * x1723)));
24363 evalcond[2]
24364 = (((cj6 * sj8 * x1719))
24365 + ((cj6 * px * x1726))
24366 + (((-1.0) * (1.0) * pz * sj6
24367 * sj8))
24368 + (((-1.0) * cj8 * x1725))
24369 + ((cj8 * x1722)));
24370 evalcond[3]
24371 = ((((0.045) * x1727))
24372 + ((x1723 * x1727))
24373 + ((sj6 * x1714))
24374 + (((0.55) * sj6))
24375 + (((-1.0) * x1721 * x1727))
24376 + (((-1.0) * x1729))
24377 + (((-1.0) * x1728))
24378 + ((sj6 * x1713)));
24379 evalcond[4]
24380 = ((0.045) + (((-1.0) * x1721))
24381 + (((-1.0) * x1727 * x1729))
24382 + (((-1.0) * x1727 * x1728))
24383 + (((-1.0) * sj8 * x1725))
24384 + x1723 + x1731 + x1730);
24385 evalcond[5]
24386 = ((-0.2125)
24387 + (((0.09) * py * x1726))
24388 + (((-0.09) * x1731))
24389 + ((x1719 * x1732))
24390 + (((1.1) * x1717))
24391 + ((px * x1716 * x1732))
24392 + (((1.1) * x1720))
24393 + (((-1.0) * (1.0) * pp))
24394 + (((1.1) * x1715))
24395 + (((-0.09) * x1730)));
24396 if (IKabs(evalcond[0])
24397 > IKFAST_EVALCOND_THRESH
24398 || IKabs(evalcond[1])
24399 > IKFAST_EVALCOND_THRESH
24400 || IKabs(evalcond[2])
24401 > IKFAST_EVALCOND_THRESH
24402 || IKabs(evalcond[3])
24403 > IKFAST_EVALCOND_THRESH
24404 || IKabs(evalcond[4])
24405 > IKFAST_EVALCOND_THRESH
24406 || IKabs(evalcond[5])
24407 > IKFAST_EVALCOND_THRESH)
24408 {
24409 continue;
24410 }
24411 }
24412
24413 rotationfunction0(solutions);
24414 }
24415 }
24416 }
24417 }
24418 }
24419 else
24420 {
24421 {
24422 IkReal j4array[1], cj4array[1], sj4array[1];
24423 bool j4valid[1] = {false};
24424 _nj4 = 1;
24425 IkReal x1733 = (cj8 * sj6);
24426 IkReal x1734 = ((0.55) * cj8);
24427 IkReal x1735 = ((0.55) * cj6);
24428 IkReal x1736 = (px * sj8);
24429 IkReal x1737 = ((0.3) * cj8 * cj9);
24430 IkReal x1738 = ((0.045) * cj8 * sj9);
24431 IkReal x1739 = (cj6 * cj8 * pz);
24432 IkReal x1740 = ((0.3) * cj6 * cj9);
24433 IkReal x1741 = ((0.045) * cj6 * sj9);
24434 IkReal x1742 = (py * sj8);
24435 CheckValue<IkReal> x1743 = IKatan2WithCheck(
24436 IkReal(
24437 ((((-1.0) * pz * x1736))
24438 + ((x1735 * x1736)) + ((py * x1739))
24439 + (((-1.0) * py * x1738))
24440 + ((x1736 * x1740))
24441 + (((-1.0) * py * x1737))
24442 + (((-1.0) * py * x1734))
24443 + ((x1736 * x1741)))),
24444 ((((-1.0) * px * x1738)) + ((px * x1739))
24445 + (((-1.0) * px * x1737))
24446 + (((-1.0) * px * x1734))
24447 + (((-1.0) * x1740 * x1742))
24448 + (((-1.0) * x1735 * x1742))
24449 + (((-1.0) * x1741 * x1742))
24450 + ((pz * x1742))),
24451 IKFAST_ATAN2_MAGTHRESH);
24452 if (!x1743.valid)
24453 {
24454 continue;
24455 }
24456 CheckValue<IkReal> x1744
24457 = IKPowWithIntegerCheck(
24458 IKsign(
24459 (((x1733 * (pz * pz)))
24460 + (((-1.0) * pp * x1733)))),
24461 -1);
24462 if (!x1744.valid)
24463 {
24464 continue;
24465 }
24466 j4array[0]
24467 = ((-1.5707963267949) + (x1743.value)
24468 + (((1.5707963267949) * (x1744.value))));
24469 sj4array[0] = IKsin(j4array[0]);
24470 cj4array[0] = IKcos(j4array[0]);
24471 if (j4array[0] > IKPI)
24472 {
24473 j4array[0] -= IK2PI;
24474 }
24475 else if (j4array[0] < -IKPI)
24476 {
24477 j4array[0] += IK2PI;
24478 }
24479 j4valid[0] = true;
24480 for (int ij4 = 0; ij4 < 1; ++ij4)
24481 {
24482 if (!j4valid[ij4])
24483 {
24484 continue;
24485 }
24486 _ij4[0] = ij4;
24487 _ij4[1] = -1;
24488 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24489 {
24490 if (j4valid[iij4]
24491 && IKabs(cj4array[ij4] - cj4array[iij4])
24492 < IKFAST_SOLUTION_THRESH
24493 && IKabs(sj4array[ij4] - sj4array[iij4])
24494 < IKFAST_SOLUTION_THRESH)
24495 {
24496 j4valid[iij4] = false;
24497 _ij4[1] = iij4;
24498 break;
24499 }
24500 }
24501 j4 = j4array[ij4];
24502 cj4 = cj4array[ij4];
24503 sj4 = sj4array[ij4];
24504 {
24505 IkReal evalcond[6];
24506 IkReal x1745 = ((0.3) * cj9);
24507 IkReal x1746 = ((0.045) * sj9);
24508 IkReal x1747 = (cj6 * pz);
24509 IkReal x1748 = IKcos(j4);
24510 IkReal x1749 = (px * sj6 * x1748);
24511 IkReal x1750 = IKsin(j4);
24512 IkReal x1751 = (py * x1750);
24513 IkReal x1752 = (sj6 * x1751);
24514 IkReal x1753 = ((0.045) * cj9);
24515 IkReal x1754 = (px * x1750);
24516 IkReal x1755 = ((0.3) * sj9);
24517 IkReal x1756 = ((1.0) * x1748);
24518 IkReal x1757 = (py * x1756);
24519 IkReal x1758 = (sj8 * x1748);
24520 IkReal x1759 = (cj6 * cj8);
24521 IkReal x1760 = (px * x1756);
24522 IkReal x1761 = ((1.0) * x1751);
24523 IkReal x1762 = (cj8 * pz * sj6);
24524 IkReal x1763 = (sj8 * x1754);
24525 IkReal x1764 = ((0.09) * cj6 * cj8);
24526 evalcond[0]
24527 = ((-0.55) + (((-1.0) * x1746))
24528 + (((-1.0) * x1745)) + x1747 + x1749
24529 + x1752);
24530 evalcond[1]
24531 = ((((0.045) * sj8))
24532 + (((-1.0) * sj8 * x1753))
24533 + ((sj8 * x1755))
24534 + (((-1.0) * x1757)) + x1754);
24535 evalcond[2]
24536 = (((cj6 * sj8 * x1751))
24537 + (((-1.0) * cj8 * x1757))
24538 + ((cj8 * x1754))
24539 + (((-1.0) * (1.0) * pz * sj6 * sj8))
24540 + ((cj6 * px * x1758)));
24541 evalcond[3]
24542 = ((((-1.0) * x1761))
24543 + (((-1.0) * x1753 * x1759))
24544 + (((0.55) * sj6))
24545 + ((x1755 * x1759)) + ((sj6 * x1746))
24546 + (((0.045) * x1759))
24547 + ((sj6 * x1745))
24548 + (((-1.0) * x1760)));
24549 evalcond[4]
24550 = ((0.045) + (((-1.0) * sj8 * x1757))
24551 + (((-1.0) * x1759 * x1760))
24552 + (((-1.0) * x1759 * x1761)) + x1762
24553 + x1763 + (((-1.0) * x1753))
24554 + x1755);
24555 evalcond[5]
24556 = ((-0.2125) + (((-0.09) * x1762))
24557 + ((px * x1748 * x1764))
24558 + (((1.1) * x1749))
24559 + ((x1751 * x1764))
24560 + (((0.09) * py * x1758))
24561 + (((-1.0) * (1.0) * pp))
24562 + (((1.1) * x1747))
24563 + (((1.1) * x1752))
24564 + (((-0.09) * x1763)));
24565 if (IKabs(evalcond[0])
24566 > IKFAST_EVALCOND_THRESH
24567 || IKabs(evalcond[1])
24568 > IKFAST_EVALCOND_THRESH
24569 || IKabs(evalcond[2])
24570 > IKFAST_EVALCOND_THRESH
24571 || IKabs(evalcond[3])
24572 > IKFAST_EVALCOND_THRESH
24573 || IKabs(evalcond[4])
24574 > IKFAST_EVALCOND_THRESH
24575 || IKabs(evalcond[5])
24576 > IKFAST_EVALCOND_THRESH)
24577 {
24578 continue;
24579 }
24580 }
24581
24582 rotationfunction0(solutions);
24583 }
24584 }
24585 }
24586 }
24587 }
24588 else
24589 {
24590 {
24591 IkReal j4array[1], cj4array[1], sj4array[1];
24592 bool j4valid[1] = {false};
24593 _nj4 = 1;
24594 IkReal x1765 = ((0.3) * cj9);
24595 IkReal x1766 = ((0.045) * sj9);
24596 IkReal x1767 = ((1.0) * cj6 * pz);
24597 IkReal x1768 = ((0.045) * sj6 * sj8);
24598 IkReal x1769 = (px * x1768);
24599 IkReal x1770 = ((0.3) * sj6 * sj8 * sj9);
24600 IkReal x1771 = (py * x1768);
24601 CheckValue<IkReal> x1772 = IKatan2WithCheck(
24602 IkReal(
24603 (((py * x1765)) + ((py * x1766))
24604 + ((cj9 * x1769)) + (((0.55) * py))
24605 + (((-1.0) * x1769))
24606 + (((-1.0) * py * x1767))
24607 + (((-1.0) * px * x1770)))),
24608 (((px * x1766)) + ((py * x1770))
24609 + (((-1.0) * cj9 * x1771)) + (((0.55) * px))
24610 + (((-1.0) * px * x1767)) + ((px * x1765))
24611 + x1771),
24612 IKFAST_ATAN2_MAGTHRESH);
24613 if (!x1772.valid)
24614 {
24615 continue;
24616 }
24617 CheckValue<IkReal> x1773 = IKPowWithIntegerCheck(
24618 IKsign(
24619 ((((-1.0) * (1.0) * sj6 * (pz * pz)))
24620 + ((pp * sj6)))),
24621 -1);
24622 if (!x1773.valid)
24623 {
24624 continue;
24625 }
24626 j4array[0]
24627 = ((-1.5707963267949) + (x1772.value)
24628 + (((1.5707963267949) * (x1773.value))));
24629 sj4array[0] = IKsin(j4array[0]);
24630 cj4array[0] = IKcos(j4array[0]);
24631 if (j4array[0] > IKPI)
24632 {
24633 j4array[0] -= IK2PI;
24634 }
24635 else if (j4array[0] < -IKPI)
24636 {
24637 j4array[0] += IK2PI;
24638 }
24639 j4valid[0] = true;
24640 for (int ij4 = 0; ij4 < 1; ++ij4)
24641 {
24642 if (!j4valid[ij4])
24643 {
24644 continue;
24645 }
24646 _ij4[0] = ij4;
24647 _ij4[1] = -1;
24648 for (int iij4 = ij4 + 1; iij4 < 1; ++iij4)
24649 {
24650 if (j4valid[iij4]
24651 && IKabs(cj4array[ij4] - cj4array[iij4])
24652 < IKFAST_SOLUTION_THRESH
24653 && IKabs(sj4array[ij4] - sj4array[iij4])
24654 < IKFAST_SOLUTION_THRESH)
24655 {
24656 j4valid[iij4] = false;
24657 _ij4[1] = iij4;
24658 break;
24659 }
24660 }
24661 j4 = j4array[ij4];
24662 cj4 = cj4array[ij4];
24663 sj4 = sj4array[ij4];
24664 {
24665 IkReal evalcond[6];
24666 IkReal x1774 = ((0.3) * cj9);
24667 IkReal x1775 = ((0.045) * sj9);
24668 IkReal x1776 = (cj6 * pz);
24669 IkReal x1777 = IKcos(j4);
24670 IkReal x1778 = (px * sj6 * x1777);
24671 IkReal x1779 = IKsin(j4);
24672 IkReal x1780 = (py * x1779);
24673 IkReal x1781 = (sj6 * x1780);
24674 IkReal x1782 = ((0.045) * cj9);
24675 IkReal x1783 = (px * x1779);
24676 IkReal x1784 = ((0.3) * sj9);
24677 IkReal x1785 = ((1.0) * x1777);
24678 IkReal x1786 = (py * x1785);
24679 IkReal x1787 = (sj8 * x1777);
24680 IkReal x1788 = (cj6 * cj8);
24681 IkReal x1789 = (px * x1785);
24682 IkReal x1790 = ((1.0) * x1780);
24683 IkReal x1791 = (cj8 * pz * sj6);
24684 IkReal x1792 = (sj8 * x1783);
24685 IkReal x1793 = ((0.09) * cj6 * cj8);
24686 evalcond[0]
24687 = ((-0.55) + x1781 + (((-1.0) * x1775))
24688 + (((-1.0) * x1774)) + x1778 + x1776);
24689 evalcond[1]
24690 = ((((0.045) * sj8)) + (((-1.0) * x1786))
24691 + x1783 + (((-1.0) * sj8 * x1782))
24692 + ((sj8 * x1784)));
24693 evalcond[2]
24694 = ((((-1.0) * cj8 * x1786))
24695 + ((cj8 * x1783)) + ((cj6 * px * x1787))
24696 + (((-1.0) * (1.0) * pz * sj6 * sj8))
24697 + ((cj6 * sj8 * x1780)));
24698 evalcond[3]
24699 = ((((-1.0) * x1790)) + ((x1784 * x1788))
24700 + (((-1.0) * x1782 * x1788))
24701 + (((0.55) * sj6)) + ((sj6 * x1775))
24702 + (((-1.0) * x1789))
24703 + (((0.045) * x1788)) + ((sj6 * x1774)));
24704 evalcond[4]
24705 = ((0.045) + (((-1.0) * x1782))
24706 + (((-1.0) * x1788 * x1790)) + x1784
24707 + (((-1.0) * x1788 * x1789))
24708 + (((-1.0) * sj8 * x1786)) + x1792
24709 + x1791);
24710 evalcond[5]
24711 = ((-0.2125) + (((1.1) * x1776))
24712 + (((1.1) * x1778)) + ((x1780 * x1793))
24713 + (((1.1) * x1781))
24714 + (((-1.0) * (1.0) * pp))
24715 + (((0.09) * py * x1787))
24716 + (((-0.09) * x1792))
24717 + ((px * x1777 * x1793))
24718 + (((-0.09) * x1791)));
24719 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
24720 || IKabs(evalcond[1])
24721 > IKFAST_EVALCOND_THRESH
24722 || IKabs(evalcond[2])
24723 > IKFAST_EVALCOND_THRESH
24724 || IKabs(evalcond[3])
24725 > IKFAST_EVALCOND_THRESH
24726 || IKabs(evalcond[4])
24727 > IKFAST_EVALCOND_THRESH
24728 || IKabs(evalcond[5])
24729 > IKFAST_EVALCOND_THRESH)
24730 {
24731 continue;
24732 }
24733 }
24734
24735 rotationfunction0(solutions);
24736 }
24737 }
24738 }
24739 }
24740 }
24741 }
24742 }
24743 }
24744 }
24745 else
24746 {
24747 {
24748 IkReal j4array[2], cj4array[2], sj4array[2];
24749 bool j4valid[2] = {false};
24750 _nj4 = 2;
24751 CheckValue<IkReal> x1797 = IKatan2WithCheck(
24752 IkReal(((-1.0) * (((1.0) * py)))),
24753 px,
24754 IKFAST_ATAN2_MAGTHRESH);
24755 if (!x1797.valid)
24756 {
24757 continue;
24758 }
24759 IkReal x1794 = ((-1.0) * (x1797.value));
24760 IkReal x1795 = ((0.045) * sj8);
24761 if ((((py * py) + (px * px))) < -0.00001)
24762 continue;
24763 CheckValue<IkReal> x1798 = IKPowWithIntegerCheck(
24764 IKabs(IKsqrt(((py * py) + (px * px)))), -1);
24765 if (!x1798.valid)
24766 {
24767 continue;
24768 }
24769 if ((((x1798.value)
24770 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24771 + x1795))))
24772 < -1 - IKFAST_SINCOS_THRESH
24773 || (((x1798.value)
24774 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24775 + x1795))))
24776 > 1 + IKFAST_SINCOS_THRESH)
24777 continue;
24778 IkReal x1796 = IKasin(
24779 ((x1798.value)
24780 * (((((-1.0) * cj9 * x1795)) + (((0.3) * sj8 * sj9))
24781 + x1795))));
24782 j4array[0] = ((((-1.0) * x1796)) + x1794);
24783 sj4array[0] = IKsin(j4array[0]);
24784 cj4array[0] = IKcos(j4array[0]);
24785 j4array[1] = ((3.14159265358979) + x1794 + x1796);
24786 sj4array[1] = IKsin(j4array[1]);
24787 cj4array[1] = IKcos(j4array[1]);
24788 if (j4array[0] > IKPI)
24789 {
24790 j4array[0] -= IK2PI;
24791 }
24792 else if (j4array[0] < -IKPI)
24793 {
24794 j4array[0] += IK2PI;
24795 }
24796 j4valid[0] = true;
24797 if (j4array[1] > IKPI)
24798 {
24799 j4array[1] -= IK2PI;
24800 }
24801 else if (j4array[1] < -IKPI)
24802 {
24803 j4array[1] += IK2PI;
24804 }
24805 j4valid[1] = true;
24806 for (int ij4 = 0; ij4 < 2; ++ij4)
24807 {
24808 if (!j4valid[ij4])
24809 {
24810 continue;
24811 }
24812 _ij4[0] = ij4;
24813 _ij4[1] = -1;
24814 for (int iij4 = ij4 + 1; iij4 < 2; ++iij4)
24815 {
24816 if (j4valid[iij4]
24817 && IKabs(cj4array[ij4] - cj4array[iij4])
24818 < IKFAST_SOLUTION_THRESH
24819 && IKabs(sj4array[ij4] - sj4array[iij4])
24820 < IKFAST_SOLUTION_THRESH)
24821 {
24822 j4valid[iij4] = false;
24823 _ij4[1] = iij4;
24824 break;
24825 }
24826 }
24827 j4 = j4array[ij4];
24828 cj4 = cj4array[ij4];
24829 sj4 = sj4array[ij4];
24830
24831 {
24832 IkReal j6eval[2];
24833 IkReal x1799 = (cj4 * px);
24834 IkReal x1800 = (cj8 * pz);
24835 IkReal x1801 = (py * sj4);
24836 IkReal x1802 = (cj4 * cj9 * px);
24837 IkReal x1803 = (cj4 * px * sj9);
24838 IkReal x1804 = (cj8 * pz * sj9);
24839 IkReal x1805 = (cj9 * py * sj4);
24840 IkReal x1806 = (py * sj4 * sj9);
24841 IkReal x1807 = ((0.045) * x1800);
24842 j6eval[0]
24843 = ((((-6.66666666666667) * x1802)) + (((-1.0) * x1806))
24844 + (((-12.2222222222222) * x1799))
24845 + (((-1.0) * x1803))
24846 + (((-6.66666666666667) * x1804))
24847 + (((-1.0) * x1800))
24848 + (((-6.66666666666667) * x1805))
24849 + (((-12.2222222222222) * x1801)) + ((cj9 * x1800)));
24850 j6eval[1] = IKsign(
24851 ((((-0.3) * x1804)) + (((-0.3) * x1802))
24852 + (((-0.045) * x1806)) + (((-0.55) * x1799))
24853 + ((cj9 * x1807)) + (((-0.3) * x1805))
24854 + (((-1.0) * x1807)) + (((-0.55) * x1801))
24855 + (((-0.045) * x1803))));
24856 if (IKabs(j6eval[0]) < 0.0000010000000000
24857 || IKabs(j6eval[1]) < 0.0000010000000000)
24858 {
24859 {
24860 IkReal j6eval[2];
24861 IkReal x1808 = (sj8 * (py * py));
24862 IkReal x1809 = cj4 * cj4;
24863 IkReal x1810
24864 = ((((-1.0) * x1808 * x1809)) + ((sj8 * (pz * pz)))
24865 + x1808 + ((sj8 * x1809 * (px * px)))
24866 + (((2.0) * cj4 * px * py * sj4 * sj8)));
24867 j6eval[0] = x1810;
24868 j6eval[1] = IKsign(x1810);
24869 if (IKabs(j6eval[0]) < 0.0000010000000000
24870 || IKabs(j6eval[1]) < 0.0000010000000000)
24871 {
24872 {
24873 IkReal j6eval[2];
24874 IkReal x1811 = (pz * sj8);
24875 IkReal x1812 = (cj9 * pz * sj8);
24876 IkReal x1813 = (pz * sj8 * sj9);
24877 IkReal x1814 = (cj8 * sj8);
24878 IkReal x1815 = (cj4 * px * x1814);
24879 IkReal x1816 = (py * sj4 * x1814);
24880 IkReal x1817 = ((1.0) * cj9);
24881 IkReal x1818 = (cj4 * cj8 * px * sj8 * sj9);
24882 IkReal x1819 = (cj8 * py * sj4 * sj8 * sj9);
24883 IkReal x1820 = ((0.045) * x1815);
24884 IkReal x1821 = ((0.045) * x1816);
24885 j6eval[0]
24886 = ((((-1.0) * x1816 * x1817))
24887 + (((6.66666666666667) * x1818))
24888 + (((-1.0) * x1813)) + x1815 + x1816
24889 + (((-1.0) * x1815 * x1817))
24890 + (((-6.66666666666667) * x1812))
24891 + (((-12.2222222222222) * x1811))
24892 + (((6.66666666666667) * x1819)));
24893 j6eval[1] = IKsign(
24894 ((((-1.0) * cj9 * x1821)) + x1821 + x1820
24895 + (((0.3) * x1819)) + (((-1.0) * cj9 * x1820))
24896 + (((-0.045) * x1813)) + (((0.3) * x1818))
24897 + (((-0.55) * x1811)) + (((-0.3) * x1812))));
24898 if (IKabs(j6eval[0]) < 0.0000010000000000
24899 || IKabs(j6eval[1]) < 0.0000010000000000)
24900 {
24901 {
24902 IkReal evalcond[4];
24903 bool bgotonextstatement = true;
24904 do
24905 {
24906 IkReal x1822
24907 = (((px * sj4))
24908 + (((-1.0) * (1.0) * cj4 * py)));
24909 evalcond[0]
24910 = ((-3.14159265358979)
24911 + (IKfmod(
24912 ((3.14159265358979)
24913 + (IKabs(j8))),
24914 6.28318530717959)));
24915 evalcond[1]
24916 = ((0.39655) + (((0.0765) * sj9))
24917 + (((0.32595) * cj9))
24918 + (((-1.0) * (1.0) * pp)));
24919 evalcond[2] = x1822;
24920 evalcond[3] = x1822;
24921 if (IKabs(evalcond[0]) < 0.0000010000000000
24922 && IKabs(evalcond[1]) < 0.0000010000000000
24923 && IKabs(evalcond[2]) < 0.0000010000000000
24924 && IKabs(evalcond[3])
24925 < 0.0000010000000000)
24926 {
24927 bgotonextstatement = false;
24928 {
24929 IkReal j6eval[2];
24930 sj8 = 0;
24931 cj8 = 1.0;
24932 j8 = 0;
24933 IkReal x1823 = (cj9 * pz);
24934 IkReal x1824 = (cj4 * px);
24935 IkReal x1825 = (pp * pz);
24936 IkReal x1826 = (py * sj4);
24937 IkReal x1827 = (cj4 * cj9 * px);
24938 IkReal x1828 = (cj4 * pp * px);
24939 IkReal x1829 = (cj9 * py * sj4);
24940 IkReal x1830 = (pp * py * sj4);
24941 j6eval[0]
24942 = ((((-36.2220411120167) * x1830))
24943 + (((13.9482024812098) * x1826))
24944 + (((5.4333061668025) * x1825))
24945 + x1823
24946 + (((13.9482024812098) * x1824))
24947 + (((-36.2220411120167) * x1828))
24948 + (((12.2222222222222) * x1827))
24949 + (((12.2222222222222) * x1829))
24950 + (((2.92556370551481) * pz)));
24951 j6eval[1] = IKsign(
24952 ((((-3.92156862745098) * x1830))
24953 + (((0.316735294117647) * pz))
24954 + (((-3.92156862745098) * x1828))
24955 + (((1.32323529411765) * x1827))
24956 + (((0.588235294117647) * x1825))
24957 + (((1.51009803921569) * x1824))
24958 + (((1.32323529411765) * x1829))
24959 + (((1.51009803921569) * x1826))
24960 + (((0.108264705882353) * x1823))));
24961 if (IKabs(j6eval[0]) < 0.0000010000000000
24962 || IKabs(j6eval[1])
24963 < 0.0000010000000000)
24964 {
24965 {
24966 IkReal j6eval[2];
24967 sj8 = 0;
24968 cj8 = 1.0;
24969 j8 = 0;
24970 IkReal x1831 = (cj4 * px);
24971 IkReal x1832 = (py * sj4);
24972 IkReal x1833 = (cj9 * pz);
24973 IkReal x1834 = (pz * sj9);
24974 IkReal x1835 = ((1.0) * cj9);
24975 IkReal x1836 = (cj4 * px * sj9);
24976 IkReal x1837 = (py * sj4 * sj9);
24977 IkReal x1838 = ((0.045) * x1831);
24978 IkReal x1839 = ((0.045) * x1832);
24979 j6eval[0]
24980 = ((((-1.0) * (12.2222222222222)
24981 * pz))
24982 + (((-1.0) * x1831 * x1835))
24983 + (((-1.0) * x1834))
24984 + (((-6.66666666666667)
24985 * x1833))
24986 + x1832 + x1831
24987 + (((6.66666666666667)
24988 * x1837))
24989 + (((-1.0) * x1832 * x1835))
24990 + (((6.66666666666667)
24991 * x1836)));
24992 j6eval[1] = IKsign(
24993 ((((-1.0) * (0.55) * pz))
24994 + (((0.3) * x1836))
24995 + (((-1.0) * cj9 * x1839))
24996 + (((-1.0) * cj9 * x1838))
24997 + (((0.3) * x1837))
24998 + (((-0.045) * x1834))
24999 + (((-0.3) * x1833)) + x1839
25000 + x1838));
25001 if (IKabs(j6eval[0])
25002 < 0.0000010000000000
25003 || IKabs(j6eval[1])
25004 < 0.0000010000000000)
25005 {
25006 {
25007 IkReal j6eval[2];
25008 sj8 = 0;
25009 cj8 = 1.0;
25010 j8 = 0;
25011 IkReal x1840 = (cj4 * px);
25012 IkReal x1841 = (cj9 * pz);
25013 IkReal x1842 = (pp * pz);
25014 IkReal x1843 = (py * sj4);
25015 IkReal x1844 = (cj4 * cj9 * px);
25016 IkReal x1845 = (cj4 * pp * px);
25017 IkReal x1846 = (cj9 * py * sj4);
25018 IkReal x1847 = (pp * py * sj4);
25019 j6eval[0]
25020 = ((((-5.4333061668025)
25021 * x1847))
25022 + (((-5.4333061668025)
25023 * x1845))
25024 + (((-36.2220411120167)
25025 * x1842))
25026 + (((-2.92556370551481)
25027 * x1843))
25028 + (((-1.0) * x1846))
25029 + (((-1.0) * x1844))
25030 + (((13.9482024812098)
25031 * pz))
25032 + (((-2.92556370551481)
25033 * x1840))
25034 + (((12.2222222222222)
25035 * x1841)));
25036 j6eval[1] = IKsign(
25037 ((((-0.108264705882353)
25038 * x1844))
25039 + (((1.51009803921569) * pz))
25040 + (((1.32323529411765)
25041 * x1841))
25042 + (((-0.588235294117647)
25043 * x1845))
25044 + (((-0.108264705882353)
25045 * x1846))
25046 + (((-0.316735294117647)
25047 * x1843))
25048 + (((-0.588235294117647)
25049 * x1847))
25050 + (((-0.316735294117647)
25051 * x1840))
25052 + (((-3.92156862745098)
25053 * x1842))));
25054 if (IKabs(j6eval[0])
25055 < 0.0000010000000000
25056 || IKabs(j6eval[1])
25057 < 0.0000010000000000)
25058 {
25059 {
25060 IkReal evalcond[1];
25061 bool bgotonextstatement
25062 = true;
25063 do
25064 {
25065 evalcond[0]
25066 = ((IKabs((
25067 (-3.14159265358979)
25068 + (IKfmod(
25069 ((3.14159265358979)
25070 + j9),
25071 6.28318530717959)))))
25072 + (IKabs(pz)));
25073 if (IKabs(evalcond[0])
25074 < 0.0000010000000000)
25075 {
25076 bgotonextstatement
25077 = false;
25078 {
25079 IkReal j6eval[1];
25080 IkReal x1848
25081 = ((1.0) * py);
25082 sj8 = 0;
25083 cj8 = 1.0;
25084 j8 = 0;
25085 pz = 0;
25086 j9 = 0;
25087 sj9 = 0;
25088 cj9 = 1.0;
25089 pp
25090 = ((py * py)
25091 + (px * px));
25092 npx
25093 = (((py * r10))
25094 + ((px * r00)));
25095 npy
25096 = (((py * r11))
25097 + ((px * r01)));
25098 npz
25099 = (((px * r02))
25100 + ((py * r12)));
25101 rxp0_0
25102 = ((-1.0) * r20
25103 * x1848);
25104 rxp0_1 = (px * r20);
25105 rxp1_0
25106 = ((-1.0) * r21
25107 * x1848);
25108 rxp1_1 = (px * r21);
25109 rxp2_0
25110 = ((-1.0) * r22
25111 * x1848);
25112 rxp2_1 = (px * r22);
25113 j6eval[0]
25114 = (((py * sj4))
25115 + ((cj4 * px)));
25116 if (IKabs(j6eval[0])
25117 < 0.0000010000000000)
25118 {
25119 {
25120 IkReal j6eval[1];
25121 IkReal x1849
25122 = ((1.0) * py);
25123 sj8 = 0;
25124 cj8 = 1.0;
25125 j8 = 0;
25126 pz = 0;
25127 j9 = 0;
25128 sj9 = 0;
25129 cj9 = 1.0;
25130 pp
25131 = ((py * py)
25132 + (px * px));
25133 npx
25134 = (((py * r10))
25135 + ((px
25136 * r00)));
25137 npy
25138 = (((py * r11))
25139 + ((px
25140 * r01)));
25141 npz
25142 = (((px * r02))
25143 + ((py
25144 * r12)));
25145 rxp0_0
25146 = ((-1.0) * r20
25147 * x1849);
25148 rxp0_1 = (px * r20);
25149 rxp1_0
25150 = ((-1.0) * r21
25151 * x1849);
25152 rxp1_1 = (px * r21);
25153 rxp2_0
25154 = ((-1.0) * r22
25155 * x1849);
25156 rxp2_1 = (px * r22);
25157 j6eval[0]
25158 = ((-1.0)
25159 + (((-1.0)
25160 * (1.3840830449827)
25161 * (px
25162 * px)))
25163 + (((-1.0)
25164 * (1.3840830449827)
25165 * (py
25166 * py))));
25167 if (IKabs(j6eval[0])
25168 < 0.0000010000000000)
25169 {
25170 {
25171 IkReal
25172 j6eval[2];
25173 IkReal x1850
25174 = ((1.0)
25175 * py);
25176 sj8 = 0;
25177 cj8 = 1.0;
25178 j8 = 0;
25179 pz = 0;
25180 j9 = 0;
25181 sj9 = 0;
25182 cj9 = 1.0;
25183 pp
25184 = ((py * py)
25185 + (px
25186 * px));
25187 npx
25188 = (((py
25189 * r10))
25190 + ((px
25191 * r00)));
25192 npy
25193 = (((py
25194 * r11))
25195 + ((px
25196 * r01)));
25197 npz
25198 = (((px
25199 * r02))
25200 + ((py
25201 * r12)));
25202 rxp0_0
25203 = ((-1.0)
25204 * r20
25205 * x1850);
25206 rxp0_1
25207 = (px
25208 * r20);
25209 rxp1_0
25210 = ((-1.0)
25211 * r21
25212 * x1850);
25213 rxp1_1
25214 = (px
25215 * r21);
25216 rxp2_0
25217 = ((-1.0)
25218 * r22
25219 * x1850);
25220 rxp2_1
25221 = (px
25222 * r22);
25223 IkReal x1851
25224 = (cj4
25225 * px);
25226 IkReal x1852
25227 = (py
25228 * sj4);
25229 j6eval[0]
25230 = (x1852
25231 + x1851);
25232 j6eval[1]
25233 = ((((-1.0)
25234 * (1.3840830449827)
25235 * cj4
25236 * (px
25237 * px
25238 * px)))
25239 + (((-1.0)
25240 * x1852))
25241 + (((-1.0)
25242 * x1851))
25243 + (((-1.3840830449827)
25244 * x1852
25245 * (px
25246 * px)))
25247 + (((-1.0)
25248 * (1.3840830449827)
25249 * sj4
25250 * (py
25251 * py
25252 * py)))
25253 + (((-1.3840830449827)
25254 * x1851
25255 * (py
25256 * py))));
25257 if (IKabs(
25258 j6eval
25259 [0])
25260 < 0.0000010000000000
25261 || IKabs(
25262 j6eval
25263 [1])
25264 < 0.0000010000000000)
25265 {
25266 {
25267 IkReal
25268 evalcond
25269 [4];
25270 bool
25271 bgotonextstatement
25272 = true;
25273 do
25274 {
25275 evalcond
25276 [0]
25277 = ((IKabs(
25278 py))
25279 + (IKabs(
25280 px)));
25281 evalcond
25282 [1]
25283 = -0.85;
25284 evalcond
25285 [2]
25286 = 0;
25287 evalcond
25288 [3]
25289 = -0.2125;
25290 if (IKabs(
25291 evalcond
25292 [0])
25293 < 0.0000010000000000
25294 && IKabs(
25295 evalcond
25296 [1])
25297 < 0.0000010000000000
25298 && IKabs(
25299 evalcond
25300 [2])
25301 < 0.0000010000000000
25302 && IKabs(
25303 evalcond
25304 [3])
25305 < 0.0000010000000000)
25306 {
25307 bgotonextstatement
25308 = false;
25309 {
25310 IkReal j6array
25311 [2],
25312 cj6array
25313 [2],
25314 sj6array
25315 [2];
25316 bool j6valid
25317 [2]
25318 = {false};
25319 _nj6
25320 = 2;
25321 j6array
25322 [0]
25323 = 2.9927027059803;
25324 sj6array
25325 [0]
25326 = IKsin(
25327 j6array
25328 [0]);
25329 cj6array
25330 [0]
25331 = IKcos(
25332 j6array
25333 [0]);
25334 j6array
25335 [1]
25336 = 6.13429535957009;
25337 sj6array
25338 [1]
25339 = IKsin(
25340 j6array
25341 [1]);
25342 cj6array
25343 [1]
25344 = IKcos(
25345 j6array
25346 [1]);
25347 if (j6array
25348 [0]
25349 > IKPI)
25350 {
25351 j6array
25352 [0]
25353 -= IK2PI;
25354 }
25355 else if (
25356 j6array
25357 [0]
25358 < -IKPI)
25359 {
25360 j6array
25361 [0]
25362 += IK2PI;
25363 }
25364 j6valid
25365 [0]
25366 = true;
25367 if (j6array
25368 [1]
25369 > IKPI)
25370 {
25371 j6array
25372 [1]
25373 -= IK2PI;
25374 }
25375 else if (
25376 j6array
25377 [1]
25378 < -IKPI)
25379 {
25380 j6array
25381 [1]
25382 += IK2PI;
25383 }
25384 j6valid
25385 [1]
25386 = true;
25387 for (
25388 int ij6
25389 = 0;
25390 ij6
25391 < 2;
25392 ++ij6)
25393 {
25394 if (!j6valid
25395 [ij6])
25396 {
25397 continue;
25398 }
25399 _ij6[0]
25400 = ij6;
25401 _ij6[1]
25402 = -1;
25403 for (
25404 int iij6
25405 = ij6
25406 + 1;
25407 iij6
25408 < 2;
25409 ++iij6)
25410 {
25411 if (j6valid
25412 [iij6]
25413 && IKabs(
25414 cj6array
25415 [ij6]
25416 - cj6array
25417 [iij6])
25418 < IKFAST_SOLUTION_THRESH
25419 && IKabs(
25420 sj6array
25421 [ij6]
25422 - sj6array
25423 [iij6])
25424 < IKFAST_SOLUTION_THRESH)
25425 {
25426 j6valid
25427 [iij6]
25428 = false;
25429 _ij6[1]
25430 = iij6;
25431 break;
25432 }
25433 }
25434 j6 = j6array
25435 [ij6];
25436 cj6 = cj6array
25437 [ij6];
25438 sj6 = sj6array
25439 [ij6];
25440 {
25441 IkReal evalcond
25442 [1];
25443 evalcond
25444 [0]
25445 = ((0.85)
25446 * (IKsin(
25447 j6)));
25448 if (IKabs(
25449 evalcond
25450 [0])
25451 > IKFAST_EVALCOND_THRESH)
25452 {
25453 continue;
25454 }
25455 }
25456
25457 rotationfunction0(
25458 solutions);
25459 }
25460 }
25461 }
25462 } while (0);
25463 if (bgotonextstatement)
25464 {
25465 bool
25466 bgotonextstatement
25467 = true;
25468 do
25469 {
25470 evalcond
25471 [0]
25472 = ((IKabs((
25473 (-3.14159265358979)
25474 + (IKfmod(
25475 ((3.14159265358979)
25476 + j4),
25477 6.28318530717959)))))
25478 + (IKabs(
25479 px)));
25480 evalcond
25481 [1]
25482 = -0.85;
25483 evalcond
25484 [2]
25485 = 0;
25486 evalcond
25487 [3]
25488 = ((-0.2125)
25489 + (((-1.0)
25490 * (1.0)
25491 * (py
25492 * py))));
25493 if (IKabs(
25494 evalcond
25495 [0])
25496 < 0.0000010000000000
25497 && IKabs(
25498 evalcond
25499 [1])
25500 < 0.0000010000000000
25501 && IKabs(
25502 evalcond
25503 [2])
25504 < 0.0000010000000000
25505 && IKabs(
25506 evalcond
25507 [3])
25508 < 0.0000010000000000)
25509 {
25510 bgotonextstatement
25511 = false;
25512 {
25513 IkReal j6eval
25514 [1];
25515 IkReal
25516 x1853
25517 = ((1.0)
25518 * py);
25519 sj8 = 0;
25520 cj8 = 1.0;
25521 j8 = 0;
25522 pz = 0;
25523 j9 = 0;
25524 sj9 = 0;
25525 cj9 = 1.0;
25526 pp = py
25527 * py;
25528 npx
25529 = (py
25530 * r10);
25531 npy
25532 = (py
25533 * r11);
25534 npz
25535 = (py
25536 * r12);
25537 rxp0_0
25538 = ((-1.0)
25539 * r20
25540 * x1853);
25541 rxp0_1
25542 = 0;
25543 rxp1_0
25544 = ((-1.0)
25545 * r21
25546 * x1853);
25547 rxp1_1
25548 = 0;
25549 rxp2_0
25550 = ((-1.0)
25551 * r22
25552 * x1853);
25553 rxp2_1
25554 = 0;
25555 px = 0;
25556 j4 = 0;
25557 sj4 = 0;
25558 cj4 = 1.0;
25559 rxp0_2
25560 = (py
25561 * r00);
25562 rxp1_2
25563 = (py
25564 * r01);
25565 rxp2_2
25566 = (py
25567 * r02);
25568 j6eval
25569 [0]
25570 = ((1.0)
25571 + (((1.91568587540858)
25572 * (py
25573 * py
25574 * py
25575 * py)))
25576 + (((-1.0)
25577 * (2.64633970947792)
25578 * (py
25579 * py))));
25580 if (IKabs(
25581 j6eval
25582 [0])
25583 < 0.0000010000000000)
25584 {
25585 continue; // no branches [j6]
25586 }
25587 else
25588 {
25589 {
25590 IkReal j6array
25591 [2],
25592 cj6array
25593 [2],
25594 sj6array
25595 [2];
25596 bool j6valid
25597 [2]
25598 = {false};
25599 _nj6
25600 = 2;
25601 IkReal
25602 x1854
25603 = py
25604 * py;
25605 CheckValue<IkReal> x1856 = IKatan2WithCheck(
25606 IkReal((
25607 (-0.425)
25608 + (((-0.588235294117647)
25609 * x1854)))),
25610 ((-2.83333333333333)
25611 + (((3.92156862745098)
25612 * x1854))),
25613 IKFAST_ATAN2_MAGTHRESH);
25614 if (!x1856
25615 .valid)
25616 {
25617 continue;
25618 }
25619 IkReal
25620 x1855
25621 = ((-1.0)
25622 * (x1856
25623 .value));
25624 j6array
25625 [0]
25626 = x1855;
25627 sj6array
25628 [0]
25629 = IKsin(
25630 j6array
25631 [0]);
25632 cj6array
25633 [0]
25634 = IKcos(
25635 j6array
25636 [0]);
25637 j6array
25638 [1]
25639 = ((3.14159265358979)
25640 + x1855);
25641 sj6array
25642 [1]
25643 = IKsin(
25644 j6array
25645 [1]);
25646 cj6array
25647 [1]
25648 = IKcos(
25649 j6array
25650 [1]);
25651 if (j6array
25652 [0]
25653 > IKPI)
25654 {
25655 j6array
25656 [0]
25657 -= IK2PI;
25658 }
25659 else if (
25660 j6array
25661 [0]
25662 < -IKPI)
25663 {
25664 j6array
25665 [0]
25666 += IK2PI;
25667 }
25668 j6valid
25669 [0]
25670 = true;
25671 if (j6array
25672 [1]
25673 > IKPI)
25674 {
25675 j6array
25676 [1]
25677 -= IK2PI;
25678 }
25679 else if (
25680 j6array
25681 [1]
25682 < -IKPI)
25683 {
25684 j6array
25685 [1]
25686 += IK2PI;
25687 }
25688 j6valid
25689 [1]
25690 = true;
25691 for (
25692 int ij6
25693 = 0;
25694 ij6
25695 < 2;
25696 ++ij6)
25697 {
25698 if (!j6valid
25699 [ij6])
25700 {
25701 continue;
25702 }
25703 _ij6[0]
25704 = ij6;
25705 _ij6[1]
25706 = -1;
25707 for (
25708 int iij6
25709 = ij6
25710 + 1;
25711 iij6
25712 < 2;
25713 ++iij6)
25714 {
25715 if (j6valid
25716 [iij6]
25717 && IKabs(
25718 cj6array
25719 [ij6]
25720 - cj6array
25721 [iij6])
25722 < IKFAST_SOLUTION_THRESH
25723 && IKabs(
25724 sj6array
25725 [ij6]
25726 - sj6array
25727 [iij6])
25728 < IKFAST_SOLUTION_THRESH)
25729 {
25730 j6valid
25731 [iij6]
25732 = false;
25733 _ij6[1]
25734 = iij6;
25735 break;
25736 }
25737 }
25738 j6 = j6array
25739 [ij6];
25740 cj6 = cj6array
25741 [ij6];
25742 sj6 = sj6array
25743 [ij6];
25744 {
25745 IkReal evalcond
25746 [1];
25747 evalcond
25748 [0]
25749 = ((0.85)
25750 * (IKsin(
25751 j6)));
25752 if (IKabs(
25753 evalcond
25754 [0])
25755 > IKFAST_EVALCOND_THRESH)
25756 {
25757 continue;
25758 }
25759 }
25760
25761 rotationfunction0(
25762 solutions);
25763 }
25764 }
25765 }
25766 }
25767 }
25768 } while (
25769 0);
25770 if (bgotonextstatement)
25771 {
25772 bool
25773 bgotonextstatement
25774 = true;
25775 do
25776 {
25777 evalcond
25778 [0]
25779 = ((IKabs((
25780 (-3.14159265358979)
25781 + (IKfmod(
25782 j4,
25783 6.28318530717959)))))
25784 + (IKabs(
25785 px)));
25786 evalcond
25787 [1]
25788 = -0.85;
25789 evalcond
25790 [2]
25791 = 0;
25792 evalcond
25793 [3]
25794 = ((-0.2125)
25795 + (((-1.0)
25796 * (1.0)
25797 * (py
25798 * py))));
25799 if (IKabs(
25800 evalcond
25801 [0])
25802 < 0.0000010000000000
25803 && IKabs(
25804 evalcond
25805 [1])
25806 < 0.0000010000000000
25807 && IKabs(
25808 evalcond
25809 [2])
25810 < 0.0000010000000000
25811 && IKabs(
25812 evalcond
25813 [3])
25814 < 0.0000010000000000)
25815 {
25816 bgotonextstatement
25817 = false;
25818 {
25819 IkReal j6eval
25820 [1];
25821 IkReal
25822 x1857
25823 = ((1.0)
25824 * py);
25825 sj8 = 0;
25826 cj8 = 1.0;
25827 j8 = 0;
25828 pz = 0;
25829 j9 = 0;
25830 sj9 = 0;
25831 cj9 = 1.0;
25832 pp = py
25833 * py;
25834 npx
25835 = (py
25836 * r10);
25837 npy
25838 = (py
25839 * r11);
25840 npz
25841 = (py
25842 * r12);
25843 rxp0_0
25844 = ((-1.0)
25845 * r20
25846 * x1857);
25847 rxp0_1
25848 = 0;
25849 rxp1_0
25850 = ((-1.0)
25851 * r21
25852 * x1857);
25853 rxp1_1
25854 = 0;
25855 rxp2_0
25856 = ((-1.0)
25857 * r22
25858 * x1857);
25859 rxp2_1
25860 = 0;
25861 px = 0;
25862 j4 = 3.14159265358979;
25863 sj4 = 0;
25864 cj4 = -1.0;
25865 rxp0_2
25866 = (py
25867 * r00);
25868 rxp1_2
25869 = (py
25870 * r01);
25871 rxp2_2
25872 = (py
25873 * r02);
25874 j6eval
25875 [0]
25876 = ((1.0)
25877 + (((1.91568587540858)
25878 * (py
25879 * py
25880 * py
25881 * py)))
25882 + (((-1.0)
25883 * (2.64633970947792)
25884 * (py
25885 * py))));
25886 if (IKabs(
25887 j6eval
25888 [0])
25889 < 0.0000010000000000)
25890 {
25891 continue; // no branches [j6]
25892 }
25893 else
25894 {
25895 {
25896 IkReal j6array
25897 [2],
25898 cj6array
25899 [2],
25900 sj6array
25901 [2];
25902 bool j6valid
25903 [2]
25904 = {false};
25905 _nj6
25906 = 2;
25907 IkReal
25908 x1858
25909 = py
25910 * py;
25911 CheckValue<IkReal> x1860 = IKatan2WithCheck(
25912 IkReal((
25913 (-0.425)
25914 + (((-0.588235294117647)
25915 * x1858)))),
25916 ((-2.83333333333333)
25917 + (((3.92156862745098)
25918 * x1858))),
25919 IKFAST_ATAN2_MAGTHRESH);
25920 if (!x1860
25921 .valid)
25922 {
25923 continue;
25924 }
25925 IkReal
25926 x1859
25927 = ((-1.0)
25928 * (x1860
25929 .value));
25930 j6array
25931 [0]
25932 = x1859;
25933 sj6array
25934 [0]
25935 = IKsin(
25936 j6array
25937 [0]);
25938 cj6array
25939 [0]
25940 = IKcos(
25941 j6array
25942 [0]);
25943 j6array
25944 [1]
25945 = ((3.14159265358979)
25946 + x1859);
25947 sj6array
25948 [1]
25949 = IKsin(
25950 j6array
25951 [1]);
25952 cj6array
25953 [1]
25954 = IKcos(
25955 j6array
25956 [1]);
25957 if (j6array
25958 [0]
25959 > IKPI)
25960 {
25961 j6array
25962 [0]
25963 -= IK2PI;
25964 }
25965 else if (
25966 j6array
25967 [0]
25968 < -IKPI)
25969 {
25970 j6array
25971 [0]
25972 += IK2PI;
25973 }
25974 j6valid
25975 [0]
25976 = true;
25977 if (j6array
25978 [1]
25979 > IKPI)
25980 {
25981 j6array
25982 [1]
25983 -= IK2PI;
25984 }
25985 else if (
25986 j6array
25987 [1]
25988 < -IKPI)
25989 {
25990 j6array
25991 [1]
25992 += IK2PI;
25993 }
25994 j6valid
25995 [1]
25996 = true;
25997 for (
25998 int ij6
25999 = 0;
26000 ij6
26001 < 2;
26002 ++ij6)
26003 {
26004 if (!j6valid
26005 [ij6])
26006 {
26007 continue;
26008 }
26009 _ij6[0]
26010 = ij6;
26011 _ij6[1]
26012 = -1;
26013 for (
26014 int iij6
26015 = ij6
26016 + 1;
26017 iij6
26018 < 2;
26019 ++iij6)
26020 {
26021 if (j6valid
26022 [iij6]
26023 && IKabs(
26024 cj6array
26025 [ij6]
26026 - cj6array
26027 [iij6])
26028 < IKFAST_SOLUTION_THRESH
26029 && IKabs(
26030 sj6array
26031 [ij6]
26032 - sj6array
26033 [iij6])
26034 < IKFAST_SOLUTION_THRESH)
26035 {
26036 j6valid
26037 [iij6]
26038 = false;
26039 _ij6[1]
26040 = iij6;
26041 break;
26042 }
26043 }
26044 j6 = j6array
26045 [ij6];
26046 cj6 = cj6array
26047 [ij6];
26048 sj6 = sj6array
26049 [ij6];
26050 {
26051 IkReal evalcond
26052 [1];
26053 evalcond
26054 [0]
26055 = ((0.85)
26056 * (IKsin(
26057 j6)));
26058 if (IKabs(
26059 evalcond
26060 [0])
26061 > IKFAST_EVALCOND_THRESH)
26062 {
26063 continue;
26064 }
26065 }
26066
26067 rotationfunction0(
26068 solutions);
26069 }
26070 }
26071 }
26072 }
26073 }
26074 } while (
26075 0);
26076 if (bgotonextstatement)
26077 {
26078 bool
26079 bgotonextstatement
26080 = true;
26081 do
26082 {
26083 evalcond
26084 [0]
26085 = ((IKabs((
26086 (-3.14159265358979)
26087 + (IKfmod(
26088 ((1.5707963267949)
26089 + j4),
26090 6.28318530717959)))))
26091 + (IKabs(
26092 py)));
26093 evalcond
26094 [1]
26095 = -0.85;
26096 evalcond
26097 [2]
26098 = 0;
26099 evalcond
26100 [3]
26101 = ((-0.2125)
26102 + (((-1.0)
26103 * (1.0)
26104 * (px
26105 * px))));
26106 if (IKabs(
26107 evalcond
26108 [0])
26109 < 0.0000010000000000
26110 && IKabs(
26111 evalcond
26112 [1])
26113 < 0.0000010000000000
26114 && IKabs(
26115 evalcond
26116 [2])
26117 < 0.0000010000000000
26118 && IKabs(
26119 evalcond
26120 [3])
26121 < 0.0000010000000000)
26122 {
26123 bgotonextstatement
26124 = false;
26125 {
26126 IkReal j6eval
26127 [1];
26128 IkReal
26129 x1861
26130 = ((1.0)
26131 * px);
26132 sj8 = 0;
26133 cj8 = 1.0;
26134 j8 = 0;
26135 pz = 0;
26136 j9 = 0;
26137 sj9 = 0;
26138 cj9 = 1.0;
26139 pp = px
26140 * px;
26141 npx
26142 = (px
26143 * r00);
26144 npy
26145 = (px
26146 * r01);
26147 npz
26148 = (px
26149 * r02);
26150 rxp0_0
26151 = 0;
26152 rxp0_1
26153 = (px
26154 * r20);
26155 rxp1_0
26156 = 0;
26157 rxp1_1
26158 = (px
26159 * r21);
26160 rxp2_0
26161 = 0;
26162 rxp2_1
26163 = (px
26164 * r22);
26165 py = 0;
26166 j4 = 1.5707963267949;
26167 sj4 = 1.0;
26168 cj4 = 0;
26169 rxp0_2
26170 = ((-1.0)
26171 * r10
26172 * x1861);
26173 rxp1_2
26174 = ((-1.0)
26175 * r11
26176 * x1861);
26177 rxp2_2
26178 = ((-1.0)
26179 * r12
26180 * x1861);
26181 j6eval
26182 [0]
26183 = ((1.0)
26184 + (((1.91568587540858)
26185 * (px
26186 * px
26187 * px
26188 * px)))
26189 + (((-1.0)
26190 * (2.64633970947792)
26191 * (px
26192 * px))));
26193 if (IKabs(
26194 j6eval
26195 [0])
26196 < 0.0000010000000000)
26197 {
26198 continue; // no branches [j6]
26199 }
26200 else
26201 {
26202 {
26203 IkReal j6array
26204 [2],
26205 cj6array
26206 [2],
26207 sj6array
26208 [2];
26209 bool j6valid
26210 [2]
26211 = {false};
26212 _nj6
26213 = 2;
26214 IkReal
26215 x1862
26216 = px
26217 * px;
26218 CheckValue<IkReal> x1864 = IKatan2WithCheck(
26219 IkReal((
26220 (-0.425)
26221 + (((-0.588235294117647)
26222 * x1862)))),
26223 ((-2.83333333333333)
26224 + (((3.92156862745098)
26225 * x1862))),
26226 IKFAST_ATAN2_MAGTHRESH);
26227 if (!x1864
26228 .valid)
26229 {
26230 continue;
26231 }
26232 IkReal
26233 x1863
26234 = ((-1.0)
26235 * (x1864
26236 .value));
26237 j6array
26238 [0]
26239 = x1863;
26240 sj6array
26241 [0]
26242 = IKsin(
26243 j6array
26244 [0]);
26245 cj6array
26246 [0]
26247 = IKcos(
26248 j6array
26249 [0]);
26250 j6array
26251 [1]
26252 = ((3.14159265358979)
26253 + x1863);
26254 sj6array
26255 [1]
26256 = IKsin(
26257 j6array
26258 [1]);
26259 cj6array
26260 [1]
26261 = IKcos(
26262 j6array
26263 [1]);
26264 if (j6array
26265 [0]
26266 > IKPI)
26267 {
26268 j6array
26269 [0]
26270 -= IK2PI;
26271 }
26272 else if (
26273 j6array
26274 [0]
26275 < -IKPI)
26276 {
26277 j6array
26278 [0]
26279 += IK2PI;
26280 }
26281 j6valid
26282 [0]
26283 = true;
26284 if (j6array
26285 [1]
26286 > IKPI)
26287 {
26288 j6array
26289 [1]
26290 -= IK2PI;
26291 }
26292 else if (
26293 j6array
26294 [1]
26295 < -IKPI)
26296 {
26297 j6array
26298 [1]
26299 += IK2PI;
26300 }
26301 j6valid
26302 [1]
26303 = true;
26304 for (
26305 int ij6
26306 = 0;
26307 ij6
26308 < 2;
26309 ++ij6)
26310 {
26311 if (!j6valid
26312 [ij6])
26313 {
26314 continue;
26315 }
26316 _ij6[0]
26317 = ij6;
26318 _ij6[1]
26319 = -1;
26320 for (
26321 int iij6
26322 = ij6
26323 + 1;
26324 iij6
26325 < 2;
26326 ++iij6)
26327 {
26328 if (j6valid
26329 [iij6]
26330 && IKabs(
26331 cj6array
26332 [ij6]
26333 - cj6array
26334 [iij6])
26335 < IKFAST_SOLUTION_THRESH
26336 && IKabs(
26337 sj6array
26338 [ij6]
26339 - sj6array
26340 [iij6])
26341 < IKFAST_SOLUTION_THRESH)
26342 {
26343 j6valid
26344 [iij6]
26345 = false;
26346 _ij6[1]
26347 = iij6;
26348 break;
26349 }
26350 }
26351 j6 = j6array
26352 [ij6];
26353 cj6 = cj6array
26354 [ij6];
26355 sj6 = sj6array
26356 [ij6];
26357 {
26358 IkReal evalcond
26359 [1];
26360 evalcond
26361 [0]
26362 = ((0.85)
26363 * (IKsin(
26364 j6)));
26365 if (IKabs(
26366 evalcond
26367 [0])
26368 > IKFAST_EVALCOND_THRESH)
26369 {
26370 continue;
26371 }
26372 }
26373
26374 rotationfunction0(
26375 solutions);
26376 }
26377 }
26378 }
26379 }
26380 }
26381 } while (
26382 0);
26383 if (bgotonextstatement)
26384 {
26385 bool
26386 bgotonextstatement
26387 = true;
26388 do
26389 {
26390 evalcond
26391 [0]
26392 = ((IKabs(
26393 py))
26394 + (IKabs((
26395 (-3.14159265358979)
26396 + (IKfmod(
26397 ((4.71238898038469)
26398 + j4),
26399 6.28318530717959))))));
26400 evalcond
26401 [1]
26402 = -0.85;
26403 evalcond
26404 [2]
26405 = 0;
26406 evalcond
26407 [3]
26408 = ((-0.2125)
26409 + (((-1.0)
26410 * (1.0)
26411 * (px
26412 * px))));
26413 if (IKabs(
26414 evalcond
26415 [0])
26416 < 0.0000010000000000
26417 && IKabs(
26418 evalcond
26419 [1])
26420 < 0.0000010000000000
26421 && IKabs(
26422 evalcond
26423 [2])
26424 < 0.0000010000000000
26425 && IKabs(
26426 evalcond
26427 [3])
26428 < 0.0000010000000000)
26429 {
26430 bgotonextstatement
26431 = false;
26432 {
26433 IkReal j6eval
26434 [1];
26435 IkReal
26436 x1865
26437 = ((1.0)
26438 * px);
26439 sj8 = 0;
26440 cj8 = 1.0;
26441 j8 = 0;
26442 pz = 0;
26443 j9 = 0;
26444 sj9 = 0;
26445 cj9 = 1.0;
26446 pp = px
26447 * px;
26448 npx
26449 = (px
26450 * r00);
26451 npy
26452 = (px
26453 * r01);
26454 npz
26455 = (px
26456 * r02);
26457 rxp0_0
26458 = 0;
26459 rxp0_1
26460 = (px
26461 * r20);
26462 rxp1_0
26463 = 0;
26464 rxp1_1
26465 = (px
26466 * r21);
26467 rxp2_0
26468 = 0;
26469 rxp2_1
26470 = (px
26471 * r22);
26472 py = 0;
26473 j4 = -1.5707963267949;
26474 sj4 = -1.0;
26475 cj4 = 0;
26476 rxp0_2
26477 = ((-1.0)
26478 * r10
26479 * x1865);
26480 rxp1_2
26481 = ((-1.0)
26482 * r11
26483 * x1865);
26484 rxp2_2
26485 = ((-1.0)
26486 * r12
26487 * x1865);
26488 j6eval
26489 [0]
26490 = ((1.0)
26491 + (((1.91568587540858)
26492 * (px
26493 * px
26494 * px
26495 * px)))
26496 + (((-1.0)
26497 * (2.64633970947792)
26498 * (px
26499 * px))));
26500 if (IKabs(
26501 j6eval
26502 [0])
26503 < 0.0000010000000000)
26504 {
26505 continue; // no branches [j6]
26506 }
26507 else
26508 {
26509 {
26510 IkReal j6array
26511 [2],
26512 cj6array
26513 [2],
26514 sj6array
26515 [2];
26516 bool j6valid
26517 [2]
26518 = {false};
26519 _nj6
26520 = 2;
26521 IkReal
26522 x1866
26523 = px
26524 * px;
26525 CheckValue<IkReal> x1868 = IKatan2WithCheck(
26526 IkReal((
26527 (-0.425)
26528 + (((-0.588235294117647)
26529 * x1866)))),
26530 ((-2.83333333333333)
26531 + (((3.92156862745098)
26532 * x1866))),
26533 IKFAST_ATAN2_MAGTHRESH);
26534 if (!x1868
26535 .valid)
26536 {
26537 continue;
26538 }
26539 IkReal
26540 x1867
26541 = ((-1.0)
26542 * (x1868
26543 .value));
26544 j6array
26545 [0]
26546 = x1867;
26547 sj6array
26548 [0]
26549 = IKsin(
26550 j6array
26551 [0]);
26552 cj6array
26553 [0]
26554 = IKcos(
26555 j6array
26556 [0]);
26557 j6array
26558 [1]
26559 = ((3.14159265358979)
26560 + x1867);
26561 sj6array
26562 [1]
26563 = IKsin(
26564 j6array
26565 [1]);
26566 cj6array
26567 [1]
26568 = IKcos(
26569 j6array
26570 [1]);
26571 if (j6array
26572 [0]
26573 > IKPI)
26574 {
26575 j6array
26576 [0]
26577 -= IK2PI;
26578 }
26579 else if (
26580 j6array
26581 [0]
26582 < -IKPI)
26583 {
26584 j6array
26585 [0]
26586 += IK2PI;
26587 }
26588 j6valid
26589 [0]
26590 = true;
26591 if (j6array
26592 [1]
26593 > IKPI)
26594 {
26595 j6array
26596 [1]
26597 -= IK2PI;
26598 }
26599 else if (
26600 j6array
26601 [1]
26602 < -IKPI)
26603 {
26604 j6array
26605 [1]
26606 += IK2PI;
26607 }
26608 j6valid
26609 [1]
26610 = true;
26611 for (
26612 int ij6
26613 = 0;
26614 ij6
26615 < 2;
26616 ++ij6)
26617 {
26618 if (!j6valid
26619 [ij6])
26620 {
26621 continue;
26622 }
26623 _ij6[0]
26624 = ij6;
26625 _ij6[1]
26626 = -1;
26627 for (
26628 int iij6
26629 = ij6
26630 + 1;
26631 iij6
26632 < 2;
26633 ++iij6)
26634 {
26635 if (j6valid
26636 [iij6]
26637 && IKabs(
26638 cj6array
26639 [ij6]
26640 - cj6array
26641 [iij6])
26642 < IKFAST_SOLUTION_THRESH
26643 && IKabs(
26644 sj6array
26645 [ij6]
26646 - sj6array
26647 [iij6])
26648 < IKFAST_SOLUTION_THRESH)
26649 {
26650 j6valid
26651 [iij6]
26652 = false;
26653 _ij6[1]
26654 = iij6;
26655 break;
26656 }
26657 }
26658 j6 = j6array
26659 [ij6];
26660 cj6 = cj6array
26661 [ij6];
26662 sj6 = sj6array
26663 [ij6];
26664 {
26665 IkReal evalcond
26666 [1];
26667 evalcond
26668 [0]
26669 = ((0.85)
26670 * (IKsin(
26671 j6)));
26672 if (IKabs(
26673 evalcond
26674 [0])
26675 > IKFAST_EVALCOND_THRESH)
26676 {
26677 continue;
26678 }
26679 }
26680
26681 rotationfunction0(
26682 solutions);
26683 }
26684 }
26685 }
26686 }
26687 }
26688 } while (
26689 0);
26690 if (bgotonextstatement)
26691 {
26692 bool
26693 bgotonextstatement
26694 = true;
26695 do
26696 {
26697 if (1)
26698 {
26699 bgotonextstatement
26700 = false;
26701 continue; // branch miss [j6]
26702 }
26703 } while (
26704 0);
26705 if (bgotonextstatement)
26706 {
26707 }
26708 }
26709 }
26710 }
26711 }
26712 }
26713 }
26714 }
26715 else
26716 {
26717 {
26718 IkReal
26719 j6array
26720 [1],
26721 cj6array
26722 [1],
26723 sj6array
26724 [1];
26725 bool j6valid
26726 [1]
26727 = {false};
26728 _nj6 = 1;
26729 IkReal x1869
26730 = (cj4
26731 * px);
26732 IkReal x1870
26733 = (py
26734 * sj4);
26735 IkReal x1871
26736 = px
26737 * px;
26738 IkReal x1872
26739 = py
26740 * py;
26741 CheckValue<
26742 IkReal>
26743 x1873
26744 = IKPowWithIntegerCheck(
26745 ((((20.0)
26746 * x1870))
26747 + (((20.0)
26748 * x1869))),
26749 -1);
26750 if (!x1873
26751 .valid)
26752 {
26753 continue;
26754 }
26755 CheckValue<IkReal> x1874 = IKPowWithIntegerCheck(
26756 ((((-1.0)
26757 * (11.7647058823529)
26758 * cj4
26759 * (px
26760 * px
26761 * px)))
26762 + (((-11.7647058823529)
26763 * x1869
26764 * x1872))
26765 + (((-8.5)
26766 * x1869))
26767 + (((-8.5)
26768 * x1870))
26769 + (((-11.7647058823529)
26770 * x1870
26771 * x1871))
26772 + (((-1.0)
26773 * (11.7647058823529)
26774 * sj4
26775 * (py
26776 * py
26777 * py)))),
26778 -1);
26779 if (!x1874
26780 .valid)
26781 {
26782 continue;
26783 }
26784 if (IKabs((
26785 (17.0)
26786 * (x1873
26787 .value)))
26788 < IKFAST_ATAN2_MAGTHRESH
26789 && IKabs((
26790 (x1874
26791 .value)
26792 * (((48.1666666666667)
26793 + (((-66.6666666666667)
26794 * x1872))
26795 + (((-66.6666666666667)
26796 * x1871))))))
26797 < IKFAST_ATAN2_MAGTHRESH
26798 && IKabs(
26799 IKsqr((
26800 (17.0)
26801 * (x1873
26802 .value)))
26803 + IKsqr((
26804 (x1874
26805 .value)
26806 * (((48.1666666666667)
26807 + (((-66.6666666666667)
26808 * x1872))
26809 + (((-66.6666666666667)
26810 * x1871))))))
26811 - 1)
26812 <= IKFAST_SINCOS_THRESH)
26813 continue;
26814 j6array[0] = IKatan2(
26815 ((17.0)
26816 * (x1873
26817 .value)),
26818 ((x1874
26819 .value)
26820 * (((48.1666666666667)
26821 + (((-66.6666666666667)
26822 * x1872))
26823 + (((-66.6666666666667)
26824 * x1871))))));
26825 sj6array[0] = IKsin(
26826 j6array
26827 [0]);
26828 cj6array[0] = IKcos(
26829 j6array
26830 [0]);
26831 if (j6array
26832 [0]
26833 > IKPI)
26834 {
26835 j6array[0]
26836 -= IK2PI;
26837 }
26838 else if (
26839 j6array
26840 [0]
26841 < -IKPI)
26842 {
26843 j6array[0]
26844 += IK2PI;
26845 }
26846 j6valid[0]
26847 = true;
26848 for (int ij6
26849 = 0;
26850 ij6
26851 < 1;
26852 ++ij6)
26853 {
26854 if (!j6valid
26855 [ij6])
26856 {
26857 continue;
26858 }
26859 _ij6[0]
26860 = ij6;
26861 _ij6[1]
26862 = -1;
26863 for (
26864 int iij6
26865 = ij6
26866 + 1;
26867 iij6
26868 < 1;
26869 ++iij6)
26870 {
26871 if (j6valid
26872 [iij6]
26873 && IKabs(
26874 cj6array
26875 [ij6]
26876 - cj6array
26877 [iij6])
26878 < IKFAST_SOLUTION_THRESH
26879 && IKabs(
26880 sj6array
26881 [ij6]
26882 - sj6array
26883 [iij6])
26884 < IKFAST_SOLUTION_THRESH)
26885 {
26886 j6valid
26887 [iij6]
26888 = false;
26889 _ij6[1]
26890 = iij6;
26891 break;
26892 }
26893 }
26894 j6 = j6array
26895 [ij6];
26896 cj6 = cj6array
26897 [ij6];
26898 sj6 = sj6array
26899 [ij6];
26900 {
26901 IkReal evalcond
26902 [5];
26903 IkReal
26904 x1875
26905 = IKsin(
26906 j6);
26907 IkReal
26908 x1876
26909 = (cj4
26910 * px);
26911 IkReal
26912 x1877
26913 = (x1875
26914 * x1876);
26915 IkReal
26916 x1878
26917 = (py
26918 * sj4);
26919 IkReal
26920 x1879
26921 = (x1875
26922 * x1878);
26923 IkReal
26924 x1880
26925 = ((1.0)
26926 * x1876);
26927 IkReal
26928 x1881
26929 = ((1.0)
26930 * x1878);
26931 IkReal
26932 x1882
26933 = IKcos(
26934 j6);
26935 IkReal
26936 x1883
26937 = px
26938 * px;
26939 IkReal
26940 x1884
26941 = ((3.92156862745098)
26942 * x1875);
26943 IkReal
26944 x1885
26945 = ((0.588235294117647)
26946 * x1882);
26947 IkReal
26948 x1886
26949 = py
26950 * py;
26951 IkReal
26952 x1887
26953 = ((0.09)
26954 * x1882);
26955 evalcond
26956 [0]
26957 = ((-0.85)
26958 + x1879
26959 + x1877);
26960 evalcond
26961 [1]
26962 = ((((-1.0)
26963 * x1880))
26964 + (((0.85)
26965 * x1875))
26966 + (((-1.0)
26967 * x1881)));
26968 evalcond
26969 [2]
26970 = ((((-1.0)
26971 * x1881
26972 * x1882))
26973 + (((-1.0)
26974 * x1880
26975 * x1882)));
26976 evalcond
26977 [3]
26978 = ((((-0.425)
26979 * x1882))
26980 + (((-1.0)
26981 * x1885
26982 * x1886))
26983 + ((x1884
26984 * x1886))
26985 + (((-1.0)
26986 * x1883
26987 * x1885))
26988 + ((x1883
26989 * x1884))
26990 + (((-2.83333333333333)
26991 * x1875)));
26992 evalcond
26993 [4]
26994 = ((-0.2125)
26995 + (((1.1)
26996 * x1879))
26997 + (((-1.0)
26998 * x1883))
26999 + (((-1.0)
27000 * x1886))
27001 + (((1.1)
27002 * x1877))
27003 + ((x1876
27004 * x1887))
27005 + ((x1878
27006 * x1887)));
27007 if (IKabs(
27008 evalcond
27009 [0])
27010 > IKFAST_EVALCOND_THRESH
27011 || IKabs(
27012 evalcond
27013 [1])
27014 > IKFAST_EVALCOND_THRESH
27015 || IKabs(
27016 evalcond
27017 [2])
27018 > IKFAST_EVALCOND_THRESH
27019 || IKabs(
27020 evalcond
27021 [3])
27022 > IKFAST_EVALCOND_THRESH
27023 || IKabs(
27024 evalcond
27025 [4])
27026 > IKFAST_EVALCOND_THRESH)
27027 {
27028 continue;
27029 }
27030 }
27031
27032 rotationfunction0(
27033 solutions);
27034 }
27035 }
27036 }
27037 }
27038 }
27039 else
27040 {
27041 {
27042 IkReal
27043 j6array[1],
27044 cj6array[1],
27045 sj6array[1];
27046 bool j6valid[1]
27047 = {false};
27048 _nj6 = 1;
27049 IkReal x1888
27050 = (cj4
27051 * px);
27052 IkReal x1889
27053 = (py
27054 * sj4);
27055 IkReal x1890
27056 = px * px;
27057 IkReal x1891
27058 = py * py;
27059 CheckValue<
27060 IkReal>
27061 x1892
27062 = IKPowWithIntegerCheck(
27063 ((-7.225)
27064 + (((-10.0)
27065 * x1891))
27066 + (((-10.0)
27067 * x1890))),
27068 -1);
27069 if (!x1892
27070 .valid)
27071 {
27072 continue;
27073 }
27074 if (IKabs((
27075 (((1.17647058823529)
27076 * x1888))
27077 + (((1.17647058823529)
27078 * x1889))))
27079 < IKFAST_ATAN2_MAGTHRESH
27080 && IKabs((
27081 (x1892
27082 .value)
27083 * (((((56.6666666666667)
27084 * x1889))
27085 + (((56.6666666666667)
27086 * x1888))
27087 + (((-1.0)
27088 * (78.4313725490196)
27089 * sj4
27090 * (py
27091 * py
27092 * py)))
27093 + (((-78.4313725490196)
27094 * x1888
27095 * x1891))
27096 + (((-1.0)
27097 * (78.4313725490196)
27098 * cj4
27099 * (px
27100 * px
27101 * px)))
27102 + (((-78.4313725490196)
27103 * x1889
27104 * x1890))))))
27105 < IKFAST_ATAN2_MAGTHRESH
27106 && IKabs(
27107 IKsqr((
27108 (((1.17647058823529)
27109 * x1888))
27110 + (((1.17647058823529)
27111 * x1889))))
27112 + IKsqr((
27113 (x1892
27114 .value)
27115 * (((((56.6666666666667)
27116 * x1889))
27117 + (((56.6666666666667)
27118 * x1888))
27119 + (((-1.0)
27120 * (78.4313725490196)
27121 * sj4
27122 * (py
27123 * py
27124 * py)))
27125 + (((-78.4313725490196)
27126 * x1888
27127 * x1891))
27128 + (((-1.0)
27129 * (78.4313725490196)
27130 * cj4
27131 * (px
27132 * px
27133 * px)))
27134 + (((-78.4313725490196)
27135 * x1889
27136 * x1890))))))
27137 - 1)
27138 <= IKFAST_SINCOS_THRESH)
27139 continue;
27140 j6array[0] = IKatan2(
27141 ((((1.17647058823529)
27142 * x1888))
27143 + (((1.17647058823529)
27144 * x1889))),
27145 ((x1892
27146 .value)
27147 * (((((56.6666666666667)
27148 * x1889))
27149 + (((56.6666666666667)
27150 * x1888))
27151 + (((-1.0)
27152 * (78.4313725490196)
27153 * sj4
27154 * (py
27155 * py
27156 * py)))
27157 + (((-78.4313725490196)
27158 * x1888
27159 * x1891))
27160 + (((-1.0)
27161 * (78.4313725490196)
27162 * cj4
27163 * (px
27164 * px
27165 * px)))
27166 + (((-78.4313725490196)
27167 * x1889
27168 * x1890))))));
27169 sj6array[0]
27170 = IKsin(
27171 j6array
27172 [0]);
27173 cj6array[0]
27174 = IKcos(
27175 j6array
27176 [0]);
27177 if (j6array[0]
27178 > IKPI)
27179 {
27180 j6array[0]
27181 -= IK2PI;
27182 }
27183 else if (
27184 j6array[0]
27185 < -IKPI)
27186 {
27187 j6array[0]
27188 += IK2PI;
27189 }
27190 j6valid[0]
27191 = true;
27192 for (int ij6
27193 = 0;
27194 ij6 < 1;
27195 ++ij6)
27196 {
27197 if (!j6valid
27198 [ij6])
27199 {
27200 continue;
27201 }
27202 _ij6[0] = ij6;
27203 _ij6[1] = -1;
27204 for (int iij6
27205 = ij6
27206 + 1;
27207 iij6 < 1;
27208 ++iij6)
27209 {
27210 if (j6valid
27211 [iij6]
27212 && IKabs(
27213 cj6array
27214 [ij6]
27215 - cj6array
27216 [iij6])
27217 < IKFAST_SOLUTION_THRESH
27218 && IKabs(
27219 sj6array
27220 [ij6]
27221 - sj6array
27222 [iij6])
27223 < IKFAST_SOLUTION_THRESH)
27224 {
27225 j6valid
27226 [iij6]
27227 = false;
27228 _ij6[1]
27229 = iij6;
27230 break;
27231 }
27232 }
27233 j6 = j6array
27234 [ij6];
27235 cj6 = cj6array
27236 [ij6];
27237 sj6 = sj6array
27238 [ij6];
27239 {
27240 IkReal
27241 evalcond
27242 [5];
27243 IkReal x1893
27244 = IKsin(
27245 j6);
27246 IkReal x1894
27247 = (cj4
27248 * px);
27249 IkReal x1895
27250 = (x1893
27251 * x1894);
27252 IkReal x1896
27253 = (py
27254 * sj4);
27255 IkReal x1897
27256 = (x1893
27257 * x1896);
27258 IkReal x1898
27259 = ((1.0)
27260 * x1894);
27261 IkReal x1899
27262 = ((1.0)
27263 * x1896);
27264 IkReal x1900
27265 = IKcos(
27266 j6);
27267 IkReal x1901
27268 = px
27269 * px;
27270 IkReal x1902
27271 = ((3.92156862745098)
27272 * x1893);
27273 IkReal x1903
27274 = ((0.588235294117647)
27275 * x1900);
27276 IkReal x1904
27277 = py
27278 * py;
27279 IkReal x1905
27280 = ((0.09)
27281 * x1900);
27282 evalcond[0]
27283 = ((-0.85)
27284 + x1897
27285 + x1895);
27286 evalcond[1]
27287 = ((((-1.0)
27288 * x1898))
27289 + (((0.85)
27290 * x1893))
27291 + (((-1.0)
27292 * x1899)));
27293 evalcond[2]
27294 = ((((-1.0)
27295 * x1898
27296 * x1900))
27297 + (((-1.0)
27298 * x1899
27299 * x1900)));
27300 evalcond[3]
27301 = ((((-1.0)
27302 * x1901
27303 * x1903))
27304 + (((-1.0)
27305 * x1903
27306 * x1904))
27307 + ((x1902
27308 * x1904))
27309 + ((x1901
27310 * x1902))
27311 + (((-0.425)
27312 * x1900))
27313 + (((-2.83333333333333)
27314 * x1893)));
27315 evalcond[4]
27316 = ((-0.2125)
27317 + (((-1.0)
27318 * x1901))
27319 + (((1.1)
27320 * x1895))
27321 + (((1.1)
27322 * x1897))
27323 + (((-1.0)
27324 * x1904))
27325 + ((x1894
27326 * x1905))
27327 + ((x1896
27328 * x1905)));
27329 if (IKabs(
27330 evalcond
27331 [0])
27332 > IKFAST_EVALCOND_THRESH
27333 || IKabs(
27334 evalcond
27335 [1])
27336 > IKFAST_EVALCOND_THRESH
27337 || IKabs(
27338 evalcond
27339 [2])
27340 > IKFAST_EVALCOND_THRESH
27341 || IKabs(
27342 evalcond
27343 [3])
27344 > IKFAST_EVALCOND_THRESH
27345 || IKabs(
27346 evalcond
27347 [4])
27348 > IKFAST_EVALCOND_THRESH)
27349 {
27350 continue;
27351 }
27352 }
27353
27354 rotationfunction0(
27355 solutions);
27356 }
27357 }
27358 }
27359 }
27360 }
27361 else
27362 {
27363 {
27364 IkReal j6array[1],
27365 cj6array[1],
27366 sj6array[1];
27367 bool j6valid[1]
27368 = {false};
27369 _nj6 = 1;
27370 IkReal x1906
27371 = (cj4 * px);
27372 IkReal x1907
27373 = (py * sj4);
27374 IkReal x1908
27375 = px * px;
27376 IkReal x1909
27377 = py * py;
27378 IkReal x1910
27379 = ((1.29411764705882)
27380 * (cj4
27381 * cj4));
27382 CheckValue<IkReal>
27383 x1911
27384 = IKPowWithIntegerCheck(
27385 ((((0.09)
27386 * x1907))
27387 + (((0.09)
27388 * x1906))),
27389 -1);
27390 if (!x1911.valid)
27391 {
27392 continue;
27393 }
27394 if (IKabs((
27395 (((1.17647058823529)
27396 * x1907))
27397 + (((1.17647058823529)
27398 * x1906))))
27399 < IKFAST_ATAN2_MAGTHRESH
27400 && IKabs((
27401 (x1911
27402 .value)
27403 * (((0.2125)
27404 + (((-1.0)
27405 * x1908
27406 * x1910))
27407 + ((x1909
27408 * x1910))
27409 + x1908
27410 + (((-0.294117647058824)
27411 * x1909))
27412 + (((-2.58823529411765)
27413 * cj4
27414 * px
27415 * x1907))))))
27416 < IKFAST_ATAN2_MAGTHRESH
27417 && IKabs(
27418 IKsqr((
27419 (((1.17647058823529)
27420 * x1907))
27421 + (((1.17647058823529)
27422 * x1906))))
27423 + IKsqr((
27424 (x1911
27425 .value)
27426 * (((0.2125)
27427 + (((-1.0)
27428 * x1908
27429 * x1910))
27430 + ((x1909
27431 * x1910))
27432 + x1908
27433 + (((-0.294117647058824)
27434 * x1909))
27435 + (((-2.58823529411765)
27436 * cj4
27437 * px
27438 * x1907))))))
27439 - 1)
27440 <= IKFAST_SINCOS_THRESH)
27441 continue;
27442 j6array[0] = IKatan2(
27443 ((((1.17647058823529)
27444 * x1907))
27445 + (((1.17647058823529)
27446 * x1906))),
27447 ((x1911.value)
27448 * (((0.2125)
27449 + (((-1.0)
27450 * x1908
27451 * x1910))
27452 + ((x1909
27453 * x1910))
27454 + x1908
27455 + (((-0.294117647058824)
27456 * x1909))
27457 + (((-2.58823529411765)
27458 * cj4
27459 * px
27460 * x1907))))));
27461 sj6array[0] = IKsin(
27462 j6array[0]);
27463 cj6array[0] = IKcos(
27464 j6array[0]);
27465 if (j6array[0]
27466 > IKPI)
27467 {
27468 j6array[0]
27469 -= IK2PI;
27470 }
27471 else if (
27472 j6array[0]
27473 < -IKPI)
27474 {
27475 j6array[0]
27476 += IK2PI;
27477 }
27478 j6valid[0] = true;
27479 for (int ij6 = 0;
27480 ij6 < 1;
27481 ++ij6)
27482 {
27483 if (!j6valid[ij6])
27484 {
27485 continue;
27486 }
27487 _ij6[0] = ij6;
27488 _ij6[1] = -1;
27489 for (int iij6
27490 = ij6 + 1;
27491 iij6 < 1;
27492 ++iij6)
27493 {
27494 if (j6valid
27495 [iij6]
27496 && IKabs(
27497 cj6array
27498 [ij6]
27499 - cj6array
27500 [iij6])
27501 < IKFAST_SOLUTION_THRESH
27502 && IKabs(
27503 sj6array
27504 [ij6]
27505 - sj6array
27506 [iij6])
27507 < IKFAST_SOLUTION_THRESH)
27508 {
27509 j6valid[iij6]
27510 = false;
27511 _ij6[1]
27512 = iij6;
27513 break;
27514 }
27515 }
27516 j6 = j6array[ij6];
27517 cj6 = cj6array
27518 [ij6];
27519 sj6 = sj6array
27520 [ij6];
27521 {
27522 IkReal
27523 evalcond[5];
27524 IkReal x1912
27525 = IKsin(j6);
27526 IkReal x1913
27527 = (cj4
27528 * px);
27529 IkReal x1914
27530 = (x1912
27531 * x1913);
27532 IkReal x1915
27533 = (py
27534 * sj4);
27535 IkReal x1916
27536 = (x1912
27537 * x1915);
27538 IkReal x1917
27539 = ((1.0)
27540 * x1913);
27541 IkReal x1918
27542 = ((1.0)
27543 * x1915);
27544 IkReal x1919
27545 = IKcos(j6);
27546 IkReal x1920
27547 = px * px;
27548 IkReal x1921
27549 = ((3.92156862745098)
27550 * x1912);
27551 IkReal x1922
27552 = ((0.588235294117647)
27553 * x1919);
27554 IkReal x1923
27555 = py * py;
27556 IkReal x1924
27557 = ((0.09)
27558 * x1919);
27559 evalcond[0]
27560 = ((-0.85)
27561 + x1914
27562 + x1916);
27563 evalcond[1]
27564 = ((((0.85)
27565 * x1912))
27566 + (((-1.0)
27567 * x1918))
27568 + (((-1.0)
27569 * x1917)));
27570 evalcond[2]
27571 = ((((-1.0)
27572 * x1917
27573 * x1919))
27574 + (((-1.0)
27575 * x1918
27576 * x1919)));
27577 evalcond[3]
27578 = ((((-1.0)
27579 * x1920
27580 * x1922))
27581 + (((-1.0)
27582 * x1922
27583 * x1923))
27584 + ((x1921
27585 * x1923))
27586 + ((x1920
27587 * x1921))
27588 + (((-0.425)
27589 * x1919))
27590 + (((-2.83333333333333)
27591 * x1912)));
27592 evalcond[4]
27593 = ((-0.2125)
27594 + ((x1913
27595 * x1924))
27596 + (((-1.0)
27597 * x1923))
27598 + (((1.1)
27599 * x1916))
27600 + ((x1915
27601 * x1924))
27602 + (((1.1)
27603 * x1914))
27604 + (((-1.0)
27605 * x1920)));
27606 if (IKabs(
27607 evalcond
27608 [0])
27609 > IKFAST_EVALCOND_THRESH
27610 || IKabs(
27611 evalcond
27612 [1])
27613 > IKFAST_EVALCOND_THRESH
27614 || IKabs(
27615 evalcond
27616 [2])
27617 > IKFAST_EVALCOND_THRESH
27618 || IKabs(
27619 evalcond
27620 [3])
27621 > IKFAST_EVALCOND_THRESH
27622 || IKabs(
27623 evalcond
27624 [4])
27625 > IKFAST_EVALCOND_THRESH)
27626 {
27627 continue;
27628 }
27629 }
27630
27631 rotationfunction0(
27632 solutions);
27633 }
27634 }
27635 }
27636 }
27637 }
27638 } while (0);
27639 if (bgotonextstatement)
27640 {
27641 bool bgotonextstatement
27642 = true;
27643 do
27644 {
27645 if (1)
27646 {
27647 bgotonextstatement
27648 = false;
27649 continue; // branch miss
27650 // [j6]
27651 }
27652 } while (0);
27653 if (bgotonextstatement)
27654 {
27655 }
27656 }
27657 }
27658 }
27659 else
27660 {
27661 {
27662 IkReal j6array[1],
27663 cj6array[1], sj6array[1];
27664 bool j6valid[1] = {false};
27665 _nj6 = 1;
27666 IkReal x1925 = (cj4 * px);
27667 IkReal x1926 = (py * sj4);
27668 IkReal x1927
27669 = ((0.108264705882353)
27670 * cj9);
27671 IkReal x1928
27672 = ((0.588235294117647)
27673 * pp);
27674 IkReal x1929 = (cj9 * pp);
27675 IkReal x1930 = (cj9 * sj9);
27676 IkReal x1931 = (pp * sj9);
27677 IkReal x1932 = cj9 * cj9;
27678 IkReal x1933 = ((1.0) * pz);
27679 CheckValue<IkReal> x1934
27680 = IKPowWithIntegerCheck(
27681 IKsign((
27682 (((1.51009803921569)
27683 * pz))
27684 + (((1.32323529411765)
27685 * cj9 * pz))
27686 + (((-1.0)
27687 * (3.92156862745098)
27688 * pp * pz))
27689 + (((-1.0) * x1926
27690 * x1928))
27691 + (((-1.0) * x1926
27692 * x1927))
27693 + (((-1.0) * x1925
27694 * x1927))
27695 + (((-0.316735294117647)
27696 * x1925))
27697 + (((-0.316735294117647)
27698 * x1926))
27699 + (((-1.0) * x1925
27700 * x1928)))),
27701 -1);
27702 if (!x1934.valid)
27703 {
27704 continue;
27705 }
27706 CheckValue<IkReal> x1935 = IKatan2WithCheck(
27707 IkReal((
27708 (-0.174204411764706)
27709 + (((-0.00487191176470588)
27710 * x1930))
27711 + (((-0.0264705882352941)
27712 * x1931))
27713 + (pz * pz)
27714 + (((-0.176470588235294)
27715 * x1929))
27716 + (((-1.0)
27717 * (0.154566176470588)
27718 * cj9))
27719 + (((-0.0324794117647059)
27720 * x1932))
27721 + (((-1.0)
27722 * (0.323529411764706)
27723 * pp))
27724 + (((-1.0)
27725 * (0.0142530882352941)
27726 * sj9)))),
27727 ((0.830553921568627)
27728 + (((-1.17647058823529)
27729 * x1929))
27730 + (((-0.176470588235294)
27731 * x1931))
27732 + (((0.396970588235294)
27733 * x1932))
27734 + (((-1.0) * x1926
27735 * x1933))
27736 + (((1.18080882352941)
27737 * cj9))
27738 + (((-1.0)
27739 * (2.15686274509804)
27740 * pp))
27741 + (((-1.0) * x1925
27742 * x1933))
27743 + (((0.0679544117647059)
27744 * sj9))
27745 + (((0.0595455882352941)
27746 * x1930))),
27747 IKFAST_ATAN2_MAGTHRESH);
27748 if (!x1935.valid)
27749 {
27750 continue;
27751 }
27752 j6array[0]
27753 = ((-1.5707963267949)
27754 + (((1.5707963267949)
27755 * (x1934.value)))
27756 + (x1935.value));
27757 sj6array[0]
27758 = IKsin(j6array[0]);
27759 cj6array[0]
27760 = IKcos(j6array[0]);
27761 if (j6array[0] > IKPI)
27762 {
27763 j6array[0] -= IK2PI;
27764 }
27765 else if (j6array[0] < -IKPI)
27766 {
27767 j6array[0] += IK2PI;
27768 }
27769 j6valid[0] = true;
27770 for (int ij6 = 0; ij6 < 1;
27771 ++ij6)
27772 {
27773 if (!j6valid[ij6])
27774 {
27775 continue;
27776 }
27777 _ij6[0] = ij6;
27778 _ij6[1] = -1;
27779 for (int iij6 = ij6 + 1;
27780 iij6 < 1;
27781 ++iij6)
27782 {
27783 if (j6valid[iij6]
27784 && IKabs(
27785 cj6array[ij6]
27786 - cj6array
27787 [iij6])
27788 < IKFAST_SOLUTION_THRESH
27789 && IKabs(
27790 sj6array[ij6]
27791 - sj6array
27792 [iij6])
27793 < IKFAST_SOLUTION_THRESH)
27794 {
27795 j6valid[iij6] = false;
27796 _ij6[1] = iij6;
27797 break;
27798 }
27799 }
27800 j6 = j6array[ij6];
27801 cj6 = cj6array[ij6];
27802 sj6 = sj6array[ij6];
27803 {
27804 IkReal evalcond[5];
27805 IkReal x1936
27806 = ((0.3) * cj9);
27807 IkReal x1937
27808 = ((0.045) * sj9);
27809 IkReal x1938 = IKcos(j6);
27810 IkReal x1939
27811 = (pz * x1938);
27812 IkReal x1940 = IKsin(j6);
27813 IkReal x1941 = (cj4 * px);
27814 IkReal x1942
27815 = (x1940 * x1941);
27816 IkReal x1943 = (py * sj4);
27817 IkReal x1944
27818 = (x1940 * x1943);
27819 IkReal x1945
27820 = ((0.045) * cj9);
27821 IkReal x1946
27822 = ((0.3) * sj9);
27823 IkReal x1947
27824 = (pz * x1940);
27825 IkReal x1948
27826 = ((1.0) * x1941);
27827 IkReal x1949
27828 = ((1.0) * x1943);
27829 IkReal x1950
27830 = ((0.09) * x1938);
27831 evalcond[0]
27832 = ((-0.55) + x1944
27833 + x1942 + x1939
27834 + (((-1.0)
27835 * x1937))
27836 + (((-1.0)
27837 * x1936)));
27838 evalcond[1]
27839 = ((0.045) + x1946
27840 + x1947
27841 + (((-1.0)
27842 * x1945))
27843 + (((-1.0) * x1938
27844 * x1948))
27845 + (((-1.0) * x1938
27846 * x1949)));
27847 evalcond[2]
27848 = ((((-1.51009803921569)
27849 * x1940))
27850 + pz
27851 + (((3.92156862745098)
27852 * pp * x1940))
27853 + (((-0.588235294117647)
27854 * pp * x1938))
27855 + (((-0.108264705882353)
27856 * cj9 * x1938))
27857 + (((-1.32323529411765)
27858 * cj9 * x1940))
27859 + (((-0.316735294117647)
27860 * x1938)));
27861 evalcond[3]
27862 = ((((-1.0) * x1948))
27863 + ((x1936 * x1940))
27864 + (((0.55)
27865 * x1940))
27866 + (((-1.0) * x1938
27867 * x1945))
27868 + (((-1.0)
27869 * x1949))
27870 + (((0.045)
27871 * x1938))
27872 + ((x1937 * x1940))
27873 + ((x1938
27874 * x1946)));
27875 evalcond[4]
27876 = ((-0.2125)
27877 + (((1.1) * x1944))
27878 + ((x1943 * x1950))
27879 + ((x1941 * x1950))
27880 + (((1.1) * x1942))
27881 + (((1.1) * x1939))
27882 + (((-0.09)
27883 * x1947))
27884 + (((-1.0) * (1.0)
27885 * pp)));
27886 if (IKabs(evalcond[0])
27887 > IKFAST_EVALCOND_THRESH
27888 || IKabs(evalcond[1])
27889 > IKFAST_EVALCOND_THRESH
27890 || IKabs(evalcond[2])
27891 > IKFAST_EVALCOND_THRESH
27892 || IKabs(evalcond[3])
27893 > IKFAST_EVALCOND_THRESH
27894 || IKabs(evalcond[4])
27895 > IKFAST_EVALCOND_THRESH)
27896 {
27897 continue;
27898 }
27899 }
27900
27901 rotationfunction0(
27902 solutions);
27903 }
27904 }
27905 }
27906 }
27907 }
27908 else
27909 {
27910 {
27911 IkReal j6array[1], cj6array[1],
27912 sj6array[1];
27913 bool j6valid[1] = {false};
27914 _nj6 = 1;
27915 IkReal x1951
27916 = ((0.045) * cj4 * px);
27917 IkReal x1952
27918 = ((0.045) * py * sj4);
27919 IkReal x1953 = ((0.3) * sj9);
27920 IkReal x1954 = (cj4 * px);
27921 IkReal x1955 = (py * sj4);
27922 IkReal x1956 = (cj9 * sj9);
27923 IkReal x1957 = cj9 * cj9;
27924 IkReal x1958 = ((1.0) * pz);
27925 IkReal x1959 = py * py;
27926 IkReal x1960 = cj4 * cj4;
27927 CheckValue<IkReal> x1961
27928 = IKatan2WithCheck(
27929 IkReal((
27930 (0.03825)
27931 + (((-1.0) * x1955
27932 * x1958))
27933 + (((0.087975)
27934 * x1956))
27935 + (((-0.027) * x1957))
27936 + (((0.167025) * sj9))
27937 + (((-1.0) * (0.01125)
27938 * cj9))
27939 + (((-1.0) * x1954
27940 * x1958)))),
27941 ((-0.304525)
27942 + (((-1.0) * x1959
27943 * x1960))
27944 + ((x1960 * (px * px)))
27945 + (((-1.0) * (0.0495)
27946 * sj9))
27947 + (((-0.087975) * x1957))
27948 + (((-1.0) * (0.33)
27949 * cj9))
27950 + (((2.0) * cj4 * px
27951 * x1955))
27952 + (((-0.027) * x1956))
27953 + x1959),
27954 IKFAST_ATAN2_MAGTHRESH);
27955 if (!x1961.valid)
27956 {
27957 continue;
27958 }
27959 CheckValue<IkReal> x1962
27960 = IKPowWithIntegerCheck(
27961 IKsign(
27962 ((((-1.0) * (0.55)
27963 * pz))
27964 + ((x1953 * x1954))
27965 + (((-1.0) * cj9
27966 * x1952))
27967 + ((x1953 * x1955))
27968 + (((-1.0) * cj9
27969 * x1951))
27970 + x1951 + x1952
27971 + (((-1.0) * (0.045)
27972 * pz * sj9))
27973 + (((-1.0) * (0.3)
27974 * cj9 * pz)))),
27975 -1);
27976 if (!x1962.valid)
27977 {
27978 continue;
27979 }
27980 j6array[0]
27981 = ((-1.5707963267949)
27982 + (x1961.value)
27983 + (((1.5707963267949)
27984 * (x1962.value))));
27985 sj6array[0] = IKsin(j6array[0]);
27986 cj6array[0] = IKcos(j6array[0]);
27987 if (j6array[0] > IKPI)
27988 {
27989 j6array[0] -= IK2PI;
27990 }
27991 else if (j6array[0] < -IKPI)
27992 {
27993 j6array[0] += IK2PI;
27994 }
27995 j6valid[0] = true;
27996 for (int ij6 = 0; ij6 < 1; ++ij6)
27997 {
27998 if (!j6valid[ij6])
27999 {
28000 continue;
28001 }
28002 _ij6[0] = ij6;
28003 _ij6[1] = -1;
28004 for (int iij6 = ij6 + 1;
28005 iij6 < 1;
28006 ++iij6)
28007 {
28008 if (j6valid[iij6]
28009 && IKabs(
28010 cj6array[ij6]
28011 - cj6array[iij6])
28012 < IKFAST_SOLUTION_THRESH
28013 && IKabs(
28014 sj6array[ij6]
28015 - sj6array[iij6])
28016 < IKFAST_SOLUTION_THRESH)
28017 {
28018 j6valid[iij6] = false;
28019 _ij6[1] = iij6;
28020 break;
28021 }
28022 }
28023 j6 = j6array[ij6];
28024 cj6 = cj6array[ij6];
28025 sj6 = sj6array[ij6];
28026 {
28027 IkReal evalcond[5];
28028 IkReal x1963 = ((0.3) * cj9);
28029 IkReal x1964
28030 = ((0.045) * sj9);
28031 IkReal x1965 = IKcos(j6);
28032 IkReal x1966 = (pz * x1965);
28033 IkReal x1967 = IKsin(j6);
28034 IkReal x1968 = (cj4 * px);
28035 IkReal x1969
28036 = (x1967 * x1968);
28037 IkReal x1970 = (py * sj4);
28038 IkReal x1971
28039 = (x1967 * x1970);
28040 IkReal x1972
28041 = ((0.045) * cj9);
28042 IkReal x1973 = ((0.3) * sj9);
28043 IkReal x1974 = (pz * x1967);
28044 IkReal x1975
28045 = ((1.0) * x1968);
28046 IkReal x1976
28047 = ((1.0) * x1970);
28048 IkReal x1977
28049 = ((0.09) * x1965);
28050 evalcond[0]
28051 = ((-0.55) + x1966 + x1969
28052 + (((-1.0) * x1964))
28053 + x1971
28054 + (((-1.0) * x1963)));
28055 evalcond[1]
28056 = ((0.045)
28057 + (((-1.0) * x1965
28058 * x1976))
28059 + (((-1.0) * x1965
28060 * x1975))
28061 + (((-1.0) * x1972))
28062 + x1973 + x1974);
28063 evalcond[2]
28064 = ((((-0.316735294117647)
28065 * x1965))
28066 + (((-0.588235294117647)
28067 * pp * x1965))
28068 + pz
28069 + (((-1.51009803921569)
28070 * x1967))
28071 + (((-0.108264705882353)
28072 * cj9 * x1965))
28073 + (((3.92156862745098)
28074 * pp * x1967))
28075 + (((-1.32323529411765)
28076 * cj9 * x1967)));
28077 evalcond[3]
28078 = ((((0.045) * x1965))
28079 + ((x1964 * x1967))
28080 + ((x1963 * x1967))
28081 + (((-1.0) * x1976))
28082 + (((-1.0) * x1965
28083 * x1972))
28084 + (((-1.0) * x1975))
28085 + (((0.55) * x1967))
28086 + ((x1965 * x1973)));
28087 evalcond[4]
28088 = ((-0.2125)
28089 + (((1.1) * x1966))
28090 + (((1.1) * x1969))
28091 + (((1.1) * x1971))
28092 + (((-0.09) * x1974))
28093 + ((x1970 * x1977))
28094 + ((x1968 * x1977))
28095 + (((-1.0) * (1.0)
28096 * pp)));
28097 if (IKabs(evalcond[0])
28098 > IKFAST_EVALCOND_THRESH
28099 || IKabs(evalcond[1])
28100 > IKFAST_EVALCOND_THRESH
28101 || IKabs(evalcond[2])
28102 > IKFAST_EVALCOND_THRESH
28103 || IKabs(evalcond[3])
28104 > IKFAST_EVALCOND_THRESH
28105 || IKabs(evalcond[4])
28106 > IKFAST_EVALCOND_THRESH)
28107 {
28108 continue;
28109 }
28110 }
28111
28112 rotationfunction0(solutions);
28113 }
28114 }
28115 }
28116 }
28117 }
28118 else
28119 {
28120 {
28121 IkReal j6array[1], cj6array[1],
28122 sj6array[1];
28123 bool j6valid[1] = {false};
28124 _nj6 = 1;
28125 IkReal x1978 = (cj4 * px);
28126 IkReal x1979 = (py * sj4);
28127 IkReal x1980
28128 = ((1.32323529411765) * cj9);
28129 IkReal x1981
28130 = ((3.92156862745098) * pp);
28131 IkReal x1982
28132 = ((0.0264705882352941) * pp);
28133 IkReal x1983 = (cj9 * sj9);
28134 IkReal x1984
28135 = ((0.176470588235294) * pp);
28136 IkReal x1985 = cj9 * cj9;
28137 CheckValue<IkReal> x1986
28138 = IKatan2WithCheck(
28139 IkReal((
28140 (-0.0142530882352941)
28141 + ((pz * x1979))
28142 + (((0.00938117647058823)
28143 * cj9))
28144 + ((cj9 * x1982))
28145 + (((-0.0324794117647059)
28146 * x1983))
28147 + (((0.00487191176470588)
28148 * x1985))
28149 + (((-1.0) * x1982))
28150 + ((pz * x1978))
28151 + (((-1.0) * sj9 * x1984))
28152 + (((-1.0)
28153 * (0.0950205882352941)
28154 * sj9)))),
28155 ((0.0679544117647059)
28156 + (((0.396970588235294)
28157 * x1983))
28158 + (((-0.0595455882352941)
28159 * x1985))
28160 + (pz * pz) + ((cj9 * x1984))
28161 + (((-1.0)
28162 * (0.00840882352941177)
28163 * cj9))
28164 + (((0.453029411764706)
28165 * sj9))
28166 + (((-1.0) * x1984))
28167 + (((-1.0)
28168 * (1.17647058823529) * pp
28169 * sj9))),
28170 IKFAST_ATAN2_MAGTHRESH);
28171 if (!x1986.valid)
28172 {
28173 continue;
28174 }
28175 CheckValue<IkReal> x1987
28176 = IKPowWithIntegerCheck(
28177 IKsign((
28178 (((-1.0) * x1978 * x1981))
28179 + (((1.51009803921569)
28180 * x1978))
28181 + (((1.51009803921569)
28182 * x1979))
28183 + (((0.316735294117647)
28184 * pz))
28185 + (((0.108264705882353)
28186 * cj9 * pz))
28187 + (((0.588235294117647)
28188 * pp * pz))
28189 + ((x1978 * x1980))
28190 + (((-1.0) * x1979
28191 * x1981))
28192 + ((x1979 * x1980)))),
28193 -1);
28194 if (!x1987.valid)
28195 {
28196 continue;
28197 }
28198 j6array[0]
28199 = ((-1.5707963267949)
28200 + (x1986.value)
28201 + (((1.5707963267949)
28202 * (x1987.value))));
28203 sj6array[0] = IKsin(j6array[0]);
28204 cj6array[0] = IKcos(j6array[0]);
28205 if (j6array[0] > IKPI)
28206 {
28207 j6array[0] -= IK2PI;
28208 }
28209 else if (j6array[0] < -IKPI)
28210 {
28211 j6array[0] += IK2PI;
28212 }
28213 j6valid[0] = true;
28214 for (int ij6 = 0; ij6 < 1; ++ij6)
28215 {
28216 if (!j6valid[ij6])
28217 {
28218 continue;
28219 }
28220 _ij6[0] = ij6;
28221 _ij6[1] = -1;
28222 for (int iij6 = ij6 + 1; iij6 < 1;
28223 ++iij6)
28224 {
28225 if (j6valid[iij6]
28226 && IKabs(
28227 cj6array[ij6]
28228 - cj6array[iij6])
28229 < IKFAST_SOLUTION_THRESH
28230 && IKabs(
28231 sj6array[ij6]
28232 - sj6array[iij6])
28233 < IKFAST_SOLUTION_THRESH)
28234 {
28235 j6valid[iij6] = false;
28236 _ij6[1] = iij6;
28237 break;
28238 }
28239 }
28240 j6 = j6array[ij6];
28241 cj6 = cj6array[ij6];
28242 sj6 = sj6array[ij6];
28243 {
28244 IkReal evalcond[5];
28245 IkReal x1988 = ((0.3) * cj9);
28246 IkReal x1989 = ((0.045) * sj9);
28247 IkReal x1990 = IKcos(j6);
28248 IkReal x1991 = (pz * x1990);
28249 IkReal x1992 = IKsin(j6);
28250 IkReal x1993 = (cj4 * px);
28251 IkReal x1994 = (x1992 * x1993);
28252 IkReal x1995 = (py * sj4);
28253 IkReal x1996 = (x1992 * x1995);
28254 IkReal x1997 = ((0.045) * cj9);
28255 IkReal x1998 = ((0.3) * sj9);
28256 IkReal x1999 = (pz * x1992);
28257 IkReal x2000 = ((1.0) * x1993);
28258 IkReal x2001 = ((1.0) * x1995);
28259 IkReal x2002 = ((0.09) * x1990);
28260 evalcond[0]
28261 = ((-0.55)
28262 + (((-1.0) * x1989))
28263 + x1996 + x1994 + x1991
28264 + (((-1.0) * x1988)));
28265 evalcond[1]
28266 = ((0.045)
28267 + (((-1.0) * x1990
28268 * x2000))
28269 + (((-1.0) * x1997))
28270 + x1998 + x1999
28271 + (((-1.0) * x1990
28272 * x2001)));
28273 evalcond[2]
28274 = ((((-0.108264705882353)
28275 * cj9 * x1990))
28276 + (((-0.588235294117647)
28277 * pp * x1990))
28278 + pz
28279 + (((-1.51009803921569)
28280 * x1992))
28281 + (((-1.32323529411765)
28282 * cj9 * x1992))
28283 + (((3.92156862745098) * pp
28284 * x1992))
28285 + (((-0.316735294117647)
28286 * x1990)));
28287 evalcond[3]
28288 = ((((0.045) * x1990))
28289 + (((-1.0) * x2001))
28290 + (((0.55) * x1992))
28291 + (((-1.0) * x2000))
28292 + (((-1.0) * x1990
28293 * x1997))
28294 + ((x1990 * x1998))
28295 + ((x1989 * x1992))
28296 + ((x1988 * x1992)));
28297 evalcond[4]
28298 = ((-0.2125)
28299 + (((1.1) * x1996))
28300 + (((1.1) * x1991))
28301 + ((x1995 * x2002))
28302 + (((-0.09) * x1999))
28303 + ((x1993 * x2002))
28304 + (((-1.0) * (1.0) * pp))
28305 + (((1.1) * x1994)));
28306 if (IKabs(evalcond[0])
28307 > IKFAST_EVALCOND_THRESH
28308 || IKabs(evalcond[1])
28309 > IKFAST_EVALCOND_THRESH
28310 || IKabs(evalcond[2])
28311 > IKFAST_EVALCOND_THRESH
28312 || IKabs(evalcond[3])
28313 > IKFAST_EVALCOND_THRESH
28314 || IKabs(evalcond[4])
28315 > IKFAST_EVALCOND_THRESH)
28316 {
28317 continue;
28318 }
28319 }
28320
28321 rotationfunction0(solutions);
28322 }
28323 }
28324 }
28325 }
28326 }
28327 } while (0);
28328 if (bgotonextstatement)
28329 {
28330 bool bgotonextstatement = true;
28331 do
28332 {
28333 IkReal x2003 = (px * sj4);
28334 IkReal x2004 = (cj4 * py);
28335 evalcond[0]
28336 = ((-3.14159265358979)
28337 + (IKfmod(
28338 ((3.14159265358979)
28339 + (IKabs(
28340 ((-3.14159265358979)
28341 + j8)))),
28342 6.28318530717959)));
28343 evalcond[1]
28344 = ((0.39655) + (((0.0765) * sj9))
28345 + (((0.32595) * cj9))
28346 + (((-1.0) * (1.0) * pp)));
28347 evalcond[2] = (x2003 + (((-1.0) * x2004)));
28348 evalcond[3] = ((((-1.0) * x2003)) + x2004);
28349 if (IKabs(evalcond[0]) < 0.0000010000000000
28350 && IKabs(evalcond[1])
28351 < 0.0000010000000000
28352 && IKabs(evalcond[2])
28353 < 0.0000010000000000
28354 && IKabs(evalcond[3])
28355 < 0.0000010000000000)
28356 {
28357 bgotonextstatement = false;
28358 {
28359 IkReal j6eval[2];
28360 sj8 = 0;
28361 cj8 = -1.0;
28362 j8 = 3.14159265358979;
28363 IkReal x2005 = py * py;
28364 IkReal x2006 = cj4 * cj4;
28365 IkReal x2007
28366 = (x2005
28367 + (((-1.0) * x2005 * x2006))
28368 + (pz * pz)
28369 + (((2.0) * cj4 * px * py * sj4))
28370 + ((x2006 * (px * px))));
28371 j6eval[0] = x2007;
28372 j6eval[1] = IKsign(x2007);
28373 if (IKabs(j6eval[0])
28374 < 0.0000010000000000
28375 || IKabs(j6eval[1])
28376 < 0.0000010000000000)
28377 {
28378 {
28379 IkReal j6eval[2];
28380 sj8 = 0;
28381 cj8 = -1.0;
28382 j8 = 3.14159265358979;
28383 IkReal x2008 = (cj4 * px);
28384 IkReal x2009 = (cj9 * pz);
28385 IkReal x2010 = (py * sj4);
28386 IkReal x2011 = (pz * sj9);
28387 IkReal x2012 = (cj4 * px * sj9);
28388 IkReal x2013 = (py * sj4 * sj9);
28389 IkReal x2014 = ((0.045) * x2008);
28390 IkReal x2015 = ((0.045) * x2010);
28391 j6eval[0]
28392 = (((cj9 * x2010))
28393 + (((-1.0) * x2010))
28394 + ((cj9 * x2008))
28395 + (((-1.0)
28396 * (12.2222222222222)
28397 * pz))
28398 + (((-1.0) * x2011))
28399 + (((-6.66666666666667)
28400 * x2009))
28401 + (((-6.66666666666667)
28402 * x2013))
28403 + (((-1.0) * x2008))
28404 + (((-6.66666666666667)
28405 * x2012)));
28406 j6eval[1] = IKsign(
28407 (((cj9 * x2014))
28408 + (((-0.3) * x2012))
28409 + (((-0.3) * x2013))
28410 + (((-1.0) * (0.55) * pz))
28411 + (((-0.045) * x2011))
28412 + ((cj9 * x2015))
28413 + (((-1.0) * x2014))
28414 + (((-1.0) * x2015))
28415 + (((-0.3) * x2009))));
28416 if (IKabs(j6eval[0])
28417 < 0.0000010000000000
28418 || IKabs(j6eval[1])
28419 < 0.0000010000000000)
28420 {
28421 {
28422 IkReal j6eval[2];
28423 sj8 = 0;
28424 cj8 = -1.0;
28425 j8 = 3.14159265358979;
28426 IkReal x2016 = (cj4 * px);
28427 IkReal x2017 = (cj9 * pz);
28428 IkReal x2018 = (pp * pz);
28429 IkReal x2019 = (py * sj4);
28430 IkReal x2020 = (cj4 * cj9 * px);
28431 IkReal x2021 = (cj4 * pp * px);
28432 IkReal x2022 = (cj9 * py * sj4);
28433 IkReal x2023 = (pp * py * sj4);
28434 j6eval[0]
28435 = ((((-2.92556370551481)
28436 * x2016))
28437 + (((-1.0)
28438 * (13.9482024812098)
28439 * pz))
28440 + (((-5.4333061668025)
28441 * x2021))
28442 + (((-1.0) * x2022))
28443 + (((-12.2222222222222)
28444 * x2017))
28445 + (((-5.4333061668025)
28446 * x2023))
28447 + (((-2.92556370551481)
28448 * x2019))
28449 + (((36.2220411120167)
28450 * x2018))
28451 + (((-1.0) * x2020)));
28452 j6eval[1] = IKsign(
28453 ((((-0.588235294117647)
28454 * x2021))
28455 + (((-1.32323529411765)
28456 * x2017))
28457 + (((3.92156862745098)
28458 * x2018))
28459 + (((-0.316735294117647)
28460 * x2019))
28461 + (((-1.0)
28462 * (1.51009803921569)
28463 * pz))
28464 + (((-0.108264705882353)
28465 * x2020))
28466 + (((-0.108264705882353)
28467 * x2022))
28468 + (((-0.588235294117647)
28469 * x2023))
28470 + (((-0.316735294117647)
28471 * x2016))));
28472 if (IKabs(j6eval[0])
28473 < 0.0000010000000000
28474 || IKabs(j6eval[1])
28475 < 0.0000010000000000)
28476 {
28477 {
28478 IkReal evalcond[1];
28479 bool bgotonextstatement
28480 = true;
28481 do
28482 {
28483 evalcond[0]
28484 = ((IKabs((
28485 (-3.14159265358979)
28486 + (IKfmod(
28487 ((3.14159265358979)
28488 + j9),
28489 6.28318530717959)))))
28490 + (IKabs(pz)));
28491 if (IKabs(evalcond[0])
28492 < 0.0000010000000000)
28493 {
28494 bgotonextstatement
28495 = false;
28496 {
28497 IkReal j6eval[1];
28498 IkReal x2024
28499 = ((1.0) * py);
28500 sj8 = 0;
28501 cj8 = -1.0;
28502 j8 = 3.14159265358979;
28503 pz = 0;
28504 j9 = 0;
28505 sj9 = 0;
28506 cj9 = 1.0;
28507 pp
28508 = ((py * py)
28509 + (px * px));
28510 npx
28511 = (((py * r10))
28512 + ((px
28513 * r00)));
28514 npy
28515 = (((py * r11))
28516 + ((px
28517 * r01)));
28518 npz
28519 = (((px * r02))
28520 + ((py
28521 * r12)));
28522 rxp0_0
28523 = ((-1.0) * r20
28524 * x2024);
28525 rxp0_1 = (px * r20);
28526 rxp1_0
28527 = ((-1.0) * r21
28528 * x2024);
28529 rxp1_1 = (px * r21);
28530 rxp2_0
28531 = ((-1.0) * r22
28532 * x2024);
28533 rxp2_1 = (px * r22);
28534 j6eval[0]
28535 = ((((-1.0)
28536 * (1.0) * cj4
28537 * px))
28538 + (((-1.0)
28539 * (1.0)
28540 * py
28541 * sj4)));
28542 if (IKabs(j6eval[0])
28543 < 0.0000010000000000)
28544 {
28545 {
28546 IkReal j6eval[1];
28547 IkReal x2025
28548 = ((1.0)
28549 * py);
28550 sj8 = 0;
28551 cj8 = -1.0;
28552 j8 = 3.14159265358979;
28553 pz = 0;
28554 j9 = 0;
28555 sj9 = 0;
28556 cj9 = 1.0;
28557 pp
28558 = ((py * py)
28559 + (px
28560 * px));
28561 npx
28562 = (((py
28563 * r10))
28564 + ((px
28565 * r00)));
28566 npy
28567 = (((py
28568 * r11))
28569 + ((px
28570 * r01)));
28571 npz
28572 = (((px
28573 * r02))
28574 + ((py
28575 * r12)));
28576 rxp0_0
28577 = ((-1.0)
28578 * r20
28579 * x2025);
28580 rxp0_1
28581 = (px * r20);
28582 rxp1_0
28583 = ((-1.0)
28584 * r21
28585 * x2025);
28586 rxp1_1
28587 = (px * r21);
28588 rxp2_0
28589 = ((-1.0)
28590 * r22
28591 * x2025);
28592 rxp2_1
28593 = (px * r22);
28594 j6eval[0]
28595 = ((-1.0)
28596 + (((-1.0)
28597 * (1.3840830449827)
28598 * (px
28599 * px)))
28600 + (((-1.0)
28601 * (1.3840830449827)
28602 * (py
28603 * py))));
28604 if (IKabs(
28605 j6eval[0])
28606 < 0.0000010000000000)
28607 {
28608 {
28609 IkReal
28610 j6eval[2];
28611 IkReal x2026
28612 = ((1.0)
28613 * py);
28614 sj8 = 0;
28615 cj8 = -1.0;
28616 j8 = 3.14159265358979;
28617 pz = 0;
28618 j9 = 0;
28619 sj9 = 0;
28620 cj9 = 1.0;
28621 pp
28622 = ((py
28623 * py)
28624 + (px
28625 * px));
28626 npx
28627 = (((py
28628 * r10))
28629 + ((px
28630 * r00)));
28631 npy
28632 = (((py
28633 * r11))
28634 + ((px
28635 * r01)));
28636 npz
28637 = (((px
28638 * r02))
28639 + ((py
28640 * r12)));
28641 rxp0_0
28642 = ((-1.0)
28643 * r20
28644 * x2026);
28645 rxp0_1
28646 = (px
28647 * r20);
28648 rxp1_0
28649 = ((-1.0)
28650 * r21
28651 * x2026);
28652 rxp1_1
28653 = (px
28654 * r21);
28655 rxp2_0
28656 = ((-1.0)
28657 * r22
28658 * x2026);
28659 rxp2_1
28660 = (px
28661 * r22);
28662 IkReal x2027
28663 = (cj4
28664 * px);
28665 IkReal x2028
28666 = (py
28667 * sj4);
28668 j6eval[0]
28669 = (x2027
28670 + x2028);
28671 j6eval[1]
28672 = ((((-1.0)
28673 * (1.3840830449827)
28674 * cj4
28675 * (px
28676 * px
28677 * px)))
28678 + (((-1.0)
28679 * x2028))
28680 + (((-1.3840830449827)
28681 * x2027
28682 * (py
28683 * py)))
28684 + (((-1.0)
28685 * (1.3840830449827)
28686 * sj4
28687 * (py
28688 * py
28689 * py)))
28690 + (((-1.0)
28691 * x2027))
28692 + (((-1.3840830449827)
28693 * x2028
28694 * (px
28695 * px))));
28696 if (IKabs(
28697 j6eval
28698 [0])
28699 < 0.0000010000000000
28700 || IKabs(
28701 j6eval
28702 [1])
28703 < 0.0000010000000000)
28704 {
28705 {
28706 IkReal evalcond
28707 [4];
28708 bool
28709 bgotonextstatement
28710 = true;
28711 do
28712 {
28713 evalcond
28714 [0]
28715 = ((IKabs(
28716 py))
28717 + (IKabs(
28718 px)));
28719 evalcond
28720 [1]
28721 = -0.85;
28722 evalcond
28723 [2]
28724 = 0;
28725 evalcond
28726 [3]
28727 = -0.2125;
28728 if (IKabs(
28729 evalcond
28730 [0])
28731 < 0.0000010000000000
28732 && IKabs(
28733 evalcond
28734 [1])
28735 < 0.0000010000000000
28736 && IKabs(
28737 evalcond
28738 [2])
28739 < 0.0000010000000000
28740 && IKabs(
28741 evalcond
28742 [3])
28743 < 0.0000010000000000)
28744 {
28745 bgotonextstatement
28746 = false;
28747 {
28748 IkReal j6array
28749 [2],
28750 cj6array
28751 [2],
28752 sj6array
28753 [2];
28754 bool j6valid
28755 [2]
28756 = {false};
28757 _nj6
28758 = 2;
28759 j6array
28760 [0]
28761 = 0.148889947609497;
28762 sj6array
28763 [0]
28764 = IKsin(
28765 j6array
28766 [0]);
28767 cj6array
28768 [0]
28769 = IKcos(
28770 j6array
28771 [0]);
28772 j6array
28773 [1]
28774 = 3.29048260119929;
28775 sj6array
28776 [1]
28777 = IKsin(
28778 j6array
28779 [1]);
28780 cj6array
28781 [1]
28782 = IKcos(
28783 j6array
28784 [1]);
28785 if (j6array
28786 [0]
28787 > IKPI)
28788 {
28789 j6array
28790 [0]
28791 -= IK2PI;
28792 }
28793 else if (
28794 j6array
28795 [0]
28796 < -IKPI)
28797 {
28798 j6array
28799 [0]
28800 += IK2PI;
28801 }
28802 j6valid
28803 [0]
28804 = true;
28805 if (j6array
28806 [1]
28807 > IKPI)
28808 {
28809 j6array
28810 [1]
28811 -= IK2PI;
28812 }
28813 else if (
28814 j6array
28815 [1]
28816 < -IKPI)
28817 {
28818 j6array
28819 [1]
28820 += IK2PI;
28821 }
28822 j6valid
28823 [1]
28824 = true;
28825 for (
28826 int ij6
28827 = 0;
28828 ij6
28829 < 2;
28830 ++ij6)
28831 {
28832 if (!j6valid
28833 [ij6])
28834 {
28835 continue;
28836 }
28837 _ij6[0]
28838 = ij6;
28839 _ij6[1]
28840 = -1;
28841 for (
28842 int iij6
28843 = ij6
28844 + 1;
28845 iij6
28846 < 2;
28847 ++iij6)
28848 {
28849 if (j6valid
28850 [iij6]
28851 && IKabs(
28852 cj6array
28853 [ij6]
28854 - cj6array
28855 [iij6])
28856 < IKFAST_SOLUTION_THRESH
28857 && IKabs(
28858 sj6array
28859 [ij6]
28860 - sj6array
28861 [iij6])
28862 < IKFAST_SOLUTION_THRESH)
28863 {
28864 j6valid
28865 [iij6]
28866 = false;
28867 _ij6[1]
28868 = iij6;
28869 break;
28870 }
28871 }
28872 j6 = j6array
28873 [ij6];
28874 cj6 = cj6array
28875 [ij6];
28876 sj6 = sj6array
28877 [ij6];
28878 {
28879 IkReal evalcond
28880 [1];
28881 evalcond
28882 [0]
28883 = ((0.85)
28884 * (IKsin(
28885 j6)));
28886 if (IKabs(
28887 evalcond
28888 [0])
28889 > IKFAST_EVALCOND_THRESH)
28890 {
28891 continue;
28892 }
28893 }
28894
28895 rotationfunction0(
28896 solutions);
28897 }
28898 }
28899 }
28900 } while (
28901 0);
28902 if (bgotonextstatement)
28903 {
28904 bool
28905 bgotonextstatement
28906 = true;
28907 do
28908 {
28909 evalcond
28910 [0]
28911 = ((IKabs((
28912 (-3.14159265358979)
28913 + (IKfmod(
28914 ((3.14159265358979)
28915 + j4),
28916 6.28318530717959)))))
28917 + (IKabs(
28918 px)));
28919 evalcond
28920 [1]
28921 = -0.85;
28922 evalcond
28923 [2]
28924 = 0;
28925 evalcond
28926 [3]
28927 = ((-0.2125)
28928 + (((-1.0)
28929 * (1.0)
28930 * (py
28931 * py))));
28932 if (IKabs(
28933 evalcond
28934 [0])
28935 < 0.0000010000000000
28936 && IKabs(
28937 evalcond
28938 [1])
28939 < 0.0000010000000000
28940 && IKabs(
28941 evalcond
28942 [2])
28943 < 0.0000010000000000
28944 && IKabs(
28945 evalcond
28946 [3])
28947 < 0.0000010000000000)
28948 {
28949 bgotonextstatement
28950 = false;
28951 {
28952 IkReal j6eval
28953 [1];
28954 IkReal
28955 x2029
28956 = ((1.0)
28957 * py);
28958 sj8 = 0;
28959 cj8 = -1.0;
28960 j8 = 3.14159265358979;
28961 pz = 0;
28962 j9 = 0;
28963 sj9 = 0;
28964 cj9 = 1.0;
28965 pp = py
28966 * py;
28967 npx
28968 = (py
28969 * r10);
28970 npy
28971 = (py
28972 * r11);
28973 npz
28974 = (py
28975 * r12);
28976 rxp0_0
28977 = ((-1.0)
28978 * r20
28979 * x2029);
28980 rxp0_1
28981 = 0;
28982 rxp1_0
28983 = ((-1.0)
28984 * r21
28985 * x2029);
28986 rxp1_1
28987 = 0;
28988 rxp2_0
28989 = ((-1.0)
28990 * r22
28991 * x2029);
28992 rxp2_1
28993 = 0;
28994 px = 0;
28995 j4 = 0;
28996 sj4 = 0;
28997 cj4 = 1.0;
28998 rxp0_2
28999 = (py
29000 * r00);
29001 rxp1_2
29002 = (py
29003 * r01);
29004 rxp2_2
29005 = (py
29006 * r02);
29007 j6eval
29008 [0]
29009 = ((1.0)
29010 + (((1.91568587540858)
29011 * (py
29012 * py
29013 * py
29014 * py)))
29015 + (((-1.0)
29016 * (2.64633970947792)
29017 * (py
29018 * py))));
29019 if (IKabs(
29020 j6eval
29021 [0])
29022 < 0.0000010000000000)
29023 {
29024 continue; // no branches [j6]
29025 }
29026 else
29027 {
29028 {
29029 IkReal j6array
29030 [2],
29031 cj6array
29032 [2],
29033 sj6array
29034 [2];
29035 bool j6valid
29036 [2]
29037 = {false};
29038 _nj6
29039 = 2;
29040 IkReal
29041 x2030
29042 = py
29043 * py;
29044 CheckValue<IkReal> x2032 = IKatan2WithCheck(
29045 IkReal((
29046 (-0.425)
29047 + (((-0.588235294117647)
29048 * x2030)))),
29049 ((2.83333333333333)
29050 + (((-3.92156862745098)
29051 * x2030))),
29052 IKFAST_ATAN2_MAGTHRESH);
29053 if (!x2032
29054 .valid)
29055 {
29056 continue;
29057 }
29058 IkReal
29059 x2031
29060 = ((-1.0)
29061 * (x2032
29062 .value));
29063 j6array
29064 [0]
29065 = x2031;
29066 sj6array
29067 [0]
29068 = IKsin(
29069 j6array
29070 [0]);
29071 cj6array
29072 [0]
29073 = IKcos(
29074 j6array
29075 [0]);
29076 j6array
29077 [1]
29078 = ((3.14159265358979)
29079 + x2031);
29080 sj6array
29081 [1]
29082 = IKsin(
29083 j6array
29084 [1]);
29085 cj6array
29086 [1]
29087 = IKcos(
29088 j6array
29089 [1]);
29090 if (j6array
29091 [0]
29092 > IKPI)
29093 {
29094 j6array
29095 [0]
29096 -= IK2PI;
29097 }
29098 else if (
29099 j6array
29100 [0]
29101 < -IKPI)
29102 {
29103 j6array
29104 [0]
29105 += IK2PI;
29106 }
29107 j6valid
29108 [0]
29109 = true;
29110 if (j6array
29111 [1]
29112 > IKPI)
29113 {
29114 j6array
29115 [1]
29116 -= IK2PI;
29117 }
29118 else if (
29119 j6array
29120 [1]
29121 < -IKPI)
29122 {
29123 j6array
29124 [1]
29125 += IK2PI;
29126 }
29127 j6valid
29128 [1]
29129 = true;
29130 for (
29131 int ij6
29132 = 0;
29133 ij6
29134 < 2;
29135 ++ij6)
29136 {
29137 if (!j6valid
29138 [ij6])
29139 {
29140 continue;
29141 }
29142 _ij6[0]
29143 = ij6;
29144 _ij6[1]
29145 = -1;
29146 for (
29147 int iij6
29148 = ij6
29149 + 1;
29150 iij6
29151 < 2;
29152 ++iij6)
29153 {
29154 if (j6valid
29155 [iij6]
29156 && IKabs(
29157 cj6array
29158 [ij6]
29159 - cj6array
29160 [iij6])
29161 < IKFAST_SOLUTION_THRESH
29162 && IKabs(
29163 sj6array
29164 [ij6]
29165 - sj6array
29166 [iij6])
29167 < IKFAST_SOLUTION_THRESH)
29168 {
29169 j6valid
29170 [iij6]
29171 = false;
29172 _ij6[1]
29173 = iij6;
29174 break;
29175 }
29176 }
29177 j6 = j6array
29178 [ij6];
29179 cj6 = cj6array
29180 [ij6];
29181 sj6 = sj6array
29182 [ij6];
29183 {
29184 IkReal evalcond
29185 [1];
29186 evalcond
29187 [0]
29188 = ((0.85)
29189 * (IKsin(
29190 j6)));
29191 if (IKabs(
29192 evalcond
29193 [0])
29194 > IKFAST_EVALCOND_THRESH)
29195 {
29196 continue;
29197 }
29198 }
29199
29200 rotationfunction0(
29201 solutions);
29202 }
29203 }
29204 }
29205 }
29206 }
29207 } while (
29208 0);
29209 if (bgotonextstatement)
29210 {
29211 bool
29212 bgotonextstatement
29213 = true;
29214 do
29215 {
29216 evalcond
29217 [0]
29218 = ((IKabs((
29219 (-3.14159265358979)
29220 + (IKfmod(
29221 j4,
29222 6.28318530717959)))))
29223 + (IKabs(
29224 px)));
29225 evalcond
29226 [1]
29227 = -0.85;
29228 evalcond
29229 [2]
29230 = 0;
29231 evalcond
29232 [3]
29233 = ((-0.2125)
29234 + (((-1.0)
29235 * (1.0)
29236 * (py
29237 * py))));
29238 if (IKabs(
29239 evalcond
29240 [0])
29241 < 0.0000010000000000
29242 && IKabs(
29243 evalcond
29244 [1])
29245 < 0.0000010000000000
29246 && IKabs(
29247 evalcond
29248 [2])
29249 < 0.0000010000000000
29250 && IKabs(
29251 evalcond
29252 [3])
29253 < 0.0000010000000000)
29254 {
29255 bgotonextstatement
29256 = false;
29257 {
29258 IkReal j6eval
29259 [1];
29260 IkReal
29261 x2033
29262 = ((1.0)
29263 * py);
29264 sj8 = 0;
29265 cj8 = -1.0;
29266 j8 = 3.14159265358979;
29267 pz = 0;
29268 j9 = 0;
29269 sj9 = 0;
29270 cj9 = 1.0;
29271 pp = py
29272 * py;
29273 npx
29274 = (py
29275 * r10);
29276 npy
29277 = (py
29278 * r11);
29279 npz
29280 = (py
29281 * r12);
29282 rxp0_0
29283 = ((-1.0)
29284 * r20
29285 * x2033);
29286 rxp0_1
29287 = 0;
29288 rxp1_0
29289 = ((-1.0)
29290 * r21
29291 * x2033);
29292 rxp1_1
29293 = 0;
29294 rxp2_0
29295 = ((-1.0)
29296 * r22
29297 * x2033);
29298 rxp2_1
29299 = 0;
29300 px = 0;
29301 j4 = 3.14159265358979;
29302 sj4 = 0;
29303 cj4 = -1.0;
29304 rxp0_2
29305 = (py
29306 * r00);
29307 rxp1_2
29308 = (py
29309 * r01);
29310 rxp2_2
29311 = (py
29312 * r02);
29313 j6eval
29314 [0]
29315 = ((1.0)
29316 + (((1.91568587540858)
29317 * (py
29318 * py
29319 * py
29320 * py)))
29321 + (((-1.0)
29322 * (2.64633970947792)
29323 * (py
29324 * py))));
29325 if (IKabs(
29326 j6eval
29327 [0])
29328 < 0.0000010000000000)
29329 {
29330 continue; // no branches [j6]
29331 }
29332 else
29333 {
29334 {
29335 IkReal j6array
29336 [2],
29337 cj6array
29338 [2],
29339 sj6array
29340 [2];
29341 bool j6valid
29342 [2]
29343 = {false};
29344 _nj6
29345 = 2;
29346 IkReal
29347 x2034
29348 = py
29349 * py;
29350 CheckValue<IkReal> x2036 = IKatan2WithCheck(
29351 IkReal((
29352 (-0.425)
29353 + (((-0.588235294117647)
29354 * x2034)))),
29355 ((2.83333333333333)
29356 + (((-3.92156862745098)
29357 * x2034))),
29358 IKFAST_ATAN2_MAGTHRESH);
29359 if (!x2036
29360 .valid)
29361 {
29362 continue;
29363 }
29364 IkReal
29365 x2035
29366 = ((-1.0)
29367 * (x2036
29368 .value));
29369 j6array
29370 [0]
29371 = x2035;
29372 sj6array
29373 [0]
29374 = IKsin(
29375 j6array
29376 [0]);
29377 cj6array
29378 [0]
29379 = IKcos(
29380 j6array
29381 [0]);
29382 j6array
29383 [1]
29384 = ((3.14159265358979)
29385 + x2035);
29386 sj6array
29387 [1]
29388 = IKsin(
29389 j6array
29390 [1]);
29391 cj6array
29392 [1]
29393 = IKcos(
29394 j6array
29395 [1]);
29396 if (j6array
29397 [0]
29398 > IKPI)
29399 {
29400 j6array
29401 [0]
29402 -= IK2PI;
29403 }
29404 else if (
29405 j6array
29406 [0]
29407 < -IKPI)
29408 {
29409 j6array
29410 [0]
29411 += IK2PI;
29412 }
29413 j6valid
29414 [0]
29415 = true;
29416 if (j6array
29417 [1]
29418 > IKPI)
29419 {
29420 j6array
29421 [1]
29422 -= IK2PI;
29423 }
29424 else if (
29425 j6array
29426 [1]
29427 < -IKPI)
29428 {
29429 j6array
29430 [1]
29431 += IK2PI;
29432 }
29433 j6valid
29434 [1]
29435 = true;
29436 for (
29437 int ij6
29438 = 0;
29439 ij6
29440 < 2;
29441 ++ij6)
29442 {
29443 if (!j6valid
29444 [ij6])
29445 {
29446 continue;
29447 }
29448 _ij6[0]
29449 = ij6;
29450 _ij6[1]
29451 = -1;
29452 for (
29453 int iij6
29454 = ij6
29455 + 1;
29456 iij6
29457 < 2;
29458 ++iij6)
29459 {
29460 if (j6valid
29461 [iij6]
29462 && IKabs(
29463 cj6array
29464 [ij6]
29465 - cj6array
29466 [iij6])
29467 < IKFAST_SOLUTION_THRESH
29468 && IKabs(
29469 sj6array
29470 [ij6]
29471 - sj6array
29472 [iij6])
29473 < IKFAST_SOLUTION_THRESH)
29474 {
29475 j6valid
29476 [iij6]
29477 = false;
29478 _ij6[1]
29479 = iij6;
29480 break;
29481 }
29482 }
29483 j6 = j6array
29484 [ij6];
29485 cj6 = cj6array
29486 [ij6];
29487 sj6 = sj6array
29488 [ij6];
29489 {
29490 IkReal evalcond
29491 [1];
29492 evalcond
29493 [0]
29494 = ((0.85)
29495 * (IKsin(
29496 j6)));
29497 if (IKabs(
29498 evalcond
29499 [0])
29500 > IKFAST_EVALCOND_THRESH)
29501 {
29502 continue;
29503 }
29504 }
29505
29506 rotationfunction0(
29507 solutions);
29508 }
29509 }
29510 }
29511 }
29512 }
29513 } while (
29514 0);
29515 if (bgotonextstatement)
29516 {
29517 bool
29518 bgotonextstatement
29519 = true;
29520 do
29521 {
29522 evalcond
29523 [0]
29524 = ((IKabs((
29525 (-3.14159265358979)
29526 + (IKfmod(
29527 ((1.5707963267949)
29528 + j4),
29529 6.28318530717959)))))
29530 + (IKabs(
29531 py)));
29532 evalcond
29533 [1]
29534 = -0.85;
29535 evalcond
29536 [2]
29537 = 0;
29538 evalcond
29539 [3]
29540 = ((-0.2125)
29541 + (((-1.0)
29542 * (1.0)
29543 * (px
29544 * px))));
29545 if (IKabs(
29546 evalcond
29547 [0])
29548 < 0.0000010000000000
29549 && IKabs(
29550 evalcond
29551 [1])
29552 < 0.0000010000000000
29553 && IKabs(
29554 evalcond
29555 [2])
29556 < 0.0000010000000000
29557 && IKabs(
29558 evalcond
29559 [3])
29560 < 0.0000010000000000)
29561 {
29562 bgotonextstatement
29563 = false;
29564 {
29565 IkReal j6eval
29566 [1];
29567 IkReal
29568 x2037
29569 = ((1.0)
29570 * px);
29571 sj8 = 0;
29572 cj8 = -1.0;
29573 j8 = 3.14159265358979;
29574 pz = 0;
29575 j9 = 0;
29576 sj9 = 0;
29577 cj9 = 1.0;
29578 pp = px
29579 * px;
29580 npx
29581 = (px
29582 * r00);
29583 npy
29584 = (px
29585 * r01);
29586 npz
29587 = (px
29588 * r02);
29589 rxp0_0
29590 = 0;
29591 rxp0_1
29592 = (px
29593 * r20);
29594 rxp1_0
29595 = 0;
29596 rxp1_1
29597 = (px
29598 * r21);
29599 rxp2_0
29600 = 0;
29601 rxp2_1
29602 = (px
29603 * r22);
29604 py = 0;
29605 j4 = 1.5707963267949;
29606 sj4 = 1.0;
29607 cj4 = 0;
29608 rxp0_2
29609 = ((-1.0)
29610 * r10
29611 * x2037);
29612 rxp1_2
29613 = ((-1.0)
29614 * r11
29615 * x2037);
29616 rxp2_2
29617 = ((-1.0)
29618 * r12
29619 * x2037);
29620 j6eval
29621 [0]
29622 = ((1.0)
29623 + (((1.91568587540858)
29624 * (px
29625 * px
29626 * px
29627 * px)))
29628 + (((-1.0)
29629 * (2.64633970947792)
29630 * (px
29631 * px))));
29632 if (IKabs(
29633 j6eval
29634 [0])
29635 < 0.0000010000000000)
29636 {
29637 continue; // no branches [j6]
29638 }
29639 else
29640 {
29641 {
29642 IkReal j6array
29643 [2],
29644 cj6array
29645 [2],
29646 sj6array
29647 [2];
29648 bool j6valid
29649 [2]
29650 = {false};
29651 _nj6
29652 = 2;
29653 IkReal
29654 x2038
29655 = px
29656 * px;
29657 CheckValue<IkReal> x2040 = IKatan2WithCheck(
29658 IkReal((
29659 (-0.425)
29660 + (((-0.588235294117647)
29661 * x2038)))),
29662 ((2.83333333333333)
29663 + (((-3.92156862745098)
29664 * x2038))),
29665 IKFAST_ATAN2_MAGTHRESH);
29666 if (!x2040
29667 .valid)
29668 {
29669 continue;
29670 }
29671 IkReal
29672 x2039
29673 = ((-1.0)
29674 * (x2040
29675 .value));
29676 j6array
29677 [0]
29678 = x2039;
29679 sj6array
29680 [0]
29681 = IKsin(
29682 j6array
29683 [0]);
29684 cj6array
29685 [0]
29686 = IKcos(
29687 j6array
29688 [0]);
29689 j6array
29690 [1]
29691 = ((3.14159265358979)
29692 + x2039);
29693 sj6array
29694 [1]
29695 = IKsin(
29696 j6array
29697 [1]);
29698 cj6array
29699 [1]
29700 = IKcos(
29701 j6array
29702 [1]);
29703 if (j6array
29704 [0]
29705 > IKPI)
29706 {
29707 j6array
29708 [0]
29709 -= IK2PI;
29710 }
29711 else if (
29712 j6array
29713 [0]
29714 < -IKPI)
29715 {
29716 j6array
29717 [0]
29718 += IK2PI;
29719 }
29720 j6valid
29721 [0]
29722 = true;
29723 if (j6array
29724 [1]
29725 > IKPI)
29726 {
29727 j6array
29728 [1]
29729 -= IK2PI;
29730 }
29731 else if (
29732 j6array
29733 [1]
29734 < -IKPI)
29735 {
29736 j6array
29737 [1]
29738 += IK2PI;
29739 }
29740 j6valid
29741 [1]
29742 = true;
29743 for (
29744 int ij6
29745 = 0;
29746 ij6
29747 < 2;
29748 ++ij6)
29749 {
29750 if (!j6valid
29751 [ij6])
29752 {
29753 continue;
29754 }
29755 _ij6[0]
29756 = ij6;
29757 _ij6[1]
29758 = -1;
29759 for (
29760 int iij6
29761 = ij6
29762 + 1;
29763 iij6
29764 < 2;
29765 ++iij6)
29766 {
29767 if (j6valid
29768 [iij6]
29769 && IKabs(
29770 cj6array
29771 [ij6]
29772 - cj6array
29773 [iij6])
29774 < IKFAST_SOLUTION_THRESH
29775 && IKabs(
29776 sj6array
29777 [ij6]
29778 - sj6array
29779 [iij6])
29780 < IKFAST_SOLUTION_THRESH)
29781 {
29782 j6valid
29783 [iij6]
29784 = false;
29785 _ij6[1]
29786 = iij6;
29787 break;
29788 }
29789 }
29790 j6 = j6array
29791 [ij6];
29792 cj6 = cj6array
29793 [ij6];
29794 sj6 = sj6array
29795 [ij6];
29796 {
29797 IkReal evalcond
29798 [1];
29799 evalcond
29800 [0]
29801 = ((0.85)
29802 * (IKsin(
29803 j6)));
29804 if (IKabs(
29805 evalcond
29806 [0])
29807 > IKFAST_EVALCOND_THRESH)
29808 {
29809 continue;
29810 }
29811 }
29812
29813 rotationfunction0(
29814 solutions);
29815 }
29816 }
29817 }
29818 }
29819 }
29820 } while (
29821 0);
29822 if (bgotonextstatement)
29823 {
29824 bool
29825 bgotonextstatement
29826 = true;
29827 do
29828 {
29829 evalcond
29830 [0]
29831 = ((IKabs(
29832 py))
29833 + (IKabs((
29834 (-3.14159265358979)
29835 + (IKfmod(
29836 ((4.71238898038469)
29837 + j4),
29838 6.28318530717959))))));
29839 evalcond
29840 [1]
29841 = -0.85;
29842 evalcond
29843 [2]
29844 = 0;
29845 evalcond
29846 [3]
29847 = ((-0.2125)
29848 + (((-1.0)
29849 * (1.0)
29850 * (px
29851 * px))));
29852 if (IKabs(
29853 evalcond
29854 [0])
29855 < 0.0000010000000000
29856 && IKabs(
29857 evalcond
29858 [1])
29859 < 0.0000010000000000
29860 && IKabs(
29861 evalcond
29862 [2])
29863 < 0.0000010000000000
29864 && IKabs(
29865 evalcond
29866 [3])
29867 < 0.0000010000000000)
29868 {
29869 bgotonextstatement
29870 = false;
29871 {
29872 IkReal j6eval
29873 [1];
29874 IkReal
29875 x2041
29876 = ((1.0)
29877 * px);
29878 sj8 = 0;
29879 cj8 = -1.0;
29880 j8 = 3.14159265358979;
29881 pz = 0;
29882 j9 = 0;
29883 sj9 = 0;
29884 cj9 = 1.0;
29885 pp = px
29886 * px;
29887 npx
29888 = (px
29889 * r00);
29890 npy
29891 = (px
29892 * r01);
29893 npz
29894 = (px
29895 * r02);
29896 rxp0_0
29897 = 0;
29898 rxp0_1
29899 = (px
29900 * r20);
29901 rxp1_0
29902 = 0;
29903 rxp1_1
29904 = (px
29905 * r21);
29906 rxp2_0
29907 = 0;
29908 rxp2_1
29909 = (px
29910 * r22);
29911 py = 0;
29912 j4 = -1.5707963267949;
29913 sj4 = -1.0;
29914 cj4 = 0;
29915 rxp0_2
29916 = ((-1.0)
29917 * r10
29918 * x2041);
29919 rxp1_2
29920 = ((-1.0)
29921 * r11
29922 * x2041);
29923 rxp2_2
29924 = ((-1.0)
29925 * r12
29926 * x2041);
29927 j6eval
29928 [0]
29929 = ((1.0)
29930 + (((1.91568587540858)
29931 * (px
29932 * px
29933 * px
29934 * px)))
29935 + (((-1.0)
29936 * (2.64633970947792)
29937 * (px
29938 * px))));
29939 if (IKabs(
29940 j6eval
29941 [0])
29942 < 0.0000010000000000)
29943 {
29944 continue; // no branches [j6]
29945 }
29946 else
29947 {
29948 {
29949 IkReal j6array
29950 [2],
29951 cj6array
29952 [2],
29953 sj6array
29954 [2];
29955 bool j6valid
29956 [2]
29957 = {false};
29958 _nj6
29959 = 2;
29960 IkReal
29961 x2042
29962 = px
29963 * px;
29964 CheckValue<IkReal> x2044 = IKatan2WithCheck(
29965 IkReal((
29966 (-0.425)
29967 + (((-0.588235294117647)
29968 * x2042)))),
29969 ((2.83333333333333)
29970 + (((-3.92156862745098)
29971 * x2042))),
29972 IKFAST_ATAN2_MAGTHRESH);
29973 if (!x2044
29974 .valid)
29975 {
29976 continue;
29977 }
29978 IkReal
29979 x2043
29980 = ((-1.0)
29981 * (x2044
29982 .value));
29983 j6array
29984 [0]
29985 = x2043;
29986 sj6array
29987 [0]
29988 = IKsin(
29989 j6array
29990 [0]);
29991 cj6array
29992 [0]
29993 = IKcos(
29994 j6array
29995 [0]);
29996 j6array
29997 [1]
29998 = ((3.14159265358979)
29999 + x2043);
30000 sj6array
30001 [1]
30002 = IKsin(
30003 j6array
30004 [1]);
30005 cj6array
30006 [1]
30007 = IKcos(
30008 j6array
30009 [1]);
30010 if (j6array
30011 [0]
30012 > IKPI)
30013 {
30014 j6array
30015 [0]
30016 -= IK2PI;
30017 }
30018 else if (
30019 j6array
30020 [0]
30021 < -IKPI)
30022 {
30023 j6array
30024 [0]
30025 += IK2PI;
30026 }
30027 j6valid
30028 [0]
30029 = true;
30030 if (j6array
30031 [1]
30032 > IKPI)
30033 {
30034 j6array
30035 [1]
30036 -= IK2PI;
30037 }
30038 else if (
30039 j6array
30040 [1]
30041 < -IKPI)
30042 {
30043 j6array
30044 [1]
30045 += IK2PI;
30046 }
30047 j6valid
30048 [1]
30049 = true;
30050 for (
30051 int ij6
30052 = 0;
30053 ij6
30054 < 2;
30055 ++ij6)
30056 {
30057 if (!j6valid
30058 [ij6])
30059 {
30060 continue;
30061 }
30062 _ij6[0]
30063 = ij6;
30064 _ij6[1]
30065 = -1;
30066 for (
30067 int iij6
30068 = ij6
30069 + 1;
30070 iij6
30071 < 2;
30072 ++iij6)
30073 {
30074 if (j6valid
30075 [iij6]
30076 && IKabs(
30077 cj6array
30078 [ij6]
30079 - cj6array
30080 [iij6])
30081 < IKFAST_SOLUTION_THRESH
30082 && IKabs(
30083 sj6array
30084 [ij6]
30085 - sj6array
30086 [iij6])
30087 < IKFAST_SOLUTION_THRESH)
30088 {
30089 j6valid
30090 [iij6]
30091 = false;
30092 _ij6[1]
30093 = iij6;
30094 break;
30095 }
30096 }
30097 j6 = j6array
30098 [ij6];
30099 cj6 = cj6array
30100 [ij6];
30101 sj6 = sj6array
30102 [ij6];
30103 {
30104 IkReal evalcond
30105 [1];
30106 evalcond
30107 [0]
30108 = ((0.85)
30109 * (IKsin(
30110 j6)));
30111 if (IKabs(
30112 evalcond
30113 [0])
30114 > IKFAST_EVALCOND_THRESH)
30115 {
30116 continue;
30117 }
30118 }
30119
30120 rotationfunction0(
30121 solutions);
30122 }
30123 }
30124 }
30125 }
30126 }
30127 } while (
30128 0);
30129 if (bgotonextstatement)
30130 {
30131 bool
30132 bgotonextstatement
30133 = true;
30134 do
30135 {
30136 if (1)
30137 {
30138 bgotonextstatement
30139 = false;
30140 continue; // branch miss [j6]
30141 }
30142 } while (
30143 0);
30144 if (bgotonextstatement)
30145 {
30146 }
30147 }
30148 }
30149 }
30150 }
30151 }
30152 }
30153 }
30154 else
30155 {
30156 {
30157 IkReal j6array
30158 [1],
30159 cj6array
30160 [1],
30161 sj6array
30162 [1];
30163 bool j6valid
30164 [1]
30165 = {false};
30166 _nj6 = 1;
30167 IkReal
30168 x2045
30169 = (cj4
30170 * px);
30171 IkReal
30172 x2046
30173 = (py
30174 * sj4);
30175 IkReal
30176 x2047
30177 = px
30178 * px;
30179 IkReal
30180 x2048
30181 = py
30182 * py;
30183 CheckValue<
30184 IkReal>
30185 x2049
30186 = IKPowWithIntegerCheck(
30187 ((((20.0)
30188 * x2046))
30189 + (((20.0)
30190 * x2045))),
30191 -1);
30192 if (!x2049
30193 .valid)
30194 {
30195 continue;
30196 }
30197 CheckValue<IkReal> x2050 = IKPowWithIntegerCheck(
30198 ((((-8.5)
30199 * x2046))
30200 + (((-1.0)
30201 * (11.7647058823529)
30202 * cj4
30203 * (px
30204 * px
30205 * px)))
30206 + (((-8.5)
30207 * x2045))
30208 + (((-11.7647058823529)
30209 * x2045
30210 * x2048))
30211 + (((-11.7647058823529)
30212 * x2046
30213 * x2047))
30214 + (((-1.0)
30215 * (11.7647058823529)
30216 * sj4
30217 * (py
30218 * py
30219 * py)))),
30220 -1);
30221 if (!x2050
30222 .valid)
30223 {
30224 continue;
30225 }
30226 if (IKabs((
30227 (17.0)
30228 * (x2049
30229 .value)))
30230 < IKFAST_ATAN2_MAGTHRESH
30231 && IKabs((
30232 (x2050
30233 .value)
30234 * (((-48.1666666666667)
30235 + (((66.6666666666667)
30236 * x2047))
30237 + (((66.6666666666667)
30238 * x2048))))))
30239 < IKFAST_ATAN2_MAGTHRESH
30240 && IKabs(
30241 IKsqr((
30242 (17.0)
30243 * (x2049
30244 .value)))
30245 + IKsqr((
30246 (x2050
30247 .value)
30248 * (((-48.1666666666667)
30249 + (((66.6666666666667)
30250 * x2047))
30251 + (((66.6666666666667)
30252 * x2048))))))
30253 - 1)
30254 <= IKFAST_SINCOS_THRESH)
30255 continue;
30256 j6array[0] = IKatan2(
30257 ((17.0)
30258 * (x2049
30259 .value)),
30260 ((x2050
30261 .value)
30262 * (((-48.1666666666667)
30263 + (((66.6666666666667)
30264 * x2047))
30265 + (((66.6666666666667)
30266 * x2048))))));
30267 sj6array[0] = IKsin(
30268 j6array
30269 [0]);
30270 cj6array[0] = IKcos(
30271 j6array
30272 [0]);
30273 if (j6array
30274 [0]
30275 > IKPI)
30276 {
30277 j6array
30278 [0]
30279 -= IK2PI;
30280 }
30281 else if (
30282 j6array
30283 [0]
30284 < -IKPI)
30285 {
30286 j6array
30287 [0]
30288 += IK2PI;
30289 }
30290 j6valid[0]
30291 = true;
30292 for (
30293 int ij6
30294 = 0;
30295 ij6
30296 < 1;
30297 ++ij6)
30298 {
30299 if (!j6valid
30300 [ij6])
30301 {
30302 continue;
30303 }
30304 _ij6[0]
30305 = ij6;
30306 _ij6[1]
30307 = -1;
30308 for (
30309 int iij6
30310 = ij6
30311 + 1;
30312 iij6
30313 < 1;
30314 ++iij6)
30315 {
30316 if (j6valid
30317 [iij6]
30318 && IKabs(
30319 cj6array
30320 [ij6]
30321 - cj6array
30322 [iij6])
30323 < IKFAST_SOLUTION_THRESH
30324 && IKabs(
30325 sj6array
30326 [ij6]
30327 - sj6array
30328 [iij6])
30329 < IKFAST_SOLUTION_THRESH)
30330 {
30331 j6valid
30332 [iij6]
30333 = false;
30334 _ij6[1]
30335 = iij6;
30336 break;
30337 }
30338 }
30339 j6 = j6array
30340 [ij6];
30341 cj6 = cj6array
30342 [ij6];
30343 sj6 = sj6array
30344 [ij6];
30345 {
30346 IkReal evalcond
30347 [5];
30348 IkReal
30349 x2051
30350 = IKcos(
30351 j6);
30352 IkReal
30353 x2052
30354 = (cj4
30355 * px);
30356 IkReal
30357 x2053
30358 = (x2051
30359 * x2052);
30360 IkReal
30361 x2054
30362 = (py
30363 * sj4);
30364 IkReal
30365 x2055
30366 = (x2051
30367 * x2054);
30368 IkReal
30369 x2056
30370 = IKsin(
30371 j6);
30372 IkReal
30373 x2057
30374 = (x2052
30375 * x2056);
30376 IkReal
30377 x2058
30378 = (x2054
30379 * x2056);
30380 IkReal
30381 x2059
30382 = px
30383 * px;
30384 IkReal
30385 x2060
30386 = ((3.92156862745098)
30387 * x2056);
30388 IkReal
30389 x2061
30390 = ((0.588235294117647)
30391 * x2051);
30392 IkReal
30393 x2062
30394 = py
30395 * py;
30396 evalcond
30397 [0]
30398 = (x2055
30399 + x2053);
30400 evalcond
30401 [1]
30402 = ((-0.85)
30403 + x2057
30404 + x2058);
30405 evalcond
30406 [2]
30407 = ((((0.85)
30408 * x2056))
30409 + (((-1.0)
30410 * x2052))
30411 + (((-1.0)
30412 * x2054)));
30413 evalcond
30414 [3]
30415 = ((((-1.0)
30416 * x2061
30417 * x2062))
30418 + (((-1.0)
30419 * x2059
30420 * x2061))
30421 + (((-0.425)
30422 * x2051))
30423 + (((-1.0)
30424 * x2059
30425 * x2060))
30426 + (((2.83333333333333)
30427 * x2056))
30428 + (((-1.0)
30429 * x2060
30430 * x2062)));
30431 evalcond
30432 [4]
30433 = ((-0.2125)
30434 + (((1.1)
30435 * x2057))
30436 + (((-1.0)
30437 * x2059))
30438 + (((-0.09)
30439 * x2055))
30440 + (((1.1)
30441 * x2058))
30442 + (((-1.0)
30443 * x2062))
30444 + (((-0.09)
30445 * x2053)));
30446 if (IKabs(
30447 evalcond
30448 [0])
30449 > IKFAST_EVALCOND_THRESH
30450 || IKabs(
30451 evalcond
30452 [1])
30453 > IKFAST_EVALCOND_THRESH
30454 || IKabs(
30455 evalcond
30456 [2])
30457 > IKFAST_EVALCOND_THRESH
30458 || IKabs(
30459 evalcond
30460 [3])
30461 > IKFAST_EVALCOND_THRESH
30462 || IKabs(
30463 evalcond
30464 [4])
30465 > IKFAST_EVALCOND_THRESH)
30466 {
30467 continue;
30468 }
30469 }
30470
30471 rotationfunction0(
30472 solutions);
30473 }
30474 }
30475 }
30476 }
30477 }
30478 else
30479 {
30480 {
30481 IkReal j6array
30482 [1],
30483 cj6array
30484 [1],
30485 sj6array
30486 [1];
30487 bool
30488 j6valid[1]
30489 = {false};
30490 _nj6 = 1;
30491 IkReal x2063
30492 = (cj4
30493 * px);
30494 IkReal x2064
30495 = (py
30496 * sj4);
30497 IkReal x2065
30498 = px * px;
30499 IkReal x2066
30500 = py * py;
30501 CheckValue<
30502 IkReal>
30503 x2067
30504 = IKPowWithIntegerCheck(
30505 ((-7.225)
30506 + (((-10.0)
30507 * x2066))
30508 + (((-10.0)
30509 * x2065))),
30510 -1);
30511 if (!x2067
30512 .valid)
30513 {
30514 continue;
30515 }
30516 if (IKabs((
30517 (((1.17647058823529)
30518 * x2063))
30519 + (((1.17647058823529)
30520 * x2064))))
30521 < IKFAST_ATAN2_MAGTHRESH
30522 && IKabs((
30523 (x2067
30524 .value)
30525 * (((((-56.6666666666667)
30526 * x2064))
30527 + (((78.4313725490196)
30528 * sj4
30529 * (py
30530 * py
30531 * py)))
30532 + (((78.4313725490196)
30533 * cj4
30534 * (px
30535 * px
30536 * px)))
30537 + (((78.4313725490196)
30538 * x2063
30539 * x2066))
30540 + (((78.4313725490196)
30541 * x2064
30542 * x2065))
30543 + (((-56.6666666666667)
30544 * x2063))))))
30545 < IKFAST_ATAN2_MAGTHRESH
30546 && IKabs(
30547 IKsqr((
30548 (((1.17647058823529)
30549 * x2063))
30550 + (((1.17647058823529)
30551 * x2064))))
30552 + IKsqr((
30553 (x2067
30554 .value)
30555 * (((((-56.6666666666667)
30556 * x2064))
30557 + (((78.4313725490196)
30558 * sj4
30559 * (py
30560 * py
30561 * py)))
30562 + (((78.4313725490196)
30563 * cj4
30564 * (px
30565 * px
30566 * px)))
30567 + (((78.4313725490196)
30568 * x2063
30569 * x2066))
30570 + (((78.4313725490196)
30571 * x2064
30572 * x2065))
30573 + (((-56.6666666666667)
30574 * x2063))))))
30575 - 1)
30576 <= IKFAST_SINCOS_THRESH)
30577 continue;
30578 j6array[0] = IKatan2(
30579 ((((1.17647058823529)
30580 * x2063))
30581 + (((1.17647058823529)
30582 * x2064))),
30583 ((x2067
30584 .value)
30585 * (((((-56.6666666666667)
30586 * x2064))
30587 + (((78.4313725490196)
30588 * sj4
30589 * (py
30590 * py
30591 * py)))
30592 + (((78.4313725490196)
30593 * cj4
30594 * (px
30595 * px
30596 * px)))
30597 + (((78.4313725490196)
30598 * x2063
30599 * x2066))
30600 + (((78.4313725490196)
30601 * x2064
30602 * x2065))
30603 + (((-56.6666666666667)
30604 * x2063))))));
30605 sj6array[0]
30606 = IKsin(
30607 j6array
30608 [0]);
30609 cj6array[0]
30610 = IKcos(
30611 j6array
30612 [0]);
30613 if (j6array[0]
30614 > IKPI)
30615 {
30616 j6array[0]
30617 -= IK2PI;
30618 }
30619 else if (
30620 j6array[0]
30621 < -IKPI)
30622 {
30623 j6array[0]
30624 += IK2PI;
30625 }
30626 j6valid[0]
30627 = true;
30628 for (int ij6
30629 = 0;
30630 ij6 < 1;
30631 ++ij6)
30632 {
30633 if (!j6valid
30634 [ij6])
30635 {
30636 continue;
30637 }
30638 _ij6[0]
30639 = ij6;
30640 _ij6[1]
30641 = -1;
30642 for (
30643 int iij6
30644 = ij6
30645 + 1;
30646 iij6
30647 < 1;
30648 ++iij6)
30649 {
30650 if (j6valid
30651 [iij6]
30652 && IKabs(
30653 cj6array
30654 [ij6]
30655 - cj6array
30656 [iij6])
30657 < IKFAST_SOLUTION_THRESH
30658 && IKabs(
30659 sj6array
30660 [ij6]
30661 - sj6array
30662 [iij6])
30663 < IKFAST_SOLUTION_THRESH)
30664 {
30665 j6valid
30666 [iij6]
30667 = false;
30668 _ij6[1]
30669 = iij6;
30670 break;
30671 }
30672 }
30673 j6 = j6array
30674 [ij6];
30675 cj6 = cj6array
30676 [ij6];
30677 sj6 = sj6array
30678 [ij6];
30679 {
30680 IkReal evalcond
30681 [5];
30682 IkReal
30683 x2068
30684 = IKcos(
30685 j6);
30686 IkReal
30687 x2069
30688 = (cj4
30689 * px);
30690 IkReal
30691 x2070
30692 = (x2068
30693 * x2069);
30694 IkReal
30695 x2071
30696 = (py
30697 * sj4);
30698 IkReal
30699 x2072
30700 = (x2068
30701 * x2071);
30702 IkReal
30703 x2073
30704 = IKsin(
30705 j6);
30706 IkReal
30707 x2074
30708 = (x2069
30709 * x2073);
30710 IkReal
30711 x2075
30712 = (x2071
30713 * x2073);
30714 IkReal
30715 x2076
30716 = px
30717 * px;
30718 IkReal
30719 x2077
30720 = ((3.92156862745098)
30721 * x2073);
30722 IkReal
30723 x2078
30724 = ((0.588235294117647)
30725 * x2068);
30726 IkReal
30727 x2079
30728 = py
30729 * py;
30730 evalcond
30731 [0]
30732 = (x2070
30733 + x2072);
30734 evalcond
30735 [1]
30736 = ((-0.85)
30737 + x2075
30738 + x2074);
30739 evalcond
30740 [2]
30741 = ((((-1.0)
30742 * x2071))
30743 + (((0.85)
30744 * x2073))
30745 + (((-1.0)
30746 * x2069)));
30747 evalcond
30748 [3]
30749 = ((((-1.0)
30750 * x2076
30751 * x2077))
30752 + (((-1.0)
30753 * x2076
30754 * x2078))
30755 + (((-0.425)
30756 * x2068))
30757 + (((-1.0)
30758 * x2078
30759 * x2079))
30760 + (((2.83333333333333)
30761 * x2073))
30762 + (((-1.0)
30763 * x2077
30764 * x2079)));
30765 evalcond
30766 [4]
30767 = ((-0.2125)
30768 + (((-1.0)
30769 * x2079))
30770 + (((-1.0)
30771 * x2076))
30772 + (((1.1)
30773 * x2075))
30774 + (((-0.09)
30775 * x2070))
30776 + (((1.1)
30777 * x2074))
30778 + (((-0.09)
30779 * x2072)));
30780 if (IKabs(
30781 evalcond
30782 [0])
30783 > IKFAST_EVALCOND_THRESH
30784 || IKabs(
30785 evalcond
30786 [1])
30787 > IKFAST_EVALCOND_THRESH
30788 || IKabs(
30789 evalcond
30790 [2])
30791 > IKFAST_EVALCOND_THRESH
30792 || IKabs(
30793 evalcond
30794 [3])
30795 > IKFAST_EVALCOND_THRESH
30796 || IKabs(
30797 evalcond
30798 [4])
30799 > IKFAST_EVALCOND_THRESH)
30800 {
30801 continue;
30802 }
30803 }
30804
30805 rotationfunction0(
30806 solutions);
30807 }
30808 }
30809 }
30810 }
30811 }
30812 else
30813 {
30814 {
30815 IkReal j6array[1],
30816 cj6array[1],
30817 sj6array[1];
30818 bool j6valid[1]
30819 = {false};
30820 _nj6 = 1;
30821 IkReal x2080
30822 = (cj4 * px);
30823 IkReal x2081
30824 = (py * sj4);
30825 IkReal x2082
30826 = px * px;
30827 IkReal x2083
30828 = py * py;
30829 IkReal x2084
30830 = ((1.29411764705882)
30831 * (cj4
30832 * cj4));
30833 CheckValue<IkReal>
30834 x2085
30835 = IKPowWithIntegerCheck(
30836 ((((-0.09)
30837 * x2080))
30838 + (((-0.09)
30839 * x2081))),
30840 -1);
30841 if (!x2085.valid)
30842 {
30843 continue;
30844 }
30845 if (IKabs((
30846 (((1.17647058823529)
30847 * x2080))
30848 + (((1.17647058823529)
30849 * x2081))))
30850 < IKFAST_ATAN2_MAGTHRESH
30851 && IKabs((
30852 (x2085
30853 .value)
30854 * (((0.2125)
30855 + (((-2.58823529411765)
30856 * cj4
30857 * px
30858 * x2081))
30859 + (((-0.294117647058824)
30860 * x2083))
30861 + ((x2083
30862 * x2084))
30863 + (((-1.0)
30864 * x2082
30865 * x2084))
30866 + x2082))))
30867 < IKFAST_ATAN2_MAGTHRESH
30868 && IKabs(
30869 IKsqr((
30870 (((1.17647058823529)
30871 * x2080))
30872 + (((1.17647058823529)
30873 * x2081))))
30874 + IKsqr((
30875 (x2085
30876 .value)
30877 * (((0.2125)
30878 + (((-2.58823529411765)
30879 * cj4
30880 * px
30881 * x2081))
30882 + (((-0.294117647058824)
30883 * x2083))
30884 + ((x2083
30885 * x2084))
30886 + (((-1.0)
30887 * x2082
30888 * x2084))
30889 + x2082))))
30890 - 1)
30891 <= IKFAST_SINCOS_THRESH)
30892 continue;
30893 j6array[0] = IKatan2(
30894 ((((1.17647058823529)
30895 * x2080))
30896 + (((1.17647058823529)
30897 * x2081))),
30898 ((x2085.value)
30899 * (((0.2125)
30900 + (((-2.58823529411765)
30901 * cj4
30902 * px
30903 * x2081))
30904 + (((-0.294117647058824)
30905 * x2083))
30906 + ((x2083
30907 * x2084))
30908 + (((-1.0)
30909 * x2082
30910 * x2084))
30911 + x2082))));
30912 sj6array[0]
30913 = IKsin(
30914 j6array
30915 [0]);
30916 cj6array[0]
30917 = IKcos(
30918 j6array
30919 [0]);
30920 if (j6array[0]
30921 > IKPI)
30922 {
30923 j6array[0]
30924 -= IK2PI;
30925 }
30926 else if (
30927 j6array[0]
30928 < -IKPI)
30929 {
30930 j6array[0]
30931 += IK2PI;
30932 }
30933 j6valid[0] = true;
30934 for (int ij6 = 0;
30935 ij6 < 1;
30936 ++ij6)
30937 {
30938 if (!j6valid
30939 [ij6])
30940 {
30941 continue;
30942 }
30943 _ij6[0] = ij6;
30944 _ij6[1] = -1;
30945 for (int iij6
30946 = ij6 + 1;
30947 iij6 < 1;
30948 ++iij6)
30949 {
30950 if (j6valid
30951 [iij6]
30952 && IKabs(
30953 cj6array
30954 [ij6]
30955 - cj6array
30956 [iij6])
30957 < IKFAST_SOLUTION_THRESH
30958 && IKabs(
30959 sj6array
30960 [ij6]
30961 - sj6array
30962 [iij6])
30963 < IKFAST_SOLUTION_THRESH)
30964 {
30965 j6valid
30966 [iij6]
30967 = false;
30968 _ij6[1]
30969 = iij6;
30970 break;
30971 }
30972 }
30973 j6 = j6array
30974 [ij6];
30975 cj6 = cj6array
30976 [ij6];
30977 sj6 = sj6array
30978 [ij6];
30979 {
30980 IkReal
30981 evalcond
30982 [5];
30983 IkReal x2086
30984 = IKcos(
30985 j6);
30986 IkReal x2087
30987 = (cj4
30988 * px);
30989 IkReal x2088
30990 = (x2086
30991 * x2087);
30992 IkReal x2089
30993 = (py
30994 * sj4);
30995 IkReal x2090
30996 = (x2086
30997 * x2089);
30998 IkReal x2091
30999 = IKsin(
31000 j6);
31001 IkReal x2092
31002 = (x2087
31003 * x2091);
31004 IkReal x2093
31005 = (x2089
31006 * x2091);
31007 IkReal x2094
31008 = px * px;
31009 IkReal x2095
31010 = ((3.92156862745098)
31011 * x2091);
31012 IkReal x2096
31013 = ((0.588235294117647)
31014 * x2086);
31015 IkReal x2097
31016 = py * py;
31017 evalcond[0]
31018 = (x2090
31019 + x2088);
31020 evalcond[1]
31021 = ((-0.85)
31022 + x2093
31023 + x2092);
31024 evalcond[2]
31025 = ((((-1.0)
31026 * x2089))
31027 + (((-1.0)
31028 * x2087))
31029 + (((0.85)
31030 * x2091)));
31031 evalcond[3]
31032 = ((((-1.0)
31033 * x2095
31034 * x2097))
31035 + (((-1.0)
31036 * x2094
31037 * x2096))
31038 + (((-1.0)
31039 * x2096
31040 * x2097))
31041 + (((2.83333333333333)
31042 * x2091))
31043 + (((-1.0)
31044 * x2094
31045 * x2095))
31046 + (((-0.425)
31047 * x2086)));
31048 evalcond[4]
31049 = ((-0.2125)
31050 + (((-1.0)
31051 * x2094))
31052 + (((1.1)
31053 * x2093))
31054 + (((1.1)
31055 * x2092))
31056 + (((-1.0)
31057 * x2097))
31058 + (((-0.09)
31059 * x2090))
31060 + (((-0.09)
31061 * x2088)));
31062 if (IKabs(
31063 evalcond
31064 [0])
31065 > IKFAST_EVALCOND_THRESH
31066 || IKabs(
31067 evalcond
31068 [1])
31069 > IKFAST_EVALCOND_THRESH
31070 || IKabs(
31071 evalcond
31072 [2])
31073 > IKFAST_EVALCOND_THRESH
31074 || IKabs(
31075 evalcond
31076 [3])
31077 > IKFAST_EVALCOND_THRESH
31078 || IKabs(
31079 evalcond
31080 [4])
31081 > IKFAST_EVALCOND_THRESH)
31082 {
31083 continue;
31084 }
31085 }
31086
31087 rotationfunction0(
31088 solutions);
31089 }
31090 }
31091 }
31092 }
31093 }
31094 } while (0);
31095 if (bgotonextstatement)
31096 {
31097 bool bgotonextstatement
31098 = true;
31099 do
31100 {
31101 if (1)
31102 {
31103 bgotonextstatement
31104 = false;
31105 continue; // branch
31106 // miss [j6]
31107 }
31108 } while (0);
31109 if (bgotonextstatement)
31110 {
31111 }
31112 }
31113 }
31114 }
31115 else
31116 {
31117 {
31118 IkReal j6array[1],
31119 cj6array[1],
31120 sj6array[1];
31121 bool j6valid[1] = {false};
31122 _nj6 = 1;
31123 IkReal x2098 = (cj4 * px);
31124 IkReal x2099 = (py * sj4);
31125 IkReal x2100
31126 = ((0.108264705882353)
31127 * cj9);
31128 IkReal x2101
31129 = ((0.588235294117647)
31130 * pp);
31131 IkReal x2102 = (cj9 * pp);
31132 IkReal x2103 = (cj9 * sj9);
31133 IkReal x2104 = (pp * sj9);
31134 IkReal x2105 = cj9 * cj9;
31135 IkReal x2106 = ((1.0) * pz);
31136 CheckValue<IkReal> x2107
31137 = IKPowWithIntegerCheck(
31138 IKsign((
31139 (((-1.0) * x2099
31140 * x2100))
31141 + (((-1.0)
31142 * x2098
31143 * x2101))
31144 + (((-1.0)
31145 * (1.32323529411765)
31146 * cj9 * pz))
31147 + (((3.92156862745098)
31148 * pp * pz))
31149 + (((-1.0)
31150 * x2098
31151 * x2100))
31152 + (((-1.0)
31153 * (1.51009803921569)
31154 * pz))
31155 + (((-0.316735294117647)
31156 * x2098))
31157 + (((-1.0)
31158 * x2099
31159 * x2101))
31160 + (((-0.316735294117647)
31161 * x2099)))),
31162 -1);
31163 if (!x2107.valid)
31164 {
31165 continue;
31166 }
31167 CheckValue<IkReal> x2108 = IKatan2WithCheck(
31168 IkReal((
31169 (-0.174204411764706)
31170 + (((-0.0324794117647059)
31171 * x2105))
31172 + (pz * pz)
31173 + (((-0.0264705882352941)
31174 * x2104))
31175 + (((-1.0)
31176 * (0.154566176470588)
31177 * cj9))
31178 + (((-1.0)
31179 * (0.323529411764706)
31180 * pp))
31181 + (((-0.00487191176470588)
31182 * x2103))
31183 + (((-1.0)
31184 * (0.0142530882352941)
31185 * sj9))
31186 + (((-0.176470588235294)
31187 * x2102)))),
31188 ((-0.830553921568627)
31189 + (((-1.0)
31190 * (0.0679544117647059)
31191 * sj9))
31192 + (((-1.0)
31193 * (1.18080882352941)
31194 * cj9))
31195 + (((-0.396970588235294)
31196 * x2105))
31197 + (((-1.0) * x2099
31198 * x2106))
31199 + (((-0.0595455882352941)
31200 * x2103))
31201 + (((2.15686274509804)
31202 * pp))
31203 + (((-1.0) * x2098
31204 * x2106))
31205 + (((0.176470588235294)
31206 * x2104))
31207 + (((1.17647058823529)
31208 * x2102))),
31209 IKFAST_ATAN2_MAGTHRESH);
31210 if (!x2108.valid)
31211 {
31212 continue;
31213 }
31214 j6array[0]
31215 = ((-1.5707963267949)
31216 + (((1.5707963267949)
31217 * (x2107.value)))
31218 + (x2108.value));
31219 sj6array[0]
31220 = IKsin(j6array[0]);
31221 cj6array[0]
31222 = IKcos(j6array[0]);
31223 if (j6array[0] > IKPI)
31224 {
31225 j6array[0] -= IK2PI;
31226 }
31227 else if (j6array[0] < -IKPI)
31228 {
31229 j6array[0] += IK2PI;
31230 }
31231 j6valid[0] = true;
31232 for (int ij6 = 0; ij6 < 1;
31233 ++ij6)
31234 {
31235 if (!j6valid[ij6])
31236 {
31237 continue;
31238 }
31239 _ij6[0] = ij6;
31240 _ij6[1] = -1;
31241 for (int iij6 = ij6 + 1;
31242 iij6 < 1;
31243 ++iij6)
31244 {
31245 if (j6valid[iij6]
31246 && IKabs(
31247 cj6array[ij6]
31248 - cj6array
31249 [iij6])
31250 < IKFAST_SOLUTION_THRESH
31251 && IKabs(
31252 sj6array[ij6]
31253 - sj6array
31254 [iij6])
31255 < IKFAST_SOLUTION_THRESH)
31256 {
31257 j6valid[iij6] = false;
31258 _ij6[1] = iij6;
31259 break;
31260 }
31261 }
31262 j6 = j6array[ij6];
31263 cj6 = cj6array[ij6];
31264 sj6 = sj6array[ij6];
31265 {
31266 IkReal evalcond[5];
31267 IkReal x2109
31268 = ((0.3) * cj9);
31269 IkReal x2110
31270 = ((0.045) * sj9);
31271 IkReal x2111
31272 = IKcos(j6);
31273 IkReal x2112
31274 = (pz * x2111);
31275 IkReal x2113
31276 = IKsin(j6);
31277 IkReal x2114
31278 = (cj4 * px);
31279 IkReal x2115
31280 = (x2113 * x2114);
31281 IkReal x2116
31282 = (py * sj4);
31283 IkReal x2117
31284 = (x2113 * x2116);
31285 IkReal x2118
31286 = ((0.045) * cj9);
31287 IkReal x2119
31288 = ((0.3) * sj9);
31289 IkReal x2120
31290 = (pz * x2113);
31291 IkReal x2121
31292 = (x2111 * x2114);
31293 IkReal x2122
31294 = (x2111 * x2116);
31295 evalcond[0]
31296 = ((-0.55) + x2115
31297 + x2117 + x2112
31298 + (((-1.0)
31299 * x2110))
31300 + (((-1.0)
31301 * x2109)));
31302 evalcond[1]
31303 = ((0.045) + x2119
31304 + x2121 + x2122
31305 + (((-1.0)
31306 * x2118))
31307 + (((-1.0)
31308 * x2120)));
31309 evalcond[2]
31310 = ((((-3.92156862745098)
31311 * pp * x2113))
31312 + pz
31313 + (((-0.316735294117647)
31314 * x2111))
31315 + (((-0.588235294117647)
31316 * pp
31317 * x2111))
31318 + (((1.32323529411765)
31319 * cj9
31320 * x2113))
31321 + (((-0.108264705882353)
31322 * cj9
31323 * x2111))
31324 + (((1.51009803921569)
31325 * x2113)));
31326 evalcond[3]
31327 = ((((-1.0)
31328 * x2116))
31329 + (((-1.0)
31330 * x2114))
31331 + ((x2111
31332 * x2118))
31333 + (((-1.0)
31334 * x2111
31335 * x2119))
31336 + ((x2110
31337 * x2113))
31338 + (((0.55)
31339 * x2113))
31340 + (((-0.045)
31341 * x2111))
31342 + ((x2109
31343 * x2113)));
31344 evalcond[4]
31345 = ((-0.2125)
31346 + (((1.1)
31347 * x2115))
31348 + (((1.1)
31349 * x2112))
31350 + (((1.1)
31351 * x2117))
31352 + (((-0.09)
31353 * x2122))
31354 + (((-1.0)
31355 * (1.0)
31356 * pp))
31357 + (((0.09)
31358 * x2120))
31359 + (((-0.09)
31360 * x2121)));
31361 if (IKabs(evalcond[0])
31362 > IKFAST_EVALCOND_THRESH
31363 || IKabs(
31364 evalcond[1])
31365 > IKFAST_EVALCOND_THRESH
31366 || IKabs(
31367 evalcond[2])
31368 > IKFAST_EVALCOND_THRESH
31369 || IKabs(
31370 evalcond[3])
31371 > IKFAST_EVALCOND_THRESH
31372 || IKabs(
31373 evalcond[4])
31374 > IKFAST_EVALCOND_THRESH)
31375 {
31376 continue;
31377 }
31378 }
31379
31380 rotationfunction0(
31381 solutions);
31382 }
31383 }
31384 }
31385 }
31386 }
31387 else
31388 {
31389 {
31390 IkReal j6array[1], cj6array[1],
31391 sj6array[1];
31392 bool j6valid[1] = {false};
31393 _nj6 = 1;
31394 IkReal x2123
31395 = ((0.045) * cj4 * px);
31396 IkReal x2124
31397 = ((0.045) * py * sj4);
31398 IkReal x2125 = ((0.3) * sj9);
31399 IkReal x2126 = (cj4 * px);
31400 IkReal x2127 = (py * sj4);
31401 IkReal x2128 = (cj9 * sj9);
31402 IkReal x2129 = cj9 * cj9;
31403 IkReal x2130 = ((1.0) * pz);
31404 IkReal x2131 = py * py;
31405 IkReal x2132 = cj4 * cj4;
31406 CheckValue<IkReal> x2133
31407 = IKPowWithIntegerCheck(
31408 IKsign((
31409 (((-1.0) * (0.55)
31410 * pz))
31411 + (((-1.0) * x2125
31412 * x2127))
31413 + (((-1.0) * x2125
31414 * x2126))
31415 + ((cj9 * x2123))
31416 + (((-1.0) * x2123))
31417 + ((cj9 * x2124))
31418 + (((-1.0) * x2124))
31419 + (((-1.0) * (0.045)
31420 * pz * sj9))
31421 + (((-1.0) * (0.3)
31422 * cj9 * pz)))),
31423 -1);
31424 if (!x2133.valid)
31425 {
31426 continue;
31427 }
31428 CheckValue<IkReal> x2134
31429 = IKatan2WithCheck(
31430 IkReal(
31431 ((-0.03825)
31432 + (((-0.087975)
31433 * x2128))
31434 + (((0.027)
31435 * x2129))
31436 + (((-1.0)
31437 * (0.167025)
31438 * sj9))
31439 + (((-1.0) * x2126
31440 * x2130))
31441 + (((-1.0) * x2127
31442 * x2130))
31443 + (((0.01125)
31444 * cj9)))),
31445 ((-0.304525)
31446 + ((x2132 * (px * px)))
31447 + (((-1.0) * (0.0495)
31448 * sj9))
31449 + (((-0.027) * x2128))
31450 + (((-1.0) * x2131
31451 * x2132))
31452 + (((-1.0) * (0.33)
31453 * cj9))
31454 + (((-0.087975)
31455 * x2129))
31456 + (((2.0) * cj4 * px
31457 * x2127))
31458 + x2131),
31459 IKFAST_ATAN2_MAGTHRESH);
31460 if (!x2134.valid)
31461 {
31462 continue;
31463 }
31464 j6array[0]
31465 = ((-1.5707963267949)
31466 + (((1.5707963267949)
31467 * (x2133.value)))
31468 + (x2134.value));
31469 sj6array[0] = IKsin(j6array[0]);
31470 cj6array[0] = IKcos(j6array[0]);
31471 if (j6array[0] > IKPI)
31472 {
31473 j6array[0] -= IK2PI;
31474 }
31475 else if (j6array[0] < -IKPI)
31476 {
31477 j6array[0] += IK2PI;
31478 }
31479 j6valid[0] = true;
31480 for (int ij6 = 0; ij6 < 1;
31481 ++ij6)
31482 {
31483 if (!j6valid[ij6])
31484 {
31485 continue;
31486 }
31487 _ij6[0] = ij6;
31488 _ij6[1] = -1;
31489 for (int iij6 = ij6 + 1;
31490 iij6 < 1;
31491 ++iij6)
31492 {
31493 if (j6valid[iij6]
31494 && IKabs(
31495 cj6array[ij6]
31496 - cj6array[iij6])
31497 < IKFAST_SOLUTION_THRESH
31498 && IKabs(
31499 sj6array[ij6]
31500 - sj6array[iij6])
31501 < IKFAST_SOLUTION_THRESH)
31502 {
31503 j6valid[iij6] = false;
31504 _ij6[1] = iij6;
31505 break;
31506 }
31507 }
31508 j6 = j6array[ij6];
31509 cj6 = cj6array[ij6];
31510 sj6 = sj6array[ij6];
31511 {
31512 IkReal evalcond[5];
31513 IkReal x2135
31514 = ((0.3) * cj9);
31515 IkReal x2136
31516 = ((0.045) * sj9);
31517 IkReal x2137 = IKcos(j6);
31518 IkReal x2138 = (pz * x2137);
31519 IkReal x2139 = IKsin(j6);
31520 IkReal x2140 = (cj4 * px);
31521 IkReal x2141
31522 = (x2139 * x2140);
31523 IkReal x2142 = (py * sj4);
31524 IkReal x2143
31525 = (x2139 * x2142);
31526 IkReal x2144
31527 = ((0.045) * cj9);
31528 IkReal x2145
31529 = ((0.3) * sj9);
31530 IkReal x2146 = (pz * x2139);
31531 IkReal x2147
31532 = (x2137 * x2140);
31533 IkReal x2148
31534 = (x2137 * x2142);
31535 evalcond[0]
31536 = ((-0.55)
31537 + (((-1.0) * x2136))
31538 + x2141 + x2143
31539 + (((-1.0) * x2135))
31540 + x2138);
31541 evalcond[1]
31542 = ((0.045) + x2148
31543 + x2145 + x2147
31544 + (((-1.0) * x2144))
31545 + (((-1.0)
31546 * x2146)));
31547 evalcond[2]
31548 = ((((-0.588235294117647)
31549 * pp * x2137))
31550 + (((1.32323529411765)
31551 * cj9 * x2139))
31552 + (((1.51009803921569)
31553 * x2139))
31554 + pz
31555 + (((-3.92156862745098)
31556 * pp * x2139))
31557 + (((-0.316735294117647)
31558 * x2137))
31559 + (((-0.108264705882353)
31560 * cj9 * x2137)));
31561 evalcond[3]
31562 = ((((0.55) * x2139))
31563 + (((-0.045)
31564 * x2137))
31565 + (((-1.0) * x2140))
31566 + ((x2135 * x2139))
31567 + (((-1.0) * x2142))
31568 + ((x2136 * x2139))
31569 + ((x2137 * x2144))
31570 + (((-1.0) * x2137
31571 * x2145)));
31572 evalcond[4]
31573 = ((-0.2125)
31574 + (((-0.09) * x2148))
31575 + (((1.1) * x2143))
31576 + (((-0.09) * x2147))
31577 + (((1.1) * x2141))
31578 + (((0.09) * x2146))
31579 + (((1.1) * x2138))
31580 + (((-1.0) * (1.0)
31581 * pp)));
31582 if (IKabs(evalcond[0])
31583 > IKFAST_EVALCOND_THRESH
31584 || IKabs(evalcond[1])
31585 > IKFAST_EVALCOND_THRESH
31586 || IKabs(evalcond[2])
31587 > IKFAST_EVALCOND_THRESH
31588 || IKabs(evalcond[3])
31589 > IKFAST_EVALCOND_THRESH
31590 || IKabs(evalcond[4])
31591 > IKFAST_EVALCOND_THRESH)
31592 {
31593 continue;
31594 }
31595 }
31596
31597 rotationfunction0(solutions);
31598 }
31599 }
31600 }
31601 }
31602 }
31603 else
31604 {
31605 {
31606 IkReal j6array[1], cj6array[1],
31607 sj6array[1];
31608 bool j6valid[1] = {false};
31609 _nj6 = 1;
31610 IkReal x2149 = py * py;
31611 IkReal x2150 = (py * sj4);
31612 IkReal x2151 = cj4 * cj4;
31613 IkReal x2152 = ((0.045) * pz);
31614 IkReal x2153 = (cj4 * px);
31615 IkReal x2154 = ((0.3) * pz);
31616 IkReal x2155 = ((0.3) * cj4 * px);
31617 IkReal x2156 = ((0.045) * x2153);
31618 IkReal x2157 = ((0.3) * py * sj4);
31619 IkReal x2158 = ((0.045) * x2150);
31620 CheckValue<IkReal> x2159
31621 = IKPowWithIntegerCheck(
31622 IKsign(
31623 ((((-1.0) * x2149
31624 * x2151))
31625 + (pz * pz) + x2149
31626 + (((2.0) * cj4 * px
31627 * x2150))
31628 + ((x2151
31629 * (px * px))))),
31630 -1);
31631 if (!x2159.valid)
31632 {
31633 continue;
31634 }
31635 CheckValue<IkReal> x2160
31636 = IKatan2WithCheck(
31637 IkReal(
31638 (((sj9 * x2158)) + x2152
31639 + (((0.55) * x2150))
31640 + ((cj9 * x2155))
31641 + (((0.55) * x2153))
31642 + ((sj9 * x2156))
31643 + ((sj9 * x2154))
31644 + (((-1.0) * cj9
31645 * x2152))
31646 + ((cj9 * x2157)))),
31647 ((((-1.0) * sj9 * x2157))
31648 + (((-1.0) * x2156))
31649 + ((cj9 * x2154))
31650 + ((sj9 * x2152))
31651 + (((-1.0) * sj9 * x2155))
31652 + ((cj9 * x2156))
31653 + (((-1.0) * x2158))
31654 + (((0.55) * pz))
31655 + ((cj9 * x2158))),
31656 IKFAST_ATAN2_MAGTHRESH);
31657 if (!x2160.valid)
31658 {
31659 continue;
31660 }
31661 j6array[0]
31662 = ((-1.5707963267949)
31663 + (((1.5707963267949)
31664 * (x2159.value)))
31665 + (x2160.value));
31666 sj6array[0] = IKsin(j6array[0]);
31667 cj6array[0] = IKcos(j6array[0]);
31668 if (j6array[0] > IKPI)
31669 {
31670 j6array[0] -= IK2PI;
31671 }
31672 else if (j6array[0] < -IKPI)
31673 {
31674 j6array[0] += IK2PI;
31675 }
31676 j6valid[0] = true;
31677 for (int ij6 = 0; ij6 < 1; ++ij6)
31678 {
31679 if (!j6valid[ij6])
31680 {
31681 continue;
31682 }
31683 _ij6[0] = ij6;
31684 _ij6[1] = -1;
31685 for (int iij6 = ij6 + 1; iij6 < 1;
31686 ++iij6)
31687 {
31688 if (j6valid[iij6]
31689 && IKabs(
31690 cj6array[ij6]
31691 - cj6array[iij6])
31692 < IKFAST_SOLUTION_THRESH
31693 && IKabs(
31694 sj6array[ij6]
31695 - sj6array[iij6])
31696 < IKFAST_SOLUTION_THRESH)
31697 {
31698 j6valid[iij6] = false;
31699 _ij6[1] = iij6;
31700 break;
31701 }
31702 }
31703 j6 = j6array[ij6];
31704 cj6 = cj6array[ij6];
31705 sj6 = sj6array[ij6];
31706 {
31707 IkReal evalcond[5];
31708 IkReal x2161 = ((0.3) * cj9);
31709 IkReal x2162 = ((0.045) * sj9);
31710 IkReal x2163 = IKcos(j6);
31711 IkReal x2164 = (pz * x2163);
31712 IkReal x2165 = IKsin(j6);
31713 IkReal x2166 = (cj4 * px);
31714 IkReal x2167 = (x2165 * x2166);
31715 IkReal x2168 = (py * sj4);
31716 IkReal x2169 = (x2165 * x2168);
31717 IkReal x2170 = ((0.045) * cj9);
31718 IkReal x2171 = ((0.3) * sj9);
31719 IkReal x2172 = (pz * x2165);
31720 IkReal x2173 = (x2163 * x2166);
31721 IkReal x2174 = (x2163 * x2168);
31722 evalcond[0]
31723 = ((-0.55)
31724 + (((-1.0) * x2162))
31725 + x2169 + x2164 + x2167
31726 + (((-1.0) * x2161)));
31727 evalcond[1]
31728 = ((0.045) + x2173 + x2174
31729 + x2171
31730 + (((-1.0) * x2172))
31731 + (((-1.0) * x2170)));
31732 evalcond[2]
31733 = ((((1.32323529411765)
31734 * cj9 * x2165))
31735 + (((-0.108264705882353)
31736 * cj9 * x2163))
31737 + (((-3.92156862745098)
31738 * pp * x2165))
31739 + (((-0.588235294117647)
31740 * pp * x2163))
31741 + pz
31742 + (((1.51009803921569)
31743 * x2165))
31744 + (((-0.316735294117647)
31745 * x2163)));
31746 evalcond[3]
31747 = (((x2163 * x2170))
31748 + (((-1.0) * x2168))
31749 + ((x2161 * x2165))
31750 + (((-1.0) * x2163
31751 * x2171))
31752 + (((-0.045) * x2163))
31753 + ((x2162 * x2165))
31754 + (((-1.0) * x2166))
31755 + (((0.55) * x2165)));
31756 evalcond[4]
31757 = ((-0.2125)
31758 + (((-0.09) * x2174))
31759 + (((0.09) * x2172))
31760 + (((1.1) * x2164))
31761 + (((1.1) * x2167))
31762 + (((1.1) * x2169))
31763 + (((-1.0) * (1.0) * pp))
31764 + (((-0.09) * x2173)));
31765 if (IKabs(evalcond[0])
31766 > IKFAST_EVALCOND_THRESH
31767 || IKabs(evalcond[1])
31768 > IKFAST_EVALCOND_THRESH
31769 || IKabs(evalcond[2])
31770 > IKFAST_EVALCOND_THRESH
31771 || IKabs(evalcond[3])
31772 > IKFAST_EVALCOND_THRESH
31773 || IKabs(evalcond[4])
31774 > IKFAST_EVALCOND_THRESH)
31775 {
31776 continue;
31777 }
31778 }
31779
31780 rotationfunction0(solutions);
31781 }
31782 }
31783 }
31784 }
31785 }
31786 } while (0);
31787 if (bgotonextstatement)
31788 {
31789 bool bgotonextstatement = true;
31790 do
31791 {
31792 if (1)
31793 {
31794 bgotonextstatement = false;
31795 continue; // branch miss [j6]
31796 }
31797 } while (0);
31798 if (bgotonextstatement)
31799 {
31800 }
31801 }
31802 }
31803 }
31804 }
31805 else
31806 {
31807 {
31808 IkReal j6array[1], cj6array[1], sj6array[1];
31809 bool j6valid[1] = {false};
31810 _nj6 = 1;
31811 IkReal x2175 = (pz * sj8);
31812 IkReal x2176 = ((0.3) * cj9);
31813 IkReal x2177 = ((0.045) * sj9);
31814 IkReal x2178 = (cj4 * px);
31815 IkReal x2179 = ((0.045) * cj8 * sj8);
31816 IkReal x2180 = (x2178 * x2179);
31817 IkReal x2181 = (py * sj4);
31818 IkReal x2182 = (x2179 * x2181);
31819 IkReal x2183 = ((0.3) * cj8 * sj8 * sj9);
31820 IkReal x2184 = ((0.55) * cj8);
31821 IkReal x2185 = (cj4 * py);
31822 IkReal x2186 = (px * sj4);
31823 IkReal x2187 = (cj4 * cj8 * py);
31824 IkReal x2188 = ((1.0) * pz * sj8);
31825 IkReal x2189 = (cj8 * px * sj4);
31826 IkReal x2190 = cj8 * cj8;
31827 IkReal x2191 = ((0.045) * x2190);
31828 IkReal x2192 = (x2185 * x2191);
31829 IkReal x2193 = (x2186 * x2191);
31830 IkReal x2194 = ((0.3) * sj9 * x2190);
31831 CheckValue<IkReal> x2195
31832 = IKPowWithIntegerCheck(
31833 IKsign(
31834 ((((-1.0) * cj9 * x2182))
31835 + ((x2181 * x2183))
31836 + (((-0.55) * x2175))
31837 + (((-1.0) * x2175 * x2177))
31838 + (((-1.0) * x2175 * x2176))
31839 + (((-1.0) * cj9 * x2180))
31840 + ((x2178 * x2183)) + x2182
31841 + x2180)),
31842 -1);
31843 if (!x2195.valid)
31844 {
31845 continue;
31846 }
31847 CheckValue<IkReal> x2196 = IKatan2WithCheck(
31848 IkReal(
31849 ((((-1.0) * x2176 * x2189))
31850 + ((x2184 * x2185)) + ((x2176 * x2187))
31851 + (((-1.0) * x2178 * x2188))
31852 + ((x2177 * x2187))
31853 + (((-1.0) * x2181 * x2188))
31854 + (((-1.0) * x2184 * x2186))
31855 + (((-1.0) * x2177 * x2189)))),
31856 ((((-1.0) * (1.0) * sj8 * (pz * pz)))
31857 + (((-1.0) * x2193)) + x2192
31858 + (((-1.0) * x2186 * x2194))
31859 + (((-1.0) * cj9 * x2192))
31860 + ((x2185 * x2194)) + ((cj9 * x2193))),
31861 IKFAST_ATAN2_MAGTHRESH);
31862 if (!x2196.valid)
31863 {
31864 continue;
31865 }
31866 j6array[0]
31867 = ((-1.5707963267949)
31868 + (((1.5707963267949) * (x2195.value)))
31869 + (x2196.value));
31870 sj6array[0] = IKsin(j6array[0]);
31871 cj6array[0] = IKcos(j6array[0]);
31872 if (j6array[0] > IKPI)
31873 {
31874 j6array[0] -= IK2PI;
31875 }
31876 else if (j6array[0] < -IKPI)
31877 {
31878 j6array[0] += IK2PI;
31879 }
31880 j6valid[0] = true;
31881 for (int ij6 = 0; ij6 < 1; ++ij6)
31882 {
31883 if (!j6valid[ij6])
31884 {
31885 continue;
31886 }
31887 _ij6[0] = ij6;
31888 _ij6[1] = -1;
31889 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
31890 {
31891 if (j6valid[iij6]
31892 && IKabs(cj6array[ij6] - cj6array[iij6])
31893 < IKFAST_SOLUTION_THRESH
31894 && IKabs(sj6array[ij6] - sj6array[iij6])
31895 < IKFAST_SOLUTION_THRESH)
31896 {
31897 j6valid[iij6] = false;
31898 _ij6[1] = iij6;
31899 break;
31900 }
31901 }
31902 j6 = j6array[ij6];
31903 cj6 = cj6array[ij6];
31904 sj6 = sj6array[ij6];
31905 {
31906 IkReal evalcond[6];
31907 IkReal x2197 = ((0.3) * cj9);
31908 IkReal x2198 = ((0.045) * sj9);
31909 IkReal x2199 = IKcos(j6);
31910 IkReal x2200 = (pz * x2199);
31911 IkReal x2201 = IKsin(j6);
31912 IkReal x2202 = (cj4 * px * x2201);
31913 IkReal x2203 = (py * sj4);
31914 IkReal x2204 = (x2201 * x2203);
31915 IkReal x2205 = (px * sj4);
31916 IkReal x2206 = ((1.0) * cj4 * py);
31917 IkReal x2207 = (cj4 * sj8);
31918 IkReal x2208 = ((0.045) * cj8);
31919 IkReal x2209 = ((0.045) * cj9);
31920 IkReal x2210 = (cj8 * x2201);
31921 IkReal x2211 = ((0.3) * sj9);
31922 IkReal x2212 = (sj8 * x2205);
31923 IkReal x2213 = (pz * x2210);
31924 IkReal x2214 = (px * (((1.0) * cj4)));
31925 IkReal x2215 = (cj8 * x2199);
31926 IkReal x2216 = ((1.0) * x2203);
31927 IkReal x2217 = ((0.09) * cj8 * x2199);
31928 evalcond[0]
31929 = ((-0.55) + x2204 + x2202 + x2200
31930 + (((-1.0) * x2198))
31931 + (((-1.0) * x2197)));
31932 evalcond[1]
31933 = (((cj8 * x2205))
31934 + (((-1.0) * pz * sj8 * x2201))
31935 + (((-1.0) * cj8 * x2206))
31936 + ((sj8 * x2199 * x2203))
31937 + ((px * x2199 * x2207)));
31938 evalcond[2]
31939 = ((((-0.55) * x2199))
31940 + (((-1.0) * x2197 * x2199))
31941 + ((x2201 * x2208)) + pz
31942 + (((-1.0) * x2209 * x2210))
31943 + (((-1.0) * x2198 * x2199))
31944 + ((x2210 * x2211)));
31945 evalcond[3]
31946 = ((0.045) + (((-1.0) * x2215 * x2216))
31947 + (((-1.0) * x2209)) + x2211 + x2213
31948 + x2212 + (((-1.0) * x2214 * x2215))
31949 + (((-1.0) * sj8 * x2206)));
31950 evalcond[4]
31951 = (((x2211 * x2215)) + ((x2198 * x2201))
31952 + (((-1.0) * x2214))
31953 + (((-1.0) * x2209 * x2215))
31954 + (((0.55) * x2201))
31955 + ((x2199 * x2208))
31956 + (((-1.0) * x2216))
31957 + ((x2197 * x2201)));
31958 evalcond[5]
31959 = ((-0.2125) + (((1.1) * x2204))
31960 + (((1.1) * x2202))
31961 + (((0.09) * py * x2207))
31962 + (((-0.09) * x2213))
31963 + (((-0.09) * x2212))
31964 + ((x2203 * x2217))
31965 + (((-1.0) * (1.0) * pp))
31966 + ((cj4 * px * x2217))
31967 + (((1.1) * x2200)));
31968 if (IKabs(evalcond[0])
31969 > IKFAST_EVALCOND_THRESH
31970 || IKabs(evalcond[1])
31971 > IKFAST_EVALCOND_THRESH
31972 || IKabs(evalcond[2])
31973 > IKFAST_EVALCOND_THRESH
31974 || IKabs(evalcond[3])
31975 > IKFAST_EVALCOND_THRESH
31976 || IKabs(evalcond[4])
31977 > IKFAST_EVALCOND_THRESH
31978 || IKabs(evalcond[5])
31979 > IKFAST_EVALCOND_THRESH)
31980 {
31981 continue;
31982 }
31983 }
31984
31985 rotationfunction0(solutions);
31986 }
31987 }
31988 }
31989 }
31990 }
31991 else
31992 {
31993 {
31994 IkReal j6array[1], cj6array[1], sj6array[1];
31995 bool j6valid[1] = {false};
31996 _nj6 = 1;
31997 IkReal x2218 = py * py;
31998 IkReal x2219 = (sj8 * x2218);
31999 IkReal x2220 = (cj4 * px * sj8);
32000 IkReal x2221 = cj4 * cj4;
32001 IkReal x2222 = px * px;
32002 IkReal x2223 = ((0.55) * sj8);
32003 IkReal x2224 = (cj8 * px);
32004 IkReal x2225 = ((0.3) * cj9);
32005 IkReal x2226 = ((0.045) * sj9);
32006 IkReal x2227 = (py * sj4 * sj8);
32007 IkReal x2228 = (pz * sj8);
32008 IkReal x2229 = (cj4 * cj8 * sj4);
32009 CheckValue<IkReal> x2230 = IKatan2WithCheck(
32010 IkReal(
32011 (((py * sj4 * x2223)) + ((x2220 * x2225))
32012 + ((pz * sj4 * x2224))
32013 + (((-1.0) * cj4 * cj8 * py * pz))
32014 + ((cj4 * px * x2223)) + ((x2226 * x2227))
32015 + ((x2225 * x2227)) + ((x2220 * x2226)))),
32016 ((((2.0) * py * x2221 * x2224))
32017 + ((x2225 * x2228)) + ((x2218 * x2229))
32018 + ((x2226 * x2228)) + ((pz * x2223))
32019 + (((-1.0) * x2222 * x2229))
32020 + (((-1.0) * py * x2224))),
32021 IKFAST_ATAN2_MAGTHRESH);
32022 if (!x2230.valid)
32023 {
32024 continue;
32025 }
32026 CheckValue<IkReal> x2231 = IKPowWithIntegerCheck(
32027 IKsign(
32028 ((((-1.0) * x2219 * x2221))
32029 + (((2.0) * py * sj4 * x2220)) + x2219
32030 + ((sj8 * (pz * pz)))
32031 + ((sj8 * x2221 * x2222)))),
32032 -1);
32033 if (!x2231.valid)
32034 {
32035 continue;
32036 }
32037 j6array[0]
32038 = ((-1.5707963267949) + (x2230.value)
32039 + (((1.5707963267949) * (x2231.value))));
32040 sj6array[0] = IKsin(j6array[0]);
32041 cj6array[0] = IKcos(j6array[0]);
32042 if (j6array[0] > IKPI)
32043 {
32044 j6array[0] -= IK2PI;
32045 }
32046 else if (j6array[0] < -IKPI)
32047 {
32048 j6array[0] += IK2PI;
32049 }
32050 j6valid[0] = true;
32051 for (int ij6 = 0; ij6 < 1; ++ij6)
32052 {
32053 if (!j6valid[ij6])
32054 {
32055 continue;
32056 }
32057 _ij6[0] = ij6;
32058 _ij6[1] = -1;
32059 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
32060 {
32061 if (j6valid[iij6]
32062 && IKabs(cj6array[ij6] - cj6array[iij6])
32063 < IKFAST_SOLUTION_THRESH
32064 && IKabs(sj6array[ij6] - sj6array[iij6])
32065 < IKFAST_SOLUTION_THRESH)
32066 {
32067 j6valid[iij6] = false;
32068 _ij6[1] = iij6;
32069 break;
32070 }
32071 }
32072 j6 = j6array[ij6];
32073 cj6 = cj6array[ij6];
32074 sj6 = sj6array[ij6];
32075 {
32076 IkReal evalcond[6];
32077 IkReal x2232 = ((0.3) * cj9);
32078 IkReal x2233 = ((0.045) * sj9);
32079 IkReal x2234 = IKcos(j6);
32080 IkReal x2235 = (pz * x2234);
32081 IkReal x2236 = IKsin(j6);
32082 IkReal x2237 = (cj4 * px * x2236);
32083 IkReal x2238 = (py * sj4);
32084 IkReal x2239 = (x2236 * x2238);
32085 IkReal x2240 = (px * sj4);
32086 IkReal x2241 = ((1.0) * cj4 * py);
32087 IkReal x2242 = (cj4 * sj8);
32088 IkReal x2243 = ((0.045) * cj8);
32089 IkReal x2244 = ((0.045) * cj9);
32090 IkReal x2245 = (cj8 * x2236);
32091 IkReal x2246 = ((0.3) * sj9);
32092 IkReal x2247 = (sj8 * x2240);
32093 IkReal x2248 = (pz * x2245);
32094 IkReal x2249 = (px * (((1.0) * cj4)));
32095 IkReal x2250 = (cj8 * x2234);
32096 IkReal x2251 = ((1.0) * x2238);
32097 IkReal x2252 = ((0.09) * cj8 * x2234);
32098 evalcond[0]
32099 = ((-0.55) + x2237 + x2235 + x2239
32100 + (((-1.0) * x2232))
32101 + (((-1.0) * x2233)));
32102 evalcond[1]
32103 = (((px * x2234 * x2242))
32104 + ((sj8 * x2234 * x2238))
32105 + (((-1.0) * cj8 * x2241))
32106 + ((cj8 * x2240))
32107 + (((-1.0) * pz * sj8 * x2236)));
32108 evalcond[2]
32109 = ((((-1.0) * x2244 * x2245))
32110 + ((x2236 * x2243)) + pz
32111 + (((-1.0) * x2233 * x2234))
32112 + (((-1.0) * x2232 * x2234))
32113 + (((-0.55) * x2234))
32114 + ((x2245 * x2246)));
32115 evalcond[3]
32116 = ((0.045) + (((-1.0) * x2244))
32117 + (((-1.0) * x2249 * x2250))
32118 + (((-1.0) * sj8 * x2241)) + x2247
32119 + x2246 + x2248
32120 + (((-1.0) * x2250 * x2251)));
32121 evalcond[4]
32122 = ((((0.55) * x2236)) + (((-1.0) * x2249))
32123 + ((x2233 * x2236))
32124 + (((-1.0) * x2244 * x2250))
32125 + ((x2246 * x2250)) + (((-1.0) * x2251))
32126 + ((x2232 * x2236)) + ((x2234 * x2243)));
32127 evalcond[5]
32128 = ((-0.2125) + (((-0.09) * x2247))
32129 + (((-0.09) * x2248)) + ((x2238 * x2252))
32130 + (((1.1) * x2239)) + (((1.1) * x2237))
32131 + (((1.1) * x2235))
32132 + ((cj4 * px * x2252))
32133 + (((-1.0) * (1.0) * pp))
32134 + (((0.09) * py * x2242)));
32135 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
32136 || IKabs(evalcond[1])
32137 > IKFAST_EVALCOND_THRESH
32138 || IKabs(evalcond[2])
32139 > IKFAST_EVALCOND_THRESH
32140 || IKabs(evalcond[3])
32141 > IKFAST_EVALCOND_THRESH
32142 || IKabs(evalcond[4])
32143 > IKFAST_EVALCOND_THRESH
32144 || IKabs(evalcond[5])
32145 > IKFAST_EVALCOND_THRESH)
32146 {
32147 continue;
32148 }
32149 }
32150
32151 rotationfunction0(solutions);
32152 }
32153 }
32154 }
32155 }
32156 }
32157 else
32158 {
32159 {
32160 IkReal j6array[1], cj6array[1], sj6array[1];
32161 bool j6valid[1] = {false};
32162 _nj6 = 1;
32163 IkReal x2253 = (cj4 * px);
32164 IkReal x2254 = ((0.045) * pz);
32165 IkReal x2255 = (py * sj4);
32166 IkReal x2256 = ((0.3) * cj9);
32167 IkReal x2257 = ((0.045) * sj9);
32168 IkReal x2258 = (cj8 * cj9);
32169 IkReal x2259 = (cj8 * sj9);
32170 IkReal x2260 = cj9 * cj9;
32171 IkReal x2261 = ((1.0) * pz);
32172 CheckValue<IkReal> x2262 = IKatan2WithCheck(
32173 IkReal(
32174 ((-0.304525) + (((-1.0) * (0.027) * cj9 * sj9))
32175 + (((-1.0) * (0.0495) * sj9)) + (pz * pz)
32176 + (((-0.087975) * x2260))
32177 + (((-1.0) * (0.33) * cj9)))),
32178 ((((-1.0) * x2255 * x2261))
32179 + (((0.027) * cj8 * x2260)) + (((0.01125) * x2258))
32180 + (((-1.0) * x2253 * x2261))
32181 + (((-1.0) * (0.03825) * cj8))
32182 + (((-0.087975) * sj9 * x2258))
32183 + (((-0.167025) * x2259))),
32184 IKFAST_ATAN2_MAGTHRESH);
32185 if (!x2262.valid)
32186 {
32187 continue;
32188 }
32189 CheckValue<IkReal> x2263 = IKPowWithIntegerCheck(
32190 IKsign((
32191 (((-1.0) * x2255 * x2257))
32192 + (((-1.0) * x2255 * x2256)) + ((x2254 * x2258))
32193 + (((-1.0) * x2253 * x2257))
32194 + (((-0.55) * x2255)) + (((-0.3) * pz * x2259))
32195 + (((-1.0) * x2253 * x2256))
32196 + (((-0.55) * x2253))
32197 + (((-1.0) * cj8 * x2254)))),
32198 -1);
32199 if (!x2263.valid)
32200 {
32201 continue;
32202 }
32203 j6array[0]
32204 = ((-1.5707963267949) + (x2262.value)
32205 + (((1.5707963267949) * (x2263.value))));
32206 sj6array[0] = IKsin(j6array[0]);
32207 cj6array[0] = IKcos(j6array[0]);
32208 if (j6array[0] > IKPI)
32209 {
32210 j6array[0] -= IK2PI;
32211 }
32212 else if (j6array[0] < -IKPI)
32213 {
32214 j6array[0] += IK2PI;
32215 }
32216 j6valid[0] = true;
32217 for (int ij6 = 0; ij6 < 1; ++ij6)
32218 {
32219 if (!j6valid[ij6])
32220 {
32221 continue;
32222 }
32223 _ij6[0] = ij6;
32224 _ij6[1] = -1;
32225 for (int iij6 = ij6 + 1; iij6 < 1; ++iij6)
32226 {
32227 if (j6valid[iij6]
32228 && IKabs(cj6array[ij6] - cj6array[iij6])
32229 < IKFAST_SOLUTION_THRESH
32230 && IKabs(sj6array[ij6] - sj6array[iij6])
32231 < IKFAST_SOLUTION_THRESH)
32232 {
32233 j6valid[iij6] = false;
32234 _ij6[1] = iij6;
32235 break;
32236 }
32237 }
32238 j6 = j6array[ij6];
32239 cj6 = cj6array[ij6];
32240 sj6 = sj6array[ij6];
32241 {
32242 IkReal evalcond[6];
32243 IkReal x2264 = ((0.3) * cj9);
32244 IkReal x2265 = ((0.045) * sj9);
32245 IkReal x2266 = IKcos(j6);
32246 IkReal x2267 = (pz * x2266);
32247 IkReal x2268 = IKsin(j6);
32248 IkReal x2269 = (cj4 * px * x2268);
32249 IkReal x2270 = (py * sj4);
32250 IkReal x2271 = (x2268 * x2270);
32251 IkReal x2272 = (px * sj4);
32252 IkReal x2273 = ((1.0) * cj4 * py);
32253 IkReal x2274 = (cj4 * sj8);
32254 IkReal x2275 = ((0.045) * cj8);
32255 IkReal x2276 = ((0.045) * cj9);
32256 IkReal x2277 = (cj8 * x2268);
32257 IkReal x2278 = ((0.3) * sj9);
32258 IkReal x2279 = (sj8 * x2272);
32259 IkReal x2280 = (pz * x2277);
32260 IkReal x2281 = (px * (((1.0) * cj4)));
32261 IkReal x2282 = (cj8 * x2266);
32262 IkReal x2283 = ((1.0) * x2270);
32263 IkReal x2284 = ((0.09) * cj8 * x2266);
32264 evalcond[0]
32265 = ((-0.55) + x2267 + x2269 + (((-1.0) * x2265))
32266 + x2271 + (((-1.0) * x2264)));
32267 evalcond[1]
32268 = (((cj8 * x2272)) + (((-1.0) * cj8 * x2273))
32269 + (((-1.0) * pz * sj8 * x2268))
32270 + ((sj8 * x2266 * x2270))
32271 + ((px * x2266 * x2274)));
32272 evalcond[2]
32273 = ((((-1.0) * x2264 * x2266))
32274 + ((x2268 * x2275)) + pz
32275 + (((-1.0) * x2265 * x2266))
32276 + ((x2277 * x2278)) + (((-0.55) * x2266))
32277 + (((-1.0) * x2276 * x2277)));
32278 evalcond[3]
32279 = ((0.045) + (((-1.0) * x2282 * x2283)) + x2280
32280 + (((-1.0) * sj8 * x2273))
32281 + (((-1.0) * x2281 * x2282))
32282 + (((-1.0) * x2276)) + x2278 + x2279);
32283 evalcond[4]
32284 = ((((-1.0) * x2281)) + ((x2266 * x2275))
32285 + (((-1.0) * x2276 * x2282))
32286 + ((x2264 * x2268)) + ((x2278 * x2282))
32287 + ((x2265 * x2268)) + (((-1.0) * x2283))
32288 + (((0.55) * x2268)));
32289 evalcond[5]
32290 = ((-0.2125) + (((-0.09) * x2279))
32291 + (((-0.09) * x2280)) + ((cj4 * px * x2284))
32292 + (((0.09) * py * x2274)) + (((1.1) * x2267))
32293 + ((x2270 * x2284)) + (((-1.0) * (1.0) * pp))
32294 + (((1.1) * x2269)) + (((1.1) * x2271)));
32295 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
32296 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
32297 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
32298 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
32299 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
32300 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH)
32301 {
32302 continue;
32303 }
32304 }
32305
32306 rotationfunction0(solutions);
32307 }
32308 }
32309 }
32310 }
32311 }
32312 }
32313 }
32314 }
32315 }
32316 }
32317 }
32318 return solutions.GetNumSolutions() > 0;
32319 }
rotationfunction0(IkSolutionListBase<IkReal> & solutions)32320 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions)
32321 {
32322 for (int rotationiter = 0; rotationiter < 1; ++rotationiter)
32323 {
32324 IkReal x158 = ((1.0) * sj9);
32325 IkReal x159 = (cj9 * sj6);
32326 IkReal x160 = ((((-1.0) * cj6 * x158)) + (((-1.0) * cj8 * x159)));
32327 IkReal x161 = (cj4 * sj8);
32328 IkReal x162 = (sj6 * x158);
32329 IkReal x163 = (cj6 * cj9);
32330 IkReal x164 = (((cj8 * x163)) + (((-1.0) * x162)));
32331 IkReal x165 = (((cj9 * x161)) + ((sj4 * x164)));
32332 IkReal x166 = ((1.0) * sj4 * sj8);
32333 IkReal x167 = ((((-1.0) * cj9 * x166)) + ((cj4 * x164)));
32334 IkReal x168 = (sj6 * sj8);
32335 IkReal x169 = (((cj4 * cj8)) + (((-1.0) * cj6 * x166)));
32336 IkReal x170 = ((((-1.0) * cj6 * x161)) + (((-1.0) * cj8 * sj4)));
32337 IkReal x171 = ((((-1.0) * cj8 * x162)) + x163);
32338 IkReal x172 = (x159 + ((cj6 * cj8 * sj9)));
32339 IkReal x173 = (((sj4 * x172)) + ((sj9 * x161)));
32340 IkReal x174 = (((cj4 * x172)) + (((-1.0) * sj4 * sj8 * x158)));
32341 new_r00 = (((r00 * x167)) + ((r20 * x160)) + ((r10 * x165)));
32342 new_r01 = (((r11 * x165)) + ((r21 * x160)) + ((r01 * x167)));
32343 new_r02 = (((r22 * x160)) + ((r12 * x165)) + ((r02 * x167)));
32344 new_r10 = (((r20 * x168)) + ((r10 * x169)) + ((r00 * x170)));
32345 new_r11 = (((r01 * x170)) + ((r21 * x168)) + ((r11 * x169)));
32346 new_r12 = (((r22 * x168)) + ((r02 * x170)) + ((r12 * x169)));
32347 new_r20 = (((r20 * x171)) + ((r10 * x173)) + ((r00 * x174)));
32348 new_r21 = (((r01 * x174)) + ((r11 * x173)) + ((r21 * x171)));
32349 new_r22 = (((r12 * x173)) + ((r22 * x171)) + ((r02 * x174)));
32350 {
32351 IkReal j11array[2], cj11array[2], sj11array[2];
32352 bool j11valid[2] = {false};
32353 _nj11 = 2;
32354 cj11array[0] = new_r22;
32355 if (cj11array[0] >= -1 - IKFAST_SINCOS_THRESH
32356 && cj11array[0] <= 1 + IKFAST_SINCOS_THRESH)
32357 {
32358 j11valid[0] = j11valid[1] = true;
32359 j11array[0] = IKacos(cj11array[0]);
32360 sj11array[0] = IKsin(j11array[0]);
32361 cj11array[1] = cj11array[0];
32362 j11array[1] = -j11array[0];
32363 sj11array[1] = -sj11array[0];
32364 }
32365 else if (std::isnan(cj11array[0]))
32366 {
32367 // probably any value will work
32368 j11valid[0] = true;
32369 cj11array[0] = 1;
32370 sj11array[0] = 0;
32371 j11array[0] = 0;
32372 }
32373 for (int ij11 = 0; ij11 < 2; ++ij11)
32374 {
32375 if (!j11valid[ij11])
32376 {
32377 continue;
32378 }
32379 _ij11[0] = ij11;
32380 _ij11[1] = -1;
32381 for (int iij11 = ij11 + 1; iij11 < 2; ++iij11)
32382 {
32383 if (j11valid[iij11]
32384 && IKabs(cj11array[ij11] - cj11array[iij11])
32385 < IKFAST_SOLUTION_THRESH
32386 && IKabs(sj11array[ij11] - sj11array[iij11])
32387 < IKFAST_SOLUTION_THRESH)
32388 {
32389 j11valid[iij11] = false;
32390 _ij11[1] = iij11;
32391 break;
32392 }
32393 }
32394 j11 = j11array[ij11];
32395 cj11 = cj11array[ij11];
32396 sj11 = sj11array[ij11];
32397
32398 {
32399 IkReal j10eval[2];
32400 IkReal x175 = ((1.0) * sj9);
32401 IkReal x176 = (cj9 * sj6);
32402 IkReal x177 = x160;
32403 IkReal x178 = (cj4 * sj8);
32404 IkReal x179 = (sj6 * x175);
32405 IkReal x180 = (cj6 * cj9);
32406 IkReal x181 = (((cj8 * x180)) + (((-1.0) * x179)));
32407 IkReal x182 = (((sj4 * x181)) + ((cj9 * x178)));
32408 IkReal x183 = ((1.0) * sj4 * sj8);
32409 IkReal x184 = (((cj4 * x181)) + (((-1.0) * cj9 * x183)));
32410 IkReal x185 = (sj6 * sj8);
32411 IkReal x186 = x169;
32412 IkReal x187 = x170;
32413 IkReal x188 = ((((-1.0) * cj8 * x179)) + x180);
32414 IkReal x189 = x172;
32415 IkReal x190 = (((sj9 * x178)) + ((sj4 * x189)));
32416 IkReal x191 = ((((-1.0) * sj4 * sj8 * x175)) + ((cj4 * x189)));
32417 new_r00 = (((r20 * x177)) + ((r00 * x184)) + ((r10 * x182)));
32418 new_r01 = (((r21 * x177)) + ((r11 * x182)) + ((r01 * x184)));
32419 new_r02 = (((r22 * x177)) + ((r12 * x182)) + ((r02 * x184)));
32420 new_r10 = (((r10 * x186)) + ((r20 * x185)) + ((r00 * x187)));
32421 new_r11 = (((r21 * x185)) + ((r11 * x186)) + ((r01 * x187)));
32422 new_r12 = (((r12 * x186)) + ((r22 * x185)) + ((r02 * x187)));
32423 new_r20 = (((r00 * x191)) + ((r20 * x188)) + ((r10 * x190)));
32424 new_r21 = (((r21 * x188)) + ((r11 * x190)) + ((r01 * x191)));
32425 new_r22 = (((r12 * x190)) + ((r22 * x188)) + ((r02 * x191)));
32426 j10eval[0] = sj11;
32427 j10eval[1] = IKsign(sj11);
32428 if (IKabs(j10eval[0]) < 0.0000010000000000
32429 || IKabs(j10eval[1]) < 0.0000010000000000)
32430 {
32431 {
32432 IkReal j10eval[1];
32433 IkReal x192 = ((1.0) * sj9);
32434 IkReal x193 = (cj9 * sj6);
32435 IkReal x194 = x160;
32436 IkReal x195 = (cj4 * sj8);
32437 IkReal x196 = (sj6 * x192);
32438 IkReal x197 = (cj6 * cj9);
32439 IkReal x198 = (((cj8 * x197)) + (((-1.0) * x196)));
32440 IkReal x199 = (((cj9 * x195)) + ((sj4 * x198)));
32441 IkReal x200 = ((1.0) * sj4 * sj8);
32442 IkReal x201 = ((((-1.0) * cj9 * x200)) + ((cj4 * x198)));
32443 IkReal x202 = (sj6 * sj8);
32444 IkReal x203 = x169;
32445 IkReal x204 = x170;
32446 IkReal x205 = ((((-1.0) * cj8 * x196)) + x197);
32447 IkReal x206 = x172;
32448 IkReal x207 = (((sj9 * x195)) + ((sj4 * x206)));
32449 IkReal x208 = (((cj4 * x206)) + (((-1.0) * sj4 * sj8 * x192)));
32450 new_r00 = (((r20 * x194)) + ((r00 * x201)) + ((r10 * x199)));
32451 new_r01 = (((r11 * x199)) + ((r01 * x201)) + ((r21 * x194)));
32452 new_r02 = (((r02 * x201)) + ((r22 * x194)) + ((r12 * x199)));
32453 new_r10 = (((r00 * x204)) + ((r10 * x203)) + ((r20 * x202)));
32454 new_r11 = (((r21 * x202)) + ((r11 * x203)) + ((r01 * x204)));
32455 new_r12 = (((r12 * x203)) + ((r22 * x202)) + ((r02 * x204)));
32456 new_r20 = (((r20 * x205)) + ((r10 * x207)) + ((r00 * x208)));
32457 new_r21 = (((r11 * x207)) + ((r01 * x208)) + ((r21 * x205)));
32458 new_r22 = (((r12 * x207)) + ((r02 * x208)) + ((r22 * x205)));
32459 j10eval[0] = sj11;
32460 if (IKabs(j10eval[0]) < 0.0000010000000000)
32461 {
32462 {
32463 IkReal evalcond[6];
32464 bool bgotonextstatement = true;
32465 do
32466 {
32467 evalcond[0]
32468 = ((-3.14159265358979)
32469 + (IKfmod(
32470 ((3.14159265358979) + (IKabs(j11))),
32471 6.28318530717959)));
32472 evalcond[1] = ((-1.0) + new_r22);
32473 evalcond[2] = new_r20;
32474 evalcond[3] = new_r02;
32475 evalcond[4] = new_r12;
32476 evalcond[5] = new_r21;
32477 if (IKabs(evalcond[0]) < 0.0000010000000000
32478 && IKabs(evalcond[1]) < 0.0000010000000000
32479 && IKabs(evalcond[2]) < 0.0000010000000000
32480 && IKabs(evalcond[3]) < 0.0000010000000000
32481 && IKabs(evalcond[4]) < 0.0000010000000000
32482 && IKabs(evalcond[5]) < 0.0000010000000000)
32483 {
32484 bgotonextstatement = false;
32485 {
32486 IkReal j10array[2], cj10array[2], sj10array[2];
32487 bool j10valid[2] = {false};
32488 _nj10 = 2;
32489 CheckValue<IkReal> x210 = IKatan2WithCheck(
32490 IkReal(new_r02), new_r12, IKFAST_ATAN2_MAGTHRESH);
32491 if (!x210.valid)
32492 {
32493 continue;
32494 }
32495 IkReal x209 = ((-1.0) * (((1.0) * (x210.value))));
32496 j10array[0] = x209;
32497 sj10array[0] = IKsin(j10array[0]);
32498 cj10array[0] = IKcos(j10array[0]);
32499 j10array[1] = ((3.14159265358979) + x209);
32500 sj10array[1] = IKsin(j10array[1]);
32501 cj10array[1] = IKcos(j10array[1]);
32502 if (j10array[0] > IKPI)
32503 {
32504 j10array[0] -= IK2PI;
32505 }
32506 else if (j10array[0] < -IKPI)
32507 {
32508 j10array[0] += IK2PI;
32509 }
32510 j10valid[0] = true;
32511 if (j10array[1] > IKPI)
32512 {
32513 j10array[1] -= IK2PI;
32514 }
32515 else if (j10array[1] < -IKPI)
32516 {
32517 j10array[1] += IK2PI;
32518 }
32519 j10valid[1] = true;
32520 for (int ij10 = 0; ij10 < 2; ++ij10)
32521 {
32522 if (!j10valid[ij10])
32523 {
32524 continue;
32525 }
32526 _ij10[0] = ij10;
32527 _ij10[1] = -1;
32528 for (int iij10 = ij10 + 1; iij10 < 2; ++iij10)
32529 {
32530 if (j10valid[iij10]
32531 && IKabs(cj10array[ij10] - cj10array[iij10])
32532 < IKFAST_SOLUTION_THRESH
32533 && IKabs(sj10array[ij10] - sj10array[iij10])
32534 < IKFAST_SOLUTION_THRESH)
32535 {
32536 j10valid[iij10] = false;
32537 _ij10[1] = iij10;
32538 break;
32539 }
32540 }
32541 j10 = j10array[ij10];
32542 cj10 = cj10array[ij10];
32543 sj10 = sj10array[ij10];
32544 {
32545 IkReal evalcond[1];
32546 evalcond[0]
32547 = (((new_r12 * (IKcos(j10))))
32548 + (((-1.0) * (1.0) * new_r02
32549 * (IKsin(j10)))));
32550 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
32551 {
32552 continue;
32553 }
32554 }
32555
32556 {
32557 IkReal j12array[1], cj12array[1], sj12array[1];
32558 bool j12valid[1] = {false};
32559 _nj12 = 1;
32560 IkReal x211 = ((1.0) * new_r01);
32561 if (IKabs(
32562 ((((-1.0) * cj10 * x211))
32563 + (((-1.0) * (1.0) * new_r00 * sj10))))
32564 < IKFAST_ATAN2_MAGTHRESH
32565 && IKabs(
32566 ((((-1.0) * sj10 * x211))
32567 + ((cj10 * new_r00))))
32568 < IKFAST_ATAN2_MAGTHRESH
32569 && IKabs(
32570 IKsqr(
32571 ((((-1.0) * cj10 * x211))
32572 + (((-1.0) * (1.0) * new_r00
32573 * sj10))))
32574 + IKsqr(
32575 ((((-1.0) * sj10 * x211))
32576 + ((cj10 * new_r00))))
32577 - 1)
32578 <= IKFAST_SINCOS_THRESH)
32579 continue;
32580 j12array[0] = IKatan2(
32581 ((((-1.0) * cj10 * x211))
32582 + (((-1.0) * (1.0) * new_r00 * sj10))),
32583 ((((-1.0) * sj10 * x211))
32584 + ((cj10 * new_r00))));
32585 sj12array[0] = IKsin(j12array[0]);
32586 cj12array[0] = IKcos(j12array[0]);
32587 if (j12array[0] > IKPI)
32588 {
32589 j12array[0] -= IK2PI;
32590 }
32591 else if (j12array[0] < -IKPI)
32592 {
32593 j12array[0] += IK2PI;
32594 }
32595 j12valid[0] = true;
32596 for (int ij12 = 0; ij12 < 1; ++ij12)
32597 {
32598 if (!j12valid[ij12])
32599 {
32600 continue;
32601 }
32602 _ij12[0] = ij12;
32603 _ij12[1] = -1;
32604 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
32605 {
32606 if (j12valid[iij12]
32607 && IKabs(
32608 cj12array[ij12] - cj12array[iij12])
32609 < IKFAST_SOLUTION_THRESH
32610 && IKabs(
32611 sj12array[ij12] - sj12array[iij12])
32612 < IKFAST_SOLUTION_THRESH)
32613 {
32614 j12valid[iij12] = false;
32615 _ij12[1] = iij12;
32616 break;
32617 }
32618 }
32619 j12 = j12array[ij12];
32620 cj12 = cj12array[ij12];
32621 sj12 = sj12array[ij12];
32622 {
32623 IkReal evalcond[8];
32624 IkReal x212 = IKsin(j12);
32625 IkReal x213 = (cj10 * x212);
32626 IkReal x214 = IKcos(j12);
32627 IkReal x215 = ((1.0) * x214);
32628 IkReal x216 = ((-1.0) * x215);
32629 IkReal x217 = ((1.0) * sj10);
32630 IkReal x218
32631 = ((((-1.0) * cj10 * x215))
32632 + ((sj10 * x212)));
32633 evalcond[0]
32634 = (((cj10 * new_r01)) + x212
32635 + ((new_r11 * sj10)));
32636 evalcond[1]
32637 = (((sj10 * x214)) + x213 + new_r01);
32638 evalcond[2]
32639 = (((cj10 * new_r00)) + ((new_r10 * sj10))
32640 + x216);
32641 evalcond[3]
32642 = ((((-1.0) * new_r00 * x217))
32643 + (((-1.0) * x212))
32644 + ((cj10 * new_r10)));
32645 evalcond[4]
32646 = (((cj10 * new_r11)) + x216
32647 + (((-1.0) * new_r01 * x217)));
32648 evalcond[5] = (x218 + new_r00);
32649 evalcond[6] = (x218 + new_r11);
32650 evalcond[7]
32651 = ((((-1.0) * x213))
32652 + (((-1.0) * x214 * x217)) + new_r10);
32653 if (IKabs(evalcond[0])
32654 > IKFAST_EVALCOND_THRESH
32655 || IKabs(evalcond[1])
32656 > IKFAST_EVALCOND_THRESH
32657 || IKabs(evalcond[2])
32658 > IKFAST_EVALCOND_THRESH
32659 || IKabs(evalcond[3])
32660 > IKFAST_EVALCOND_THRESH
32661 || IKabs(evalcond[4])
32662 > IKFAST_EVALCOND_THRESH
32663 || IKabs(evalcond[5])
32664 > IKFAST_EVALCOND_THRESH
32665 || IKabs(evalcond[6])
32666 > IKFAST_EVALCOND_THRESH
32667 || IKabs(evalcond[7])
32668 > IKFAST_EVALCOND_THRESH)
32669 {
32670 continue;
32671 }
32672 }
32673
32674 {
32675 std::vector<IkSingleDOFSolutionBase<IkReal> >
32676 vinfos(7);
32677 vinfos[0].jointtype = 1;
32678 vinfos[0].foffset = j4;
32679 vinfos[0].indices[0] = _ij4[0];
32680 vinfos[0].indices[1] = _ij4[1];
32681 vinfos[0].maxsolutions = _nj4;
32682 vinfos[1].jointtype = 1;
32683 vinfos[1].foffset = j6;
32684 vinfos[1].indices[0] = _ij6[0];
32685 vinfos[1].indices[1] = _ij6[1];
32686 vinfos[1].maxsolutions = _nj6;
32687 vinfos[2].jointtype = 1;
32688 vinfos[2].foffset = j8;
32689 vinfos[2].indices[0] = _ij8[0];
32690 vinfos[2].indices[1] = _ij8[1];
32691 vinfos[2].maxsolutions = _nj8;
32692 vinfos[3].jointtype = 1;
32693 vinfos[3].foffset = j9;
32694 vinfos[3].indices[0] = _ij9[0];
32695 vinfos[3].indices[1] = _ij9[1];
32696 vinfos[3].maxsolutions = _nj9;
32697 vinfos[4].jointtype = 1;
32698 vinfos[4].foffset = j10;
32699 vinfos[4].indices[0] = _ij10[0];
32700 vinfos[4].indices[1] = _ij10[1];
32701 vinfos[4].maxsolutions = _nj10;
32702 vinfos[5].jointtype = 1;
32703 vinfos[5].foffset = j11;
32704 vinfos[5].indices[0] = _ij11[0];
32705 vinfos[5].indices[1] = _ij11[1];
32706 vinfos[5].maxsolutions = _nj11;
32707 vinfos[6].jointtype = 1;
32708 vinfos[6].foffset = j12;
32709 vinfos[6].indices[0] = _ij12[0];
32710 vinfos[6].indices[1] = _ij12[1];
32711 vinfos[6].maxsolutions = _nj12;
32712 std::vector<int> vfree(0);
32713 solutions.AddSolution(vinfos, vfree);
32714 }
32715 }
32716 }
32717 }
32718 }
32719 }
32720 } while (0);
32721 if (bgotonextstatement)
32722 {
32723 bool bgotonextstatement = true;
32724 do
32725 {
32726 evalcond[0]
32727 = ((-3.14159265358979)
32728 + (IKfmod(
32729 ((3.14159265358979)
32730 + (IKabs(((-3.14159265358979) + j11)))),
32731 6.28318530717959)));
32732 evalcond[1] = ((1.0) + new_r22);
32733 evalcond[2] = new_r20;
32734 evalcond[3] = new_r02;
32735 evalcond[4] = new_r12;
32736 evalcond[5] = new_r21;
32737 if (IKabs(evalcond[0]) < 0.0000010000000000
32738 && IKabs(evalcond[1]) < 0.0000010000000000
32739 && IKabs(evalcond[2]) < 0.0000010000000000
32740 && IKabs(evalcond[3]) < 0.0000010000000000
32741 && IKabs(evalcond[4]) < 0.0000010000000000
32742 && IKabs(evalcond[5]) < 0.0000010000000000)
32743 {
32744 bgotonextstatement = false;
32745 {
32746 IkReal j10array[2], cj10array[2], sj10array[2];
32747 bool j10valid[2] = {false};
32748 _nj10 = 2;
32749 CheckValue<IkReal> x220 = IKatan2WithCheck(
32750 IkReal(new_r02),
32751 new_r12,
32752 IKFAST_ATAN2_MAGTHRESH);
32753 if (!x220.valid)
32754 {
32755 continue;
32756 }
32757 IkReal x219 = ((-1.0) * (((1.0) * (x220.value))));
32758 j10array[0] = x219;
32759 sj10array[0] = IKsin(j10array[0]);
32760 cj10array[0] = IKcos(j10array[0]);
32761 j10array[1] = ((3.14159265358979) + x219);
32762 sj10array[1] = IKsin(j10array[1]);
32763 cj10array[1] = IKcos(j10array[1]);
32764 if (j10array[0] > IKPI)
32765 {
32766 j10array[0] -= IK2PI;
32767 }
32768 else if (j10array[0] < -IKPI)
32769 {
32770 j10array[0] += IK2PI;
32771 }
32772 j10valid[0] = true;
32773 if (j10array[1] > IKPI)
32774 {
32775 j10array[1] -= IK2PI;
32776 }
32777 else if (j10array[1] < -IKPI)
32778 {
32779 j10array[1] += IK2PI;
32780 }
32781 j10valid[1] = true;
32782 for (int ij10 = 0; ij10 < 2; ++ij10)
32783 {
32784 if (!j10valid[ij10])
32785 {
32786 continue;
32787 }
32788 _ij10[0] = ij10;
32789 _ij10[1] = -1;
32790 for (int iij10 = ij10 + 1; iij10 < 2; ++iij10)
32791 {
32792 if (j10valid[iij10]
32793 && IKabs(cj10array[ij10] - cj10array[iij10])
32794 < IKFAST_SOLUTION_THRESH
32795 && IKabs(sj10array[ij10] - sj10array[iij10])
32796 < IKFAST_SOLUTION_THRESH)
32797 {
32798 j10valid[iij10] = false;
32799 _ij10[1] = iij10;
32800 break;
32801 }
32802 }
32803 j10 = j10array[ij10];
32804 cj10 = cj10array[ij10];
32805 sj10 = sj10array[ij10];
32806 {
32807 IkReal evalcond[1];
32808 evalcond[0]
32809 = (((new_r12 * (IKcos(j10))))
32810 + (((-1.0) * (1.0) * new_r02
32811 * (IKsin(j10)))));
32812 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH)
32813 {
32814 continue;
32815 }
32816 }
32817
32818 {
32819 IkReal j12array[1], cj12array[1], sj12array[1];
32820 bool j12valid[1] = {false};
32821 _nj12 = 1;
32822 IkReal x221 = ((1.0) * new_r00);
32823 if (IKabs(
32824 ((((-1.0) * sj10 * x221))
32825 + ((cj10 * new_r01))))
32826 < IKFAST_ATAN2_MAGTHRESH
32827 && IKabs(
32828 ((((-1.0) * cj10 * x221))
32829 + (((-1.0) * (1.0) * new_r01
32830 * sj10))))
32831 < IKFAST_ATAN2_MAGTHRESH
32832 && IKabs(
32833 IKsqr(
32834 ((((-1.0) * sj10 * x221))
32835 + ((cj10 * new_r01))))
32836 + IKsqr(
32837 ((((-1.0) * cj10 * x221))
32838 + (((-1.0) * (1.0) * new_r01
32839 * sj10))))
32840 - 1)
32841 <= IKFAST_SINCOS_THRESH)
32842 continue;
32843 j12array[0] = IKatan2(
32844 ((((-1.0) * sj10 * x221))
32845 + ((cj10 * new_r01))),
32846 ((((-1.0) * cj10 * x221))
32847 + (((-1.0) * (1.0) * new_r01 * sj10))));
32848 sj12array[0] = IKsin(j12array[0]);
32849 cj12array[0] = IKcos(j12array[0]);
32850 if (j12array[0] > IKPI)
32851 {
32852 j12array[0] -= IK2PI;
32853 }
32854 else if (j12array[0] < -IKPI)
32855 {
32856 j12array[0] += IK2PI;
32857 }
32858 j12valid[0] = true;
32859 for (int ij12 = 0; ij12 < 1; ++ij12)
32860 {
32861 if (!j12valid[ij12])
32862 {
32863 continue;
32864 }
32865 _ij12[0] = ij12;
32866 _ij12[1] = -1;
32867 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
32868 {
32869 if (j12valid[iij12]
32870 && IKabs(
32871 cj12array[ij12]
32872 - cj12array[iij12])
32873 < IKFAST_SOLUTION_THRESH
32874 && IKabs(
32875 sj12array[ij12]
32876 - sj12array[iij12])
32877 < IKFAST_SOLUTION_THRESH)
32878 {
32879 j12valid[iij12] = false;
32880 _ij12[1] = iij12;
32881 break;
32882 }
32883 }
32884 j12 = j12array[ij12];
32885 cj12 = cj12array[ij12];
32886 sj12 = sj12array[ij12];
32887 {
32888 IkReal evalcond[8];
32889 IkReal x222 = IKcos(j12);
32890 IkReal x223 = IKsin(j12);
32891 IkReal x224 = ((1.0) * x223);
32892 IkReal x225 = ((-1.0) * x224);
32893 IkReal x226 = (cj10 * x222);
32894 IkReal x227 = ((1.0) * sj10);
32895 IkReal x228
32896 = (((sj10 * x222))
32897 + (((-1.0) * cj10 * x224)));
32898 evalcond[0]
32899 = (((cj10 * new_r00))
32900 + ((new_r10 * sj10)) + x222);
32901 evalcond[1]
32902 = (((cj10 * new_r01))
32903 + ((new_r11 * sj10)) + x225);
32904 evalcond[2]
32905 = (((sj10 * x223)) + new_r00 + x226);
32906 evalcond[3]
32907 = (((cj10 * new_r10))
32908 + (((-1.0) * new_r00 * x227))
32909 + x225);
32910 evalcond[4]
32911 = (((cj10 * new_r11))
32912 + (((-1.0) * x222))
32913 + (((-1.0) * new_r01 * x227)));
32914 evalcond[5] = (new_r01 + x228);
32915 evalcond[6] = (new_r10 + x228);
32916 evalcond[7]
32917 = ((((-1.0) * x226)) + new_r11
32918 + (((-1.0) * x223 * x227)));
32919 if (IKabs(evalcond[0])
32920 > IKFAST_EVALCOND_THRESH
32921 || IKabs(evalcond[1])
32922 > IKFAST_EVALCOND_THRESH
32923 || IKabs(evalcond[2])
32924 > IKFAST_EVALCOND_THRESH
32925 || IKabs(evalcond[3])
32926 > IKFAST_EVALCOND_THRESH
32927 || IKabs(evalcond[4])
32928 > IKFAST_EVALCOND_THRESH
32929 || IKabs(evalcond[5])
32930 > IKFAST_EVALCOND_THRESH
32931 || IKabs(evalcond[6])
32932 > IKFAST_EVALCOND_THRESH
32933 || IKabs(evalcond[7])
32934 > IKFAST_EVALCOND_THRESH)
32935 {
32936 continue;
32937 }
32938 }
32939
32940 {
32941 std::vector<
32942 IkSingleDOFSolutionBase<IkReal> >
32943 vinfos(7);
32944 vinfos[0].jointtype = 1;
32945 vinfos[0].foffset = j4;
32946 vinfos[0].indices[0] = _ij4[0];
32947 vinfos[0].indices[1] = _ij4[1];
32948 vinfos[0].maxsolutions = _nj4;
32949 vinfos[1].jointtype = 1;
32950 vinfos[1].foffset = j6;
32951 vinfos[1].indices[0] = _ij6[0];
32952 vinfos[1].indices[1] = _ij6[1];
32953 vinfos[1].maxsolutions = _nj6;
32954 vinfos[2].jointtype = 1;
32955 vinfos[2].foffset = j8;
32956 vinfos[2].indices[0] = _ij8[0];
32957 vinfos[2].indices[1] = _ij8[1];
32958 vinfos[2].maxsolutions = _nj8;
32959 vinfos[3].jointtype = 1;
32960 vinfos[3].foffset = j9;
32961 vinfos[3].indices[0] = _ij9[0];
32962 vinfos[3].indices[1] = _ij9[1];
32963 vinfos[3].maxsolutions = _nj9;
32964 vinfos[4].jointtype = 1;
32965 vinfos[4].foffset = j10;
32966 vinfos[4].indices[0] = _ij10[0];
32967 vinfos[4].indices[1] = _ij10[1];
32968 vinfos[4].maxsolutions = _nj10;
32969 vinfos[5].jointtype = 1;
32970 vinfos[5].foffset = j11;
32971 vinfos[5].indices[0] = _ij11[0];
32972 vinfos[5].indices[1] = _ij11[1];
32973 vinfos[5].maxsolutions = _nj11;
32974 vinfos[6].jointtype = 1;
32975 vinfos[6].foffset = j12;
32976 vinfos[6].indices[0] = _ij12[0];
32977 vinfos[6].indices[1] = _ij12[1];
32978 vinfos[6].maxsolutions = _nj12;
32979 std::vector<int> vfree(0);
32980 solutions.AddSolution(vinfos, vfree);
32981 }
32982 }
32983 }
32984 }
32985 }
32986 }
32987 } while (0);
32988 if (bgotonextstatement)
32989 {
32990 bool bgotonextstatement = true;
32991 do
32992 {
32993 if (1)
32994 {
32995 bgotonextstatement = false;
32996 continue; // branch miss [j10, j12]
32997 }
32998 } while (0);
32999 if (bgotonextstatement)
33000 {
33001 }
33002 }
33003 }
33004 }
33005 }
33006 else
33007 {
33008 {
33009 IkReal j10array[1], cj10array[1], sj10array[1];
33010 bool j10valid[1] = {false};
33011 _nj10 = 1;
33012 CheckValue<IkReal> x230 = IKPowWithIntegerCheck(sj11, -1);
33013 if (!x230.valid)
33014 {
33015 continue;
33016 }
33017 IkReal x229 = x230.value;
33018 CheckValue<IkReal> x231
33019 = IKPowWithIntegerCheck(new_r12, -1);
33020 if (!x231.valid)
33021 {
33022 continue;
33023 }
33024 if (IKabs(
33025 (x229 * (x231.value)
33026 * (((1.0) + (((-1.0) * (1.0) * (cj11 * cj11)))
33027 + (((-1.0) * (1.0) * (new_r02 * new_r02)))))))
33028 < IKFAST_ATAN2_MAGTHRESH
33029 && IKabs((new_r02 * x229)) < IKFAST_ATAN2_MAGTHRESH
33030 && IKabs(
33031 IKsqr(
33032 (x229 * (x231.value)
33033 * (((1.0)
33034 + (((-1.0) * (1.0) * (cj11 * cj11)))
33035 + (((-1.0) * (1.0)
33036 * (new_r02 * new_r02)))))))
33037 + IKsqr((new_r02 * x229)) - 1)
33038 <= IKFAST_SINCOS_THRESH)
33039 continue;
33040 j10array[0] = IKatan2(
33041 (x229 * (x231.value)
33042 * (((1.0) + (((-1.0) * (1.0) * (cj11 * cj11)))
33043 + (((-1.0) * (1.0) * (new_r02 * new_r02)))))),
33044 (new_r02 * x229));
33045 sj10array[0] = IKsin(j10array[0]);
33046 cj10array[0] = IKcos(j10array[0]);
33047 if (j10array[0] > IKPI)
33048 {
33049 j10array[0] -= IK2PI;
33050 }
33051 else if (j10array[0] < -IKPI)
33052 {
33053 j10array[0] += IK2PI;
33054 }
33055 j10valid[0] = true;
33056 for (int ij10 = 0; ij10 < 1; ++ij10)
33057 {
33058 if (!j10valid[ij10])
33059 {
33060 continue;
33061 }
33062 _ij10[0] = ij10;
33063 _ij10[1] = -1;
33064 for (int iij10 = ij10 + 1; iij10 < 1; ++iij10)
33065 {
33066 if (j10valid[iij10]
33067 && IKabs(cj10array[ij10] - cj10array[iij10])
33068 < IKFAST_SOLUTION_THRESH
33069 && IKabs(sj10array[ij10] - sj10array[iij10])
33070 < IKFAST_SOLUTION_THRESH)
33071 {
33072 j10valid[iij10] = false;
33073 _ij10[1] = iij10;
33074 break;
33075 }
33076 }
33077 j10 = j10array[ij10];
33078 cj10 = cj10array[ij10];
33079 sj10 = sj10array[ij10];
33080 {
33081 IkReal evalcond[8];
33082 IkReal x232 = IKcos(j10);
33083 IkReal x233 = ((1.0) * sj11);
33084 IkReal x234 = (x232 * x233);
33085 IkReal x235 = IKsin(j10);
33086 IkReal x236 = (x233 * x235);
33087 IkReal x237 = (new_r02 * x232);
33088 IkReal x238 = (new_r12 * x235);
33089 IkReal x239 = ((1.0) * cj11);
33090 evalcond[0] = ((((-1.0) * x234)) + new_r02);
33091 evalcond[1] = (new_r12 + (((-1.0) * x236)));
33092 evalcond[2]
33093 = (((new_r12 * x232))
33094 + (((-1.0) * new_r02 * x235)));
33095 evalcond[3] = ((((-1.0) * x233)) + x237 + x238);
33096 evalcond[4]
33097 = (((cj11 * x238)) + (((-1.0) * new_r22 * x233))
33098 + ((cj11 * x237)));
33099 evalcond[5]
33100 = ((((-1.0) * new_r00 * x234))
33101 + (((-1.0) * new_r10 * x236))
33102 + (((-1.0) * new_r20 * x239)));
33103 evalcond[6]
33104 = ((((-1.0) * new_r01 * x234))
33105 + (((-1.0) * new_r21 * x239))
33106 + (((-1.0) * new_r11 * x236)));
33107 evalcond[7]
33108 = ((1.0) + (((-1.0) * new_r22 * x239))
33109 + (((-1.0) * x233 * x238))
33110 + (((-1.0) * x233 * x237)));
33111 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
33112 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
33113 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
33114 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
33115 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
33116 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
33117 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
33118 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
33119 {
33120 continue;
33121 }
33122 }
33123
33124 {
33125 IkReal j12eval[2];
33126 IkReal x240 = ((1.0) * sj9);
33127 IkReal x241 = (cj9 * sj6);
33128 IkReal x242 = x160;
33129 IkReal x243 = (cj4 * sj8);
33130 IkReal x244 = (sj6 * x240);
33131 IkReal x245 = (cj6 * cj9);
33132 IkReal x246 = (((cj8 * x245)) + (((-1.0) * x244)));
33133 IkReal x247 = (((cj9 * x243)) + ((sj4 * x246)));
33134 IkReal x248 = ((1.0) * sj4 * sj8);
33135 IkReal x249
33136 = ((((-1.0) * cj9 * x248)) + ((cj4 * x246)));
33137 IkReal x250 = (sj6 * sj8);
33138 IkReal x251 = x169;
33139 IkReal x252 = x170;
33140 IkReal x253 = (x245 + (((-1.0) * cj8 * x244)));
33141 IkReal x254 = x172;
33142 IkReal x255 = (((sj9 * x243)) + ((sj4 * x254)));
33143 IkReal x256
33144 = (((cj4 * x254)) + (((-1.0) * sj4 * sj8 * x240)));
33145 new_r00
33146 = (((r20 * x242)) + ((r00 * x249))
33147 + ((r10 * x247)));
33148 new_r01
33149 = (((r21 * x242)) + ((r01 * x249))
33150 + ((r11 * x247)));
33151 new_r02
33152 = (((r22 * x242)) + ((r12 * x247))
33153 + ((r02 * x249)));
33154 new_r10
33155 = (((r10 * x251)) + ((r20 * x250))
33156 + ((r00 * x252)));
33157 new_r11
33158 = (((r11 * x251)) + ((r21 * x250))
33159 + ((r01 * x252)));
33160 new_r12
33161 = (((r22 * x250)) + ((r12 * x251))
33162 + ((r02 * x252)));
33163 new_r20
33164 = (((r20 * x253)) + ((r00 * x256))
33165 + ((r10 * x255)));
33166 new_r21
33167 = (((r21 * x253)) + ((r11 * x255))
33168 + ((r01 * x256)));
33169 new_r22
33170 = (((r02 * x256)) + ((r12 * x255))
33171 + ((r22 * x253)));
33172 j12eval[0] = sj11;
33173 j12eval[1] = IKsign(sj11);
33174 if (IKabs(j12eval[0]) < 0.0000010000000000
33175 || IKabs(j12eval[1]) < 0.0000010000000000)
33176 {
33177 {
33178 IkReal j12eval[2];
33179 IkReal x257 = ((1.0) * sj9);
33180 IkReal x258 = (cj9 * sj6);
33181 IkReal x259 = x160;
33182 IkReal x260 = (cj4 * sj8);
33183 IkReal x261 = (sj6 * x257);
33184 IkReal x262 = (cj6 * cj9);
33185 IkReal x263 = (((cj8 * x262)) + (((-1.0) * x261)));
33186 IkReal x264 = (((sj4 * x263)) + ((cj9 * x260)));
33187 IkReal x265 = ((1.0) * sj4 * sj8);
33188 IkReal x266
33189 = ((((-1.0) * cj9 * x265)) + ((cj4 * x263)));
33190 IkReal x267 = (sj6 * sj8);
33191 IkReal x268 = x169;
33192 IkReal x269 = x170;
33193 IkReal x270 = (x262 + (((-1.0) * cj8 * x261)));
33194 IkReal x271 = x172;
33195 IkReal x272 = (((sj4 * x271)) + ((sj9 * x260)));
33196 IkReal x273
33197 = (((cj4 * x271))
33198 + (((-1.0) * sj4 * sj8 * x257)));
33199 new_r00
33200 = (((r00 * x266)) + ((r20 * x259))
33201 + ((r10 * x264)));
33202 new_r01
33203 = (((r21 * x259)) + ((r01 * x266))
33204 + ((r11 * x264)));
33205 new_r02
33206 = (((r02 * x266)) + ((r12 * x264))
33207 + ((r22 * x259)));
33208 new_r10
33209 = (((r20 * x267)) + ((r00 * x269))
33210 + ((r10 * x268)));
33211 new_r11
33212 = (((r01 * x269)) + ((r21 * x267))
33213 + ((r11 * x268)));
33214 new_r12
33215 = (((r22 * x267)) + ((r12 * x268))
33216 + ((r02 * x269)));
33217 new_r20
33218 = (((r00 * x273)) + ((r10 * x272))
33219 + ((r20 * x270)));
33220 new_r21
33221 = (((r11 * x272)) + ((r01 * x273))
33222 + ((r21 * x270)));
33223 new_r22
33224 = (((r12 * x272)) + ((r02 * x273))
33225 + ((r22 * x270)));
33226 j12eval[0] = sj10;
33227 j12eval[1] = sj11;
33228 if (IKabs(j12eval[0]) < 0.0000010000000000
33229 || IKabs(j12eval[1]) < 0.0000010000000000)
33230 {
33231 {
33232 IkReal j12eval[3];
33233 IkReal x274 = ((1.0) * sj9);
33234 IkReal x275 = (cj9 * sj6);
33235 IkReal x276 = x160;
33236 IkReal x277 = (cj4 * sj8);
33237 IkReal x278 = (sj6 * x274);
33238 IkReal x279 = (cj6 * cj9);
33239 IkReal x280
33240 = (((cj8 * x279)) + (((-1.0) * x278)));
33241 IkReal x281 = (((cj9 * x277)) + ((sj4 * x280)));
33242 IkReal x282 = ((1.0) * sj4 * sj8);
33243 IkReal x283
33244 = (((cj4 * x280))
33245 + (((-1.0) * cj9 * x282)));
33246 IkReal x284 = (sj6 * sj8);
33247 IkReal x285 = x169;
33248 IkReal x286 = x170;
33249 IkReal x287 = ((((-1.0) * cj8 * x278)) + x279);
33250 IkReal x288 = x172;
33251 IkReal x289 = (((sj9 * x277)) + ((sj4 * x288)));
33252 IkReal x290
33253 = (((cj4 * x288))
33254 + (((-1.0) * sj4 * sj8 * x274)));
33255 new_r00
33256 = (((r00 * x283)) + ((r10 * x281))
33257 + ((r20 * x276)));
33258 new_r01
33259 = (((r01 * x283)) + ((r11 * x281))
33260 + ((r21 * x276)));
33261 new_r02
33262 = (((r22 * x276)) + ((r02 * x283))
33263 + ((r12 * x281)));
33264 new_r10
33265 = (((r10 * x285)) + ((r00 * x286))
33266 + ((r20 * x284)));
33267 new_r11
33268 = (((r01 * x286)) + ((r21 * x284))
33269 + ((r11 * x285)));
33270 new_r12
33271 = (((r22 * x284)) + ((r02 * x286))
33272 + ((r12 * x285)));
33273 new_r20
33274 = (((r00 * x290)) + ((r20 * x287))
33275 + ((r10 * x289)));
33276 new_r21
33277 = (((r21 * x287)) + ((r11 * x289))
33278 + ((r01 * x290)));
33279 new_r22
33280 = (((r22 * x287)) + ((r12 * x289))
33281 + ((r02 * x290)));
33282 j12eval[0] = cj10;
33283 j12eval[1] = cj11;
33284 j12eval[2] = sj11;
33285 if (IKabs(j12eval[0]) < 0.0000010000000000
33286 || IKabs(j12eval[1]) < 0.0000010000000000
33287 || IKabs(j12eval[2]) < 0.0000010000000000)
33288 {
33289 {
33290 IkReal evalcond[12];
33291 bool bgotonextstatement = true;
33292 do
33293 {
33294 IkReal x291 = ((1.0) * cj11);
33295 IkReal x292
33296 = ((((-1.0) * x291)) + new_r22);
33297 IkReal x293 = ((1.0) * sj11);
33298 IkReal x294
33299 = ((((-1.0) * x293)) + new_r12);
33300 evalcond[0]
33301 = ((-3.14159265358979)
33302 + (IKfmod(
33303 ((3.14159265358979)
33304 + (IKabs(
33305 ((-1.5707963267949)
33306 + j10)))),
33307 6.28318530717959)));
33308 evalcond[1] = x292;
33309 evalcond[2] = x292;
33310 evalcond[3] = new_r02;
33311 evalcond[4] = x294;
33312 evalcond[5] = x294;
33313 evalcond[6]
33314 = ((((-1.0) * new_r22 * x293))
33315 + ((cj11 * new_r12)));
33316 evalcond[7]
33317 = ((((-1.0) * new_r10 * x293))
33318 + (((-1.0) * new_r20 * x291)));
33319 evalcond[8]
33320 = ((((-1.0) * new_r11 * x293))
33321 + (((-1.0) * new_r21 * x291)));
33322 evalcond[9]
33323 = ((1.0) + (((-1.0) * new_r12 * x293))
33324 + (((-1.0) * new_r22 * x291)));
33325 if (IKabs(evalcond[0])
33326 < 0.0000010000000000
33327 && IKabs(evalcond[1])
33328 < 0.0000010000000000
33329 && IKabs(evalcond[2])
33330 < 0.0000010000000000
33331 && IKabs(evalcond[3])
33332 < 0.0000010000000000
33333 && IKabs(evalcond[4])
33334 < 0.0000010000000000
33335 && IKabs(evalcond[5])
33336 < 0.0000010000000000
33337 && IKabs(evalcond[6])
33338 < 0.0000010000000000
33339 && IKabs(evalcond[7])
33340 < 0.0000010000000000
33341 && IKabs(evalcond[8])
33342 < 0.0000010000000000
33343 && IKabs(evalcond[9])
33344 < 0.0000010000000000)
33345 {
33346 bgotonextstatement = false;
33347 {
33348 IkReal j12array[1], cj12array[1],
33349 sj12array[1];
33350 bool j12valid[1] = {false};
33351 _nj12 = 1;
33352 CheckValue<IkReal> x295
33353 = IKPowWithIntegerCheck(
33354 IKsign(new_r12), -1);
33355 if (!x295.valid)
33356 {
33357 continue;
33358 }
33359 CheckValue<IkReal> x296
33360 = IKatan2WithCheck(
33361 IkReal(new_r21),
33362 ((-1.0)
33363 * (((1.0) * new_r20))),
33364 IKFAST_ATAN2_MAGTHRESH);
33365 if (!x296.valid)
33366 {
33367 continue;
33368 }
33369 j12array[0]
33370 = ((-1.5707963267949)
33371 + (((1.5707963267949)
33372 * (x295.value)))
33373 + (x296.value));
33374 sj12array[0] = IKsin(j12array[0]);
33375 cj12array[0] = IKcos(j12array[0]);
33376 if (j12array[0] > IKPI)
33377 {
33378 j12array[0] -= IK2PI;
33379 }
33380 else if (j12array[0] < -IKPI)
33381 {
33382 j12array[0] += IK2PI;
33383 }
33384 j12valid[0] = true;
33385 for (int ij12 = 0; ij12 < 1; ++ij12)
33386 {
33387 if (!j12valid[ij12])
33388 {
33389 continue;
33390 }
33391 _ij12[0] = ij12;
33392 _ij12[1] = -1;
33393 for (int iij12 = ij12 + 1;
33394 iij12 < 1;
33395 ++iij12)
33396 {
33397 if (j12valid[iij12]
33398 && IKabs(
33399 cj12array[ij12]
33400 - cj12array[iij12])
33401 < IKFAST_SOLUTION_THRESH
33402 && IKabs(
33403 sj12array[ij12]
33404 - sj12array[iij12])
33405 < IKFAST_SOLUTION_THRESH)
33406 {
33407 j12valid[iij12] = false;
33408 _ij12[1] = iij12;
33409 break;
33410 }
33411 }
33412 j12 = j12array[ij12];
33413 cj12 = cj12array[ij12];
33414 sj12 = sj12array[ij12];
33415 {
33416 IkReal evalcond[8];
33417 IkReal x297 = IKcos(j12);
33418 IkReal x298 = IKsin(j12);
33419 IkReal x299 = ((1.0) * new_r12);
33420 IkReal x300 = ((1.0) * x297);
33421 IkReal x301 = ((-1.0) * x300);
33422 evalcond[0]
33423 = (((new_r12 * x297))
33424 + new_r20);
33425 evalcond[1]
33426 = (((new_r22 * x298))
33427 + new_r11);
33428 evalcond[2]
33429 = ((((-1.0) * x298 * x299))
33430 + new_r21);
33431 evalcond[3]
33432 = ((((-1.0) * new_r22 * x300))
33433 + new_r10);
33434 evalcond[4]
33435 = ((((-1.0) * (1.0)
33436 * new_r00))
33437 + (((-1.0) * x298)));
33438 evalcond[5]
33439 = (x301
33440 + (((-1.0) * (1.0)
33441 * new_r01)));
33442 evalcond[6]
33443 = (((new_r11 * new_r22))
33444 + x298
33445 + (((-1.0) * new_r21
33446 * x299)));
33447 evalcond[7]
33448 = ((((-1.0) * new_r20 * x299))
33449 + x301
33450 + ((new_r10 * new_r22)));
33451 if (IKabs(evalcond[0])
33452 > IKFAST_EVALCOND_THRESH
33453 || IKabs(evalcond[1])
33454 > IKFAST_EVALCOND_THRESH
33455 || IKabs(evalcond[2])
33456 > IKFAST_EVALCOND_THRESH
33457 || IKabs(evalcond[3])
33458 > IKFAST_EVALCOND_THRESH
33459 || IKabs(evalcond[4])
33460 > IKFAST_EVALCOND_THRESH
33461 || IKabs(evalcond[5])
33462 > IKFAST_EVALCOND_THRESH
33463 || IKabs(evalcond[6])
33464 > IKFAST_EVALCOND_THRESH
33465 || IKabs(evalcond[7])
33466 > IKFAST_EVALCOND_THRESH)
33467 {
33468 continue;
33469 }
33470 }
33471
33472 {
33473 std::vector<
33474 IkSingleDOFSolutionBase<
33475 IkReal> >
33476 vinfos(7);
33477 vinfos[0].jointtype = 1;
33478 vinfos[0].foffset = j4;
33479 vinfos[0].indices[0] = _ij4[0];
33480 vinfos[0].indices[1] = _ij4[1];
33481 vinfos[0].maxsolutions = _nj4;
33482 vinfos[1].jointtype = 1;
33483 vinfos[1].foffset = j6;
33484 vinfos[1].indices[0] = _ij6[0];
33485 vinfos[1].indices[1] = _ij6[1];
33486 vinfos[1].maxsolutions = _nj6;
33487 vinfos[2].jointtype = 1;
33488 vinfos[2].foffset = j8;
33489 vinfos[2].indices[0] = _ij8[0];
33490 vinfos[2].indices[1] = _ij8[1];
33491 vinfos[2].maxsolutions = _nj8;
33492 vinfos[3].jointtype = 1;
33493 vinfos[3].foffset = j9;
33494 vinfos[3].indices[0] = _ij9[0];
33495 vinfos[3].indices[1] = _ij9[1];
33496 vinfos[3].maxsolutions = _nj9;
33497 vinfos[4].jointtype = 1;
33498 vinfos[4].foffset = j10;
33499 vinfos[4].indices[0] = _ij10[0];
33500 vinfos[4].indices[1] = _ij10[1];
33501 vinfos[4].maxsolutions = _nj10;
33502 vinfos[5].jointtype = 1;
33503 vinfos[5].foffset = j11;
33504 vinfos[5].indices[0] = _ij11[0];
33505 vinfos[5].indices[1] = _ij11[1];
33506 vinfos[5].maxsolutions = _nj11;
33507 vinfos[6].jointtype = 1;
33508 vinfos[6].foffset = j12;
33509 vinfos[6].indices[0] = _ij12[0];
33510 vinfos[6].indices[1] = _ij12[1];
33511 vinfos[6].maxsolutions = _nj12;
33512 std::vector<int> vfree(0);
33513 solutions.AddSolution(
33514 vinfos, vfree);
33515 }
33516 }
33517 }
33518 }
33519 } while (0);
33520 if (bgotonextstatement)
33521 {
33522 bool bgotonextstatement = true;
33523 do
33524 {
33525 IkReal x302 = ((1.0) * cj11);
33526 IkReal x303
33527 = ((((-1.0) * x302)) + new_r22);
33528 IkReal x304 = ((1.0) * sj11);
33529 evalcond[0]
33530 = ((-3.14159265358979)
33531 + (IKfmod(
33532 ((3.14159265358979)
33533 + (IKabs(
33534 ((1.5707963267949)
33535 + j10)))),
33536 6.28318530717959)));
33537 evalcond[1] = x303;
33538 evalcond[2] = x303;
33539 evalcond[3] = new_r02;
33540 evalcond[4] = (sj11 + new_r12);
33541 evalcond[5]
33542 = ((((-1.0) * (1.0) * new_r12))
33543 + (((-1.0) * x304)));
33544 evalcond[6]
33545 = ((((-1.0) * new_r22 * x304))
33546 + (((-1.0) * new_r12 * x302)));
33547 evalcond[7]
33548 = (((new_r10 * sj11))
33549 + (((-1.0) * new_r20 * x302)));
33550 evalcond[8]
33551 = ((((-1.0) * new_r21 * x302))
33552 + ((new_r11 * sj11)));
33553 evalcond[9]
33554 = ((1.0)
33555 + (((-1.0) * new_r22 * x302))
33556 + ((new_r12 * sj11)));
33557 if (IKabs(evalcond[0])
33558 < 0.0000010000000000
33559 && IKabs(evalcond[1])
33560 < 0.0000010000000000
33561 && IKabs(evalcond[2])
33562 < 0.0000010000000000
33563 && IKabs(evalcond[3])
33564 < 0.0000010000000000
33565 && IKabs(evalcond[4])
33566 < 0.0000010000000000
33567 && IKabs(evalcond[5])
33568 < 0.0000010000000000
33569 && IKabs(evalcond[6])
33570 < 0.0000010000000000
33571 && IKabs(evalcond[7])
33572 < 0.0000010000000000
33573 && IKabs(evalcond[8])
33574 < 0.0000010000000000
33575 && IKabs(evalcond[9])
33576 < 0.0000010000000000)
33577 {
33578 bgotonextstatement = false;
33579 {
33580 IkReal j12array[1], cj12array[1],
33581 sj12array[1];
33582 bool j12valid[1] = {false};
33583 _nj12 = 1;
33584 if (IKabs(new_r00)
33585 < IKFAST_ATAN2_MAGTHRESH
33586 && IKabs(new_r01)
33587 < IKFAST_ATAN2_MAGTHRESH
33588 && IKabs(
33589 IKsqr(new_r00)
33590 + IKsqr(new_r01) - 1)
33591 <= IKFAST_SINCOS_THRESH)
33592 continue;
33593 j12array[0]
33594 = IKatan2(new_r00, new_r01);
33595 sj12array[0] = IKsin(j12array[0]);
33596 cj12array[0] = IKcos(j12array[0]);
33597 if (j12array[0] > IKPI)
33598 {
33599 j12array[0] -= IK2PI;
33600 }
33601 else if (j12array[0] < -IKPI)
33602 {
33603 j12array[0] += IK2PI;
33604 }
33605 j12valid[0] = true;
33606 for (int ij12 = 0; ij12 < 1; ++ij12)
33607 {
33608 if (!j12valid[ij12])
33609 {
33610 continue;
33611 }
33612 _ij12[0] = ij12;
33613 _ij12[1] = -1;
33614 for (int iij12 = ij12 + 1;
33615 iij12 < 1;
33616 ++iij12)
33617 {
33618 if (j12valid[iij12]
33619 && IKabs(
33620 cj12array[ij12]
33621 - cj12array[iij12])
33622 < IKFAST_SOLUTION_THRESH
33623 && IKabs(
33624 sj12array[ij12]
33625 - sj12array[iij12])
33626 < IKFAST_SOLUTION_THRESH)
33627 {
33628 j12valid[iij12] = false;
33629 _ij12[1] = iij12;
33630 break;
33631 }
33632 }
33633 j12 = j12array[ij12];
33634 cj12 = cj12array[ij12];
33635 sj12 = sj12array[ij12];
33636 {
33637 IkReal evalcond[8];
33638 IkReal x305 = IKsin(j12);
33639 IkReal x306
33640 = ((1.0) * (IKcos(j12)));
33641 IkReal x307 = ((-1.0) * x306);
33642 IkReal x308 = ((1.0) * new_r11);
33643 IkReal x309 = ((1.0) * new_r10);
33644 evalcond[0]
33645 = (((new_r12 * x305))
33646 + new_r21);
33647 evalcond[1]
33648 = ((((-1.0) * x305))
33649 + new_r00);
33650 evalcond[2] = (x307 + new_r01);
33651 evalcond[3]
33652 = ((((-1.0) * new_r12
33653 * x306))
33654 + new_r20);
33655 evalcond[4]
33656 = ((((-1.0) * x308))
33657 + ((new_r22 * x305)));
33658 evalcond[5]
33659 = ((((-1.0) * x309))
33660 + (((-1.0) * new_r22
33661 * x306)));
33662 evalcond[6]
33663 = (x305
33664 + ((new_r12 * new_r21))
33665 + (((-1.0) * new_r22
33666 * x308)));
33667 evalcond[7]
33668 = (x307
33669 + ((new_r12 * new_r20))
33670 + (((-1.0) * new_r22
33671 * x309)));
33672 if (IKabs(evalcond[0])
33673 > IKFAST_EVALCOND_THRESH
33674 || IKabs(evalcond[1])
33675 > IKFAST_EVALCOND_THRESH
33676 || IKabs(evalcond[2])
33677 > IKFAST_EVALCOND_THRESH
33678 || IKabs(evalcond[3])
33679 > IKFAST_EVALCOND_THRESH
33680 || IKabs(evalcond[4])
33681 > IKFAST_EVALCOND_THRESH
33682 || IKabs(evalcond[5])
33683 > IKFAST_EVALCOND_THRESH
33684 || IKabs(evalcond[6])
33685 > IKFAST_EVALCOND_THRESH
33686 || IKabs(evalcond[7])
33687 > IKFAST_EVALCOND_THRESH)
33688 {
33689 continue;
33690 }
33691 }
33692
33693 {
33694 std::vector<
33695 IkSingleDOFSolutionBase<
33696 IkReal> >
33697 vinfos(7);
33698 vinfos[0].jointtype = 1;
33699 vinfos[0].foffset = j4;
33700 vinfos[0].indices[0] = _ij4[0];
33701 vinfos[0].indices[1] = _ij4[1];
33702 vinfos[0].maxsolutions = _nj4;
33703 vinfos[1].jointtype = 1;
33704 vinfos[1].foffset = j6;
33705 vinfos[1].indices[0] = _ij6[0];
33706 vinfos[1].indices[1] = _ij6[1];
33707 vinfos[1].maxsolutions = _nj6;
33708 vinfos[2].jointtype = 1;
33709 vinfos[2].foffset = j8;
33710 vinfos[2].indices[0] = _ij8[0];
33711 vinfos[2].indices[1] = _ij8[1];
33712 vinfos[2].maxsolutions = _nj8;
33713 vinfos[3].jointtype = 1;
33714 vinfos[3].foffset = j9;
33715 vinfos[3].indices[0] = _ij9[0];
33716 vinfos[3].indices[1] = _ij9[1];
33717 vinfos[3].maxsolutions = _nj9;
33718 vinfos[4].jointtype = 1;
33719 vinfos[4].foffset = j10;
33720 vinfos[4].indices[0] = _ij10[0];
33721 vinfos[4].indices[1] = _ij10[1];
33722 vinfos[4].maxsolutions = _nj10;
33723 vinfos[5].jointtype = 1;
33724 vinfos[5].foffset = j11;
33725 vinfos[5].indices[0] = _ij11[0];
33726 vinfos[5].indices[1] = _ij11[1];
33727 vinfos[5].maxsolutions = _nj11;
33728 vinfos[6].jointtype = 1;
33729 vinfos[6].foffset = j12;
33730 vinfos[6].indices[0] = _ij12[0];
33731 vinfos[6].indices[1] = _ij12[1];
33732 vinfos[6].maxsolutions = _nj12;
33733 std::vector<int> vfree(0);
33734 solutions.AddSolution(
33735 vinfos, vfree);
33736 }
33737 }
33738 }
33739 }
33740 } while (0);
33741 if (bgotonextstatement)
33742 {
33743 bool bgotonextstatement = true;
33744 do
33745 {
33746 IkReal x310 = ((1.0) * cj10);
33747 IkReal x311 = ((1.0) * sj10);
33748 IkReal x312
33749 = ((((-1.0) * new_r02 * x311))
33750 + ((cj10 * new_r12)));
33751 evalcond[0]
33752 = ((-3.14159265358979)
33753 + (IKfmod(
33754 ((3.14159265358979)
33755 + (IKabs((
33756 (-1.5707963267949)
33757 + j11)))),
33758 6.28318530717959)));
33759 evalcond[1] = new_r22;
33760 evalcond[2]
33761 = ((((-1.0) * x310)) + new_r02);
33762 evalcond[3]
33763 = (new_r12 + (((-1.0) * x311)));
33764 evalcond[4] = x312;
33765 evalcond[5] = x312;
33766 evalcond[6]
33767 = ((-1.0) + ((cj10 * new_r02))
33768 + ((new_r12 * sj10)));
33769 evalcond[7]
33770 = (((cj10 * new_r01))
33771 + ((new_r11 * sj10)));
33772 evalcond[8]
33773 = (((cj10 * new_r00))
33774 + ((new_r10 * sj10)));
33775 evalcond[9]
33776 = ((((-1.0) * new_r10 * x311))
33777 + (((-1.0) * new_r00 * x310)));
33778 evalcond[10]
33779 = ((((-1.0) * new_r01 * x310))
33780 + (((-1.0) * new_r11 * x311)));
33781 evalcond[11]
33782 = ((1.0)
33783 + (((-1.0) * new_r12 * x311))
33784 + (((-1.0) * new_r02 * x310)));
33785 if (IKabs(evalcond[0])
33786 < 0.0000010000000000
33787 && IKabs(evalcond[1])
33788 < 0.0000010000000000
33789 && IKabs(evalcond[2])
33790 < 0.0000010000000000
33791 && IKabs(evalcond[3])
33792 < 0.0000010000000000
33793 && IKabs(evalcond[4])
33794 < 0.0000010000000000
33795 && IKabs(evalcond[5])
33796 < 0.0000010000000000
33797 && IKabs(evalcond[6])
33798 < 0.0000010000000000
33799 && IKabs(evalcond[7])
33800 < 0.0000010000000000
33801 && IKabs(evalcond[8])
33802 < 0.0000010000000000
33803 && IKabs(evalcond[9])
33804 < 0.0000010000000000
33805 && IKabs(evalcond[10])
33806 < 0.0000010000000000
33807 && IKabs(evalcond[11])
33808 < 0.0000010000000000)
33809 {
33810 bgotonextstatement = false;
33811 {
33812 IkReal j12array[1], cj12array[1],
33813 sj12array[1];
33814 bool j12valid[1] = {false};
33815 _nj12 = 1;
33816 if (IKabs(new_r21)
33817 < IKFAST_ATAN2_MAGTHRESH
33818 && IKabs((
33819 (-1.0)
33820 * (((1.0) * new_r20))))
33821 < IKFAST_ATAN2_MAGTHRESH
33822 && IKabs(
33823 IKsqr(new_r21)
33824 + IKsqr((
33825 (-1.0)
33826 * (((1.0)
33827 * new_r20))))
33828 - 1)
33829 <= IKFAST_SINCOS_THRESH)
33830 continue;
33831 j12array[0] = IKatan2(
33832 new_r21,
33833 ((-1.0)
33834 * (((1.0) * new_r20))));
33835 sj12array[0] = IKsin(j12array[0]);
33836 cj12array[0] = IKcos(j12array[0]);
33837 if (j12array[0] > IKPI)
33838 {
33839 j12array[0] -= IK2PI;
33840 }
33841 else if (j12array[0] < -IKPI)
33842 {
33843 j12array[0] += IK2PI;
33844 }
33845 j12valid[0] = true;
33846 for (int ij12 = 0; ij12 < 1;
33847 ++ij12)
33848 {
33849 if (!j12valid[ij12])
33850 {
33851 continue;
33852 }
33853 _ij12[0] = ij12;
33854 _ij12[1] = -1;
33855 for (int iij12 = ij12 + 1;
33856 iij12 < 1;
33857 ++iij12)
33858 {
33859 if (j12valid[iij12]
33860 && IKabs(
33861 cj12array[ij12]
33862 - cj12array[iij12])
33863 < IKFAST_SOLUTION_THRESH
33864 && IKabs(
33865 sj12array[ij12]
33866 - sj12array[iij12])
33867 < IKFAST_SOLUTION_THRESH)
33868 {
33869 j12valid[iij12] = false;
33870 _ij12[1] = iij12;
33871 break;
33872 }
33873 }
33874 j12 = j12array[ij12];
33875 cj12 = cj12array[ij12];
33876 sj12 = sj12array[ij12];
33877 {
33878 IkReal evalcond[8];
33879 IkReal x313 = IKcos(j12);
33880 IkReal x314 = IKsin(j12);
33881 IkReal x315 = ((1.0) * x314);
33882 IkReal x316 = ((-1.0) * x315);
33883 IkReal x317 = ((1.0) * x313);
33884 IkReal x318
33885 = ((1.0) * new_r12);
33886 evalcond[0]
33887 = (x313 + new_r20);
33888 evalcond[1]
33889 = (x316 + new_r21);
33890 evalcond[2]
33891 = (((new_r12 * x313))
33892 + new_r01);
33893 evalcond[3]
33894 = (((new_r12 * x314))
33895 + new_r00);
33896 evalcond[4]
33897 = ((((-1.0) * new_r02
33898 * x317))
33899 + new_r11);
33900 evalcond[5]
33901 = (new_r10
33902 + (((-1.0) * new_r02
33903 * x315)));
33904 evalcond[6]
33905 = ((((-1.0) * new_r00
33906 * x318))
33907 + x316
33908 + ((new_r02
33909 * new_r10)));
33910 evalcond[7]
33911 = (((new_r02 * new_r11))
33912 + (((-1.0) * new_r01
33913 * x318))
33914 + (((-1.0) * x317)));
33915 if (IKabs(evalcond[0])
33916 > IKFAST_EVALCOND_THRESH
33917 || IKabs(evalcond[1])
33918 > IKFAST_EVALCOND_THRESH
33919 || IKabs(evalcond[2])
33920 > IKFAST_EVALCOND_THRESH
33921 || IKabs(evalcond[3])
33922 > IKFAST_EVALCOND_THRESH
33923 || IKabs(evalcond[4])
33924 > IKFAST_EVALCOND_THRESH
33925 || IKabs(evalcond[5])
33926 > IKFAST_EVALCOND_THRESH
33927 || IKabs(evalcond[6])
33928 > IKFAST_EVALCOND_THRESH
33929 || IKabs(evalcond[7])
33930 > IKFAST_EVALCOND_THRESH)
33931 {
33932 continue;
33933 }
33934 }
33935
33936 {
33937 std::vector<
33938 IkSingleDOFSolutionBase<
33939 IkReal> >
33940 vinfos(7);
33941 vinfos[0].jointtype = 1;
33942 vinfos[0].foffset = j4;
33943 vinfos[0].indices[0]
33944 = _ij4[0];
33945 vinfos[0].indices[1]
33946 = _ij4[1];
33947 vinfos[0].maxsolutions = _nj4;
33948 vinfos[1].jointtype = 1;
33949 vinfos[1].foffset = j6;
33950 vinfos[1].indices[0]
33951 = _ij6[0];
33952 vinfos[1].indices[1]
33953 = _ij6[1];
33954 vinfos[1].maxsolutions = _nj6;
33955 vinfos[2].jointtype = 1;
33956 vinfos[2].foffset = j8;
33957 vinfos[2].indices[0]
33958 = _ij8[0];
33959 vinfos[2].indices[1]
33960 = _ij8[1];
33961 vinfos[2].maxsolutions = _nj8;
33962 vinfos[3].jointtype = 1;
33963 vinfos[3].foffset = j9;
33964 vinfos[3].indices[0]
33965 = _ij9[0];
33966 vinfos[3].indices[1]
33967 = _ij9[1];
33968 vinfos[3].maxsolutions = _nj9;
33969 vinfos[4].jointtype = 1;
33970 vinfos[4].foffset = j10;
33971 vinfos[4].indices[0]
33972 = _ij10[0];
33973 vinfos[4].indices[1]
33974 = _ij10[1];
33975 vinfos[4].maxsolutions
33976 = _nj10;
33977 vinfos[5].jointtype = 1;
33978 vinfos[5].foffset = j11;
33979 vinfos[5].indices[0]
33980 = _ij11[0];
33981 vinfos[5].indices[1]
33982 = _ij11[1];
33983 vinfos[5].maxsolutions
33984 = _nj11;
33985 vinfos[6].jointtype = 1;
33986 vinfos[6].foffset = j12;
33987 vinfos[6].indices[0]
33988 = _ij12[0];
33989 vinfos[6].indices[1]
33990 = _ij12[1];
33991 vinfos[6].maxsolutions
33992 = _nj12;
33993 std::vector<int> vfree(0);
33994 solutions.AddSolution(
33995 vinfos, vfree);
33996 }
33997 }
33998 }
33999 }
34000 } while (0);
34001 if (bgotonextstatement)
34002 {
34003 bool bgotonextstatement = true;
34004 do
34005 {
34006 IkReal x319
34007 = ((((-1.0) * (1.0) * new_r02
34008 * sj10))
34009 + ((cj10 * new_r12)));
34010 IkReal x320
34011 = ((1.0) + ((cj10 * new_r02))
34012 + ((new_r12 * sj10)));
34013 IkReal x321
34014 = (((cj10 * new_r01))
34015 + ((new_r11 * sj10)));
34016 IkReal x322
34017 = (((cj10 * new_r00))
34018 + ((new_r10 * sj10)));
34019 evalcond[0]
34020 = ((-3.14159265358979)
34021 + (IKfmod(
34022 ((3.14159265358979)
34023 + (IKabs((
34024 (1.5707963267949)
34025 + j11)))),
34026 6.28318530717959)));
34027 evalcond[1] = new_r22;
34028 evalcond[2] = (cj10 + new_r02);
34029 evalcond[3] = (sj10 + new_r12);
34030 evalcond[4] = x319;
34031 evalcond[5] = x319;
34032 evalcond[6] = x320;
34033 evalcond[7] = x321;
34034 evalcond[8] = x322;
34035 evalcond[9] = x322;
34036 evalcond[10] = x321;
34037 evalcond[11] = x320;
34038 if (IKabs(evalcond[0])
34039 < 0.0000010000000000
34040 && IKabs(evalcond[1])
34041 < 0.0000010000000000
34042 && IKabs(evalcond[2])
34043 < 0.0000010000000000
34044 && IKabs(evalcond[3])
34045 < 0.0000010000000000
34046 && IKabs(evalcond[4])
34047 < 0.0000010000000000
34048 && IKabs(evalcond[5])
34049 < 0.0000010000000000
34050 && IKabs(evalcond[6])
34051 < 0.0000010000000000
34052 && IKabs(evalcond[7])
34053 < 0.0000010000000000
34054 && IKabs(evalcond[8])
34055 < 0.0000010000000000
34056 && IKabs(evalcond[9])
34057 < 0.0000010000000000
34058 && IKabs(evalcond[10])
34059 < 0.0000010000000000
34060 && IKabs(evalcond[11])
34061 < 0.0000010000000000)
34062 {
34063 bgotonextstatement = false;
34064 {
34065 IkReal j12array[1],
34066 cj12array[1], sj12array[1];
34067 bool j12valid[1] = {false};
34068 _nj12 = 1;
34069 if (IKabs(
34070 ((-1.0)
34071 * (((1.0) * new_r21))))
34072 < IKFAST_ATAN2_MAGTHRESH
34073 && IKabs(new_r20)
34074 < IKFAST_ATAN2_MAGTHRESH
34075 && IKabs(
34076 IKsqr((
34077 (-1.0)
34078 * (((1.0)
34079 * new_r21))))
34080 + IKsqr(new_r20) - 1)
34081 <= IKFAST_SINCOS_THRESH)
34082 continue;
34083 j12array[0] = IKatan2(
34084 ((-1.0)
34085 * (((1.0) * new_r21))),
34086 new_r20);
34087 sj12array[0]
34088 = IKsin(j12array[0]);
34089 cj12array[0]
34090 = IKcos(j12array[0]);
34091 if (j12array[0] > IKPI)
34092 {
34093 j12array[0] -= IK2PI;
34094 }
34095 else if (j12array[0] < -IKPI)
34096 {
34097 j12array[0] += IK2PI;
34098 }
34099 j12valid[0] = true;
34100 for (int ij12 = 0; ij12 < 1;
34101 ++ij12)
34102 {
34103 if (!j12valid[ij12])
34104 {
34105 continue;
34106 }
34107 _ij12[0] = ij12;
34108 _ij12[1] = -1;
34109 for (int iij12 = ij12 + 1;
34110 iij12 < 1;
34111 ++iij12)
34112 {
34113 if (j12valid[iij12]
34114 && IKabs(
34115 cj12array[ij12]
34116 - cj12array
34117 [iij12])
34118 < IKFAST_SOLUTION_THRESH
34119 && IKabs(
34120 sj12array[ij12]
34121 - sj12array
34122 [iij12])
34123 < IKFAST_SOLUTION_THRESH)
34124 {
34125 j12valid[iij12] = false;
34126 _ij12[1] = iij12;
34127 break;
34128 }
34129 }
34130 j12 = j12array[ij12];
34131 cj12 = cj12array[ij12];
34132 sj12 = sj12array[ij12];
34133 {
34134 IkReal evalcond[8];
34135 IkReal x323 = IKsin(j12);
34136 IkReal x324 = IKcos(j12);
34137 IkReal x325
34138 = ((1.0) * x324);
34139 IkReal x326
34140 = ((-1.0) * x325);
34141 IkReal x327
34142 = ((1.0) * x323);
34143 IkReal x328
34144 = ((1.0) * new_r02);
34145 evalcond[0]
34146 = (x323 + new_r21);
34147 evalcond[1]
34148 = (x326 + new_r20);
34149 evalcond[2]
34150 = (((new_r02 * x324))
34151 + new_r11);
34152 evalcond[3]
34153 = (((new_r02 * x323))
34154 + new_r10);
34155 evalcond[4]
34156 = ((((-1.0) * new_r12
34157 * x325))
34158 + new_r01);
34159 evalcond[5]
34160 = ((((-1.0) * new_r12
34161 * x327))
34162 + new_r00);
34163 evalcond[6]
34164 = (((new_r00 * new_r12))
34165 + (((-1.0) * x327))
34166 + (((-1.0) * new_r10
34167 * x328)));
34168 evalcond[7]
34169 = (((new_r01 * new_r12))
34170 + x326
34171 + (((-1.0) * new_r11
34172 * x328)));
34173 if (IKabs(evalcond[0])
34174 > IKFAST_EVALCOND_THRESH
34175 || IKabs(evalcond[1])
34176 > IKFAST_EVALCOND_THRESH
34177 || IKabs(evalcond[2])
34178 > IKFAST_EVALCOND_THRESH
34179 || IKabs(evalcond[3])
34180 > IKFAST_EVALCOND_THRESH
34181 || IKabs(evalcond[4])
34182 > IKFAST_EVALCOND_THRESH
34183 || IKabs(evalcond[5])
34184 > IKFAST_EVALCOND_THRESH
34185 || IKabs(evalcond[6])
34186 > IKFAST_EVALCOND_THRESH
34187 || IKabs(evalcond[7])
34188 > IKFAST_EVALCOND_THRESH)
34189 {
34190 continue;
34191 }
34192 }
34193
34194 {
34195 std::vector<
34196 IkSingleDOFSolutionBase<
34197 IkReal> >
34198 vinfos(7);
34199 vinfos[0].jointtype = 1;
34200 vinfos[0].foffset = j4;
34201 vinfos[0].indices[0]
34202 = _ij4[0];
34203 vinfos[0].indices[1]
34204 = _ij4[1];
34205 vinfos[0].maxsolutions
34206 = _nj4;
34207 vinfos[1].jointtype = 1;
34208 vinfos[1].foffset = j6;
34209 vinfos[1].indices[0]
34210 = _ij6[0];
34211 vinfos[1].indices[1]
34212 = _ij6[1];
34213 vinfos[1].maxsolutions
34214 = _nj6;
34215 vinfos[2].jointtype = 1;
34216 vinfos[2].foffset = j8;
34217 vinfos[2].indices[0]
34218 = _ij8[0];
34219 vinfos[2].indices[1]
34220 = _ij8[1];
34221 vinfos[2].maxsolutions
34222 = _nj8;
34223 vinfos[3].jointtype = 1;
34224 vinfos[3].foffset = j9;
34225 vinfos[3].indices[0]
34226 = _ij9[0];
34227 vinfos[3].indices[1]
34228 = _ij9[1];
34229 vinfos[3].maxsolutions
34230 = _nj9;
34231 vinfos[4].jointtype = 1;
34232 vinfos[4].foffset = j10;
34233 vinfos[4].indices[0]
34234 = _ij10[0];
34235 vinfos[4].indices[1]
34236 = _ij10[1];
34237 vinfos[4].maxsolutions
34238 = _nj10;
34239 vinfos[5].jointtype = 1;
34240 vinfos[5].foffset = j11;
34241 vinfos[5].indices[0]
34242 = _ij11[0];
34243 vinfos[5].indices[1]
34244 = _ij11[1];
34245 vinfos[5].maxsolutions
34246 = _nj11;
34247 vinfos[6].jointtype = 1;
34248 vinfos[6].foffset = j12;
34249 vinfos[6].indices[0]
34250 = _ij12[0];
34251 vinfos[6].indices[1]
34252 = _ij12[1];
34253 vinfos[6].maxsolutions
34254 = _nj12;
34255 std::vector<int> vfree(0);
34256 solutions.AddSolution(
34257 vinfos, vfree);
34258 }
34259 }
34260 }
34261 }
34262 } while (0);
34263 if (bgotonextstatement)
34264 {
34265 bool bgotonextstatement = true;
34266 do
34267 {
34268 IkReal x329
34269 = ((((-1.0) * (1.0) * new_r02
34270 * sj10))
34271 + ((cj10 * new_r12)));
34272 IkReal x330
34273 = (((cj10 * new_r02))
34274 + ((new_r12 * sj10)));
34275 evalcond[0]
34276 = ((-3.14159265358979)
34277 + (IKfmod(
34278 ((3.14159265358979)
34279 + (IKabs(j11))),
34280 6.28318530717959)));
34281 evalcond[1] = ((-1.0) + new_r22);
34282 evalcond[2] = new_r20;
34283 evalcond[3] = new_r02;
34284 evalcond[4] = new_r12;
34285 evalcond[5] = new_r21;
34286 evalcond[6] = x329;
34287 evalcond[7] = x329;
34288 evalcond[8] = x330;
34289 evalcond[9] = x330;
34290 if (IKabs(evalcond[0])
34291 < 0.0000010000000000
34292 && IKabs(evalcond[1])
34293 < 0.0000010000000000
34294 && IKabs(evalcond[2])
34295 < 0.0000010000000000
34296 && IKabs(evalcond[3])
34297 < 0.0000010000000000
34298 && IKabs(evalcond[4])
34299 < 0.0000010000000000
34300 && IKabs(evalcond[5])
34301 < 0.0000010000000000
34302 && IKabs(evalcond[6])
34303 < 0.0000010000000000
34304 && IKabs(evalcond[7])
34305 < 0.0000010000000000
34306 && IKabs(evalcond[8])
34307 < 0.0000010000000000
34308 && IKabs(evalcond[9])
34309 < 0.0000010000000000)
34310 {
34311 bgotonextstatement = false;
34312 {
34313 IkReal j12array[1],
34314 cj12array[1],
34315 sj12array[1];
34316 bool j12valid[1] = {false};
34317 _nj12 = 1;
34318 IkReal x331
34319 = ((1.0) * new_r01);
34320 if (IKabs(
34321 ((((-1.0) * cj10
34322 * x331))
34323 + (((-1.0) * (1.0)
34324 * new_r00
34325 * sj10))))
34326 < IKFAST_ATAN2_MAGTHRESH
34327 && IKabs(
34328 (((cj10 * new_r00))
34329 + (((-1.0) * sj10
34330 * x331))))
34331 < IKFAST_ATAN2_MAGTHRESH
34332 && IKabs(
34333 IKsqr((
34334 (((-1.0) * cj10
34335 * x331))
34336 + (((-1.0)
34337 * (1.0)
34338 * new_r00
34339 * sj10))))
34340 + IKsqr((
34341 ((cj10
34342 * new_r00))
34343 + (((-1.0)
34344 * sj10
34345 * x331))))
34346 - 1)
34347 <= IKFAST_SINCOS_THRESH)
34348 continue;
34349 j12array[0] = IKatan2(
34350 ((((-1.0) * cj10 * x331))
34351 + (((-1.0) * (1.0)
34352 * new_r00 * sj10))),
34353 (((cj10 * new_r00))
34354 + (((-1.0) * sj10
34355 * x331))));
34356 sj12array[0]
34357 = IKsin(j12array[0]);
34358 cj12array[0]
34359 = IKcos(j12array[0]);
34360 if (j12array[0] > IKPI)
34361 {
34362 j12array[0] -= IK2PI;
34363 }
34364 else if (j12array[0] < -IKPI)
34365 {
34366 j12array[0] += IK2PI;
34367 }
34368 j12valid[0] = true;
34369 for (int ij12 = 0; ij12 < 1;
34370 ++ij12)
34371 {
34372 if (!j12valid[ij12])
34373 {
34374 continue;
34375 }
34376 _ij12[0] = ij12;
34377 _ij12[1] = -1;
34378 for (int iij12 = ij12 + 1;
34379 iij12 < 1;
34380 ++iij12)
34381 {
34382 if (j12valid[iij12]
34383 && IKabs(
34384 cj12array[ij12]
34385 - cj12array
34386 [iij12])
34387 < IKFAST_SOLUTION_THRESH
34388 && IKabs(
34389 sj12array[ij12]
34390 - sj12array
34391 [iij12])
34392 < IKFAST_SOLUTION_THRESH)
34393 {
34394 j12valid[iij12] = false;
34395 _ij12[1] = iij12;
34396 break;
34397 }
34398 }
34399 j12 = j12array[ij12];
34400 cj12 = cj12array[ij12];
34401 sj12 = sj12array[ij12];
34402 {
34403 IkReal evalcond[8];
34404 IkReal x332 = IKsin(j12);
34405 IkReal x333
34406 = (cj10 * x332);
34407 IkReal x334 = IKcos(j12);
34408 IkReal x335
34409 = ((1.0) * x334);
34410 IkReal x336
34411 = ((-1.0) * x335);
34412 IkReal x337
34413 = ((1.0) * sj10);
34414 IkReal x338
34415 = (((sj10 * x332))
34416 + (((-1.0) * cj10
34417 * x335)));
34418 evalcond[0]
34419 = (((cj10 * new_r01))
34420 + x332
34421 + ((new_r11
34422 * sj10)));
34423 evalcond[1]
34424 = (((sj10 * x334))
34425 + x333 + new_r01);
34426 evalcond[2]
34427 = (((cj10 * new_r00))
34428 + ((new_r10
34429 * sj10))
34430 + x336);
34431 evalcond[3]
34432 = (((cj10 * new_r10))
34433 + (((-1.0) * x332))
34434 + (((-1.0)
34435 * new_r00
34436 * x337)));
34437 evalcond[4]
34438 = (((cj10 * new_r11))
34439 + x336
34440 + (((-1.0)
34441 * new_r01
34442 * x337)));
34443 evalcond[5]
34444 = (x338 + new_r00);
34445 evalcond[6]
34446 = (x338 + new_r11);
34447 evalcond[7]
34448 = ((((-1.0) * x334
34449 * x337))
34450 + (((-1.0) * x333))
34451 + new_r10);
34452 if (IKabs(evalcond[0])
34453 > IKFAST_EVALCOND_THRESH
34454 || IKabs(evalcond[1])
34455 > IKFAST_EVALCOND_THRESH
34456 || IKabs(evalcond[2])
34457 > IKFAST_EVALCOND_THRESH
34458 || IKabs(evalcond[3])
34459 > IKFAST_EVALCOND_THRESH
34460 || IKabs(evalcond[4])
34461 > IKFAST_EVALCOND_THRESH
34462 || IKabs(evalcond[5])
34463 > IKFAST_EVALCOND_THRESH
34464 || IKabs(evalcond[6])
34465 > IKFAST_EVALCOND_THRESH
34466 || IKabs(evalcond[7])
34467 > IKFAST_EVALCOND_THRESH)
34468 {
34469 continue;
34470 }
34471 }
34472
34473 {
34474 std::vector<
34475 IkSingleDOFSolutionBase<
34476 IkReal> >
34477 vinfos(7);
34478 vinfos[0].jointtype = 1;
34479 vinfos[0].foffset = j4;
34480 vinfos[0].indices[0]
34481 = _ij4[0];
34482 vinfos[0].indices[1]
34483 = _ij4[1];
34484 vinfos[0].maxsolutions
34485 = _nj4;
34486 vinfos[1].jointtype = 1;
34487 vinfos[1].foffset = j6;
34488 vinfos[1].indices[0]
34489 = _ij6[0];
34490 vinfos[1].indices[1]
34491 = _ij6[1];
34492 vinfos[1].maxsolutions
34493 = _nj6;
34494 vinfos[2].jointtype = 1;
34495 vinfos[2].foffset = j8;
34496 vinfos[2].indices[0]
34497 = _ij8[0];
34498 vinfos[2].indices[1]
34499 = _ij8[1];
34500 vinfos[2].maxsolutions
34501 = _nj8;
34502 vinfos[3].jointtype = 1;
34503 vinfos[3].foffset = j9;
34504 vinfos[3].indices[0]
34505 = _ij9[0];
34506 vinfos[3].indices[1]
34507 = _ij9[1];
34508 vinfos[3].maxsolutions
34509 = _nj9;
34510 vinfos[4].jointtype = 1;
34511 vinfos[4].foffset = j10;
34512 vinfos[4].indices[0]
34513 = _ij10[0];
34514 vinfos[4].indices[1]
34515 = _ij10[1];
34516 vinfos[4].maxsolutions
34517 = _nj10;
34518 vinfos[5].jointtype = 1;
34519 vinfos[5].foffset = j11;
34520 vinfos[5].indices[0]
34521 = _ij11[0];
34522 vinfos[5].indices[1]
34523 = _ij11[1];
34524 vinfos[5].maxsolutions
34525 = _nj11;
34526 vinfos[6].jointtype = 1;
34527 vinfos[6].foffset = j12;
34528 vinfos[6].indices[0]
34529 = _ij12[0];
34530 vinfos[6].indices[1]
34531 = _ij12[1];
34532 vinfos[6].maxsolutions
34533 = _nj12;
34534 std::vector<int> vfree(0);
34535 solutions.AddSolution(
34536 vinfos, vfree);
34537 }
34538 }
34539 }
34540 }
34541 } while (0);
34542 if (bgotonextstatement)
34543 {
34544 bool bgotonextstatement = true;
34545 do
34546 {
34547 IkReal x339
34548 = ((((-1.0) * (1.0)
34549 * new_r02 * sj10))
34550 + ((cj10 * new_r12)));
34551 IkReal x340 = (cj10 * new_r02);
34552 IkReal x341 = (new_r12 * sj10);
34553 evalcond[0]
34554 = ((-3.14159265358979)
34555 + (IKfmod(
34556 ((3.14159265358979)
34557 + (IKabs((
34558 (-3.14159265358979)
34559 + j11)))),
34560 6.28318530717959)));
34561 evalcond[1] = ((1.0) + new_r22);
34562 evalcond[2] = new_r20;
34563 evalcond[3] = new_r02;
34564 evalcond[4] = new_r12;
34565 evalcond[5] = new_r21;
34566 evalcond[6] = x339;
34567 evalcond[7] = x339;
34568 evalcond[8] = (x340 + x341);
34569 evalcond[9]
34570 = ((((-1.0) * x341))
34571 + (((-1.0) * x340)));
34572 if (IKabs(evalcond[0])
34573 < 0.0000010000000000
34574 && IKabs(evalcond[1])
34575 < 0.0000010000000000
34576 && IKabs(evalcond[2])
34577 < 0.0000010000000000
34578 && IKabs(evalcond[3])
34579 < 0.0000010000000000
34580 && IKabs(evalcond[4])
34581 < 0.0000010000000000
34582 && IKabs(evalcond[5])
34583 < 0.0000010000000000
34584 && IKabs(evalcond[6])
34585 < 0.0000010000000000
34586 && IKabs(evalcond[7])
34587 < 0.0000010000000000
34588 && IKabs(evalcond[8])
34589 < 0.0000010000000000
34590 && IKabs(evalcond[9])
34591 < 0.0000010000000000)
34592 {
34593 bgotonextstatement = false;
34594 {
34595 IkReal j12array[1],
34596 cj12array[1],
34597 sj12array[1];
34598 bool j12valid[1] = {false};
34599 _nj12 = 1;
34600 IkReal x342
34601 = ((1.0) * new_r00);
34602 if (IKabs(
34603 (((cj10 * new_r01))
34604 + (((-1.0) * sj10
34605 * x342))))
34606 < IKFAST_ATAN2_MAGTHRESH
34607 && IKabs(
34608 ((((-1.0) * cj10
34609 * x342))
34610 + (((-1.0)
34611 * (1.0)
34612 * new_r01
34613 * sj10))))
34614 < IKFAST_ATAN2_MAGTHRESH
34615 && IKabs(
34616 IKsqr((
34617 ((cj10
34618 * new_r01))
34619 + (((-1.0)
34620 * sj10
34621 * x342))))
34622 + IKsqr((
34623 (((-1.0)
34624 * cj10
34625 * x342))
34626 + (((-1.0)
34627 * (1.0)
34628 * new_r01
34629 * sj10))))
34630 - 1)
34631 <= IKFAST_SINCOS_THRESH)
34632 continue;
34633 j12array[0] = IKatan2(
34634 (((cj10 * new_r01))
34635 + (((-1.0) * sj10
34636 * x342))),
34637 ((((-1.0) * cj10
34638 * x342))
34639 + (((-1.0) * (1.0)
34640 * new_r01
34641 * sj10))));
34642 sj12array[0]
34643 = IKsin(j12array[0]);
34644 cj12array[0]
34645 = IKcos(j12array[0]);
34646 if (j12array[0] > IKPI)
34647 {
34648 j12array[0] -= IK2PI;
34649 }
34650 else if (
34651 j12array[0] < -IKPI)
34652 {
34653 j12array[0] += IK2PI;
34654 }
34655 j12valid[0] = true;
34656 for (int ij12 = 0; ij12 < 1;
34657 ++ij12)
34658 {
34659 if (!j12valid[ij12])
34660 {
34661 continue;
34662 }
34663 _ij12[0] = ij12;
34664 _ij12[1] = -1;
34665 for (int iij12 = ij12 + 1;
34666 iij12 < 1;
34667 ++iij12)
34668 {
34669 if (j12valid[iij12]
34670 && IKabs(
34671 cj12array
34672 [ij12]
34673 - cj12array
34674 [iij12])
34675 < IKFAST_SOLUTION_THRESH
34676 && IKabs(
34677 sj12array
34678 [ij12]
34679 - sj12array
34680 [iij12])
34681 < IKFAST_SOLUTION_THRESH)
34682 {
34683 j12valid[iij12]
34684 = false;
34685 _ij12[1] = iij12;
34686 break;
34687 }
34688 }
34689 j12 = j12array[ij12];
34690 cj12 = cj12array[ij12];
34691 sj12 = sj12array[ij12];
34692 {
34693 IkReal evalcond[8];
34694 IkReal x343
34695 = IKcos(j12);
34696 IkReal x344
34697 = IKsin(j12);
34698 IkReal x345
34699 = ((1.0) * x344);
34700 IkReal x346
34701 = ((-1.0) * x345);
34702 IkReal x347
34703 = (cj10 * x343);
34704 IkReal x348
34705 = ((1.0) * sj10);
34706 IkReal x349
34707 = (((sj10 * x343))
34708 + (((-1.0) * cj10
34709 * x345)));
34710 evalcond[0]
34711 = (((cj10
34712 * new_r00))
34713 + ((new_r10
34714 * sj10))
34715 + x343);
34716 evalcond[1]
34717 = (((cj10
34718 * new_r01))
34719 + ((new_r11
34720 * sj10))
34721 + x346);
34722 evalcond[2]
34723 = (((sj10 * x344))
34724 + x347
34725 + new_r00);
34726 evalcond[3]
34727 = (((cj10
34728 * new_r10))
34729 + (((-1.0)
34730 * new_r00
34731 * x348))
34732 + x346);
34733 evalcond[4]
34734 = (((cj10
34735 * new_r11))
34736 + (((-1.0)
34737 * new_r01
34738 * x348))
34739 + (((-1.0)
34740 * x343)));
34741 evalcond[5]
34742 = (x349 + new_r01);
34743 evalcond[6]
34744 = (new_r10 + x349);
34745 evalcond[7]
34746 = ((((-1.0) * x347))
34747 + (((-1.0) * x344
34748 * x348))
34749 + new_r11);
34750 if (IKabs(evalcond[0])
34751 > IKFAST_EVALCOND_THRESH
34752 || IKabs(
34753 evalcond[1])
34754 > IKFAST_EVALCOND_THRESH
34755 || IKabs(
34756 evalcond[2])
34757 > IKFAST_EVALCOND_THRESH
34758 || IKabs(
34759 evalcond[3])
34760 > IKFAST_EVALCOND_THRESH
34761 || IKabs(
34762 evalcond[4])
34763 > IKFAST_EVALCOND_THRESH
34764 || IKabs(
34765 evalcond[5])
34766 > IKFAST_EVALCOND_THRESH
34767 || IKabs(
34768 evalcond[6])
34769 > IKFAST_EVALCOND_THRESH
34770 || IKabs(
34771 evalcond[7])
34772 > IKFAST_EVALCOND_THRESH)
34773 {
34774 continue;
34775 }
34776 }
34777
34778 {
34779 std::vector<
34780 IkSingleDOFSolutionBase<
34781 IkReal> >
34782 vinfos(7);
34783 vinfos[0].jointtype = 1;
34784 vinfos[0].foffset = j4;
34785 vinfos[0].indices[0]
34786 = _ij4[0];
34787 vinfos[0].indices[1]
34788 = _ij4[1];
34789 vinfos[0].maxsolutions
34790 = _nj4;
34791 vinfos[1].jointtype = 1;
34792 vinfos[1].foffset = j6;
34793 vinfos[1].indices[0]
34794 = _ij6[0];
34795 vinfos[1].indices[1]
34796 = _ij6[1];
34797 vinfos[1].maxsolutions
34798 = _nj6;
34799 vinfos[2].jointtype = 1;
34800 vinfos[2].foffset = j8;
34801 vinfos[2].indices[0]
34802 = _ij8[0];
34803 vinfos[2].indices[1]
34804 = _ij8[1];
34805 vinfos[2].maxsolutions
34806 = _nj8;
34807 vinfos[3].jointtype = 1;
34808 vinfos[3].foffset = j9;
34809 vinfos[3].indices[0]
34810 = _ij9[0];
34811 vinfos[3].indices[1]
34812 = _ij9[1];
34813 vinfos[3].maxsolutions
34814 = _nj9;
34815 vinfos[4].jointtype = 1;
34816 vinfos[4].foffset = j10;
34817 vinfos[4].indices[0]
34818 = _ij10[0];
34819 vinfos[4].indices[1]
34820 = _ij10[1];
34821 vinfos[4].maxsolutions
34822 = _nj10;
34823 vinfos[5].jointtype = 1;
34824 vinfos[5].foffset = j11;
34825 vinfos[5].indices[0]
34826 = _ij11[0];
34827 vinfos[5].indices[1]
34828 = _ij11[1];
34829 vinfos[5].maxsolutions
34830 = _nj11;
34831 vinfos[6].jointtype = 1;
34832 vinfos[6].foffset = j12;
34833 vinfos[6].indices[0]
34834 = _ij12[0];
34835 vinfos[6].indices[1]
34836 = _ij12[1];
34837 vinfos[6].maxsolutions
34838 = _nj12;
34839 std::vector<int> vfree(
34840 0);
34841 solutions.AddSolution(
34842 vinfos, vfree);
34843 }
34844 }
34845 }
34846 }
34847 } while (0);
34848 if (bgotonextstatement)
34849 {
34850 bool bgotonextstatement = true;
34851 do
34852 {
34853 IkReal x350 = ((1.0) * cj11);
34854 IkReal x351
34855 = ((((-1.0) * x350))
34856 + new_r22);
34857 IkReal x352 = ((1.0) * sj11);
34858 IkReal x353
34859 = ((((-1.0) * x352))
34860 + new_r02);
34861 evalcond[0]
34862 = ((-3.14159265358979)
34863 + (IKfmod(
34864 ((3.14159265358979)
34865 + (IKabs(j10))),
34866 6.28318530717959)));
34867 evalcond[1] = x351;
34868 evalcond[2] = x351;
34869 evalcond[3] = x353;
34870 evalcond[4] = new_r12;
34871 evalcond[5] = x353;
34872 evalcond[6]
34873 = ((((-1.0) * new_r22
34874 * x352))
34875 + ((cj11 * new_r02)));
34876 evalcond[7]
34877 = ((((-1.0) * new_r00
34878 * x352))
34879 + (((-1.0) * new_r20
34880 * x350)));
34881 evalcond[8]
34882 = ((((-1.0) * new_r01
34883 * x352))
34884 + (((-1.0) * new_r21
34885 * x350)));
34886 evalcond[9]
34887 = ((1.0)
34888 + (((-1.0) * new_r02
34889 * x352))
34890 + (((-1.0) * new_r22
34891 * x350)));
34892 if (IKabs(evalcond[0])
34893 < 0.0000010000000000
34894 && IKabs(evalcond[1])
34895 < 0.0000010000000000
34896 && IKabs(evalcond[2])
34897 < 0.0000010000000000
34898 && IKabs(evalcond[3])
34899 < 0.0000010000000000
34900 && IKabs(evalcond[4])
34901 < 0.0000010000000000
34902 && IKabs(evalcond[5])
34903 < 0.0000010000000000
34904 && IKabs(evalcond[6])
34905 < 0.0000010000000000
34906 && IKabs(evalcond[7])
34907 < 0.0000010000000000
34908 && IKabs(evalcond[8])
34909 < 0.0000010000000000
34910 && IKabs(evalcond[9])
34911 < 0.0000010000000000)
34912 {
34913 bgotonextstatement = false;
34914 {
34915 IkReal j12array[1],
34916 cj12array[1],
34917 sj12array[1];
34918 bool j12valid[1]
34919 = {false};
34920 _nj12 = 1;
34921 if (IKabs(new_r10)
34922 < IKFAST_ATAN2_MAGTHRESH
34923 && IKabs(new_r11)
34924 < IKFAST_ATAN2_MAGTHRESH
34925 && IKabs(
34926 IKsqr(new_r10)
34927 + IKsqr(
34928 new_r11)
34929 - 1)
34930 <= IKFAST_SINCOS_THRESH)
34931 continue;
34932 j12array[0] = IKatan2(
34933 new_r10, new_r11);
34934 sj12array[0]
34935 = IKsin(j12array[0]);
34936 cj12array[0]
34937 = IKcos(j12array[0]);
34938 if (j12array[0] > IKPI)
34939 {
34940 j12array[0] -= IK2PI;
34941 }
34942 else if (
34943 j12array[0] < -IKPI)
34944 {
34945 j12array[0] += IK2PI;
34946 }
34947 j12valid[0] = true;
34948 for (int ij12 = 0;
34949 ij12 < 1;
34950 ++ij12)
34951 {
34952 if (!j12valid[ij12])
34953 {
34954 continue;
34955 }
34956 _ij12[0] = ij12;
34957 _ij12[1] = -1;
34958 for (int iij12
34959 = ij12 + 1;
34960 iij12 < 1;
34961 ++iij12)
34962 {
34963 if (j12valid[iij12]
34964 && IKabs(
34965 cj12array
34966 [ij12]
34967 - cj12array
34968 [iij12])
34969 < IKFAST_SOLUTION_THRESH
34970 && IKabs(
34971 sj12array
34972 [ij12]
34973 - sj12array
34974 [iij12])
34975 < IKFAST_SOLUTION_THRESH)
34976 {
34977 j12valid[iij12]
34978 = false;
34979 _ij12[1] = iij12;
34980 break;
34981 }
34982 }
34983 j12 = j12array[ij12];
34984 cj12 = cj12array[ij12];
34985 sj12 = sj12array[ij12];
34986 {
34987 IkReal evalcond[8];
34988 IkReal x354
34989 = IKcos(j12);
34990 IkReal x355
34991 = IKsin(j12);
34992 IkReal x356
34993 = ((1.0) * x354);
34994 IkReal x357
34995 = ((-1.0) * x356);
34996 IkReal x358
34997 = ((1.0)
34998 * new_r02);
34999 evalcond[0]
35000 = (((new_r02
35001 * x354))
35002 + new_r20);
35003 evalcond[1]
35004 = ((((-1.0)
35005 * x355))
35006 + new_r10);
35007 evalcond[2]
35008 = (x357
35009 + new_r11);
35010 evalcond[3]
35011 = (((new_r22
35012 * x355))
35013 + new_r01);
35014 evalcond[4]
35015 = ((((-1.0) * x355
35016 * x358))
35017 + new_r21);
35018 evalcond[5]
35019 = ((((-1.0)
35020 * new_r22
35021 * x356))
35022 + new_r00);
35023 evalcond[6]
35024 = (x355
35025 + ((new_r01
35026 * new_r22))
35027 + (((-1.0)
35028 * new_r21
35029 * x358)));
35030 evalcond[7]
35031 = (x357
35032 + ((new_r00
35033 * new_r22))
35034 + (((-1.0)
35035 * new_r20
35036 * x358)));
35037 if (IKabs(evalcond[0])
35038 > IKFAST_EVALCOND_THRESH
35039 || IKabs(evalcond
35040 [1])
35041 > IKFAST_EVALCOND_THRESH
35042 || IKabs(evalcond
35043 [2])
35044 > IKFAST_EVALCOND_THRESH
35045 || IKabs(evalcond
35046 [3])
35047 > IKFAST_EVALCOND_THRESH
35048 || IKabs(evalcond
35049 [4])
35050 > IKFAST_EVALCOND_THRESH
35051 || IKabs(evalcond
35052 [5])
35053 > IKFAST_EVALCOND_THRESH
35054 || IKabs(evalcond
35055 [6])
35056 > IKFAST_EVALCOND_THRESH
35057 || IKabs(evalcond
35058 [7])
35059 > IKFAST_EVALCOND_THRESH)
35060 {
35061 continue;
35062 }
35063 }
35064
35065 {
35066 std::vector<
35067 IkSingleDOFSolutionBase<
35068 IkReal> >
35069 vinfos(7);
35070 vinfos[0].jointtype
35071 = 1;
35072 vinfos[0].foffset
35073 = j4;
35074 vinfos[0].indices[0]
35075 = _ij4[0];
35076 vinfos[0].indices[1]
35077 = _ij4[1];
35078 vinfos[0].maxsolutions
35079 = _nj4;
35080 vinfos[1].jointtype
35081 = 1;
35082 vinfos[1].foffset
35083 = j6;
35084 vinfos[1].indices[0]
35085 = _ij6[0];
35086 vinfos[1].indices[1]
35087 = _ij6[1];
35088 vinfos[1].maxsolutions
35089 = _nj6;
35090 vinfos[2].jointtype
35091 = 1;
35092 vinfos[2].foffset
35093 = j8;
35094 vinfos[2].indices[0]
35095 = _ij8[0];
35096 vinfos[2].indices[1]
35097 = _ij8[1];
35098 vinfos[2].maxsolutions
35099 = _nj8;
35100 vinfos[3].jointtype
35101 = 1;
35102 vinfos[3].foffset
35103 = j9;
35104 vinfos[3].indices[0]
35105 = _ij9[0];
35106 vinfos[3].indices[1]
35107 = _ij9[1];
35108 vinfos[3].maxsolutions
35109 = _nj9;
35110 vinfos[4].jointtype
35111 = 1;
35112 vinfos[4].foffset
35113 = j10;
35114 vinfos[4].indices[0]
35115 = _ij10[0];
35116 vinfos[4].indices[1]
35117 = _ij10[1];
35118 vinfos[4].maxsolutions
35119 = _nj10;
35120 vinfos[5].jointtype
35121 = 1;
35122 vinfos[5].foffset
35123 = j11;
35124 vinfos[5].indices[0]
35125 = _ij11[0];
35126 vinfos[5].indices[1]
35127 = _ij11[1];
35128 vinfos[5].maxsolutions
35129 = _nj11;
35130 vinfos[6].jointtype
35131 = 1;
35132 vinfos[6].foffset
35133 = j12;
35134 vinfos[6].indices[0]
35135 = _ij12[0];
35136 vinfos[6].indices[1]
35137 = _ij12[1];
35138 vinfos[6].maxsolutions
35139 = _nj12;
35140 std::vector<int>
35141 vfree(0);
35142 solutions.AddSolution(
35143 vinfos, vfree);
35144 }
35145 }
35146 }
35147 }
35148 } while (0);
35149 if (bgotonextstatement)
35150 {
35151 bool bgotonextstatement
35152 = true;
35153 do
35154 {
35155 IkReal x359
35156 = ((1.0) * cj11);
35157 IkReal x360
35158 = ((((-1.0) * x359))
35159 + new_r22);
35160 IkReal x361
35161 = ((1.0) * sj11);
35162 evalcond[0]
35163 = ((-3.14159265358979)
35164 + (IKfmod(
35165 ((3.14159265358979)
35166 + (IKabs((
35167 (-3.14159265358979)
35168 + j10)))),
35169 6.28318530717959)));
35170 evalcond[1] = x360;
35171 evalcond[2] = x360;
35172 evalcond[3]
35173 = (sj11 + new_r02);
35174 evalcond[4] = new_r12;
35175 evalcond[5]
35176 = ((((-1.0) * (1.0)
35177 * new_r02))
35178 + (((-1.0) * x361)));
35179 evalcond[6]
35180 = ((((-1.0) * new_r02
35181 * x359))
35182 + (((-1.0) * new_r22
35183 * x361)));
35184 evalcond[7]
35185 = ((((-1.0) * new_r20
35186 * x359))
35187 + ((new_r00
35188 * sj11)));
35189 evalcond[8]
35190 = (((new_r01 * sj11))
35191 + (((-1.0) * new_r21
35192 * x359)));
35193 evalcond[9]
35194 = ((1.0)
35195 + (((-1.0) * new_r22
35196 * x359))
35197 + ((new_r02
35198 * sj11)));
35199 if (IKabs(evalcond[0])
35200 < 0.0000010000000000
35201 && IKabs(evalcond[1])
35202 < 0.0000010000000000
35203 && IKabs(evalcond[2])
35204 < 0.0000010000000000
35205 && IKabs(evalcond[3])
35206 < 0.0000010000000000
35207 && IKabs(evalcond[4])
35208 < 0.0000010000000000
35209 && IKabs(evalcond[5])
35210 < 0.0000010000000000
35211 && IKabs(evalcond[6])
35212 < 0.0000010000000000
35213 && IKabs(evalcond[7])
35214 < 0.0000010000000000
35215 && IKabs(evalcond[8])
35216 < 0.0000010000000000
35217 && IKabs(evalcond[9])
35218 < 0.0000010000000000)
35219 {
35220 bgotonextstatement
35221 = false;
35222 {
35223 IkReal j12array[1],
35224 cj12array[1],
35225 sj12array[1];
35226 bool j12valid[1]
35227 = {false};
35228 _nj12 = 1;
35229 CheckValue<IkReal> x362
35230 = IKatan2WithCheck(
35231 IkReal((
35232 (-1.0)
35233 * (((1.0)
35234 * new_r21)))),
35235 new_r20,
35236 IKFAST_ATAN2_MAGTHRESH);
35237 if (!x362.valid)
35238 {
35239 continue;
35240 }
35241 CheckValue<IkReal> x363
35242 = IKPowWithIntegerCheck(
35243 IKsign(new_r02),
35244 -1);
35245 if (!x363.valid)
35246 {
35247 continue;
35248 }
35249 j12array[0]
35250 = ((-1.5707963267949)
35251 + (x362.value)
35252 + (((1.5707963267949)
35253 * (x363.value))));
35254 sj12array[0] = IKsin(
35255 j12array[0]);
35256 cj12array[0] = IKcos(
35257 j12array[0]);
35258 if (j12array[0] > IKPI)
35259 {
35260 j12array[0] -= IK2PI;
35261 }
35262 else if (
35263 j12array[0] < -IKPI)
35264 {
35265 j12array[0] += IK2PI;
35266 }
35267 j12valid[0] = true;
35268 for (int ij12 = 0;
35269 ij12 < 1;
35270 ++ij12)
35271 {
35272 if (!j12valid[ij12])
35273 {
35274 continue;
35275 }
35276 _ij12[0] = ij12;
35277 _ij12[1] = -1;
35278 for (int iij12
35279 = ij12 + 1;
35280 iij12 < 1;
35281 ++iij12)
35282 {
35283 if (j12valid[iij12]
35284 && IKabs(
35285 cj12array
35286 [ij12]
35287 - cj12array
35288 [iij12])
35289 < IKFAST_SOLUTION_THRESH
35290 && IKabs(
35291 sj12array
35292 [ij12]
35293 - sj12array
35294 [iij12])
35295 < IKFAST_SOLUTION_THRESH)
35296 {
35297 j12valid[iij12]
35298 = false;
35299 _ij12[1] = iij12;
35300 break;
35301 }
35302 }
35303 j12 = j12array[ij12];
35304 cj12
35305 = cj12array[ij12];
35306 sj12
35307 = sj12array[ij12];
35308 {
35309 IkReal evalcond[8];
35310 IkReal x364
35311 = IKsin(j12);
35312 IkReal x365
35313 = ((1.0)
35314 * (IKcos(
35315 j12)));
35316 IkReal x366
35317 = ((-1.0)
35318 * x365);
35319 IkReal x367
35320 = ((1.0)
35321 * new_r01);
35322 IkReal x368
35323 = ((1.0)
35324 * new_r00);
35325 evalcond[0]
35326 = (((new_r02
35327 * x364))
35328 + new_r21);
35329 evalcond[1]
35330 = ((((-1.0)
35331 * new_r02
35332 * x365))
35333 + new_r20);
35334 evalcond[2]
35335 = ((((-1.0)
35336 * x364))
35337 + (((-1.0)
35338 * (1.0)
35339 * new_r10)));
35340 evalcond[3]
35341 = (x366
35342 + (((-1.0)
35343 * (1.0)
35344 * new_r11)));
35345 evalcond[4]
35346 = ((((-1.0)
35347 * x367))
35348 + ((new_r22
35349 * x364)));
35350 evalcond[5]
35351 = ((((-1.0)
35352 * new_r22
35353 * x365))
35354 + (((-1.0)
35355 * x368)));
35356 evalcond[6]
35357 = (((new_r02
35358 * new_r21))
35359 + x364
35360 + (((-1.0)
35361 * new_r22
35362 * x367)));
35363 evalcond[7]
35364 = (((new_r02
35365 * new_r20))
35366 + x366
35367 + (((-1.0)
35368 * new_r22
35369 * x368)));
35370 if (IKabs(
35371 evalcond[0])
35372 > IKFAST_EVALCOND_THRESH
35373 || IKabs(
35374 evalcond
35375 [1])
35376 > IKFAST_EVALCOND_THRESH
35377 || IKabs(
35378 evalcond
35379 [2])
35380 > IKFAST_EVALCOND_THRESH
35381 || IKabs(
35382 evalcond
35383 [3])
35384 > IKFAST_EVALCOND_THRESH
35385 || IKabs(
35386 evalcond
35387 [4])
35388 > IKFAST_EVALCOND_THRESH
35389 || IKabs(
35390 evalcond
35391 [5])
35392 > IKFAST_EVALCOND_THRESH
35393 || IKabs(
35394 evalcond
35395 [6])
35396 > IKFAST_EVALCOND_THRESH
35397 || IKabs(
35398 evalcond
35399 [7])
35400 > IKFAST_EVALCOND_THRESH)
35401 {
35402 continue;
35403 }
35404 }
35405
35406 {
35407 std::vector<
35408 IkSingleDOFSolutionBase<
35409 IkReal> >
35410 vinfos(7);
35411 vinfos[0].jointtype
35412 = 1;
35413 vinfos[0].foffset
35414 = j4;
35415 vinfos[0].indices[0]
35416 = _ij4[0];
35417 vinfos[0].indices[1]
35418 = _ij4[1];
35419 vinfos[0]
35420 .maxsolutions
35421 = _nj4;
35422 vinfos[1].jointtype
35423 = 1;
35424 vinfos[1].foffset
35425 = j6;
35426 vinfos[1].indices[0]
35427 = _ij6[0];
35428 vinfos[1].indices[1]
35429 = _ij6[1];
35430 vinfos[1]
35431 .maxsolutions
35432 = _nj6;
35433 vinfos[2].jointtype
35434 = 1;
35435 vinfos[2].foffset
35436 = j8;
35437 vinfos[2].indices[0]
35438 = _ij8[0];
35439 vinfos[2].indices[1]
35440 = _ij8[1];
35441 vinfos[2]
35442 .maxsolutions
35443 = _nj8;
35444 vinfos[3].jointtype
35445 = 1;
35446 vinfos[3].foffset
35447 = j9;
35448 vinfos[3].indices[0]
35449 = _ij9[0];
35450 vinfos[3].indices[1]
35451 = _ij9[1];
35452 vinfos[3]
35453 .maxsolutions
35454 = _nj9;
35455 vinfos[4].jointtype
35456 = 1;
35457 vinfos[4].foffset
35458 = j10;
35459 vinfos[4].indices[0]
35460 = _ij10[0];
35461 vinfos[4].indices[1]
35462 = _ij10[1];
35463 vinfos[4]
35464 .maxsolutions
35465 = _nj10;
35466 vinfos[5].jointtype
35467 = 1;
35468 vinfos[5].foffset
35469 = j11;
35470 vinfos[5].indices[0]
35471 = _ij11[0];
35472 vinfos[5].indices[1]
35473 = _ij11[1];
35474 vinfos[5]
35475 .maxsolutions
35476 = _nj11;
35477 vinfos[6].jointtype
35478 = 1;
35479 vinfos[6].foffset
35480 = j12;
35481 vinfos[6].indices[0]
35482 = _ij12[0];
35483 vinfos[6].indices[1]
35484 = _ij12[1];
35485 vinfos[6]
35486 .maxsolutions
35487 = _nj12;
35488 std::vector<int>
35489 vfree(0);
35490 solutions
35491 .AddSolution(
35492 vinfos,
35493 vfree);
35494 }
35495 }
35496 }
35497 }
35498 } while (0);
35499 if (bgotonextstatement)
35500 {
35501 bool bgotonextstatement
35502 = true;
35503 do
35504 {
35505 if (1)
35506 {
35507 bgotonextstatement
35508 = false;
35509 continue; // branch miss
35510 // [j12]
35511 }
35512 } while (0);
35513 if (bgotonextstatement)
35514 {
35515 }
35516 }
35517 }
35518 }
35519 }
35520 }
35521 }
35522 }
35523 }
35524 }
35525 }
35526 else
35527 {
35528 {
35529 IkReal j12array[1], cj12array[1],
35530 sj12array[1];
35531 bool j12valid[1] = {false};
35532 _nj12 = 1;
35533 CheckValue<IkReal> x370
35534 = IKPowWithIntegerCheck(sj11, -1);
35535 if (!x370.valid)
35536 {
35537 continue;
35538 }
35539 IkReal x369 = x370.value;
35540 CheckValue<IkReal> x371
35541 = IKPowWithIntegerCheck(cj10, -1);
35542 if (!x371.valid)
35543 {
35544 continue;
35545 }
35546 CheckValue<IkReal> x372
35547 = IKPowWithIntegerCheck(cj11, -1);
35548 if (!x372.valid)
35549 {
35550 continue;
35551 }
35552 if (IKabs(
35553 (x369 * (x371.value) * (x372.value)
35554 * (((((-1.0) * (1.0) * new_r01
35555 * sj11))
35556 + ((new_r20 * sj10))))))
35557 < IKFAST_ATAN2_MAGTHRESH
35558 && IKabs(((-1.0) * new_r20 * x369))
35559 < IKFAST_ATAN2_MAGTHRESH
35560 && IKabs(
35561 IKsqr(
35562 (x369 * (x371.value)
35563 * (x372.value)
35564 * (((((-1.0) * (1.0)
35565 * new_r01 * sj11))
35566 + ((new_r20 * sj10))))))
35567 + IKsqr(
35568 ((-1.0) * new_r20 * x369))
35569 - 1)
35570 <= IKFAST_SINCOS_THRESH)
35571 continue;
35572 j12array[0] = IKatan2(
35573 (x369 * (x371.value) * (x372.value)
35574 * (((((-1.0) * (1.0) * new_r01 * sj11))
35575 + ((new_r20 * sj10))))),
35576 ((-1.0) * new_r20 * x369));
35577 sj12array[0] = IKsin(j12array[0]);
35578 cj12array[0] = IKcos(j12array[0]);
35579 if (j12array[0] > IKPI)
35580 {
35581 j12array[0] -= IK2PI;
35582 }
35583 else if (j12array[0] < -IKPI)
35584 {
35585 j12array[0] += IK2PI;
35586 }
35587 j12valid[0] = true;
35588 for (int ij12 = 0; ij12 < 1; ++ij12)
35589 {
35590 if (!j12valid[ij12])
35591 {
35592 continue;
35593 }
35594 _ij12[0] = ij12;
35595 _ij12[1] = -1;
35596 for (int iij12 = ij12 + 1; iij12 < 1;
35597 ++iij12)
35598 {
35599 if (j12valid[iij12]
35600 && IKabs(
35601 cj12array[ij12]
35602 - cj12array[iij12])
35603 < IKFAST_SOLUTION_THRESH
35604 && IKabs(
35605 sj12array[ij12]
35606 - sj12array[iij12])
35607 < IKFAST_SOLUTION_THRESH)
35608 {
35609 j12valid[iij12] = false;
35610 _ij12[1] = iij12;
35611 break;
35612 }
35613 }
35614 j12 = j12array[ij12];
35615 cj12 = cj12array[ij12];
35616 sj12 = sj12array[ij12];
35617 {
35618 IkReal evalcond[12];
35619 IkReal x373 = IKcos(j12);
35620 IkReal x374 = IKsin(j12);
35621 IkReal x375 = ((1.0) * sj11);
35622 IkReal x376 = (cj10 * new_r01);
35623 IkReal x377 = (new_r11 * sj10);
35624 IkReal x378 = (cj11 * x374);
35625 IkReal x379 = ((1.0) * sj10);
35626 IkReal x380 = ((1.0) * x374);
35627 IkReal x381 = ((1.0) * x373);
35628 IkReal x382 = ((-1.0) * x381);
35629 IkReal x383 = (cj10 * new_r00);
35630 IkReal x384 = (new_r10 * sj10);
35631 IkReal x385 = (cj10 * x381);
35632 evalcond[0]
35633 = (((sj11 * x373)) + new_r20);
35634 evalcond[1]
35635 = ((((-1.0) * x374 * x375))
35636 + new_r21);
35637 evalcond[2] = (x378 + x377 + x376);
35638 evalcond[3]
35639 = (((cj10 * new_r10))
35640 + (((-1.0) * x380))
35641 + (((-1.0) * new_r00 * x379)));
35642 evalcond[4]
35643 = (((cj10 * new_r11)) + x382
35644 + (((-1.0) * new_r01 * x379)));
35645 evalcond[5]
35646 = (((sj10 * x373)) + ((cj10 * x378))
35647 + new_r01);
35648 evalcond[6]
35649 = ((((-1.0) * cj11 * x381)) + x384
35650 + x383);
35651 evalcond[7]
35652 = (((sj10 * x374))
35653 + (((-1.0) * cj11 * x385))
35654 + new_r00);
35655 evalcond[8]
35656 = ((((-1.0) * x385)) + new_r11
35657 + ((sj10 * x378)));
35658 evalcond[9]
35659 = ((((-1.0) * cj10 * x380))
35660 + (((-1.0) * cj11 * x373 * x379))
35661 + new_r10);
35662 evalcond[10]
35663 = (((cj11 * x376)) + ((cj11 * x377))
35664 + (((-1.0) * new_r21 * x375))
35665 + x374);
35666 evalcond[11]
35667 = ((((-1.0) * new_r20 * x375))
35668 + x382 + ((cj11 * x384))
35669 + ((cj11 * x383)));
35670 if (IKabs(evalcond[0])
35671 > IKFAST_EVALCOND_THRESH
35672 || IKabs(evalcond[1])
35673 > IKFAST_EVALCOND_THRESH
35674 || IKabs(evalcond[2])
35675 > IKFAST_EVALCOND_THRESH
35676 || IKabs(evalcond[3])
35677 > IKFAST_EVALCOND_THRESH
35678 || IKabs(evalcond[4])
35679 > IKFAST_EVALCOND_THRESH
35680 || IKabs(evalcond[5])
35681 > IKFAST_EVALCOND_THRESH
35682 || IKabs(evalcond[6])
35683 > IKFAST_EVALCOND_THRESH
35684 || IKabs(evalcond[7])
35685 > IKFAST_EVALCOND_THRESH
35686 || IKabs(evalcond[8])
35687 > IKFAST_EVALCOND_THRESH
35688 || IKabs(evalcond[9])
35689 > IKFAST_EVALCOND_THRESH
35690 || IKabs(evalcond[10])
35691 > IKFAST_EVALCOND_THRESH
35692 || IKabs(evalcond[11])
35693 > IKFAST_EVALCOND_THRESH)
35694 {
35695 continue;
35696 }
35697 }
35698
35699 {
35700 std::vector<
35701 IkSingleDOFSolutionBase<IkReal> >
35702 vinfos(7);
35703 vinfos[0].jointtype = 1;
35704 vinfos[0].foffset = j4;
35705 vinfos[0].indices[0] = _ij4[0];
35706 vinfos[0].indices[1] = _ij4[1];
35707 vinfos[0].maxsolutions = _nj4;
35708 vinfos[1].jointtype = 1;
35709 vinfos[1].foffset = j6;
35710 vinfos[1].indices[0] = _ij6[0];
35711 vinfos[1].indices[1] = _ij6[1];
35712 vinfos[1].maxsolutions = _nj6;
35713 vinfos[2].jointtype = 1;
35714 vinfos[2].foffset = j8;
35715 vinfos[2].indices[0] = _ij8[0];
35716 vinfos[2].indices[1] = _ij8[1];
35717 vinfos[2].maxsolutions = _nj8;
35718 vinfos[3].jointtype = 1;
35719 vinfos[3].foffset = j9;
35720 vinfos[3].indices[0] = _ij9[0];
35721 vinfos[3].indices[1] = _ij9[1];
35722 vinfos[3].maxsolutions = _nj9;
35723 vinfos[4].jointtype = 1;
35724 vinfos[4].foffset = j10;
35725 vinfos[4].indices[0] = _ij10[0];
35726 vinfos[4].indices[1] = _ij10[1];
35727 vinfos[4].maxsolutions = _nj10;
35728 vinfos[5].jointtype = 1;
35729 vinfos[5].foffset = j11;
35730 vinfos[5].indices[0] = _ij11[0];
35731 vinfos[5].indices[1] = _ij11[1];
35732 vinfos[5].maxsolutions = _nj11;
35733 vinfos[6].jointtype = 1;
35734 vinfos[6].foffset = j12;
35735 vinfos[6].indices[0] = _ij12[0];
35736 vinfos[6].indices[1] = _ij12[1];
35737 vinfos[6].maxsolutions = _nj12;
35738 std::vector<int> vfree(0);
35739 solutions.AddSolution(vinfos, vfree);
35740 }
35741 }
35742 }
35743 }
35744 }
35745 }
35746 else
35747 {
35748 {
35749 IkReal j12array[1], cj12array[1], sj12array[1];
35750 bool j12valid[1] = {false};
35751 _nj12 = 1;
35752 CheckValue<IkReal> x388
35753 = IKPowWithIntegerCheck(sj11, -1);
35754 if (!x388.valid)
35755 {
35756 continue;
35757 }
35758 IkReal x386 = x388.value;
35759 IkReal x387 = ((1.0) * new_r20);
35760 CheckValue<IkReal> x389
35761 = IKPowWithIntegerCheck(sj10, -1);
35762 if (!x389.valid)
35763 {
35764 continue;
35765 }
35766 if (IKabs(
35767 (x386 * (x389.value)
35768 * (((((-1.0) * cj10 * cj11 * x387))
35769 + (((-1.0) * (1.0) * new_r00
35770 * sj11))))))
35771 < IKFAST_ATAN2_MAGTHRESH
35772 && IKabs(((-1.0) * x386 * x387))
35773 < IKFAST_ATAN2_MAGTHRESH
35774 && IKabs(
35775 IKsqr(
35776 (x386 * (x389.value)
35777 * (((((-1.0) * cj10 * cj11
35778 * x387))
35779 + (((-1.0) * (1.0) * new_r00
35780 * sj11))))))
35781 + IKsqr(((-1.0) * x386 * x387)) - 1)
35782 <= IKFAST_SINCOS_THRESH)
35783 continue;
35784 j12array[0] = IKatan2(
35785 (x386 * (x389.value)
35786 * (((((-1.0) * cj10 * cj11 * x387))
35787 + (((-1.0) * (1.0) * new_r00
35788 * sj11))))),
35789 ((-1.0) * x386 * x387));
35790 sj12array[0] = IKsin(j12array[0]);
35791 cj12array[0] = IKcos(j12array[0]);
35792 if (j12array[0] > IKPI)
35793 {
35794 j12array[0] -= IK2PI;
35795 }
35796 else if (j12array[0] < -IKPI)
35797 {
35798 j12array[0] += IK2PI;
35799 }
35800 j12valid[0] = true;
35801 for (int ij12 = 0; ij12 < 1; ++ij12)
35802 {
35803 if (!j12valid[ij12])
35804 {
35805 continue;
35806 }
35807 _ij12[0] = ij12;
35808 _ij12[1] = -1;
35809 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
35810 {
35811 if (j12valid[iij12]
35812 && IKabs(
35813 cj12array[ij12]
35814 - cj12array[iij12])
35815 < IKFAST_SOLUTION_THRESH
35816 && IKabs(
35817 sj12array[ij12]
35818 - sj12array[iij12])
35819 < IKFAST_SOLUTION_THRESH)
35820 {
35821 j12valid[iij12] = false;
35822 _ij12[1] = iij12;
35823 break;
35824 }
35825 }
35826 j12 = j12array[ij12];
35827 cj12 = cj12array[ij12];
35828 sj12 = sj12array[ij12];
35829 {
35830 IkReal evalcond[12];
35831 IkReal x390 = IKcos(j12);
35832 IkReal x391 = IKsin(j12);
35833 IkReal x392 = ((1.0) * sj11);
35834 IkReal x393 = (cj10 * new_r01);
35835 IkReal x394 = (new_r11 * sj10);
35836 IkReal x395 = (cj11 * x391);
35837 IkReal x396 = ((1.0) * sj10);
35838 IkReal x397 = ((1.0) * x391);
35839 IkReal x398 = ((1.0) * x390);
35840 IkReal x399 = ((-1.0) * x398);
35841 IkReal x400 = (cj10 * new_r00);
35842 IkReal x401 = (new_r10 * sj10);
35843 IkReal x402 = (cj10 * x398);
35844 evalcond[0] = (((sj11 * x390)) + new_r20);
35845 evalcond[1]
35846 = ((((-1.0) * x391 * x392)) + new_r21);
35847 evalcond[2] = (x394 + x395 + x393);
35848 evalcond[3]
35849 = (((cj10 * new_r10))
35850 + (((-1.0) * x397))
35851 + (((-1.0) * new_r00 * x396)));
35852 evalcond[4]
35853 = (((cj10 * new_r11)) + x399
35854 + (((-1.0) * new_r01 * x396)));
35855 evalcond[5]
35856 = (((cj10 * x395)) + ((sj10 * x390))
35857 + new_r01);
35858 evalcond[6]
35859 = (x401 + x400
35860 + (((-1.0) * cj11 * x398)));
35861 evalcond[7]
35862 = (new_r00 + (((-1.0) * cj11 * x402))
35863 + ((sj10 * x391)));
35864 evalcond[8]
35865 = ((((-1.0) * x402)) + ((sj10 * x395))
35866 + new_r11);
35867 evalcond[9]
35868 = ((((-1.0) * cj10 * x397)) + new_r10
35869 + (((-1.0) * cj11 * x390 * x396)));
35870 evalcond[10]
35871 = (x391 + ((cj11 * x393))
35872 + ((cj11 * x394))
35873 + (((-1.0) * new_r21 * x392)));
35874 evalcond[11]
35875 = (x399 + ((cj11 * x400))
35876 + (((-1.0) * new_r20 * x392))
35877 + ((cj11 * x401)));
35878 if (IKabs(evalcond[0])
35879 > IKFAST_EVALCOND_THRESH
35880 || IKabs(evalcond[1])
35881 > IKFAST_EVALCOND_THRESH
35882 || IKabs(evalcond[2])
35883 > IKFAST_EVALCOND_THRESH
35884 || IKabs(evalcond[3])
35885 > IKFAST_EVALCOND_THRESH
35886 || IKabs(evalcond[4])
35887 > IKFAST_EVALCOND_THRESH
35888 || IKabs(evalcond[5])
35889 > IKFAST_EVALCOND_THRESH
35890 || IKabs(evalcond[6])
35891 > IKFAST_EVALCOND_THRESH
35892 || IKabs(evalcond[7])
35893 > IKFAST_EVALCOND_THRESH
35894 || IKabs(evalcond[8])
35895 > IKFAST_EVALCOND_THRESH
35896 || IKabs(evalcond[9])
35897 > IKFAST_EVALCOND_THRESH
35898 || IKabs(evalcond[10])
35899 > IKFAST_EVALCOND_THRESH
35900 || IKabs(evalcond[11])
35901 > IKFAST_EVALCOND_THRESH)
35902 {
35903 continue;
35904 }
35905 }
35906
35907 {
35908 std::vector<
35909 IkSingleDOFSolutionBase<IkReal> >
35910 vinfos(7);
35911 vinfos[0].jointtype = 1;
35912 vinfos[0].foffset = j4;
35913 vinfos[0].indices[0] = _ij4[0];
35914 vinfos[0].indices[1] = _ij4[1];
35915 vinfos[0].maxsolutions = _nj4;
35916 vinfos[1].jointtype = 1;
35917 vinfos[1].foffset = j6;
35918 vinfos[1].indices[0] = _ij6[0];
35919 vinfos[1].indices[1] = _ij6[1];
35920 vinfos[1].maxsolutions = _nj6;
35921 vinfos[2].jointtype = 1;
35922 vinfos[2].foffset = j8;
35923 vinfos[2].indices[0] = _ij8[0];
35924 vinfos[2].indices[1] = _ij8[1];
35925 vinfos[2].maxsolutions = _nj8;
35926 vinfos[3].jointtype = 1;
35927 vinfos[3].foffset = j9;
35928 vinfos[3].indices[0] = _ij9[0];
35929 vinfos[3].indices[1] = _ij9[1];
35930 vinfos[3].maxsolutions = _nj9;
35931 vinfos[4].jointtype = 1;
35932 vinfos[4].foffset = j10;
35933 vinfos[4].indices[0] = _ij10[0];
35934 vinfos[4].indices[1] = _ij10[1];
35935 vinfos[4].maxsolutions = _nj10;
35936 vinfos[5].jointtype = 1;
35937 vinfos[5].foffset = j11;
35938 vinfos[5].indices[0] = _ij11[0];
35939 vinfos[5].indices[1] = _ij11[1];
35940 vinfos[5].maxsolutions = _nj11;
35941 vinfos[6].jointtype = 1;
35942 vinfos[6].foffset = j12;
35943 vinfos[6].indices[0] = _ij12[0];
35944 vinfos[6].indices[1] = _ij12[1];
35945 vinfos[6].maxsolutions = _nj12;
35946 std::vector<int> vfree(0);
35947 solutions.AddSolution(vinfos, vfree);
35948 }
35949 }
35950 }
35951 }
35952 }
35953 }
35954 else
35955 {
35956 {
35957 IkReal j12array[1], cj12array[1], sj12array[1];
35958 bool j12valid[1] = {false};
35959 _nj12 = 1;
35960 CheckValue<IkReal> x403
35961 = IKPowWithIntegerCheck(IKsign(sj11), -1);
35962 if (!x403.valid)
35963 {
35964 continue;
35965 }
35966 CheckValue<IkReal> x404 = IKatan2WithCheck(
35967 IkReal(new_r21),
35968 ((-1.0) * (((1.0) * new_r20))),
35969 IKFAST_ATAN2_MAGTHRESH);
35970 if (!x404.valid)
35971 {
35972 continue;
35973 }
35974 j12array[0]
35975 = ((-1.5707963267949)
35976 + (((1.5707963267949) * (x403.value)))
35977 + (x404.value));
35978 sj12array[0] = IKsin(j12array[0]);
35979 cj12array[0] = IKcos(j12array[0]);
35980 if (j12array[0] > IKPI)
35981 {
35982 j12array[0] -= IK2PI;
35983 }
35984 else if (j12array[0] < -IKPI)
35985 {
35986 j12array[0] += IK2PI;
35987 }
35988 j12valid[0] = true;
35989 for (int ij12 = 0; ij12 < 1; ++ij12)
35990 {
35991 if (!j12valid[ij12])
35992 {
35993 continue;
35994 }
35995 _ij12[0] = ij12;
35996 _ij12[1] = -1;
35997 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
35998 {
35999 if (j12valid[iij12]
36000 && IKabs(cj12array[ij12] - cj12array[iij12])
36001 < IKFAST_SOLUTION_THRESH
36002 && IKabs(sj12array[ij12] - sj12array[iij12])
36003 < IKFAST_SOLUTION_THRESH)
36004 {
36005 j12valid[iij12] = false;
36006 _ij12[1] = iij12;
36007 break;
36008 }
36009 }
36010 j12 = j12array[ij12];
36011 cj12 = cj12array[ij12];
36012 sj12 = sj12array[ij12];
36013 {
36014 IkReal evalcond[12];
36015 IkReal x405 = IKcos(j12);
36016 IkReal x406 = IKsin(j12);
36017 IkReal x407 = ((1.0) * sj11);
36018 IkReal x408 = (cj10 * new_r01);
36019 IkReal x409 = (new_r11 * sj10);
36020 IkReal x410 = (cj11 * x406);
36021 IkReal x411 = ((1.0) * sj10);
36022 IkReal x412 = ((1.0) * x406);
36023 IkReal x413 = ((1.0) * x405);
36024 IkReal x414 = ((-1.0) * x413);
36025 IkReal x415 = (cj10 * new_r00);
36026 IkReal x416 = (new_r10 * sj10);
36027 IkReal x417 = (cj10 * x413);
36028 evalcond[0] = (((sj11 * x405)) + new_r20);
36029 evalcond[1]
36030 = ((((-1.0) * x406 * x407)) + new_r21);
36031 evalcond[2] = (x410 + x409 + x408);
36032 evalcond[3]
36033 = ((((-1.0) * new_r00 * x411))
36034 + ((cj10 * new_r10))
36035 + (((-1.0) * x412)));
36036 evalcond[4]
36037 = (((cj10 * new_r11)) + x414
36038 + (((-1.0) * new_r01 * x411)));
36039 evalcond[5]
36040 = (((cj10 * x410)) + ((sj10 * x405))
36041 + new_r01);
36042 evalcond[6]
36043 = ((((-1.0) * cj11 * x413)) + x415 + x416);
36044 evalcond[7]
36045 = ((((-1.0) * cj11 * x417))
36046 + ((sj10 * x406)) + new_r00);
36047 evalcond[8]
36048 = (((sj10 * x410)) + (((-1.0) * x417))
36049 + new_r11);
36050 evalcond[9]
36051 = ((((-1.0) * cj10 * x412))
36052 + (((-1.0) * cj11 * x405 * x411))
36053 + new_r10);
36054 evalcond[10]
36055 = (((cj11 * x409))
36056 + (((-1.0) * new_r21 * x407)) + x406
36057 + ((cj11 * x408)));
36058 evalcond[11]
36059 = (x414 + ((cj11 * x416)) + ((cj11 * x415))
36060 + (((-1.0) * new_r20 * x407)));
36061 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
36062 || IKabs(evalcond[1])
36063 > IKFAST_EVALCOND_THRESH
36064 || IKabs(evalcond[2])
36065 > IKFAST_EVALCOND_THRESH
36066 || IKabs(evalcond[3])
36067 > IKFAST_EVALCOND_THRESH
36068 || IKabs(evalcond[4])
36069 > IKFAST_EVALCOND_THRESH
36070 || IKabs(evalcond[5])
36071 > IKFAST_EVALCOND_THRESH
36072 || IKabs(evalcond[6])
36073 > IKFAST_EVALCOND_THRESH
36074 || IKabs(evalcond[7])
36075 > IKFAST_EVALCOND_THRESH
36076 || IKabs(evalcond[8])
36077 > IKFAST_EVALCOND_THRESH
36078 || IKabs(evalcond[9])
36079 > IKFAST_EVALCOND_THRESH
36080 || IKabs(evalcond[10])
36081 > IKFAST_EVALCOND_THRESH
36082 || IKabs(evalcond[11])
36083 > IKFAST_EVALCOND_THRESH)
36084 {
36085 continue;
36086 }
36087 }
36088
36089 {
36090 std::vector<IkSingleDOFSolutionBase<IkReal> >
36091 vinfos(7);
36092 vinfos[0].jointtype = 1;
36093 vinfos[0].foffset = j4;
36094 vinfos[0].indices[0] = _ij4[0];
36095 vinfos[0].indices[1] = _ij4[1];
36096 vinfos[0].maxsolutions = _nj4;
36097 vinfos[1].jointtype = 1;
36098 vinfos[1].foffset = j6;
36099 vinfos[1].indices[0] = _ij6[0];
36100 vinfos[1].indices[1] = _ij6[1];
36101 vinfos[1].maxsolutions = _nj6;
36102 vinfos[2].jointtype = 1;
36103 vinfos[2].foffset = j8;
36104 vinfos[2].indices[0] = _ij8[0];
36105 vinfos[2].indices[1] = _ij8[1];
36106 vinfos[2].maxsolutions = _nj8;
36107 vinfos[3].jointtype = 1;
36108 vinfos[3].foffset = j9;
36109 vinfos[3].indices[0] = _ij9[0];
36110 vinfos[3].indices[1] = _ij9[1];
36111 vinfos[3].maxsolutions = _nj9;
36112 vinfos[4].jointtype = 1;
36113 vinfos[4].foffset = j10;
36114 vinfos[4].indices[0] = _ij10[0];
36115 vinfos[4].indices[1] = _ij10[1];
36116 vinfos[4].maxsolutions = _nj10;
36117 vinfos[5].jointtype = 1;
36118 vinfos[5].foffset = j11;
36119 vinfos[5].indices[0] = _ij11[0];
36120 vinfos[5].indices[1] = _ij11[1];
36121 vinfos[5].maxsolutions = _nj11;
36122 vinfos[6].jointtype = 1;
36123 vinfos[6].foffset = j12;
36124 vinfos[6].indices[0] = _ij12[0];
36125 vinfos[6].indices[1] = _ij12[1];
36126 vinfos[6].maxsolutions = _nj12;
36127 std::vector<int> vfree(0);
36128 solutions.AddSolution(vinfos, vfree);
36129 }
36130 }
36131 }
36132 }
36133 }
36134 }
36135 }
36136 }
36137 }
36138 }
36139 else
36140 {
36141 {
36142 IkReal j10array[1], cj10array[1], sj10array[1];
36143 bool j10valid[1] = {false};
36144 _nj10 = 1;
36145 CheckValue<IkReal> x418 = IKatan2WithCheck(
36146 IkReal(new_r12), new_r02, IKFAST_ATAN2_MAGTHRESH);
36147 if (!x418.valid)
36148 {
36149 continue;
36150 }
36151 CheckValue<IkReal> x419
36152 = IKPowWithIntegerCheck(IKsign(sj11), -1);
36153 if (!x419.valid)
36154 {
36155 continue;
36156 }
36157 j10array[0]
36158 = ((-1.5707963267949) + (x418.value)
36159 + (((1.5707963267949) * (x419.value))));
36160 sj10array[0] = IKsin(j10array[0]);
36161 cj10array[0] = IKcos(j10array[0]);
36162 if (j10array[0] > IKPI)
36163 {
36164 j10array[0] -= IK2PI;
36165 }
36166 else if (j10array[0] < -IKPI)
36167 {
36168 j10array[0] += IK2PI;
36169 }
36170 j10valid[0] = true;
36171 for (int ij10 = 0; ij10 < 1; ++ij10)
36172 {
36173 if (!j10valid[ij10])
36174 {
36175 continue;
36176 }
36177 _ij10[0] = ij10;
36178 _ij10[1] = -1;
36179 for (int iij10 = ij10 + 1; iij10 < 1; ++iij10)
36180 {
36181 if (j10valid[iij10]
36182 && IKabs(cj10array[ij10] - cj10array[iij10])
36183 < IKFAST_SOLUTION_THRESH
36184 && IKabs(sj10array[ij10] - sj10array[iij10])
36185 < IKFAST_SOLUTION_THRESH)
36186 {
36187 j10valid[iij10] = false;
36188 _ij10[1] = iij10;
36189 break;
36190 }
36191 }
36192 j10 = j10array[ij10];
36193 cj10 = cj10array[ij10];
36194 sj10 = sj10array[ij10];
36195 {
36196 IkReal evalcond[8];
36197 IkReal x420 = IKcos(j10);
36198 IkReal x421 = ((1.0) * sj11);
36199 IkReal x422 = (x420 * x421);
36200 IkReal x423 = IKsin(j10);
36201 IkReal x424 = (x421 * x423);
36202 IkReal x425 = (new_r02 * x420);
36203 IkReal x426 = (new_r12 * x423);
36204 IkReal x427 = ((1.0) * cj11);
36205 evalcond[0] = ((((-1.0) * x422)) + new_r02);
36206 evalcond[1] = ((((-1.0) * x424)) + new_r12);
36207 evalcond[2]
36208 = ((((-1.0) * new_r02 * x423)) + ((new_r12 * x420)));
36209 evalcond[3] = ((((-1.0) * x421)) + x426 + x425);
36210 evalcond[4]
36211 = (((cj11 * x426)) + ((cj11 * x425))
36212 + (((-1.0) * new_r22 * x421)));
36213 evalcond[5]
36214 = ((((-1.0) * new_r10 * x424))
36215 + (((-1.0) * new_r20 * x427))
36216 + (((-1.0) * new_r00 * x422)));
36217 evalcond[6]
36218 = ((((-1.0) * new_r21 * x427))
36219 + (((-1.0) * new_r01 * x422))
36220 + (((-1.0) * new_r11 * x424)));
36221 evalcond[7]
36222 = ((1.0) + (((-1.0) * new_r22 * x427))
36223 + (((-1.0) * x421 * x425))
36224 + (((-1.0) * x421 * x426)));
36225 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
36226 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
36227 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
36228 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
36229 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
36230 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
36231 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
36232 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH)
36233 {
36234 continue;
36235 }
36236 }
36237
36238 {
36239 IkReal j12eval[2];
36240 IkReal x428 = ((1.0) * sj9);
36241 IkReal x429 = (cj9 * sj6);
36242 IkReal x430 = x160;
36243 IkReal x431 = (cj4 * sj8);
36244 IkReal x432 = (sj6 * x428);
36245 IkReal x433 = (cj6 * cj9);
36246 IkReal x434 = ((((-1.0) * x432)) + ((cj8 * x433)));
36247 IkReal x435 = (((cj9 * x431)) + ((sj4 * x434)));
36248 IkReal x436 = ((1.0) * sj4 * sj8);
36249 IkReal x437 = ((((-1.0) * cj9 * x436)) + ((cj4 * x434)));
36250 IkReal x438 = (sj6 * sj8);
36251 IkReal x439 = x169;
36252 IkReal x440 = x170;
36253 IkReal x441 = ((((-1.0) * cj8 * x432)) + x433);
36254 IkReal x442 = x172;
36255 IkReal x443 = (((sj9 * x431)) + ((sj4 * x442)));
36256 IkReal x444
36257 = (((cj4 * x442)) + (((-1.0) * sj4 * sj8 * x428)));
36258 new_r00
36259 = (((r20 * x430)) + ((r10 * x435)) + ((r00 * x437)));
36260 new_r01
36261 = (((r11 * x435)) + ((r01 * x437)) + ((r21 * x430)));
36262 new_r02
36263 = (((r12 * x435)) + ((r02 * x437)) + ((r22 * x430)));
36264 new_r10
36265 = (((r20 * x438)) + ((r00 * x440)) + ((r10 * x439)));
36266 new_r11
36267 = (((r11 * x439)) + ((r21 * x438)) + ((r01 * x440)));
36268 new_r12
36269 = (((r22 * x438)) + ((r12 * x439)) + ((r02 * x440)));
36270 new_r20
36271 = (((r10 * x443)) + ((r20 * x441)) + ((r00 * x444)));
36272 new_r21
36273 = (((r01 * x444)) + ((r21 * x441)) + ((r11 * x443)));
36274 new_r22
36275 = (((r22 * x441)) + ((r12 * x443)) + ((r02 * x444)));
36276 j12eval[0] = sj11;
36277 j12eval[1] = IKsign(sj11);
36278 if (IKabs(j12eval[0]) < 0.0000010000000000
36279 || IKabs(j12eval[1]) < 0.0000010000000000)
36280 {
36281 {
36282 IkReal j12eval[2];
36283 IkReal x445 = ((1.0) * sj9);
36284 IkReal x446 = (cj9 * sj6);
36285 IkReal x447 = x160;
36286 IkReal x448 = (cj4 * sj8);
36287 IkReal x449 = (sj6 * x445);
36288 IkReal x450 = (cj6 * cj9);
36289 IkReal x451 = (((cj8 * x450)) + (((-1.0) * x449)));
36290 IkReal x452 = (((cj9 * x448)) + ((sj4 * x451)));
36291 IkReal x453 = ((1.0) * sj4 * sj8);
36292 IkReal x454
36293 = (((cj4 * x451)) + (((-1.0) * cj9 * x453)));
36294 IkReal x455 = (sj6 * sj8);
36295 IkReal x456 = x169;
36296 IkReal x457 = x170;
36297 IkReal x458 = (x450 + (((-1.0) * cj8 * x449)));
36298 IkReal x459 = x172;
36299 IkReal x460 = (((sj4 * x459)) + ((sj9 * x448)));
36300 IkReal x461
36301 = (((cj4 * x459)) + (((-1.0) * sj4 * sj8 * x445)));
36302 new_r00
36303 = (((r00 * x454)) + ((r20 * x447))
36304 + ((r10 * x452)));
36305 new_r01
36306 = (((r01 * x454)) + ((r21 * x447))
36307 + ((r11 * x452)));
36308 new_r02
36309 = (((r02 * x454)) + ((r12 * x452))
36310 + ((r22 * x447)));
36311 new_r10
36312 = (((r20 * x455)) + ((r00 * x457))
36313 + ((r10 * x456)));
36314 new_r11
36315 = (((r21 * x455)) + ((r11 * x456))
36316 + ((r01 * x457)));
36317 new_r12
36318 = (((r12 * x456)) + ((r22 * x455))
36319 + ((r02 * x457)));
36320 new_r20
36321 = (((r10 * x460)) + ((r20 * x458))
36322 + ((r00 * x461)));
36323 new_r21
36324 = (((r11 * x460)) + ((r21 * x458))
36325 + ((r01 * x461)));
36326 new_r22
36327 = (((r12 * x460)) + ((r22 * x458))
36328 + ((r02 * x461)));
36329 j12eval[0] = sj10;
36330 j12eval[1] = sj11;
36331 if (IKabs(j12eval[0]) < 0.0000010000000000
36332 || IKabs(j12eval[1]) < 0.0000010000000000)
36333 {
36334 {
36335 IkReal j12eval[3];
36336 IkReal x462 = ((1.0) * sj9);
36337 IkReal x463 = (cj9 * sj6);
36338 IkReal x464 = x160;
36339 IkReal x465 = (cj4 * sj8);
36340 IkReal x466 = (sj6 * x462);
36341 IkReal x467 = (cj6 * cj9);
36342 IkReal x468 = ((((-1.0) * x466)) + ((cj8 * x467)));
36343 IkReal x469 = (((cj9 * x465)) + ((sj4 * x468)));
36344 IkReal x470 = ((1.0) * sj4 * sj8);
36345 IkReal x471
36346 = ((((-1.0) * cj9 * x470)) + ((cj4 * x468)));
36347 IkReal x472 = (sj6 * sj8);
36348 IkReal x473 = x169;
36349 IkReal x474 = x170;
36350 IkReal x475 = ((((-1.0) * cj8 * x466)) + x467);
36351 IkReal x476 = x172;
36352 IkReal x477 = (((sj9 * x465)) + ((sj4 * x476)));
36353 IkReal x478
36354 = ((((-1.0) * sj4 * sj8 * x462))
36355 + ((cj4 * x476)));
36356 new_r00
36357 = (((r10 * x469)) + ((r00 * x471))
36358 + ((r20 * x464)));
36359 new_r01
36360 = (((r11 * x469)) + ((r01 * x471))
36361 + ((r21 * x464)));
36362 new_r02
36363 = (((r22 * x464)) + ((r02 * x471))
36364 + ((r12 * x469)));
36365 new_r10
36366 = (((r00 * x474)) + ((r10 * x473))
36367 + ((r20 * x472)));
36368 new_r11
36369 = (((r01 * x474)) + ((r11 * x473))
36370 + ((r21 * x472)));
36371 new_r12
36372 = (((r02 * x474)) + ((r12 * x473))
36373 + ((r22 * x472)));
36374 new_r20
36375 = (((r20 * x475)) + ((r10 * x477))
36376 + ((r00 * x478)));
36377 new_r21
36378 = (((r01 * x478)) + ((r21 * x475))
36379 + ((r11 * x477)));
36380 new_r22
36381 = (((r22 * x475)) + ((r02 * x478))
36382 + ((r12 * x477)));
36383 j12eval[0] = cj10;
36384 j12eval[1] = cj11;
36385 j12eval[2] = sj11;
36386 if (IKabs(j12eval[0]) < 0.0000010000000000
36387 || IKabs(j12eval[1]) < 0.0000010000000000
36388 || IKabs(j12eval[2]) < 0.0000010000000000)
36389 {
36390 {
36391 IkReal evalcond[12];
36392 bool bgotonextstatement = true;
36393 do
36394 {
36395 IkReal x479 = ((1.0) * cj11);
36396 IkReal x480 = ((((-1.0) * x479)) + new_r22);
36397 IkReal x481 = ((1.0) * sj11);
36398 IkReal x482 = ((((-1.0) * x481)) + new_r12);
36399 evalcond[0]
36400 = ((-3.14159265358979)
36401 + (IKfmod(
36402 ((3.14159265358979)
36403 + (IKabs(
36404 ((-1.5707963267949)
36405 + j10)))),
36406 6.28318530717959)));
36407 evalcond[1] = x480;
36408 evalcond[2] = x480;
36409 evalcond[3] = new_r02;
36410 evalcond[4] = x482;
36411 evalcond[5] = x482;
36412 evalcond[6]
36413 = ((((-1.0) * new_r22 * x481))
36414 + ((cj11 * new_r12)));
36415 evalcond[7]
36416 = ((((-1.0) * new_r10 * x481))
36417 + (((-1.0) * new_r20 * x479)));
36418 evalcond[8]
36419 = ((((-1.0) * new_r11 * x481))
36420 + (((-1.0) * new_r21 * x479)));
36421 evalcond[9]
36422 = ((1.0) + (((-1.0) * new_r12 * x481))
36423 + (((-1.0) * new_r22 * x479)));
36424 if (IKabs(evalcond[0]) < 0.0000010000000000
36425 && IKabs(evalcond[1]) < 0.0000010000000000
36426 && IKabs(evalcond[2]) < 0.0000010000000000
36427 && IKabs(evalcond[3]) < 0.0000010000000000
36428 && IKabs(evalcond[4]) < 0.0000010000000000
36429 && IKabs(evalcond[5]) < 0.0000010000000000
36430 && IKabs(evalcond[6]) < 0.0000010000000000
36431 && IKabs(evalcond[7]) < 0.0000010000000000
36432 && IKabs(evalcond[8]) < 0.0000010000000000
36433 && IKabs(evalcond[9])
36434 < 0.0000010000000000)
36435 {
36436 bgotonextstatement = false;
36437 {
36438 IkReal j12array[1], cj12array[1],
36439 sj12array[1];
36440 bool j12valid[1] = {false};
36441 _nj12 = 1;
36442 CheckValue<IkReal> x483
36443 = IKPowWithIntegerCheck(
36444 IKsign(new_r12), -1);
36445 if (!x483.valid)
36446 {
36447 continue;
36448 }
36449 CheckValue<IkReal> x484
36450 = IKatan2WithCheck(
36451 IkReal(new_r21),
36452 ((-1.0) * (((1.0) * new_r20))),
36453 IKFAST_ATAN2_MAGTHRESH);
36454 if (!x484.valid)
36455 {
36456 continue;
36457 }
36458 j12array[0]
36459 = ((-1.5707963267949)
36460 + (((1.5707963267949)
36461 * (x483.value)))
36462 + (x484.value));
36463 sj12array[0] = IKsin(j12array[0]);
36464 cj12array[0] = IKcos(j12array[0]);
36465 if (j12array[0] > IKPI)
36466 {
36467 j12array[0] -= IK2PI;
36468 }
36469 else if (j12array[0] < -IKPI)
36470 {
36471 j12array[0] += IK2PI;
36472 }
36473 j12valid[0] = true;
36474 for (int ij12 = 0; ij12 < 1; ++ij12)
36475 {
36476 if (!j12valid[ij12])
36477 {
36478 continue;
36479 }
36480 _ij12[0] = ij12;
36481 _ij12[1] = -1;
36482 for (int iij12 = ij12 + 1; iij12 < 1;
36483 ++iij12)
36484 {
36485 if (j12valid[iij12]
36486 && IKabs(
36487 cj12array[ij12]
36488 - cj12array[iij12])
36489 < IKFAST_SOLUTION_THRESH
36490 && IKabs(
36491 sj12array[ij12]
36492 - sj12array[iij12])
36493 < IKFAST_SOLUTION_THRESH)
36494 {
36495 j12valid[iij12] = false;
36496 _ij12[1] = iij12;
36497 break;
36498 }
36499 }
36500 j12 = j12array[ij12];
36501 cj12 = cj12array[ij12];
36502 sj12 = sj12array[ij12];
36503 {
36504 IkReal evalcond[8];
36505 IkReal x485 = IKcos(j12);
36506 IkReal x486 = IKsin(j12);
36507 IkReal x487 = ((1.0) * new_r12);
36508 IkReal x488 = ((1.0) * x485);
36509 IkReal x489 = ((-1.0) * x488);
36510 evalcond[0]
36511 = (((new_r12 * x485)) + new_r20);
36512 evalcond[1]
36513 = (((new_r22 * x486)) + new_r11);
36514 evalcond[2]
36515 = ((((-1.0) * x486 * x487))
36516 + new_r21);
36517 evalcond[3]
36518 = ((((-1.0) * new_r22 * x488))
36519 + new_r10);
36520 evalcond[4]
36521 = ((((-1.0) * (1.0) * new_r00))
36522 + (((-1.0) * x486)));
36523 evalcond[5]
36524 = ((((-1.0) * (1.0) * new_r01))
36525 + x489);
36526 evalcond[6]
36527 = (((new_r11 * new_r22))
36528 + (((-1.0) * new_r21 * x487))
36529 + x486);
36530 evalcond[7]
36531 = ((((-1.0) * new_r20 * x487))
36532 + ((new_r10 * new_r22))
36533 + x489);
36534 if (IKabs(evalcond[0])
36535 > IKFAST_EVALCOND_THRESH
36536 || IKabs(evalcond[1])
36537 > IKFAST_EVALCOND_THRESH
36538 || IKabs(evalcond[2])
36539 > IKFAST_EVALCOND_THRESH
36540 || IKabs(evalcond[3])
36541 > IKFAST_EVALCOND_THRESH
36542 || IKabs(evalcond[4])
36543 > IKFAST_EVALCOND_THRESH
36544 || IKabs(evalcond[5])
36545 > IKFAST_EVALCOND_THRESH
36546 || IKabs(evalcond[6])
36547 > IKFAST_EVALCOND_THRESH
36548 || IKabs(evalcond[7])
36549 > IKFAST_EVALCOND_THRESH)
36550 {
36551 continue;
36552 }
36553 }
36554
36555 {
36556 std::vector<
36557 IkSingleDOFSolutionBase<IkReal> >
36558 vinfos(7);
36559 vinfos[0].jointtype = 1;
36560 vinfos[0].foffset = j4;
36561 vinfos[0].indices[0] = _ij4[0];
36562 vinfos[0].indices[1] = _ij4[1];
36563 vinfos[0].maxsolutions = _nj4;
36564 vinfos[1].jointtype = 1;
36565 vinfos[1].foffset = j6;
36566 vinfos[1].indices[0] = _ij6[0];
36567 vinfos[1].indices[1] = _ij6[1];
36568 vinfos[1].maxsolutions = _nj6;
36569 vinfos[2].jointtype = 1;
36570 vinfos[2].foffset = j8;
36571 vinfos[2].indices[0] = _ij8[0];
36572 vinfos[2].indices[1] = _ij8[1];
36573 vinfos[2].maxsolutions = _nj8;
36574 vinfos[3].jointtype = 1;
36575 vinfos[3].foffset = j9;
36576 vinfos[3].indices[0] = _ij9[0];
36577 vinfos[3].indices[1] = _ij9[1];
36578 vinfos[3].maxsolutions = _nj9;
36579 vinfos[4].jointtype = 1;
36580 vinfos[4].foffset = j10;
36581 vinfos[4].indices[0] = _ij10[0];
36582 vinfos[4].indices[1] = _ij10[1];
36583 vinfos[4].maxsolutions = _nj10;
36584 vinfos[5].jointtype = 1;
36585 vinfos[5].foffset = j11;
36586 vinfos[5].indices[0] = _ij11[0];
36587 vinfos[5].indices[1] = _ij11[1];
36588 vinfos[5].maxsolutions = _nj11;
36589 vinfos[6].jointtype = 1;
36590 vinfos[6].foffset = j12;
36591 vinfos[6].indices[0] = _ij12[0];
36592 vinfos[6].indices[1] = _ij12[1];
36593 vinfos[6].maxsolutions = _nj12;
36594 std::vector<int> vfree(0);
36595 solutions.AddSolution(vinfos, vfree);
36596 }
36597 }
36598 }
36599 }
36600 } while (0);
36601 if (bgotonextstatement)
36602 {
36603 bool bgotonextstatement = true;
36604 do
36605 {
36606 IkReal x490 = ((1.0) * cj11);
36607 IkReal x491 = ((((-1.0) * x490)) + new_r22);
36608 IkReal x492 = ((1.0) * sj11);
36609 evalcond[0]
36610 = ((-3.14159265358979)
36611 + (IKfmod(
36612 ((3.14159265358979)
36613 + (IKabs(
36614 ((1.5707963267949)
36615 + j10)))),
36616 6.28318530717959)));
36617 evalcond[1] = x491;
36618 evalcond[2] = x491;
36619 evalcond[3] = new_r02;
36620 evalcond[4] = (sj11 + new_r12);
36621 evalcond[5]
36622 = ((((-1.0) * (1.0) * new_r12))
36623 + (((-1.0) * x492)));
36624 evalcond[6]
36625 = ((((-1.0) * new_r22 * x492))
36626 + (((-1.0) * new_r12 * x490)));
36627 evalcond[7]
36628 = (((new_r10 * sj11))
36629 + (((-1.0) * new_r20 * x490)));
36630 evalcond[8]
36631 = ((((-1.0) * new_r21 * x490))
36632 + ((new_r11 * sj11)));
36633 evalcond[9]
36634 = ((1.0) + (((-1.0) * new_r22 * x490))
36635 + ((new_r12 * sj11)));
36636 if (IKabs(evalcond[0]) < 0.0000010000000000
36637 && IKabs(evalcond[1])
36638 < 0.0000010000000000
36639 && IKabs(evalcond[2])
36640 < 0.0000010000000000
36641 && IKabs(evalcond[3])
36642 < 0.0000010000000000
36643 && IKabs(evalcond[4])
36644 < 0.0000010000000000
36645 && IKabs(evalcond[5])
36646 < 0.0000010000000000
36647 && IKabs(evalcond[6])
36648 < 0.0000010000000000
36649 && IKabs(evalcond[7])
36650 < 0.0000010000000000
36651 && IKabs(evalcond[8])
36652 < 0.0000010000000000
36653 && IKabs(evalcond[9])
36654 < 0.0000010000000000)
36655 {
36656 bgotonextstatement = false;
36657 {
36658 IkReal j12array[1], cj12array[1],
36659 sj12array[1];
36660 bool j12valid[1] = {false};
36661 _nj12 = 1;
36662 if (IKabs(new_r00)
36663 < IKFAST_ATAN2_MAGTHRESH
36664 && IKabs(new_r01)
36665 < IKFAST_ATAN2_MAGTHRESH
36666 && IKabs(
36667 IKsqr(new_r00)
36668 + IKsqr(new_r01) - 1)
36669 <= IKFAST_SINCOS_THRESH)
36670 continue;
36671 j12array[0] = IKatan2(new_r00, new_r01);
36672 sj12array[0] = IKsin(j12array[0]);
36673 cj12array[0] = IKcos(j12array[0]);
36674 if (j12array[0] > IKPI)
36675 {
36676 j12array[0] -= IK2PI;
36677 }
36678 else if (j12array[0] < -IKPI)
36679 {
36680 j12array[0] += IK2PI;
36681 }
36682 j12valid[0] = true;
36683 for (int ij12 = 0; ij12 < 1; ++ij12)
36684 {
36685 if (!j12valid[ij12])
36686 {
36687 continue;
36688 }
36689 _ij12[0] = ij12;
36690 _ij12[1] = -1;
36691 for (int iij12 = ij12 + 1; iij12 < 1;
36692 ++iij12)
36693 {
36694 if (j12valid[iij12]
36695 && IKabs(
36696 cj12array[ij12]
36697 - cj12array[iij12])
36698 < IKFAST_SOLUTION_THRESH
36699 && IKabs(
36700 sj12array[ij12]
36701 - sj12array[iij12])
36702 < IKFAST_SOLUTION_THRESH)
36703 {
36704 j12valid[iij12] = false;
36705 _ij12[1] = iij12;
36706 break;
36707 }
36708 }
36709 j12 = j12array[ij12];
36710 cj12 = cj12array[ij12];
36711 sj12 = sj12array[ij12];
36712 {
36713 IkReal evalcond[8];
36714 IkReal x493 = IKsin(j12);
36715 IkReal x494
36716 = ((1.0) * (IKcos(j12)));
36717 IkReal x495 = ((-1.0) * x494);
36718 IkReal x496 = ((1.0) * new_r11);
36719 IkReal x497 = ((1.0) * new_r10);
36720 evalcond[0]
36721 = (((new_r12 * x493))
36722 + new_r21);
36723 evalcond[1]
36724 = (new_r00 + (((-1.0) * x493)));
36725 evalcond[2] = (new_r01 + x495);
36726 evalcond[3]
36727 = ((((-1.0) * new_r12 * x494))
36728 + new_r20);
36729 evalcond[4]
36730 = (((new_r22 * x493))
36731 + (((-1.0) * x496)));
36732 evalcond[5]
36733 = ((((-1.0) * new_r22 * x494))
36734 + (((-1.0) * x497)));
36735 evalcond[6]
36736 = (((new_r12 * new_r21))
36737 + (((-1.0) * new_r22 * x496))
36738 + x493);
36739 evalcond[7]
36740 = (((new_r12 * new_r20))
36741 + (((-1.0) * new_r22 * x497))
36742 + x495);
36743 if (IKabs(evalcond[0])
36744 > IKFAST_EVALCOND_THRESH
36745 || IKabs(evalcond[1])
36746 > IKFAST_EVALCOND_THRESH
36747 || IKabs(evalcond[2])
36748 > IKFAST_EVALCOND_THRESH
36749 || IKabs(evalcond[3])
36750 > IKFAST_EVALCOND_THRESH
36751 || IKabs(evalcond[4])
36752 > IKFAST_EVALCOND_THRESH
36753 || IKabs(evalcond[5])
36754 > IKFAST_EVALCOND_THRESH
36755 || IKabs(evalcond[6])
36756 > IKFAST_EVALCOND_THRESH
36757 || IKabs(evalcond[7])
36758 > IKFAST_EVALCOND_THRESH)
36759 {
36760 continue;
36761 }
36762 }
36763
36764 {
36765 std::vector<IkSingleDOFSolutionBase<
36766 IkReal> >
36767 vinfos(7);
36768 vinfos[0].jointtype = 1;
36769 vinfos[0].foffset = j4;
36770 vinfos[0].indices[0] = _ij4[0];
36771 vinfos[0].indices[1] = _ij4[1];
36772 vinfos[0].maxsolutions = _nj4;
36773 vinfos[1].jointtype = 1;
36774 vinfos[1].foffset = j6;
36775 vinfos[1].indices[0] = _ij6[0];
36776 vinfos[1].indices[1] = _ij6[1];
36777 vinfos[1].maxsolutions = _nj6;
36778 vinfos[2].jointtype = 1;
36779 vinfos[2].foffset = j8;
36780 vinfos[2].indices[0] = _ij8[0];
36781 vinfos[2].indices[1] = _ij8[1];
36782 vinfos[2].maxsolutions = _nj8;
36783 vinfos[3].jointtype = 1;
36784 vinfos[3].foffset = j9;
36785 vinfos[3].indices[0] = _ij9[0];
36786 vinfos[3].indices[1] = _ij9[1];
36787 vinfos[3].maxsolutions = _nj9;
36788 vinfos[4].jointtype = 1;
36789 vinfos[4].foffset = j10;
36790 vinfos[4].indices[0] = _ij10[0];
36791 vinfos[4].indices[1] = _ij10[1];
36792 vinfos[4].maxsolutions = _nj10;
36793 vinfos[5].jointtype = 1;
36794 vinfos[5].foffset = j11;
36795 vinfos[5].indices[0] = _ij11[0];
36796 vinfos[5].indices[1] = _ij11[1];
36797 vinfos[5].maxsolutions = _nj11;
36798 vinfos[6].jointtype = 1;
36799 vinfos[6].foffset = j12;
36800 vinfos[6].indices[0] = _ij12[0];
36801 vinfos[6].indices[1] = _ij12[1];
36802 vinfos[6].maxsolutions = _nj12;
36803 std::vector<int> vfree(0);
36804 solutions.AddSolution(
36805 vinfos, vfree);
36806 }
36807 }
36808 }
36809 }
36810 } while (0);
36811 if (bgotonextstatement)
36812 {
36813 bool bgotonextstatement = true;
36814 do
36815 {
36816 IkReal x498 = ((1.0) * cj10);
36817 IkReal x499 = ((1.0) * sj10);
36818 IkReal x500
36819 = ((((-1.0) * new_r02 * x499))
36820 + ((cj10 * new_r12)));
36821 evalcond[0]
36822 = ((-3.14159265358979)
36823 + (IKfmod(
36824 ((3.14159265358979)
36825 + (IKabs(
36826 ((-1.5707963267949)
36827 + j11)))),
36828 6.28318530717959)));
36829 evalcond[1] = new_r22;
36830 evalcond[2]
36831 = ((((-1.0) * x498)) + new_r02);
36832 evalcond[3]
36833 = ((((-1.0) * x499)) + new_r12);
36834 evalcond[4] = x500;
36835 evalcond[5] = x500;
36836 evalcond[6]
36837 = ((-1.0) + ((cj10 * new_r02))
36838 + ((new_r12 * sj10)));
36839 evalcond[7]
36840 = (((cj10 * new_r01))
36841 + ((new_r11 * sj10)));
36842 evalcond[8]
36843 = (((cj10 * new_r00))
36844 + ((new_r10 * sj10)));
36845 evalcond[9]
36846 = ((((-1.0) * new_r10 * x499))
36847 + (((-1.0) * new_r00 * x498)));
36848 evalcond[10]
36849 = ((((-1.0) * new_r11 * x499))
36850 + (((-1.0) * new_r01 * x498)));
36851 evalcond[11]
36852 = ((1.0) + (((-1.0) * new_r02 * x498))
36853 + (((-1.0) * new_r12 * x499)));
36854 if (IKabs(evalcond[0])
36855 < 0.0000010000000000
36856 && IKabs(evalcond[1])
36857 < 0.0000010000000000
36858 && IKabs(evalcond[2])
36859 < 0.0000010000000000
36860 && IKabs(evalcond[3])
36861 < 0.0000010000000000
36862 && IKabs(evalcond[4])
36863 < 0.0000010000000000
36864 && IKabs(evalcond[5])
36865 < 0.0000010000000000
36866 && IKabs(evalcond[6])
36867 < 0.0000010000000000
36868 && IKabs(evalcond[7])
36869 < 0.0000010000000000
36870 && IKabs(evalcond[8])
36871 < 0.0000010000000000
36872 && IKabs(evalcond[9])
36873 < 0.0000010000000000
36874 && IKabs(evalcond[10])
36875 < 0.0000010000000000
36876 && IKabs(evalcond[11])
36877 < 0.0000010000000000)
36878 {
36879 bgotonextstatement = false;
36880 {
36881 IkReal j12array[1], cj12array[1],
36882 sj12array[1];
36883 bool j12valid[1] = {false};
36884 _nj12 = 1;
36885 if (IKabs(new_r21)
36886 < IKFAST_ATAN2_MAGTHRESH
36887 && IKabs(
36888 ((-1.0)
36889 * (((1.0) * new_r20))))
36890 < IKFAST_ATAN2_MAGTHRESH
36891 && IKabs(
36892 IKsqr(new_r21)
36893 + IKsqr(
36894 ((-1.0)
36895 * (((1.0)
36896 * new_r20))))
36897 - 1)
36898 <= IKFAST_SINCOS_THRESH)
36899 continue;
36900 j12array[0] = IKatan2(
36901 new_r21,
36902 ((-1.0) * (((1.0) * new_r20))));
36903 sj12array[0] = IKsin(j12array[0]);
36904 cj12array[0] = IKcos(j12array[0]);
36905 if (j12array[0] > IKPI)
36906 {
36907 j12array[0] -= IK2PI;
36908 }
36909 else if (j12array[0] < -IKPI)
36910 {
36911 j12array[0] += IK2PI;
36912 }
36913 j12valid[0] = true;
36914 for (int ij12 = 0; ij12 < 1; ++ij12)
36915 {
36916 if (!j12valid[ij12])
36917 {
36918 continue;
36919 }
36920 _ij12[0] = ij12;
36921 _ij12[1] = -1;
36922 for (int iij12 = ij12 + 1;
36923 iij12 < 1;
36924 ++iij12)
36925 {
36926 if (j12valid[iij12]
36927 && IKabs(
36928 cj12array[ij12]
36929 - cj12array[iij12])
36930 < IKFAST_SOLUTION_THRESH
36931 && IKabs(
36932 sj12array[ij12]
36933 - sj12array[iij12])
36934 < IKFAST_SOLUTION_THRESH)
36935 {
36936 j12valid[iij12] = false;
36937 _ij12[1] = iij12;
36938 break;
36939 }
36940 }
36941 j12 = j12array[ij12];
36942 cj12 = cj12array[ij12];
36943 sj12 = sj12array[ij12];
36944 {
36945 IkReal evalcond[8];
36946 IkReal x501 = IKcos(j12);
36947 IkReal x502 = IKsin(j12);
36948 IkReal x503 = ((1.0) * x502);
36949 IkReal x504 = ((-1.0) * x503);
36950 IkReal x505 = ((1.0) * x501);
36951 IkReal x506 = ((1.0) * new_r12);
36952 evalcond[0] = (x501 + new_r20);
36953 evalcond[1] = (x504 + new_r21);
36954 evalcond[2]
36955 = (((new_r12 * x501))
36956 + new_r01);
36957 evalcond[3]
36958 = (((new_r12 * x502))
36959 + new_r00);
36960 evalcond[4]
36961 = ((((-1.0) * new_r02 * x505))
36962 + new_r11);
36963 evalcond[5]
36964 = ((((-1.0) * new_r02 * x503))
36965 + new_r10);
36966 evalcond[6]
36967 = ((((-1.0) * new_r00 * x506))
36968 + x504
36969 + ((new_r02 * new_r10)));
36970 evalcond[7]
36971 = ((((-1.0) * new_r01 * x506))
36972 + ((new_r02 * new_r11))
36973 + (((-1.0) * x505)));
36974 if (IKabs(evalcond[0])
36975 > IKFAST_EVALCOND_THRESH
36976 || IKabs(evalcond[1])
36977 > IKFAST_EVALCOND_THRESH
36978 || IKabs(evalcond[2])
36979 > IKFAST_EVALCOND_THRESH
36980 || IKabs(evalcond[3])
36981 > IKFAST_EVALCOND_THRESH
36982 || IKabs(evalcond[4])
36983 > IKFAST_EVALCOND_THRESH
36984 || IKabs(evalcond[5])
36985 > IKFAST_EVALCOND_THRESH
36986 || IKabs(evalcond[6])
36987 > IKFAST_EVALCOND_THRESH
36988 || IKabs(evalcond[7])
36989 > IKFAST_EVALCOND_THRESH)
36990 {
36991 continue;
36992 }
36993 }
36994
36995 {
36996 std::vector<
36997 IkSingleDOFSolutionBase<
36998 IkReal> >
36999 vinfos(7);
37000 vinfos[0].jointtype = 1;
37001 vinfos[0].foffset = j4;
37002 vinfos[0].indices[0] = _ij4[0];
37003 vinfos[0].indices[1] = _ij4[1];
37004 vinfos[0].maxsolutions = _nj4;
37005 vinfos[1].jointtype = 1;
37006 vinfos[1].foffset = j6;
37007 vinfos[1].indices[0] = _ij6[0];
37008 vinfos[1].indices[1] = _ij6[1];
37009 vinfos[1].maxsolutions = _nj6;
37010 vinfos[2].jointtype = 1;
37011 vinfos[2].foffset = j8;
37012 vinfos[2].indices[0] = _ij8[0];
37013 vinfos[2].indices[1] = _ij8[1];
37014 vinfos[2].maxsolutions = _nj8;
37015 vinfos[3].jointtype = 1;
37016 vinfos[3].foffset = j9;
37017 vinfos[3].indices[0] = _ij9[0];
37018 vinfos[3].indices[1] = _ij9[1];
37019 vinfos[3].maxsolutions = _nj9;
37020 vinfos[4].jointtype = 1;
37021 vinfos[4].foffset = j10;
37022 vinfos[4].indices[0] = _ij10[0];
37023 vinfos[4].indices[1] = _ij10[1];
37024 vinfos[4].maxsolutions = _nj10;
37025 vinfos[5].jointtype = 1;
37026 vinfos[5].foffset = j11;
37027 vinfos[5].indices[0] = _ij11[0];
37028 vinfos[5].indices[1] = _ij11[1];
37029 vinfos[5].maxsolutions = _nj11;
37030 vinfos[6].jointtype = 1;
37031 vinfos[6].foffset = j12;
37032 vinfos[6].indices[0] = _ij12[0];
37033 vinfos[6].indices[1] = _ij12[1];
37034 vinfos[6].maxsolutions = _nj12;
37035 std::vector<int> vfree(0);
37036 solutions.AddSolution(
37037 vinfos, vfree);
37038 }
37039 }
37040 }
37041 }
37042 } while (0);
37043 if (bgotonextstatement)
37044 {
37045 bool bgotonextstatement = true;
37046 do
37047 {
37048 IkReal x507
37049 = ((((-1.0) * (1.0) * new_r02
37050 * sj10))
37051 + ((cj10 * new_r12)));
37052 IkReal x508
37053 = ((1.0) + ((cj10 * new_r02))
37054 + ((new_r12 * sj10)));
37055 IkReal x509
37056 = (((cj10 * new_r01))
37057 + ((new_r11 * sj10)));
37058 IkReal x510
37059 = (((cj10 * new_r00))
37060 + ((new_r10 * sj10)));
37061 evalcond[0]
37062 = ((-3.14159265358979)
37063 + (IKfmod(
37064 ((3.14159265358979)
37065 + (IKabs(
37066 ((1.5707963267949)
37067 + j11)))),
37068 6.28318530717959)));
37069 evalcond[1] = new_r22;
37070 evalcond[2] = (cj10 + new_r02);
37071 evalcond[3] = (sj10 + new_r12);
37072 evalcond[4] = x507;
37073 evalcond[5] = x507;
37074 evalcond[6] = x508;
37075 evalcond[7] = x509;
37076 evalcond[8] = x510;
37077 evalcond[9] = x510;
37078 evalcond[10] = x509;
37079 evalcond[11] = x508;
37080 if (IKabs(evalcond[0])
37081 < 0.0000010000000000
37082 && IKabs(evalcond[1])
37083 < 0.0000010000000000
37084 && IKabs(evalcond[2])
37085 < 0.0000010000000000
37086 && IKabs(evalcond[3])
37087 < 0.0000010000000000
37088 && IKabs(evalcond[4])
37089 < 0.0000010000000000
37090 && IKabs(evalcond[5])
37091 < 0.0000010000000000
37092 && IKabs(evalcond[6])
37093 < 0.0000010000000000
37094 && IKabs(evalcond[7])
37095 < 0.0000010000000000
37096 && IKabs(evalcond[8])
37097 < 0.0000010000000000
37098 && IKabs(evalcond[9])
37099 < 0.0000010000000000
37100 && IKabs(evalcond[10])
37101 < 0.0000010000000000
37102 && IKabs(evalcond[11])
37103 < 0.0000010000000000)
37104 {
37105 bgotonextstatement = false;
37106 {
37107 IkReal j12array[1], cj12array[1],
37108 sj12array[1];
37109 bool j12valid[1] = {false};
37110 _nj12 = 1;
37111 if (IKabs(
37112 ((-1.0)
37113 * (((1.0) * new_r21))))
37114 < IKFAST_ATAN2_MAGTHRESH
37115 && IKabs(new_r20)
37116 < IKFAST_ATAN2_MAGTHRESH
37117 && IKabs(
37118 IKsqr(
37119 ((-1.0)
37120 * (((1.0)
37121 * new_r21))))
37122 + IKsqr(new_r20) - 1)
37123 <= IKFAST_SINCOS_THRESH)
37124 continue;
37125 j12array[0] = IKatan2(
37126 ((-1.0) * (((1.0) * new_r21))),
37127 new_r20);
37128 sj12array[0] = IKsin(j12array[0]);
37129 cj12array[0] = IKcos(j12array[0]);
37130 if (j12array[0] > IKPI)
37131 {
37132 j12array[0] -= IK2PI;
37133 }
37134 else if (j12array[0] < -IKPI)
37135 {
37136 j12array[0] += IK2PI;
37137 }
37138 j12valid[0] = true;
37139 for (int ij12 = 0; ij12 < 1; ++ij12)
37140 {
37141 if (!j12valid[ij12])
37142 {
37143 continue;
37144 }
37145 _ij12[0] = ij12;
37146 _ij12[1] = -1;
37147 for (int iij12 = ij12 + 1;
37148 iij12 < 1;
37149 ++iij12)
37150 {
37151 if (j12valid[iij12]
37152 && IKabs(
37153 cj12array[ij12]
37154 - cj12array[iij12])
37155 < IKFAST_SOLUTION_THRESH
37156 && IKabs(
37157 sj12array[ij12]
37158 - sj12array[iij12])
37159 < IKFAST_SOLUTION_THRESH)
37160 {
37161 j12valid[iij12] = false;
37162 _ij12[1] = iij12;
37163 break;
37164 }
37165 }
37166 j12 = j12array[ij12];
37167 cj12 = cj12array[ij12];
37168 sj12 = sj12array[ij12];
37169 {
37170 IkReal evalcond[8];
37171 IkReal x511 = IKsin(j12);
37172 IkReal x512 = IKcos(j12);
37173 IkReal x513 = ((1.0) * x512);
37174 IkReal x514 = ((-1.0) * x513);
37175 IkReal x515 = ((1.0) * x511);
37176 IkReal x516 = ((1.0) * new_r02);
37177 evalcond[0] = (x511 + new_r21);
37178 evalcond[1] = (x514 + new_r20);
37179 evalcond[2]
37180 = (((new_r02 * x512))
37181 + new_r11);
37182 evalcond[3]
37183 = (((new_r02 * x511))
37184 + new_r10);
37185 evalcond[4]
37186 = ((((-1.0) * new_r12
37187 * x513))
37188 + new_r01);
37189 evalcond[5]
37190 = ((((-1.0) * new_r12
37191 * x515))
37192 + new_r00);
37193 evalcond[6]
37194 = ((((-1.0) * new_r10
37195 * x516))
37196 + ((new_r00 * new_r12))
37197 + (((-1.0) * x515)));
37198 evalcond[7]
37199 = ((((-1.0) * new_r11
37200 * x516))
37201 + ((new_r01 * new_r12))
37202 + x514);
37203 if (IKabs(evalcond[0])
37204 > IKFAST_EVALCOND_THRESH
37205 || IKabs(evalcond[1])
37206 > IKFAST_EVALCOND_THRESH
37207 || IKabs(evalcond[2])
37208 > IKFAST_EVALCOND_THRESH
37209 || IKabs(evalcond[3])
37210 > IKFAST_EVALCOND_THRESH
37211 || IKabs(evalcond[4])
37212 > IKFAST_EVALCOND_THRESH
37213 || IKabs(evalcond[5])
37214 > IKFAST_EVALCOND_THRESH
37215 || IKabs(evalcond[6])
37216 > IKFAST_EVALCOND_THRESH
37217 || IKabs(evalcond[7])
37218 > IKFAST_EVALCOND_THRESH)
37219 {
37220 continue;
37221 }
37222 }
37223
37224 {
37225 std::vector<
37226 IkSingleDOFSolutionBase<
37227 IkReal> >
37228 vinfos(7);
37229 vinfos[0].jointtype = 1;
37230 vinfos[0].foffset = j4;
37231 vinfos[0].indices[0] = _ij4[0];
37232 vinfos[0].indices[1] = _ij4[1];
37233 vinfos[0].maxsolutions = _nj4;
37234 vinfos[1].jointtype = 1;
37235 vinfos[1].foffset = j6;
37236 vinfos[1].indices[0] = _ij6[0];
37237 vinfos[1].indices[1] = _ij6[1];
37238 vinfos[1].maxsolutions = _nj6;
37239 vinfos[2].jointtype = 1;
37240 vinfos[2].foffset = j8;
37241 vinfos[2].indices[0] = _ij8[0];
37242 vinfos[2].indices[1] = _ij8[1];
37243 vinfos[2].maxsolutions = _nj8;
37244 vinfos[3].jointtype = 1;
37245 vinfos[3].foffset = j9;
37246 vinfos[3].indices[0] = _ij9[0];
37247 vinfos[3].indices[1] = _ij9[1];
37248 vinfos[3].maxsolutions = _nj9;
37249 vinfos[4].jointtype = 1;
37250 vinfos[4].foffset = j10;
37251 vinfos[4].indices[0] = _ij10[0];
37252 vinfos[4].indices[1] = _ij10[1];
37253 vinfos[4].maxsolutions = _nj10;
37254 vinfos[5].jointtype = 1;
37255 vinfos[5].foffset = j11;
37256 vinfos[5].indices[0] = _ij11[0];
37257 vinfos[5].indices[1] = _ij11[1];
37258 vinfos[5].maxsolutions = _nj11;
37259 vinfos[6].jointtype = 1;
37260 vinfos[6].foffset = j12;
37261 vinfos[6].indices[0] = _ij12[0];
37262 vinfos[6].indices[1] = _ij12[1];
37263 vinfos[6].maxsolutions = _nj12;
37264 std::vector<int> vfree(0);
37265 solutions.AddSolution(
37266 vinfos, vfree);
37267 }
37268 }
37269 }
37270 }
37271 } while (0);
37272 if (bgotonextstatement)
37273 {
37274 bool bgotonextstatement = true;
37275 do
37276 {
37277 IkReal x517
37278 = ((((-1.0) * (1.0) * new_r02
37279 * sj10))
37280 + ((cj10 * new_r12)));
37281 IkReal x518
37282 = (((cj10 * new_r02))
37283 + ((new_r12 * sj10)));
37284 evalcond[0]
37285 = ((-3.14159265358979)
37286 + (IKfmod(
37287 ((3.14159265358979)
37288 + (IKabs(j11))),
37289 6.28318530717959)));
37290 evalcond[1] = ((-1.0) + new_r22);
37291 evalcond[2] = new_r20;
37292 evalcond[3] = new_r02;
37293 evalcond[4] = new_r12;
37294 evalcond[5] = new_r21;
37295 evalcond[6] = x517;
37296 evalcond[7] = x517;
37297 evalcond[8] = x518;
37298 evalcond[9] = x518;
37299 if (IKabs(evalcond[0])
37300 < 0.0000010000000000
37301 && IKabs(evalcond[1])
37302 < 0.0000010000000000
37303 && IKabs(evalcond[2])
37304 < 0.0000010000000000
37305 && IKabs(evalcond[3])
37306 < 0.0000010000000000
37307 && IKabs(evalcond[4])
37308 < 0.0000010000000000
37309 && IKabs(evalcond[5])
37310 < 0.0000010000000000
37311 && IKabs(evalcond[6])
37312 < 0.0000010000000000
37313 && IKabs(evalcond[7])
37314 < 0.0000010000000000
37315 && IKabs(evalcond[8])
37316 < 0.0000010000000000
37317 && IKabs(evalcond[9])
37318 < 0.0000010000000000)
37319 {
37320 bgotonextstatement = false;
37321 {
37322 IkReal j12array[1], cj12array[1],
37323 sj12array[1];
37324 bool j12valid[1] = {false};
37325 _nj12 = 1;
37326 IkReal x519 = ((1.0) * new_r01);
37327 if (IKabs(
37328 ((((-1.0) * cj10 * x519))
37329 + (((-1.0) * (1.0)
37330 * new_r00 * sj10))))
37331 < IKFAST_ATAN2_MAGTHRESH
37332 && IKabs(
37333 (((cj10 * new_r00))
37334 + (((-1.0) * sj10
37335 * x519))))
37336 < IKFAST_ATAN2_MAGTHRESH
37337 && IKabs(
37338 IKsqr(
37339 ((((-1.0) * cj10
37340 * x519))
37341 + (((-1.0) * (1.0)
37342 * new_r00
37343 * sj10))))
37344 + IKsqr((
37345 ((cj10
37346 * new_r00))
37347 + (((-1.0) * sj10
37348 * x519))))
37349 - 1)
37350 <= IKFAST_SINCOS_THRESH)
37351 continue;
37352 j12array[0] = IKatan2(
37353 ((((-1.0) * cj10 * x519))
37354 + (((-1.0) * (1.0) * new_r00
37355 * sj10))),
37356 (((cj10 * new_r00))
37357 + (((-1.0) * sj10 * x519))));
37358 sj12array[0] = IKsin(j12array[0]);
37359 cj12array[0] = IKcos(j12array[0]);
37360 if (j12array[0] > IKPI)
37361 {
37362 j12array[0] -= IK2PI;
37363 }
37364 else if (j12array[0] < -IKPI)
37365 {
37366 j12array[0] += IK2PI;
37367 }
37368 j12valid[0] = true;
37369 for (int ij12 = 0; ij12 < 1;
37370 ++ij12)
37371 {
37372 if (!j12valid[ij12])
37373 {
37374 continue;
37375 }
37376 _ij12[0] = ij12;
37377 _ij12[1] = -1;
37378 for (int iij12 = ij12 + 1;
37379 iij12 < 1;
37380 ++iij12)
37381 {
37382 if (j12valid[iij12]
37383 && IKabs(
37384 cj12array[ij12]
37385 - cj12array[iij12])
37386 < IKFAST_SOLUTION_THRESH
37387 && IKabs(
37388 sj12array[ij12]
37389 - sj12array[iij12])
37390 < IKFAST_SOLUTION_THRESH)
37391 {
37392 j12valid[iij12] = false;
37393 _ij12[1] = iij12;
37394 break;
37395 }
37396 }
37397 j12 = j12array[ij12];
37398 cj12 = cj12array[ij12];
37399 sj12 = sj12array[ij12];
37400 {
37401 IkReal evalcond[8];
37402 IkReal x520 = IKsin(j12);
37403 IkReal x521 = (cj10 * x520);
37404 IkReal x522 = IKcos(j12);
37405 IkReal x523 = ((1.0) * x522);
37406 IkReal x524 = ((-1.0) * x523);
37407 IkReal x525 = ((1.0) * sj10);
37408 IkReal x526
37409 = ((((-1.0) * cj10
37410 * x523))
37411 + ((sj10 * x520)));
37412 evalcond[0]
37413 = (((cj10 * new_r01))
37414 + ((new_r11 * sj10))
37415 + x520);
37416 evalcond[1]
37417 = (((sj10 * x522))
37418 + new_r01 + x521);
37419 evalcond[2]
37420 = (((cj10 * new_r00))
37421 + ((new_r10 * sj10))
37422 + x524);
37423 evalcond[3]
37424 = (((cj10 * new_r10))
37425 + (((-1.0) * new_r00
37426 * x525))
37427 + (((-1.0) * x520)));
37428 evalcond[4]
37429 = (((cj10 * new_r11))
37430 + (((-1.0) * new_r01
37431 * x525))
37432 + x524);
37433 evalcond[5]
37434 = (new_r00 + x526);
37435 evalcond[6]
37436 = (new_r11 + x526);
37437 evalcond[7]
37438 = ((((-1.0) * x521))
37439 + (((-1.0) * x522
37440 * x525))
37441 + new_r10);
37442 if (IKabs(evalcond[0])
37443 > IKFAST_EVALCOND_THRESH
37444 || IKabs(evalcond[1])
37445 > IKFAST_EVALCOND_THRESH
37446 || IKabs(evalcond[2])
37447 > IKFAST_EVALCOND_THRESH
37448 || IKabs(evalcond[3])
37449 > IKFAST_EVALCOND_THRESH
37450 || IKabs(evalcond[4])
37451 > IKFAST_EVALCOND_THRESH
37452 || IKabs(evalcond[5])
37453 > IKFAST_EVALCOND_THRESH
37454 || IKabs(evalcond[6])
37455 > IKFAST_EVALCOND_THRESH
37456 || IKabs(evalcond[7])
37457 > IKFAST_EVALCOND_THRESH)
37458 {
37459 continue;
37460 }
37461 }
37462
37463 {
37464 std::vector<
37465 IkSingleDOFSolutionBase<
37466 IkReal> >
37467 vinfos(7);
37468 vinfos[0].jointtype = 1;
37469 vinfos[0].foffset = j4;
37470 vinfos[0].indices[0]
37471 = _ij4[0];
37472 vinfos[0].indices[1]
37473 = _ij4[1];
37474 vinfos[0].maxsolutions = _nj4;
37475 vinfos[1].jointtype = 1;
37476 vinfos[1].foffset = j6;
37477 vinfos[1].indices[0]
37478 = _ij6[0];
37479 vinfos[1].indices[1]
37480 = _ij6[1];
37481 vinfos[1].maxsolutions = _nj6;
37482 vinfos[2].jointtype = 1;
37483 vinfos[2].foffset = j8;
37484 vinfos[2].indices[0]
37485 = _ij8[0];
37486 vinfos[2].indices[1]
37487 = _ij8[1];
37488 vinfos[2].maxsolutions = _nj8;
37489 vinfos[3].jointtype = 1;
37490 vinfos[3].foffset = j9;
37491 vinfos[3].indices[0]
37492 = _ij9[0];
37493 vinfos[3].indices[1]
37494 = _ij9[1];
37495 vinfos[3].maxsolutions = _nj9;
37496 vinfos[4].jointtype = 1;
37497 vinfos[4].foffset = j10;
37498 vinfos[4].indices[0]
37499 = _ij10[0];
37500 vinfos[4].indices[1]
37501 = _ij10[1];
37502 vinfos[4].maxsolutions
37503 = _nj10;
37504 vinfos[5].jointtype = 1;
37505 vinfos[5].foffset = j11;
37506 vinfos[5].indices[0]
37507 = _ij11[0];
37508 vinfos[5].indices[1]
37509 = _ij11[1];
37510 vinfos[5].maxsolutions
37511 = _nj11;
37512 vinfos[6].jointtype = 1;
37513 vinfos[6].foffset = j12;
37514 vinfos[6].indices[0]
37515 = _ij12[0];
37516 vinfos[6].indices[1]
37517 = _ij12[1];
37518 vinfos[6].maxsolutions
37519 = _nj12;
37520 std::vector<int> vfree(0);
37521 solutions.AddSolution(
37522 vinfos, vfree);
37523 }
37524 }
37525 }
37526 }
37527 } while (0);
37528 if (bgotonextstatement)
37529 {
37530 bool bgotonextstatement = true;
37531 do
37532 {
37533 IkReal x527
37534 = ((((-1.0) * (1.0) * new_r02
37535 * sj10))
37536 + ((cj10 * new_r12)));
37537 IkReal x528 = (cj10 * new_r02);
37538 IkReal x529 = (new_r12 * sj10);
37539 evalcond[0]
37540 = ((-3.14159265358979)
37541 + (IKfmod(
37542 ((3.14159265358979)
37543 + (IKabs((
37544 (-3.14159265358979)
37545 + j11)))),
37546 6.28318530717959)));
37547 evalcond[1] = ((1.0) + new_r22);
37548 evalcond[2] = new_r20;
37549 evalcond[3] = new_r02;
37550 evalcond[4] = new_r12;
37551 evalcond[5] = new_r21;
37552 evalcond[6] = x527;
37553 evalcond[7] = x527;
37554 evalcond[8] = (x529 + x528);
37555 evalcond[9]
37556 = ((((-1.0) * x528))
37557 + (((-1.0) * x529)));
37558 if (IKabs(evalcond[0])
37559 < 0.0000010000000000
37560 && IKabs(evalcond[1])
37561 < 0.0000010000000000
37562 && IKabs(evalcond[2])
37563 < 0.0000010000000000
37564 && IKabs(evalcond[3])
37565 < 0.0000010000000000
37566 && IKabs(evalcond[4])
37567 < 0.0000010000000000
37568 && IKabs(evalcond[5])
37569 < 0.0000010000000000
37570 && IKabs(evalcond[6])
37571 < 0.0000010000000000
37572 && IKabs(evalcond[7])
37573 < 0.0000010000000000
37574 && IKabs(evalcond[8])
37575 < 0.0000010000000000
37576 && IKabs(evalcond[9])
37577 < 0.0000010000000000)
37578 {
37579 bgotonextstatement = false;
37580 {
37581 IkReal j12array[1],
37582 cj12array[1], sj12array[1];
37583 bool j12valid[1] = {false};
37584 _nj12 = 1;
37585 IkReal x530 = ((1.0) * new_r00);
37586 if (IKabs((
37587 (((-1.0) * sj10 * x530))
37588 + ((cj10 * new_r01))))
37589 < IKFAST_ATAN2_MAGTHRESH
37590 && IKabs(
37591 ((((-1.0) * cj10
37592 * x530))
37593 + (((-1.0) * (1.0)
37594 * new_r01
37595 * sj10))))
37596 < IKFAST_ATAN2_MAGTHRESH
37597 && IKabs(
37598 IKsqr((
37599 (((-1.0) * sj10
37600 * x530))
37601 + ((cj10
37602 * new_r01))))
37603 + IKsqr((
37604 (((-1.0) * cj10
37605 * x530))
37606 + (((-1.0)
37607 * (1.0)
37608 * new_r01
37609 * sj10))))
37610 - 1)
37611 <= IKFAST_SINCOS_THRESH)
37612 continue;
37613 j12array[0] = IKatan2(
37614 ((((-1.0) * sj10 * x530))
37615 + ((cj10 * new_r01))),
37616 ((((-1.0) * cj10 * x530))
37617 + (((-1.0) * (1.0)
37618 * new_r01 * sj10))));
37619 sj12array[0]
37620 = IKsin(j12array[0]);
37621 cj12array[0]
37622 = IKcos(j12array[0]);
37623 if (j12array[0] > IKPI)
37624 {
37625 j12array[0] -= IK2PI;
37626 }
37627 else if (j12array[0] < -IKPI)
37628 {
37629 j12array[0] += IK2PI;
37630 }
37631 j12valid[0] = true;
37632 for (int ij12 = 0; ij12 < 1;
37633 ++ij12)
37634 {
37635 if (!j12valid[ij12])
37636 {
37637 continue;
37638 }
37639 _ij12[0] = ij12;
37640 _ij12[1] = -1;
37641 for (int iij12 = ij12 + 1;
37642 iij12 < 1;
37643 ++iij12)
37644 {
37645 if (j12valid[iij12]
37646 && IKabs(
37647 cj12array[ij12]
37648 - cj12array
37649 [iij12])
37650 < IKFAST_SOLUTION_THRESH
37651 && IKabs(
37652 sj12array[ij12]
37653 - sj12array
37654 [iij12])
37655 < IKFAST_SOLUTION_THRESH)
37656 {
37657 j12valid[iij12] = false;
37658 _ij12[1] = iij12;
37659 break;
37660 }
37661 }
37662 j12 = j12array[ij12];
37663 cj12 = cj12array[ij12];
37664 sj12 = sj12array[ij12];
37665 {
37666 IkReal evalcond[8];
37667 IkReal x531 = IKcos(j12);
37668 IkReal x532 = IKsin(j12);
37669 IkReal x533
37670 = ((1.0) * x532);
37671 IkReal x534
37672 = ((-1.0) * x533);
37673 IkReal x535 = (cj10 * x531);
37674 IkReal x536
37675 = ((1.0) * sj10);
37676 IkReal x537
37677 = ((((-1.0) * cj10
37678 * x533))
37679 + ((sj10 * x531)));
37680 evalcond[0]
37681 = (x531
37682 + ((cj10 * new_r00))
37683 + ((new_r10
37684 * sj10)));
37685 evalcond[1]
37686 = (x534
37687 + ((cj10 * new_r01))
37688 + ((new_r11
37689 * sj10)));
37690 evalcond[2]
37691 = (x535
37692 + ((sj10 * x532))
37693 + new_r00);
37694 evalcond[3]
37695 = (x534
37696 + ((cj10 * new_r10))
37697 + (((-1.0) * new_r00
37698 * x536)));
37699 evalcond[4]
37700 = (((cj10 * new_r11))
37701 + (((-1.0) * x531))
37702 + (((-1.0) * new_r01
37703 * x536)));
37704 evalcond[5]
37705 = (x537 + new_r01);
37706 evalcond[6]
37707 = (x537 + new_r10);
37708 evalcond[7]
37709 = ((((-1.0) * x532
37710 * x536))
37711 + new_r11
37712 + (((-1.0) * x535)));
37713 if (IKabs(evalcond[0])
37714 > IKFAST_EVALCOND_THRESH
37715 || IKabs(evalcond[1])
37716 > IKFAST_EVALCOND_THRESH
37717 || IKabs(evalcond[2])
37718 > IKFAST_EVALCOND_THRESH
37719 || IKabs(evalcond[3])
37720 > IKFAST_EVALCOND_THRESH
37721 || IKabs(evalcond[4])
37722 > IKFAST_EVALCOND_THRESH
37723 || IKabs(evalcond[5])
37724 > IKFAST_EVALCOND_THRESH
37725 || IKabs(evalcond[6])
37726 > IKFAST_EVALCOND_THRESH
37727 || IKabs(evalcond[7])
37728 > IKFAST_EVALCOND_THRESH)
37729 {
37730 continue;
37731 }
37732 }
37733
37734 {
37735 std::vector<
37736 IkSingleDOFSolutionBase<
37737 IkReal> >
37738 vinfos(7);
37739 vinfos[0].jointtype = 1;
37740 vinfos[0].foffset = j4;
37741 vinfos[0].indices[0]
37742 = _ij4[0];
37743 vinfos[0].indices[1]
37744 = _ij4[1];
37745 vinfos[0].maxsolutions
37746 = _nj4;
37747 vinfos[1].jointtype = 1;
37748 vinfos[1].foffset = j6;
37749 vinfos[1].indices[0]
37750 = _ij6[0];
37751 vinfos[1].indices[1]
37752 = _ij6[1];
37753 vinfos[1].maxsolutions
37754 = _nj6;
37755 vinfos[2].jointtype = 1;
37756 vinfos[2].foffset = j8;
37757 vinfos[2].indices[0]
37758 = _ij8[0];
37759 vinfos[2].indices[1]
37760 = _ij8[1];
37761 vinfos[2].maxsolutions
37762 = _nj8;
37763 vinfos[3].jointtype = 1;
37764 vinfos[3].foffset = j9;
37765 vinfos[3].indices[0]
37766 = _ij9[0];
37767 vinfos[3].indices[1]
37768 = _ij9[1];
37769 vinfos[3].maxsolutions
37770 = _nj9;
37771 vinfos[4].jointtype = 1;
37772 vinfos[4].foffset = j10;
37773 vinfos[4].indices[0]
37774 = _ij10[0];
37775 vinfos[4].indices[1]
37776 = _ij10[1];
37777 vinfos[4].maxsolutions
37778 = _nj10;
37779 vinfos[5].jointtype = 1;
37780 vinfos[5].foffset = j11;
37781 vinfos[5].indices[0]
37782 = _ij11[0];
37783 vinfos[5].indices[1]
37784 = _ij11[1];
37785 vinfos[5].maxsolutions
37786 = _nj11;
37787 vinfos[6].jointtype = 1;
37788 vinfos[6].foffset = j12;
37789 vinfos[6].indices[0]
37790 = _ij12[0];
37791 vinfos[6].indices[1]
37792 = _ij12[1];
37793 vinfos[6].maxsolutions
37794 = _nj12;
37795 std::vector<int> vfree(0);
37796 solutions.AddSolution(
37797 vinfos, vfree);
37798 }
37799 }
37800 }
37801 }
37802 } while (0);
37803 if (bgotonextstatement)
37804 {
37805 bool bgotonextstatement = true;
37806 do
37807 {
37808 IkReal x538 = ((1.0) * cj11);
37809 IkReal x539
37810 = ((((-1.0) * x538))
37811 + new_r22);
37812 IkReal x540 = ((1.0) * sj11);
37813 IkReal x541
37814 = ((((-1.0) * x540))
37815 + new_r02);
37816 evalcond[0]
37817 = ((-3.14159265358979)
37818 + (IKfmod(
37819 ((3.14159265358979)
37820 + (IKabs(j10))),
37821 6.28318530717959)));
37822 evalcond[1] = x539;
37823 evalcond[2] = x539;
37824 evalcond[3] = x541;
37825 evalcond[4] = new_r12;
37826 evalcond[5] = x541;
37827 evalcond[6]
37828 = ((((-1.0) * new_r22 * x540))
37829 + ((cj11 * new_r02)));
37830 evalcond[7]
37831 = ((((-1.0) * new_r20 * x538))
37832 + (((-1.0) * new_r00
37833 * x540)));
37834 evalcond[8]
37835 = ((((-1.0) * new_r21 * x538))
37836 + (((-1.0) * new_r01
37837 * x540)));
37838 evalcond[9]
37839 = ((1.0)
37840 + (((-1.0) * new_r02
37841 * x540))
37842 + (((-1.0) * new_r22
37843 * x538)));
37844 if (IKabs(evalcond[0])
37845 < 0.0000010000000000
37846 && IKabs(evalcond[1])
37847 < 0.0000010000000000
37848 && IKabs(evalcond[2])
37849 < 0.0000010000000000
37850 && IKabs(evalcond[3])
37851 < 0.0000010000000000
37852 && IKabs(evalcond[4])
37853 < 0.0000010000000000
37854 && IKabs(evalcond[5])
37855 < 0.0000010000000000
37856 && IKabs(evalcond[6])
37857 < 0.0000010000000000
37858 && IKabs(evalcond[7])
37859 < 0.0000010000000000
37860 && IKabs(evalcond[8])
37861 < 0.0000010000000000
37862 && IKabs(evalcond[9])
37863 < 0.0000010000000000)
37864 {
37865 bgotonextstatement = false;
37866 {
37867 IkReal j12array[1],
37868 cj12array[1],
37869 sj12array[1];
37870 bool j12valid[1] = {false};
37871 _nj12 = 1;
37872 if (IKabs(new_r10)
37873 < IKFAST_ATAN2_MAGTHRESH
37874 && IKabs(new_r11)
37875 < IKFAST_ATAN2_MAGTHRESH
37876 && IKabs(
37877 IKsqr(new_r10)
37878 + IKsqr(new_r11)
37879 - 1)
37880 <= IKFAST_SINCOS_THRESH)
37881 continue;
37882 j12array[0] = IKatan2(
37883 new_r10, new_r11);
37884 sj12array[0]
37885 = IKsin(j12array[0]);
37886 cj12array[0]
37887 = IKcos(j12array[0]);
37888 if (j12array[0] > IKPI)
37889 {
37890 j12array[0] -= IK2PI;
37891 }
37892 else if (j12array[0] < -IKPI)
37893 {
37894 j12array[0] += IK2PI;
37895 }
37896 j12valid[0] = true;
37897 for (int ij12 = 0; ij12 < 1;
37898 ++ij12)
37899 {
37900 if (!j12valid[ij12])
37901 {
37902 continue;
37903 }
37904 _ij12[0] = ij12;
37905 _ij12[1] = -1;
37906 for (int iij12 = ij12 + 1;
37907 iij12 < 1;
37908 ++iij12)
37909 {
37910 if (j12valid[iij12]
37911 && IKabs(
37912 cj12array[ij12]
37913 - cj12array
37914 [iij12])
37915 < IKFAST_SOLUTION_THRESH
37916 && IKabs(
37917 sj12array[ij12]
37918 - sj12array
37919 [iij12])
37920 < IKFAST_SOLUTION_THRESH)
37921 {
37922 j12valid[iij12] = false;
37923 _ij12[1] = iij12;
37924 break;
37925 }
37926 }
37927 j12 = j12array[ij12];
37928 cj12 = cj12array[ij12];
37929 sj12 = sj12array[ij12];
37930 {
37931 IkReal evalcond[8];
37932 IkReal x542 = IKcos(j12);
37933 IkReal x543 = IKsin(j12);
37934 IkReal x544
37935 = ((1.0) * x542);
37936 IkReal x545
37937 = ((-1.0) * x544);
37938 IkReal x546
37939 = ((1.0) * new_r02);
37940 evalcond[0]
37941 = (((new_r02 * x542))
37942 + new_r20);
37943 evalcond[1]
37944 = ((((-1.0) * x543))
37945 + new_r10);
37946 evalcond[2]
37947 = (new_r11 + x545);
37948 evalcond[3]
37949 = (((new_r22 * x543))
37950 + new_r01);
37951 evalcond[4]
37952 = ((((-1.0) * x543
37953 * x546))
37954 + new_r21);
37955 evalcond[5]
37956 = ((((-1.0) * new_r22
37957 * x544))
37958 + new_r00);
37959 evalcond[6]
37960 = (((new_r01
37961 * new_r22))
37962 + (((-1.0)
37963 * new_r21
37964 * x546))
37965 + x543);
37966 evalcond[7]
37967 = ((((-1.0) * new_r20
37968 * x546))
37969 + ((new_r00
37970 * new_r22))
37971 + x545);
37972 if (IKabs(evalcond[0])
37973 > IKFAST_EVALCOND_THRESH
37974 || IKabs(evalcond[1])
37975 > IKFAST_EVALCOND_THRESH
37976 || IKabs(evalcond[2])
37977 > IKFAST_EVALCOND_THRESH
37978 || IKabs(evalcond[3])
37979 > IKFAST_EVALCOND_THRESH
37980 || IKabs(evalcond[4])
37981 > IKFAST_EVALCOND_THRESH
37982 || IKabs(evalcond[5])
37983 > IKFAST_EVALCOND_THRESH
37984 || IKabs(evalcond[6])
37985 > IKFAST_EVALCOND_THRESH
37986 || IKabs(evalcond[7])
37987 > IKFAST_EVALCOND_THRESH)
37988 {
37989 continue;
37990 }
37991 }
37992
37993 {
37994 std::vector<
37995 IkSingleDOFSolutionBase<
37996 IkReal> >
37997 vinfos(7);
37998 vinfos[0].jointtype = 1;
37999 vinfos[0].foffset = j4;
38000 vinfos[0].indices[0]
38001 = _ij4[0];
38002 vinfos[0].indices[1]
38003 = _ij4[1];
38004 vinfos[0].maxsolutions
38005 = _nj4;
38006 vinfos[1].jointtype = 1;
38007 vinfos[1].foffset = j6;
38008 vinfos[1].indices[0]
38009 = _ij6[0];
38010 vinfos[1].indices[1]
38011 = _ij6[1];
38012 vinfos[1].maxsolutions
38013 = _nj6;
38014 vinfos[2].jointtype = 1;
38015 vinfos[2].foffset = j8;
38016 vinfos[2].indices[0]
38017 = _ij8[0];
38018 vinfos[2].indices[1]
38019 = _ij8[1];
38020 vinfos[2].maxsolutions
38021 = _nj8;
38022 vinfos[3].jointtype = 1;
38023 vinfos[3].foffset = j9;
38024 vinfos[3].indices[0]
38025 = _ij9[0];
38026 vinfos[3].indices[1]
38027 = _ij9[1];
38028 vinfos[3].maxsolutions
38029 = _nj9;
38030 vinfos[4].jointtype = 1;
38031 vinfos[4].foffset = j10;
38032 vinfos[4].indices[0]
38033 = _ij10[0];
38034 vinfos[4].indices[1]
38035 = _ij10[1];
38036 vinfos[4].maxsolutions
38037 = _nj10;
38038 vinfos[5].jointtype = 1;
38039 vinfos[5].foffset = j11;
38040 vinfos[5].indices[0]
38041 = _ij11[0];
38042 vinfos[5].indices[1]
38043 = _ij11[1];
38044 vinfos[5].maxsolutions
38045 = _nj11;
38046 vinfos[6].jointtype = 1;
38047 vinfos[6].foffset = j12;
38048 vinfos[6].indices[0]
38049 = _ij12[0];
38050 vinfos[6].indices[1]
38051 = _ij12[1];
38052 vinfos[6].maxsolutions
38053 = _nj12;
38054 std::vector<int> vfree(0);
38055 solutions.AddSolution(
38056 vinfos, vfree);
38057 }
38058 }
38059 }
38060 }
38061 } while (0);
38062 if (bgotonextstatement)
38063 {
38064 bool bgotonextstatement = true;
38065 do
38066 {
38067 IkReal x547 = ((1.0) * cj11);
38068 IkReal x548
38069 = ((((-1.0) * x547))
38070 + new_r22);
38071 IkReal x549 = ((1.0) * sj11);
38072 evalcond[0]
38073 = ((-3.14159265358979)
38074 + (IKfmod(
38075 ((3.14159265358979)
38076 + (IKabs((
38077 (-3.14159265358979)
38078 + j10)))),
38079 6.28318530717959)));
38080 evalcond[1] = x548;
38081 evalcond[2] = x548;
38082 evalcond[3] = (sj11 + new_r02);
38083 evalcond[4] = new_r12;
38084 evalcond[5]
38085 = ((((-1.0) * x549))
38086 + (((-1.0) * (1.0)
38087 * new_r02)));
38088 evalcond[6]
38089 = ((((-1.0) * new_r22
38090 * x549))
38091 + (((-1.0) * new_r02
38092 * x547)));
38093 evalcond[7]
38094 = ((((-1.0) * new_r20
38095 * x547))
38096 + ((new_r00 * sj11)));
38097 evalcond[8]
38098 = ((((-1.0) * new_r21
38099 * x547))
38100 + ((new_r01 * sj11)));
38101 evalcond[9]
38102 = ((1.0)
38103 + ((new_r02 * sj11))
38104 + (((-1.0) * new_r22
38105 * x547)));
38106 if (IKabs(evalcond[0])
38107 < 0.0000010000000000
38108 && IKabs(evalcond[1])
38109 < 0.0000010000000000
38110 && IKabs(evalcond[2])
38111 < 0.0000010000000000
38112 && IKabs(evalcond[3])
38113 < 0.0000010000000000
38114 && IKabs(evalcond[4])
38115 < 0.0000010000000000
38116 && IKabs(evalcond[5])
38117 < 0.0000010000000000
38118 && IKabs(evalcond[6])
38119 < 0.0000010000000000
38120 && IKabs(evalcond[7])
38121 < 0.0000010000000000
38122 && IKabs(evalcond[8])
38123 < 0.0000010000000000
38124 && IKabs(evalcond[9])
38125 < 0.0000010000000000)
38126 {
38127 bgotonextstatement = false;
38128 {
38129 IkReal j12array[1],
38130 cj12array[1],
38131 sj12array[1];
38132 bool j12valid[1] = {false};
38133 _nj12 = 1;
38134 CheckValue<IkReal> x550
38135 = IKatan2WithCheck(
38136 IkReal((
38137 (-1.0)
38138 * (((1.0)
38139 * new_r21)))),
38140 new_r20,
38141 IKFAST_ATAN2_MAGTHRESH);
38142 if (!x550.valid)
38143 {
38144 continue;
38145 }
38146 CheckValue<IkReal> x551
38147 = IKPowWithIntegerCheck(
38148 IKsign(new_r02),
38149 -1);
38150 if (!x551.valid)
38151 {
38152 continue;
38153 }
38154 j12array[0]
38155 = ((-1.5707963267949)
38156 + (x550.value)
38157 + (((1.5707963267949)
38158 * (x551.value))));
38159 sj12array[0]
38160 = IKsin(j12array[0]);
38161 cj12array[0]
38162 = IKcos(j12array[0]);
38163 if (j12array[0] > IKPI)
38164 {
38165 j12array[0] -= IK2PI;
38166 }
38167 else if (
38168 j12array[0] < -IKPI)
38169 {
38170 j12array[0] += IK2PI;
38171 }
38172 j12valid[0] = true;
38173 for (int ij12 = 0; ij12 < 1;
38174 ++ij12)
38175 {
38176 if (!j12valid[ij12])
38177 {
38178 continue;
38179 }
38180 _ij12[0] = ij12;
38181 _ij12[1] = -1;
38182 for (int iij12 = ij12 + 1;
38183 iij12 < 1;
38184 ++iij12)
38185 {
38186 if (j12valid[iij12]
38187 && IKabs(
38188 cj12array
38189 [ij12]
38190 - cj12array
38191 [iij12])
38192 < IKFAST_SOLUTION_THRESH
38193 && IKabs(
38194 sj12array
38195 [ij12]
38196 - sj12array
38197 [iij12])
38198 < IKFAST_SOLUTION_THRESH)
38199 {
38200 j12valid[iij12]
38201 = false;
38202 _ij12[1] = iij12;
38203 break;
38204 }
38205 }
38206 j12 = j12array[ij12];
38207 cj12 = cj12array[ij12];
38208 sj12 = sj12array[ij12];
38209 {
38210 IkReal evalcond[8];
38211 IkReal x552
38212 = IKsin(j12);
38213 IkReal x553
38214 = ((1.0)
38215 * (IKcos(j12)));
38216 IkReal x554
38217 = ((-1.0) * x553);
38218 IkReal x555
38219 = ((1.0) * new_r01);
38220 IkReal x556
38221 = ((1.0) * new_r00);
38222 evalcond[0]
38223 = (((new_r02
38224 * x552))
38225 + new_r21);
38226 evalcond[1]
38227 = ((((-1.0)
38228 * new_r02
38229 * x553))
38230 + new_r20);
38231 evalcond[2]
38232 = ((((-1.0) * (1.0)
38233 * new_r10))
38234 + (((-1.0)
38235 * x552)));
38236 evalcond[3]
38237 = (x554
38238 + (((-1.0)
38239 * (1.0)
38240 * new_r11)));
38241 evalcond[4]
38242 = (((new_r22
38243 * x552))
38244 + (((-1.0)
38245 * x555)));
38246 evalcond[5]
38247 = ((((-1.0) * x556))
38248 + (((-1.0)
38249 * new_r22
38250 * x553)));
38251 evalcond[6]
38252 = (((new_r02
38253 * new_r21))
38254 + (((-1.0)
38255 * new_r22
38256 * x555))
38257 + x552);
38258 evalcond[7]
38259 = (((new_r02
38260 * new_r20))
38261 + x554
38262 + (((-1.0)
38263 * new_r22
38264 * x556)));
38265 if (IKabs(evalcond[0])
38266 > IKFAST_EVALCOND_THRESH
38267 || IKabs(
38268 evalcond[1])
38269 > IKFAST_EVALCOND_THRESH
38270 || IKabs(
38271 evalcond[2])
38272 > IKFAST_EVALCOND_THRESH
38273 || IKabs(
38274 evalcond[3])
38275 > IKFAST_EVALCOND_THRESH
38276 || IKabs(
38277 evalcond[4])
38278 > IKFAST_EVALCOND_THRESH
38279 || IKabs(
38280 evalcond[5])
38281 > IKFAST_EVALCOND_THRESH
38282 || IKabs(
38283 evalcond[6])
38284 > IKFAST_EVALCOND_THRESH
38285 || IKabs(
38286 evalcond[7])
38287 > IKFAST_EVALCOND_THRESH)
38288 {
38289 continue;
38290 }
38291 }
38292
38293 {
38294 std::vector<
38295 IkSingleDOFSolutionBase<
38296 IkReal> >
38297 vinfos(7);
38298 vinfos[0].jointtype = 1;
38299 vinfos[0].foffset = j4;
38300 vinfos[0].indices[0]
38301 = _ij4[0];
38302 vinfos[0].indices[1]
38303 = _ij4[1];
38304 vinfos[0].maxsolutions
38305 = _nj4;
38306 vinfos[1].jointtype = 1;
38307 vinfos[1].foffset = j6;
38308 vinfos[1].indices[0]
38309 = _ij6[0];
38310 vinfos[1].indices[1]
38311 = _ij6[1];
38312 vinfos[1].maxsolutions
38313 = _nj6;
38314 vinfos[2].jointtype = 1;
38315 vinfos[2].foffset = j8;
38316 vinfos[2].indices[0]
38317 = _ij8[0];
38318 vinfos[2].indices[1]
38319 = _ij8[1];
38320 vinfos[2].maxsolutions
38321 = _nj8;
38322 vinfos[3].jointtype = 1;
38323 vinfos[3].foffset = j9;
38324 vinfos[3].indices[0]
38325 = _ij9[0];
38326 vinfos[3].indices[1]
38327 = _ij9[1];
38328 vinfos[3].maxsolutions
38329 = _nj9;
38330 vinfos[4].jointtype = 1;
38331 vinfos[4].foffset = j10;
38332 vinfos[4].indices[0]
38333 = _ij10[0];
38334 vinfos[4].indices[1]
38335 = _ij10[1];
38336 vinfos[4].maxsolutions
38337 = _nj10;
38338 vinfos[5].jointtype = 1;
38339 vinfos[5].foffset = j11;
38340 vinfos[5].indices[0]
38341 = _ij11[0];
38342 vinfos[5].indices[1]
38343 = _ij11[1];
38344 vinfos[5].maxsolutions
38345 = _nj11;
38346 vinfos[6].jointtype = 1;
38347 vinfos[6].foffset = j12;
38348 vinfos[6].indices[0]
38349 = _ij12[0];
38350 vinfos[6].indices[1]
38351 = _ij12[1];
38352 vinfos[6].maxsolutions
38353 = _nj12;
38354 std::vector<int> vfree(
38355 0);
38356 solutions.AddSolution(
38357 vinfos, vfree);
38358 }
38359 }
38360 }
38361 }
38362 } while (0);
38363 if (bgotonextstatement)
38364 {
38365 bool bgotonextstatement = true;
38366 do
38367 {
38368 if (1)
38369 {
38370 bgotonextstatement = false;
38371 continue; // branch miss
38372 // [j12]
38373 }
38374 } while (0);
38375 if (bgotonextstatement)
38376 {
38377 }
38378 }
38379 }
38380 }
38381 }
38382 }
38383 }
38384 }
38385 }
38386 }
38387 }
38388 else
38389 {
38390 {
38391 IkReal j12array[1], cj12array[1], sj12array[1];
38392 bool j12valid[1] = {false};
38393 _nj12 = 1;
38394 CheckValue<IkReal> x558
38395 = IKPowWithIntegerCheck(sj11, -1);
38396 if (!x558.valid)
38397 {
38398 continue;
38399 }
38400 IkReal x557 = x558.value;
38401 CheckValue<IkReal> x559
38402 = IKPowWithIntegerCheck(cj10, -1);
38403 if (!x559.valid)
38404 {
38405 continue;
38406 }
38407 CheckValue<IkReal> x560
38408 = IKPowWithIntegerCheck(cj11, -1);
38409 if (!x560.valid)
38410 {
38411 continue;
38412 }
38413 if (IKabs(
38414 (x557 * (x559.value) * (x560.value)
38415 * (((((-1.0) * (1.0) * new_r01 * sj11))
38416 + ((new_r20 * sj10))))))
38417 < IKFAST_ATAN2_MAGTHRESH
38418 && IKabs(((-1.0) * new_r20 * x557))
38419 < IKFAST_ATAN2_MAGTHRESH
38420 && IKabs(
38421 IKsqr(
38422 (x557 * (x559.value)
38423 * (x560.value)
38424 * (((((-1.0) * (1.0) * new_r01
38425 * sj11))
38426 + ((new_r20 * sj10))))))
38427 + IKsqr(((-1.0) * new_r20 * x557))
38428 - 1)
38429 <= IKFAST_SINCOS_THRESH)
38430 continue;
38431 j12array[0] = IKatan2(
38432 (x557 * (x559.value) * (x560.value)
38433 * (((((-1.0) * (1.0) * new_r01 * sj11))
38434 + ((new_r20 * sj10))))),
38435 ((-1.0) * new_r20 * x557));
38436 sj12array[0] = IKsin(j12array[0]);
38437 cj12array[0] = IKcos(j12array[0]);
38438 if (j12array[0] > IKPI)
38439 {
38440 j12array[0] -= IK2PI;
38441 }
38442 else if (j12array[0] < -IKPI)
38443 {
38444 j12array[0] += IK2PI;
38445 }
38446 j12valid[0] = true;
38447 for (int ij12 = 0; ij12 < 1; ++ij12)
38448 {
38449 if (!j12valid[ij12])
38450 {
38451 continue;
38452 }
38453 _ij12[0] = ij12;
38454 _ij12[1] = -1;
38455 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38456 {
38457 if (j12valid[iij12]
38458 && IKabs(
38459 cj12array[ij12]
38460 - cj12array[iij12])
38461 < IKFAST_SOLUTION_THRESH
38462 && IKabs(
38463 sj12array[ij12]
38464 - sj12array[iij12])
38465 < IKFAST_SOLUTION_THRESH)
38466 {
38467 j12valid[iij12] = false;
38468 _ij12[1] = iij12;
38469 break;
38470 }
38471 }
38472 j12 = j12array[ij12];
38473 cj12 = cj12array[ij12];
38474 sj12 = sj12array[ij12];
38475 {
38476 IkReal evalcond[12];
38477 IkReal x561 = IKcos(j12);
38478 IkReal x562 = IKsin(j12);
38479 IkReal x563 = ((1.0) * sj11);
38480 IkReal x564 = (cj10 * new_r01);
38481 IkReal x565 = (new_r11 * sj10);
38482 IkReal x566 = (cj11 * x562);
38483 IkReal x567 = ((1.0) * sj10);
38484 IkReal x568 = ((1.0) * x562);
38485 IkReal x569 = ((1.0) * x561);
38486 IkReal x570 = ((-1.0) * x569);
38487 IkReal x571 = (cj10 * new_r00);
38488 IkReal x572 = (new_r10 * sj10);
38489 IkReal x573 = (cj10 * x569);
38490 evalcond[0] = (((sj11 * x561)) + new_r20);
38491 evalcond[1]
38492 = ((((-1.0) * x562 * x563)) + new_r21);
38493 evalcond[2] = (x564 + x565 + x566);
38494 evalcond[3]
38495 = (((cj10 * new_r10))
38496 + (((-1.0) * x568))
38497 + (((-1.0) * new_r00 * x567)));
38498 evalcond[4]
38499 = (((cj10 * new_r11)) + x570
38500 + (((-1.0) * new_r01 * x567)));
38501 evalcond[5]
38502 = (((cj10 * x566)) + new_r01
38503 + ((sj10 * x561)));
38504 evalcond[6]
38505 = (x572 + x571
38506 + (((-1.0) * cj11 * x569)));
38507 evalcond[7]
38508 = (((sj10 * x562))
38509 + (((-1.0) * cj11 * x573))
38510 + new_r00);
38511 evalcond[8]
38512 = ((((-1.0) * x573)) + new_r11
38513 + ((sj10 * x566)));
38514 evalcond[9]
38515 = ((((-1.0) * cj10 * x568))
38516 + (((-1.0) * cj11 * x561 * x567))
38517 + new_r10);
38518 evalcond[10]
38519 = ((((-1.0) * new_r21 * x563))
38520 + ((cj11 * x565)) + ((cj11 * x564))
38521 + x562);
38522 evalcond[11]
38523 = (x570 + ((cj11 * x572))
38524 + ((cj11 * x571))
38525 + (((-1.0) * new_r20 * x563)));
38526 if (IKabs(evalcond[0])
38527 > IKFAST_EVALCOND_THRESH
38528 || IKabs(evalcond[1])
38529 > IKFAST_EVALCOND_THRESH
38530 || IKabs(evalcond[2])
38531 > IKFAST_EVALCOND_THRESH
38532 || IKabs(evalcond[3])
38533 > IKFAST_EVALCOND_THRESH
38534 || IKabs(evalcond[4])
38535 > IKFAST_EVALCOND_THRESH
38536 || IKabs(evalcond[5])
38537 > IKFAST_EVALCOND_THRESH
38538 || IKabs(evalcond[6])
38539 > IKFAST_EVALCOND_THRESH
38540 || IKabs(evalcond[7])
38541 > IKFAST_EVALCOND_THRESH
38542 || IKabs(evalcond[8])
38543 > IKFAST_EVALCOND_THRESH
38544 || IKabs(evalcond[9])
38545 > IKFAST_EVALCOND_THRESH
38546 || IKabs(evalcond[10])
38547 > IKFAST_EVALCOND_THRESH
38548 || IKabs(evalcond[11])
38549 > IKFAST_EVALCOND_THRESH)
38550 {
38551 continue;
38552 }
38553 }
38554
38555 {
38556 std::vector<
38557 IkSingleDOFSolutionBase<IkReal> >
38558 vinfos(7);
38559 vinfos[0].jointtype = 1;
38560 vinfos[0].foffset = j4;
38561 vinfos[0].indices[0] = _ij4[0];
38562 vinfos[0].indices[1] = _ij4[1];
38563 vinfos[0].maxsolutions = _nj4;
38564 vinfos[1].jointtype = 1;
38565 vinfos[1].foffset = j6;
38566 vinfos[1].indices[0] = _ij6[0];
38567 vinfos[1].indices[1] = _ij6[1];
38568 vinfos[1].maxsolutions = _nj6;
38569 vinfos[2].jointtype = 1;
38570 vinfos[2].foffset = j8;
38571 vinfos[2].indices[0] = _ij8[0];
38572 vinfos[2].indices[1] = _ij8[1];
38573 vinfos[2].maxsolutions = _nj8;
38574 vinfos[3].jointtype = 1;
38575 vinfos[3].foffset = j9;
38576 vinfos[3].indices[0] = _ij9[0];
38577 vinfos[3].indices[1] = _ij9[1];
38578 vinfos[3].maxsolutions = _nj9;
38579 vinfos[4].jointtype = 1;
38580 vinfos[4].foffset = j10;
38581 vinfos[4].indices[0] = _ij10[0];
38582 vinfos[4].indices[1] = _ij10[1];
38583 vinfos[4].maxsolutions = _nj10;
38584 vinfos[5].jointtype = 1;
38585 vinfos[5].foffset = j11;
38586 vinfos[5].indices[0] = _ij11[0];
38587 vinfos[5].indices[1] = _ij11[1];
38588 vinfos[5].maxsolutions = _nj11;
38589 vinfos[6].jointtype = 1;
38590 vinfos[6].foffset = j12;
38591 vinfos[6].indices[0] = _ij12[0];
38592 vinfos[6].indices[1] = _ij12[1];
38593 vinfos[6].maxsolutions = _nj12;
38594 std::vector<int> vfree(0);
38595 solutions.AddSolution(vinfos, vfree);
38596 }
38597 }
38598 }
38599 }
38600 }
38601 }
38602 else
38603 {
38604 {
38605 IkReal j12array[1], cj12array[1], sj12array[1];
38606 bool j12valid[1] = {false};
38607 _nj12 = 1;
38608 CheckValue<IkReal> x576
38609 = IKPowWithIntegerCheck(sj11, -1);
38610 if (!x576.valid)
38611 {
38612 continue;
38613 }
38614 IkReal x574 = x576.value;
38615 IkReal x575 = ((1.0) * new_r20);
38616 CheckValue<IkReal> x577
38617 = IKPowWithIntegerCheck(sj10, -1);
38618 if (!x577.valid)
38619 {
38620 continue;
38621 }
38622 if (IKabs(
38623 (x574 * (x577.value)
38624 * (((((-1.0) * cj10 * cj11 * x575))
38625 + (((-1.0) * (1.0) * new_r00
38626 * sj11))))))
38627 < IKFAST_ATAN2_MAGTHRESH
38628 && IKabs(((-1.0) * x574 * x575))
38629 < IKFAST_ATAN2_MAGTHRESH
38630 && IKabs(
38631 IKsqr(
38632 (x574 * (x577.value)
38633 * (((((-1.0) * cj10 * cj11 * x575))
38634 + (((-1.0) * (1.0) * new_r00
38635 * sj11))))))
38636 + IKsqr(((-1.0) * x574 * x575)) - 1)
38637 <= IKFAST_SINCOS_THRESH)
38638 continue;
38639 j12array[0] = IKatan2(
38640 (x574 * (x577.value)
38641 * (((((-1.0) * cj10 * cj11 * x575))
38642 + (((-1.0) * (1.0) * new_r00 * sj11))))),
38643 ((-1.0) * x574 * x575));
38644 sj12array[0] = IKsin(j12array[0]);
38645 cj12array[0] = IKcos(j12array[0]);
38646 if (j12array[0] > IKPI)
38647 {
38648 j12array[0] -= IK2PI;
38649 }
38650 else if (j12array[0] < -IKPI)
38651 {
38652 j12array[0] += IK2PI;
38653 }
38654 j12valid[0] = true;
38655 for (int ij12 = 0; ij12 < 1; ++ij12)
38656 {
38657 if (!j12valid[ij12])
38658 {
38659 continue;
38660 }
38661 _ij12[0] = ij12;
38662 _ij12[1] = -1;
38663 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38664 {
38665 if (j12valid[iij12]
38666 && IKabs(cj12array[ij12] - cj12array[iij12])
38667 < IKFAST_SOLUTION_THRESH
38668 && IKabs(sj12array[ij12] - sj12array[iij12])
38669 < IKFAST_SOLUTION_THRESH)
38670 {
38671 j12valid[iij12] = false;
38672 _ij12[1] = iij12;
38673 break;
38674 }
38675 }
38676 j12 = j12array[ij12];
38677 cj12 = cj12array[ij12];
38678 sj12 = sj12array[ij12];
38679 {
38680 IkReal evalcond[12];
38681 IkReal x578 = IKcos(j12);
38682 IkReal x579 = IKsin(j12);
38683 IkReal x580 = ((1.0) * sj11);
38684 IkReal x581 = (cj10 * new_r01);
38685 IkReal x582 = (new_r11 * sj10);
38686 IkReal x583 = (cj11 * x579);
38687 IkReal x584 = ((1.0) * sj10);
38688 IkReal x585 = ((1.0) * x579);
38689 IkReal x586 = ((1.0) * x578);
38690 IkReal x587 = ((-1.0) * x586);
38691 IkReal x588 = (cj10 * new_r00);
38692 IkReal x589 = (new_r10 * sj10);
38693 IkReal x590 = (cj10 * x586);
38694 evalcond[0] = (((sj11 * x578)) + new_r20);
38695 evalcond[1]
38696 = ((((-1.0) * x579 * x580)) + new_r21);
38697 evalcond[2] = (x581 + x582 + x583);
38698 evalcond[3]
38699 = (((cj10 * new_r10)) + (((-1.0) * x585))
38700 + (((-1.0) * new_r00 * x584)));
38701 evalcond[4]
38702 = (((cj10 * new_r11))
38703 + (((-1.0) * new_r01 * x584)) + x587);
38704 evalcond[5]
38705 = (((sj10 * x578)) + ((cj10 * x583))
38706 + new_r01);
38707 evalcond[6]
38708 = ((((-1.0) * cj11 * x586)) + x589 + x588);
38709 evalcond[7]
38710 = (new_r00 + ((sj10 * x579))
38711 + (((-1.0) * cj11 * x590)));
38712 evalcond[8]
38713 = ((((-1.0) * x590)) + ((sj10 * x583))
38714 + new_r11);
38715 evalcond[9]
38716 = ((((-1.0) * cj10 * x585))
38717 + (((-1.0) * cj11 * x578 * x584))
38718 + new_r10);
38719 evalcond[10]
38720 = (x579 + ((cj11 * x581)) + ((cj11 * x582))
38721 + (((-1.0) * new_r21 * x580)));
38722 evalcond[11]
38723 = (((cj11 * x589))
38724 + (((-1.0) * new_r20 * x580))
38725 + ((cj11 * x588)) + x587);
38726 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
38727 || IKabs(evalcond[1])
38728 > IKFAST_EVALCOND_THRESH
38729 || IKabs(evalcond[2])
38730 > IKFAST_EVALCOND_THRESH
38731 || IKabs(evalcond[3])
38732 > IKFAST_EVALCOND_THRESH
38733 || IKabs(evalcond[4])
38734 > IKFAST_EVALCOND_THRESH
38735 || IKabs(evalcond[5])
38736 > IKFAST_EVALCOND_THRESH
38737 || IKabs(evalcond[6])
38738 > IKFAST_EVALCOND_THRESH
38739 || IKabs(evalcond[7])
38740 > IKFAST_EVALCOND_THRESH
38741 || IKabs(evalcond[8])
38742 > IKFAST_EVALCOND_THRESH
38743 || IKabs(evalcond[9])
38744 > IKFAST_EVALCOND_THRESH
38745 || IKabs(evalcond[10])
38746 > IKFAST_EVALCOND_THRESH
38747 || IKabs(evalcond[11])
38748 > IKFAST_EVALCOND_THRESH)
38749 {
38750 continue;
38751 }
38752 }
38753
38754 {
38755 std::vector<IkSingleDOFSolutionBase<IkReal> >
38756 vinfos(7);
38757 vinfos[0].jointtype = 1;
38758 vinfos[0].foffset = j4;
38759 vinfos[0].indices[0] = _ij4[0];
38760 vinfos[0].indices[1] = _ij4[1];
38761 vinfos[0].maxsolutions = _nj4;
38762 vinfos[1].jointtype = 1;
38763 vinfos[1].foffset = j6;
38764 vinfos[1].indices[0] = _ij6[0];
38765 vinfos[1].indices[1] = _ij6[1];
38766 vinfos[1].maxsolutions = _nj6;
38767 vinfos[2].jointtype = 1;
38768 vinfos[2].foffset = j8;
38769 vinfos[2].indices[0] = _ij8[0];
38770 vinfos[2].indices[1] = _ij8[1];
38771 vinfos[2].maxsolutions = _nj8;
38772 vinfos[3].jointtype = 1;
38773 vinfos[3].foffset = j9;
38774 vinfos[3].indices[0] = _ij9[0];
38775 vinfos[3].indices[1] = _ij9[1];
38776 vinfos[3].maxsolutions = _nj9;
38777 vinfos[4].jointtype = 1;
38778 vinfos[4].foffset = j10;
38779 vinfos[4].indices[0] = _ij10[0];
38780 vinfos[4].indices[1] = _ij10[1];
38781 vinfos[4].maxsolutions = _nj10;
38782 vinfos[5].jointtype = 1;
38783 vinfos[5].foffset = j11;
38784 vinfos[5].indices[0] = _ij11[0];
38785 vinfos[5].indices[1] = _ij11[1];
38786 vinfos[5].maxsolutions = _nj11;
38787 vinfos[6].jointtype = 1;
38788 vinfos[6].foffset = j12;
38789 vinfos[6].indices[0] = _ij12[0];
38790 vinfos[6].indices[1] = _ij12[1];
38791 vinfos[6].maxsolutions = _nj12;
38792 std::vector<int> vfree(0);
38793 solutions.AddSolution(vinfos, vfree);
38794 }
38795 }
38796 }
38797 }
38798 }
38799 }
38800 else
38801 {
38802 {
38803 IkReal j12array[1], cj12array[1], sj12array[1];
38804 bool j12valid[1] = {false};
38805 _nj12 = 1;
38806 CheckValue<IkReal> x591
38807 = IKPowWithIntegerCheck(IKsign(sj11), -1);
38808 if (!x591.valid)
38809 {
38810 continue;
38811 }
38812 CheckValue<IkReal> x592 = IKatan2WithCheck(
38813 IkReal(new_r21),
38814 ((-1.0) * (((1.0) * new_r20))),
38815 IKFAST_ATAN2_MAGTHRESH);
38816 if (!x592.valid)
38817 {
38818 continue;
38819 }
38820 j12array[0]
38821 = ((-1.5707963267949)
38822 + (((1.5707963267949) * (x591.value)))
38823 + (x592.value));
38824 sj12array[0] = IKsin(j12array[0]);
38825 cj12array[0] = IKcos(j12array[0]);
38826 if (j12array[0] > IKPI)
38827 {
38828 j12array[0] -= IK2PI;
38829 }
38830 else if (j12array[0] < -IKPI)
38831 {
38832 j12array[0] += IK2PI;
38833 }
38834 j12valid[0] = true;
38835 for (int ij12 = 0; ij12 < 1; ++ij12)
38836 {
38837 if (!j12valid[ij12])
38838 {
38839 continue;
38840 }
38841 _ij12[0] = ij12;
38842 _ij12[1] = -1;
38843 for (int iij12 = ij12 + 1; iij12 < 1; ++iij12)
38844 {
38845 if (j12valid[iij12]
38846 && IKabs(cj12array[ij12] - cj12array[iij12])
38847 < IKFAST_SOLUTION_THRESH
38848 && IKabs(sj12array[ij12] - sj12array[iij12])
38849 < IKFAST_SOLUTION_THRESH)
38850 {
38851 j12valid[iij12] = false;
38852 _ij12[1] = iij12;
38853 break;
38854 }
38855 }
38856 j12 = j12array[ij12];
38857 cj12 = cj12array[ij12];
38858 sj12 = sj12array[ij12];
38859 {
38860 IkReal evalcond[12];
38861 IkReal x593 = IKcos(j12);
38862 IkReal x594 = IKsin(j12);
38863 IkReal x595 = ((1.0) * sj11);
38864 IkReal x596 = (cj10 * new_r01);
38865 IkReal x597 = (new_r11 * sj10);
38866 IkReal x598 = (cj11 * x594);
38867 IkReal x599 = ((1.0) * sj10);
38868 IkReal x600 = ((1.0) * x594);
38869 IkReal x601 = ((1.0) * x593);
38870 IkReal x602 = ((-1.0) * x601);
38871 IkReal x603 = (cj10 * new_r00);
38872 IkReal x604 = (new_r10 * sj10);
38873 IkReal x605 = (cj10 * x601);
38874 evalcond[0] = (new_r20 + ((sj11 * x593)));
38875 evalcond[1] = ((((-1.0) * x594 * x595)) + new_r21);
38876 evalcond[2] = (x598 + x596 + x597);
38877 evalcond[3]
38878 = (((cj10 * new_r10)) + (((-1.0) * x600))
38879 + (((-1.0) * new_r00 * x599)));
38880 evalcond[4]
38881 = (((cj10 * new_r11))
38882 + (((-1.0) * new_r01 * x599)) + x602);
38883 evalcond[5]
38884 = (((cj10 * x598)) + ((sj10 * x593)) + new_r01);
38885 evalcond[6]
38886 = ((((-1.0) * cj11 * x601)) + x603 + x604);
38887 evalcond[7]
38888 = ((((-1.0) * cj11 * x605)) + ((sj10 * x594))
38889 + new_r00);
38890 evalcond[8]
38891 = (((sj10 * x598)) + new_r11
38892 + (((-1.0) * x605)));
38893 evalcond[9]
38894 = ((((-1.0) * cj10 * x600))
38895 + (((-1.0) * cj11 * x593 * x599)) + new_r10);
38896 evalcond[10]
38897 = (((cj11 * x597)) + x594
38898 + (((-1.0) * new_r21 * x595))
38899 + ((cj11 * x596)));
38900 evalcond[11]
38901 = (((cj11 * x603)) + x602 + ((cj11 * x604))
38902 + (((-1.0) * new_r20 * x595)));
38903 if (IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH
38904 || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH
38905 || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH
38906 || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH
38907 || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH
38908 || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH
38909 || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH
38910 || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH
38911 || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH
38912 || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH
38913 || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH
38914 || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH)
38915 {
38916 continue;
38917 }
38918 }
38919
38920 {
38921 std::vector<IkSingleDOFSolutionBase<IkReal> >
38922 vinfos(7);
38923 vinfos[0].jointtype = 1;
38924 vinfos[0].foffset = j4;
38925 vinfos[0].indices[0] = _ij4[0];
38926 vinfos[0].indices[1] = _ij4[1];
38927 vinfos[0].maxsolutions = _nj4;
38928 vinfos[1].jointtype = 1;
38929 vinfos[1].foffset = j6;
38930 vinfos[1].indices[0] = _ij6[0];
38931 vinfos[1].indices[1] = _ij6[1];
38932 vinfos[1].maxsolutions = _nj6;
38933 vinfos[2].jointtype = 1;
38934 vinfos[2].foffset = j8;
38935 vinfos[2].indices[0] = _ij8[0];
38936 vinfos[2].indices[1] = _ij8[1];
38937 vinfos[2].maxsolutions = _nj8;
38938 vinfos[3].jointtype = 1;
38939 vinfos[3].foffset = j9;
38940 vinfos[3].indices[0] = _ij9[0];
38941 vinfos[3].indices[1] = _ij9[1];
38942 vinfos[3].maxsolutions = _nj9;
38943 vinfos[4].jointtype = 1;
38944 vinfos[4].foffset = j10;
38945 vinfos[4].indices[0] = _ij10[0];
38946 vinfos[4].indices[1] = _ij10[1];
38947 vinfos[4].maxsolutions = _nj10;
38948 vinfos[5].jointtype = 1;
38949 vinfos[5].foffset = j11;
38950 vinfos[5].indices[0] = _ij11[0];
38951 vinfos[5].indices[1] = _ij11[1];
38952 vinfos[5].maxsolutions = _nj11;
38953 vinfos[6].jointtype = 1;
38954 vinfos[6].foffset = j12;
38955 vinfos[6].indices[0] = _ij12[0];
38956 vinfos[6].indices[1] = _ij12[1];
38957 vinfos[6].maxsolutions = _nj12;
38958 std::vector<int> vfree(0);
38959 solutions.AddSolution(vinfos, vfree);
38960 }
38961 }
38962 }
38963 }
38964 }
38965 }
38966 }
38967 }
38968 }
38969 }
38970 }
38971 }
38972 }
38973 };
38974
38975 /// solves the inverse kinematics equations.
38976 /// \param pfree is an array specifying the free joints of the chain.
ComputeIk(const IkReal * eetrans,const IkReal * eerot,const IkReal * pfree,IkSolutionListBase<IkReal> & solutions)38977 IKFAST_API bool ComputeIk(
38978 const IkReal* eetrans,
38979 const IkReal* eerot,
38980 const IkReal* pfree,
38981 IkSolutionListBase<IkReal>& solutions)
38982 {
38983 IKSolver solver;
38984 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
38985 }
38986
ComputeIk2(const IkReal * eetrans,const IkReal * eerot,const IkReal * pfree,IkSolutionListBase<IkReal> & solutions,void * pOpenRAVEManip)38987 IKFAST_API bool ComputeIk2(
38988 const IkReal* eetrans,
38989 const IkReal* eerot,
38990 const IkReal* pfree,
38991 IkSolutionListBase<IkReal>& solutions,
38992 void* pOpenRAVEManip)
38993 {
38994 IKSolver solver;
38995 return solver.ComputeIk(eetrans, eerot, pfree, solutions);
38996 }
38997
GetKinematicsHash()38998 IKFAST_API const char* GetKinematicsHash()
38999 {
39000 return "268c2c509bc1bb657da055f0ef2eb7e1";
39001 }
39002
GetIkFastVersion()39003 IKFAST_API const char* GetIkFastVersion()
39004 {
39005 return IKFAST_STRINGIZE(IKFAST_VERSION);
39006 }
39007
39008 #ifdef IKFAST_NAMESPACE
39009 } // end namespace
39010 #endif
39011
39012 #ifndef IKFAST_NO_MAIN
39013 # include <stdio.h>
39014 # include <stdlib.h>
39015 # ifdef IKFAST_NAMESPACE
39016 using namespace IKFAST_NAMESPACE;
39017 # endif
main(int argc,char ** argv)39018 int main(int argc, char** argv)
39019 {
39020 if (argc != 12 + GetNumFreeParameters() + 1)
39021 {
39022 printf(
39023 "\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 "
39024 "...\n\n"
39025 "Returns the ik solutions given the transformation of the end effector "
39026 "specified by\n"
39027 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
39028 "There are %d free parameters that have to be specified.\n\n",
39029 GetNumFreeParameters());
39030 return 1;
39031 }
39032
39033 IkSolutionList<IkReal> solutions;
39034 std::vector<IkReal> vfree(GetNumFreeParameters());
39035 IkReal eerot[9], eetrans[3];
39036 eerot[0] = atof(argv[1]);
39037 eerot[1] = atof(argv[2]);
39038 eerot[2] = atof(argv[3]);
39039 eetrans[0] = atof(argv[4]);
39040 eerot[3] = atof(argv[5]);
39041 eerot[4] = atof(argv[6]);
39042 eerot[5] = atof(argv[7]);
39043 eetrans[1] = atof(argv[8]);
39044 eerot[6] = atof(argv[9]);
39045 eerot[7] = atof(argv[10]);
39046 eerot[8] = atof(argv[11]);
39047 eetrans[2] = atof(argv[12]);
39048 for (std::size_t i = 0; i < vfree.size(); ++i)
39049 vfree[i] = atof(argv[13 + i]);
39050 bool bSuccess = ComputeIk(
39051 eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
39052
39053 if (!bSuccess)
39054 {
39055 fprintf(stderr, "Failed to get ik solution\n");
39056 return -1;
39057 }
39058
39059 printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
39060 std::vector<IkReal> solvalues(GetNumJoints());
39061 for (std::size_t i = 0; i < solutions.GetNumSolutions(); ++i)
39062 {
39063 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
39064
39065 printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
39066
39067 std::vector<IkReal> vsolfree(sol.GetFree().size());
39068
39069 sol.GetSolution(&solvalues[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
39070
39071 for (std::size_t j = 0; j < solvalues.size(); ++j)
39072 printf("%.15f, ", solvalues[j]);
39073 printf("\n");
39074 }
39075 return 0;
39076 }
39077
39078 #endif
39079