moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
prbt_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
1
24#define IKFAST_HAS_LIBRARY
25#include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
26using namespace ikfast;
27
28// check if the included ikfast version matches what this file was compiled with
29static_assert(IKFAST_VERSION==61); // version found in ikfast.h
30
31#include <cmath>
32#include <vector>
33#include <limits>
34#include <algorithm>
35#include <complex>
36
37#ifndef IKFAST_ASSERT
38#include <stdexcept>
39#include <sstream>
40#include <iostream>
41
42#ifdef _MSC_VER
43#ifndef __PRETTY_FUNCTION__
44#define __PRETTY_FUNCTION__ __FUNCDNAME__
45#endif
46#endif
47
48#ifndef __PRETTY_FUNCTION__
49#define __PRETTY_FUNCTION__ __func__
50#endif
51
52#define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ':' << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
53
54#endif
55
56#if defined(_MSC_VER)
57#define IKFAST_ALIGNED16(x) __declspec(align(16)) x
58#else
59#define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
60#endif
61
62#define IK2PI ((IkReal)6.28318530717959)
63#define IKPI ((IkReal)3.14159265358979)
64#define IKPI_2 ((IkReal)1.57079632679490)
65
66#ifdef _MSC_VER
67#ifndef isnan
68#define isnan _isnan
69#endif
70#ifndef isinf
71#define isinf _isinf
72#endif
73//#ifndef isfinite
74//#define isfinite _isfinite
75//#endif
76#endif // _MSC_VER
77
78using namespace std; // necessary to get std math routines
79
80#ifdef IKFAST_NAMESPACE
81namespace IKFAST_NAMESPACE {
82#endif
83
84inline float IKabs(float f) { return fabsf(f); }
85inline double IKabs(double f) { return fabs(f); }
86
87inline float IKsqr(float f) { return f*f; }
88inline double IKsqr(double f) { return f*f; }
89
90inline float IKlog(float f) { return logf(f); }
91inline double IKlog(double f) { return log(f); }
92
93// allows asin and acos to exceed 1. has to be smaller than thresholds used for branch conds and evaluation
94#ifndef IKFAST_SINCOS_THRESH
95#define IKFAST_SINCOS_THRESH ((IkReal)1e-7)
96#endif
97
98// used to check input to atan2 for degenerate cases. has to be smaller than thresholds used for branch conds and evaluation
99#ifndef IKFAST_ATAN2_MAGTHRESH
100#define IKFAST_ATAN2_MAGTHRESH ((IkReal)1e-7)
101#endif
102
103// minimum distance of separate solutions
104#ifndef IKFAST_SOLUTION_THRESH
105#define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
106#endif
107
108// there are checkpoints in ikfast that are evaluated to make sure they are 0. This threshold specifies by how much they can deviate
109#ifndef IKFAST_EVALCOND_THRESH
110#define IKFAST_EVALCOND_THRESH ((IkReal)0.00001)
111#endif
112
113
114inline float IKasin(float f)
115{
116IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
117if( f <= -1 ) return float(-IKPI_2);
118else if( f >= 1 ) return float(IKPI_2);
119return asinf(f);
120}
121inline double IKasin(double f)
122{
123IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
124if( f <= -1 ) return -IKPI_2;
125else if( f >= 1 ) return IKPI_2;
126return asin(f);
127}
128
129// return positive value in [0,y)
130inline float IKfmod(float x, float y)
131{
132 while(x < 0) {
133 x += y;
134 }
135 return fmodf(x,y);
136}
137
138// return positive value in [0,y)
139inline double IKfmod(double x, double y)
140{
141 while(x < 0) {
142 x += y;
143 }
144 return fmod(x,y);
145}
146
147inline float IKacos(float f)
148{
149IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
150if( f <= -1 ) return float(IKPI);
151else if( f >= 1 ) return float(0);
152return acosf(f);
153}
154inline double IKacos(double f)
155{
156IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
157if( f <= -1 ) return IKPI;
158else if( f >= 1 ) return 0;
159return acos(f);
160}
161inline float IKsin(float f) { return sinf(f); }
162inline double IKsin(double f) { return sin(f); }
163inline float IKcos(float f) { return cosf(f); }
164inline double IKcos(double f) { return cos(f); }
165inline float IKtan(float f) { return tanf(f); }
166inline double IKtan(double f) { return tan(f); }
167inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
168inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
169inline float IKatan2Simple(float fy, float fx) {
170 return atan2f(fy,fx);
171}
172inline float IKatan2(float fy, float fx) {
173 if( isnan(fy) ) {
174 IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
175 return float(IKPI_2);
176 }
177 else if( isnan(fx) ) {
178 return 0;
179 }
180 return atan2f(fy,fx);
181}
182inline double IKatan2Simple(double fy, double fx) {
183 return atan2(fy,fx);
184}
185inline double IKatan2(double fy, double fx) {
186 if( isnan(fy) ) {
187 IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
188 return IKPI_2;
189 }
190 else if( isnan(fx) ) {
191 return 0;
192 }
193 return atan2(fy,fx);
194}
195
196template <typename T>
197struct CheckValue
198{
199 T value;
200 bool valid;
201};
202
203template <typename T>
204inline CheckValue<T> IKatan2WithCheck(T fy, T fx, T /* unused */)
205{
206 CheckValue<T> ret;
207 ret.valid = false;
208 ret.value = 0;
209 if( !isnan(fy) && !isnan(fx) ) {
210 if( IKabs(fy) >= IKFAST_ATAN2_MAGTHRESH || IKabs(fx) > IKFAST_ATAN2_MAGTHRESH ) {
211 ret.value = IKatan2Simple(fy,fx);
212 ret.valid = true;
213 }
214 }
215 return ret;
216}
217
218inline float IKsign(float f) {
219 if( f > 0 ) {
220 return float(1);
221 }
222 else if( f < 0 ) {
223 return float(-1);
224 }
225 return 0;
226}
227
228inline double IKsign(double f) {
229 if( f > 0 ) {
230 return 1.0;
231 }
232 else if( f < 0 ) {
233 return -1.0;
234 }
235 return 0;
236}
237
238template <typename T>
239inline CheckValue<T> IKPowWithIntegerCheck(T f, int n)
240{
241 CheckValue<T> ret;
242 ret.valid = true;
243 if( n == 0 ) {
244 ret.value = 1.0;
245 return ret;
246 }
247 else if( n == 1 )
248 {
249 ret.value = f;
250 return ret;
251 }
252 else if( n < 0 )
253 {
254 if( f == 0 )
255 {
256 ret.valid = false;
257 ret.value = (T)1.0e30;
258 return ret;
259 }
260 if( n == -1 ) {
261 ret.value = T(1.0)/f;
262 return ret;
263 }
264 }
265
266 int num = n > 0 ? n : -n;
267 if( num == 2 ) {
268 ret.value = f*f;
269 }
270 else if( num == 3 ) {
271 ret.value = f*f*f;
272 }
273 else {
274 ret.value = 1.0;
275 while(num>0) {
276 if( num & 1 ) {
277 ret.value *= f;
278 }
279 num >>= 1;
280 f *= f;
281 }
282 }
283
284 if( n < 0 ) {
285 ret.value = T(1.0)/ret.value;
286 }
287 return ret;
288}
289
292IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
293IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46;
294x0=IKcos(j[0]);
295x1=IKcos(j[1]);
296x2=IKcos(j[2]);
297x3=IKsin(j[1]);
298x4=IKsin(j[2]);
299x5=IKsin(j[3]);
300x6=IKcos(j[3]);
301x7=IKsin(j[0]);
302x8=IKcos(j[5]);
303x9=IKsin(j[5]);
304x10=IKsin(j[4]);
305x11=IKcos(j[4]);
306x12=((1.0)*x7);
307x13=((1.0)*x6);
308x14=((0.307)*x7);
309x15=((0.084)*x3);
310x16=((0.084)*x7);
311x17=((1.0)*x0);
312x18=((1.0)*x10);
313x19=((0.35)*x3);
314x20=((0.084)*x6);
315x21=((0.307)*x0);
316x22=((0.084)*x0);
317x23=(x1*x2);
318x24=(x1*x4);
319x25=((-1.0)*x6);
320x26=(x2*x3);
321x27=(x5*x7);
322x28=(x3*x4);
323x29=(x10*x6);
324x30=(x17*x5);
325x31=((1.0)*x26);
326x32=((((-1.0)*x31))+x24);
327x33=((((-1.0)*x24))+x31);
328x34=((((1.0)*x23))+(((1.0)*x28)));
329x35=(x11*x32);
330x36=(x33*x5);
331x37=(x17*(((((-1.0)*x24))+x26)));
332x38=(x12*(((((-1.0)*x24))+x26)));
333x39=(x17*(((((-1.0)*x23))+(((-1.0)*x28)))));
334x40=(x12*(((((-1.0)*x23))+(((-1.0)*x28)))));
335x41=(x10*x38);
336x42=(x39*x5);
337x43=(x39*x6);
338x44=(((x0*x6))+((x40*x5)));
339x45=(x30+((x25*x40)));
340x46=(x11*x45);
341eerot[0]=(((x9*(((((-1.0)*x12*x6))+x42))))+((x8*((((x10*x37))+((x11*(((((-1.0)*x27))+((x25*x39)))))))))));
342eerot[1]=((((-1.0)*x9*((((x18*x37))+(((1.0)*x11*(((((-1.0)*x12*x5))+(((-1.0)*x13*x39))))))))))+((x8*((x42+((x25*x7)))))));
343eerot[2]=(((x10*((x43+x27))))+((x11*x37)));
344IkReal x47=((1.0)*x24);
345eetrans[0]=(((x21*x26))+((x0*x19))+((x11*(((((-1.0)*x22*x47))+((x0*x15*x2))))))+(((-1.0)*x21*x47))+((x10*((((x20*x39))+((x16*x5)))))));
346eerot[3]=(((x44*x9))+((x8*((x46+x41)))));
347eerot[4]=(((x9*(((((-1.0)*x41))+(((-1.0)*x46))))))+((x44*x8)));
348eerot[5]=(((x10*(((((-1.0)*x30))+((x40*x6))))))+((x11*x38)));
349IkReal x48=((1.0)*x24);
350eetrans[1]=(((x19*x7))+((x14*x26))+((x10*((((x20*x40))+(((-1.0)*x22*x5))))))+((x11*(((((-1.0)*x16*x48))+((x15*x2*x7))))))+(((-1.0)*x14*x48)));
351eerot[6]=(((x8*((((x10*x34))+((x35*x6))))))+((x36*x9)));
352eerot[7]=(((x9*(((((-1.0)*x18*x34))+(((-1.0)*x13*x35))))))+((x36*x8)));
353eerot[8]=(((x29*x33))+((x11*x34)));
354eetrans[2]=((0.2604)+(((0.35)*x1))+((x29*(((((-0.084)*x24))+((x15*x2))))))+((x11*(((((0.084)*x23))+((x15*x4))))))+(((0.307)*x23))+(((0.307)*x28)));
355}
356
357IKFAST_API int GetNumFreeParameters() { return 0; }
358IKFAST_API int* GetFreeParameters() { return nullptr; }
359IKFAST_API int GetNumJoints() { return 6; }
360
361IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
362
363IKFAST_API int GetIkType() { return 0x67000001; }
364
365class IKSolver {
366public:
367IkReal j0,cj0,sj0,htj0,j0mul,j1,cj1,sj1,htj1,j1mul,j2,cj2,sj2,htj2,j2mul,j3,cj3,sj3,htj3,j3mul,j4,cj4,sj4,htj4,j4mul,j5,cj5,sj5,htj5,j5mul,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
368unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
369
370IkReal j100, cj100, sj100;
371unsigned char _ij100[2], _nj100;
372 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* /* unused */, IkSolutionListBase<IkReal>& solutions) {
373j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1;
374for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
375 solutions.Clear();
376r00 = eerot[0*3+0];
377r01 = eerot[0*3+1];
378r02 = eerot[0*3+2];
379r10 = eerot[1*3+0];
380r11 = eerot[1*3+1];
381r12 = eerot[1*3+2];
382r20 = eerot[2*3+0];
383r21 = eerot[2*3+1];
384r22 = eerot[2*3+2];
385px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
386
387new_r00=r00;
388new_r01=r01;
389new_r02=r02;
390new_px=(px+(((-0.084)*r02)));
391new_r10=r10;
392new_r11=r11;
393new_r12=r12;
394new_py=(py+(((-0.084)*r12)));
395new_r20=r20;
396new_r21=r21;
397new_r22=r22;
398new_pz=((-0.2604)+pz+(((-0.084)*r22)));
399r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz;
400IkReal x49=((1.0)*px);
401IkReal x50=((1.0)*pz);
402IkReal x51=((1.0)*py);
403pp=((px*px)+(py*py)+(pz*pz));
404npx=(((px*r00))+((py*r10))+((pz*r20)));
405npy=(((px*r01))+((py*r11))+((pz*r21)));
406npz=(((px*r02))+((py*r12))+((pz*r22)));
407rxp0_0=((((-1.0)*r20*x51))+((pz*r10)));
408rxp0_1=(((px*r20))+(((-1.0)*r00*x50)));
409rxp0_2=((((-1.0)*r10*x49))+((py*r00)));
410rxp1_0=((((-1.0)*r21*x51))+((pz*r11)));
411rxp1_1=(((px*r21))+(((-1.0)*r01*x50)));
412rxp1_2=((((-1.0)*r11*x49))+((py*r01)));
413rxp2_0=(((pz*r12))+(((-1.0)*r22*x51)));
414rxp2_1=(((px*r22))+(((-1.0)*r02*x50)));
415rxp2_2=((((-1.0)*r12*x49))+((py*r02)));
416{
417IkReal j2array[2], cj2array[2], sj2array[2];
418bool j2valid[2]={false};
419_nj2 = 2;
420cj2array[0]=((-1.00860400186133)+(((4.65332712889716)*pp)));
421if( cj2array[0] >= -1-IKFAST_SINCOS_THRESH && cj2array[0] <= 1+IKFAST_SINCOS_THRESH )
422{
423 j2valid[0] = j2valid[1] = true;
424 j2array[0] = IKacos(cj2array[0]);
425 sj2array[0] = IKsin(j2array[0]);
426 cj2array[1] = cj2array[0];
427 j2array[1] = -j2array[0];
428 sj2array[1] = -sj2array[0];
429}
430else if( isnan(cj2array[0]) )
431{
432 // probably any value will work
433 j2valid[0] = true;
434 cj2array[0] = 1; sj2array[0] = 0; j2array[0] = 0;
435}
436for(int ij2 = 0; ij2 < 2; ++ij2)
437{
438if( !j2valid[ij2] )
439{
440 continue;
441}
442_ij2[0] = ij2; _ij2[1] = -1;
443for(int iij2 = ij2+1; iij2 < 2; ++iij2)
444{
445if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
446{
447 j2valid[iij2]=false; _ij2[1] = iij2; break;
448}
449}
450j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
451
452{
453IkReal j0eval[1];
454j0eval[0]=((IKabs(px))+(IKabs(py)));
455if( IKabs(j0eval[0]) < 0.0000010000000000 )
456{
457{
458IkReal j1eval[2];
459j1eval[0]=((IKabs(sj2))+(((3.25732899022801)*(IKabs(((0.35)+(((0.307)*cj2))))))));
460j1eval[1]=((1.29974853844603)+(sj2*sj2)+(cj2*cj2)+(((2.28013029315961)*cj2)));
461if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 )
462{
463continue; // no branches [j0, j1]
464
465} else
466{
467{
468IkReal j1array[2], cj1array[2], sj1array[2];
469bool j1valid[2]={false};
470_nj1 = 2;
471IkReal x52=((0.35)+(((0.307)*cj2)));
472CheckValue<IkReal> x55 = IKatan2WithCheck(IkReal(x52),IkReal(((0.307)*sj2)),IKFAST_ATAN2_MAGTHRESH);
473if(!x55.valid){
474continue;
475}
476IkReal x53=((1.0)*(x55.value));
477if((((((0.094249)*(sj2*sj2)))+(x52*x52))) < -0.00001)
478continue;
479CheckValue<IkReal> x56=IKPowWithIntegerCheck(IKabs(IKsqrt(((((0.094249)*(sj2*sj2)))+(x52*x52)))),-1);
480if(!x56.valid){
481continue;
482}
483if( ((pz*(x56.value))) < -1-IKFAST_SINCOS_THRESH || ((pz*(x56.value))) > 1+IKFAST_SINCOS_THRESH )
484 continue;
485IkReal x54=IKasin((pz*(x56.value)));
486j1array[0]=(x54+(((-1.0)*x53)));
487sj1array[0]=IKsin(j1array[0]);
488cj1array[0]=IKcos(j1array[0]);
489j1array[1]=((3.14159265358979)+(((-1.0)*x53))+(((-1.0)*x54)));
490sj1array[1]=IKsin(j1array[1]);
491cj1array[1]=IKcos(j1array[1]);
492if( j1array[0] > IKPI )
493{
494 j1array[0]-=IK2PI;
495}
496else if( j1array[0] < -IKPI )
497{ j1array[0]+=IK2PI;
498}
499j1valid[0] = true;
500if( j1array[1] > IKPI )
501{
502 j1array[1]-=IK2PI;
503}
504else if( j1array[1] < -IKPI )
505{ j1array[1]+=IK2PI;
506}
507j1valid[1] = true;
508for(int ij1 = 0; ij1 < 2; ++ij1)
509{
510if( !j1valid[ij1] )
511{
512 continue;
513}
514_ij1[0] = ij1; _ij1[1] = -1;
515for(int iij1 = ij1+1; iij1 < 2; ++iij1)
516{
517if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
518{
519 j1valid[iij1]=false; _ij1[1] = iij1; break;
520}
521}
522j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
523
524// The remainder of this scope is removed as it does not return a solution.
525// This is the branch (IKabs(px))+(IKabs(py)), from the kinematics we know,
526// that j0 is free. So we simply choose 4 values and pass directly to the
527// rotationfunction (orientation computation is unchanged).
528_nj0=4;
529for(int ij0=1; ij0 < _nj0; ++ij0)
530{
531j0=(IkReal)(ij0 - 1) * (3.14159265358979) / 2.; // -pi/2, 0, pi/2, pi
532cj0=IKcos(j0);
533sj0=IKsin(j0);
534_ij0[0] = ij0;
535rotationfunction0(solutions);
536}
537
538}
539}
540
541}
542
543}
544
545} else
546{
547{
548IkReal j0array[2], cj0array[2], sj0array[2];
549bool j0valid[2]={false};
550_nj0 = 2;
551CheckValue<IkReal> x1691 = IKatan2WithCheck(IkReal(py),IkReal(((-1.0)*px)),IKFAST_ATAN2_MAGTHRESH);
552if(!x1691.valid){
553continue;
554}
555IkReal x1690=x1691.value;
556j0array[0]=((-1.0)*x1690);
557sj0array[0]=IKsin(j0array[0]);
558cj0array[0]=IKcos(j0array[0]);
559j0array[1]=((3.14159265358979)+(((-1.0)*x1690)));
560sj0array[1]=IKsin(j0array[1]);
561cj0array[1]=IKcos(j0array[1]);
562if( j0array[0] > IKPI )
563{
564 j0array[0]-=IK2PI;
565}
566else if( j0array[0] < -IKPI )
567{ j0array[0]+=IK2PI;
568}
569j0valid[0] = true;
570if( j0array[1] > IKPI )
571{
572 j0array[1]-=IK2PI;
573}
574else if( j0array[1] < -IKPI )
575{ j0array[1]+=IK2PI;
576}
577j0valid[1] = true;
578for(int ij0 = 0; ij0 < 2; ++ij0)
579{
580if( !j0valid[ij0] )
581{
582 continue;
583}
584_ij0[0] = ij0; _ij0[1] = -1;
585for(int iij0 = ij0+1; iij0 < 2; ++iij0)
586{
587if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
588{
589 j0valid[iij0]=false; _ij0[1] = iij0; break;
590}
591}
592j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
593
594{
595IkReal j1eval[3];
596IkReal x1692=((307000.0)*cj2);
597IkReal x1693=(cj0*px);
598IkReal x1694=((307000.0)*sj2);
599IkReal x1695=(py*sj0);
600j1eval[0]=((-1.00860400186133)+(((-1.0)*cj2)));
601j1eval[1]=((IKabs(((((-1.0)*pz*x1692))+(((-350000.0)*pz))+((x1693*x1694))+((x1694*x1695)))))+(IKabs(((((-1.0)*x1692*x1693))+(((-1.0)*x1692*x1695))+(((-1.0)*pz*x1694))+(((-350000.0)*x1693))+(((-350000.0)*x1695))))));
602j1eval[2]=IKsign(((-216749.0)+(((-214900.0)*cj2))));
603if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
604{
605{
606IkReal j1eval[3];
607IkReal x1696=(py*sj0);
608IkReal x1697=((1000.0)*pz);
609IkReal x1698=(pz*sj2);
610IkReal x1699=(cj0*px);
611IkReal x1700=(cj2*x1699);
612j1eval[0]=((((-1.0)*x1700))+x1698+(((-1.0)*cj2*x1696))+(((-1.1400651465798)*x1696))+(((-1.1400651465798)*x1699)));
613j1eval[1]=((IKabs(((((-1.0)*x1696*x1697))+(((-1.0)*x1697*x1699))+(((94.249)*cj2*sj2))+(((107.45)*sj2)))))+(IKabs(((-122.5)+(((-94.249)*(cj2*cj2)))+(((-214.9)*cj2))+((pz*x1697))))));
614j1eval[2]=IKsign(((((-307.0)*x1700))+(((-350.0)*x1699))+(((-350.0)*x1696))+(((-307.0)*cj2*x1696))+(((307.0)*x1698))));
615if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
616{
617{
618IkReal j1eval[3];
619IkReal x1701=(cj0*px);
620IkReal x1702=((1.0)*cj2);
621IkReal x1703=((7000.0)*pz);
622IkReal x1704=(py*sj0);
623IkReal x1705=((2149.0)*cj2);
624IkReal x1706=(pz*sj2);
625IkReal x1707=((3070.0)*pp);
626j1eval[0]=((((-1.1400651465798)*x1704))+(((-1.1400651465798)*x1701))+(((-1.0)*x1702*x1704))+x1706+(((-1.0)*x1701*x1702)));
627j1eval[1]=((IKabs(((-98.8785)+(((-86.73057)*cj2))+(((-1.0)*cj2*x1707))+(((-3500.0)*pp))+((pz*x1703)))))+(IKabs(((((-1.0)*x1703*x1704))+((sj2*x1707))+(((-1.0)*x1701*x1703))+(((86.73057)*sj2))))));
628j1eval[2]=IKsign(((((2149.0)*x1706))+(((-2450.0)*x1704))+(((-2450.0)*x1701))+(((-1.0)*x1704*x1705))+(((-1.0)*x1701*x1705))));
629if( IKabs(j1eval[0]) < 0.0000010000000000 || IKabs(j1eval[1]) < 0.0000010000000000 || IKabs(j1eval[2]) < 0.0000010000000000 )
630{
631continue; // no branches [j1]
632
633} else
634{
635{
636IkReal j1array[1], cj1array[1], sj1array[1];
637bool j1valid[1]={false};
638_nj1 = 1;
639IkReal x1708=((7000.0)*pz);
640IkReal x1709=(cj0*px);
641IkReal x1710=(py*sj0);
642IkReal x1711=((2149.0)*cj2);
643IkReal x1712=((3070.0)*pp);
644CheckValue<IkReal> x1713 = IKatan2WithCheck(IkReal(((-98.8785)+(((-86.73057)*cj2))+(((-3500.0)*pp))+(((-1.0)*cj2*x1712))+((pz*x1708)))),IkReal(((((-1.0)*x1708*x1709))+(((-1.0)*x1708*x1710))+((sj2*x1712))+(((86.73057)*sj2)))),IKFAST_ATAN2_MAGTHRESH);
645if(!x1713.valid){
646continue;
647}
648CheckValue<IkReal> x1714=IKPowWithIntegerCheck(IKsign(((((-1.0)*x1710*x1711))+(((-1.0)*x1709*x1711))+(((-2450.0)*x1709))+(((-2450.0)*x1710))+(((2149.0)*pz*sj2)))),-1);
649if(!x1714.valid){
650continue;
651}
652j1array[0]=((-1.5707963267949)+(x1713.value)+(((1.5707963267949)*(x1714.value))));
653sj1array[0]=IKsin(j1array[0]);
654cj1array[0]=IKcos(j1array[0]);
655if( j1array[0] > IKPI )
656{
657 j1array[0]-=IK2PI;
658}
659else if( j1array[0] < -IKPI )
660{ j1array[0]+=IK2PI;
661}
662j1valid[0] = true;
663for(int ij1 = 0; ij1 < 1; ++ij1)
664{
665if( !j1valid[ij1] )
666{
667 continue;
668}
669_ij1[0] = ij1; _ij1[1] = -1;
670for(int iij1 = ij1+1; iij1 < 1; ++iij1)
671{
672if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
673{
674 j1valid[iij1]=false; _ij1[1] = iij1; break;
675}
676}
677j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
678{
679IkReal evalcond[5];
680IkReal x1715=IKcos(j1);
681IkReal x1716=IKsin(j1);
682IkReal x1717=(cj0*px);
683IkReal x1718=(py*sj0);
684IkReal x1719=((0.307)*x1715);
685IkReal x1720=((1.0)*x1718);
686IkReal x1721=(pz*x1715);
687IkReal x1722=((0.7)*x1716);
688IkReal x1723=((0.307)*x1716);
689evalcond[0]=((((-1.0)*pz))+((cj2*x1719))+(((0.35)*x1715))+((sj2*x1723)));
690evalcond[1]=((((-1.0)*x1715*x1720))+(((-1.0)*x1715*x1717))+(((-0.307)*sj2))+((pz*x1716)));
691evalcond[2]=((((-0.35)*x1716))+x1717+x1718+(((-1.0)*cj2*x1723))+((sj2*x1719)));
692evalcond[3]=((-0.028251)+((x1718*x1722))+((x1717*x1722))+(((-1.0)*pp))+(((0.7)*x1721)));
693evalcond[4]=((0.35)+(((-1.0)*x1716*x1717))+(((-1.0)*x1721))+(((-1.0)*x1716*x1720))+(((0.307)*cj2)));
694if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
695{
696continue;
697}
698}
699
700rotationfunction0(solutions);
701}
702}
703
704}
705
706}
707
708} else
709{
710{
711IkReal j1array[1], cj1array[1], sj1array[1];
712bool j1valid[1]={false};
713_nj1 = 1;
714IkReal x1724=((307.0)*cj2);
715IkReal x1725=(cj0*px);
716IkReal x1726=(py*sj0);
717IkReal x1727=((1000.0)*pz);
718CheckValue<IkReal> x1728 = IKatan2WithCheck(IkReal(((-122.5)+(((-94.249)*(cj2*cj2)))+((pz*x1727))+(((-214.9)*cj2)))),IkReal(((((-1.0)*x1726*x1727))+(((94.249)*cj2*sj2))+(((107.45)*sj2))+(((-1.0)*x1725*x1727)))),IKFAST_ATAN2_MAGTHRESH);
719if(!x1728.valid){
720continue;
721}
722CheckValue<IkReal> x1729=IKPowWithIntegerCheck(IKsign(((((-350.0)*x1726))+(((-350.0)*x1725))+(((307.0)*pz*sj2))+(((-1.0)*x1724*x1725))+(((-1.0)*x1724*x1726)))),-1);
723if(!x1729.valid){
724continue;
725}
726j1array[0]=((-1.5707963267949)+(x1728.value)+(((1.5707963267949)*(x1729.value))));
727sj1array[0]=IKsin(j1array[0]);
728cj1array[0]=IKcos(j1array[0]);
729if( j1array[0] > IKPI )
730{
731 j1array[0]-=IK2PI;
732}
733else if( j1array[0] < -IKPI )
734{ j1array[0]+=IK2PI;
735}
736j1valid[0] = true;
737for(int ij1 = 0; ij1 < 1; ++ij1)
738{
739if( !j1valid[ij1] )
740{
741 continue;
742}
743_ij1[0] = ij1; _ij1[1] = -1;
744for(int iij1 = ij1+1; iij1 < 1; ++iij1)
745{
746if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
747{
748 j1valid[iij1]=false; _ij1[1] = iij1; break;
749}
750}
751j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
752{
753IkReal evalcond[5];
754IkReal x1730=IKcos(j1);
755IkReal x1731=IKsin(j1);
756IkReal x1732=(cj0*px);
757IkReal x1733=(py*sj0);
758IkReal x1734=((0.307)*x1730);
759IkReal x1735=((1.0)*x1733);
760IkReal x1736=(pz*x1730);
761IkReal x1737=((0.7)*x1731);
762IkReal x1738=((0.307)*x1731);
763evalcond[0]=(((cj2*x1734))+(((0.35)*x1730))+(((-1.0)*pz))+((sj2*x1738)));
764evalcond[1]=(((pz*x1731))+(((-0.307)*sj2))+(((-1.0)*x1730*x1735))+(((-1.0)*x1730*x1732)));
765evalcond[2]=(x1733+x1732+(((-1.0)*cj2*x1738))+(((-0.35)*x1731))+((sj2*x1734)));
766evalcond[3]=((-0.028251)+(((-1.0)*pp))+(((0.7)*x1736))+((x1732*x1737))+((x1733*x1737)));
767evalcond[4]=((0.35)+(((-1.0)*x1731*x1735))+(((-1.0)*x1731*x1732))+(((-1.0)*x1736))+(((0.307)*cj2)));
768if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
769{
770continue;
771}
772}
773
774rotationfunction0(solutions);
775}
776}
777
778}
779
780}
781
782} else
783{
784{
785IkReal j1array[1], cj1array[1], sj1array[1];
786bool j1valid[1]={false};
787_nj1 = 1;
788IkReal x1739=((307000.0)*cj2);
789IkReal x1740=(cj0*px);
790IkReal x1741=((307000.0)*sj2);
791IkReal x1742=(py*sj0);
792CheckValue<IkReal> x1743=IKPowWithIntegerCheck(IKsign(((-216749.0)+(((-214900.0)*cj2)))),-1);
793if(!x1743.valid){
794continue;
795}
796CheckValue<IkReal> x1744 = IKatan2WithCheck(IkReal(((((-350000.0)*x1742))+(((-350000.0)*x1740))+(((-1.0)*x1739*x1740))+(((-1.0)*x1739*x1742))+(((-1.0)*pz*x1741)))),IkReal(((((-1.0)*pz*x1739))+(((-350000.0)*pz))+((x1740*x1741))+((x1741*x1742)))),IKFAST_ATAN2_MAGTHRESH);
797if(!x1744.valid){
798continue;
799}
800j1array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1743.value)))+(x1744.value));
801sj1array[0]=IKsin(j1array[0]);
802cj1array[0]=IKcos(j1array[0]);
803if( j1array[0] > IKPI )
804{
805 j1array[0]-=IK2PI;
806}
807else if( j1array[0] < -IKPI )
808{ j1array[0]+=IK2PI;
809}
810j1valid[0] = true;
811for(int ij1 = 0; ij1 < 1; ++ij1)
812{
813if( !j1valid[ij1] )
814{
815 continue;
816}
817_ij1[0] = ij1; _ij1[1] = -1;
818for(int iij1 = ij1+1; iij1 < 1; ++iij1)
819{
820if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
821{
822 j1valid[iij1]=false; _ij1[1] = iij1; break;
823}
824}
825j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
826{
827IkReal evalcond[5];
828IkReal x1745=IKcos(j1);
829IkReal x1746=IKsin(j1);
830IkReal x1747=(cj0*px);
831IkReal x1748=(py*sj0);
832IkReal x1749=((0.307)*x1745);
833IkReal x1750=((1.0)*x1748);
834IkReal x1751=(pz*x1745);
835IkReal x1752=((0.7)*x1746);
836IkReal x1753=((0.307)*x1746);
837evalcond[0]=((((0.35)*x1745))+((cj2*x1749))+(((-1.0)*pz))+((sj2*x1753)));
838evalcond[1]=(((pz*x1746))+(((-0.307)*sj2))+(((-1.0)*x1745*x1750))+(((-1.0)*x1745*x1747)));
839evalcond[2]=(x1748+x1747+(((-1.0)*cj2*x1753))+((sj2*x1749))+(((-0.35)*x1746)));
840evalcond[3]=((-0.028251)+((x1747*x1752))+(((-1.0)*pp))+((x1748*x1752))+(((0.7)*x1751)));
841evalcond[4]=((0.35)+(((-1.0)*x1746*x1747))+(((-1.0)*x1746*x1750))+(((-1.0)*x1751))+(((0.307)*cj2)));
842if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
843{
844continue;
845}
846}
847
848rotationfunction0(solutions);
849}
850}
851
852}
853
854}
855}
856}
857
858}
859
860}
861}
862}
863}
864return solutions.GetNumSolutions()>0;
865}
866inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
867for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
868IkReal x70=(r11*sj0);
869IkReal x71=(cj0*r02);
870IkReal x72=(r10*sj0);
871IkReal x73=((1.0)*sj0);
872IkReal x74=(cj2*sj1);
873IkReal x75=(cj1*sj2);
874IkReal x76=(r12*sj0);
875IkReal x77=(((sj1*sj2))+((cj1*cj2)));
876IkReal x78=(x75+(((-1.0)*x74)));
877IkReal x79=(x74+(((-1.0)*x75)));
878IkReal x80=(sj0*x79);
879IkReal x81=(cj0*x79);
880IkReal x82=(cj0*x77);
881new_r00=(((r00*x82))+((x72*x77))+((r20*x78)));
882new_r01=(((r21*x78))+((r01*x82))+((x70*x77)));
883new_r02=(((r22*x78))+((x76*x77))+((x71*x77)));
884new_r10=((((-1.0)*r00*x73))+((cj0*r10)));
885new_r11=((((-1.0)*r01*x73))+((cj0*r11)));
886new_r12=((((-1.0)*r02*x73))+((cj0*r12)));
887new_r20=(((r00*x81))+((x72*x79))+((r20*x77)));
888new_r21=(((r21*x77))+((r01*x81))+((x70*x79)));
889new_r22=(((r22*x77))+((x76*x79))+((x71*x79)));
890{
891IkReal j4array[2], cj4array[2], sj4array[2];
892bool j4valid[2]={false};
893_nj4 = 2;
894cj4array[0]=new_r22;
895if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
896{
897 j4valid[0] = j4valid[1] = true;
898 j4array[0] = IKacos(cj4array[0]);
899 sj4array[0] = IKsin(j4array[0]);
900 cj4array[1] = cj4array[0];
901 j4array[1] = -j4array[0];
902 sj4array[1] = -sj4array[0];
903}
904else if( isnan(cj4array[0]) )
905{
906 // probably any value will work
907 j4valid[0] = true;
908 cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
909}
910for(int ij4 = 0; ij4 < 2; ++ij4)
911{
912if( !j4valid[ij4] )
913{
914 continue;
915}
916_ij4[0] = ij4; _ij4[1] = -1;
917for(int iij4 = ij4+1; iij4 < 2; ++iij4)
918{
919if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
920{
921 j4valid[iij4]=false; _ij4[1] = iij4; break;
922}
923}
924j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
925
926{
927IkReal j5eval[3];
928j5eval[0]=sj4;
929j5eval[1]=IKsign(sj4);
930j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
931if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
932{
933{
934IkReal j3eval[3];
935j3eval[0]=sj4;
936j3eval[1]=IKsign(sj4);
937j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
938if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
939{
940{
941IkReal j3eval[2];
942j3eval[0]=new_r12;
943j3eval[1]=sj4;
944if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
945{
946{
947IkReal evalcond[5];
948bool bgotonextstatement = true;
949do
950{
951evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
952evalcond[1]=new_r02;
953evalcond[2]=new_r12;
954evalcond[3]=new_r21;
955evalcond[4]=new_r20;
956if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
957{
958bgotonextstatement=false;
959IkReal j5mul = 1;
960j5=0;
961j3mul=-1.0;
962if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH )
963 continue;
964j3=IKatan2(((-1.0)*new_r01), new_r00);
965{
966std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
967vinfos[0].jointtype = 1;
968vinfos[0].foffset = j0;
969vinfos[0].indices[0] = _ij0[0];
970vinfos[0].indices[1] = _ij0[1];
971vinfos[0].maxsolutions = _nj0;
972vinfos[1].jointtype = 1;
973vinfos[1].foffset = j1;
974vinfos[1].indices[0] = _ij1[0];
975vinfos[1].indices[1] = _ij1[1];
976vinfos[1].maxsolutions = _nj1;
977vinfos[2].jointtype = 1;
978vinfos[2].foffset = j2;
979vinfos[2].indices[0] = _ij2[0];
980vinfos[2].indices[1] = _ij2[1];
981vinfos[2].maxsolutions = _nj2;
982vinfos[3].jointtype = 1;
983vinfos[3].foffset = j3;
984vinfos[3].fmul = j3mul;
985vinfos[3].freeind = 0;
986vinfos[3].maxsolutions = 0;
987vinfos[4].jointtype = 1;
988vinfos[4].foffset = j4;
989vinfos[4].indices[0] = _ij4[0];
990vinfos[4].indices[1] = _ij4[1];
991vinfos[4].maxsolutions = _nj4;
992vinfos[5].jointtype = 1;
993vinfos[5].foffset = j5;
994vinfos[5].fmul = j5mul;
995vinfos[5].freeind = 0;
996vinfos[5].maxsolutions = 0;
997std::vector<int> vfree(1);
998vfree[0] = 5;
999solutions.AddSolution(vinfos,vfree);
1000}
1001
1002}
1003} while(0);
1004if( bgotonextstatement )
1005{
1006bool bgotonextstatement = true;
1007do
1008{
1009evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
1010evalcond[1]=new_r02;
1011evalcond[2]=new_r12;
1012evalcond[3]=new_r21;
1013evalcond[4]=new_r20;
1014if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
1015{
1016bgotonextstatement=false;
1017IkReal j5mul = 1;
1018j5=0;
1019j3mul=1.0;
1020if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
1021 continue;
1022j3=IKatan2(((-1.0)*new_r01), ((-1.0)*new_r00));
1023{
1024std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1025vinfos[0].jointtype = 1;
1026vinfos[0].foffset = j0;
1027vinfos[0].indices[0] = _ij0[0];
1028vinfos[0].indices[1] = _ij0[1];
1029vinfos[0].maxsolutions = _nj0;
1030vinfos[1].jointtype = 1;
1031vinfos[1].foffset = j1;
1032vinfos[1].indices[0] = _ij1[0];
1033vinfos[1].indices[1] = _ij1[1];
1034vinfos[1].maxsolutions = _nj1;
1035vinfos[2].jointtype = 1;
1036vinfos[2].foffset = j2;
1037vinfos[2].indices[0] = _ij2[0];
1038vinfos[2].indices[1] = _ij2[1];
1039vinfos[2].maxsolutions = _nj2;
1040vinfos[3].jointtype = 1;
1041vinfos[3].foffset = j3;
1042vinfos[3].fmul = j3mul;
1043vinfos[3].freeind = 0;
1044vinfos[3].maxsolutions = 0;
1045vinfos[4].jointtype = 1;
1046vinfos[4].foffset = j4;
1047vinfos[4].indices[0] = _ij4[0];
1048vinfos[4].indices[1] = _ij4[1];
1049vinfos[4].maxsolutions = _nj4;
1050vinfos[5].jointtype = 1;
1051vinfos[5].foffset = j5;
1052vinfos[5].fmul = j5mul;
1053vinfos[5].freeind = 0;
1054vinfos[5].maxsolutions = 0;
1055std::vector<int> vfree(1);
1056vfree[0] = 5;
1057solutions.AddSolution(vinfos,vfree);
1058}
1059
1060}
1061} while(0);
1062if( bgotonextstatement )
1063{
1064bool bgotonextstatement = true;
1065do
1066{
1067evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
1068if( IKabs(evalcond[0]) < 0.0000050000000000 )
1069{
1070bgotonextstatement=false;
1071{
1072IkReal j3eval[1];
1073new_r21=0;
1074new_r20=0;
1075new_r02=0;
1076new_r12=0;
1077IkReal x83=new_r22*new_r22;
1078IkReal x84=((16.0)*new_r10);
1079IkReal x85=((16.0)*new_r01);
1080IkReal x86=((16.0)*new_r22);
1081IkReal x87=((8.0)*new_r11);
1082IkReal x88=((8.0)*new_r00);
1083IkReal x89=(x83*x84);
1084IkReal x90=(x83*x85);
1085j3eval[0]=((IKabs((x89+(((-1.0)*x84)))))+(IKabs((((new_r22*x88))+(((-1.0)*x83*x87)))))+(IKabs(((((16.0)*new_r00))+((new_r11*x86))+(((-32.0)*new_r00*x83)))))+(IKabs((x85+(((-1.0)*x90)))))+(IKabs((((new_r22*x87))+(((-1.0)*x88)))))+(IKabs(((((16.0)*new_r11*x83))+((new_r00*x86))+(((-32.0)*new_r11)))))+(IKabs((x90+(((-1.0)*x85)))))+(IKabs((x84+(((-1.0)*x89))))));
1086if( IKabs(j3eval[0]) < 0.0000000100000000 )
1087{
1088continue; // no branches [j3, j5]
1089
1090} else
1091{
1092IkReal op[4+1], zeror[4];
1093int numroots;
1094IkReal j3evalpoly[1];
1095IkReal x91=new_r22*new_r22;
1096IkReal x92=((16.0)*new_r10);
1097IkReal x93=(new_r11*new_r22);
1098IkReal x94=(x91*x92);
1099IkReal x95=((((8.0)*x93))+(((-8.0)*new_r00)));
1100op[0]=x95;
1101op[1]=(x92+(((-1.0)*x94)));
1102op[2]=((((16.0)*x93))+(((16.0)*new_r00))+(((-32.0)*new_r00*x91)));
1103op[3]=(x94+(((-1.0)*x92)));
1104op[4]=x95;
1105polyroots4(op,zeror,numroots);
1106IkReal j3array[4], cj3array[4], sj3array[4], tempj3array[1];
1107int numsolutions = 0;
1108for(int ij3 = 0; ij3 < numroots; ++ij3)
1109{
1110IkReal htj3 = zeror[ij3];
1111tempj3array[0]=((2.0)*(atan(htj3)));
1112for(int kj3 = 0; kj3 < 1; ++kj3)
1113{
1114j3array[numsolutions] = tempj3array[kj3];
1115if( j3array[numsolutions] > IKPI )
1116{
1117 j3array[numsolutions]-=IK2PI;
1118}
1119else if( j3array[numsolutions] < -IKPI )
1120{
1121 j3array[numsolutions]+=IK2PI;
1122}
1123sj3array[numsolutions] = IKsin(j3array[numsolutions]);
1124cj3array[numsolutions] = IKcos(j3array[numsolutions]);
1125numsolutions++;
1126}
1127}
1128bool j3valid[4]={true,true,true,true};
1129_nj3 = 4;
1130for(int ij3 = 0; ij3 < numsolutions; ++ij3)
1131 {
1132if( !j3valid[ij3] )
1133{
1134 continue;
1135}
1136 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1137htj3 = IKtan(j3/2);
1138
1139IkReal x96=((16.0)*new_r01);
1140IkReal x97=new_r22*new_r22;
1141IkReal x98=(new_r00*new_r22);
1142IkReal x99=((8.0)*x98);
1143IkReal x100=(new_r11*x97);
1144IkReal x101=(x96*x97);
1145IkReal x102=((8.0)*x100);
1146j3evalpoly[0]=((((-1.0)*x102))+(((htj3*htj3*htj3)*(((((-1.0)*x101))+x96))))+(((htj3*htj3*htj3*htj3)*(((((-1.0)*x102))+x99))))+((htj3*((x101+(((-1.0)*x96))))))+(((htj3*htj3)*(((((16.0)*x98))+(((16.0)*x100))+(((-32.0)*new_r11))))))+x99);
1147if( IKabs(j3evalpoly[0]) > 0.0000001000000000 )
1148{
1149 continue;
1150}
1151_ij3[0] = ij3; _ij3[1] = -1;
1152for(int iij3 = ij3+1; iij3 < numsolutions; ++iij3)
1153{
1154if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1155{
1156 j3valid[iij3]=false; _ij3[1] = iij3; break;
1157}
1158}
1159{
1160IkReal j5eval[3];
1161new_r21=0;
1162new_r20=0;
1163new_r02=0;
1164new_r12=0;
1165IkReal x103=cj3*cj3;
1166IkReal x104=(cj3*new_r22);
1167IkReal x105=((-1.0)+x103+(((-1.0)*x103*(new_r22*new_r22))));
1168j5eval[0]=x105;
1169j5eval[1]=((IKabs((((new_r01*sj3))+(((-1.0)*new_r00*x104)))))+(IKabs((((new_r00*sj3))+((new_r01*x104))))));
1170j5eval[2]=IKsign(x105);
1171if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
1172{
1173{
1174IkReal j5eval[1];
1175new_r21=0;
1176new_r20=0;
1177new_r02=0;
1178new_r12=0;
1179j5eval[0]=new_r22;
1180if( IKabs(j5eval[0]) < 0.0000010000000000 )
1181{
1182{
1183IkReal j5eval[1];
1184new_r21=0;
1185new_r20=0;
1186new_r02=0;
1187new_r12=0;
1188j5eval[0]=sj3;
1189if( IKabs(j5eval[0]) < 0.0000010000000000 )
1190{
1191{
1192IkReal evalcond[1];
1193bool bgotonextstatement = true;
1194do
1195{
1196evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
1197if( IKabs(evalcond[0]) < 0.0000050000000000 )
1198{
1199bgotonextstatement=false;
1200{
1201IkReal j5array[1], cj5array[1], sj5array[1];
1202bool j5valid[1]={false};
1203_nj5 = 1;
1204if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
1205 continue;
1206j5array[0]=IKatan2(new_r10, new_r11);
1207sj5array[0]=IKsin(j5array[0]);
1208cj5array[0]=IKcos(j5array[0]);
1209if( j5array[0] > IKPI )
1210{
1211 j5array[0]-=IK2PI;
1212}
1213else if( j5array[0] < -IKPI )
1214{ j5array[0]+=IK2PI;
1215}
1216j5valid[0] = true;
1217for(int ij5 = 0; ij5 < 1; ++ij5)
1218{
1219if( !j5valid[ij5] )
1220{
1221 continue;
1222}
1223_ij5[0] = ij5; _ij5[1] = -1;
1224for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1225{
1226if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1227{
1228 j5valid[iij5]=false; _ij5[1] = iij5; break;
1229}
1230}
1231j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1232{
1233IkReal evalcond[4];
1234IkReal x106=IKsin(j5);
1235IkReal x107=IKcos(j5);
1236evalcond[0]=x107;
1237evalcond[1]=((-1.0)*x106);
1238evalcond[2]=(x106+(((-1.0)*new_r10)));
1239evalcond[3]=(x107+(((-1.0)*new_r11)));
1240if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1241{
1242continue;
1243}
1244}
1245
1246{
1247std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1248vinfos[0].jointtype = 1;
1249vinfos[0].foffset = j0;
1250vinfos[0].indices[0] = _ij0[0];
1251vinfos[0].indices[1] = _ij0[1];
1252vinfos[0].maxsolutions = _nj0;
1253vinfos[1].jointtype = 1;
1254vinfos[1].foffset = j1;
1255vinfos[1].indices[0] = _ij1[0];
1256vinfos[1].indices[1] = _ij1[1];
1257vinfos[1].maxsolutions = _nj1;
1258vinfos[2].jointtype = 1;
1259vinfos[2].foffset = j2;
1260vinfos[2].indices[0] = _ij2[0];
1261vinfos[2].indices[1] = _ij2[1];
1262vinfos[2].maxsolutions = _nj2;
1263vinfos[3].jointtype = 1;
1264vinfos[3].foffset = j3;
1265vinfos[3].indices[0] = _ij3[0];
1266vinfos[3].indices[1] = _ij3[1];
1267vinfos[3].maxsolutions = _nj3;
1268vinfos[4].jointtype = 1;
1269vinfos[4].foffset = j4;
1270vinfos[4].indices[0] = _ij4[0];
1271vinfos[4].indices[1] = _ij4[1];
1272vinfos[4].maxsolutions = _nj4;
1273vinfos[5].jointtype = 1;
1274vinfos[5].foffset = j5;
1275vinfos[5].indices[0] = _ij5[0];
1276vinfos[5].indices[1] = _ij5[1];
1277vinfos[5].maxsolutions = _nj5;
1278std::vector<int> vfree(0);
1279solutions.AddSolution(vinfos,vfree);
1280}
1281}
1282}
1283
1284}
1285} while(0);
1286if( bgotonextstatement )
1287{
1288bool bgotonextstatement = true;
1289do
1290{
1291evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
1292if( IKabs(evalcond[0]) < 0.0000050000000000 )
1293{
1294bgotonextstatement=false;
1295{
1296IkReal j5array[1], cj5array[1], sj5array[1];
1297bool j5valid[1]={false};
1298_nj5 = 1;
1299if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
1300 continue;
1301j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
1302sj5array[0]=IKsin(j5array[0]);
1303cj5array[0]=IKcos(j5array[0]);
1304if( j5array[0] > IKPI )
1305{
1306 j5array[0]-=IK2PI;
1307}
1308else if( j5array[0] < -IKPI )
1309{ j5array[0]+=IK2PI;
1310}
1311j5valid[0] = true;
1312for(int ij5 = 0; ij5 < 1; ++ij5)
1313{
1314if( !j5valid[ij5] )
1315{
1316 continue;
1317}
1318_ij5[0] = ij5; _ij5[1] = -1;
1319for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1320{
1321if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1322{
1323 j5valid[iij5]=false; _ij5[1] = iij5; break;
1324}
1325}
1326j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1327{
1328IkReal evalcond[4];
1329IkReal x108=IKcos(j5);
1330IkReal x109=IKsin(j5);
1331evalcond[0]=x108;
1332evalcond[1]=(x109+new_r10);
1333evalcond[2]=(x108+new_r11);
1334evalcond[3]=((-1.0)*x109);
1335if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH )
1336{
1337continue;
1338}
1339}
1340
1341{
1342std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1343vinfos[0].jointtype = 1;
1344vinfos[0].foffset = j0;
1345vinfos[0].indices[0] = _ij0[0];
1346vinfos[0].indices[1] = _ij0[1];
1347vinfos[0].maxsolutions = _nj0;
1348vinfos[1].jointtype = 1;
1349vinfos[1].foffset = j1;
1350vinfos[1].indices[0] = _ij1[0];
1351vinfos[1].indices[1] = _ij1[1];
1352vinfos[1].maxsolutions = _nj1;
1353vinfos[2].jointtype = 1;
1354vinfos[2].foffset = j2;
1355vinfos[2].indices[0] = _ij2[0];
1356vinfos[2].indices[1] = _ij2[1];
1357vinfos[2].maxsolutions = _nj2;
1358vinfos[3].jointtype = 1;
1359vinfos[3].foffset = j3;
1360vinfos[3].indices[0] = _ij3[0];
1361vinfos[3].indices[1] = _ij3[1];
1362vinfos[3].maxsolutions = _nj3;
1363vinfos[4].jointtype = 1;
1364vinfos[4].foffset = j4;
1365vinfos[4].indices[0] = _ij4[0];
1366vinfos[4].indices[1] = _ij4[1];
1367vinfos[4].maxsolutions = _nj4;
1368vinfos[5].jointtype = 1;
1369vinfos[5].foffset = j5;
1370vinfos[5].indices[0] = _ij5[0];
1371vinfos[5].indices[1] = _ij5[1];
1372vinfos[5].maxsolutions = _nj5;
1373std::vector<int> vfree(0);
1374solutions.AddSolution(vinfos,vfree);
1375}
1376}
1377}
1378
1379}
1380} while(0);
1381if( bgotonextstatement )
1382{
1383bool bgotonextstatement = true;
1384do
1385{
1386CheckValue<IkReal> x110=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1387if(!x110.valid){
1388continue;
1389}
1390if((x110.value) < -0.00001)
1391continue;
1392IkReal gconst18=((-1.0)*(IKsqrt(x110.value)));
1393evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst18)))))), 6.28318530717959)));
1394if( IKabs(evalcond[0]) < 0.0000050000000000 )
1395{
1396bgotonextstatement=false;
1397{
1398IkReal j5eval[1];
1399new_r21=0;
1400new_r20=0;
1401new_r02=0;
1402new_r12=0;
1403if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1404continue;
1405sj3=IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))));
1406cj3=gconst18;
1407if( (gconst18) < -1-IKFAST_SINCOS_THRESH || (gconst18) > 1+IKFAST_SINCOS_THRESH )
1408 continue;
1409j3=IKacos(gconst18);
1410CheckValue<IkReal> x111=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1411if(!x111.valid){
1412continue;
1413}
1414if((x111.value) < -0.00001)
1415continue;
1416IkReal gconst18=((-1.0)*(IKsqrt(x111.value)));
1417j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1418if( IKabs(j5eval[0]) < 0.0000010000000000 )
1419{
1420{
1421IkReal j5array[1], cj5array[1], sj5array[1];
1422bool j5valid[1]={false};
1423_nj5 = 1;
1424if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1425continue;
1426CheckValue<IkReal> x112=IKPowWithIntegerCheck(gconst18,-1);
1427if(!x112.valid){
1428continue;
1429}
1430if( IKabs((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x112.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))))+IKsqr((new_r11*(x112.value)))-1) <= IKFAST_SINCOS_THRESH )
1431 continue;
1432j5array[0]=IKatan2((((gconst18*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))), (new_r11*(x112.value)));
1433sj5array[0]=IKsin(j5array[0]);
1434cj5array[0]=IKcos(j5array[0]);
1435if( j5array[0] > IKPI )
1436{
1437 j5array[0]-=IK2PI;
1438}
1439else if( j5array[0] < -IKPI )
1440{ j5array[0]+=IK2PI;
1441}
1442j5valid[0] = true;
1443for(int ij5 = 0; ij5 < 1; ++ij5)
1444{
1445if( !j5valid[ij5] )
1446{
1447 continue;
1448}
1449_ij5[0] = ij5; _ij5[1] = -1;
1450for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1451{
1452if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1453{
1454 j5valid[iij5]=false; _ij5[1] = iij5; break;
1455}
1456}
1457j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1458{
1459IkReal evalcond[8];
1460IkReal x113=IKcos(j5);
1461IkReal x114=IKsin(j5);
1462IkReal x115=((1.0)*gconst18);
1463if((((1.0)+(((-1.0)*gconst18*x115)))) < -0.00001)
1464continue;
1465IkReal x116=IKsqrt(((1.0)+(((-1.0)*gconst18*x115))));
1466evalcond[0]=x113;
1467evalcond[1]=((-1.0)*x114);
1468evalcond[2]=((((-1.0)*x113*x115))+new_r11);
1469evalcond[3]=((((-1.0)*x114*x115))+new_r10);
1470evalcond[4]=(((x113*x116))+new_r01);
1471evalcond[5]=(((x114*x116))+new_r00);
1472evalcond[6]=((((-1.0)*new_r10*x115))+x114+((new_r00*x116)));
1473evalcond[7]=((((-1.0)*new_r11*x115))+x113+((new_r01*x116)));
1474if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1475{
1476continue;
1477}
1478}
1479
1480{
1481std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1482vinfos[0].jointtype = 1;
1483vinfos[0].foffset = j0;
1484vinfos[0].indices[0] = _ij0[0];
1485vinfos[0].indices[1] = _ij0[1];
1486vinfos[0].maxsolutions = _nj0;
1487vinfos[1].jointtype = 1;
1488vinfos[1].foffset = j1;
1489vinfos[1].indices[0] = _ij1[0];
1490vinfos[1].indices[1] = _ij1[1];
1491vinfos[1].maxsolutions = _nj1;
1492vinfos[2].jointtype = 1;
1493vinfos[2].foffset = j2;
1494vinfos[2].indices[0] = _ij2[0];
1495vinfos[2].indices[1] = _ij2[1];
1496vinfos[2].maxsolutions = _nj2;
1497vinfos[3].jointtype = 1;
1498vinfos[3].foffset = j3;
1499vinfos[3].indices[0] = _ij3[0];
1500vinfos[3].indices[1] = _ij3[1];
1501vinfos[3].maxsolutions = _nj3;
1502vinfos[4].jointtype = 1;
1503vinfos[4].foffset = j4;
1504vinfos[4].indices[0] = _ij4[0];
1505vinfos[4].indices[1] = _ij4[1];
1506vinfos[4].maxsolutions = _nj4;
1507vinfos[5].jointtype = 1;
1508vinfos[5].foffset = j5;
1509vinfos[5].indices[0] = _ij5[0];
1510vinfos[5].indices[1] = _ij5[1];
1511vinfos[5].maxsolutions = _nj5;
1512std::vector<int> vfree(0);
1513solutions.AddSolution(vinfos,vfree);
1514}
1515}
1516}
1517
1518} else
1519{
1520{
1521IkReal j5array[1], cj5array[1], sj5array[1];
1522bool j5valid[1]={false};
1523_nj5 = 1;
1524CheckValue<IkReal> x117=IKPowWithIntegerCheck(IKsign(gconst18),-1);
1525if(!x117.valid){
1526continue;
1527}
1528CheckValue<IkReal> x118 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1529if(!x118.valid){
1530continue;
1531}
1532j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x117.value)))+(x118.value));
1533sj5array[0]=IKsin(j5array[0]);
1534cj5array[0]=IKcos(j5array[0]);
1535if( j5array[0] > IKPI )
1536{
1537 j5array[0]-=IK2PI;
1538}
1539else if( j5array[0] < -IKPI )
1540{ j5array[0]+=IK2PI;
1541}
1542j5valid[0] = true;
1543for(int ij5 = 0; ij5 < 1; ++ij5)
1544{
1545if( !j5valid[ij5] )
1546{
1547 continue;
1548}
1549_ij5[0] = ij5; _ij5[1] = -1;
1550for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1551{
1552if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1553{
1554 j5valid[iij5]=false; _ij5[1] = iij5; break;
1555}
1556}
1557j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1558{
1559IkReal evalcond[8];
1560IkReal x119=IKcos(j5);
1561IkReal x120=IKsin(j5);
1562IkReal x121=((1.0)*gconst18);
1563if((((1.0)+(((-1.0)*gconst18*x121)))) < -0.00001)
1564continue;
1565IkReal x122=IKsqrt(((1.0)+(((-1.0)*gconst18*x121))));
1566evalcond[0]=x119;
1567evalcond[1]=((-1.0)*x120);
1568evalcond[2]=((((-1.0)*x119*x121))+new_r11);
1569evalcond[3]=((((-1.0)*x120*x121))+new_r10);
1570evalcond[4]=(new_r01+((x119*x122)));
1571evalcond[5]=(((x120*x122))+new_r00);
1572evalcond[6]=((((-1.0)*new_r10*x121))+((new_r00*x122))+x120);
1573evalcond[7]=(((new_r01*x122))+x119+(((-1.0)*new_r11*x121)));
1574if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1575{
1576continue;
1577}
1578}
1579
1580{
1581std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1582vinfos[0].jointtype = 1;
1583vinfos[0].foffset = j0;
1584vinfos[0].indices[0] = _ij0[0];
1585vinfos[0].indices[1] = _ij0[1];
1586vinfos[0].maxsolutions = _nj0;
1587vinfos[1].jointtype = 1;
1588vinfos[1].foffset = j1;
1589vinfos[1].indices[0] = _ij1[0];
1590vinfos[1].indices[1] = _ij1[1];
1591vinfos[1].maxsolutions = _nj1;
1592vinfos[2].jointtype = 1;
1593vinfos[2].foffset = j2;
1594vinfos[2].indices[0] = _ij2[0];
1595vinfos[2].indices[1] = _ij2[1];
1596vinfos[2].maxsolutions = _nj2;
1597vinfos[3].jointtype = 1;
1598vinfos[3].foffset = j3;
1599vinfos[3].indices[0] = _ij3[0];
1600vinfos[3].indices[1] = _ij3[1];
1601vinfos[3].maxsolutions = _nj3;
1602vinfos[4].jointtype = 1;
1603vinfos[4].foffset = j4;
1604vinfos[4].indices[0] = _ij4[0];
1605vinfos[4].indices[1] = _ij4[1];
1606vinfos[4].maxsolutions = _nj4;
1607vinfos[5].jointtype = 1;
1608vinfos[5].foffset = j5;
1609vinfos[5].indices[0] = _ij5[0];
1610vinfos[5].indices[1] = _ij5[1];
1611vinfos[5].maxsolutions = _nj5;
1612std::vector<int> vfree(0);
1613solutions.AddSolution(vinfos,vfree);
1614}
1615}
1616}
1617
1618}
1619
1620}
1621
1622}
1623} while(0);
1624if( bgotonextstatement )
1625{
1626bool bgotonextstatement = true;
1627do
1628{
1629CheckValue<IkReal> x123=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1630if(!x123.valid){
1631continue;
1632}
1633if((x123.value) < -0.00001)
1634continue;
1635IkReal gconst18=((-1.0)*(IKsqrt(x123.value)));
1636evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst18)))))), 6.28318530717959)));
1637if( IKabs(evalcond[0]) < 0.0000050000000000 )
1638{
1639bgotonextstatement=false;
1640{
1641IkReal j5eval[1];
1642new_r21=0;
1643new_r20=0;
1644new_r02=0;
1645new_r12=0;
1646if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1647continue;
1648sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))));
1649cj3=gconst18;
1650if( (gconst18) < -1-IKFAST_SINCOS_THRESH || (gconst18) > 1+IKFAST_SINCOS_THRESH )
1651 continue;
1652j3=((-1.0)*(IKacos(gconst18)));
1653CheckValue<IkReal> x124=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1654if(!x124.valid){
1655continue;
1656}
1657if((x124.value) < -0.00001)
1658continue;
1659IkReal gconst18=((-1.0)*(IKsqrt(x124.value)));
1660j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1661if( IKabs(j5eval[0]) < 0.0000010000000000 )
1662{
1663{
1664IkReal j5array[1], cj5array[1], sj5array[1];
1665bool j5valid[1]={false};
1666_nj5 = 1;
1667if((((1.0)+(((-1.0)*(gconst18*gconst18))))) < -0.00001)
1668continue;
1669CheckValue<IkReal> x125=IKPowWithIntegerCheck(gconst18,-1);
1670if(!x125.valid){
1671continue;
1672}
1673if( IKabs((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x125.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))))+IKsqr((new_r11*(x125.value)))-1) <= IKFAST_SINCOS_THRESH )
1674 continue;
1675j5array[0]=IKatan2((((gconst18*new_r10))+((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst18*gconst18))))))))), (new_r11*(x125.value)));
1676sj5array[0]=IKsin(j5array[0]);
1677cj5array[0]=IKcos(j5array[0]);
1678if( j5array[0] > IKPI )
1679{
1680 j5array[0]-=IK2PI;
1681}
1682else if( j5array[0] < -IKPI )
1683{ j5array[0]+=IK2PI;
1684}
1685j5valid[0] = true;
1686for(int ij5 = 0; ij5 < 1; ++ij5)
1687{
1688if( !j5valid[ij5] )
1689{
1690 continue;
1691}
1692_ij5[0] = ij5; _ij5[1] = -1;
1693for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1694{
1695if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1696{
1697 j5valid[iij5]=false; _ij5[1] = iij5; break;
1698}
1699}
1700j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1701{
1702IkReal evalcond[8];
1703IkReal x126=IKcos(j5);
1704IkReal x127=IKsin(j5);
1705IkReal x128=((1.0)*gconst18);
1706if((((1.0)+(((-1.0)*gconst18*x128)))) < -0.00001)
1707continue;
1708IkReal x129=IKsqrt(((1.0)+(((-1.0)*gconst18*x128))));
1709IkReal x130=((1.0)*x129);
1710evalcond[0]=x126;
1711evalcond[1]=((-1.0)*x127);
1712evalcond[2]=((((-1.0)*x126*x128))+new_r11);
1713evalcond[3]=(new_r10+(((-1.0)*x127*x128)));
1714evalcond[4]=((((-1.0)*x126*x130))+new_r01);
1715evalcond[5]=(new_r00+(((-1.0)*x127*x130)));
1716evalcond[6]=((((-1.0)*new_r10*x128))+(((-1.0)*new_r00*x130))+x127);
1717evalcond[7]=((((-1.0)*new_r01*x130))+x126+(((-1.0)*new_r11*x128)));
1718if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1719{
1720continue;
1721}
1722}
1723
1724{
1725std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1726vinfos[0].jointtype = 1;
1727vinfos[0].foffset = j0;
1728vinfos[0].indices[0] = _ij0[0];
1729vinfos[0].indices[1] = _ij0[1];
1730vinfos[0].maxsolutions = _nj0;
1731vinfos[1].jointtype = 1;
1732vinfos[1].foffset = j1;
1733vinfos[1].indices[0] = _ij1[0];
1734vinfos[1].indices[1] = _ij1[1];
1735vinfos[1].maxsolutions = _nj1;
1736vinfos[2].jointtype = 1;
1737vinfos[2].foffset = j2;
1738vinfos[2].indices[0] = _ij2[0];
1739vinfos[2].indices[1] = _ij2[1];
1740vinfos[2].maxsolutions = _nj2;
1741vinfos[3].jointtype = 1;
1742vinfos[3].foffset = j3;
1743vinfos[3].indices[0] = _ij3[0];
1744vinfos[3].indices[1] = _ij3[1];
1745vinfos[3].maxsolutions = _nj3;
1746vinfos[4].jointtype = 1;
1747vinfos[4].foffset = j4;
1748vinfos[4].indices[0] = _ij4[0];
1749vinfos[4].indices[1] = _ij4[1];
1750vinfos[4].maxsolutions = _nj4;
1751vinfos[5].jointtype = 1;
1752vinfos[5].foffset = j5;
1753vinfos[5].indices[0] = _ij5[0];
1754vinfos[5].indices[1] = _ij5[1];
1755vinfos[5].maxsolutions = _nj5;
1756std::vector<int> vfree(0);
1757solutions.AddSolution(vinfos,vfree);
1758}
1759}
1760}
1761
1762} else
1763{
1764{
1765IkReal j5array[1], cj5array[1], sj5array[1];
1766bool j5valid[1]={false};
1767_nj5 = 1;
1768CheckValue<IkReal> x131=IKPowWithIntegerCheck(IKsign(gconst18),-1);
1769if(!x131.valid){
1770continue;
1771}
1772CheckValue<IkReal> x132 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
1773if(!x132.valid){
1774continue;
1775}
1776j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x131.value)))+(x132.value));
1777sj5array[0]=IKsin(j5array[0]);
1778cj5array[0]=IKcos(j5array[0]);
1779if( j5array[0] > IKPI )
1780{
1781 j5array[0]-=IK2PI;
1782}
1783else if( j5array[0] < -IKPI )
1784{ j5array[0]+=IK2PI;
1785}
1786j5valid[0] = true;
1787for(int ij5 = 0; ij5 < 1; ++ij5)
1788{
1789if( !j5valid[ij5] )
1790{
1791 continue;
1792}
1793_ij5[0] = ij5; _ij5[1] = -1;
1794for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1795{
1796if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1797{
1798 j5valid[iij5]=false; _ij5[1] = iij5; break;
1799}
1800}
1801j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1802{
1803IkReal evalcond[8];
1804IkReal x133=IKcos(j5);
1805IkReal x134=IKsin(j5);
1806IkReal x135=((1.0)*gconst18);
1807if((((1.0)+(((-1.0)*gconst18*x135)))) < -0.00001)
1808continue;
1809IkReal x136=IKsqrt(((1.0)+(((-1.0)*gconst18*x135))));
1810IkReal x137=((1.0)*x136);
1811evalcond[0]=x133;
1812evalcond[1]=((-1.0)*x134);
1813evalcond[2]=((((-1.0)*x133*x135))+new_r11);
1814evalcond[3]=((((-1.0)*x134*x135))+new_r10);
1815evalcond[4]=((((-1.0)*x133*x137))+new_r01);
1816evalcond[5]=((((-1.0)*x134*x137))+new_r00);
1817evalcond[6]=((((-1.0)*new_r10*x135))+(((-1.0)*new_r00*x137))+x134);
1818evalcond[7]=((((-1.0)*new_r11*x135))+(((-1.0)*new_r01*x137))+x133);
1819if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1820{
1821continue;
1822}
1823}
1824
1825{
1826std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1827vinfos[0].jointtype = 1;
1828vinfos[0].foffset = j0;
1829vinfos[0].indices[0] = _ij0[0];
1830vinfos[0].indices[1] = _ij0[1];
1831vinfos[0].maxsolutions = _nj0;
1832vinfos[1].jointtype = 1;
1833vinfos[1].foffset = j1;
1834vinfos[1].indices[0] = _ij1[0];
1835vinfos[1].indices[1] = _ij1[1];
1836vinfos[1].maxsolutions = _nj1;
1837vinfos[2].jointtype = 1;
1838vinfos[2].foffset = j2;
1839vinfos[2].indices[0] = _ij2[0];
1840vinfos[2].indices[1] = _ij2[1];
1841vinfos[2].maxsolutions = _nj2;
1842vinfos[3].jointtype = 1;
1843vinfos[3].foffset = j3;
1844vinfos[3].indices[0] = _ij3[0];
1845vinfos[3].indices[1] = _ij3[1];
1846vinfos[3].maxsolutions = _nj3;
1847vinfos[4].jointtype = 1;
1848vinfos[4].foffset = j4;
1849vinfos[4].indices[0] = _ij4[0];
1850vinfos[4].indices[1] = _ij4[1];
1851vinfos[4].maxsolutions = _nj4;
1852vinfos[5].jointtype = 1;
1853vinfos[5].foffset = j5;
1854vinfos[5].indices[0] = _ij5[0];
1855vinfos[5].indices[1] = _ij5[1];
1856vinfos[5].maxsolutions = _nj5;
1857std::vector<int> vfree(0);
1858solutions.AddSolution(vinfos,vfree);
1859}
1860}
1861}
1862
1863}
1864
1865}
1866
1867}
1868} while(0);
1869if( bgotonextstatement )
1870{
1871bool bgotonextstatement = true;
1872do
1873{
1874CheckValue<IkReal> x138=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1875if(!x138.valid){
1876continue;
1877}
1878if((x138.value) < -0.00001)
1879continue;
1880IkReal gconst19=IKsqrt(x138.value);
1881evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst19)))))), 6.28318530717959)));
1882if( IKabs(evalcond[0]) < 0.0000050000000000 )
1883{
1884bgotonextstatement=false;
1885{
1886IkReal j5eval[1];
1887new_r21=0;
1888new_r20=0;
1889new_r02=0;
1890new_r12=0;
1891if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
1892continue;
1893sj3=IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))));
1894cj3=gconst19;
1895if( (gconst19) < -1-IKFAST_SINCOS_THRESH || (gconst19) > 1+IKFAST_SINCOS_THRESH )
1896 continue;
1897j3=IKacos(gconst19);
1898CheckValue<IkReal> x139=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
1899if(!x139.valid){
1900continue;
1901}
1902if((x139.value) < -0.00001)
1903continue;
1904IkReal gconst19=IKsqrt(x139.value);
1905j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
1906if( IKabs(j5eval[0]) < 0.0000010000000000 )
1907{
1908{
1909IkReal j5array[1], cj5array[1], sj5array[1];
1910bool j5valid[1]={false};
1911_nj5 = 1;
1912if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
1913continue;
1914CheckValue<IkReal> x140=IKPowWithIntegerCheck(gconst19,-1);
1915if(!x140.valid){
1916continue;
1917}
1918if( IKabs((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x140.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))))+IKsqr((new_r11*(x140.value)))-1) <= IKFAST_SINCOS_THRESH )
1919 continue;
1920j5array[0]=IKatan2((((gconst19*new_r10))+(((-1.0)*new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))), (new_r11*(x140.value)));
1921sj5array[0]=IKsin(j5array[0]);
1922cj5array[0]=IKcos(j5array[0]);
1923if( j5array[0] > IKPI )
1924{
1925 j5array[0]-=IK2PI;
1926}
1927else if( j5array[0] < -IKPI )
1928{ j5array[0]+=IK2PI;
1929}
1930j5valid[0] = true;
1931for(int ij5 = 0; ij5 < 1; ++ij5)
1932{
1933if( !j5valid[ij5] )
1934{
1935 continue;
1936}
1937_ij5[0] = ij5; _ij5[1] = -1;
1938for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1939{
1940if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1941{
1942 j5valid[iij5]=false; _ij5[1] = iij5; break;
1943}
1944}
1945j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1946{
1947IkReal evalcond[8];
1948IkReal x141=IKcos(j5);
1949IkReal x142=IKsin(j5);
1950IkReal x143=((1.0)*gconst19);
1951if((((1.0)+(((-1.0)*gconst19*x143)))) < -0.00001)
1952continue;
1953IkReal x144=IKsqrt(((1.0)+(((-1.0)*gconst19*x143))));
1954evalcond[0]=x141;
1955evalcond[1]=((-1.0)*x142);
1956evalcond[2]=((((-1.0)*x141*x143))+new_r11);
1957evalcond[3]=((((-1.0)*x142*x143))+new_r10);
1958evalcond[4]=(((x141*x144))+new_r01);
1959evalcond[5]=(((x142*x144))+new_r00);
1960evalcond[6]=(((new_r00*x144))+x142+(((-1.0)*new_r10*x143)));
1961evalcond[7]=(((new_r01*x144))+(((-1.0)*new_r11*x143))+x141);
1962if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
1963{
1964continue;
1965}
1966}
1967
1968{
1969std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1970vinfos[0].jointtype = 1;
1971vinfos[0].foffset = j0;
1972vinfos[0].indices[0] = _ij0[0];
1973vinfos[0].indices[1] = _ij0[1];
1974vinfos[0].maxsolutions = _nj0;
1975vinfos[1].jointtype = 1;
1976vinfos[1].foffset = j1;
1977vinfos[1].indices[0] = _ij1[0];
1978vinfos[1].indices[1] = _ij1[1];
1979vinfos[1].maxsolutions = _nj1;
1980vinfos[2].jointtype = 1;
1981vinfos[2].foffset = j2;
1982vinfos[2].indices[0] = _ij2[0];
1983vinfos[2].indices[1] = _ij2[1];
1984vinfos[2].maxsolutions = _nj2;
1985vinfos[3].jointtype = 1;
1986vinfos[3].foffset = j3;
1987vinfos[3].indices[0] = _ij3[0];
1988vinfos[3].indices[1] = _ij3[1];
1989vinfos[3].maxsolutions = _nj3;
1990vinfos[4].jointtype = 1;
1991vinfos[4].foffset = j4;
1992vinfos[4].indices[0] = _ij4[0];
1993vinfos[4].indices[1] = _ij4[1];
1994vinfos[4].maxsolutions = _nj4;
1995vinfos[5].jointtype = 1;
1996vinfos[5].foffset = j5;
1997vinfos[5].indices[0] = _ij5[0];
1998vinfos[5].indices[1] = _ij5[1];
1999vinfos[5].maxsolutions = _nj5;
2000std::vector<int> vfree(0);
2001solutions.AddSolution(vinfos,vfree);
2002}
2003}
2004}
2005
2006} else
2007{
2008{
2009IkReal j5array[1], cj5array[1], sj5array[1];
2010bool j5valid[1]={false};
2011_nj5 = 1;
2012CheckValue<IkReal> x145=IKPowWithIntegerCheck(IKsign(gconst19),-1);
2013if(!x145.valid){
2014continue;
2015}
2016CheckValue<IkReal> x146 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2017if(!x146.valid){
2018continue;
2019}
2020j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x145.value)))+(x146.value));
2021sj5array[0]=IKsin(j5array[0]);
2022cj5array[0]=IKcos(j5array[0]);
2023if( j5array[0] > IKPI )
2024{
2025 j5array[0]-=IK2PI;
2026}
2027else if( j5array[0] < -IKPI )
2028{ j5array[0]+=IK2PI;
2029}
2030j5valid[0] = true;
2031for(int ij5 = 0; ij5 < 1; ++ij5)
2032{
2033if( !j5valid[ij5] )
2034{
2035 continue;
2036}
2037_ij5[0] = ij5; _ij5[1] = -1;
2038for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2039{
2040if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2041{
2042 j5valid[iij5]=false; _ij5[1] = iij5; break;
2043}
2044}
2045j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2046{
2047IkReal evalcond[8];
2048IkReal x147=IKcos(j5);
2049IkReal x148=IKsin(j5);
2050IkReal x149=((1.0)*gconst19);
2051if((((1.0)+(((-1.0)*gconst19*x149)))) < -0.00001)
2052continue;
2053IkReal x150=IKsqrt(((1.0)+(((-1.0)*gconst19*x149))));
2054evalcond[0]=x147;
2055evalcond[1]=((-1.0)*x148);
2056evalcond[2]=((((-1.0)*x147*x149))+new_r11);
2057evalcond[3]=((((-1.0)*x148*x149))+new_r10);
2058evalcond[4]=(((x147*x150))+new_r01);
2059evalcond[5]=(((x148*x150))+new_r00);
2060evalcond[6]=(((new_r00*x150))+x148+(((-1.0)*new_r10*x149)));
2061evalcond[7]=(((new_r01*x150))+(((-1.0)*new_r11*x149))+x147);
2062if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2063{
2064continue;
2065}
2066}
2067
2068{
2069std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2070vinfos[0].jointtype = 1;
2071vinfos[0].foffset = j0;
2072vinfos[0].indices[0] = _ij0[0];
2073vinfos[0].indices[1] = _ij0[1];
2074vinfos[0].maxsolutions = _nj0;
2075vinfos[1].jointtype = 1;
2076vinfos[1].foffset = j1;
2077vinfos[1].indices[0] = _ij1[0];
2078vinfos[1].indices[1] = _ij1[1];
2079vinfos[1].maxsolutions = _nj1;
2080vinfos[2].jointtype = 1;
2081vinfos[2].foffset = j2;
2082vinfos[2].indices[0] = _ij2[0];
2083vinfos[2].indices[1] = _ij2[1];
2084vinfos[2].maxsolutions = _nj2;
2085vinfos[3].jointtype = 1;
2086vinfos[3].foffset = j3;
2087vinfos[3].indices[0] = _ij3[0];
2088vinfos[3].indices[1] = _ij3[1];
2089vinfos[3].maxsolutions = _nj3;
2090vinfos[4].jointtype = 1;
2091vinfos[4].foffset = j4;
2092vinfos[4].indices[0] = _ij4[0];
2093vinfos[4].indices[1] = _ij4[1];
2094vinfos[4].maxsolutions = _nj4;
2095vinfos[5].jointtype = 1;
2096vinfos[5].foffset = j5;
2097vinfos[5].indices[0] = _ij5[0];
2098vinfos[5].indices[1] = _ij5[1];
2099vinfos[5].maxsolutions = _nj5;
2100std::vector<int> vfree(0);
2101solutions.AddSolution(vinfos,vfree);
2102}
2103}
2104}
2105
2106}
2107
2108}
2109
2110}
2111} while(0);
2112if( bgotonextstatement )
2113{
2114bool bgotonextstatement = true;
2115do
2116{
2117CheckValue<IkReal> x151=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2118if(!x151.valid){
2119continue;
2120}
2121if((x151.value) < -0.00001)
2122continue;
2123IkReal gconst19=IKsqrt(x151.value);
2124evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.0)+(IKsign(sj3)))))+(IKabs((cj3+(((-1.0)*gconst19)))))), 6.28318530717959)));
2125if( IKabs(evalcond[0]) < 0.0000050000000000 )
2126{
2127bgotonextstatement=false;
2128{
2129IkReal j5eval[1];
2130new_r21=0;
2131new_r20=0;
2132new_r02=0;
2133new_r12=0;
2134if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
2135continue;
2136sj3=((-1.0)*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19)))))));
2137cj3=gconst19;
2138if( (gconst19) < -1-IKFAST_SINCOS_THRESH || (gconst19) > 1+IKFAST_SINCOS_THRESH )
2139 continue;
2140j3=((-1.0)*(IKacos(gconst19)));
2141CheckValue<IkReal> x152=IKPowWithIntegerCheck(((1.0)+(((-1.0)*(new_r22*new_r22)))),-1);
2142if(!x152.valid){
2143continue;
2144}
2145if((x152.value) < -0.00001)
2146continue;
2147IkReal gconst19=IKsqrt(x152.value);
2148j5eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
2149if( IKabs(j5eval[0]) < 0.0000010000000000 )
2150{
2151{
2152IkReal j5array[1], cj5array[1], sj5array[1];
2153bool j5valid[1]={false};
2154_nj5 = 1;
2155if((((1.0)+(((-1.0)*(gconst19*gconst19))))) < -0.00001)
2156continue;
2157CheckValue<IkReal> x153=IKPowWithIntegerCheck(gconst19,-1);
2158if(!x153.valid){
2159continue;
2160}
2161if( IKabs((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r11*(x153.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10))))+IKsqr((new_r11*(x153.value)))-1) <= IKFAST_SINCOS_THRESH )
2162 continue;
2163j5array[0]=IKatan2((((new_r00*(IKsqrt(((1.0)+(((-1.0)*(gconst19*gconst19))))))))+((gconst19*new_r10))), (new_r11*(x153.value)));
2164sj5array[0]=IKsin(j5array[0]);
2165cj5array[0]=IKcos(j5array[0]);
2166if( j5array[0] > IKPI )
2167{
2168 j5array[0]-=IK2PI;
2169}
2170else if( j5array[0] < -IKPI )
2171{ j5array[0]+=IK2PI;
2172}
2173j5valid[0] = true;
2174for(int ij5 = 0; ij5 < 1; ++ij5)
2175{
2176if( !j5valid[ij5] )
2177{
2178 continue;
2179}
2180_ij5[0] = ij5; _ij5[1] = -1;
2181for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2182{
2183if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2184{
2185 j5valid[iij5]=false; _ij5[1] = iij5; break;
2186}
2187}
2188j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2189{
2190IkReal evalcond[8];
2191IkReal x154=IKcos(j5);
2192IkReal x155=IKsin(j5);
2193IkReal x156=((1.0)*gconst19);
2194if((((1.0)+(((-1.0)*gconst19*x156)))) < -0.00001)
2195continue;
2196IkReal x157=IKsqrt(((1.0)+(((-1.0)*gconst19*x156))));
2197IkReal x158=((1.0)*x157);
2198evalcond[0]=x154;
2199evalcond[1]=((-1.0)*x155);
2200evalcond[2]=((((-1.0)*x154*x156))+new_r11);
2201evalcond[3]=((((-1.0)*x155*x156))+new_r10);
2202evalcond[4]=((((-1.0)*x154*x158))+new_r01);
2203evalcond[5]=((((-1.0)*x155*x158))+new_r00);
2204evalcond[6]=(x155+(((-1.0)*new_r10*x156))+(((-1.0)*new_r00*x158)));
2205evalcond[7]=(x154+(((-1.0)*new_r11*x156))+(((-1.0)*new_r01*x158)));
2206if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2207{
2208continue;
2209}
2210}
2211
2212{
2213std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2214vinfos[0].jointtype = 1;
2215vinfos[0].foffset = j0;
2216vinfos[0].indices[0] = _ij0[0];
2217vinfos[0].indices[1] = _ij0[1];
2218vinfos[0].maxsolutions = _nj0;
2219vinfos[1].jointtype = 1;
2220vinfos[1].foffset = j1;
2221vinfos[1].indices[0] = _ij1[0];
2222vinfos[1].indices[1] = _ij1[1];
2223vinfos[1].maxsolutions = _nj1;
2224vinfos[2].jointtype = 1;
2225vinfos[2].foffset = j2;
2226vinfos[2].indices[0] = _ij2[0];
2227vinfos[2].indices[1] = _ij2[1];
2228vinfos[2].maxsolutions = _nj2;
2229vinfos[3].jointtype = 1;
2230vinfos[3].foffset = j3;
2231vinfos[3].indices[0] = _ij3[0];
2232vinfos[3].indices[1] = _ij3[1];
2233vinfos[3].maxsolutions = _nj3;
2234vinfos[4].jointtype = 1;
2235vinfos[4].foffset = j4;
2236vinfos[4].indices[0] = _ij4[0];
2237vinfos[4].indices[1] = _ij4[1];
2238vinfos[4].maxsolutions = _nj4;
2239vinfos[5].jointtype = 1;
2240vinfos[5].foffset = j5;
2241vinfos[5].indices[0] = _ij5[0];
2242vinfos[5].indices[1] = _ij5[1];
2243vinfos[5].maxsolutions = _nj5;
2244std::vector<int> vfree(0);
2245solutions.AddSolution(vinfos,vfree);
2246}
2247}
2248}
2249
2250} else
2251{
2252{
2253IkReal j5array[1], cj5array[1], sj5array[1];
2254bool j5valid[1]={false};
2255_nj5 = 1;
2256CheckValue<IkReal> x159=IKPowWithIntegerCheck(IKsign(gconst19),-1);
2257if(!x159.valid){
2258continue;
2259}
2260CheckValue<IkReal> x160 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
2261if(!x160.valid){
2262continue;
2263}
2264j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x159.value)))+(x160.value));
2265sj5array[0]=IKsin(j5array[0]);
2266cj5array[0]=IKcos(j5array[0]);
2267if( j5array[0] > IKPI )
2268{
2269 j5array[0]-=IK2PI;
2270}
2271else if( j5array[0] < -IKPI )
2272{ j5array[0]+=IK2PI;
2273}
2274j5valid[0] = true;
2275for(int ij5 = 0; ij5 < 1; ++ij5)
2276{
2277if( !j5valid[ij5] )
2278{
2279 continue;
2280}
2281_ij5[0] = ij5; _ij5[1] = -1;
2282for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2283{
2284if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2285{
2286 j5valid[iij5]=false; _ij5[1] = iij5; break;
2287}
2288}
2289j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2290{
2291IkReal evalcond[8];
2292IkReal x161=IKcos(j5);
2293IkReal x162=IKsin(j5);
2294IkReal x163=((1.0)*gconst19);
2295if((((1.0)+(((-1.0)*gconst19*x163)))) < -0.00001)
2296continue;
2297IkReal x164=IKsqrt(((1.0)+(((-1.0)*gconst19*x163))));
2298IkReal x165=((1.0)*x164);
2299evalcond[0]=x161;
2300evalcond[1]=((-1.0)*x162);
2301evalcond[2]=((((-1.0)*x161*x163))+new_r11);
2302evalcond[3]=((((-1.0)*x162*x163))+new_r10);
2303evalcond[4]=((((-1.0)*x161*x165))+new_r01);
2304evalcond[5]=((((-1.0)*x162*x165))+new_r00);
2305evalcond[6]=((((-1.0)*new_r00*x165))+(((-1.0)*new_r10*x163))+x162);
2306evalcond[7]=((((-1.0)*new_r11*x163))+x161+(((-1.0)*new_r01*x165)));
2307if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2308{
2309continue;
2310}
2311}
2312
2313{
2314std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2315vinfos[0].jointtype = 1;
2316vinfos[0].foffset = j0;
2317vinfos[0].indices[0] = _ij0[0];
2318vinfos[0].indices[1] = _ij0[1];
2319vinfos[0].maxsolutions = _nj0;
2320vinfos[1].jointtype = 1;
2321vinfos[1].foffset = j1;
2322vinfos[1].indices[0] = _ij1[0];
2323vinfos[1].indices[1] = _ij1[1];
2324vinfos[1].maxsolutions = _nj1;
2325vinfos[2].jointtype = 1;
2326vinfos[2].foffset = j2;
2327vinfos[2].indices[0] = _ij2[0];
2328vinfos[2].indices[1] = _ij2[1];
2329vinfos[2].maxsolutions = _nj2;
2330vinfos[3].jointtype = 1;
2331vinfos[3].foffset = j3;
2332vinfos[3].indices[0] = _ij3[0];
2333vinfos[3].indices[1] = _ij3[1];
2334vinfos[3].maxsolutions = _nj3;
2335vinfos[4].jointtype = 1;
2336vinfos[4].foffset = j4;
2337vinfos[4].indices[0] = _ij4[0];
2338vinfos[4].indices[1] = _ij4[1];
2339vinfos[4].maxsolutions = _nj4;
2340vinfos[5].jointtype = 1;
2341vinfos[5].foffset = j5;
2342vinfos[5].indices[0] = _ij5[0];
2343vinfos[5].indices[1] = _ij5[1];
2344vinfos[5].maxsolutions = _nj5;
2345std::vector<int> vfree(0);
2346solutions.AddSolution(vinfos,vfree);
2347}
2348}
2349}
2350
2351}
2352
2353}
2354
2355}
2356} while(0);
2357if( bgotonextstatement )
2358{
2359bool bgotonextstatement = true;
2360do
2361{
2362if( 1 )
2363{
2364bgotonextstatement=false;
2365continue; // branch miss [j5]
2366
2367}
2368} while(0);
2369if( bgotonextstatement )
2370{
2371}
2372}
2373}
2374}
2375}
2376}
2377}
2378}
2379
2380} else
2381{
2382{
2383IkReal j5array[1], cj5array[1], sj5array[1];
2384bool j5valid[1]={false};
2385_nj5 = 1;
2386IkReal x166=(new_r00*sj3);
2387CheckValue<IkReal> x167=IKPowWithIntegerCheck(sj3,-1);
2388if(!x167.valid){
2389continue;
2390}
2391if( IKabs(((((-1.0)*x166))+((cj3*new_r10)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*x166))+((cj3*new_r10))))+IKsqr(((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))))-1) <= IKFAST_SINCOS_THRESH )
2392 continue;
2393j5array[0]=IKatan2(((((-1.0)*x166))+((cj3*new_r10))), ((x167.value)*((((cj3*new_r22*x166))+(((-1.0)*new_r01))+(((-1.0)*new_r10*new_r22*(cj3*cj3)))))));
2394sj5array[0]=IKsin(j5array[0]);
2395cj5array[0]=IKcos(j5array[0]);
2396if( j5array[0] > IKPI )
2397{
2398 j5array[0]-=IK2PI;
2399}
2400else if( j5array[0] < -IKPI )
2401{ j5array[0]+=IK2PI;
2402}
2403j5valid[0] = true;
2404for(int ij5 = 0; ij5 < 1; ++ij5)
2405{
2406if( !j5valid[ij5] )
2407{
2408 continue;
2409}
2410_ij5[0] = ij5; _ij5[1] = -1;
2411for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2412{
2413if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2414{
2415 j5valid[iij5]=false; _ij5[1] = iij5; break;
2416}
2417}
2418j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2419{
2420IkReal evalcond[10];
2421IkReal x168=IKsin(j5);
2422IkReal x169=IKcos(j5);
2423IkReal x170=((1.0)*new_r11);
2424IkReal x171=(new_r22*sj3);
2425IkReal x172=((1.0)*new_r10);
2426IkReal x173=((1.0)*cj3*new_r22);
2427IkReal x174=(sj3*x168);
2428IkReal x175=(new_r22*x168);
2429IkReal x176=((1.0)*x169);
2430IkReal x177=((1.0)*x168);
2431evalcond[0]=(((new_r00*sj3))+x168+(((-1.0)*cj3*x172)));
2432evalcond[1]=(((new_r01*sj3))+x169+(((-1.0)*cj3*x170)));
2433evalcond[2]=(((new_r11*sj3))+x175+((cj3*new_r01)));
2434evalcond[3]=(((sj3*x169))+((cj3*x175))+new_r01);
2435evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x176))+((cj3*new_r00)));
2436evalcond[5]=(x174+new_r00+(((-1.0)*x169*x173)));
2437evalcond[6]=(((x168*x171))+(((-1.0)*cj3*x176))+new_r11);
2438evalcond[7]=(x169+(((-1.0)*x171*x172))+(((-1.0)*new_r00*x173)));
2439evalcond[8]=((((-1.0)*cj3*x177))+(((-1.0)*x171*x176))+new_r10);
2440evalcond[9]=((((-1.0)*x177))+(((-1.0)*x170*x171))+(((-1.0)*new_r01*x173)));
2441if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH )
2442{
2443continue;
2444}
2445}
2446
2447{
2448std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2449vinfos[0].jointtype = 1;
2450vinfos[0].foffset = j0;
2451vinfos[0].indices[0] = _ij0[0];
2452vinfos[0].indices[1] = _ij0[1];
2453vinfos[0].maxsolutions = _nj0;
2454vinfos[1].jointtype = 1;
2455vinfos[1].foffset = j1;
2456vinfos[1].indices[0] = _ij1[0];
2457vinfos[1].indices[1] = _ij1[1];
2458vinfos[1].maxsolutions = _nj1;
2459vinfos[2].jointtype = 1;
2460vinfos[2].foffset = j2;
2461vinfos[2].indices[0] = _ij2[0];
2462vinfos[2].indices[1] = _ij2[1];
2463vinfos[2].maxsolutions = _nj2;
2464vinfos[3].jointtype = 1;
2465vinfos[3].foffset = j3;
2466vinfos[3].indices[0] = _ij3[0];
2467vinfos[3].indices[1] = _ij3[1];
2468vinfos[3].maxsolutions = _nj3;
2469vinfos[4].jointtype = 1;
2470vinfos[4].foffset = j4;
2471vinfos[4].indices[0] = _ij4[0];
2472vinfos[4].indices[1] = _ij4[1];
2473vinfos[4].maxsolutions = _nj4;
2474vinfos[5].jointtype = 1;
2475vinfos[5].foffset = j5;
2476vinfos[5].indices[0] = _ij5[0];
2477vinfos[5].indices[1] = _ij5[1];
2478vinfos[5].maxsolutions = _nj5;
2479std::vector<int> vfree(0);
2480solutions.AddSolution(vinfos,vfree);
2481}
2482}
2483}
2484
2485}
2486
2487}
2488
2489} else
2490{
2491{
2492IkReal j5array[1], cj5array[1], sj5array[1];
2493bool j5valid[1]={false};
2494_nj5 = 1;
2495IkReal x178=((1.0)*new_r01);
2496CheckValue<IkReal> x179=IKPowWithIntegerCheck(new_r22,-1);
2497if(!x179.valid){
2498continue;
2499}
2500if( IKabs(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs((((cj3*new_r11))+(((-1.0)*sj3*x178)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3))))))+IKsqr((((cj3*new_r11))+(((-1.0)*sj3*x178))))-1) <= IKFAST_SINCOS_THRESH )
2501 continue;
2502j5array[0]=IKatan2(((x179.value)*(((((-1.0)*cj3*x178))+(((-1.0)*new_r11*sj3))))), (((cj3*new_r11))+(((-1.0)*sj3*x178))));
2503sj5array[0]=IKsin(j5array[0]);
2504cj5array[0]=IKcos(j5array[0]);
2505if( j5array[0] > IKPI )
2506{
2507 j5array[0]-=IK2PI;
2508}
2509else if( j5array[0] < -IKPI )
2510{ j5array[0]+=IK2PI;
2511}
2512j5valid[0] = true;
2513for(int ij5 = 0; ij5 < 1; ++ij5)
2514{
2515if( !j5valid[ij5] )
2516{
2517 continue;
2518}
2519_ij5[0] = ij5; _ij5[1] = -1;
2520for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2521{
2522if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2523{
2524 j5valid[iij5]=false; _ij5[1] = iij5; break;
2525}
2526}
2527j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2528{
2529IkReal evalcond[10];
2530IkReal x180=IKsin(j5);
2531IkReal x181=IKcos(j5);
2532IkReal x182=((1.0)*new_r11);
2533IkReal x183=(new_r22*sj3);
2534IkReal x184=((1.0)*new_r10);
2535IkReal x185=((1.0)*cj3*new_r22);
2536IkReal x186=(sj3*x180);
2537IkReal x187=(new_r22*x180);
2538IkReal x188=((1.0)*x181);
2539IkReal x189=((1.0)*x180);
2540evalcond[0]=(((new_r00*sj3))+x180+(((-1.0)*cj3*x184)));
2541evalcond[1]=(((new_r01*sj3))+x181+(((-1.0)*cj3*x182)));
2542evalcond[2]=(((new_r11*sj3))+x187+((cj3*new_r01)));
2543evalcond[3]=(((sj3*x181))+((cj3*x187))+new_r01);
2544evalcond[4]=(((new_r10*sj3))+(((-1.0)*new_r22*x188))+((cj3*new_r00)));
2545evalcond[5]=(x186+new_r00+(((-1.0)*x181*x185)));
2546evalcond[6]=(((x180*x183))+new_r11+(((-1.0)*cj3*x188)));
2547evalcond[7]=(x181+(((-1.0)*new_r00*x185))+(((-1.0)*x183*x184)));
2548evalcond[8]=(new_r10+(((-1.0)*x183*x188))+(((-1.0)*cj3*x189)));
2549evalcond[9]=((((-1.0)*x189))+(((-1.0)*new_r01*x185))+(((-1.0)*x182*x183)));
2550if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH )
2551{
2552continue;
2553}
2554}
2555
2556{
2557std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2558vinfos[0].jointtype = 1;
2559vinfos[0].foffset = j0;
2560vinfos[0].indices[0] = _ij0[0];
2561vinfos[0].indices[1] = _ij0[1];
2562vinfos[0].maxsolutions = _nj0;
2563vinfos[1].jointtype = 1;
2564vinfos[1].foffset = j1;
2565vinfos[1].indices[0] = _ij1[0];
2566vinfos[1].indices[1] = _ij1[1];
2567vinfos[1].maxsolutions = _nj1;
2568vinfos[2].jointtype = 1;
2569vinfos[2].foffset = j2;
2570vinfos[2].indices[0] = _ij2[0];
2571vinfos[2].indices[1] = _ij2[1];
2572vinfos[2].maxsolutions = _nj2;
2573vinfos[3].jointtype = 1;
2574vinfos[3].foffset = j3;
2575vinfos[3].indices[0] = _ij3[0];
2576vinfos[3].indices[1] = _ij3[1];
2577vinfos[3].maxsolutions = _nj3;
2578vinfos[4].jointtype = 1;
2579vinfos[4].foffset = j4;
2580vinfos[4].indices[0] = _ij4[0];
2581vinfos[4].indices[1] = _ij4[1];
2582vinfos[4].maxsolutions = _nj4;
2583vinfos[5].jointtype = 1;
2584vinfos[5].foffset = j5;
2585vinfos[5].indices[0] = _ij5[0];
2586vinfos[5].indices[1] = _ij5[1];
2587vinfos[5].maxsolutions = _nj5;
2588std::vector<int> vfree(0);
2589solutions.AddSolution(vinfos,vfree);
2590}
2591}
2592}
2593
2594}
2595
2596}
2597
2598} else
2599{
2600{
2601IkReal j5array[1], cj5array[1], sj5array[1];
2602bool j5valid[1]={false};
2603_nj5 = 1;
2604IkReal x190=cj3*cj3;
2605IkReal x191=(cj3*new_r22);
2606CheckValue<IkReal> x192=IKPowWithIntegerCheck(IKsign(((-1.0)+(((-1.0)*x190*(new_r22*new_r22)))+x190)),-1);
2607if(!x192.valid){
2608continue;
2609}
2610CheckValue<IkReal> x193 = IKatan2WithCheck(IkReal((((new_r01*x191))+((new_r00*sj3)))),IkReal((((new_r01*sj3))+(((-1.0)*new_r00*x191)))),IKFAST_ATAN2_MAGTHRESH);
2611if(!x193.valid){
2612continue;
2613}
2614j5array[0]=((-1.5707963267949)+(((1.5707963267949)*(x192.value)))+(x193.value));
2615sj5array[0]=IKsin(j5array[0]);
2616cj5array[0]=IKcos(j5array[0]);
2617if( j5array[0] > IKPI )
2618{
2619 j5array[0]-=IK2PI;
2620}
2621else if( j5array[0] < -IKPI )
2622{ j5array[0]+=IK2PI;
2623}
2624j5valid[0] = true;
2625for(int ij5 = 0; ij5 < 1; ++ij5)
2626{
2627if( !j5valid[ij5] )
2628{
2629 continue;
2630}
2631_ij5[0] = ij5; _ij5[1] = -1;
2632for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2633{
2634if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2635{
2636 j5valid[iij5]=false; _ij5[1] = iij5; break;
2637}
2638}
2639j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2640{
2641IkReal evalcond[10];
2642IkReal x194=IKsin(j5);
2643IkReal x195=IKcos(j5);
2644IkReal x196=((1.0)*new_r11);
2645IkReal x197=(new_r22*sj3);
2646IkReal x198=((1.0)*new_r10);
2647IkReal x199=((1.0)*cj3*new_r22);
2648IkReal x200=(sj3*x194);
2649IkReal x201=(new_r22*x194);
2650IkReal x202=((1.0)*x195);
2651IkReal x203=((1.0)*x194);
2652evalcond[0]=(((new_r00*sj3))+x194+(((-1.0)*cj3*x198)));
2653evalcond[1]=(((new_r01*sj3))+x195+(((-1.0)*cj3*x196)));
2654evalcond[2]=(((new_r11*sj3))+x201+((cj3*new_r01)));
2655evalcond[3]=(((sj3*x195))+((cj3*x201))+new_r01);
2656evalcond[4]=((((-1.0)*new_r22*x202))+((new_r10*sj3))+((cj3*new_r00)));
2657evalcond[5]=((((-1.0)*x195*x199))+x200+new_r00);
2658evalcond[6]=(((x194*x197))+new_r11+(((-1.0)*cj3*x202)));
2659evalcond[7]=((((-1.0)*x197*x198))+x195+(((-1.0)*new_r00*x199)));
2660evalcond[8]=((((-1.0)*x197*x202))+new_r10+(((-1.0)*cj3*x203)));
2661evalcond[9]=((((-1.0)*x196*x197))+(((-1.0)*x203))+(((-1.0)*new_r01*x199)));
2662if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH )
2663{
2664continue;
2665}
2666}
2667
2668{
2669std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2670vinfos[0].jointtype = 1;
2671vinfos[0].foffset = j0;
2672vinfos[0].indices[0] = _ij0[0];
2673vinfos[0].indices[1] = _ij0[1];
2674vinfos[0].maxsolutions = _nj0;
2675vinfos[1].jointtype = 1;
2676vinfos[1].foffset = j1;
2677vinfos[1].indices[0] = _ij1[0];
2678vinfos[1].indices[1] = _ij1[1];
2679vinfos[1].maxsolutions = _nj1;
2680vinfos[2].jointtype = 1;
2681vinfos[2].foffset = j2;
2682vinfos[2].indices[0] = _ij2[0];
2683vinfos[2].indices[1] = _ij2[1];
2684vinfos[2].maxsolutions = _nj2;
2685vinfos[3].jointtype = 1;
2686vinfos[3].foffset = j3;
2687vinfos[3].indices[0] = _ij3[0];
2688vinfos[3].indices[1] = _ij3[1];
2689vinfos[3].maxsolutions = _nj3;
2690vinfos[4].jointtype = 1;
2691vinfos[4].foffset = j4;
2692vinfos[4].indices[0] = _ij4[0];
2693vinfos[4].indices[1] = _ij4[1];
2694vinfos[4].maxsolutions = _nj4;
2695vinfos[5].jointtype = 1;
2696vinfos[5].foffset = j5;
2697vinfos[5].indices[0] = _ij5[0];
2698vinfos[5].indices[1] = _ij5[1];
2699vinfos[5].maxsolutions = _nj5;
2700std::vector<int> vfree(0);
2701solutions.AddSolution(vinfos,vfree);
2702}
2703}
2704}
2705
2706}
2707
2708}
2709 }
2710
2711}
2712
2713}
2714
2715}
2716} while(0);
2717if( bgotonextstatement )
2718{
2719bool bgotonextstatement = true;
2720do
2721{
2722if( 1 )
2723{
2724bgotonextstatement=false;
2725continue; // branch miss [j3, j5]
2726
2727}
2728} while(0);
2729if( bgotonextstatement )
2730{
2731}
2732}
2733}
2734}
2735}
2736
2737} else
2738{
2739{
2740IkReal j3array[1], cj3array[1], sj3array[1];
2741bool j3valid[1]={false};
2742_nj3 = 1;
2743CheckValue<IkReal> x205=IKPowWithIntegerCheck(sj4,-1);
2744if(!x205.valid){
2745continue;
2746}
2747IkReal x204=x205.value;
2748CheckValue<IkReal> x206=IKPowWithIntegerCheck(new_r12,-1);
2749if(!x206.valid){
2750continue;
2751}
2752if( IKabs((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x204)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x204))-1) <= IKFAST_SINCOS_THRESH )
2753 continue;
2754j3array[0]=IKatan2((x204*(x206.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x204));
2755sj3array[0]=IKsin(j3array[0]);
2756cj3array[0]=IKcos(j3array[0]);
2757if( j3array[0] > IKPI )
2758{
2759 j3array[0]-=IK2PI;
2760}
2761else if( j3array[0] < -IKPI )
2762{ j3array[0]+=IK2PI;
2763}
2764j3valid[0] = true;
2765for(int ij3 = 0; ij3 < 1; ++ij3)
2766{
2767if( !j3valid[ij3] )
2768{
2769 continue;
2770}
2771_ij3[0] = ij3; _ij3[1] = -1;
2772for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2773{
2774if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2775{
2776 j3valid[iij3]=false; _ij3[1] = iij3; break;
2777}
2778}
2779j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2780{
2781IkReal evalcond[8];
2782IkReal x207=IKcos(j3);
2783IkReal x208=IKsin(j3);
2784IkReal x209=((1.0)*cj4);
2785IkReal x210=(sj4*x208);
2786IkReal x211=(new_r12*x208);
2787IkReal x212=(sj4*x207);
2788IkReal x213=(new_r02*x207);
2789evalcond[0]=(x212+new_r02);
2790evalcond[1]=(x210+new_r12);
2791evalcond[2]=((((-1.0)*new_r02*x208))+((new_r12*x207)));
2792evalcond[3]=(sj4+x211+x213);
2793evalcond[4]=((((-1.0)*new_r20*x209))+((new_r00*x212))+((new_r10*x210)));
2794evalcond[5]=((((-1.0)*new_r21*x209))+((new_r01*x212))+((new_r11*x210)));
2795evalcond[6]=((1.0)+(((-1.0)*new_r22*x209))+((new_r02*x212))+((new_r12*x210)));
2796evalcond[7]=((((-1.0)*new_r22*sj4))+(((-1.0)*x209*x213))+(((-1.0)*x209*x211)));
2797if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2798{
2799continue;
2800}
2801}
2802
2803{
2804IkReal j5eval[3];
2805j5eval[0]=sj4;
2806j5eval[1]=IKsign(sj4);
2807j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
2808if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
2809{
2810{
2811IkReal j5eval[2];
2812j5eval[0]=sj4;
2813j5eval[1]=sj3;
2814if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2815{
2816{
2817IkReal j5eval[2];
2818j5eval[0]=sj4;
2819j5eval[1]=cj3;
2820if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
2821{
2822{
2823IkReal evalcond[5];
2824bool bgotonextstatement = true;
2825do
2826{
2827evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
2828evalcond[1]=new_r02;
2829evalcond[2]=new_r12;
2830evalcond[3]=new_r21;
2831evalcond[4]=new_r20;
2832if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
2833{
2834bgotonextstatement=false;
2835{
2836IkReal j5array[1], cj5array[1], sj5array[1];
2837bool j5valid[1]={false};
2838_nj5 = 1;
2839IkReal x214=((1.0)*new_r01);
2840if( IKabs(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x214))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x214))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2841 continue;
2842j5array[0]=IKatan2(((((-1.0)*cj3*x214))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x214))+((cj3*new_r00))));
2843sj5array[0]=IKsin(j5array[0]);
2844cj5array[0]=IKcos(j5array[0]);
2845if( j5array[0] > IKPI )
2846{
2847 j5array[0]-=IK2PI;
2848}
2849else if( j5array[0] < -IKPI )
2850{ j5array[0]+=IK2PI;
2851}
2852j5valid[0] = true;
2853for(int ij5 = 0; ij5 < 1; ++ij5)
2854{
2855if( !j5valid[ij5] )
2856{
2857 continue;
2858}
2859_ij5[0] = ij5; _ij5[1] = -1;
2860for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2861{
2862if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2863{
2864 j5valid[iij5]=false; _ij5[1] = iij5; break;
2865}
2866}
2867j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2868{
2869IkReal evalcond[8];
2870IkReal x215=IKsin(j5);
2871IkReal x216=IKcos(j5);
2872IkReal x217=((1.0)*cj3);
2873IkReal x218=(sj3*x215);
2874IkReal x219=((1.0)*x216);
2875IkReal x220=(x216*x217);
2876evalcond[0]=(((new_r11*sj3))+x215+((cj3*new_r01)));
2877evalcond[1]=(((new_r00*sj3))+(((-1.0)*new_r10*x217))+x215);
2878evalcond[2]=((((-1.0)*new_r11*x217))+((new_r01*sj3))+x216);
2879evalcond[3]=(((sj3*x216))+((cj3*x215))+new_r01);
2880evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x219)));
2881evalcond[5]=(x218+new_r00+(((-1.0)*x220)));
2882evalcond[6]=(x218+new_r11+(((-1.0)*x220)));
2883evalcond[7]=((((-1.0)*sj3*x219))+new_r10+(((-1.0)*x215*x217)));
2884if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2885{
2886continue;
2887}
2888}
2889
2890{
2891std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2892vinfos[0].jointtype = 1;
2893vinfos[0].foffset = j0;
2894vinfos[0].indices[0] = _ij0[0];
2895vinfos[0].indices[1] = _ij0[1];
2896vinfos[0].maxsolutions = _nj0;
2897vinfos[1].jointtype = 1;
2898vinfos[1].foffset = j1;
2899vinfos[1].indices[0] = _ij1[0];
2900vinfos[1].indices[1] = _ij1[1];
2901vinfos[1].maxsolutions = _nj1;
2902vinfos[2].jointtype = 1;
2903vinfos[2].foffset = j2;
2904vinfos[2].indices[0] = _ij2[0];
2905vinfos[2].indices[1] = _ij2[1];
2906vinfos[2].maxsolutions = _nj2;
2907vinfos[3].jointtype = 1;
2908vinfos[3].foffset = j3;
2909vinfos[3].indices[0] = _ij3[0];
2910vinfos[3].indices[1] = _ij3[1];
2911vinfos[3].maxsolutions = _nj3;
2912vinfos[4].jointtype = 1;
2913vinfos[4].foffset = j4;
2914vinfos[4].indices[0] = _ij4[0];
2915vinfos[4].indices[1] = _ij4[1];
2916vinfos[4].maxsolutions = _nj4;
2917vinfos[5].jointtype = 1;
2918vinfos[5].foffset = j5;
2919vinfos[5].indices[0] = _ij5[0];
2920vinfos[5].indices[1] = _ij5[1];
2921vinfos[5].maxsolutions = _nj5;
2922std::vector<int> vfree(0);
2923solutions.AddSolution(vinfos,vfree);
2924}
2925}
2926}
2927
2928}
2929} while(0);
2930if( bgotonextstatement )
2931{
2932bool bgotonextstatement = true;
2933do
2934{
2935evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
2936evalcond[1]=new_r02;
2937evalcond[2]=new_r12;
2938evalcond[3]=new_r21;
2939evalcond[4]=new_r20;
2940if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
2941{
2942bgotonextstatement=false;
2943{
2944IkReal j5array[1], cj5array[1], sj5array[1];
2945bool j5valid[1]={false};
2946_nj5 = 1;
2947IkReal x221=((1.0)*sj3);
2948if( IKabs((((cj3*new_r01))+(((-1.0)*new_r00*x221)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r01))+(((-1.0)*new_r00*x221))))+IKsqr(((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
2949 continue;
2950j5array[0]=IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x221))), ((((-1.0)*new_r01*x221))+(((-1.0)*cj3*new_r00))));
2951sj5array[0]=IKsin(j5array[0]);
2952cj5array[0]=IKcos(j5array[0]);
2953if( j5array[0] > IKPI )
2954{
2955 j5array[0]-=IK2PI;
2956}
2957else if( j5array[0] < -IKPI )
2958{ j5array[0]+=IK2PI;
2959}
2960j5valid[0] = true;
2961for(int ij5 = 0; ij5 < 1; ++ij5)
2962{
2963if( !j5valid[ij5] )
2964{
2965 continue;
2966}
2967_ij5[0] = ij5; _ij5[1] = -1;
2968for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2969{
2970if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2971{
2972 j5valid[iij5]=false; _ij5[1] = iij5; break;
2973}
2974}
2975j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2976{
2977IkReal evalcond[8];
2978IkReal x222=IKcos(j5);
2979IkReal x223=IKsin(j5);
2980IkReal x224=((1.0)*cj3);
2981IkReal x225=(sj3*x222);
2982IkReal x226=((1.0)*x223);
2983IkReal x227=(x223*x224);
2984evalcond[0]=(((new_r10*sj3))+x222+((cj3*new_r00)));
2985evalcond[1]=((((-1.0)*new_r10*x224))+((new_r00*sj3))+x223);
2986evalcond[2]=((((-1.0)*new_r11*x224))+((new_r01*sj3))+x222);
2987evalcond[3]=(((new_r11*sj3))+((cj3*new_r01))+(((-1.0)*x226)));
2988evalcond[4]=(((cj3*x222))+((sj3*x223))+new_r00);
2989evalcond[5]=(x225+new_r01+(((-1.0)*x227)));
2990evalcond[6]=(x225+new_r10+(((-1.0)*x227)));
2991evalcond[7]=((((-1.0)*x222*x224))+(((-1.0)*sj3*x226))+new_r11);
2992if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
2993{
2994continue;
2995}
2996}
2997
2998{
2999std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3000vinfos[0].jointtype = 1;
3001vinfos[0].foffset = j0;
3002vinfos[0].indices[0] = _ij0[0];
3003vinfos[0].indices[1] = _ij0[1];
3004vinfos[0].maxsolutions = _nj0;
3005vinfos[1].jointtype = 1;
3006vinfos[1].foffset = j1;
3007vinfos[1].indices[0] = _ij1[0];
3008vinfos[1].indices[1] = _ij1[1];
3009vinfos[1].maxsolutions = _nj1;
3010vinfos[2].jointtype = 1;
3011vinfos[2].foffset = j2;
3012vinfos[2].indices[0] = _ij2[0];
3013vinfos[2].indices[1] = _ij2[1];
3014vinfos[2].maxsolutions = _nj2;
3015vinfos[3].jointtype = 1;
3016vinfos[3].foffset = j3;
3017vinfos[3].indices[0] = _ij3[0];
3018vinfos[3].indices[1] = _ij3[1];
3019vinfos[3].maxsolutions = _nj3;
3020vinfos[4].jointtype = 1;
3021vinfos[4].foffset = j4;
3022vinfos[4].indices[0] = _ij4[0];
3023vinfos[4].indices[1] = _ij4[1];
3024vinfos[4].maxsolutions = _nj4;
3025vinfos[5].jointtype = 1;
3026vinfos[5].foffset = j5;
3027vinfos[5].indices[0] = _ij5[0];
3028vinfos[5].indices[1] = _ij5[1];
3029vinfos[5].maxsolutions = _nj5;
3030std::vector<int> vfree(0);
3031solutions.AddSolution(vinfos,vfree);
3032}
3033}
3034}
3035
3036}
3037} while(0);
3038if( bgotonextstatement )
3039{
3040bool bgotonextstatement = true;
3041do
3042{
3043evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
3044evalcond[1]=new_r02;
3045if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3046{
3047bgotonextstatement=false;
3048{
3049IkReal j5array[1], cj5array[1], sj5array[1];
3050bool j5valid[1]={false};
3051_nj5 = 1;
3052if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
3053 continue;
3054j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
3055sj5array[0]=IKsin(j5array[0]);
3056cj5array[0]=IKcos(j5array[0]);
3057if( j5array[0] > IKPI )
3058{
3059 j5array[0]-=IK2PI;
3060}
3061else if( j5array[0] < -IKPI )
3062{ j5array[0]+=IK2PI;
3063}
3064j5valid[0] = true;
3065for(int ij5 = 0; ij5 < 1; ++ij5)
3066{
3067if( !j5valid[ij5] )
3068{
3069 continue;
3070}
3071_ij5[0] = ij5; _ij5[1] = -1;
3072for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3073{
3074if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3075{
3076 j5valid[iij5]=false; _ij5[1] = iij5; break;
3077}
3078}
3079j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3080{
3081IkReal evalcond[8];
3082IkReal x228=IKsin(j5);
3083IkReal x229=IKcos(j5);
3084IkReal x230=((1.0)*cj4);
3085IkReal x231=((1.0)*sj4);
3086evalcond[0]=(x228+new_r00);
3087evalcond[1]=(x229+new_r01);
3088evalcond[2]=(((sj4*x228))+new_r21);
3089evalcond[3]=(((cj4*x228))+new_r11);
3090evalcond[4]=(new_r20+(((-1.0)*x229*x231)));
3091evalcond[5]=(new_r10+(((-1.0)*x229*x230)));
3092evalcond[6]=((((-1.0)*new_r20*x231))+x229+(((-1.0)*new_r10*x230)));
3093evalcond[7]=((((-1.0)*new_r21*x231))+(((-1.0)*new_r11*x230))+(((-1.0)*x228)));
3094if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3095{
3096continue;
3097}
3098}
3099
3100{
3101std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3102vinfos[0].jointtype = 1;
3103vinfos[0].foffset = j0;
3104vinfos[0].indices[0] = _ij0[0];
3105vinfos[0].indices[1] = _ij0[1];
3106vinfos[0].maxsolutions = _nj0;
3107vinfos[1].jointtype = 1;
3108vinfos[1].foffset = j1;
3109vinfos[1].indices[0] = _ij1[0];
3110vinfos[1].indices[1] = _ij1[1];
3111vinfos[1].maxsolutions = _nj1;
3112vinfos[2].jointtype = 1;
3113vinfos[2].foffset = j2;
3114vinfos[2].indices[0] = _ij2[0];
3115vinfos[2].indices[1] = _ij2[1];
3116vinfos[2].maxsolutions = _nj2;
3117vinfos[3].jointtype = 1;
3118vinfos[3].foffset = j3;
3119vinfos[3].indices[0] = _ij3[0];
3120vinfos[3].indices[1] = _ij3[1];
3121vinfos[3].maxsolutions = _nj3;
3122vinfos[4].jointtype = 1;
3123vinfos[4].foffset = j4;
3124vinfos[4].indices[0] = _ij4[0];
3125vinfos[4].indices[1] = _ij4[1];
3126vinfos[4].maxsolutions = _nj4;
3127vinfos[5].jointtype = 1;
3128vinfos[5].foffset = j5;
3129vinfos[5].indices[0] = _ij5[0];
3130vinfos[5].indices[1] = _ij5[1];
3131vinfos[5].maxsolutions = _nj5;
3132std::vector<int> vfree(0);
3133solutions.AddSolution(vinfos,vfree);
3134}
3135}
3136}
3137
3138}
3139} while(0);
3140if( bgotonextstatement )
3141{
3142bool bgotonextstatement = true;
3143do
3144{
3145evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
3146evalcond[1]=new_r02;
3147if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3148{
3149bgotonextstatement=false;
3150{
3151IkReal j5array[1], cj5array[1], sj5array[1];
3152bool j5valid[1]={false};
3153_nj5 = 1;
3154if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
3155 continue;
3156j5array[0]=IKatan2(new_r00, new_r01);
3157sj5array[0]=IKsin(j5array[0]);
3158cj5array[0]=IKcos(j5array[0]);
3159if( j5array[0] > IKPI )
3160{
3161 j5array[0]-=IK2PI;
3162}
3163else if( j5array[0] < -IKPI )
3164{ j5array[0]+=IK2PI;
3165}
3166j5valid[0] = true;
3167for(int ij5 = 0; ij5 < 1; ++ij5)
3168{
3169if( !j5valid[ij5] )
3170{
3171 continue;
3172}
3173_ij5[0] = ij5; _ij5[1] = -1;
3174for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3175{
3176if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3177{
3178 j5valid[iij5]=false; _ij5[1] = iij5; break;
3179}
3180}
3181j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3182{
3183IkReal evalcond[8];
3184IkReal x232=IKsin(j5);
3185IkReal x233=IKcos(j5);
3186IkReal x234=((1.0)*sj4);
3187IkReal x235=((1.0)*x233);
3188evalcond[0]=(((sj4*x232))+new_r21);
3189evalcond[1]=(x232+(((-1.0)*new_r00)));
3190evalcond[2]=(x233+(((-1.0)*new_r01)));
3191evalcond[3]=((((-1.0)*x233*x234))+new_r20);
3192evalcond[4]=(((cj4*x232))+(((-1.0)*new_r11)));
3193evalcond[5]=((((-1.0)*cj4*x235))+(((-1.0)*new_r10)));
3194evalcond[6]=((((-1.0)*new_r20*x234))+((cj4*new_r10))+x233);
3195evalcond[7]=((((-1.0)*new_r21*x234))+((cj4*new_r11))+(((-1.0)*x232)));
3196if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3197{
3198continue;
3199}
3200}
3201
3202{
3203std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3204vinfos[0].jointtype = 1;
3205vinfos[0].foffset = j0;
3206vinfos[0].indices[0] = _ij0[0];
3207vinfos[0].indices[1] = _ij0[1];
3208vinfos[0].maxsolutions = _nj0;
3209vinfos[1].jointtype = 1;
3210vinfos[1].foffset = j1;
3211vinfos[1].indices[0] = _ij1[0];
3212vinfos[1].indices[1] = _ij1[1];
3213vinfos[1].maxsolutions = _nj1;
3214vinfos[2].jointtype = 1;
3215vinfos[2].foffset = j2;
3216vinfos[2].indices[0] = _ij2[0];
3217vinfos[2].indices[1] = _ij2[1];
3218vinfos[2].maxsolutions = _nj2;
3219vinfos[3].jointtype = 1;
3220vinfos[3].foffset = j3;
3221vinfos[3].indices[0] = _ij3[0];
3222vinfos[3].indices[1] = _ij3[1];
3223vinfos[3].maxsolutions = _nj3;
3224vinfos[4].jointtype = 1;
3225vinfos[4].foffset = j4;
3226vinfos[4].indices[0] = _ij4[0];
3227vinfos[4].indices[1] = _ij4[1];
3228vinfos[4].maxsolutions = _nj4;
3229vinfos[5].jointtype = 1;
3230vinfos[5].foffset = j5;
3231vinfos[5].indices[0] = _ij5[0];
3232vinfos[5].indices[1] = _ij5[1];
3233vinfos[5].maxsolutions = _nj5;
3234std::vector<int> vfree(0);
3235solutions.AddSolution(vinfos,vfree);
3236}
3237}
3238}
3239
3240}
3241} while(0);
3242if( bgotonextstatement )
3243{
3244bool bgotonextstatement = true;
3245do
3246{
3247evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
3248evalcond[1]=new_r12;
3249if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3250{
3251bgotonextstatement=false;
3252{
3253IkReal j5array[1], cj5array[1], sj5array[1];
3254bool j5valid[1]={false};
3255_nj5 = 1;
3256if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
3257 continue;
3258j5array[0]=IKatan2(new_r10, new_r11);
3259sj5array[0]=IKsin(j5array[0]);
3260cj5array[0]=IKcos(j5array[0]);
3261if( j5array[0] > IKPI )
3262{
3263 j5array[0]-=IK2PI;
3264}
3265else if( j5array[0] < -IKPI )
3266{ j5array[0]+=IK2PI;
3267}
3268j5valid[0] = true;
3269for(int ij5 = 0; ij5 < 1; ++ij5)
3270{
3271if( !j5valid[ij5] )
3272{
3273 continue;
3274}
3275_ij5[0] = ij5; _ij5[1] = -1;
3276for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3277{
3278if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3279{
3280 j5valid[iij5]=false; _ij5[1] = iij5; break;
3281}
3282}
3283j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3284{
3285IkReal evalcond[8];
3286IkReal x236=IKcos(j5);
3287IkReal x237=IKsin(j5);
3288IkReal x238=((1.0)*cj4);
3289IkReal x239=((1.0)*sj4);
3290IkReal x240=((1.0)*x237);
3291evalcond[0]=(((new_r02*x236))+new_r20);
3292evalcond[1]=(x237+(((-1.0)*new_r10)));
3293evalcond[2]=(x236+(((-1.0)*new_r11)));
3294evalcond[3]=(((cj4*x237))+new_r01);
3295evalcond[4]=((((-1.0)*new_r02*x240))+new_r21);
3296evalcond[5]=((((-1.0)*x236*x238))+new_r00);
3297evalcond[6]=((((-1.0)*new_r20*x239))+x236+(((-1.0)*new_r00*x238)));
3298evalcond[7]=((((-1.0)*new_r21*x239))+(((-1.0)*x240))+(((-1.0)*new_r01*x238)));
3299if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3300{
3301continue;
3302}
3303}
3304
3305{
3306std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3307vinfos[0].jointtype = 1;
3308vinfos[0].foffset = j0;
3309vinfos[0].indices[0] = _ij0[0];
3310vinfos[0].indices[1] = _ij0[1];
3311vinfos[0].maxsolutions = _nj0;
3312vinfos[1].jointtype = 1;
3313vinfos[1].foffset = j1;
3314vinfos[1].indices[0] = _ij1[0];
3315vinfos[1].indices[1] = _ij1[1];
3316vinfos[1].maxsolutions = _nj1;
3317vinfos[2].jointtype = 1;
3318vinfos[2].foffset = j2;
3319vinfos[2].indices[0] = _ij2[0];
3320vinfos[2].indices[1] = _ij2[1];
3321vinfos[2].maxsolutions = _nj2;
3322vinfos[3].jointtype = 1;
3323vinfos[3].foffset = j3;
3324vinfos[3].indices[0] = _ij3[0];
3325vinfos[3].indices[1] = _ij3[1];
3326vinfos[3].maxsolutions = _nj3;
3327vinfos[4].jointtype = 1;
3328vinfos[4].foffset = j4;
3329vinfos[4].indices[0] = _ij4[0];
3330vinfos[4].indices[1] = _ij4[1];
3331vinfos[4].maxsolutions = _nj4;
3332vinfos[5].jointtype = 1;
3333vinfos[5].foffset = j5;
3334vinfos[5].indices[0] = _ij5[0];
3335vinfos[5].indices[1] = _ij5[1];
3336vinfos[5].maxsolutions = _nj5;
3337std::vector<int> vfree(0);
3338solutions.AddSolution(vinfos,vfree);
3339}
3340}
3341}
3342
3343}
3344} while(0);
3345if( bgotonextstatement )
3346{
3347bool bgotonextstatement = true;
3348do
3349{
3350evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
3351evalcond[1]=new_r12;
3352if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
3353{
3354bgotonextstatement=false;
3355{
3356IkReal j5array[1], cj5array[1], sj5array[1];
3357bool j5valid[1]={false};
3358_nj5 = 1;
3359if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
3360 continue;
3361j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
3362sj5array[0]=IKsin(j5array[0]);
3363cj5array[0]=IKcos(j5array[0]);
3364if( j5array[0] > IKPI )
3365{
3366 j5array[0]-=IK2PI;
3367}
3368else if( j5array[0] < -IKPI )
3369{ j5array[0]+=IK2PI;
3370}
3371j5valid[0] = true;
3372for(int ij5 = 0; ij5 < 1; ++ij5)
3373{
3374if( !j5valid[ij5] )
3375{
3376 continue;
3377}
3378_ij5[0] = ij5; _ij5[1] = -1;
3379for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3380{
3381if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3382{
3383 j5valid[iij5]=false; _ij5[1] = iij5; break;
3384}
3385}
3386j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3387{
3388IkReal evalcond[8];
3389IkReal x241=IKsin(j5);
3390IkReal x242=IKcos(j5);
3391IkReal x243=((1.0)*sj4);
3392IkReal x244=((1.0)*x242);
3393evalcond[0]=(x241+new_r10);
3394evalcond[1]=(x242+new_r11);
3395evalcond[2]=(new_r21+((new_r02*x241)));
3396evalcond[3]=((((-1.0)*new_r02*x244))+new_r20);
3397evalcond[4]=(((cj4*x241))+(((-1.0)*new_r01)));
3398evalcond[5]=((((-1.0)*cj4*x244))+(((-1.0)*new_r00)));
3399evalcond[6]=(((cj4*new_r00))+x242+(((-1.0)*new_r20*x243)));
3400evalcond[7]=(((cj4*new_r01))+(((-1.0)*x241))+(((-1.0)*new_r21*x243)));
3401if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3402{
3403continue;
3404}
3405}
3406
3407{
3408std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3409vinfos[0].jointtype = 1;
3410vinfos[0].foffset = j0;
3411vinfos[0].indices[0] = _ij0[0];
3412vinfos[0].indices[1] = _ij0[1];
3413vinfos[0].maxsolutions = _nj0;
3414vinfos[1].jointtype = 1;
3415vinfos[1].foffset = j1;
3416vinfos[1].indices[0] = _ij1[0];
3417vinfos[1].indices[1] = _ij1[1];
3418vinfos[1].maxsolutions = _nj1;
3419vinfos[2].jointtype = 1;
3420vinfos[2].foffset = j2;
3421vinfos[2].indices[0] = _ij2[0];
3422vinfos[2].indices[1] = _ij2[1];
3423vinfos[2].maxsolutions = _nj2;
3424vinfos[3].jointtype = 1;
3425vinfos[3].foffset = j3;
3426vinfos[3].indices[0] = _ij3[0];
3427vinfos[3].indices[1] = _ij3[1];
3428vinfos[3].maxsolutions = _nj3;
3429vinfos[4].jointtype = 1;
3430vinfos[4].foffset = j4;
3431vinfos[4].indices[0] = _ij4[0];
3432vinfos[4].indices[1] = _ij4[1];
3433vinfos[4].maxsolutions = _nj4;
3434vinfos[5].jointtype = 1;
3435vinfos[5].foffset = j5;
3436vinfos[5].indices[0] = _ij5[0];
3437vinfos[5].indices[1] = _ij5[1];
3438vinfos[5].maxsolutions = _nj5;
3439std::vector<int> vfree(0);
3440solutions.AddSolution(vinfos,vfree);
3441}
3442}
3443}
3444
3445}
3446} while(0);
3447if( bgotonextstatement )
3448{
3449bool bgotonextstatement = true;
3450do
3451{
3452evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
3453if( IKabs(evalcond[0]) < 0.0000050000000000 )
3454{
3455bgotonextstatement=false;
3456{
3457IkReal j5eval[1];
3458new_r21=0;
3459new_r20=0;
3460new_r02=0;
3461new_r12=0;
3462j5eval[0]=1.0;
3463if( IKabs(j5eval[0]) < 0.0000000100000000 )
3464{
3465continue; // no branches [j5]
3466
3467} else
3468{
3469IkReal op[2+1], zeror[2];
3470int numroots;
3471op[0]=-1.0;
3472op[1]=0;
3473op[2]=1.0;
3474polyroots2(op,zeror,numroots);
3475IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
3476int numsolutions = 0;
3477for(int ij5 = 0; ij5 < numroots; ++ij5)
3478{
3479IkReal htj5 = zeror[ij5];
3480tempj5array[0]=((2.0)*(atan(htj5)));
3481for(int kj5 = 0; kj5 < 1; ++kj5)
3482{
3483j5array[numsolutions] = tempj5array[kj5];
3484if( j5array[numsolutions] > IKPI )
3485{
3486 j5array[numsolutions]-=IK2PI;
3487}
3488else if( j5array[numsolutions] < -IKPI )
3489{
3490 j5array[numsolutions]+=IK2PI;
3491}
3492sj5array[numsolutions] = IKsin(j5array[numsolutions]);
3493cj5array[numsolutions] = IKcos(j5array[numsolutions]);
3494numsolutions++;
3495}
3496}
3497bool j5valid[2]={true,true};
3498_nj5 = 2;
3499for(int ij5 = 0; ij5 < numsolutions; ++ij5)
3500 {
3501if( !j5valid[ij5] )
3502{
3503 continue;
3504}
3505 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3506htj5 = IKtan(j5/2);
3507
3508_ij5[0] = ij5; _ij5[1] = -1;
3509for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
3510{
3511if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3512{
3513 j5valid[iij5]=false; _ij5[1] = iij5; break;
3514}
3515}
3516{
3517std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3518vinfos[0].jointtype = 1;
3519vinfos[0].foffset = j0;
3520vinfos[0].indices[0] = _ij0[0];
3521vinfos[0].indices[1] = _ij0[1];
3522vinfos[0].maxsolutions = _nj0;
3523vinfos[1].jointtype = 1;
3524vinfos[1].foffset = j1;
3525vinfos[1].indices[0] = _ij1[0];
3526vinfos[1].indices[1] = _ij1[1];
3527vinfos[1].maxsolutions = _nj1;
3528vinfos[2].jointtype = 1;
3529vinfos[2].foffset = j2;
3530vinfos[2].indices[0] = _ij2[0];
3531vinfos[2].indices[1] = _ij2[1];
3532vinfos[2].maxsolutions = _nj2;
3533vinfos[3].jointtype = 1;
3534vinfos[3].foffset = j3;
3535vinfos[3].indices[0] = _ij3[0];
3536vinfos[3].indices[1] = _ij3[1];
3537vinfos[3].maxsolutions = _nj3;
3538vinfos[4].jointtype = 1;
3539vinfos[4].foffset = j4;
3540vinfos[4].indices[0] = _ij4[0];
3541vinfos[4].indices[1] = _ij4[1];
3542vinfos[4].maxsolutions = _nj4;
3543vinfos[5].jointtype = 1;
3544vinfos[5].foffset = j5;
3545vinfos[5].indices[0] = _ij5[0];
3546vinfos[5].indices[1] = _ij5[1];
3547vinfos[5].maxsolutions = _nj5;
3548std::vector<int> vfree(0);
3549solutions.AddSolution(vinfos,vfree);
3550}
3551 }
3552
3553}
3554
3555}
3556
3557}
3558} while(0);
3559if( bgotonextstatement )
3560{
3561bool bgotonextstatement = true;
3562do
3563{
3564if( 1 )
3565{
3566bgotonextstatement=false;
3567continue; // branch miss [j5]
3568
3569}
3570} while(0);
3571if( bgotonextstatement )
3572{
3573}
3574}
3575}
3576}
3577}
3578}
3579}
3580}
3581}
3582
3583} else
3584{
3585{
3586IkReal j5array[1], cj5array[1], sj5array[1];
3587bool j5valid[1]={false};
3588_nj5 = 1;
3589CheckValue<IkReal> x246=IKPowWithIntegerCheck(sj4,-1);
3590if(!x246.valid){
3591continue;
3592}
3593IkReal x245=x246.value;
3594CheckValue<IkReal> x247=IKPowWithIntegerCheck(cj3,-1);
3595if(!x247.valid){
3596continue;
3597}
3598if( IKabs(((-1.0)*new_r21*x245)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x245))+IKsqr((x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
3599 continue;
3600j5array[0]=IKatan2(((-1.0)*new_r21*x245), (x245*(x247.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
3601sj5array[0]=IKsin(j5array[0]);
3602cj5array[0]=IKcos(j5array[0]);
3603if( j5array[0] > IKPI )
3604{
3605 j5array[0]-=IK2PI;
3606}
3607else if( j5array[0] < -IKPI )
3608{ j5array[0]+=IK2PI;
3609}
3610j5valid[0] = true;
3611for(int ij5 = 0; ij5 < 1; ++ij5)
3612{
3613if( !j5valid[ij5] )
3614{
3615 continue;
3616}
3617_ij5[0] = ij5; _ij5[1] = -1;
3618for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3619{
3620if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3621{
3622 j5valid[iij5]=false; _ij5[1] = iij5; break;
3623}
3624}
3625j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3626{
3627IkReal evalcond[12];
3628IkReal x248=IKsin(j5);
3629IkReal x249=IKcos(j5);
3630IkReal x250=(cj3*new_r00);
3631IkReal x251=(cj3*cj4);
3632IkReal x252=((1.0)*cj3);
3633IkReal x253=(new_r11*sj3);
3634IkReal x254=((1.0)*cj4);
3635IkReal x255=(new_r10*sj3);
3636IkReal x256=((1.0)*sj4);
3637IkReal x257=(sj3*x248);
3638IkReal x258=((1.0)*x249);
3639IkReal x259=(sj3*x249);
3640evalcond[0]=(new_r21+((sj4*x248)));
3641evalcond[1]=((((-1.0)*x249*x256))+new_r20);
3642evalcond[2]=((((-1.0)*new_r10*x252))+((new_r00*sj3))+x248);
3643evalcond[3]=((((-1.0)*new_r11*x252))+((new_r01*sj3))+x249);
3644evalcond[4]=(((cj4*x248))+x253+((cj3*new_r01)));
3645evalcond[5]=(((x248*x251))+x259+new_r01);
3646evalcond[6]=(x255+x250+(((-1.0)*x249*x254)));
3647evalcond[7]=((((-1.0)*x251*x258))+x257+new_r00);
3648evalcond[8]=((((-1.0)*x249*x252))+new_r11+((cj4*x257)));
3649evalcond[9]=((((-1.0)*x254*x259))+(((-1.0)*x248*x252))+new_r10);
3650evalcond[10]=((((-1.0)*x250*x254))+(((-1.0)*x254*x255))+x249+(((-1.0)*new_r20*x256)));
3651evalcond[11]=((((-1.0)*new_r21*x256))+(((-1.0)*x248))+(((-1.0)*x253*x254))+(((-1.0)*new_r01*x251)));
3652if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
3653{
3654continue;
3655}
3656}
3657
3658{
3659std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3660vinfos[0].jointtype = 1;
3661vinfos[0].foffset = j0;
3662vinfos[0].indices[0] = _ij0[0];
3663vinfos[0].indices[1] = _ij0[1];
3664vinfos[0].maxsolutions = _nj0;
3665vinfos[1].jointtype = 1;
3666vinfos[1].foffset = j1;
3667vinfos[1].indices[0] = _ij1[0];
3668vinfos[1].indices[1] = _ij1[1];
3669vinfos[1].maxsolutions = _nj1;
3670vinfos[2].jointtype = 1;
3671vinfos[2].foffset = j2;
3672vinfos[2].indices[0] = _ij2[0];
3673vinfos[2].indices[1] = _ij2[1];
3674vinfos[2].maxsolutions = _nj2;
3675vinfos[3].jointtype = 1;
3676vinfos[3].foffset = j3;
3677vinfos[3].indices[0] = _ij3[0];
3678vinfos[3].indices[1] = _ij3[1];
3679vinfos[3].maxsolutions = _nj3;
3680vinfos[4].jointtype = 1;
3681vinfos[4].foffset = j4;
3682vinfos[4].indices[0] = _ij4[0];
3683vinfos[4].indices[1] = _ij4[1];
3684vinfos[4].maxsolutions = _nj4;
3685vinfos[5].jointtype = 1;
3686vinfos[5].foffset = j5;
3687vinfos[5].indices[0] = _ij5[0];
3688vinfos[5].indices[1] = _ij5[1];
3689vinfos[5].maxsolutions = _nj5;
3690std::vector<int> vfree(0);
3691solutions.AddSolution(vinfos,vfree);
3692}
3693}
3694}
3695
3696}
3697
3698}
3699
3700} else
3701{
3702{
3703IkReal j5array[1], cj5array[1], sj5array[1];
3704bool j5valid[1]={false};
3705_nj5 = 1;
3706CheckValue<IkReal> x261=IKPowWithIntegerCheck(sj4,-1);
3707if(!x261.valid){
3708continue;
3709}
3710IkReal x260=x261.value;
3711CheckValue<IkReal> x262=IKPowWithIntegerCheck(sj3,-1);
3712if(!x262.valid){
3713continue;
3714}
3715if( IKabs(((-1.0)*new_r21*x260)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x260))+IKsqr((x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
3716 continue;
3717j5array[0]=IKatan2(((-1.0)*new_r21*x260), (x260*(x262.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
3718sj5array[0]=IKsin(j5array[0]);
3719cj5array[0]=IKcos(j5array[0]);
3720if( j5array[0] > IKPI )
3721{
3722 j5array[0]-=IK2PI;
3723}
3724else if( j5array[0] < -IKPI )
3725{ j5array[0]+=IK2PI;
3726}
3727j5valid[0] = true;
3728for(int ij5 = 0; ij5 < 1; ++ij5)
3729{
3730if( !j5valid[ij5] )
3731{
3732 continue;
3733}
3734_ij5[0] = ij5; _ij5[1] = -1;
3735for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3736{
3737if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3738{
3739 j5valid[iij5]=false; _ij5[1] = iij5; break;
3740}
3741}
3742j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3743{
3744IkReal evalcond[12];
3745IkReal x263=IKsin(j5);
3746IkReal x264=IKcos(j5);
3747IkReal x265=(cj3*new_r00);
3748IkReal x266=(cj3*cj4);
3749IkReal x267=((1.0)*cj3);
3750IkReal x268=(new_r11*sj3);
3751IkReal x269=((1.0)*cj4);
3752IkReal x270=(new_r10*sj3);
3753IkReal x271=((1.0)*sj4);
3754IkReal x272=(sj3*x263);
3755IkReal x273=((1.0)*x264);
3756IkReal x274=(sj3*x264);
3757evalcond[0]=(new_r21+((sj4*x263)));
3758evalcond[1]=(new_r20+(((-1.0)*x264*x271)));
3759evalcond[2]=(((new_r00*sj3))+x263+(((-1.0)*new_r10*x267)));
3760evalcond[3]=(((new_r01*sj3))+x264+(((-1.0)*new_r11*x267)));
3761evalcond[4]=(((cj4*x263))+x268+((cj3*new_r01)));
3762evalcond[5]=(((x263*x266))+x274+new_r01);
3763evalcond[6]=(x265+x270+(((-1.0)*x264*x269)));
3764evalcond[7]=(x272+(((-1.0)*x266*x273))+new_r00);
3765evalcond[8]=(((cj4*x272))+new_r11+(((-1.0)*x264*x267)));
3766evalcond[9]=((((-1.0)*x263*x267))+(((-1.0)*x269*x274))+new_r10);
3767evalcond[10]=((((-1.0)*x269*x270))+x264+(((-1.0)*new_r20*x271))+(((-1.0)*x265*x269)));
3768evalcond[11]=((((-1.0)*x263))+(((-1.0)*new_r01*x266))+(((-1.0)*new_r21*x271))+(((-1.0)*x268*x269)));
3769if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
3770{
3771continue;
3772}
3773}
3774
3775{
3776std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3777vinfos[0].jointtype = 1;
3778vinfos[0].foffset = j0;
3779vinfos[0].indices[0] = _ij0[0];
3780vinfos[0].indices[1] = _ij0[1];
3781vinfos[0].maxsolutions = _nj0;
3782vinfos[1].jointtype = 1;
3783vinfos[1].foffset = j1;
3784vinfos[1].indices[0] = _ij1[0];
3785vinfos[1].indices[1] = _ij1[1];
3786vinfos[1].maxsolutions = _nj1;
3787vinfos[2].jointtype = 1;
3788vinfos[2].foffset = j2;
3789vinfos[2].indices[0] = _ij2[0];
3790vinfos[2].indices[1] = _ij2[1];
3791vinfos[2].maxsolutions = _nj2;
3792vinfos[3].jointtype = 1;
3793vinfos[3].foffset = j3;
3794vinfos[3].indices[0] = _ij3[0];
3795vinfos[3].indices[1] = _ij3[1];
3796vinfos[3].maxsolutions = _nj3;
3797vinfos[4].jointtype = 1;
3798vinfos[4].foffset = j4;
3799vinfos[4].indices[0] = _ij4[0];
3800vinfos[4].indices[1] = _ij4[1];
3801vinfos[4].maxsolutions = _nj4;
3802vinfos[5].jointtype = 1;
3803vinfos[5].foffset = j5;
3804vinfos[5].indices[0] = _ij5[0];
3805vinfos[5].indices[1] = _ij5[1];
3806vinfos[5].maxsolutions = _nj5;
3807std::vector<int> vfree(0);
3808solutions.AddSolution(vinfos,vfree);
3809}
3810}
3811}
3812
3813}
3814
3815}
3816
3817} else
3818{
3819{
3820IkReal j5array[1], cj5array[1], sj5array[1];
3821bool j5valid[1]={false};
3822_nj5 = 1;
3823CheckValue<IkReal> x275 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
3824if(!x275.valid){
3825continue;
3826}
3827CheckValue<IkReal> x276=IKPowWithIntegerCheck(IKsign(sj4),-1);
3828if(!x276.valid){
3829continue;
3830}
3831j5array[0]=((-1.5707963267949)+(x275.value)+(((1.5707963267949)*(x276.value))));
3832sj5array[0]=IKsin(j5array[0]);
3833cj5array[0]=IKcos(j5array[0]);
3834if( j5array[0] > IKPI )
3835{
3836 j5array[0]-=IK2PI;
3837}
3838else if( j5array[0] < -IKPI )
3839{ j5array[0]+=IK2PI;
3840}
3841j5valid[0] = true;
3842for(int ij5 = 0; ij5 < 1; ++ij5)
3843{
3844if( !j5valid[ij5] )
3845{
3846 continue;
3847}
3848_ij5[0] = ij5; _ij5[1] = -1;
3849for(int iij5 = ij5+1; iij5 < 1; ++iij5)
3850{
3851if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
3852{
3853 j5valid[iij5]=false; _ij5[1] = iij5; break;
3854}
3855}
3856j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
3857{
3858IkReal evalcond[12];
3859IkReal x277=IKsin(j5);
3860IkReal x278=IKcos(j5);
3861IkReal x279=(cj3*new_r00);
3862IkReal x280=(cj3*cj4);
3863IkReal x281=((1.0)*cj3);
3864IkReal x282=(new_r11*sj3);
3865IkReal x283=((1.0)*cj4);
3866IkReal x284=(new_r10*sj3);
3867IkReal x285=((1.0)*sj4);
3868IkReal x286=(sj3*x277);
3869IkReal x287=((1.0)*x278);
3870IkReal x288=(sj3*x278);
3871evalcond[0]=(new_r21+((sj4*x277)));
3872evalcond[1]=(new_r20+(((-1.0)*x278*x285)));
3873evalcond[2]=(((new_r00*sj3))+x277+(((-1.0)*new_r10*x281)));
3874evalcond[3]=(((new_r01*sj3))+x278+(((-1.0)*new_r11*x281)));
3875evalcond[4]=(((cj4*x277))+x282+((cj3*new_r01)));
3876evalcond[5]=(x288+new_r01+((x277*x280)));
3877evalcond[6]=(x279+x284+(((-1.0)*x278*x283)));
3878evalcond[7]=((((-1.0)*x280*x287))+x286+new_r00);
3879evalcond[8]=(new_r11+((cj4*x286))+(((-1.0)*x278*x281)));
3880evalcond[9]=((((-1.0)*x277*x281))+new_r10+(((-1.0)*x283*x288)));
3881evalcond[10]=(x278+(((-1.0)*new_r20*x285))+(((-1.0)*x279*x283))+(((-1.0)*x283*x284)));
3882evalcond[11]=((((-1.0)*x277))+(((-1.0)*x282*x283))+(((-1.0)*new_r01*x280))+(((-1.0)*new_r21*x285)));
3883if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
3884{
3885continue;
3886}
3887}
3888
3889{
3890std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3891vinfos[0].jointtype = 1;
3892vinfos[0].foffset = j0;
3893vinfos[0].indices[0] = _ij0[0];
3894vinfos[0].indices[1] = _ij0[1];
3895vinfos[0].maxsolutions = _nj0;
3896vinfos[1].jointtype = 1;
3897vinfos[1].foffset = j1;
3898vinfos[1].indices[0] = _ij1[0];
3899vinfos[1].indices[1] = _ij1[1];
3900vinfos[1].maxsolutions = _nj1;
3901vinfos[2].jointtype = 1;
3902vinfos[2].foffset = j2;
3903vinfos[2].indices[0] = _ij2[0];
3904vinfos[2].indices[1] = _ij2[1];
3905vinfos[2].maxsolutions = _nj2;
3906vinfos[3].jointtype = 1;
3907vinfos[3].foffset = j3;
3908vinfos[3].indices[0] = _ij3[0];
3909vinfos[3].indices[1] = _ij3[1];
3910vinfos[3].maxsolutions = _nj3;
3911vinfos[4].jointtype = 1;
3912vinfos[4].foffset = j4;
3913vinfos[4].indices[0] = _ij4[0];
3914vinfos[4].indices[1] = _ij4[1];
3915vinfos[4].maxsolutions = _nj4;
3916vinfos[5].jointtype = 1;
3917vinfos[5].foffset = j5;
3918vinfos[5].indices[0] = _ij5[0];
3919vinfos[5].indices[1] = _ij5[1];
3920vinfos[5].maxsolutions = _nj5;
3921std::vector<int> vfree(0);
3922solutions.AddSolution(vinfos,vfree);
3923}
3924}
3925}
3926
3927}
3928
3929}
3930}
3931}
3932
3933}
3934
3935}
3936
3937} else
3938{
3939{
3940IkReal j3array[1], cj3array[1], sj3array[1];
3941bool j3valid[1]={false};
3942_nj3 = 1;
3943CheckValue<IkReal> x289=IKPowWithIntegerCheck(IKsign(sj4),-1);
3944if(!x289.valid){
3945continue;
3946}
3947CheckValue<IkReal> x290 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
3948if(!x290.valid){
3949continue;
3950}
3951j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x289.value)))+(x290.value));
3952sj3array[0]=IKsin(j3array[0]);
3953cj3array[0]=IKcos(j3array[0]);
3954if( j3array[0] > IKPI )
3955{
3956 j3array[0]-=IK2PI;
3957}
3958else if( j3array[0] < -IKPI )
3959{ j3array[0]+=IK2PI;
3960}
3961j3valid[0] = true;
3962for(int ij3 = 0; ij3 < 1; ++ij3)
3963{
3964if( !j3valid[ij3] )
3965{
3966 continue;
3967}
3968_ij3[0] = ij3; _ij3[1] = -1;
3969for(int iij3 = ij3+1; iij3 < 1; ++iij3)
3970{
3971if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
3972{
3973 j3valid[iij3]=false; _ij3[1] = iij3; break;
3974}
3975}
3976j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
3977{
3978IkReal evalcond[8];
3979IkReal x291=IKcos(j3);
3980IkReal x292=IKsin(j3);
3981IkReal x293=((1.0)*cj4);
3982IkReal x294=(sj4*x292);
3983IkReal x295=(new_r12*x292);
3984IkReal x296=(sj4*x291);
3985IkReal x297=(new_r02*x291);
3986evalcond[0]=(x296+new_r02);
3987evalcond[1]=(x294+new_r12);
3988evalcond[2]=(((new_r12*x291))+(((-1.0)*new_r02*x292)));
3989evalcond[3]=(sj4+x295+x297);
3990evalcond[4]=((((-1.0)*new_r20*x293))+((new_r10*x294))+((new_r00*x296)));
3991evalcond[5]=((((-1.0)*new_r21*x293))+((new_r11*x294))+((new_r01*x296)));
3992evalcond[6]=((1.0)+((new_r02*x296))+((new_r12*x294))+(((-1.0)*new_r22*x293)));
3993evalcond[7]=((((-1.0)*x293*x297))+(((-1.0)*x293*x295))+(((-1.0)*new_r22*sj4)));
3994if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
3995{
3996continue;
3997}
3998}
3999
4000{
4001IkReal j5eval[3];
4002j5eval[0]=sj4;
4003j5eval[1]=IKsign(sj4);
4004j5eval[2]=((IKabs(new_r20))+(IKabs(new_r21)));
4005if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 || IKabs(j5eval[2]) < 0.0000010000000000 )
4006{
4007{
4008IkReal j5eval[2];
4009j5eval[0]=sj4;
4010j5eval[1]=sj3;
4011if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
4012{
4013{
4014IkReal j5eval[2];
4015j5eval[0]=sj4;
4016j5eval[1]=cj3;
4017if( IKabs(j5eval[0]) < 0.0000010000000000 || IKabs(j5eval[1]) < 0.0000010000000000 )
4018{
4019{
4020IkReal evalcond[5];
4021bool bgotonextstatement = true;
4022do
4023{
4024evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
4025evalcond[1]=new_r02;
4026evalcond[2]=new_r12;
4027evalcond[3]=new_r21;
4028evalcond[4]=new_r20;
4029if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
4030{
4031bgotonextstatement=false;
4032{
4033IkReal j5array[1], cj5array[1], sj5array[1];
4034bool j5valid[1]={false};
4035_nj5 = 1;
4036IkReal x298=((1.0)*new_r01);
4037if( IKabs(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*sj3*x298))+((cj3*new_r00)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3))))+IKsqr(((((-1.0)*sj3*x298))+((cj3*new_r00))))-1) <= IKFAST_SINCOS_THRESH )
4038 continue;
4039j5array[0]=IKatan2(((((-1.0)*cj3*x298))+(((-1.0)*new_r00*sj3))), ((((-1.0)*sj3*x298))+((cj3*new_r00))));
4040sj5array[0]=IKsin(j5array[0]);
4041cj5array[0]=IKcos(j5array[0]);
4042if( j5array[0] > IKPI )
4043{
4044 j5array[0]-=IK2PI;
4045}
4046else if( j5array[0] < -IKPI )
4047{ j5array[0]+=IK2PI;
4048}
4049j5valid[0] = true;
4050for(int ij5 = 0; ij5 < 1; ++ij5)
4051{
4052if( !j5valid[ij5] )
4053{
4054 continue;
4055}
4056_ij5[0] = ij5; _ij5[1] = -1;
4057for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4058{
4059if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4060{
4061 j5valid[iij5]=false; _ij5[1] = iij5; break;
4062}
4063}
4064j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4065{
4066IkReal evalcond[8];
4067IkReal x299=IKsin(j5);
4068IkReal x300=IKcos(j5);
4069IkReal x301=((1.0)*cj3);
4070IkReal x302=(sj3*x299);
4071IkReal x303=((1.0)*x300);
4072IkReal x304=(x300*x301);
4073evalcond[0]=(((new_r11*sj3))+x299+((cj3*new_r01)));
4074evalcond[1]=(((new_r00*sj3))+x299+(((-1.0)*new_r10*x301)));
4075evalcond[2]=(((new_r01*sj3))+x300+(((-1.0)*new_r11*x301)));
4076evalcond[3]=(((cj3*x299))+((sj3*x300))+new_r01);
4077evalcond[4]=(((new_r10*sj3))+((cj3*new_r00))+(((-1.0)*x303)));
4078evalcond[5]=(x302+new_r00+(((-1.0)*x304)));
4079evalcond[6]=(x302+new_r11+(((-1.0)*x304)));
4080evalcond[7]=((((-1.0)*sj3*x303))+(((-1.0)*x299*x301))+new_r10);
4081if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4082{
4083continue;
4084}
4085}
4086
4087{
4088std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4089vinfos[0].jointtype = 1;
4090vinfos[0].foffset = j0;
4091vinfos[0].indices[0] = _ij0[0];
4092vinfos[0].indices[1] = _ij0[1];
4093vinfos[0].maxsolutions = _nj0;
4094vinfos[1].jointtype = 1;
4095vinfos[1].foffset = j1;
4096vinfos[1].indices[0] = _ij1[0];
4097vinfos[1].indices[1] = _ij1[1];
4098vinfos[1].maxsolutions = _nj1;
4099vinfos[2].jointtype = 1;
4100vinfos[2].foffset = j2;
4101vinfos[2].indices[0] = _ij2[0];
4102vinfos[2].indices[1] = _ij2[1];
4103vinfos[2].maxsolutions = _nj2;
4104vinfos[3].jointtype = 1;
4105vinfos[3].foffset = j3;
4106vinfos[3].indices[0] = _ij3[0];
4107vinfos[3].indices[1] = _ij3[1];
4108vinfos[3].maxsolutions = _nj3;
4109vinfos[4].jointtype = 1;
4110vinfos[4].foffset = j4;
4111vinfos[4].indices[0] = _ij4[0];
4112vinfos[4].indices[1] = _ij4[1];
4113vinfos[4].maxsolutions = _nj4;
4114vinfos[5].jointtype = 1;
4115vinfos[5].foffset = j5;
4116vinfos[5].indices[0] = _ij5[0];
4117vinfos[5].indices[1] = _ij5[1];
4118vinfos[5].maxsolutions = _nj5;
4119std::vector<int> vfree(0);
4120solutions.AddSolution(vinfos,vfree);
4121}
4122}
4123}
4124
4125}
4126} while(0);
4127if( bgotonextstatement )
4128{
4129bool bgotonextstatement = true;
4130do
4131{
4132evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
4133evalcond[1]=new_r02;
4134evalcond[2]=new_r12;
4135evalcond[3]=new_r21;
4136evalcond[4]=new_r20;
4137if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
4138{
4139bgotonextstatement=false;
4140{
4141IkReal j5array[1], cj5array[1], sj5array[1];
4142bool j5valid[1]={false};
4143_nj5 = 1;
4144IkReal x305=((1.0)*sj3);
4145if( IKabs((((cj3*new_r01))+(((-1.0)*new_r00*x305)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((((cj3*new_r01))+(((-1.0)*new_r00*x305))))+IKsqr(((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305))))-1) <= IKFAST_SINCOS_THRESH )
4146 continue;
4147j5array[0]=IKatan2((((cj3*new_r01))+(((-1.0)*new_r00*x305))), ((((-1.0)*cj3*new_r00))+(((-1.0)*new_r01*x305))));
4148sj5array[0]=IKsin(j5array[0]);
4149cj5array[0]=IKcos(j5array[0]);
4150if( j5array[0] > IKPI )
4151{
4152 j5array[0]-=IK2PI;
4153}
4154else if( j5array[0] < -IKPI )
4155{ j5array[0]+=IK2PI;
4156}
4157j5valid[0] = true;
4158for(int ij5 = 0; ij5 < 1; ++ij5)
4159{
4160if( !j5valid[ij5] )
4161{
4162 continue;
4163}
4164_ij5[0] = ij5; _ij5[1] = -1;
4165for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4166{
4167if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4168{
4169 j5valid[iij5]=false; _ij5[1] = iij5; break;
4170}
4171}
4172j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4173{
4174IkReal evalcond[8];
4175IkReal x306=IKcos(j5);
4176IkReal x307=IKsin(j5);
4177IkReal x308=((1.0)*cj3);
4178IkReal x309=(sj3*x306);
4179IkReal x310=((1.0)*x307);
4180IkReal x311=(x307*x308);
4181evalcond[0]=(((new_r10*sj3))+x306+((cj3*new_r00)));
4182evalcond[1]=(((new_r00*sj3))+x307+(((-1.0)*new_r10*x308)));
4183evalcond[2]=(((new_r01*sj3))+x306+(((-1.0)*new_r11*x308)));
4184evalcond[3]=(((new_r11*sj3))+(((-1.0)*x310))+((cj3*new_r01)));
4185evalcond[4]=(((sj3*x307))+((cj3*x306))+new_r00);
4186evalcond[5]=(x309+(((-1.0)*x311))+new_r01);
4187evalcond[6]=(x309+(((-1.0)*x311))+new_r10);
4188evalcond[7]=((((-1.0)*sj3*x310))+(((-1.0)*x306*x308))+new_r11);
4189if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4190{
4191continue;
4192}
4193}
4194
4195{
4196std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4197vinfos[0].jointtype = 1;
4198vinfos[0].foffset = j0;
4199vinfos[0].indices[0] = _ij0[0];
4200vinfos[0].indices[1] = _ij0[1];
4201vinfos[0].maxsolutions = _nj0;
4202vinfos[1].jointtype = 1;
4203vinfos[1].foffset = j1;
4204vinfos[1].indices[0] = _ij1[0];
4205vinfos[1].indices[1] = _ij1[1];
4206vinfos[1].maxsolutions = _nj1;
4207vinfos[2].jointtype = 1;
4208vinfos[2].foffset = j2;
4209vinfos[2].indices[0] = _ij2[0];
4210vinfos[2].indices[1] = _ij2[1];
4211vinfos[2].maxsolutions = _nj2;
4212vinfos[3].jointtype = 1;
4213vinfos[3].foffset = j3;
4214vinfos[3].indices[0] = _ij3[0];
4215vinfos[3].indices[1] = _ij3[1];
4216vinfos[3].maxsolutions = _nj3;
4217vinfos[4].jointtype = 1;
4218vinfos[4].foffset = j4;
4219vinfos[4].indices[0] = _ij4[0];
4220vinfos[4].indices[1] = _ij4[1];
4221vinfos[4].maxsolutions = _nj4;
4222vinfos[5].jointtype = 1;
4223vinfos[5].foffset = j5;
4224vinfos[5].indices[0] = _ij5[0];
4225vinfos[5].indices[1] = _ij5[1];
4226vinfos[5].maxsolutions = _nj5;
4227std::vector<int> vfree(0);
4228solutions.AddSolution(vinfos,vfree);
4229}
4230}
4231}
4232
4233}
4234} while(0);
4235if( bgotonextstatement )
4236{
4237bool bgotonextstatement = true;
4238do
4239{
4240evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j3)))), 6.28318530717959)));
4241evalcond[1]=new_r02;
4242if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4243{
4244bgotonextstatement=false;
4245{
4246IkReal j5array[1], cj5array[1], sj5array[1];
4247bool j5valid[1]={false};
4248_nj5 = 1;
4249if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
4250 continue;
4251j5array[0]=IKatan2(((-1.0)*new_r00), ((-1.0)*new_r01));
4252sj5array[0]=IKsin(j5array[0]);
4253cj5array[0]=IKcos(j5array[0]);
4254if( j5array[0] > IKPI )
4255{
4256 j5array[0]-=IK2PI;
4257}
4258else if( j5array[0] < -IKPI )
4259{ j5array[0]+=IK2PI;
4260}
4261j5valid[0] = true;
4262for(int ij5 = 0; ij5 < 1; ++ij5)
4263{
4264if( !j5valid[ij5] )
4265{
4266 continue;
4267}
4268_ij5[0] = ij5; _ij5[1] = -1;
4269for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4270{
4271if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4272{
4273 j5valid[iij5]=false; _ij5[1] = iij5; break;
4274}
4275}
4276j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4277{
4278IkReal evalcond[8];
4279IkReal x312=IKsin(j5);
4280IkReal x313=IKcos(j5);
4281IkReal x314=((1.0)*cj4);
4282IkReal x315=((1.0)*sj4);
4283evalcond[0]=(x312+new_r00);
4284evalcond[1]=(x313+new_r01);
4285evalcond[2]=(((sj4*x312))+new_r21);
4286evalcond[3]=(((cj4*x312))+new_r11);
4287evalcond[4]=((((-1.0)*x313*x315))+new_r20);
4288evalcond[5]=((((-1.0)*x313*x314))+new_r10);
4289evalcond[6]=((((-1.0)*new_r20*x315))+x313+(((-1.0)*new_r10*x314)));
4290evalcond[7]=((((-1.0)*new_r21*x315))+(((-1.0)*new_r11*x314))+(((-1.0)*x312)));
4291if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4292{
4293continue;
4294}
4295}
4296
4297{
4298std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4299vinfos[0].jointtype = 1;
4300vinfos[0].foffset = j0;
4301vinfos[0].indices[0] = _ij0[0];
4302vinfos[0].indices[1] = _ij0[1];
4303vinfos[0].maxsolutions = _nj0;
4304vinfos[1].jointtype = 1;
4305vinfos[1].foffset = j1;
4306vinfos[1].indices[0] = _ij1[0];
4307vinfos[1].indices[1] = _ij1[1];
4308vinfos[1].maxsolutions = _nj1;
4309vinfos[2].jointtype = 1;
4310vinfos[2].foffset = j2;
4311vinfos[2].indices[0] = _ij2[0];
4312vinfos[2].indices[1] = _ij2[1];
4313vinfos[2].maxsolutions = _nj2;
4314vinfos[3].jointtype = 1;
4315vinfos[3].foffset = j3;
4316vinfos[3].indices[0] = _ij3[0];
4317vinfos[3].indices[1] = _ij3[1];
4318vinfos[3].maxsolutions = _nj3;
4319vinfos[4].jointtype = 1;
4320vinfos[4].foffset = j4;
4321vinfos[4].indices[0] = _ij4[0];
4322vinfos[4].indices[1] = _ij4[1];
4323vinfos[4].maxsolutions = _nj4;
4324vinfos[5].jointtype = 1;
4325vinfos[5].foffset = j5;
4326vinfos[5].indices[0] = _ij5[0];
4327vinfos[5].indices[1] = _ij5[1];
4328vinfos[5].maxsolutions = _nj5;
4329std::vector<int> vfree(0);
4330solutions.AddSolution(vinfos,vfree);
4331}
4332}
4333}
4334
4335}
4336} while(0);
4337if( bgotonextstatement )
4338{
4339bool bgotonextstatement = true;
4340do
4341{
4342evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j3)))), 6.28318530717959)));
4343evalcond[1]=new_r02;
4344if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4345{
4346bgotonextstatement=false;
4347{
4348IkReal j5array[1], cj5array[1], sj5array[1];
4349bool j5valid[1]={false};
4350_nj5 = 1;
4351if( IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r00)+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
4352 continue;
4353j5array[0]=IKatan2(new_r00, new_r01);
4354sj5array[0]=IKsin(j5array[0]);
4355cj5array[0]=IKcos(j5array[0]);
4356if( j5array[0] > IKPI )
4357{
4358 j5array[0]-=IK2PI;
4359}
4360else if( j5array[0] < -IKPI )
4361{ j5array[0]+=IK2PI;
4362}
4363j5valid[0] = true;
4364for(int ij5 = 0; ij5 < 1; ++ij5)
4365{
4366if( !j5valid[ij5] )
4367{
4368 continue;
4369}
4370_ij5[0] = ij5; _ij5[1] = -1;
4371for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4372{
4373if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4374{
4375 j5valid[iij5]=false; _ij5[1] = iij5; break;
4376}
4377}
4378j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4379{
4380IkReal evalcond[8];
4381IkReal x316=IKsin(j5);
4382IkReal x317=IKcos(j5);
4383IkReal x318=((1.0)*sj4);
4384IkReal x319=((1.0)*x317);
4385evalcond[0]=(((sj4*x316))+new_r21);
4386evalcond[1]=(x316+(((-1.0)*new_r00)));
4387evalcond[2]=(x317+(((-1.0)*new_r01)));
4388evalcond[3]=(new_r20+(((-1.0)*x317*x318)));
4389evalcond[4]=(((cj4*x316))+(((-1.0)*new_r11)));
4390evalcond[5]=((((-1.0)*cj4*x319))+(((-1.0)*new_r10)));
4391evalcond[6]=((((-1.0)*new_r20*x318))+((cj4*new_r10))+x317);
4392evalcond[7]=((((-1.0)*new_r21*x318))+((cj4*new_r11))+(((-1.0)*x316)));
4393if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4394{
4395continue;
4396}
4397}
4398
4399{
4400std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4401vinfos[0].jointtype = 1;
4402vinfos[0].foffset = j0;
4403vinfos[0].indices[0] = _ij0[0];
4404vinfos[0].indices[1] = _ij0[1];
4405vinfos[0].maxsolutions = _nj0;
4406vinfos[1].jointtype = 1;
4407vinfos[1].foffset = j1;
4408vinfos[1].indices[0] = _ij1[0];
4409vinfos[1].indices[1] = _ij1[1];
4410vinfos[1].maxsolutions = _nj1;
4411vinfos[2].jointtype = 1;
4412vinfos[2].foffset = j2;
4413vinfos[2].indices[0] = _ij2[0];
4414vinfos[2].indices[1] = _ij2[1];
4415vinfos[2].maxsolutions = _nj2;
4416vinfos[3].jointtype = 1;
4417vinfos[3].foffset = j3;
4418vinfos[3].indices[0] = _ij3[0];
4419vinfos[3].indices[1] = _ij3[1];
4420vinfos[3].maxsolutions = _nj3;
4421vinfos[4].jointtype = 1;
4422vinfos[4].foffset = j4;
4423vinfos[4].indices[0] = _ij4[0];
4424vinfos[4].indices[1] = _ij4[1];
4425vinfos[4].maxsolutions = _nj4;
4426vinfos[5].jointtype = 1;
4427vinfos[5].foffset = j5;
4428vinfos[5].indices[0] = _ij5[0];
4429vinfos[5].indices[1] = _ij5[1];
4430vinfos[5].maxsolutions = _nj5;
4431std::vector<int> vfree(0);
4432solutions.AddSolution(vinfos,vfree);
4433}
4434}
4435}
4436
4437}
4438} while(0);
4439if( bgotonextstatement )
4440{
4441bool bgotonextstatement = true;
4442do
4443{
4444evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j3))), 6.28318530717959)));
4445evalcond[1]=new_r12;
4446if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4447{
4448bgotonextstatement=false;
4449{
4450IkReal j5array[1], cj5array[1], sj5array[1];
4451bool j5valid[1]={false};
4452_nj5 = 1;
4453if( IKabs(new_r10) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r11) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r10)+IKsqr(new_r11)-1) <= IKFAST_SINCOS_THRESH )
4454 continue;
4455j5array[0]=IKatan2(new_r10, new_r11);
4456sj5array[0]=IKsin(j5array[0]);
4457cj5array[0]=IKcos(j5array[0]);
4458if( j5array[0] > IKPI )
4459{
4460 j5array[0]-=IK2PI;
4461}
4462else if( j5array[0] < -IKPI )
4463{ j5array[0]+=IK2PI;
4464}
4465j5valid[0] = true;
4466for(int ij5 = 0; ij5 < 1; ++ij5)
4467{
4468if( !j5valid[ij5] )
4469{
4470 continue;
4471}
4472_ij5[0] = ij5; _ij5[1] = -1;
4473for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4474{
4475if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4476{
4477 j5valid[iij5]=false; _ij5[1] = iij5; break;
4478}
4479}
4480j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4481{
4482IkReal evalcond[8];
4483IkReal x320=IKcos(j5);
4484IkReal x321=IKsin(j5);
4485IkReal x322=((1.0)*cj4);
4486IkReal x323=((1.0)*sj4);
4487IkReal x324=((1.0)*x321);
4488evalcond[0]=(new_r20+((new_r02*x320)));
4489evalcond[1]=(x321+(((-1.0)*new_r10)));
4490evalcond[2]=(x320+(((-1.0)*new_r11)));
4491evalcond[3]=(((cj4*x321))+new_r01);
4492evalcond[4]=((((-1.0)*new_r02*x324))+new_r21);
4493evalcond[5]=((((-1.0)*x320*x322))+new_r00);
4494evalcond[6]=(x320+(((-1.0)*new_r00*x322))+(((-1.0)*new_r20*x323)));
4495evalcond[7]=((((-1.0)*x324))+(((-1.0)*new_r01*x322))+(((-1.0)*new_r21*x323)));
4496if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4497{
4498continue;
4499}
4500}
4501
4502{
4503std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4504vinfos[0].jointtype = 1;
4505vinfos[0].foffset = j0;
4506vinfos[0].indices[0] = _ij0[0];
4507vinfos[0].indices[1] = _ij0[1];
4508vinfos[0].maxsolutions = _nj0;
4509vinfos[1].jointtype = 1;
4510vinfos[1].foffset = j1;
4511vinfos[1].indices[0] = _ij1[0];
4512vinfos[1].indices[1] = _ij1[1];
4513vinfos[1].maxsolutions = _nj1;
4514vinfos[2].jointtype = 1;
4515vinfos[2].foffset = j2;
4516vinfos[2].indices[0] = _ij2[0];
4517vinfos[2].indices[1] = _ij2[1];
4518vinfos[2].maxsolutions = _nj2;
4519vinfos[3].jointtype = 1;
4520vinfos[3].foffset = j3;
4521vinfos[3].indices[0] = _ij3[0];
4522vinfos[3].indices[1] = _ij3[1];
4523vinfos[3].maxsolutions = _nj3;
4524vinfos[4].jointtype = 1;
4525vinfos[4].foffset = j4;
4526vinfos[4].indices[0] = _ij4[0];
4527vinfos[4].indices[1] = _ij4[1];
4528vinfos[4].maxsolutions = _nj4;
4529vinfos[5].jointtype = 1;
4530vinfos[5].foffset = j5;
4531vinfos[5].indices[0] = _ij5[0];
4532vinfos[5].indices[1] = _ij5[1];
4533vinfos[5].maxsolutions = _nj5;
4534std::vector<int> vfree(0);
4535solutions.AddSolution(vinfos,vfree);
4536}
4537}
4538}
4539
4540}
4541} while(0);
4542if( bgotonextstatement )
4543{
4544bool bgotonextstatement = true;
4545do
4546{
4547evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j3)))), 6.28318530717959)));
4548evalcond[1]=new_r12;
4549if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
4550{
4551bgotonextstatement=false;
4552{
4553IkReal j5array[1], cj5array[1], sj5array[1];
4554bool j5valid[1]={false};
4555_nj5 = 1;
4556if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r11))-1) <= IKFAST_SINCOS_THRESH )
4557 continue;
4558j5array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r11));
4559sj5array[0]=IKsin(j5array[0]);
4560cj5array[0]=IKcos(j5array[0]);
4561if( j5array[0] > IKPI )
4562{
4563 j5array[0]-=IK2PI;
4564}
4565else if( j5array[0] < -IKPI )
4566{ j5array[0]+=IK2PI;
4567}
4568j5valid[0] = true;
4569for(int ij5 = 0; ij5 < 1; ++ij5)
4570{
4571if( !j5valid[ij5] )
4572{
4573 continue;
4574}
4575_ij5[0] = ij5; _ij5[1] = -1;
4576for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4577{
4578if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4579{
4580 j5valid[iij5]=false; _ij5[1] = iij5; break;
4581}
4582}
4583j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4584{
4585IkReal evalcond[8];
4586IkReal x325=IKsin(j5);
4587IkReal x326=IKcos(j5);
4588IkReal x327=((1.0)*sj4);
4589IkReal x328=((1.0)*x326);
4590evalcond[0]=(x325+new_r10);
4591evalcond[1]=(x326+new_r11);
4592evalcond[2]=(new_r21+((new_r02*x325)));
4593evalcond[3]=((((-1.0)*new_r02*x328))+new_r20);
4594evalcond[4]=(((cj4*x325))+(((-1.0)*new_r01)));
4595evalcond[5]=((((-1.0)*new_r00))+(((-1.0)*cj4*x328)));
4596evalcond[6]=(((cj4*new_r00))+x326+(((-1.0)*new_r20*x327)));
4597evalcond[7]=(((cj4*new_r01))+(((-1.0)*x325))+(((-1.0)*new_r21*x327)));
4598if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
4599{
4600continue;
4601}
4602}
4603
4604{
4605std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4606vinfos[0].jointtype = 1;
4607vinfos[0].foffset = j0;
4608vinfos[0].indices[0] = _ij0[0];
4609vinfos[0].indices[1] = _ij0[1];
4610vinfos[0].maxsolutions = _nj0;
4611vinfos[1].jointtype = 1;
4612vinfos[1].foffset = j1;
4613vinfos[1].indices[0] = _ij1[0];
4614vinfos[1].indices[1] = _ij1[1];
4615vinfos[1].maxsolutions = _nj1;
4616vinfos[2].jointtype = 1;
4617vinfos[2].foffset = j2;
4618vinfos[2].indices[0] = _ij2[0];
4619vinfos[2].indices[1] = _ij2[1];
4620vinfos[2].maxsolutions = _nj2;
4621vinfos[3].jointtype = 1;
4622vinfos[3].foffset = j3;
4623vinfos[3].indices[0] = _ij3[0];
4624vinfos[3].indices[1] = _ij3[1];
4625vinfos[3].maxsolutions = _nj3;
4626vinfos[4].jointtype = 1;
4627vinfos[4].foffset = j4;
4628vinfos[4].indices[0] = _ij4[0];
4629vinfos[4].indices[1] = _ij4[1];
4630vinfos[4].maxsolutions = _nj4;
4631vinfos[5].jointtype = 1;
4632vinfos[5].foffset = j5;
4633vinfos[5].indices[0] = _ij5[0];
4634vinfos[5].indices[1] = _ij5[1];
4635vinfos[5].maxsolutions = _nj5;
4636std::vector<int> vfree(0);
4637solutions.AddSolution(vinfos,vfree);
4638}
4639}
4640}
4641
4642}
4643} while(0);
4644if( bgotonextstatement )
4645{
4646bool bgotonextstatement = true;
4647do
4648{
4649evalcond[0]=((IKabs(new_r20))+(IKabs(new_r21)));
4650if( IKabs(evalcond[0]) < 0.0000050000000000 )
4651{
4652bgotonextstatement=false;
4653{
4654IkReal j5eval[1];
4655new_r21=0;
4656new_r20=0;
4657new_r02=0;
4658new_r12=0;
4659j5eval[0]=1.0;
4660if( IKabs(j5eval[0]) < 0.0000000100000000 )
4661{
4662continue; // no branches [j5]
4663
4664} else
4665{
4666IkReal op[2+1], zeror[2];
4667int numroots;
4668op[0]=-1.0;
4669op[1]=0;
4670op[2]=1.0;
4671polyroots2(op,zeror,numroots);
4672IkReal j5array[2], cj5array[2], sj5array[2], tempj5array[1];
4673int numsolutions = 0;
4674for(int ij5 = 0; ij5 < numroots; ++ij5)
4675{
4676IkReal htj5 = zeror[ij5];
4677tempj5array[0]=((2.0)*(atan(htj5)));
4678for(int kj5 = 0; kj5 < 1; ++kj5)
4679{
4680j5array[numsolutions] = tempj5array[kj5];
4681if( j5array[numsolutions] > IKPI )
4682{
4683 j5array[numsolutions]-=IK2PI;
4684}
4685else if( j5array[numsolutions] < -IKPI )
4686{
4687 j5array[numsolutions]+=IK2PI;
4688}
4689sj5array[numsolutions] = IKsin(j5array[numsolutions]);
4690cj5array[numsolutions] = IKcos(j5array[numsolutions]);
4691numsolutions++;
4692}
4693}
4694bool j5valid[2]={true,true};
4695_nj5 = 2;
4696for(int ij5 = 0; ij5 < numsolutions; ++ij5)
4697 {
4698if( !j5valid[ij5] )
4699{
4700 continue;
4701}
4702 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4703htj5 = IKtan(j5/2);
4704
4705_ij5[0] = ij5; _ij5[1] = -1;
4706for(int iij5 = ij5+1; iij5 < numsolutions; ++iij5)
4707{
4708if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4709{
4710 j5valid[iij5]=false; _ij5[1] = iij5; break;
4711}
4712}
4713{
4714std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4715vinfos[0].jointtype = 1;
4716vinfos[0].foffset = j0;
4717vinfos[0].indices[0] = _ij0[0];
4718vinfos[0].indices[1] = _ij0[1];
4719vinfos[0].maxsolutions = _nj0;
4720vinfos[1].jointtype = 1;
4721vinfos[1].foffset = j1;
4722vinfos[1].indices[0] = _ij1[0];
4723vinfos[1].indices[1] = _ij1[1];
4724vinfos[1].maxsolutions = _nj1;
4725vinfos[2].jointtype = 1;
4726vinfos[2].foffset = j2;
4727vinfos[2].indices[0] = _ij2[0];
4728vinfos[2].indices[1] = _ij2[1];
4729vinfos[2].maxsolutions = _nj2;
4730vinfos[3].jointtype = 1;
4731vinfos[3].foffset = j3;
4732vinfos[3].indices[0] = _ij3[0];
4733vinfos[3].indices[1] = _ij3[1];
4734vinfos[3].maxsolutions = _nj3;
4735vinfos[4].jointtype = 1;
4736vinfos[4].foffset = j4;
4737vinfos[4].indices[0] = _ij4[0];
4738vinfos[4].indices[1] = _ij4[1];
4739vinfos[4].maxsolutions = _nj4;
4740vinfos[5].jointtype = 1;
4741vinfos[5].foffset = j5;
4742vinfos[5].indices[0] = _ij5[0];
4743vinfos[5].indices[1] = _ij5[1];
4744vinfos[5].maxsolutions = _nj5;
4745std::vector<int> vfree(0);
4746solutions.AddSolution(vinfos,vfree);
4747}
4748 }
4749
4750}
4751
4752}
4753
4754}
4755} while(0);
4756if( bgotonextstatement )
4757{
4758bool bgotonextstatement = true;
4759do
4760{
4761if( 1 )
4762{
4763bgotonextstatement=false;
4764continue; // branch miss [j5]
4765
4766}
4767} while(0);
4768if( bgotonextstatement )
4769{
4770}
4771}
4772}
4773}
4774}
4775}
4776}
4777}
4778}
4779
4780} else
4781{
4782{
4783IkReal j5array[1], cj5array[1], sj5array[1];
4784bool j5valid[1]={false};
4785_nj5 = 1;
4786CheckValue<IkReal> x330=IKPowWithIntegerCheck(sj4,-1);
4787if(!x330.valid){
4788continue;
4789}
4790IkReal x329=x330.value;
4791CheckValue<IkReal> x331=IKPowWithIntegerCheck(cj3,-1);
4792if(!x331.valid){
4793continue;
4794}
4795if( IKabs(((-1.0)*new_r21*x329)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x329))+IKsqr((x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))))-1) <= IKFAST_SINCOS_THRESH )
4796 continue;
4797j5array[0]=IKatan2(((-1.0)*new_r21*x329), (x329*(x331.value)*((((new_r11*sj4))+(((-1.0)*cj4*new_r21*sj3))))));
4798sj5array[0]=IKsin(j5array[0]);
4799cj5array[0]=IKcos(j5array[0]);
4800if( j5array[0] > IKPI )
4801{
4802 j5array[0]-=IK2PI;
4803}
4804else if( j5array[0] < -IKPI )
4805{ j5array[0]+=IK2PI;
4806}
4807j5valid[0] = true;
4808for(int ij5 = 0; ij5 < 1; ++ij5)
4809{
4810if( !j5valid[ij5] )
4811{
4812 continue;
4813}
4814_ij5[0] = ij5; _ij5[1] = -1;
4815for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4816{
4817if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4818{
4819 j5valid[iij5]=false; _ij5[1] = iij5; break;
4820}
4821}
4822j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4823{
4824IkReal evalcond[12];
4825IkReal x332=IKsin(j5);
4826IkReal x333=IKcos(j5);
4827IkReal x334=(cj3*new_r00);
4828IkReal x335=(cj3*cj4);
4829IkReal x336=((1.0)*cj3);
4830IkReal x337=(new_r11*sj3);
4831IkReal x338=((1.0)*cj4);
4832IkReal x339=(new_r10*sj3);
4833IkReal x340=((1.0)*sj4);
4834IkReal x341=(sj3*x332);
4835IkReal x342=((1.0)*x333);
4836IkReal x343=(sj3*x333);
4837evalcond[0]=(((sj4*x332))+new_r21);
4838evalcond[1]=(new_r20+(((-1.0)*x333*x340)));
4839evalcond[2]=((((-1.0)*new_r10*x336))+((new_r00*sj3))+x332);
4840evalcond[3]=((((-1.0)*new_r11*x336))+((new_r01*sj3))+x333);
4841evalcond[4]=(x337+((cj4*x332))+((cj3*new_r01)));
4842evalcond[5]=(((x332*x335))+x343+new_r01);
4843evalcond[6]=((((-1.0)*x333*x338))+x339+x334);
4844evalcond[7]=((((-1.0)*x335*x342))+x341+new_r00);
4845evalcond[8]=((((-1.0)*x333*x336))+((cj4*x341))+new_r11);
4846evalcond[9]=((((-1.0)*x332*x336))+new_r10+(((-1.0)*x338*x343)));
4847evalcond[10]=((((-1.0)*x338*x339))+x333+(((-1.0)*new_r20*x340))+(((-1.0)*x334*x338)));
4848evalcond[11]=((((-1.0)*x337*x338))+(((-1.0)*new_r01*x335))+(((-1.0)*x332))+(((-1.0)*new_r21*x340)));
4849if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
4850{
4851continue;
4852}
4853}
4854
4855{
4856std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4857vinfos[0].jointtype = 1;
4858vinfos[0].foffset = j0;
4859vinfos[0].indices[0] = _ij0[0];
4860vinfos[0].indices[1] = _ij0[1];
4861vinfos[0].maxsolutions = _nj0;
4862vinfos[1].jointtype = 1;
4863vinfos[1].foffset = j1;
4864vinfos[1].indices[0] = _ij1[0];
4865vinfos[1].indices[1] = _ij1[1];
4866vinfos[1].maxsolutions = _nj1;
4867vinfos[2].jointtype = 1;
4868vinfos[2].foffset = j2;
4869vinfos[2].indices[0] = _ij2[0];
4870vinfos[2].indices[1] = _ij2[1];
4871vinfos[2].maxsolutions = _nj2;
4872vinfos[3].jointtype = 1;
4873vinfos[3].foffset = j3;
4874vinfos[3].indices[0] = _ij3[0];
4875vinfos[3].indices[1] = _ij3[1];
4876vinfos[3].maxsolutions = _nj3;
4877vinfos[4].jointtype = 1;
4878vinfos[4].foffset = j4;
4879vinfos[4].indices[0] = _ij4[0];
4880vinfos[4].indices[1] = _ij4[1];
4881vinfos[4].maxsolutions = _nj4;
4882vinfos[5].jointtype = 1;
4883vinfos[5].foffset = j5;
4884vinfos[5].indices[0] = _ij5[0];
4885vinfos[5].indices[1] = _ij5[1];
4886vinfos[5].maxsolutions = _nj5;
4887std::vector<int> vfree(0);
4888solutions.AddSolution(vinfos,vfree);
4889}
4890}
4891}
4892
4893}
4894
4895}
4896
4897} else
4898{
4899{
4900IkReal j5array[1], cj5array[1], sj5array[1];
4901bool j5valid[1]={false};
4902_nj5 = 1;
4903CheckValue<IkReal> x345=IKPowWithIntegerCheck(sj4,-1);
4904if(!x345.valid){
4905continue;
4906}
4907IkReal x344=x345.value;
4908CheckValue<IkReal> x346=IKPowWithIntegerCheck(sj3,-1);
4909if(!x346.valid){
4910continue;
4911}
4912if( IKabs(((-1.0)*new_r21*x344)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21)))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r21*x344))+IKsqr((x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))))-1) <= IKFAST_SINCOS_THRESH )
4913 continue;
4914j5array[0]=IKatan2(((-1.0)*new_r21*x344), (x344*(x346.value)*(((((-1.0)*new_r01*sj4))+((cj3*cj4*new_r21))))));
4915sj5array[0]=IKsin(j5array[0]);
4916cj5array[0]=IKcos(j5array[0]);
4917if( j5array[0] > IKPI )
4918{
4919 j5array[0]-=IK2PI;
4920}
4921else if( j5array[0] < -IKPI )
4922{ j5array[0]+=IK2PI;
4923}
4924j5valid[0] = true;
4925for(int ij5 = 0; ij5 < 1; ++ij5)
4926{
4927if( !j5valid[ij5] )
4928{
4929 continue;
4930}
4931_ij5[0] = ij5; _ij5[1] = -1;
4932for(int iij5 = ij5+1; iij5 < 1; ++iij5)
4933{
4934if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
4935{
4936 j5valid[iij5]=false; _ij5[1] = iij5; break;
4937}
4938}
4939j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
4940{
4941IkReal evalcond[12];
4942IkReal x347=IKsin(j5);
4943IkReal x348=IKcos(j5);
4944IkReal x349=(cj3*new_r00);
4945IkReal x350=(cj3*cj4);
4946IkReal x351=((1.0)*cj3);
4947IkReal x352=(new_r11*sj3);
4948IkReal x353=((1.0)*cj4);
4949IkReal x354=(new_r10*sj3);
4950IkReal x355=((1.0)*sj4);
4951IkReal x356=(sj3*x347);
4952IkReal x357=((1.0)*x348);
4953IkReal x358=(sj3*x348);
4954evalcond[0]=(((sj4*x347))+new_r21);
4955evalcond[1]=((((-1.0)*x348*x355))+new_r20);
4956evalcond[2]=(((new_r00*sj3))+x347+(((-1.0)*new_r10*x351)));
4957evalcond[3]=(((new_r01*sj3))+x348+(((-1.0)*new_r11*x351)));
4958evalcond[4]=(x352+((cj4*x347))+((cj3*new_r01)));
4959evalcond[5]=(x358+((x347*x350))+new_r01);
4960evalcond[6]=((((-1.0)*x348*x353))+x354+x349);
4961evalcond[7]=(x356+new_r00+(((-1.0)*x350*x357)));
4962evalcond[8]=((((-1.0)*x348*x351))+((cj4*x356))+new_r11);
4963evalcond[9]=((((-1.0)*x347*x351))+(((-1.0)*x353*x358))+new_r10);
4964evalcond[10]=((((-1.0)*x349*x353))+x348+(((-1.0)*x353*x354))+(((-1.0)*new_r20*x355)));
4965evalcond[11]=((((-1.0)*new_r01*x350))+(((-1.0)*new_r21*x355))+(((-1.0)*x352*x353))+(((-1.0)*x347)));
4966if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
4967{
4968continue;
4969}
4970}
4971
4972{
4973std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4974vinfos[0].jointtype = 1;
4975vinfos[0].foffset = j0;
4976vinfos[0].indices[0] = _ij0[0];
4977vinfos[0].indices[1] = _ij0[1];
4978vinfos[0].maxsolutions = _nj0;
4979vinfos[1].jointtype = 1;
4980vinfos[1].foffset = j1;
4981vinfos[1].indices[0] = _ij1[0];
4982vinfos[1].indices[1] = _ij1[1];
4983vinfos[1].maxsolutions = _nj1;
4984vinfos[2].jointtype = 1;
4985vinfos[2].foffset = j2;
4986vinfos[2].indices[0] = _ij2[0];
4987vinfos[2].indices[1] = _ij2[1];
4988vinfos[2].maxsolutions = _nj2;
4989vinfos[3].jointtype = 1;
4990vinfos[3].foffset = j3;
4991vinfos[3].indices[0] = _ij3[0];
4992vinfos[3].indices[1] = _ij3[1];
4993vinfos[3].maxsolutions = _nj3;
4994vinfos[4].jointtype = 1;
4995vinfos[4].foffset = j4;
4996vinfos[4].indices[0] = _ij4[0];
4997vinfos[4].indices[1] = _ij4[1];
4998vinfos[4].maxsolutions = _nj4;
4999vinfos[5].jointtype = 1;
5000vinfos[5].foffset = j5;
5001vinfos[5].indices[0] = _ij5[0];
5002vinfos[5].indices[1] = _ij5[1];
5003vinfos[5].maxsolutions = _nj5;
5004std::vector<int> vfree(0);
5005solutions.AddSolution(vinfos,vfree);
5006}
5007}
5008}
5009
5010}
5011
5012}
5013
5014} else
5015{
5016{
5017IkReal j5array[1], cj5array[1], sj5array[1];
5018bool j5valid[1]={false};
5019_nj5 = 1;
5020CheckValue<IkReal> x359 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5021if(!x359.valid){
5022continue;
5023}
5024CheckValue<IkReal> x360=IKPowWithIntegerCheck(IKsign(sj4),-1);
5025if(!x360.valid){
5026continue;
5027}
5028j5array[0]=((-1.5707963267949)+(x359.value)+(((1.5707963267949)*(x360.value))));
5029sj5array[0]=IKsin(j5array[0]);
5030cj5array[0]=IKcos(j5array[0]);
5031if( j5array[0] > IKPI )
5032{
5033 j5array[0]-=IK2PI;
5034}
5035else if( j5array[0] < -IKPI )
5036{ j5array[0]+=IK2PI;
5037}
5038j5valid[0] = true;
5039for(int ij5 = 0; ij5 < 1; ++ij5)
5040{
5041if( !j5valid[ij5] )
5042{
5043 continue;
5044}
5045_ij5[0] = ij5; _ij5[1] = -1;
5046for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5047{
5048if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5049{
5050 j5valid[iij5]=false; _ij5[1] = iij5; break;
5051}
5052}
5053j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5054{
5055IkReal evalcond[12];
5056IkReal x361=IKsin(j5);
5057IkReal x362=IKcos(j5);
5058IkReal x363=(cj3*new_r00);
5059IkReal x364=(cj3*cj4);
5060IkReal x365=((1.0)*cj3);
5061IkReal x366=(new_r11*sj3);
5062IkReal x367=((1.0)*cj4);
5063IkReal x368=(new_r10*sj3);
5064IkReal x369=((1.0)*sj4);
5065IkReal x370=(sj3*x361);
5066IkReal x371=((1.0)*x362);
5067IkReal x372=(sj3*x362);
5068evalcond[0]=(((sj4*x361))+new_r21);
5069evalcond[1]=((((-1.0)*x362*x369))+new_r20);
5070evalcond[2]=(((new_r00*sj3))+x361+(((-1.0)*new_r10*x365)));
5071evalcond[3]=(((new_r01*sj3))+x362+(((-1.0)*new_r11*x365)));
5072evalcond[4]=(((cj4*x361))+x366+((cj3*new_r01)));
5073evalcond[5]=(((x361*x364))+x372+new_r01);
5074evalcond[6]=((((-1.0)*x362*x367))+x368+x363);
5075evalcond[7]=(x370+new_r00+(((-1.0)*x364*x371)));
5076evalcond[8]=((((-1.0)*x362*x365))+((cj4*x370))+new_r11);
5077evalcond[9]=((((-1.0)*x361*x365))+(((-1.0)*x367*x372))+new_r10);
5078evalcond[10]=((((-1.0)*x363*x367))+(((-1.0)*new_r20*x369))+(((-1.0)*x367*x368))+x362);
5079evalcond[11]=((((-1.0)*x361))+(((-1.0)*x366*x367))+(((-1.0)*new_r01*x364))+(((-1.0)*new_r21*x369)));
5080if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH )
5081{
5082continue;
5083}
5084}
5085
5086{
5087std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5088vinfos[0].jointtype = 1;
5089vinfos[0].foffset = j0;
5090vinfos[0].indices[0] = _ij0[0];
5091vinfos[0].indices[1] = _ij0[1];
5092vinfos[0].maxsolutions = _nj0;
5093vinfos[1].jointtype = 1;
5094vinfos[1].foffset = j1;
5095vinfos[1].indices[0] = _ij1[0];
5096vinfos[1].indices[1] = _ij1[1];
5097vinfos[1].maxsolutions = _nj1;
5098vinfos[2].jointtype = 1;
5099vinfos[2].foffset = j2;
5100vinfos[2].indices[0] = _ij2[0];
5101vinfos[2].indices[1] = _ij2[1];
5102vinfos[2].maxsolutions = _nj2;
5103vinfos[3].jointtype = 1;
5104vinfos[3].foffset = j3;
5105vinfos[3].indices[0] = _ij3[0];
5106vinfos[3].indices[1] = _ij3[1];
5107vinfos[3].maxsolutions = _nj3;
5108vinfos[4].jointtype = 1;
5109vinfos[4].foffset = j4;
5110vinfos[4].indices[0] = _ij4[0];
5111vinfos[4].indices[1] = _ij4[1];
5112vinfos[4].maxsolutions = _nj4;
5113vinfos[5].jointtype = 1;
5114vinfos[5].foffset = j5;
5115vinfos[5].indices[0] = _ij5[0];
5116vinfos[5].indices[1] = _ij5[1];
5117vinfos[5].maxsolutions = _nj5;
5118std::vector<int> vfree(0);
5119solutions.AddSolution(vinfos,vfree);
5120}
5121}
5122}
5123
5124}
5125
5126}
5127}
5128}
5129
5130}
5131
5132}
5133
5134} else
5135{
5136{
5137IkReal j5array[1], cj5array[1], sj5array[1];
5138bool j5valid[1]={false};
5139_nj5 = 1;
5140CheckValue<IkReal> x373 = IKatan2WithCheck(IkReal(((-1.0)*new_r21)),IkReal(new_r20),IKFAST_ATAN2_MAGTHRESH);
5141if(!x373.valid){
5142continue;
5143}
5144CheckValue<IkReal> x374=IKPowWithIntegerCheck(IKsign(sj4),-1);
5145if(!x374.valid){
5146continue;
5147}
5148j5array[0]=((-1.5707963267949)+(x373.value)+(((1.5707963267949)*(x374.value))));
5149sj5array[0]=IKsin(j5array[0]);
5150cj5array[0]=IKcos(j5array[0]);
5151if( j5array[0] > IKPI )
5152{
5153 j5array[0]-=IK2PI;
5154}
5155else if( j5array[0] < -IKPI )
5156{ j5array[0]+=IK2PI;
5157}
5158j5valid[0] = true;
5159for(int ij5 = 0; ij5 < 1; ++ij5)
5160{
5161if( !j5valid[ij5] )
5162{
5163 continue;
5164}
5165_ij5[0] = ij5; _ij5[1] = -1;
5166for(int iij5 = ij5+1; iij5 < 1; ++iij5)
5167{
5168if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
5169{
5170 j5valid[iij5]=false; _ij5[1] = iij5; break;
5171}
5172}
5173j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
5174{
5175IkReal evalcond[2];
5176evalcond[0]=(((sj4*(IKsin(j5))))+new_r21);
5177evalcond[1]=((((-1.0)*sj4*(IKcos(j5))))+new_r20);
5178if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH )
5179{
5180continue;
5181}
5182}
5183
5184{
5185IkReal j3eval[3];
5186j3eval[0]=sj4;
5187j3eval[1]=IKsign(sj4);
5188j3eval[2]=((IKabs(new_r12))+(IKabs(new_r02)));
5189if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5190{
5191{
5192IkReal j3eval[2];
5193j3eval[0]=new_r12;
5194j3eval[1]=sj4;
5195if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
5196{
5197{
5198IkReal evalcond[5];
5199bool bgotonextstatement = true;
5200do
5201{
5202evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j4))), 6.28318530717959)));
5203evalcond[1]=new_r02;
5204evalcond[2]=new_r12;
5205evalcond[3]=new_r21;
5206evalcond[4]=new_r20;
5207if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
5208{
5209bgotonextstatement=false;
5210{
5211IkReal j3eval[3];
5212sj4=0;
5213cj4=1.0;
5214j4=0;
5215IkReal x375=((1.0)*new_r11);
5216IkReal x376=((((-1.0)*new_r10*x375))+(((-1.0)*new_r00*new_r01)));
5217j3eval[0]=x376;
5218j3eval[1]=((IKabs((((new_r10*sj5))+((new_r01*sj5)))))+(IKabs(((((-1.0)*sj5*x375))+((new_r00*sj5))))));
5219j3eval[2]=IKsign(x376);
5220if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5221{
5222{
5223IkReal j3eval[3];
5224sj4=0;
5225cj4=1.0;
5226j4=0;
5227IkReal x377=((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))));
5228j3eval[0]=x377;
5229j3eval[1]=((IKabs((((new_r11*sj5))+((cj5*new_r01)))))+(IKabs((((new_r01*sj5))+(((-1.0)*cj5*new_r11))))));
5230j3eval[2]=IKsign(x377);
5231if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5232{
5233{
5234IkReal j3eval[3];
5235sj4=0;
5236cj4=1.0;
5237j4=0;
5238IkReal x378=((1.0)*new_r11);
5239IkReal x379=((((-1.0)*cj5*x378))+(((-1.0)*new_r01*sj5)));
5240j3eval[0]=x379;
5241j3eval[1]=IKsign(x379);
5242j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((1.0)+(((-1.0)*new_r00*x378))+(((-1.0)*(cj5*cj5)))))));
5243if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5244{
5245{
5246IkReal evalcond[1];
5247bool bgotonextstatement = true;
5248do
5249{
5250IkReal x380=((-1.0)*new_r01);
5251IkReal x382 = ((new_r01*new_r01)+(new_r11*new_r11));
5252if(IKabs(x382)==0){
5253continue;
5254}
5255IkReal x381=pow(x382,-0.5);
5256CheckValue<IkReal> x383 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x380),IKFAST_ATAN2_MAGTHRESH);
5257if(!x383.valid){
5258continue;
5259}
5260IkReal gconst0=((-1.0)*(x383.value));
5261IkReal gconst1=(new_r11*x381);
5262IkReal gconst2=(x380*x381);
5263CheckValue<IkReal> x384 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5264if(!x384.valid){
5265continue;
5266}
5267evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x384.value)+j5)))), 6.28318530717959)));
5268if( IKabs(evalcond[0]) < 0.0000050000000000 )
5269{
5270bgotonextstatement=false;
5271{
5272IkReal j3eval[3];
5273IkReal x385=((-1.0)*new_r01);
5274CheckValue<IkReal> x388 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x385),IKFAST_ATAN2_MAGTHRESH);
5275if(!x388.valid){
5276continue;
5277}
5278IkReal x386=((-1.0)*(x388.value));
5279IkReal x387=x381;
5280sj4=0;
5281cj4=1.0;
5282j4=0;
5283sj5=gconst1;
5284cj5=gconst2;
5285j5=x386;
5286IkReal gconst0=x386;
5287IkReal gconst1=(new_r11*x387);
5288IkReal gconst2=(x385*x387);
5289IkReal x389=new_r11*new_r11;
5290IkReal x390=(new_r10*new_r11);
5291IkReal x391=((((-1.0)*x390))+(((-1.0)*new_r00*new_r01)));
5292IkReal x392=x381;
5293IkReal x393=(new_r11*x392);
5294j3eval[0]=x391;
5295j3eval[1]=((IKabs(((((-1.0)*x389*x392))+((new_r00*x393)))))+(IKabs((((new_r01*x393))+((x390*x392))))));
5296j3eval[2]=IKsign(x391);
5297if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5298{
5299{
5300IkReal j3eval[1];
5301IkReal x394=((-1.0)*new_r01);
5302CheckValue<IkReal> x397 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x394),IKFAST_ATAN2_MAGTHRESH);
5303if(!x397.valid){
5304continue;
5305}
5306IkReal x395=((-1.0)*(x397.value));
5307IkReal x396=x381;
5308sj4=0;
5309cj4=1.0;
5310j4=0;
5311sj5=gconst1;
5312cj5=gconst2;
5313j5=x395;
5314IkReal gconst0=x395;
5315IkReal gconst1=(new_r11*x396);
5316IkReal gconst2=(x394*x396);
5317IkReal x398=new_r11*new_r11;
5318CheckValue<IkReal> x401=IKPowWithIntegerCheck(((new_r01*new_r01)+x398),-1);
5319if(!x401.valid){
5320continue;
5321}
5322IkReal x399=x401.value;
5323IkReal x400=(x398*x399);
5324j3eval[0]=((IKabs((((new_r01*new_r10))+x400)))+(IKabs((((new_r00*new_r01*x400))+((new_r00*x399*(new_r01*new_r01*new_r01)))+((new_r01*new_r11*x399))))));
5325if( IKabs(j3eval[0]) < 0.0000010000000000 )
5326{
5327{
5328IkReal j3eval[1];
5329IkReal x402=((-1.0)*new_r01);
5330CheckValue<IkReal> x405 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x402),IKFAST_ATAN2_MAGTHRESH);
5331if(!x405.valid){
5332continue;
5333}
5334IkReal x403=((-1.0)*(x405.value));
5335IkReal x404=x381;
5336sj4=0;
5337cj4=1.0;
5338j4=0;
5339sj5=gconst1;
5340cj5=gconst2;
5341j5=x403;
5342IkReal gconst0=x403;
5343IkReal gconst1=(new_r11*x404);
5344IkReal gconst2=(x402*x404);
5345IkReal x406=new_r01*new_r01;
5346IkReal x407=new_r11*new_r11;
5347CheckValue<IkReal> x414=IKPowWithIntegerCheck((x407+x406),-1);
5348if(!x414.valid){
5349continue;
5350}
5351IkReal x408=x414.value;
5352IkReal x409=(x407*x408);
5353CheckValue<IkReal> x415=IKPowWithIntegerCheck(((((-1.0)*x406))+(((-1.0)*x407))),-1);
5354if(!x415.valid){
5355continue;
5356}
5357IkReal x410=x415.value;
5358IkReal x411=((1.0)*x410);
5359IkReal x412=(new_r11*x411);
5360IkReal x413=(new_r01*x411);
5361j3eval[0]=((IKabs((((x406*x409))+((x408*(x406*x406)))+(((-1.0)*x409)))))+(IKabs(((((-1.0)*new_r01*x412*(new_r11*new_r11)))+(((-1.0)*x412*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x412))))));
5362if( IKabs(j3eval[0]) < 0.0000010000000000 )
5363{
5364{
5365IkReal evalcond[3];
5366bool bgotonextstatement = true;
5367do
5368{
5369evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
5370if( IKabs(evalcond[0]) < 0.0000050000000000 )
5371{
5372bgotonextstatement=false;
5373{
5374IkReal j3array[2], cj3array[2], sj3array[2];
5375bool j3valid[2]={false};
5376_nj3 = 2;
5377CheckValue<IkReal> x416=IKPowWithIntegerCheck(gconst2,-1);
5378if(!x416.valid){
5379continue;
5380}
5381sj3array[0]=(new_r10*(x416.value));
5382if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5383{
5384 j3valid[0] = j3valid[1] = true;
5385 j3array[0] = IKasin(sj3array[0]);
5386 cj3array[0] = IKcos(j3array[0]);
5387 sj3array[1] = sj3array[0];
5388 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5389 cj3array[1] = -cj3array[0];
5390}
5391else if( isnan(sj3array[0]) )
5392{
5393 // probably any value will work
5394 j3valid[0] = true;
5395 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5396}
5397for(int ij3 = 0; ij3 < 2; ++ij3)
5398{
5399if( !j3valid[ij3] )
5400{
5401 continue;
5402}
5403_ij3[0] = ij3; _ij3[1] = -1;
5404for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5405{
5406if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5407{
5408 j3valid[iij3]=false; _ij3[1] = iij3; break;
5409}
5410}
5411j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5412{
5413IkReal evalcond[6];
5414IkReal x417=IKcos(j3);
5415IkReal x418=IKsin(j3);
5416IkReal x419=((-1.0)*x417);
5417evalcond[0]=(new_r01*x417);
5418evalcond[1]=(new_r10*x419);
5419evalcond[2]=(gconst2*x419);
5420evalcond[3]=(gconst2+((new_r01*x418)));
5421evalcond[4]=(((gconst2*x418))+new_r01);
5422evalcond[5]=((((-1.0)*gconst2))+((new_r10*x418)));
5423if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5424{
5425continue;
5426}
5427}
5428
5429{
5430std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5431vinfos[0].jointtype = 1;
5432vinfos[0].foffset = j0;
5433vinfos[0].indices[0] = _ij0[0];
5434vinfos[0].indices[1] = _ij0[1];
5435vinfos[0].maxsolutions = _nj0;
5436vinfos[1].jointtype = 1;
5437vinfos[1].foffset = j1;
5438vinfos[1].indices[0] = _ij1[0];
5439vinfos[1].indices[1] = _ij1[1];
5440vinfos[1].maxsolutions = _nj1;
5441vinfos[2].jointtype = 1;
5442vinfos[2].foffset = j2;
5443vinfos[2].indices[0] = _ij2[0];
5444vinfos[2].indices[1] = _ij2[1];
5445vinfos[2].maxsolutions = _nj2;
5446vinfos[3].jointtype = 1;
5447vinfos[3].foffset = j3;
5448vinfos[3].indices[0] = _ij3[0];
5449vinfos[3].indices[1] = _ij3[1];
5450vinfos[3].maxsolutions = _nj3;
5451vinfos[4].jointtype = 1;
5452vinfos[4].foffset = j4;
5453vinfos[4].indices[0] = _ij4[0];
5454vinfos[4].indices[1] = _ij4[1];
5455vinfos[4].maxsolutions = _nj4;
5456vinfos[5].jointtype = 1;
5457vinfos[5].foffset = j5;
5458vinfos[5].indices[0] = _ij5[0];
5459vinfos[5].indices[1] = _ij5[1];
5460vinfos[5].maxsolutions = _nj5;
5461std::vector<int> vfree(0);
5462solutions.AddSolution(vinfos,vfree);
5463}
5464}
5465}
5466
5467}
5468} while(0);
5469if( bgotonextstatement )
5470{
5471bool bgotonextstatement = true;
5472do
5473{
5474evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
5475evalcond[1]=gconst1;
5476evalcond[2]=gconst2;
5477if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
5478{
5479bgotonextstatement=false;
5480{
5481IkReal j3eval[3];
5482IkReal x420=((-1.0)*new_r01);
5483CheckValue<IkReal> x422 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x420),IKFAST_ATAN2_MAGTHRESH);
5484if(!x422.valid){
5485continue;
5486}
5487IkReal x421=((-1.0)*(x422.value));
5488sj4=0;
5489cj4=1.0;
5490j4=0;
5491sj5=gconst1;
5492cj5=gconst2;
5493j5=x421;
5494new_r00=0;
5495new_r10=0;
5496new_r21=0;
5497new_r22=0;
5498IkReal gconst0=x421;
5499IkReal gconst1=new_r11;
5500IkReal gconst2=x420;
5501j3eval[0]=-1.0;
5502j3eval[1]=-1.0;
5503j3eval[2]=((IKabs((new_r01*new_r11)))+(IKabs(((1.0)+(((-1.0)*(new_r01*new_r01)))))));
5504if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5505{
5506{
5507IkReal j3eval[3];
5508IkReal x423=((-1.0)*new_r01);
5509CheckValue<IkReal> x425 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x423),IKFAST_ATAN2_MAGTHRESH);
5510if(!x425.valid){
5511continue;
5512}
5513IkReal x424=((-1.0)*(x425.value));
5514sj4=0;
5515cj4=1.0;
5516j4=0;
5517sj5=gconst1;
5518cj5=gconst2;
5519j5=x424;
5520new_r00=0;
5521new_r10=0;
5522new_r21=0;
5523new_r22=0;
5524IkReal gconst0=x424;
5525IkReal gconst1=new_r11;
5526IkReal gconst2=x423;
5527j3eval[0]=-1.0;
5528j3eval[1]=((IKabs(new_r01*new_r01))+(IKabs((new_r01*new_r11))));
5529j3eval[2]=-1.0;
5530if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5531{
5532{
5533IkReal j3eval[3];
5534IkReal x426=((-1.0)*new_r01);
5535CheckValue<IkReal> x428 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(x426),IKFAST_ATAN2_MAGTHRESH);
5536if(!x428.valid){
5537continue;
5538}
5539IkReal x427=((-1.0)*(x428.value));
5540sj4=0;
5541cj4=1.0;
5542j4=0;
5543sj5=gconst1;
5544cj5=gconst2;
5545j5=x427;
5546new_r00=0;
5547new_r10=0;
5548new_r21=0;
5549new_r22=0;
5550IkReal gconst0=x427;
5551IkReal gconst1=new_r11;
5552IkReal gconst2=x426;
5553j3eval[0]=1.0;
5554j3eval[1]=1.0;
5555j3eval[2]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01))))))))+(IKabs((new_r01*new_r11))));
5556if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
5557{
5558continue; // 3 cases reached
5559
5560} else
5561{
5562{
5563IkReal j3array[1], cj3array[1], sj3array[1];
5564bool j3valid[1]={false};
5565_nj3 = 1;
5566IkReal x429=((1.0)*new_r01);
5567CheckValue<IkReal> x430 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst1*gconst1))))),IkReal(((((-1.0)*new_r11*x429))+((gconst1*gconst2)))),IKFAST_ATAN2_MAGTHRESH);
5568if(!x430.valid){
5569continue;
5570}
5571CheckValue<IkReal> x431=IKPowWithIntegerCheck(IKsign((((gconst1*new_r11))+(((-1.0)*gconst2*x429)))),-1);
5572if(!x431.valid){
5573continue;
5574}
5575j3array[0]=((-1.5707963267949)+(x430.value)+(((1.5707963267949)*(x431.value))));
5576sj3array[0]=IKsin(j3array[0]);
5577cj3array[0]=IKcos(j3array[0]);
5578if( j3array[0] > IKPI )
5579{
5580 j3array[0]-=IK2PI;
5581}
5582else if( j3array[0] < -IKPI )
5583{ j3array[0]+=IK2PI;
5584}
5585j3valid[0] = true;
5586for(int ij3 = 0; ij3 < 1; ++ij3)
5587{
5588if( !j3valid[ij3] )
5589{
5590 continue;
5591}
5592_ij3[0] = ij3; _ij3[1] = -1;
5593for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5594{
5595if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5596{
5597 j3valid[iij3]=false; _ij3[1] = iij3; break;
5598}
5599}
5600j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5601{
5602IkReal evalcond[6];
5603IkReal x432=IKcos(j3);
5604IkReal x433=IKsin(j3);
5605IkReal x434=(gconst1*x433);
5606IkReal x435=(gconst2*x433);
5607IkReal x436=((1.0)*x432);
5608IkReal x437=(gconst2*x436);
5609evalcond[0]=(((new_r01*x432))+gconst1+((new_r11*x433)));
5610evalcond[1]=(((gconst1*x432))+x435+new_r01);
5611evalcond[2]=((((-1.0)*x437))+x434);
5612evalcond[3]=(((new_r01*x433))+gconst2+(((-1.0)*new_r11*x436)));
5613evalcond[4]=((((-1.0)*x437))+x434+new_r11);
5614evalcond[5]=((((-1.0)*x435))+(((-1.0)*gconst1*x436)));
5615if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5616{
5617continue;
5618}
5619}
5620
5621{
5622std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5623vinfos[0].jointtype = 1;
5624vinfos[0].foffset = j0;
5625vinfos[0].indices[0] = _ij0[0];
5626vinfos[0].indices[1] = _ij0[1];
5627vinfos[0].maxsolutions = _nj0;
5628vinfos[1].jointtype = 1;
5629vinfos[1].foffset = j1;
5630vinfos[1].indices[0] = _ij1[0];
5631vinfos[1].indices[1] = _ij1[1];
5632vinfos[1].maxsolutions = _nj1;
5633vinfos[2].jointtype = 1;
5634vinfos[2].foffset = j2;
5635vinfos[2].indices[0] = _ij2[0];
5636vinfos[2].indices[1] = _ij2[1];
5637vinfos[2].maxsolutions = _nj2;
5638vinfos[3].jointtype = 1;
5639vinfos[3].foffset = j3;
5640vinfos[3].indices[0] = _ij3[0];
5641vinfos[3].indices[1] = _ij3[1];
5642vinfos[3].maxsolutions = _nj3;
5643vinfos[4].jointtype = 1;
5644vinfos[4].foffset = j4;
5645vinfos[4].indices[0] = _ij4[0];
5646vinfos[4].indices[1] = _ij4[1];
5647vinfos[4].maxsolutions = _nj4;
5648vinfos[5].jointtype = 1;
5649vinfos[5].foffset = j5;
5650vinfos[5].indices[0] = _ij5[0];
5651vinfos[5].indices[1] = _ij5[1];
5652vinfos[5].maxsolutions = _nj5;
5653std::vector<int> vfree(0);
5654solutions.AddSolution(vinfos,vfree);
5655}
5656}
5657}
5658
5659}
5660
5661}
5662
5663} else
5664{
5665{
5666IkReal j3array[1], cj3array[1], sj3array[1];
5667bool j3valid[1]={false};
5668_nj3 = 1;
5669CheckValue<IkReal> x438=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst2*gconst2)))+(((-1.0)*(gconst1*gconst1))))),-1);
5670if(!x438.valid){
5671continue;
5672}
5673CheckValue<IkReal> x439 = IKatan2WithCheck(IkReal((gconst2*new_r01)),IkReal((gconst1*new_r01)),IKFAST_ATAN2_MAGTHRESH);
5674if(!x439.valid){
5675continue;
5676}
5677j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x438.value)))+(x439.value));
5678sj3array[0]=IKsin(j3array[0]);
5679cj3array[0]=IKcos(j3array[0]);
5680if( j3array[0] > IKPI )
5681{
5682 j3array[0]-=IK2PI;
5683}
5684else if( j3array[0] < -IKPI )
5685{ j3array[0]+=IK2PI;
5686}
5687j3valid[0] = true;
5688for(int ij3 = 0; ij3 < 1; ++ij3)
5689{
5690if( !j3valid[ij3] )
5691{
5692 continue;
5693}
5694_ij3[0] = ij3; _ij3[1] = -1;
5695for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5696{
5697if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5698{
5699 j3valid[iij3]=false; _ij3[1] = iij3; break;
5700}
5701}
5702j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5703{
5704IkReal evalcond[6];
5705IkReal x440=IKcos(j3);
5706IkReal x441=IKsin(j3);
5707IkReal x442=(gconst1*x441);
5708IkReal x443=(gconst2*x441);
5709IkReal x444=((1.0)*x440);
5710IkReal x445=(gconst2*x444);
5711evalcond[0]=(((new_r01*x440))+gconst1+((new_r11*x441)));
5712evalcond[1]=(((gconst1*x440))+x443+new_r01);
5713evalcond[2]=((((-1.0)*x445))+x442);
5714evalcond[3]=(((new_r01*x441))+gconst2+(((-1.0)*new_r11*x444)));
5715evalcond[4]=((((-1.0)*x445))+x442+new_r11);
5716evalcond[5]=((((-1.0)*x443))+(((-1.0)*gconst1*x444)));
5717if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5718{
5719continue;
5720}
5721}
5722
5723{
5724std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5725vinfos[0].jointtype = 1;
5726vinfos[0].foffset = j0;
5727vinfos[0].indices[0] = _ij0[0];
5728vinfos[0].indices[1] = _ij0[1];
5729vinfos[0].maxsolutions = _nj0;
5730vinfos[1].jointtype = 1;
5731vinfos[1].foffset = j1;
5732vinfos[1].indices[0] = _ij1[0];
5733vinfos[1].indices[1] = _ij1[1];
5734vinfos[1].maxsolutions = _nj1;
5735vinfos[2].jointtype = 1;
5736vinfos[2].foffset = j2;
5737vinfos[2].indices[0] = _ij2[0];
5738vinfos[2].indices[1] = _ij2[1];
5739vinfos[2].maxsolutions = _nj2;
5740vinfos[3].jointtype = 1;
5741vinfos[3].foffset = j3;
5742vinfos[3].indices[0] = _ij3[0];
5743vinfos[3].indices[1] = _ij3[1];
5744vinfos[3].maxsolutions = _nj3;
5745vinfos[4].jointtype = 1;
5746vinfos[4].foffset = j4;
5747vinfos[4].indices[0] = _ij4[0];
5748vinfos[4].indices[1] = _ij4[1];
5749vinfos[4].maxsolutions = _nj4;
5750vinfos[5].jointtype = 1;
5751vinfos[5].foffset = j5;
5752vinfos[5].indices[0] = _ij5[0];
5753vinfos[5].indices[1] = _ij5[1];
5754vinfos[5].maxsolutions = _nj5;
5755std::vector<int> vfree(0);
5756solutions.AddSolution(vinfos,vfree);
5757}
5758}
5759}
5760
5761}
5762
5763}
5764
5765} else
5766{
5767{
5768IkReal j3array[1], cj3array[1], sj3array[1];
5769bool j3valid[1]={false};
5770_nj3 = 1;
5771CheckValue<IkReal> x446 = IKatan2WithCheck(IkReal(gconst1*gconst1),IkReal(((-1.0)*gconst1*gconst2)),IKFAST_ATAN2_MAGTHRESH);
5772if(!x446.valid){
5773continue;
5774}
5775CheckValue<IkReal> x447=IKPowWithIntegerCheck(IKsign((((gconst2*new_r01))+(((-1.0)*gconst1*new_r11)))),-1);
5776if(!x447.valid){
5777continue;
5778}
5779j3array[0]=((-1.5707963267949)+(x446.value)+(((1.5707963267949)*(x447.value))));
5780sj3array[0]=IKsin(j3array[0]);
5781cj3array[0]=IKcos(j3array[0]);
5782if( j3array[0] > IKPI )
5783{
5784 j3array[0]-=IK2PI;
5785}
5786else if( j3array[0] < -IKPI )
5787{ j3array[0]+=IK2PI;
5788}
5789j3valid[0] = true;
5790for(int ij3 = 0; ij3 < 1; ++ij3)
5791{
5792if( !j3valid[ij3] )
5793{
5794 continue;
5795}
5796_ij3[0] = ij3; _ij3[1] = -1;
5797for(int iij3 = ij3+1; iij3 < 1; ++iij3)
5798{
5799if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5800{
5801 j3valid[iij3]=false; _ij3[1] = iij3; break;
5802}
5803}
5804j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5805{
5806IkReal evalcond[6];
5807IkReal x448=IKcos(j3);
5808IkReal x449=IKsin(j3);
5809IkReal x450=(gconst1*x449);
5810IkReal x451=(gconst2*x449);
5811IkReal x452=((1.0)*x448);
5812IkReal x453=(gconst2*x452);
5813evalcond[0]=(((new_r01*x448))+gconst1+((new_r11*x449)));
5814evalcond[1]=(((gconst1*x448))+x451+new_r01);
5815evalcond[2]=((((-1.0)*x453))+x450);
5816evalcond[3]=(((new_r01*x449))+gconst2+(((-1.0)*new_r11*x452)));
5817evalcond[4]=((((-1.0)*x453))+x450+new_r11);
5818evalcond[5]=((((-1.0)*x451))+(((-1.0)*gconst1*x452)));
5819if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5820{
5821continue;
5822}
5823}
5824
5825{
5826std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5827vinfos[0].jointtype = 1;
5828vinfos[0].foffset = j0;
5829vinfos[0].indices[0] = _ij0[0];
5830vinfos[0].indices[1] = _ij0[1];
5831vinfos[0].maxsolutions = _nj0;
5832vinfos[1].jointtype = 1;
5833vinfos[1].foffset = j1;
5834vinfos[1].indices[0] = _ij1[0];
5835vinfos[1].indices[1] = _ij1[1];
5836vinfos[1].maxsolutions = _nj1;
5837vinfos[2].jointtype = 1;
5838vinfos[2].foffset = j2;
5839vinfos[2].indices[0] = _ij2[0];
5840vinfos[2].indices[1] = _ij2[1];
5841vinfos[2].maxsolutions = _nj2;
5842vinfos[3].jointtype = 1;
5843vinfos[3].foffset = j3;
5844vinfos[3].indices[0] = _ij3[0];
5845vinfos[3].indices[1] = _ij3[1];
5846vinfos[3].maxsolutions = _nj3;
5847vinfos[4].jointtype = 1;
5848vinfos[4].foffset = j4;
5849vinfos[4].indices[0] = _ij4[0];
5850vinfos[4].indices[1] = _ij4[1];
5851vinfos[4].maxsolutions = _nj4;
5852vinfos[5].jointtype = 1;
5853vinfos[5].foffset = j5;
5854vinfos[5].indices[0] = _ij5[0];
5855vinfos[5].indices[1] = _ij5[1];
5856vinfos[5].maxsolutions = _nj5;
5857std::vector<int> vfree(0);
5858solutions.AddSolution(vinfos,vfree);
5859}
5860}
5861}
5862
5863}
5864
5865}
5866
5867}
5868} while(0);
5869if( bgotonextstatement )
5870{
5871bool bgotonextstatement = true;
5872do
5873{
5874evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
5875if( IKabs(evalcond[0]) < 0.0000050000000000 )
5876{
5877bgotonextstatement=false;
5878{
5879IkReal j3eval[1];
5880CheckValue<IkReal> x455 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5881if(!x455.valid){
5882continue;
5883}
5884IkReal x454=((-1.0)*(x455.value));
5885sj4=0;
5886cj4=1.0;
5887j4=0;
5888sj5=gconst1;
5889cj5=gconst2;
5890j5=x454;
5891new_r01=0;
5892new_r10=0;
5893IkReal gconst0=x454;
5894IkReal x456 = new_r11*new_r11;
5895if(IKabs(x456)==0){
5896continue;
5897}
5898IkReal gconst1=(new_r11*(pow(x456,-0.5)));
5899IkReal gconst2=0;
5900j3eval[0]=new_r00;
5901if( IKabs(j3eval[0]) < 0.0000010000000000 )
5902{
5903{
5904IkReal j3eval[1];
5905CheckValue<IkReal> x458 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
5906if(!x458.valid){
5907continue;
5908}
5909IkReal x457=((-1.0)*(x458.value));
5910sj4=0;
5911cj4=1.0;
5912j4=0;
5913sj5=gconst1;
5914cj5=gconst2;
5915j5=x457;
5916new_r01=0;
5917new_r10=0;
5918IkReal gconst0=x457;
5919IkReal x459 = new_r11*new_r11;
5920if(IKabs(x459)==0){
5921continue;
5922}
5923IkReal gconst1=(new_r11*(pow(x459,-0.5)));
5924IkReal gconst2=0;
5925j3eval[0]=new_r11;
5926if( IKabs(j3eval[0]) < 0.0000010000000000 )
5927{
5928{
5929IkReal j3array[2], cj3array[2], sj3array[2];
5930bool j3valid[2]={false};
5931_nj3 = 2;
5932CheckValue<IkReal> x460=IKPowWithIntegerCheck(gconst1,-1);
5933if(!x460.valid){
5934continue;
5935}
5936sj3array[0]=((-1.0)*new_r00*(x460.value));
5937if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
5938{
5939 j3valid[0] = j3valid[1] = true;
5940 j3array[0] = IKasin(sj3array[0]);
5941 cj3array[0] = IKcos(j3array[0]);
5942 sj3array[1] = sj3array[0];
5943 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
5944 cj3array[1] = -cj3array[0];
5945}
5946else if( isnan(sj3array[0]) )
5947{
5948 // probably any value will work
5949 j3valid[0] = true;
5950 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
5951}
5952for(int ij3 = 0; ij3 < 2; ++ij3)
5953{
5954if( !j3valid[ij3] )
5955{
5956 continue;
5957}
5958_ij3[0] = ij3; _ij3[1] = -1;
5959for(int iij3 = ij3+1; iij3 < 2; ++iij3)
5960{
5961if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
5962{
5963 j3valid[iij3]=false; _ij3[1] = iij3; break;
5964}
5965}
5966j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
5967{
5968IkReal evalcond[6];
5969IkReal x461=IKcos(j3);
5970IkReal x462=IKsin(j3);
5971evalcond[0]=(gconst1*x461);
5972evalcond[1]=(new_r00*x461);
5973evalcond[2]=((-1.0)*new_r11*x461);
5974evalcond[3]=(((new_r00*x462))+gconst1);
5975evalcond[4]=(((new_r11*x462))+gconst1);
5976evalcond[5]=(((gconst1*x462))+new_r11);
5977if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
5978{
5979continue;
5980}
5981}
5982
5983{
5984std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5985vinfos[0].jointtype = 1;
5986vinfos[0].foffset = j0;
5987vinfos[0].indices[0] = _ij0[0];
5988vinfos[0].indices[1] = _ij0[1];
5989vinfos[0].maxsolutions = _nj0;
5990vinfos[1].jointtype = 1;
5991vinfos[1].foffset = j1;
5992vinfos[1].indices[0] = _ij1[0];
5993vinfos[1].indices[1] = _ij1[1];
5994vinfos[1].maxsolutions = _nj1;
5995vinfos[2].jointtype = 1;
5996vinfos[2].foffset = j2;
5997vinfos[2].indices[0] = _ij2[0];
5998vinfos[2].indices[1] = _ij2[1];
5999vinfos[2].maxsolutions = _nj2;
6000vinfos[3].jointtype = 1;
6001vinfos[3].foffset = j3;
6002vinfos[3].indices[0] = _ij3[0];
6003vinfos[3].indices[1] = _ij3[1];
6004vinfos[3].maxsolutions = _nj3;
6005vinfos[4].jointtype = 1;
6006vinfos[4].foffset = j4;
6007vinfos[4].indices[0] = _ij4[0];
6008vinfos[4].indices[1] = _ij4[1];
6009vinfos[4].maxsolutions = _nj4;
6010vinfos[5].jointtype = 1;
6011vinfos[5].foffset = j5;
6012vinfos[5].indices[0] = _ij5[0];
6013vinfos[5].indices[1] = _ij5[1];
6014vinfos[5].maxsolutions = _nj5;
6015std::vector<int> vfree(0);
6016solutions.AddSolution(vinfos,vfree);
6017}
6018}
6019}
6020
6021} else
6022{
6023{
6024IkReal j3array[2], cj3array[2], sj3array[2];
6025bool j3valid[2]={false};
6026_nj3 = 2;
6027CheckValue<IkReal> x463=IKPowWithIntegerCheck(new_r11,-1);
6028if(!x463.valid){
6029continue;
6030}
6031sj3array[0]=((-1.0)*gconst1*(x463.value));
6032if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6033{
6034 j3valid[0] = j3valid[1] = true;
6035 j3array[0] = IKasin(sj3array[0]);
6036 cj3array[0] = IKcos(j3array[0]);
6037 sj3array[1] = sj3array[0];
6038 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6039 cj3array[1] = -cj3array[0];
6040}
6041else if( isnan(sj3array[0]) )
6042{
6043 // probably any value will work
6044 j3valid[0] = true;
6045 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6046}
6047for(int ij3 = 0; ij3 < 2; ++ij3)
6048{
6049if( !j3valid[ij3] )
6050{
6051 continue;
6052}
6053_ij3[0] = ij3; _ij3[1] = -1;
6054for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6055{
6056if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6057{
6058 j3valid[iij3]=false; _ij3[1] = iij3; break;
6059}
6060}
6061j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6062{
6063IkReal evalcond[6];
6064IkReal x464=IKcos(j3);
6065IkReal x465=IKsin(j3);
6066IkReal x466=(gconst1*x465);
6067evalcond[0]=(gconst1*x464);
6068evalcond[1]=(new_r00*x464);
6069evalcond[2]=((-1.0)*new_r11*x464);
6070evalcond[3]=(((new_r00*x465))+gconst1);
6071evalcond[4]=(x466+new_r00);
6072evalcond[5]=(x466+new_r11);
6073if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
6074{
6075continue;
6076}
6077}
6078
6079{
6080std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6081vinfos[0].jointtype = 1;
6082vinfos[0].foffset = j0;
6083vinfos[0].indices[0] = _ij0[0];
6084vinfos[0].indices[1] = _ij0[1];
6085vinfos[0].maxsolutions = _nj0;
6086vinfos[1].jointtype = 1;
6087vinfos[1].foffset = j1;
6088vinfos[1].indices[0] = _ij1[0];
6089vinfos[1].indices[1] = _ij1[1];
6090vinfos[1].maxsolutions = _nj1;
6091vinfos[2].jointtype = 1;
6092vinfos[2].foffset = j2;
6093vinfos[2].indices[0] = _ij2[0];
6094vinfos[2].indices[1] = _ij2[1];
6095vinfos[2].maxsolutions = _nj2;
6096vinfos[3].jointtype = 1;
6097vinfos[3].foffset = j3;
6098vinfos[3].indices[0] = _ij3[0];
6099vinfos[3].indices[1] = _ij3[1];
6100vinfos[3].maxsolutions = _nj3;
6101vinfos[4].jointtype = 1;
6102vinfos[4].foffset = j4;
6103vinfos[4].indices[0] = _ij4[0];
6104vinfos[4].indices[1] = _ij4[1];
6105vinfos[4].maxsolutions = _nj4;
6106vinfos[5].jointtype = 1;
6107vinfos[5].foffset = j5;
6108vinfos[5].indices[0] = _ij5[0];
6109vinfos[5].indices[1] = _ij5[1];
6110vinfos[5].maxsolutions = _nj5;
6111std::vector<int> vfree(0);
6112solutions.AddSolution(vinfos,vfree);
6113}
6114}
6115}
6116
6117}
6118
6119}
6120
6121} else
6122{
6123{
6124IkReal j3array[2], cj3array[2], sj3array[2];
6125bool j3valid[2]={false};
6126_nj3 = 2;
6127CheckValue<IkReal> x467=IKPowWithIntegerCheck(new_r00,-1);
6128if(!x467.valid){
6129continue;
6130}
6131sj3array[0]=((-1.0)*gconst1*(x467.value));
6132if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
6133{
6134 j3valid[0] = j3valid[1] = true;
6135 j3array[0] = IKasin(sj3array[0]);
6136 cj3array[0] = IKcos(j3array[0]);
6137 sj3array[1] = sj3array[0];
6138 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
6139 cj3array[1] = -cj3array[0];
6140}
6141else if( isnan(sj3array[0]) )
6142{
6143 // probably any value will work
6144 j3valid[0] = true;
6145 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
6146}
6147for(int ij3 = 0; ij3 < 2; ++ij3)
6148{
6149if( !j3valid[ij3] )
6150{
6151 continue;
6152}
6153_ij3[0] = ij3; _ij3[1] = -1;
6154for(int iij3 = ij3+1; iij3 < 2; ++iij3)
6155{
6156if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6157{
6158 j3valid[iij3]=false; _ij3[1] = iij3; break;
6159}
6160}
6161j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6162{
6163IkReal evalcond[6];
6164IkReal x468=IKcos(j3);
6165IkReal x469=IKsin(j3);
6166IkReal x470=(gconst1*x469);
6167evalcond[0]=(gconst1*x468);
6168evalcond[1]=(new_r00*x468);
6169evalcond[2]=((-1.0)*new_r11*x468);
6170evalcond[3]=(((new_r11*x469))+gconst1);
6171evalcond[4]=(x470+new_r00);
6172evalcond[5]=(x470+new_r11);
6173if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
6174{
6175continue;
6176}
6177}
6178
6179{
6180std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6181vinfos[0].jointtype = 1;
6182vinfos[0].foffset = j0;
6183vinfos[0].indices[0] = _ij0[0];
6184vinfos[0].indices[1] = _ij0[1];
6185vinfos[0].maxsolutions = _nj0;
6186vinfos[1].jointtype = 1;
6187vinfos[1].foffset = j1;
6188vinfos[1].indices[0] = _ij1[0];
6189vinfos[1].indices[1] = _ij1[1];
6190vinfos[1].maxsolutions = _nj1;
6191vinfos[2].jointtype = 1;
6192vinfos[2].foffset = j2;
6193vinfos[2].indices[0] = _ij2[0];
6194vinfos[2].indices[1] = _ij2[1];
6195vinfos[2].maxsolutions = _nj2;
6196vinfos[3].jointtype = 1;
6197vinfos[3].foffset = j3;
6198vinfos[3].indices[0] = _ij3[0];
6199vinfos[3].indices[1] = _ij3[1];
6200vinfos[3].maxsolutions = _nj3;
6201vinfos[4].jointtype = 1;
6202vinfos[4].foffset = j4;
6203vinfos[4].indices[0] = _ij4[0];
6204vinfos[4].indices[1] = _ij4[1];
6205vinfos[4].maxsolutions = _nj4;
6206vinfos[5].jointtype = 1;
6207vinfos[5].foffset = j5;
6208vinfos[5].indices[0] = _ij5[0];
6209vinfos[5].indices[1] = _ij5[1];
6210vinfos[5].maxsolutions = _nj5;
6211std::vector<int> vfree(0);
6212solutions.AddSolution(vinfos,vfree);
6213}
6214}
6215}
6216
6217}
6218
6219}
6220
6221}
6222} while(0);
6223if( bgotonextstatement )
6224{
6225bool bgotonextstatement = true;
6226do
6227{
6228evalcond[0]=IKabs(new_r11);
6229if( IKabs(evalcond[0]) < 0.0000050000000000 )
6230{
6231bgotonextstatement=false;
6232{
6233IkReal j3eval[1];
6234IkReal x471=((-1.0)*new_r01);
6235CheckValue<IkReal> x473 = IKatan2WithCheck(IkReal(0),IkReal(x471),IKFAST_ATAN2_MAGTHRESH);
6236if(!x473.valid){
6237continue;
6238}
6239IkReal x472=((-1.0)*(x473.value));
6240sj4=0;
6241cj4=1.0;
6242j4=0;
6243sj5=gconst1;
6244cj5=gconst2;
6245j5=x472;
6246new_r11=0;
6247IkReal gconst0=x472;
6248IkReal gconst1=0;
6249IkReal x474 = new_r01*new_r01;
6250if(IKabs(x474)==0){
6251continue;
6252}
6253IkReal gconst2=(x471*(pow(x474,-0.5)));
6254j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
6255if( IKabs(j3eval[0]) < 0.0000010000000000 )
6256{
6257{
6258IkReal j3eval[1];
6259IkReal x475=((-1.0)*new_r01);
6260CheckValue<IkReal> x477 = IKatan2WithCheck(IkReal(0),IkReal(x475),IKFAST_ATAN2_MAGTHRESH);
6261if(!x477.valid){
6262continue;
6263}
6264IkReal x476=((-1.0)*(x477.value));
6265sj4=0;
6266cj4=1.0;
6267j4=0;
6268sj5=gconst1;
6269cj5=gconst2;
6270j5=x476;
6271new_r11=0;
6272IkReal gconst0=x476;
6273IkReal gconst1=0;
6274IkReal x478 = new_r01*new_r01;
6275if(IKabs(x478)==0){
6276continue;
6277}
6278IkReal gconst2=(x475*(pow(x478,-0.5)));
6279j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
6280if( IKabs(j3eval[0]) < 0.0000010000000000 )
6281{
6282{
6283IkReal j3eval[1];
6284IkReal x479=((-1.0)*new_r01);
6285CheckValue<IkReal> x481 = IKatan2WithCheck(IkReal(0),IkReal(x479),IKFAST_ATAN2_MAGTHRESH);
6286if(!x481.valid){
6287continue;
6288}
6289IkReal x480=((-1.0)*(x481.value));
6290sj4=0;
6291cj4=1.0;
6292j4=0;
6293sj5=gconst1;
6294cj5=gconst2;
6295j5=x480;
6296new_r11=0;
6297IkReal gconst0=x480;
6298IkReal gconst1=0;
6299IkReal x482 = new_r01*new_r01;
6300if(IKabs(x482)==0){
6301continue;
6302}
6303IkReal gconst2=(x479*(pow(x482,-0.5)));
6304j3eval[0]=new_r01;
6305if( IKabs(j3eval[0]) < 0.0000010000000000 )
6306{
6307continue; // 3 cases reached
6308
6309} else
6310{
6311{
6312IkReal j3array[1], cj3array[1], sj3array[1];
6313bool j3valid[1]={false};
6314_nj3 = 1;
6315CheckValue<IkReal> x483=IKPowWithIntegerCheck(new_r01,-1);
6316if(!x483.valid){
6317continue;
6318}
6319CheckValue<IkReal> x484=IKPowWithIntegerCheck(gconst2,-1);
6320if(!x484.valid){
6321continue;
6322}
6323if( IKabs(((-1.0)*gconst2*(x483.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x484.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst2*(x483.value)))+IKsqr((new_r00*(x484.value)))-1) <= IKFAST_SINCOS_THRESH )
6324 continue;
6325j3array[0]=IKatan2(((-1.0)*gconst2*(x483.value)), (new_r00*(x484.value)));
6326sj3array[0]=IKsin(j3array[0]);
6327cj3array[0]=IKcos(j3array[0]);
6328if( j3array[0] > IKPI )
6329{
6330 j3array[0]-=IK2PI;
6331}
6332else if( j3array[0] < -IKPI )
6333{ j3array[0]+=IK2PI;
6334}
6335j3valid[0] = true;
6336for(int ij3 = 0; ij3 < 1; ++ij3)
6337{
6338if( !j3valid[ij3] )
6339{
6340 continue;
6341}
6342_ij3[0] = ij3; _ij3[1] = -1;
6343for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6344{
6345if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6346{
6347 j3valid[iij3]=false; _ij3[1] = iij3; break;
6348}
6349}
6350j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6351{
6352IkReal evalcond[8];
6353IkReal x485=IKcos(j3);
6354IkReal x486=IKsin(j3);
6355IkReal x487=((1.0)*gconst2);
6356IkReal x488=(gconst2*x486);
6357evalcond[0]=(new_r01*x485);
6358evalcond[1]=((-1.0)*gconst2*x485);
6359evalcond[2]=(gconst2+((new_r01*x486)));
6360evalcond[3]=(x488+new_r01);
6361evalcond[4]=(new_r00+(((-1.0)*x485*x487)));
6362evalcond[5]=((((-1.0)*x486*x487))+new_r10);
6363evalcond[6]=((((-1.0)*new_r10*x485))+((new_r00*x486)));
6364evalcond[7]=((((-1.0)*x487))+((new_r10*x486))+((new_r00*x485)));
6365if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6366{
6367continue;
6368}
6369}
6370
6371{
6372std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6373vinfos[0].jointtype = 1;
6374vinfos[0].foffset = j0;
6375vinfos[0].indices[0] = _ij0[0];
6376vinfos[0].indices[1] = _ij0[1];
6377vinfos[0].maxsolutions = _nj0;
6378vinfos[1].jointtype = 1;
6379vinfos[1].foffset = j1;
6380vinfos[1].indices[0] = _ij1[0];
6381vinfos[1].indices[1] = _ij1[1];
6382vinfos[1].maxsolutions = _nj1;
6383vinfos[2].jointtype = 1;
6384vinfos[2].foffset = j2;
6385vinfos[2].indices[0] = _ij2[0];
6386vinfos[2].indices[1] = _ij2[1];
6387vinfos[2].maxsolutions = _nj2;
6388vinfos[3].jointtype = 1;
6389vinfos[3].foffset = j3;
6390vinfos[3].indices[0] = _ij3[0];
6391vinfos[3].indices[1] = _ij3[1];
6392vinfos[3].maxsolutions = _nj3;
6393vinfos[4].jointtype = 1;
6394vinfos[4].foffset = j4;
6395vinfos[4].indices[0] = _ij4[0];
6396vinfos[4].indices[1] = _ij4[1];
6397vinfos[4].maxsolutions = _nj4;
6398vinfos[5].jointtype = 1;
6399vinfos[5].foffset = j5;
6400vinfos[5].indices[0] = _ij5[0];
6401vinfos[5].indices[1] = _ij5[1];
6402vinfos[5].maxsolutions = _nj5;
6403std::vector<int> vfree(0);
6404solutions.AddSolution(vinfos,vfree);
6405}
6406}
6407}
6408
6409}
6410
6411}
6412
6413} else
6414{
6415{
6416IkReal j3array[1], cj3array[1], sj3array[1];
6417bool j3valid[1]={false};
6418_nj3 = 1;
6419CheckValue<IkReal> x489 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
6420if(!x489.valid){
6421continue;
6422}
6423CheckValue<IkReal> x490=IKPowWithIntegerCheck(IKsign(gconst2),-1);
6424if(!x490.valid){
6425continue;
6426}
6427j3array[0]=((-1.5707963267949)+(x489.value)+(((1.5707963267949)*(x490.value))));
6428sj3array[0]=IKsin(j3array[0]);
6429cj3array[0]=IKcos(j3array[0]);
6430if( j3array[0] > IKPI )
6431{
6432 j3array[0]-=IK2PI;
6433}
6434else if( j3array[0] < -IKPI )
6435{ j3array[0]+=IK2PI;
6436}
6437j3valid[0] = true;
6438for(int ij3 = 0; ij3 < 1; ++ij3)
6439{
6440if( !j3valid[ij3] )
6441{
6442 continue;
6443}
6444_ij3[0] = ij3; _ij3[1] = -1;
6445for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6446{
6447if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6448{
6449 j3valid[iij3]=false; _ij3[1] = iij3; break;
6450}
6451}
6452j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6453{
6454IkReal evalcond[8];
6455IkReal x491=IKcos(j3);
6456IkReal x492=IKsin(j3);
6457IkReal x493=((1.0)*gconst2);
6458IkReal x494=(gconst2*x492);
6459evalcond[0]=(new_r01*x491);
6460evalcond[1]=((-1.0)*gconst2*x491);
6461evalcond[2]=(gconst2+((new_r01*x492)));
6462evalcond[3]=(x494+new_r01);
6463evalcond[4]=((((-1.0)*x491*x493))+new_r00);
6464evalcond[5]=(new_r10+(((-1.0)*x492*x493)));
6465evalcond[6]=((((-1.0)*new_r10*x491))+((new_r00*x492)));
6466evalcond[7]=((((-1.0)*x493))+((new_r10*x492))+((new_r00*x491)));
6467if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6468{
6469continue;
6470}
6471}
6472
6473{
6474std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6475vinfos[0].jointtype = 1;
6476vinfos[0].foffset = j0;
6477vinfos[0].indices[0] = _ij0[0];
6478vinfos[0].indices[1] = _ij0[1];
6479vinfos[0].maxsolutions = _nj0;
6480vinfos[1].jointtype = 1;
6481vinfos[1].foffset = j1;
6482vinfos[1].indices[0] = _ij1[0];
6483vinfos[1].indices[1] = _ij1[1];
6484vinfos[1].maxsolutions = _nj1;
6485vinfos[2].jointtype = 1;
6486vinfos[2].foffset = j2;
6487vinfos[2].indices[0] = _ij2[0];
6488vinfos[2].indices[1] = _ij2[1];
6489vinfos[2].maxsolutions = _nj2;
6490vinfos[3].jointtype = 1;
6491vinfos[3].foffset = j3;
6492vinfos[3].indices[0] = _ij3[0];
6493vinfos[3].indices[1] = _ij3[1];
6494vinfos[3].maxsolutions = _nj3;
6495vinfos[4].jointtype = 1;
6496vinfos[4].foffset = j4;
6497vinfos[4].indices[0] = _ij4[0];
6498vinfos[4].indices[1] = _ij4[1];
6499vinfos[4].maxsolutions = _nj4;
6500vinfos[5].jointtype = 1;
6501vinfos[5].foffset = j5;
6502vinfos[5].indices[0] = _ij5[0];
6503vinfos[5].indices[1] = _ij5[1];
6504vinfos[5].maxsolutions = _nj5;
6505std::vector<int> vfree(0);
6506solutions.AddSolution(vinfos,vfree);
6507}
6508}
6509}
6510
6511}
6512
6513}
6514
6515} else
6516{
6517{
6518IkReal j3array[1], cj3array[1], sj3array[1];
6519bool j3valid[1]={false};
6520_nj3 = 1;
6521CheckValue<IkReal> x495=IKPowWithIntegerCheck(IKsign(gconst2),-1);
6522if(!x495.valid){
6523continue;
6524}
6525CheckValue<IkReal> x496 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
6526if(!x496.valid){
6527continue;
6528}
6529j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x495.value)))+(x496.value));
6530sj3array[0]=IKsin(j3array[0]);
6531cj3array[0]=IKcos(j3array[0]);
6532if( j3array[0] > IKPI )
6533{
6534 j3array[0]-=IK2PI;
6535}
6536else if( j3array[0] < -IKPI )
6537{ j3array[0]+=IK2PI;
6538}
6539j3valid[0] = true;
6540for(int ij3 = 0; ij3 < 1; ++ij3)
6541{
6542if( !j3valid[ij3] )
6543{
6544 continue;
6545}
6546_ij3[0] = ij3; _ij3[1] = -1;
6547for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6548{
6549if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6550{
6551 j3valid[iij3]=false; _ij3[1] = iij3; break;
6552}
6553}
6554j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6555{
6556IkReal evalcond[8];
6557IkReal x497=IKcos(j3);
6558IkReal x498=IKsin(j3);
6559IkReal x499=((1.0)*gconst2);
6560IkReal x500=(gconst2*x498);
6561evalcond[0]=(new_r01*x497);
6562evalcond[1]=((-1.0)*gconst2*x497);
6563evalcond[2]=(gconst2+((new_r01*x498)));
6564evalcond[3]=(x500+new_r01);
6565evalcond[4]=((((-1.0)*x497*x499))+new_r00);
6566evalcond[5]=((((-1.0)*x498*x499))+new_r10);
6567evalcond[6]=((((-1.0)*new_r10*x497))+((new_r00*x498)));
6568evalcond[7]=((((-1.0)*x499))+((new_r10*x498))+((new_r00*x497)));
6569if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6570{
6571continue;
6572}
6573}
6574
6575{
6576std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6577vinfos[0].jointtype = 1;
6578vinfos[0].foffset = j0;
6579vinfos[0].indices[0] = _ij0[0];
6580vinfos[0].indices[1] = _ij0[1];
6581vinfos[0].maxsolutions = _nj0;
6582vinfos[1].jointtype = 1;
6583vinfos[1].foffset = j1;
6584vinfos[1].indices[0] = _ij1[0];
6585vinfos[1].indices[1] = _ij1[1];
6586vinfos[1].maxsolutions = _nj1;
6587vinfos[2].jointtype = 1;
6588vinfos[2].foffset = j2;
6589vinfos[2].indices[0] = _ij2[0];
6590vinfos[2].indices[1] = _ij2[1];
6591vinfos[2].maxsolutions = _nj2;
6592vinfos[3].jointtype = 1;
6593vinfos[3].foffset = j3;
6594vinfos[3].indices[0] = _ij3[0];
6595vinfos[3].indices[1] = _ij3[1];
6596vinfos[3].maxsolutions = _nj3;
6597vinfos[4].jointtype = 1;
6598vinfos[4].foffset = j4;
6599vinfos[4].indices[0] = _ij4[0];
6600vinfos[4].indices[1] = _ij4[1];
6601vinfos[4].maxsolutions = _nj4;
6602vinfos[5].jointtype = 1;
6603vinfos[5].foffset = j5;
6604vinfos[5].indices[0] = _ij5[0];
6605vinfos[5].indices[1] = _ij5[1];
6606vinfos[5].maxsolutions = _nj5;
6607std::vector<int> vfree(0);
6608solutions.AddSolution(vinfos,vfree);
6609}
6610}
6611}
6612
6613}
6614
6615}
6616
6617}
6618} while(0);
6619if( bgotonextstatement )
6620{
6621bool bgotonextstatement = true;
6622do
6623{
6624if( 1 )
6625{
6626bgotonextstatement=false;
6627continue; // branch miss [j3]
6628
6629}
6630} while(0);
6631if( bgotonextstatement )
6632{
6633}
6634}
6635}
6636}
6637}
6638}
6639
6640} else
6641{
6642{
6643IkReal j3array[1], cj3array[1], sj3array[1];
6644bool j3valid[1]={false};
6645_nj3 = 1;
6646IkReal x501=((1.0)*new_r01);
6647CheckValue<IkReal> x502 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst1*gconst1))))),IkReal(((((-1.0)*new_r11*x501))+((gconst1*gconst2)))),IKFAST_ATAN2_MAGTHRESH);
6648if(!x502.valid){
6649continue;
6650}
6651CheckValue<IkReal> x503=IKPowWithIntegerCheck(IKsign((((gconst1*new_r11))+(((-1.0)*gconst2*x501)))),-1);
6652if(!x503.valid){
6653continue;
6654}
6655j3array[0]=((-1.5707963267949)+(x502.value)+(((1.5707963267949)*(x503.value))));
6656sj3array[0]=IKsin(j3array[0]);
6657cj3array[0]=IKcos(j3array[0]);
6658if( j3array[0] > IKPI )
6659{
6660 j3array[0]-=IK2PI;
6661}
6662else if( j3array[0] < -IKPI )
6663{ j3array[0]+=IK2PI;
6664}
6665j3valid[0] = true;
6666for(int ij3 = 0; ij3 < 1; ++ij3)
6667{
6668if( !j3valid[ij3] )
6669{
6670 continue;
6671}
6672_ij3[0] = ij3; _ij3[1] = -1;
6673for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6674{
6675if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6676{
6677 j3valid[iij3]=false; _ij3[1] = iij3; break;
6678}
6679}
6680j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6681{
6682IkReal evalcond[8];
6683IkReal x504=IKcos(j3);
6684IkReal x505=IKsin(j3);
6685IkReal x506=((1.0)*gconst2);
6686IkReal x507=(gconst1*x505);
6687IkReal x508=(gconst2*x505);
6688IkReal x509=(gconst1*x504);
6689IkReal x510=((1.0)*x504);
6690IkReal x511=(x504*x506);
6691evalcond[0]=(gconst1+((new_r11*x505))+((new_r01*x504)));
6692evalcond[1]=(x508+x509+new_r01);
6693evalcond[2]=((((-1.0)*new_r10*x510))+gconst1+((new_r00*x505)));
6694evalcond[3]=((((-1.0)*new_r11*x510))+gconst2+((new_r01*x505)));
6695evalcond[4]=(x507+new_r00+(((-1.0)*x511)));
6696evalcond[5]=(x507+new_r11+(((-1.0)*x511)));
6697evalcond[6]=((((-1.0)*x506))+((new_r10*x505))+((new_r00*x504)));
6698evalcond[7]=((((-1.0)*x505*x506))+new_r10+(((-1.0)*x509)));
6699if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6700{
6701continue;
6702}
6703}
6704
6705{
6706std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6707vinfos[0].jointtype = 1;
6708vinfos[0].foffset = j0;
6709vinfos[0].indices[0] = _ij0[0];
6710vinfos[0].indices[1] = _ij0[1];
6711vinfos[0].maxsolutions = _nj0;
6712vinfos[1].jointtype = 1;
6713vinfos[1].foffset = j1;
6714vinfos[1].indices[0] = _ij1[0];
6715vinfos[1].indices[1] = _ij1[1];
6716vinfos[1].maxsolutions = _nj1;
6717vinfos[2].jointtype = 1;
6718vinfos[2].foffset = j2;
6719vinfos[2].indices[0] = _ij2[0];
6720vinfos[2].indices[1] = _ij2[1];
6721vinfos[2].maxsolutions = _nj2;
6722vinfos[3].jointtype = 1;
6723vinfos[3].foffset = j3;
6724vinfos[3].indices[0] = _ij3[0];
6725vinfos[3].indices[1] = _ij3[1];
6726vinfos[3].maxsolutions = _nj3;
6727vinfos[4].jointtype = 1;
6728vinfos[4].foffset = j4;
6729vinfos[4].indices[0] = _ij4[0];
6730vinfos[4].indices[1] = _ij4[1];
6731vinfos[4].maxsolutions = _nj4;
6732vinfos[5].jointtype = 1;
6733vinfos[5].foffset = j5;
6734vinfos[5].indices[0] = _ij5[0];
6735vinfos[5].indices[1] = _ij5[1];
6736vinfos[5].maxsolutions = _nj5;
6737std::vector<int> vfree(0);
6738solutions.AddSolution(vinfos,vfree);
6739}
6740}
6741}
6742
6743}
6744
6745}
6746
6747} else
6748{
6749{
6750IkReal j3array[1], cj3array[1], sj3array[1];
6751bool j3valid[1]={false};
6752_nj3 = 1;
6753IkReal x512=((1.0)*gconst2);
6754CheckValue<IkReal> x513 = IKatan2WithCheck(IkReal(((gconst1*gconst1)+((new_r01*new_r10)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst1*x512)))),IKFAST_ATAN2_MAGTHRESH);
6755if(!x513.valid){
6756continue;
6757}
6758CheckValue<IkReal> x514=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x512))+(((-1.0)*gconst1*new_r00)))),-1);
6759if(!x514.valid){
6760continue;
6761}
6762j3array[0]=((-1.5707963267949)+(x513.value)+(((1.5707963267949)*(x514.value))));
6763sj3array[0]=IKsin(j3array[0]);
6764cj3array[0]=IKcos(j3array[0]);
6765if( j3array[0] > IKPI )
6766{
6767 j3array[0]-=IK2PI;
6768}
6769else if( j3array[0] < -IKPI )
6770{ j3array[0]+=IK2PI;
6771}
6772j3valid[0] = true;
6773for(int ij3 = 0; ij3 < 1; ++ij3)
6774{
6775if( !j3valid[ij3] )
6776{
6777 continue;
6778}
6779_ij3[0] = ij3; _ij3[1] = -1;
6780for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6781{
6782if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6783{
6784 j3valid[iij3]=false; _ij3[1] = iij3; break;
6785}
6786}
6787j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6788{
6789IkReal evalcond[8];
6790IkReal x515=IKcos(j3);
6791IkReal x516=IKsin(j3);
6792IkReal x517=((1.0)*gconst2);
6793IkReal x518=(gconst1*x516);
6794IkReal x519=(gconst2*x516);
6795IkReal x520=(gconst1*x515);
6796IkReal x521=((1.0)*x515);
6797IkReal x522=(x515*x517);
6798evalcond[0]=(((new_r01*x515))+((new_r11*x516))+gconst1);
6799evalcond[1]=(x520+x519+new_r01);
6800evalcond[2]=(((new_r00*x516))+gconst1+(((-1.0)*new_r10*x521)));
6801evalcond[3]=(((new_r01*x516))+gconst2+(((-1.0)*new_r11*x521)));
6802evalcond[4]=((((-1.0)*x522))+x518+new_r00);
6803evalcond[5]=((((-1.0)*x522))+x518+new_r11);
6804evalcond[6]=(((new_r00*x515))+((new_r10*x516))+(((-1.0)*x517)));
6805evalcond[7]=((((-1.0)*x520))+(((-1.0)*x516*x517))+new_r10);
6806if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6807{
6808continue;
6809}
6810}
6811
6812{
6813std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6814vinfos[0].jointtype = 1;
6815vinfos[0].foffset = j0;
6816vinfos[0].indices[0] = _ij0[0];
6817vinfos[0].indices[1] = _ij0[1];
6818vinfos[0].maxsolutions = _nj0;
6819vinfos[1].jointtype = 1;
6820vinfos[1].foffset = j1;
6821vinfos[1].indices[0] = _ij1[0];
6822vinfos[1].indices[1] = _ij1[1];
6823vinfos[1].maxsolutions = _nj1;
6824vinfos[2].jointtype = 1;
6825vinfos[2].foffset = j2;
6826vinfos[2].indices[0] = _ij2[0];
6827vinfos[2].indices[1] = _ij2[1];
6828vinfos[2].maxsolutions = _nj2;
6829vinfos[3].jointtype = 1;
6830vinfos[3].foffset = j3;
6831vinfos[3].indices[0] = _ij3[0];
6832vinfos[3].indices[1] = _ij3[1];
6833vinfos[3].maxsolutions = _nj3;
6834vinfos[4].jointtype = 1;
6835vinfos[4].foffset = j4;
6836vinfos[4].indices[0] = _ij4[0];
6837vinfos[4].indices[1] = _ij4[1];
6838vinfos[4].maxsolutions = _nj4;
6839vinfos[5].jointtype = 1;
6840vinfos[5].foffset = j5;
6841vinfos[5].indices[0] = _ij5[0];
6842vinfos[5].indices[1] = _ij5[1];
6843vinfos[5].maxsolutions = _nj5;
6844std::vector<int> vfree(0);
6845solutions.AddSolution(vinfos,vfree);
6846}
6847}
6848}
6849
6850}
6851
6852}
6853
6854} else
6855{
6856{
6857IkReal j3array[1], cj3array[1], sj3array[1];
6858bool j3valid[1]={false};
6859_nj3 = 1;
6860IkReal x523=((1.0)*new_r11);
6861CheckValue<IkReal> x524 = IKatan2WithCheck(IkReal((((gconst1*new_r10))+((gconst1*new_r01)))),IkReal((((gconst1*new_r00))+(((-1.0)*gconst1*x523)))),IKFAST_ATAN2_MAGTHRESH);
6862if(!x524.valid){
6863continue;
6864}
6865CheckValue<IkReal> x525=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x523))+(((-1.0)*new_r00*new_r01)))),-1);
6866if(!x525.valid){
6867continue;
6868}
6869j3array[0]=((-1.5707963267949)+(x524.value)+(((1.5707963267949)*(x525.value))));
6870sj3array[0]=IKsin(j3array[0]);
6871cj3array[0]=IKcos(j3array[0]);
6872if( j3array[0] > IKPI )
6873{
6874 j3array[0]-=IK2PI;
6875}
6876else if( j3array[0] < -IKPI )
6877{ j3array[0]+=IK2PI;
6878}
6879j3valid[0] = true;
6880for(int ij3 = 0; ij3 < 1; ++ij3)
6881{
6882if( !j3valid[ij3] )
6883{
6884 continue;
6885}
6886_ij3[0] = ij3; _ij3[1] = -1;
6887for(int iij3 = ij3+1; iij3 < 1; ++iij3)
6888{
6889if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
6890{
6891 j3valid[iij3]=false; _ij3[1] = iij3; break;
6892}
6893}
6894j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
6895{
6896IkReal evalcond[8];
6897IkReal x526=IKcos(j3);
6898IkReal x527=IKsin(j3);
6899IkReal x528=((1.0)*gconst2);
6900IkReal x529=(gconst1*x527);
6901IkReal x530=(gconst2*x527);
6902IkReal x531=(gconst1*x526);
6903IkReal x532=((1.0)*x526);
6904IkReal x533=(x526*x528);
6905evalcond[0]=(((new_r01*x526))+gconst1+((new_r11*x527)));
6906evalcond[1]=(x531+x530+new_r01);
6907evalcond[2]=(gconst1+(((-1.0)*new_r10*x532))+((new_r00*x527)));
6908evalcond[3]=(((new_r01*x527))+gconst2+(((-1.0)*new_r11*x532)));
6909evalcond[4]=((((-1.0)*x533))+x529+new_r00);
6910evalcond[5]=((((-1.0)*x533))+x529+new_r11);
6911evalcond[6]=((((-1.0)*x528))+((new_r10*x527))+((new_r00*x526)));
6912evalcond[7]=((((-1.0)*x527*x528))+(((-1.0)*x531))+new_r10);
6913if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
6914{
6915continue;
6916}
6917}
6918
6919{
6920std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6921vinfos[0].jointtype = 1;
6922vinfos[0].foffset = j0;
6923vinfos[0].indices[0] = _ij0[0];
6924vinfos[0].indices[1] = _ij0[1];
6925vinfos[0].maxsolutions = _nj0;
6926vinfos[1].jointtype = 1;
6927vinfos[1].foffset = j1;
6928vinfos[1].indices[0] = _ij1[0];
6929vinfos[1].indices[1] = _ij1[1];
6930vinfos[1].maxsolutions = _nj1;
6931vinfos[2].jointtype = 1;
6932vinfos[2].foffset = j2;
6933vinfos[2].indices[0] = _ij2[0];
6934vinfos[2].indices[1] = _ij2[1];
6935vinfos[2].maxsolutions = _nj2;
6936vinfos[3].jointtype = 1;
6937vinfos[3].foffset = j3;
6938vinfos[3].indices[0] = _ij3[0];
6939vinfos[3].indices[1] = _ij3[1];
6940vinfos[3].maxsolutions = _nj3;
6941vinfos[4].jointtype = 1;
6942vinfos[4].foffset = j4;
6943vinfos[4].indices[0] = _ij4[0];
6944vinfos[4].indices[1] = _ij4[1];
6945vinfos[4].maxsolutions = _nj4;
6946vinfos[5].jointtype = 1;
6947vinfos[5].foffset = j5;
6948vinfos[5].indices[0] = _ij5[0];
6949vinfos[5].indices[1] = _ij5[1];
6950vinfos[5].maxsolutions = _nj5;
6951std::vector<int> vfree(0);
6952solutions.AddSolution(vinfos,vfree);
6953}
6954}
6955}
6956
6957}
6958
6959}
6960
6961}
6962} while(0);
6963if( bgotonextstatement )
6964{
6965bool bgotonextstatement = true;
6966do
6967{
6968IkReal x534=((-1.0)*new_r11);
6969IkReal x536 = ((new_r01*new_r01)+(new_r11*new_r11));
6970if(IKabs(x536)==0){
6971continue;
6972}
6973IkReal x535=pow(x536,-0.5);
6974CheckValue<IkReal> x537 = IKatan2WithCheck(IkReal(x534),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6975if(!x537.valid){
6976continue;
6977}
6978IkReal gconst3=((3.14159265358979)+(((-1.0)*(x537.value))));
6979IkReal gconst4=(x534*x535);
6980IkReal gconst5=((1.0)*new_r01*x535);
6981CheckValue<IkReal> x538 = IKatan2WithCheck(IkReal(((-1.0)*new_r11)),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6982if(!x538.valid){
6983continue;
6984}
6985evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x538.value)+j5)))), 6.28318530717959)));
6986if( IKabs(evalcond[0]) < 0.0000050000000000 )
6987{
6988bgotonextstatement=false;
6989{
6990IkReal j3eval[3];
6991IkReal x539=((-1.0)*new_r11);
6992CheckValue<IkReal> x542 = IKatan2WithCheck(IkReal(x539),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
6993if(!x542.valid){
6994continue;
6995}
6996IkReal x540=((1.0)*(x542.value));
6997IkReal x541=x535;
6998sj4=0;
6999cj4=1.0;
7000j4=0;
7001sj5=gconst4;
7002cj5=gconst5;
7003j5=((3.14159265)+(((-1.0)*x540)));
7004IkReal gconst3=((3.14159265358979)+(((-1.0)*x540)));
7005IkReal gconst4=(x539*x541);
7006IkReal gconst5=((1.0)*new_r01*x541);
7007IkReal x543=new_r11*new_r11;
7008IkReal x544=((1.0)*new_r01);
7009IkReal x545=((1.0)*new_r10);
7010IkReal x546=((((-1.0)*new_r00*x544))+(((-1.0)*new_r11*x545)));
7011IkReal x547=x535;
7012IkReal x548=(new_r11*x547);
7013j3eval[0]=x546;
7014j3eval[1]=((IKabs((((x543*x547))+(((-1.0)*new_r00*x548)))))+(IKabs(((((-1.0)*x544*x548))+(((-1.0)*x545*x548))))));
7015j3eval[2]=IKsign(x546);
7016if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7017{
7018{
7019IkReal j3eval[1];
7020IkReal x549=((-1.0)*new_r11);
7021CheckValue<IkReal> x552 = IKatan2WithCheck(IkReal(x549),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7022if(!x552.valid){
7023continue;
7024}
7025IkReal x550=((1.0)*(x552.value));
7026IkReal x551=x535;
7027sj4=0;
7028cj4=1.0;
7029j4=0;
7030sj5=gconst4;
7031cj5=gconst5;
7032j5=((3.14159265)+(((-1.0)*x550)));
7033IkReal gconst3=((3.14159265358979)+(((-1.0)*x550)));
7034IkReal gconst4=(x549*x551);
7035IkReal gconst5=((1.0)*new_r01*x551);
7036IkReal x553=new_r11*new_r11;
7037IkReal x554=new_r01*new_r01*new_r01;
7038CheckValue<IkReal> x558=IKPowWithIntegerCheck(((new_r01*new_r01)+x553),-1);
7039if(!x558.valid){
7040continue;
7041}
7042IkReal x555=x558.value;
7043IkReal x556=(x553*x555);
7044IkReal x557=(x554*x555);
7045j3eval[0]=((IKabs((((new_r10*x557))+x556+((new_r01*new_r10*x556)))))+(IKabs((((new_r00*new_r01*x556))+((new_r01*new_r11*x555))+((new_r00*x557))))));
7046if( IKabs(j3eval[0]) < 0.0000010000000000 )
7047{
7048{
7049IkReal j3eval[1];
7050IkReal x559=((-1.0)*new_r11);
7051CheckValue<IkReal> x562 = IKatan2WithCheck(IkReal(x559),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7052if(!x562.valid){
7053continue;
7054}
7055IkReal x560=((1.0)*(x562.value));
7056IkReal x561=x535;
7057sj4=0;
7058cj4=1.0;
7059j4=0;
7060sj5=gconst4;
7061cj5=gconst5;
7062j5=((3.14159265)+(((-1.0)*x560)));
7063IkReal gconst3=((3.14159265358979)+(((-1.0)*x560)));
7064IkReal gconst4=(x559*x561);
7065IkReal gconst5=((1.0)*new_r01*x561);
7066IkReal x563=new_r01*new_r01;
7067IkReal x564=new_r11*new_r11;
7068CheckValue<IkReal> x571=IKPowWithIntegerCheck((x564+x563),-1);
7069if(!x571.valid){
7070continue;
7071}
7072IkReal x565=x571.value;
7073IkReal x566=(x564*x565);
7074CheckValue<IkReal> x572=IKPowWithIntegerCheck(((((-1.0)*x563))+(((-1.0)*x564))),-1);
7075if(!x572.valid){
7076continue;
7077}
7078IkReal x567=x572.value;
7079IkReal x568=((1.0)*x567);
7080IkReal x569=(new_r11*x568);
7081IkReal x570=(new_r01*x568);
7082j3eval[0]=((IKabs((((x563*x566))+((x565*(x563*x563)))+(((-1.0)*x566)))))+(IKabs(((((-1.0)*new_r01*x569*(new_r11*new_r11)))+(((-1.0)*x569*(new_r01*new_r01*new_r01)))+(((-1.0)*new_r01*x569))))));
7083if( IKabs(j3eval[0]) < 0.0000010000000000 )
7084{
7085{
7086IkReal evalcond[3];
7087bool bgotonextstatement = true;
7088do
7089{
7090evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
7091if( IKabs(evalcond[0]) < 0.0000050000000000 )
7092{
7093bgotonextstatement=false;
7094{
7095IkReal j3array[2], cj3array[2], sj3array[2];
7096bool j3valid[2]={false};
7097_nj3 = 2;
7098CheckValue<IkReal> x573=IKPowWithIntegerCheck(gconst5,-1);
7099if(!x573.valid){
7100continue;
7101}
7102sj3array[0]=(new_r10*(x573.value));
7103if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7104{
7105 j3valid[0] = j3valid[1] = true;
7106 j3array[0] = IKasin(sj3array[0]);
7107 cj3array[0] = IKcos(j3array[0]);
7108 sj3array[1] = sj3array[0];
7109 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7110 cj3array[1] = -cj3array[0];
7111}
7112else if( isnan(sj3array[0]) )
7113{
7114 // probably any value will work
7115 j3valid[0] = true;
7116 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7117}
7118for(int ij3 = 0; ij3 < 2; ++ij3)
7119{
7120if( !j3valid[ij3] )
7121{
7122 continue;
7123}
7124_ij3[0] = ij3; _ij3[1] = -1;
7125for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7126{
7127if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7128{
7129 j3valid[iij3]=false; _ij3[1] = iij3; break;
7130}
7131}
7132j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7133{
7134IkReal evalcond[6];
7135IkReal x574=IKcos(j3);
7136IkReal x575=IKsin(j3);
7137IkReal x576=((-1.0)*x574);
7138evalcond[0]=(new_r01*x574);
7139evalcond[1]=(new_r10*x576);
7140evalcond[2]=(gconst5*x576);
7141evalcond[3]=(((new_r01*x575))+gconst5);
7142evalcond[4]=(((gconst5*x575))+new_r01);
7143evalcond[5]=(((new_r10*x575))+(((-1.0)*gconst5)));
7144if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7145{
7146continue;
7147}
7148}
7149
7150{
7151std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7152vinfos[0].jointtype = 1;
7153vinfos[0].foffset = j0;
7154vinfos[0].indices[0] = _ij0[0];
7155vinfos[0].indices[1] = _ij0[1];
7156vinfos[0].maxsolutions = _nj0;
7157vinfos[1].jointtype = 1;
7158vinfos[1].foffset = j1;
7159vinfos[1].indices[0] = _ij1[0];
7160vinfos[1].indices[1] = _ij1[1];
7161vinfos[1].maxsolutions = _nj1;
7162vinfos[2].jointtype = 1;
7163vinfos[2].foffset = j2;
7164vinfos[2].indices[0] = _ij2[0];
7165vinfos[2].indices[1] = _ij2[1];
7166vinfos[2].maxsolutions = _nj2;
7167vinfos[3].jointtype = 1;
7168vinfos[3].foffset = j3;
7169vinfos[3].indices[0] = _ij3[0];
7170vinfos[3].indices[1] = _ij3[1];
7171vinfos[3].maxsolutions = _nj3;
7172vinfos[4].jointtype = 1;
7173vinfos[4].foffset = j4;
7174vinfos[4].indices[0] = _ij4[0];
7175vinfos[4].indices[1] = _ij4[1];
7176vinfos[4].maxsolutions = _nj4;
7177vinfos[5].jointtype = 1;
7178vinfos[5].foffset = j5;
7179vinfos[5].indices[0] = _ij5[0];
7180vinfos[5].indices[1] = _ij5[1];
7181vinfos[5].maxsolutions = _nj5;
7182std::vector<int> vfree(0);
7183solutions.AddSolution(vinfos,vfree);
7184}
7185}
7186}
7187
7188}
7189} while(0);
7190if( bgotonextstatement )
7191{
7192bool bgotonextstatement = true;
7193do
7194{
7195evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7196evalcond[1]=gconst4;
7197evalcond[2]=gconst5;
7198if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 )
7199{
7200bgotonextstatement=false;
7201{
7202IkReal j3eval[3];
7203IkReal x577=((-1.0)*new_r11);
7204CheckValue<IkReal> x579 = IKatan2WithCheck(IkReal(x577),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7205if(!x579.valid){
7206continue;
7207}
7208IkReal x578=((1.0)*(x579.value));
7209sj4=0;
7210cj4=1.0;
7211j4=0;
7212sj5=gconst4;
7213cj5=gconst5;
7214j5=((3.14159265)+(((-1.0)*x578)));
7215new_r00=0;
7216new_r10=0;
7217new_r21=0;
7218new_r22=0;
7219IkReal gconst3=((3.14159265358979)+(((-1.0)*x578)));
7220IkReal gconst4=x577;
7221IkReal gconst5=((1.0)*new_r01);
7222j3eval[0]=1.0;
7223j3eval[1]=1.0;
7224j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r01*new_r01))))))+(IKabs(((1.0)*new_r01*new_r11))));
7225if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7226{
7227{
7228IkReal j3eval[4];
7229IkReal x580=((-1.0)*new_r11);
7230CheckValue<IkReal> x582 = IKatan2WithCheck(IkReal(x580),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7231if(!x582.valid){
7232continue;
7233}
7234IkReal x581=((1.0)*(x582.value));
7235sj4=0;
7236cj4=1.0;
7237j4=0;
7238sj5=gconst4;
7239cj5=gconst5;
7240j5=((3.14159265)+(((-1.0)*x581)));
7241new_r00=0;
7242new_r10=0;
7243new_r21=0;
7244new_r22=0;
7245IkReal gconst3=((3.14159265358979)+(((-1.0)*x581)));
7246IkReal gconst4=x580;
7247IkReal gconst5=((1.0)*new_r01);
7248j3eval[0]=-1.0;
7249j3eval[1]=new_r01;
7250j3eval[2]=1.0;
7251j3eval[3]=-1.0;
7252if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 || IKabs(j3eval[3]) < 0.0000010000000000 )
7253{
7254{
7255IkReal j3eval[3];
7256IkReal x583=((-1.0)*new_r11);
7257CheckValue<IkReal> x585 = IKatan2WithCheck(IkReal(x583),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7258if(!x585.valid){
7259continue;
7260}
7261IkReal x584=((1.0)*(x585.value));
7262sj4=0;
7263cj4=1.0;
7264j4=0;
7265sj5=gconst4;
7266cj5=gconst5;
7267j5=((3.14159265)+(((-1.0)*x584)));
7268new_r00=0;
7269new_r10=0;
7270new_r21=0;
7271new_r22=0;
7272IkReal gconst3=((3.14159265358979)+(((-1.0)*x584)));
7273IkReal gconst4=x583;
7274IkReal gconst5=((1.0)*new_r01);
7275j3eval[0]=-1.0;
7276j3eval[1]=-1.0;
7277j3eval[2]=((IKabs(((2.0)*new_r01*new_r11)))+(IKabs(((-1.0)+(((2.0)*(new_r01*new_r01)))))));
7278if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
7279{
7280continue; // 3 cases reached
7281
7282} else
7283{
7284{
7285IkReal j3array[1], cj3array[1], sj3array[1];
7286bool j3valid[1]={false};
7287_nj3 = 1;
7288IkReal x586=((1.0)*new_r01);
7289CheckValue<IkReal> x587 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst4*gconst4))))),IkReal(((((-1.0)*new_r11*x586))+((gconst4*gconst5)))),IKFAST_ATAN2_MAGTHRESH);
7290if(!x587.valid){
7291continue;
7292}
7293CheckValue<IkReal> x588=IKPowWithIntegerCheck(IKsign((((gconst4*new_r11))+(((-1.0)*gconst5*x586)))),-1);
7294if(!x588.valid){
7295continue;
7296}
7297j3array[0]=((-1.5707963267949)+(x587.value)+(((1.5707963267949)*(x588.value))));
7298sj3array[0]=IKsin(j3array[0]);
7299cj3array[0]=IKcos(j3array[0]);
7300if( j3array[0] > IKPI )
7301{
7302 j3array[0]-=IK2PI;
7303}
7304else if( j3array[0] < -IKPI )
7305{ j3array[0]+=IK2PI;
7306}
7307j3valid[0] = true;
7308for(int ij3 = 0; ij3 < 1; ++ij3)
7309{
7310if( !j3valid[ij3] )
7311{
7312 continue;
7313}
7314_ij3[0] = ij3; _ij3[1] = -1;
7315for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7316{
7317if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7318{
7319 j3valid[iij3]=false; _ij3[1] = iij3; break;
7320}
7321}
7322j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7323{
7324IkReal evalcond[6];
7325IkReal x589=IKsin(j3);
7326IkReal x590=IKcos(j3);
7327IkReal x591=(gconst4*x589);
7328IkReal x592=(gconst4*x590);
7329IkReal x593=((1.0)*x590);
7330IkReal x594=(gconst5*x589);
7331IkReal x595=(gconst5*x593);
7332evalcond[0]=(gconst4+((new_r01*x590))+((new_r11*x589)));
7333evalcond[1]=(x594+x592+new_r01);
7334evalcond[2]=((((-1.0)*x595))+x591);
7335evalcond[3]=((((-1.0)*new_r11*x593))+gconst5+((new_r01*x589)));
7336evalcond[4]=((((-1.0)*x595))+x591+new_r11);
7337evalcond[5]=((((-1.0)*x594))+(((-1.0)*x592)));
7338if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7339{
7340continue;
7341}
7342}
7343
7344{
7345std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7346vinfos[0].jointtype = 1;
7347vinfos[0].foffset = j0;
7348vinfos[0].indices[0] = _ij0[0];
7349vinfos[0].indices[1] = _ij0[1];
7350vinfos[0].maxsolutions = _nj0;
7351vinfos[1].jointtype = 1;
7352vinfos[1].foffset = j1;
7353vinfos[1].indices[0] = _ij1[0];
7354vinfos[1].indices[1] = _ij1[1];
7355vinfos[1].maxsolutions = _nj1;
7356vinfos[2].jointtype = 1;
7357vinfos[2].foffset = j2;
7358vinfos[2].indices[0] = _ij2[0];
7359vinfos[2].indices[1] = _ij2[1];
7360vinfos[2].maxsolutions = _nj2;
7361vinfos[3].jointtype = 1;
7362vinfos[3].foffset = j3;
7363vinfos[3].indices[0] = _ij3[0];
7364vinfos[3].indices[1] = _ij3[1];
7365vinfos[3].maxsolutions = _nj3;
7366vinfos[4].jointtype = 1;
7367vinfos[4].foffset = j4;
7368vinfos[4].indices[0] = _ij4[0];
7369vinfos[4].indices[1] = _ij4[1];
7370vinfos[4].maxsolutions = _nj4;
7371vinfos[5].jointtype = 1;
7372vinfos[5].foffset = j5;
7373vinfos[5].indices[0] = _ij5[0];
7374vinfos[5].indices[1] = _ij5[1];
7375vinfos[5].maxsolutions = _nj5;
7376std::vector<int> vfree(0);
7377solutions.AddSolution(vinfos,vfree);
7378}
7379}
7380}
7381
7382}
7383
7384}
7385
7386} else
7387{
7388{
7389IkReal j3array[1], cj3array[1], sj3array[1];
7390bool j3valid[1]={false};
7391_nj3 = 1;
7392CheckValue<IkReal> x596=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst4*gconst4)))+(((-1.0)*(gconst5*gconst5))))),-1);
7393if(!x596.valid){
7394continue;
7395}
7396CheckValue<IkReal> x597 = IKatan2WithCheck(IkReal((gconst5*new_r01)),IkReal((gconst4*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7397if(!x597.valid){
7398continue;
7399}
7400j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x596.value)))+(x597.value));
7401sj3array[0]=IKsin(j3array[0]);
7402cj3array[0]=IKcos(j3array[0]);
7403if( j3array[0] > IKPI )
7404{
7405 j3array[0]-=IK2PI;
7406}
7407else if( j3array[0] < -IKPI )
7408{ j3array[0]+=IK2PI;
7409}
7410j3valid[0] = true;
7411for(int ij3 = 0; ij3 < 1; ++ij3)
7412{
7413if( !j3valid[ij3] )
7414{
7415 continue;
7416}
7417_ij3[0] = ij3; _ij3[1] = -1;
7418for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7419{
7420if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7421{
7422 j3valid[iij3]=false; _ij3[1] = iij3; break;
7423}
7424}
7425j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7426{
7427IkReal evalcond[6];
7428IkReal x598=IKsin(j3);
7429IkReal x599=IKcos(j3);
7430IkReal x600=(gconst4*x598);
7431IkReal x601=(gconst4*x599);
7432IkReal x602=((1.0)*x599);
7433IkReal x603=(gconst5*x598);
7434IkReal x604=(gconst5*x602);
7435evalcond[0]=(((new_r11*x598))+gconst4+((new_r01*x599)));
7436evalcond[1]=(x603+x601+new_r01);
7437evalcond[2]=(x600+(((-1.0)*x604)));
7438evalcond[3]=((((-1.0)*new_r11*x602))+gconst5+((new_r01*x598)));
7439evalcond[4]=(x600+(((-1.0)*x604))+new_r11);
7440evalcond[5]=((((-1.0)*x601))+(((-1.0)*x603)));
7441if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7442{
7443continue;
7444}
7445}
7446
7447{
7448std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7449vinfos[0].jointtype = 1;
7450vinfos[0].foffset = j0;
7451vinfos[0].indices[0] = _ij0[0];
7452vinfos[0].indices[1] = _ij0[1];
7453vinfos[0].maxsolutions = _nj0;
7454vinfos[1].jointtype = 1;
7455vinfos[1].foffset = j1;
7456vinfos[1].indices[0] = _ij1[0];
7457vinfos[1].indices[1] = _ij1[1];
7458vinfos[1].maxsolutions = _nj1;
7459vinfos[2].jointtype = 1;
7460vinfos[2].foffset = j2;
7461vinfos[2].indices[0] = _ij2[0];
7462vinfos[2].indices[1] = _ij2[1];
7463vinfos[2].maxsolutions = _nj2;
7464vinfos[3].jointtype = 1;
7465vinfos[3].foffset = j3;
7466vinfos[3].indices[0] = _ij3[0];
7467vinfos[3].indices[1] = _ij3[1];
7468vinfos[3].maxsolutions = _nj3;
7469vinfos[4].jointtype = 1;
7470vinfos[4].foffset = j4;
7471vinfos[4].indices[0] = _ij4[0];
7472vinfos[4].indices[1] = _ij4[1];
7473vinfos[4].maxsolutions = _nj4;
7474vinfos[5].jointtype = 1;
7475vinfos[5].foffset = j5;
7476vinfos[5].indices[0] = _ij5[0];
7477vinfos[5].indices[1] = _ij5[1];
7478vinfos[5].maxsolutions = _nj5;
7479std::vector<int> vfree(0);
7480solutions.AddSolution(vinfos,vfree);
7481}
7482}
7483}
7484
7485}
7486
7487}
7488
7489} else
7490{
7491{
7492IkReal j3array[1], cj3array[1], sj3array[1];
7493bool j3valid[1]={false};
7494_nj3 = 1;
7495CheckValue<IkReal> x605=IKPowWithIntegerCheck(IKsign((((gconst5*new_r01))+(((-1.0)*gconst4*new_r11)))),-1);
7496if(!x605.valid){
7497continue;
7498}
7499CheckValue<IkReal> x606 = IKatan2WithCheck(IkReal(gconst4*gconst4),IkReal(((-1.0)*gconst4*gconst5)),IKFAST_ATAN2_MAGTHRESH);
7500if(!x606.valid){
7501continue;
7502}
7503j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x605.value)))+(x606.value));
7504sj3array[0]=IKsin(j3array[0]);
7505cj3array[0]=IKcos(j3array[0]);
7506if( j3array[0] > IKPI )
7507{
7508 j3array[0]-=IK2PI;
7509}
7510else if( j3array[0] < -IKPI )
7511{ j3array[0]+=IK2PI;
7512}
7513j3valid[0] = true;
7514for(int ij3 = 0; ij3 < 1; ++ij3)
7515{
7516if( !j3valid[ij3] )
7517{
7518 continue;
7519}
7520_ij3[0] = ij3; _ij3[1] = -1;
7521for(int iij3 = ij3+1; iij3 < 1; ++iij3)
7522{
7523if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7524{
7525 j3valid[iij3]=false; _ij3[1] = iij3; break;
7526}
7527}
7528j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7529{
7530IkReal evalcond[6];
7531IkReal x607=IKsin(j3);
7532IkReal x608=IKcos(j3);
7533IkReal x609=(gconst4*x607);
7534IkReal x610=(gconst4*x608);
7535IkReal x611=((1.0)*x608);
7536IkReal x612=(gconst5*x607);
7537IkReal x613=(gconst5*x611);
7538evalcond[0]=(gconst4+((new_r11*x607))+((new_r01*x608)));
7539evalcond[1]=(x610+x612+new_r01);
7540evalcond[2]=((((-1.0)*x613))+x609);
7541evalcond[3]=(gconst5+(((-1.0)*new_r11*x611))+((new_r01*x607)));
7542evalcond[4]=((((-1.0)*x613))+x609+new_r11);
7543evalcond[5]=((((-1.0)*x610))+(((-1.0)*x612)));
7544if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7545{
7546continue;
7547}
7548}
7549
7550{
7551std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7552vinfos[0].jointtype = 1;
7553vinfos[0].foffset = j0;
7554vinfos[0].indices[0] = _ij0[0];
7555vinfos[0].indices[1] = _ij0[1];
7556vinfos[0].maxsolutions = _nj0;
7557vinfos[1].jointtype = 1;
7558vinfos[1].foffset = j1;
7559vinfos[1].indices[0] = _ij1[0];
7560vinfos[1].indices[1] = _ij1[1];
7561vinfos[1].maxsolutions = _nj1;
7562vinfos[2].jointtype = 1;
7563vinfos[2].foffset = j2;
7564vinfos[2].indices[0] = _ij2[0];
7565vinfos[2].indices[1] = _ij2[1];
7566vinfos[2].maxsolutions = _nj2;
7567vinfos[3].jointtype = 1;
7568vinfos[3].foffset = j3;
7569vinfos[3].indices[0] = _ij3[0];
7570vinfos[3].indices[1] = _ij3[1];
7571vinfos[3].maxsolutions = _nj3;
7572vinfos[4].jointtype = 1;
7573vinfos[4].foffset = j4;
7574vinfos[4].indices[0] = _ij4[0];
7575vinfos[4].indices[1] = _ij4[1];
7576vinfos[4].maxsolutions = _nj4;
7577vinfos[5].jointtype = 1;
7578vinfos[5].foffset = j5;
7579vinfos[5].indices[0] = _ij5[0];
7580vinfos[5].indices[1] = _ij5[1];
7581vinfos[5].maxsolutions = _nj5;
7582std::vector<int> vfree(0);
7583solutions.AddSolution(vinfos,vfree);
7584}
7585}
7586}
7587
7588}
7589
7590}
7591
7592}
7593} while(0);
7594if( bgotonextstatement )
7595{
7596bool bgotonextstatement = true;
7597do
7598{
7599evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
7600if( IKabs(evalcond[0]) < 0.0000050000000000 )
7601{
7602bgotonextstatement=false;
7603{
7604IkReal j3eval[1];
7605IkReal x614=((-1.0)*new_r11);
7606CheckValue<IkReal> x616 = IKatan2WithCheck(IkReal(x614),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7607if(!x616.valid){
7608continue;
7609}
7610IkReal x615=((1.0)*(x616.value));
7611sj4=0;
7612cj4=1.0;
7613j4=0;
7614sj5=gconst4;
7615cj5=gconst5;
7616j5=((3.14159265)+(((-1.0)*x615)));
7617new_r01=0;
7618new_r10=0;
7619IkReal gconst3=((3.14159265358979)+(((-1.0)*x615)));
7620IkReal x617 = new_r11*new_r11;
7621if(IKabs(x617)==0){
7622continue;
7623}
7624IkReal gconst4=(x614*(pow(x617,-0.5)));
7625IkReal gconst5=0;
7626j3eval[0]=new_r00;
7627if( IKabs(j3eval[0]) < 0.0000010000000000 )
7628{
7629{
7630IkReal j3eval[1];
7631IkReal x618=((-1.0)*new_r11);
7632CheckValue<IkReal> x620 = IKatan2WithCheck(IkReal(x618),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
7633if(!x620.valid){
7634continue;
7635}
7636IkReal x619=((1.0)*(x620.value));
7637sj4=0;
7638cj4=1.0;
7639j4=0;
7640sj5=gconst4;
7641cj5=gconst5;
7642j5=((3.14159265)+(((-1.0)*x619)));
7643new_r01=0;
7644new_r10=0;
7645IkReal gconst3=((3.14159265358979)+(((-1.0)*x619)));
7646IkReal x621 = new_r11*new_r11;
7647if(IKabs(x621)==0){
7648continue;
7649}
7650IkReal gconst4=(x618*(pow(x621,-0.5)));
7651IkReal gconst5=0;
7652j3eval[0]=new_r11;
7653if( IKabs(j3eval[0]) < 0.0000010000000000 )
7654{
7655{
7656IkReal j3array[2], cj3array[2], sj3array[2];
7657bool j3valid[2]={false};
7658_nj3 = 2;
7659CheckValue<IkReal> x622=IKPowWithIntegerCheck(gconst4,-1);
7660if(!x622.valid){
7661continue;
7662}
7663sj3array[0]=((-1.0)*new_r00*(x622.value));
7664if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7665{
7666 j3valid[0] = j3valid[1] = true;
7667 j3array[0] = IKasin(sj3array[0]);
7668 cj3array[0] = IKcos(j3array[0]);
7669 sj3array[1] = sj3array[0];
7670 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7671 cj3array[1] = -cj3array[0];
7672}
7673else if( isnan(sj3array[0]) )
7674{
7675 // probably any value will work
7676 j3valid[0] = true;
7677 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7678}
7679for(int ij3 = 0; ij3 < 2; ++ij3)
7680{
7681if( !j3valid[ij3] )
7682{
7683 continue;
7684}
7685_ij3[0] = ij3; _ij3[1] = -1;
7686for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7687{
7688if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7689{
7690 j3valid[iij3]=false; _ij3[1] = iij3; break;
7691}
7692}
7693j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7694{
7695IkReal evalcond[6];
7696IkReal x623=IKcos(j3);
7697IkReal x624=IKsin(j3);
7698evalcond[0]=(gconst4*x623);
7699evalcond[1]=(new_r00*x623);
7700evalcond[2]=((-1.0)*new_r11*x623);
7701evalcond[3]=(gconst4+((new_r00*x624)));
7702evalcond[4]=(gconst4+((new_r11*x624)));
7703evalcond[5]=(((gconst4*x624))+new_r11);
7704if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7705{
7706continue;
7707}
7708}
7709
7710{
7711std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7712vinfos[0].jointtype = 1;
7713vinfos[0].foffset = j0;
7714vinfos[0].indices[0] = _ij0[0];
7715vinfos[0].indices[1] = _ij0[1];
7716vinfos[0].maxsolutions = _nj0;
7717vinfos[1].jointtype = 1;
7718vinfos[1].foffset = j1;
7719vinfos[1].indices[0] = _ij1[0];
7720vinfos[1].indices[1] = _ij1[1];
7721vinfos[1].maxsolutions = _nj1;
7722vinfos[2].jointtype = 1;
7723vinfos[2].foffset = j2;
7724vinfos[2].indices[0] = _ij2[0];
7725vinfos[2].indices[1] = _ij2[1];
7726vinfos[2].maxsolutions = _nj2;
7727vinfos[3].jointtype = 1;
7728vinfos[3].foffset = j3;
7729vinfos[3].indices[0] = _ij3[0];
7730vinfos[3].indices[1] = _ij3[1];
7731vinfos[3].maxsolutions = _nj3;
7732vinfos[4].jointtype = 1;
7733vinfos[4].foffset = j4;
7734vinfos[4].indices[0] = _ij4[0];
7735vinfos[4].indices[1] = _ij4[1];
7736vinfos[4].maxsolutions = _nj4;
7737vinfos[5].jointtype = 1;
7738vinfos[5].foffset = j5;
7739vinfos[5].indices[0] = _ij5[0];
7740vinfos[5].indices[1] = _ij5[1];
7741vinfos[5].maxsolutions = _nj5;
7742std::vector<int> vfree(0);
7743solutions.AddSolution(vinfos,vfree);
7744}
7745}
7746}
7747
7748} else
7749{
7750{
7751IkReal j3array[2], cj3array[2], sj3array[2];
7752bool j3valid[2]={false};
7753_nj3 = 2;
7754CheckValue<IkReal> x625=IKPowWithIntegerCheck(new_r11,-1);
7755if(!x625.valid){
7756continue;
7757}
7758sj3array[0]=((-1.0)*gconst4*(x625.value));
7759if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7760{
7761 j3valid[0] = j3valid[1] = true;
7762 j3array[0] = IKasin(sj3array[0]);
7763 cj3array[0] = IKcos(j3array[0]);
7764 sj3array[1] = sj3array[0];
7765 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7766 cj3array[1] = -cj3array[0];
7767}
7768else if( isnan(sj3array[0]) )
7769{
7770 // probably any value will work
7771 j3valid[0] = true;
7772 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7773}
7774for(int ij3 = 0; ij3 < 2; ++ij3)
7775{
7776if( !j3valid[ij3] )
7777{
7778 continue;
7779}
7780_ij3[0] = ij3; _ij3[1] = -1;
7781for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7782{
7783if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7784{
7785 j3valid[iij3]=false; _ij3[1] = iij3; break;
7786}
7787}
7788j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7789{
7790IkReal evalcond[6];
7791IkReal x626=IKcos(j3);
7792IkReal x627=IKsin(j3);
7793IkReal x628=(gconst4*x627);
7794evalcond[0]=(gconst4*x626);
7795evalcond[1]=(new_r00*x626);
7796evalcond[2]=((-1.0)*new_r11*x626);
7797evalcond[3]=(gconst4+((new_r00*x627)));
7798evalcond[4]=(x628+new_r00);
7799evalcond[5]=(x628+new_r11);
7800if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7801{
7802continue;
7803}
7804}
7805
7806{
7807std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7808vinfos[0].jointtype = 1;
7809vinfos[0].foffset = j0;
7810vinfos[0].indices[0] = _ij0[0];
7811vinfos[0].indices[1] = _ij0[1];
7812vinfos[0].maxsolutions = _nj0;
7813vinfos[1].jointtype = 1;
7814vinfos[1].foffset = j1;
7815vinfos[1].indices[0] = _ij1[0];
7816vinfos[1].indices[1] = _ij1[1];
7817vinfos[1].maxsolutions = _nj1;
7818vinfos[2].jointtype = 1;
7819vinfos[2].foffset = j2;
7820vinfos[2].indices[0] = _ij2[0];
7821vinfos[2].indices[1] = _ij2[1];
7822vinfos[2].maxsolutions = _nj2;
7823vinfos[3].jointtype = 1;
7824vinfos[3].foffset = j3;
7825vinfos[3].indices[0] = _ij3[0];
7826vinfos[3].indices[1] = _ij3[1];
7827vinfos[3].maxsolutions = _nj3;
7828vinfos[4].jointtype = 1;
7829vinfos[4].foffset = j4;
7830vinfos[4].indices[0] = _ij4[0];
7831vinfos[4].indices[1] = _ij4[1];
7832vinfos[4].maxsolutions = _nj4;
7833vinfos[5].jointtype = 1;
7834vinfos[5].foffset = j5;
7835vinfos[5].indices[0] = _ij5[0];
7836vinfos[5].indices[1] = _ij5[1];
7837vinfos[5].maxsolutions = _nj5;
7838std::vector<int> vfree(0);
7839solutions.AddSolution(vinfos,vfree);
7840}
7841}
7842}
7843
7844}
7845
7846}
7847
7848} else
7849{
7850{
7851IkReal j3array[2], cj3array[2], sj3array[2];
7852bool j3valid[2]={false};
7853_nj3 = 2;
7854CheckValue<IkReal> x629=IKPowWithIntegerCheck(new_r00,-1);
7855if(!x629.valid){
7856continue;
7857}
7858sj3array[0]=((-1.0)*gconst4*(x629.value));
7859if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
7860{
7861 j3valid[0] = j3valid[1] = true;
7862 j3array[0] = IKasin(sj3array[0]);
7863 cj3array[0] = IKcos(j3array[0]);
7864 sj3array[1] = sj3array[0];
7865 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
7866 cj3array[1] = -cj3array[0];
7867}
7868else if( isnan(sj3array[0]) )
7869{
7870 // probably any value will work
7871 j3valid[0] = true;
7872 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
7873}
7874for(int ij3 = 0; ij3 < 2; ++ij3)
7875{
7876if( !j3valid[ij3] )
7877{
7878 continue;
7879}
7880_ij3[0] = ij3; _ij3[1] = -1;
7881for(int iij3 = ij3+1; iij3 < 2; ++iij3)
7882{
7883if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
7884{
7885 j3valid[iij3]=false; _ij3[1] = iij3; break;
7886}
7887}
7888j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
7889{
7890IkReal evalcond[6];
7891IkReal x630=IKcos(j3);
7892IkReal x631=IKsin(j3);
7893IkReal x632=(gconst4*x631);
7894evalcond[0]=(gconst4*x630);
7895evalcond[1]=(new_r00*x630);
7896evalcond[2]=((-1.0)*new_r11*x630);
7897evalcond[3]=(gconst4+((new_r11*x631)));
7898evalcond[4]=(x632+new_r00);
7899evalcond[5]=(x632+new_r11);
7900if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
7901{
7902continue;
7903}
7904}
7905
7906{
7907std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7908vinfos[0].jointtype = 1;
7909vinfos[0].foffset = j0;
7910vinfos[0].indices[0] = _ij0[0];
7911vinfos[0].indices[1] = _ij0[1];
7912vinfos[0].maxsolutions = _nj0;
7913vinfos[1].jointtype = 1;
7914vinfos[1].foffset = j1;
7915vinfos[1].indices[0] = _ij1[0];
7916vinfos[1].indices[1] = _ij1[1];
7917vinfos[1].maxsolutions = _nj1;
7918vinfos[2].jointtype = 1;
7919vinfos[2].foffset = j2;
7920vinfos[2].indices[0] = _ij2[0];
7921vinfos[2].indices[1] = _ij2[1];
7922vinfos[2].maxsolutions = _nj2;
7923vinfos[3].jointtype = 1;
7924vinfos[3].foffset = j3;
7925vinfos[3].indices[0] = _ij3[0];
7926vinfos[3].indices[1] = _ij3[1];
7927vinfos[3].maxsolutions = _nj3;
7928vinfos[4].jointtype = 1;
7929vinfos[4].foffset = j4;
7930vinfos[4].indices[0] = _ij4[0];
7931vinfos[4].indices[1] = _ij4[1];
7932vinfos[4].maxsolutions = _nj4;
7933vinfos[5].jointtype = 1;
7934vinfos[5].foffset = j5;
7935vinfos[5].indices[0] = _ij5[0];
7936vinfos[5].indices[1] = _ij5[1];
7937vinfos[5].maxsolutions = _nj5;
7938std::vector<int> vfree(0);
7939solutions.AddSolution(vinfos,vfree);
7940}
7941}
7942}
7943
7944}
7945
7946}
7947
7948}
7949} while(0);
7950if( bgotonextstatement )
7951{
7952bool bgotonextstatement = true;
7953do
7954{
7955evalcond[0]=IKabs(new_r11);
7956if( IKabs(evalcond[0]) < 0.0000050000000000 )
7957{
7958bgotonextstatement=false;
7959{
7960IkReal j3eval[1];
7961CheckValue<IkReal> x634 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7962if(!x634.valid){
7963continue;
7964}
7965IkReal x633=((1.0)*(x634.value));
7966sj4=0;
7967cj4=1.0;
7968j4=0;
7969sj5=gconst4;
7970cj5=gconst5;
7971j5=((3.14159265)+(((-1.0)*x633)));
7972new_r11=0;
7973IkReal gconst3=((3.14159265358979)+(((-1.0)*x633)));
7974IkReal gconst4=0;
7975IkReal x635 = new_r01*new_r01;
7976if(IKabs(x635)==0){
7977continue;
7978}
7979IkReal gconst5=((1.0)*new_r01*(pow(x635,-0.5)));
7980j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
7981if( IKabs(j3eval[0]) < 0.0000010000000000 )
7982{
7983{
7984IkReal j3eval[1];
7985CheckValue<IkReal> x637 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
7986if(!x637.valid){
7987continue;
7988}
7989IkReal x636=((1.0)*(x637.value));
7990sj4=0;
7991cj4=1.0;
7992j4=0;
7993sj5=gconst4;
7994cj5=gconst5;
7995j5=((3.14159265)+(((-1.0)*x636)));
7996new_r11=0;
7997IkReal gconst3=((3.14159265358979)+(((-1.0)*x636)));
7998IkReal gconst4=0;
7999IkReal x638 = new_r01*new_r01;
8000if(IKabs(x638)==0){
8001continue;
8002}
8003IkReal gconst5=((1.0)*new_r01*(pow(x638,-0.5)));
8004j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
8005if( IKabs(j3eval[0]) < 0.0000010000000000 )
8006{
8007{
8008IkReal j3eval[1];
8009CheckValue<IkReal> x640 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r01)),IKFAST_ATAN2_MAGTHRESH);
8010if(!x640.valid){
8011continue;
8012}
8013IkReal x639=((1.0)*(x640.value));
8014sj4=0;
8015cj4=1.0;
8016j4=0;
8017sj5=gconst4;
8018cj5=gconst5;
8019j5=((3.14159265)+(((-1.0)*x639)));
8020new_r11=0;
8021IkReal gconst3=((3.14159265358979)+(((-1.0)*x639)));
8022IkReal gconst4=0;
8023IkReal x641 = new_r01*new_r01;
8024if(IKabs(x641)==0){
8025continue;
8026}
8027IkReal gconst5=((1.0)*new_r01*(pow(x641,-0.5)));
8028j3eval[0]=new_r01;
8029if( IKabs(j3eval[0]) < 0.0000010000000000 )
8030{
8031continue; // 3 cases reached
8032
8033} else
8034{
8035{
8036IkReal j3array[1], cj3array[1], sj3array[1];
8037bool j3valid[1]={false};
8038_nj3 = 1;
8039CheckValue<IkReal> x642=IKPowWithIntegerCheck(new_r01,-1);
8040if(!x642.valid){
8041continue;
8042}
8043CheckValue<IkReal> x643=IKPowWithIntegerCheck(gconst5,-1);
8044if(!x643.valid){
8045continue;
8046}
8047if( IKabs(((-1.0)*gconst5*(x642.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r00*(x643.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst5*(x642.value)))+IKsqr((new_r00*(x643.value)))-1) <= IKFAST_SINCOS_THRESH )
8048 continue;
8049j3array[0]=IKatan2(((-1.0)*gconst5*(x642.value)), (new_r00*(x643.value)));
8050sj3array[0]=IKsin(j3array[0]);
8051cj3array[0]=IKcos(j3array[0]);
8052if( j3array[0] > IKPI )
8053{
8054 j3array[0]-=IK2PI;
8055}
8056else if( j3array[0] < -IKPI )
8057{ j3array[0]+=IK2PI;
8058}
8059j3valid[0] = true;
8060for(int ij3 = 0; ij3 < 1; ++ij3)
8061{
8062if( !j3valid[ij3] )
8063{
8064 continue;
8065}
8066_ij3[0] = ij3; _ij3[1] = -1;
8067for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8068{
8069if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8070{
8071 j3valid[iij3]=false; _ij3[1] = iij3; break;
8072}
8073}
8074j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8075{
8076IkReal evalcond[8];
8077IkReal x644=IKcos(j3);
8078IkReal x645=IKsin(j3);
8079IkReal x646=((1.0)*gconst5);
8080evalcond[0]=(new_r01*x644);
8081evalcond[1]=((-1.0)*gconst5*x644);
8082evalcond[2]=(gconst5+((new_r01*x645)));
8083evalcond[3]=(new_r01+((gconst5*x645)));
8084evalcond[4]=((((-1.0)*x644*x646))+new_r00);
8085evalcond[5]=((((-1.0)*x645*x646))+new_r10);
8086evalcond[6]=((((-1.0)*new_r10*x644))+((new_r00*x645)));
8087evalcond[7]=((((-1.0)*x646))+((new_r10*x645))+((new_r00*x644)));
8088if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8089{
8090continue;
8091}
8092}
8093
8094{
8095std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8096vinfos[0].jointtype = 1;
8097vinfos[0].foffset = j0;
8098vinfos[0].indices[0] = _ij0[0];
8099vinfos[0].indices[1] = _ij0[1];
8100vinfos[0].maxsolutions = _nj0;
8101vinfos[1].jointtype = 1;
8102vinfos[1].foffset = j1;
8103vinfos[1].indices[0] = _ij1[0];
8104vinfos[1].indices[1] = _ij1[1];
8105vinfos[1].maxsolutions = _nj1;
8106vinfos[2].jointtype = 1;
8107vinfos[2].foffset = j2;
8108vinfos[2].indices[0] = _ij2[0];
8109vinfos[2].indices[1] = _ij2[1];
8110vinfos[2].maxsolutions = _nj2;
8111vinfos[3].jointtype = 1;
8112vinfos[3].foffset = j3;
8113vinfos[3].indices[0] = _ij3[0];
8114vinfos[3].indices[1] = _ij3[1];
8115vinfos[3].maxsolutions = _nj3;
8116vinfos[4].jointtype = 1;
8117vinfos[4].foffset = j4;
8118vinfos[4].indices[0] = _ij4[0];
8119vinfos[4].indices[1] = _ij4[1];
8120vinfos[4].maxsolutions = _nj4;
8121vinfos[5].jointtype = 1;
8122vinfos[5].foffset = j5;
8123vinfos[5].indices[0] = _ij5[0];
8124vinfos[5].indices[1] = _ij5[1];
8125vinfos[5].maxsolutions = _nj5;
8126std::vector<int> vfree(0);
8127solutions.AddSolution(vinfos,vfree);
8128}
8129}
8130}
8131
8132}
8133
8134}
8135
8136} else
8137{
8138{
8139IkReal j3array[1], cj3array[1], sj3array[1];
8140bool j3valid[1]={false};
8141_nj3 = 1;
8142CheckValue<IkReal> x647 = IKatan2WithCheck(IkReal(((-1.0)*new_r01)),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8143if(!x647.valid){
8144continue;
8145}
8146CheckValue<IkReal> x648=IKPowWithIntegerCheck(IKsign(gconst5),-1);
8147if(!x648.valid){
8148continue;
8149}
8150j3array[0]=((-1.5707963267949)+(x647.value)+(((1.5707963267949)*(x648.value))));
8151sj3array[0]=IKsin(j3array[0]);
8152cj3array[0]=IKcos(j3array[0]);
8153if( j3array[0] > IKPI )
8154{
8155 j3array[0]-=IK2PI;
8156}
8157else if( j3array[0] < -IKPI )
8158{ j3array[0]+=IK2PI;
8159}
8160j3valid[0] = true;
8161for(int ij3 = 0; ij3 < 1; ++ij3)
8162{
8163if( !j3valid[ij3] )
8164{
8165 continue;
8166}
8167_ij3[0] = ij3; _ij3[1] = -1;
8168for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8169{
8170if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8171{
8172 j3valid[iij3]=false; _ij3[1] = iij3; break;
8173}
8174}
8175j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8176{
8177IkReal evalcond[8];
8178IkReal x649=IKcos(j3);
8179IkReal x650=IKsin(j3);
8180IkReal x651=((1.0)*gconst5);
8181evalcond[0]=(new_r01*x649);
8182evalcond[1]=((-1.0)*gconst5*x649);
8183evalcond[2]=(gconst5+((new_r01*x650)));
8184evalcond[3]=(new_r01+((gconst5*x650)));
8185evalcond[4]=((((-1.0)*x649*x651))+new_r00);
8186evalcond[5]=((((-1.0)*x650*x651))+new_r10);
8187evalcond[6]=((((-1.0)*new_r10*x649))+((new_r00*x650)));
8188evalcond[7]=((((-1.0)*x651))+((new_r00*x649))+((new_r10*x650)));
8189if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8190{
8191continue;
8192}
8193}
8194
8195{
8196std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8197vinfos[0].jointtype = 1;
8198vinfos[0].foffset = j0;
8199vinfos[0].indices[0] = _ij0[0];
8200vinfos[0].indices[1] = _ij0[1];
8201vinfos[0].maxsolutions = _nj0;
8202vinfos[1].jointtype = 1;
8203vinfos[1].foffset = j1;
8204vinfos[1].indices[0] = _ij1[0];
8205vinfos[1].indices[1] = _ij1[1];
8206vinfos[1].maxsolutions = _nj1;
8207vinfos[2].jointtype = 1;
8208vinfos[2].foffset = j2;
8209vinfos[2].indices[0] = _ij2[0];
8210vinfos[2].indices[1] = _ij2[1];
8211vinfos[2].maxsolutions = _nj2;
8212vinfos[3].jointtype = 1;
8213vinfos[3].foffset = j3;
8214vinfos[3].indices[0] = _ij3[0];
8215vinfos[3].indices[1] = _ij3[1];
8216vinfos[3].maxsolutions = _nj3;
8217vinfos[4].jointtype = 1;
8218vinfos[4].foffset = j4;
8219vinfos[4].indices[0] = _ij4[0];
8220vinfos[4].indices[1] = _ij4[1];
8221vinfos[4].maxsolutions = _nj4;
8222vinfos[5].jointtype = 1;
8223vinfos[5].foffset = j5;
8224vinfos[5].indices[0] = _ij5[0];
8225vinfos[5].indices[1] = _ij5[1];
8226vinfos[5].maxsolutions = _nj5;
8227std::vector<int> vfree(0);
8228solutions.AddSolution(vinfos,vfree);
8229}
8230}
8231}
8232
8233}
8234
8235}
8236
8237} else
8238{
8239{
8240IkReal j3array[1], cj3array[1], sj3array[1];
8241bool j3valid[1]={false};
8242_nj3 = 1;
8243CheckValue<IkReal> x652 = IKatan2WithCheck(IkReal(new_r10),IkReal(new_r00),IKFAST_ATAN2_MAGTHRESH);
8244if(!x652.valid){
8245continue;
8246}
8247CheckValue<IkReal> x653=IKPowWithIntegerCheck(IKsign(gconst5),-1);
8248if(!x653.valid){
8249continue;
8250}
8251j3array[0]=((-1.5707963267949)+(x652.value)+(((1.5707963267949)*(x653.value))));
8252sj3array[0]=IKsin(j3array[0]);
8253cj3array[0]=IKcos(j3array[0]);
8254if( j3array[0] > IKPI )
8255{
8256 j3array[0]-=IK2PI;
8257}
8258else if( j3array[0] < -IKPI )
8259{ j3array[0]+=IK2PI;
8260}
8261j3valid[0] = true;
8262for(int ij3 = 0; ij3 < 1; ++ij3)
8263{
8264if( !j3valid[ij3] )
8265{
8266 continue;
8267}
8268_ij3[0] = ij3; _ij3[1] = -1;
8269for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8270{
8271if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8272{
8273 j3valid[iij3]=false; _ij3[1] = iij3; break;
8274}
8275}
8276j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8277{
8278IkReal evalcond[8];
8279IkReal x654=IKcos(j3);
8280IkReal x655=IKsin(j3);
8281IkReal x656=((1.0)*gconst5);
8282evalcond[0]=(new_r01*x654);
8283evalcond[1]=((-1.0)*gconst5*x654);
8284evalcond[2]=(gconst5+((new_r01*x655)));
8285evalcond[3]=(new_r01+((gconst5*x655)));
8286evalcond[4]=(new_r00+(((-1.0)*x654*x656)));
8287evalcond[5]=((((-1.0)*x655*x656))+new_r10);
8288evalcond[6]=((((-1.0)*new_r10*x654))+((new_r00*x655)));
8289evalcond[7]=((((-1.0)*x656))+((new_r10*x655))+((new_r00*x654)));
8290if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8291{
8292continue;
8293}
8294}
8295
8296{
8297std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8298vinfos[0].jointtype = 1;
8299vinfos[0].foffset = j0;
8300vinfos[0].indices[0] = _ij0[0];
8301vinfos[0].indices[1] = _ij0[1];
8302vinfos[0].maxsolutions = _nj0;
8303vinfos[1].jointtype = 1;
8304vinfos[1].foffset = j1;
8305vinfos[1].indices[0] = _ij1[0];
8306vinfos[1].indices[1] = _ij1[1];
8307vinfos[1].maxsolutions = _nj1;
8308vinfos[2].jointtype = 1;
8309vinfos[2].foffset = j2;
8310vinfos[2].indices[0] = _ij2[0];
8311vinfos[2].indices[1] = _ij2[1];
8312vinfos[2].maxsolutions = _nj2;
8313vinfos[3].jointtype = 1;
8314vinfos[3].foffset = j3;
8315vinfos[3].indices[0] = _ij3[0];
8316vinfos[3].indices[1] = _ij3[1];
8317vinfos[3].maxsolutions = _nj3;
8318vinfos[4].jointtype = 1;
8319vinfos[4].foffset = j4;
8320vinfos[4].indices[0] = _ij4[0];
8321vinfos[4].indices[1] = _ij4[1];
8322vinfos[4].maxsolutions = _nj4;
8323vinfos[5].jointtype = 1;
8324vinfos[5].foffset = j5;
8325vinfos[5].indices[0] = _ij5[0];
8326vinfos[5].indices[1] = _ij5[1];
8327vinfos[5].maxsolutions = _nj5;
8328std::vector<int> vfree(0);
8329solutions.AddSolution(vinfos,vfree);
8330}
8331}
8332}
8333
8334}
8335
8336}
8337
8338}
8339} while(0);
8340if( bgotonextstatement )
8341{
8342bool bgotonextstatement = true;
8343do
8344{
8345if( 1 )
8346{
8347bgotonextstatement=false;
8348continue; // branch miss [j3]
8349
8350}
8351} while(0);
8352if( bgotonextstatement )
8353{
8354}
8355}
8356}
8357}
8358}
8359}
8360
8361} else
8362{
8363{
8364IkReal j3array[1], cj3array[1], sj3array[1];
8365bool j3valid[1]={false};
8366_nj3 = 1;
8367IkReal x657=((1.0)*new_r01);
8368CheckValue<IkReal> x658 = IKatan2WithCheck(IkReal(((new_r01*new_r01)+(((-1.0)*(gconst4*gconst4))))),IkReal(((((-1.0)*new_r11*x657))+((gconst4*gconst5)))),IKFAST_ATAN2_MAGTHRESH);
8369if(!x658.valid){
8370continue;
8371}
8372CheckValue<IkReal> x659=IKPowWithIntegerCheck(IKsign((((gconst4*new_r11))+(((-1.0)*gconst5*x657)))),-1);
8373if(!x659.valid){
8374continue;
8375}
8376j3array[0]=((-1.5707963267949)+(x658.value)+(((1.5707963267949)*(x659.value))));
8377sj3array[0]=IKsin(j3array[0]);
8378cj3array[0]=IKcos(j3array[0]);
8379if( j3array[0] > IKPI )
8380{
8381 j3array[0]-=IK2PI;
8382}
8383else if( j3array[0] < -IKPI )
8384{ j3array[0]+=IK2PI;
8385}
8386j3valid[0] = true;
8387for(int ij3 = 0; ij3 < 1; ++ij3)
8388{
8389if( !j3valid[ij3] )
8390{
8391 continue;
8392}
8393_ij3[0] = ij3; _ij3[1] = -1;
8394for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8395{
8396if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8397{
8398 j3valid[iij3]=false; _ij3[1] = iij3; break;
8399}
8400}
8401j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8402{
8403IkReal evalcond[8];
8404IkReal x660=IKsin(j3);
8405IkReal x661=IKcos(j3);
8406IkReal x662=((1.0)*gconst5);
8407IkReal x663=(gconst4*x660);
8408IkReal x664=(gconst4*x661);
8409IkReal x665=((1.0)*x661);
8410IkReal x666=(x661*x662);
8411evalcond[0]=(gconst4+((new_r11*x660))+((new_r01*x661)));
8412evalcond[1]=(((gconst5*x660))+x664+new_r01);
8413evalcond[2]=(gconst4+(((-1.0)*new_r10*x665))+((new_r00*x660)));
8414evalcond[3]=(gconst5+(((-1.0)*new_r11*x665))+((new_r01*x660)));
8415evalcond[4]=((((-1.0)*x666))+x663+new_r00);
8416evalcond[5]=((((-1.0)*x666))+x663+new_r11);
8417evalcond[6]=(((new_r10*x660))+(((-1.0)*x662))+((new_r00*x661)));
8418evalcond[7]=((((-1.0)*x660*x662))+(((-1.0)*x664))+new_r10);
8419if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8420{
8421continue;
8422}
8423}
8424
8425{
8426std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8427vinfos[0].jointtype = 1;
8428vinfos[0].foffset = j0;
8429vinfos[0].indices[0] = _ij0[0];
8430vinfos[0].indices[1] = _ij0[1];
8431vinfos[0].maxsolutions = _nj0;
8432vinfos[1].jointtype = 1;
8433vinfos[1].foffset = j1;
8434vinfos[1].indices[0] = _ij1[0];
8435vinfos[1].indices[1] = _ij1[1];
8436vinfos[1].maxsolutions = _nj1;
8437vinfos[2].jointtype = 1;
8438vinfos[2].foffset = j2;
8439vinfos[2].indices[0] = _ij2[0];
8440vinfos[2].indices[1] = _ij2[1];
8441vinfos[2].maxsolutions = _nj2;
8442vinfos[3].jointtype = 1;
8443vinfos[3].foffset = j3;
8444vinfos[3].indices[0] = _ij3[0];
8445vinfos[3].indices[1] = _ij3[1];
8446vinfos[3].maxsolutions = _nj3;
8447vinfos[4].jointtype = 1;
8448vinfos[4].foffset = j4;
8449vinfos[4].indices[0] = _ij4[0];
8450vinfos[4].indices[1] = _ij4[1];
8451vinfos[4].maxsolutions = _nj4;
8452vinfos[5].jointtype = 1;
8453vinfos[5].foffset = j5;
8454vinfos[5].indices[0] = _ij5[0];
8455vinfos[5].indices[1] = _ij5[1];
8456vinfos[5].maxsolutions = _nj5;
8457std::vector<int> vfree(0);
8458solutions.AddSolution(vinfos,vfree);
8459}
8460}
8461}
8462
8463}
8464
8465}
8466
8467} else
8468{
8469{
8470IkReal j3array[1], cj3array[1], sj3array[1];
8471bool j3valid[1]={false};
8472_nj3 = 1;
8473IkReal x667=((1.0)*gconst4);
8474CheckValue<IkReal> x668 = IKatan2WithCheck(IkReal(((gconst4*gconst4)+((new_r01*new_r10)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst5*x667)))),IKFAST_ATAN2_MAGTHRESH);
8475if(!x668.valid){
8476continue;
8477}
8478CheckValue<IkReal> x669=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst5*new_r10))+(((-1.0)*new_r00*x667)))),-1);
8479if(!x669.valid){
8480continue;
8481}
8482j3array[0]=((-1.5707963267949)+(x668.value)+(((1.5707963267949)*(x669.value))));
8483sj3array[0]=IKsin(j3array[0]);
8484cj3array[0]=IKcos(j3array[0]);
8485if( j3array[0] > IKPI )
8486{
8487 j3array[0]-=IK2PI;
8488}
8489else if( j3array[0] < -IKPI )
8490{ j3array[0]+=IK2PI;
8491}
8492j3valid[0] = true;
8493for(int ij3 = 0; ij3 < 1; ++ij3)
8494{
8495if( !j3valid[ij3] )
8496{
8497 continue;
8498}
8499_ij3[0] = ij3; _ij3[1] = -1;
8500for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8501{
8502if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8503{
8504 j3valid[iij3]=false; _ij3[1] = iij3; break;
8505}
8506}
8507j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8508{
8509IkReal evalcond[8];
8510IkReal x670=IKsin(j3);
8511IkReal x671=IKcos(j3);
8512IkReal x672=((1.0)*gconst5);
8513IkReal x673=(gconst4*x670);
8514IkReal x674=(gconst4*x671);
8515IkReal x675=((1.0)*x671);
8516IkReal x676=(x671*x672);
8517evalcond[0]=(((new_r11*x670))+((new_r01*x671))+gconst4);
8518evalcond[1]=(((gconst5*x670))+x674+new_r01);
8519evalcond[2]=(((new_r00*x670))+gconst4+(((-1.0)*new_r10*x675)));
8520evalcond[3]=(((new_r01*x670))+gconst5+(((-1.0)*new_r11*x675)));
8521evalcond[4]=(x673+new_r00+(((-1.0)*x676)));
8522evalcond[5]=(x673+new_r11+(((-1.0)*x676)));
8523evalcond[6]=(((new_r00*x671))+((new_r10*x670))+(((-1.0)*x672)));
8524evalcond[7]=((((-1.0)*x670*x672))+(((-1.0)*x674))+new_r10);
8525if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8526{
8527continue;
8528}
8529}
8530
8531{
8532std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8533vinfos[0].jointtype = 1;
8534vinfos[0].foffset = j0;
8535vinfos[0].indices[0] = _ij0[0];
8536vinfos[0].indices[1] = _ij0[1];
8537vinfos[0].maxsolutions = _nj0;
8538vinfos[1].jointtype = 1;
8539vinfos[1].foffset = j1;
8540vinfos[1].indices[0] = _ij1[0];
8541vinfos[1].indices[1] = _ij1[1];
8542vinfos[1].maxsolutions = _nj1;
8543vinfos[2].jointtype = 1;
8544vinfos[2].foffset = j2;
8545vinfos[2].indices[0] = _ij2[0];
8546vinfos[2].indices[1] = _ij2[1];
8547vinfos[2].maxsolutions = _nj2;
8548vinfos[3].jointtype = 1;
8549vinfos[3].foffset = j3;
8550vinfos[3].indices[0] = _ij3[0];
8551vinfos[3].indices[1] = _ij3[1];
8552vinfos[3].maxsolutions = _nj3;
8553vinfos[4].jointtype = 1;
8554vinfos[4].foffset = j4;
8555vinfos[4].indices[0] = _ij4[0];
8556vinfos[4].indices[1] = _ij4[1];
8557vinfos[4].maxsolutions = _nj4;
8558vinfos[5].jointtype = 1;
8559vinfos[5].foffset = j5;
8560vinfos[5].indices[0] = _ij5[0];
8561vinfos[5].indices[1] = _ij5[1];
8562vinfos[5].maxsolutions = _nj5;
8563std::vector<int> vfree(0);
8564solutions.AddSolution(vinfos,vfree);
8565}
8566}
8567}
8568
8569}
8570
8571}
8572
8573} else
8574{
8575{
8576IkReal j3array[1], cj3array[1], sj3array[1];
8577bool j3valid[1]={false};
8578_nj3 = 1;
8579IkReal x677=((1.0)*new_r11);
8580CheckValue<IkReal> x678=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r10*x677)))),-1);
8581if(!x678.valid){
8582continue;
8583}
8584CheckValue<IkReal> x679 = IKatan2WithCheck(IkReal((((gconst4*new_r10))+((gconst4*new_r01)))),IkReal((((gconst4*new_r00))+(((-1.0)*gconst4*x677)))),IKFAST_ATAN2_MAGTHRESH);
8585if(!x679.valid){
8586continue;
8587}
8588j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x678.value)))+(x679.value));
8589sj3array[0]=IKsin(j3array[0]);
8590cj3array[0]=IKcos(j3array[0]);
8591if( j3array[0] > IKPI )
8592{
8593 j3array[0]-=IK2PI;
8594}
8595else if( j3array[0] < -IKPI )
8596{ j3array[0]+=IK2PI;
8597}
8598j3valid[0] = true;
8599for(int ij3 = 0; ij3 < 1; ++ij3)
8600{
8601if( !j3valid[ij3] )
8602{
8603 continue;
8604}
8605_ij3[0] = ij3; _ij3[1] = -1;
8606for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8607{
8608if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8609{
8610 j3valid[iij3]=false; _ij3[1] = iij3; break;
8611}
8612}
8613j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8614{
8615IkReal evalcond[8];
8616IkReal x680=IKsin(j3);
8617IkReal x681=IKcos(j3);
8618IkReal x682=((1.0)*gconst5);
8619IkReal x683=(gconst4*x680);
8620IkReal x684=(gconst4*x681);
8621IkReal x685=((1.0)*x681);
8622IkReal x686=(x681*x682);
8623evalcond[0]=(gconst4+((new_r01*x681))+((new_r11*x680)));
8624evalcond[1]=(((gconst5*x680))+x684+new_r01);
8625evalcond[2]=((((-1.0)*new_r10*x685))+gconst4+((new_r00*x680)));
8626evalcond[3]=(gconst5+((new_r01*x680))+(((-1.0)*new_r11*x685)));
8627evalcond[4]=((((-1.0)*x686))+x683+new_r00);
8628evalcond[5]=((((-1.0)*x686))+x683+new_r11);
8629evalcond[6]=((((-1.0)*x682))+((new_r00*x681))+((new_r10*x680)));
8630evalcond[7]=((((-1.0)*x680*x682))+new_r10+(((-1.0)*x684)));
8631if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8632{
8633continue;
8634}
8635}
8636
8637{
8638std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8639vinfos[0].jointtype = 1;
8640vinfos[0].foffset = j0;
8641vinfos[0].indices[0] = _ij0[0];
8642vinfos[0].indices[1] = _ij0[1];
8643vinfos[0].maxsolutions = _nj0;
8644vinfos[1].jointtype = 1;
8645vinfos[1].foffset = j1;
8646vinfos[1].indices[0] = _ij1[0];
8647vinfos[1].indices[1] = _ij1[1];
8648vinfos[1].maxsolutions = _nj1;
8649vinfos[2].jointtype = 1;
8650vinfos[2].foffset = j2;
8651vinfos[2].indices[0] = _ij2[0];
8652vinfos[2].indices[1] = _ij2[1];
8653vinfos[2].maxsolutions = _nj2;
8654vinfos[3].jointtype = 1;
8655vinfos[3].foffset = j3;
8656vinfos[3].indices[0] = _ij3[0];
8657vinfos[3].indices[1] = _ij3[1];
8658vinfos[3].maxsolutions = _nj3;
8659vinfos[4].jointtype = 1;
8660vinfos[4].foffset = j4;
8661vinfos[4].indices[0] = _ij4[0];
8662vinfos[4].indices[1] = _ij4[1];
8663vinfos[4].maxsolutions = _nj4;
8664vinfos[5].jointtype = 1;
8665vinfos[5].foffset = j5;
8666vinfos[5].indices[0] = _ij5[0];
8667vinfos[5].indices[1] = _ij5[1];
8668vinfos[5].maxsolutions = _nj5;
8669std::vector<int> vfree(0);
8670solutions.AddSolution(vinfos,vfree);
8671}
8672}
8673}
8674
8675}
8676
8677}
8678
8679}
8680} while(0);
8681if( bgotonextstatement )
8682{
8683bool bgotonextstatement = true;
8684do
8685{
8686evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
8687if( IKabs(evalcond[0]) < 0.0000050000000000 )
8688{
8689bgotonextstatement=false;
8690{
8691IkReal j3array[1], cj3array[1], sj3array[1];
8692bool j3valid[1]={false};
8693_nj3 = 1;
8694if( IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r00) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r01))+IKsqr(new_r00)-1) <= IKFAST_SINCOS_THRESH )
8695 continue;
8696j3array[0]=IKatan2(((-1.0)*new_r01), new_r00);
8697sj3array[0]=IKsin(j3array[0]);
8698cj3array[0]=IKcos(j3array[0]);
8699if( j3array[0] > IKPI )
8700{
8701 j3array[0]-=IK2PI;
8702}
8703else if( j3array[0] < -IKPI )
8704{ j3array[0]+=IK2PI;
8705}
8706j3valid[0] = true;
8707for(int ij3 = 0; ij3 < 1; ++ij3)
8708{
8709if( !j3valid[ij3] )
8710{
8711 continue;
8712}
8713_ij3[0] = ij3; _ij3[1] = -1;
8714for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8715{
8716if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8717{
8718 j3valid[iij3]=false; _ij3[1] = iij3; break;
8719}
8720}
8721j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8722{
8723IkReal evalcond[8];
8724IkReal x687=IKsin(j3);
8725IkReal x688=IKcos(j3);
8726IkReal x689=((1.0)*x688);
8727evalcond[0]=(x687+new_r01);
8728evalcond[1]=((((-1.0)*x689))+new_r00);
8729evalcond[2]=((((-1.0)*x689))+new_r11);
8730evalcond[3]=(new_r10+(((-1.0)*x687)));
8731evalcond[4]=(((new_r01*x688))+((new_r11*x687)));
8732evalcond[5]=((((-1.0)*new_r10*x689))+((new_r00*x687)));
8733evalcond[6]=((-1.0)+((new_r00*x688))+((new_r10*x687)));
8734evalcond[7]=((1.0)+((new_r01*x687))+(((-1.0)*new_r11*x689)));
8735if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8736{
8737continue;
8738}
8739}
8740
8741{
8742std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8743vinfos[0].jointtype = 1;
8744vinfos[0].foffset = j0;
8745vinfos[0].indices[0] = _ij0[0];
8746vinfos[0].indices[1] = _ij0[1];
8747vinfos[0].maxsolutions = _nj0;
8748vinfos[1].jointtype = 1;
8749vinfos[1].foffset = j1;
8750vinfos[1].indices[0] = _ij1[0];
8751vinfos[1].indices[1] = _ij1[1];
8752vinfos[1].maxsolutions = _nj1;
8753vinfos[2].jointtype = 1;
8754vinfos[2].foffset = j2;
8755vinfos[2].indices[0] = _ij2[0];
8756vinfos[2].indices[1] = _ij2[1];
8757vinfos[2].maxsolutions = _nj2;
8758vinfos[3].jointtype = 1;
8759vinfos[3].foffset = j3;
8760vinfos[3].indices[0] = _ij3[0];
8761vinfos[3].indices[1] = _ij3[1];
8762vinfos[3].maxsolutions = _nj3;
8763vinfos[4].jointtype = 1;
8764vinfos[4].foffset = j4;
8765vinfos[4].indices[0] = _ij4[0];
8766vinfos[4].indices[1] = _ij4[1];
8767vinfos[4].maxsolutions = _nj4;
8768vinfos[5].jointtype = 1;
8769vinfos[5].foffset = j5;
8770vinfos[5].indices[0] = _ij5[0];
8771vinfos[5].indices[1] = _ij5[1];
8772vinfos[5].maxsolutions = _nj5;
8773std::vector<int> vfree(0);
8774solutions.AddSolution(vinfos,vfree);
8775}
8776}
8777}
8778
8779}
8780} while(0);
8781if( bgotonextstatement )
8782{
8783bool bgotonextstatement = true;
8784do
8785{
8786evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
8787if( IKabs(evalcond[0]) < 0.0000050000000000 )
8788{
8789bgotonextstatement=false;
8790{
8791IkReal j3array[1], cj3array[1], sj3array[1];
8792bool j3valid[1]={false};
8793_nj3 = 1;
8794if( IKabs(((-1.0)*new_r10)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r10))+IKsqr(((-1.0)*new_r00))-1) <= IKFAST_SINCOS_THRESH )
8795 continue;
8796j3array[0]=IKatan2(((-1.0)*new_r10), ((-1.0)*new_r00));
8797sj3array[0]=IKsin(j3array[0]);
8798cj3array[0]=IKcos(j3array[0]);
8799if( j3array[0] > IKPI )
8800{
8801 j3array[0]-=IK2PI;
8802}
8803else if( j3array[0] < -IKPI )
8804{ j3array[0]+=IK2PI;
8805}
8806j3valid[0] = true;
8807for(int ij3 = 0; ij3 < 1; ++ij3)
8808{
8809if( !j3valid[ij3] )
8810{
8811 continue;
8812}
8813_ij3[0] = ij3; _ij3[1] = -1;
8814for(int iij3 = ij3+1; iij3 < 1; ++iij3)
8815{
8816if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8817{
8818 j3valid[iij3]=false; _ij3[1] = iij3; break;
8819}
8820}
8821j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8822{
8823IkReal evalcond[8];
8824IkReal x690=IKcos(j3);
8825IkReal x691=IKsin(j3);
8826IkReal x692=((1.0)*x690);
8827evalcond[0]=(x690+new_r00);
8828evalcond[1]=(x690+new_r11);
8829evalcond[2]=(x691+new_r10);
8830evalcond[3]=(new_r01+(((-1.0)*x691)));
8831evalcond[4]=(((new_r11*x691))+((new_r01*x690)));
8832evalcond[5]=((((-1.0)*new_r10*x692))+((new_r00*x691)));
8833evalcond[6]=((1.0)+((new_r10*x691))+((new_r00*x690)));
8834evalcond[7]=((-1.0)+(((-1.0)*new_r11*x692))+((new_r01*x691)));
8835if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
8836{
8837continue;
8838}
8839}
8840
8841{
8842std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8843vinfos[0].jointtype = 1;
8844vinfos[0].foffset = j0;
8845vinfos[0].indices[0] = _ij0[0];
8846vinfos[0].indices[1] = _ij0[1];
8847vinfos[0].maxsolutions = _nj0;
8848vinfos[1].jointtype = 1;
8849vinfos[1].foffset = j1;
8850vinfos[1].indices[0] = _ij1[0];
8851vinfos[1].indices[1] = _ij1[1];
8852vinfos[1].maxsolutions = _nj1;
8853vinfos[2].jointtype = 1;
8854vinfos[2].foffset = j2;
8855vinfos[2].indices[0] = _ij2[0];
8856vinfos[2].indices[1] = _ij2[1];
8857vinfos[2].maxsolutions = _nj2;
8858vinfos[3].jointtype = 1;
8859vinfos[3].foffset = j3;
8860vinfos[3].indices[0] = _ij3[0];
8861vinfos[3].indices[1] = _ij3[1];
8862vinfos[3].maxsolutions = _nj3;
8863vinfos[4].jointtype = 1;
8864vinfos[4].foffset = j4;
8865vinfos[4].indices[0] = _ij4[0];
8866vinfos[4].indices[1] = _ij4[1];
8867vinfos[4].maxsolutions = _nj4;
8868vinfos[5].jointtype = 1;
8869vinfos[5].foffset = j5;
8870vinfos[5].indices[0] = _ij5[0];
8871vinfos[5].indices[1] = _ij5[1];
8872vinfos[5].maxsolutions = _nj5;
8873std::vector<int> vfree(0);
8874solutions.AddSolution(vinfos,vfree);
8875}
8876}
8877}
8878
8879}
8880} while(0);
8881if( bgotonextstatement )
8882{
8883bool bgotonextstatement = true;
8884do
8885{
8886evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
8887if( IKabs(evalcond[0]) < 0.0000050000000000 )
8888{
8889bgotonextstatement=false;
8890{
8891IkReal j3eval[3];
8892sj4=0;
8893cj4=1.0;
8894j4=0;
8895new_r11=0;
8896new_r00=0;
8897j3eval[0]=new_r01;
8898j3eval[1]=IKsign(new_r01);
8899j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
8900if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
8901{
8902{
8903IkReal j3eval[2];
8904sj4=0;
8905cj4=1.0;
8906j4=0;
8907new_r11=0;
8908new_r00=0;
8909j3eval[0]=new_r01;
8910j3eval[1]=new_r10;
8911if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
8912{
8913{
8914IkReal j3eval[2];
8915sj4=0;
8916cj4=1.0;
8917j4=0;
8918new_r11=0;
8919new_r00=0;
8920j3eval[0]=new_r01;
8921j3eval[1]=sj5;
8922if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
8923{
8924{
8925IkReal evalcond[1];
8926bool bgotonextstatement = true;
8927do
8928{
8929evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(j5))), 6.28318530717959)));
8930if( IKabs(evalcond[0]) < 0.0000050000000000 )
8931{
8932bgotonextstatement=false;
8933{
8934IkReal j3array[2], cj3array[2], sj3array[2];
8935bool j3valid[2]={false};
8936_nj3 = 2;
8937sj3array[0]=new_r10;
8938if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
8939{
8940 j3valid[0] = j3valid[1] = true;
8941 j3array[0] = IKasin(sj3array[0]);
8942 cj3array[0] = IKcos(j3array[0]);
8943 sj3array[1] = sj3array[0];
8944 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
8945 cj3array[1] = -cj3array[0];
8946}
8947else if( isnan(sj3array[0]) )
8948{
8949 // probably any value will work
8950 j3valid[0] = true;
8951 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
8952}
8953for(int ij3 = 0; ij3 < 2; ++ij3)
8954{
8955if( !j3valid[ij3] )
8956{
8957 continue;
8958}
8959_ij3[0] = ij3; _ij3[1] = -1;
8960for(int iij3 = ij3+1; iij3 < 2; ++iij3)
8961{
8962if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
8963{
8964 j3valid[iij3]=false; _ij3[1] = iij3; break;
8965}
8966}
8967j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
8968{
8969IkReal evalcond[6];
8970IkReal x693=IKcos(j3);
8971IkReal x694=IKsin(j3);
8972IkReal x695=((-1.0)*x693);
8973evalcond[0]=(new_r01*x693);
8974evalcond[1]=(x694+new_r01);
8975evalcond[2]=x695;
8976evalcond[3]=(new_r10*x695);
8977evalcond[4]=((1.0)+((new_r01*x694)));
8978evalcond[5]=((-1.0)+((new_r10*x694)));
8979if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
8980{
8981continue;
8982}
8983}
8984
8985{
8986std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8987vinfos[0].jointtype = 1;
8988vinfos[0].foffset = j0;
8989vinfos[0].indices[0] = _ij0[0];
8990vinfos[0].indices[1] = _ij0[1];
8991vinfos[0].maxsolutions = _nj0;
8992vinfos[1].jointtype = 1;
8993vinfos[1].foffset = j1;
8994vinfos[1].indices[0] = _ij1[0];
8995vinfos[1].indices[1] = _ij1[1];
8996vinfos[1].maxsolutions = _nj1;
8997vinfos[2].jointtype = 1;
8998vinfos[2].foffset = j2;
8999vinfos[2].indices[0] = _ij2[0];
9000vinfos[2].indices[1] = _ij2[1];
9001vinfos[2].maxsolutions = _nj2;
9002vinfos[3].jointtype = 1;
9003vinfos[3].foffset = j3;
9004vinfos[3].indices[0] = _ij3[0];
9005vinfos[3].indices[1] = _ij3[1];
9006vinfos[3].maxsolutions = _nj3;
9007vinfos[4].jointtype = 1;
9008vinfos[4].foffset = j4;
9009vinfos[4].indices[0] = _ij4[0];
9010vinfos[4].indices[1] = _ij4[1];
9011vinfos[4].maxsolutions = _nj4;
9012vinfos[5].jointtype = 1;
9013vinfos[5].foffset = j5;
9014vinfos[5].indices[0] = _ij5[0];
9015vinfos[5].indices[1] = _ij5[1];
9016vinfos[5].maxsolutions = _nj5;
9017std::vector<int> vfree(0);
9018solutions.AddSolution(vinfos,vfree);
9019}
9020}
9021}
9022
9023}
9024} while(0);
9025if( bgotonextstatement )
9026{
9027bool bgotonextstatement = true;
9028do
9029{
9030evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5)))), 6.28318530717959)));
9031if( IKabs(evalcond[0]) < 0.0000050000000000 )
9032{
9033bgotonextstatement=false;
9034{
9035IkReal j3array[2], cj3array[2], sj3array[2];
9036bool j3valid[2]={false};
9037_nj3 = 2;
9038sj3array[0]=new_r01;
9039if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
9040{
9041 j3valid[0] = j3valid[1] = true;
9042 j3array[0] = IKasin(sj3array[0]);
9043 cj3array[0] = IKcos(j3array[0]);
9044 sj3array[1] = sj3array[0];
9045 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
9046 cj3array[1] = -cj3array[0];
9047}
9048else if( isnan(sj3array[0]) )
9049{
9050 // probably any value will work
9051 j3valid[0] = true;
9052 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
9053}
9054for(int ij3 = 0; ij3 < 2; ++ij3)
9055{
9056if( !j3valid[ij3] )
9057{
9058 continue;
9059}
9060_ij3[0] = ij3; _ij3[1] = -1;
9061for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9062{
9063if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9064{
9065 j3valid[iij3]=false; _ij3[1] = iij3; break;
9066}
9067}
9068j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9069{
9070IkReal evalcond[6];
9071IkReal x696=IKcos(j3);
9072IkReal x697=IKsin(j3);
9073evalcond[0]=x696;
9074evalcond[1]=(new_r01*x696);
9075evalcond[2]=(x697+new_r10);
9076evalcond[3]=((-1.0)*new_r10*x696);
9077evalcond[4]=((-1.0)+((new_r01*x697)));
9078evalcond[5]=((1.0)+((new_r10*x697)));
9079if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
9080{
9081continue;
9082}
9083}
9084
9085{
9086std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9087vinfos[0].jointtype = 1;
9088vinfos[0].foffset = j0;
9089vinfos[0].indices[0] = _ij0[0];
9090vinfos[0].indices[1] = _ij0[1];
9091vinfos[0].maxsolutions = _nj0;
9092vinfos[1].jointtype = 1;
9093vinfos[1].foffset = j1;
9094vinfos[1].indices[0] = _ij1[0];
9095vinfos[1].indices[1] = _ij1[1];
9096vinfos[1].maxsolutions = _nj1;
9097vinfos[2].jointtype = 1;
9098vinfos[2].foffset = j2;
9099vinfos[2].indices[0] = _ij2[0];
9100vinfos[2].indices[1] = _ij2[1];
9101vinfos[2].maxsolutions = _nj2;
9102vinfos[3].jointtype = 1;
9103vinfos[3].foffset = j3;
9104vinfos[3].indices[0] = _ij3[0];
9105vinfos[3].indices[1] = _ij3[1];
9106vinfos[3].maxsolutions = _nj3;
9107vinfos[4].jointtype = 1;
9108vinfos[4].foffset = j4;
9109vinfos[4].indices[0] = _ij4[0];
9110vinfos[4].indices[1] = _ij4[1];
9111vinfos[4].maxsolutions = _nj4;
9112vinfos[5].jointtype = 1;
9113vinfos[5].foffset = j5;
9114vinfos[5].indices[0] = _ij5[0];
9115vinfos[5].indices[1] = _ij5[1];
9116vinfos[5].maxsolutions = _nj5;
9117std::vector<int> vfree(0);
9118solutions.AddSolution(vinfos,vfree);
9119}
9120}
9121}
9122
9123}
9124} while(0);
9125if( bgotonextstatement )
9126{
9127bool bgotonextstatement = true;
9128do
9129{
9130if( 1 )
9131{
9132bgotonextstatement=false;
9133continue; // branch miss [j3]
9134
9135}
9136} while(0);
9137if( bgotonextstatement )
9138{
9139}
9140}
9141}
9142}
9143
9144} else
9145{
9146{
9147IkReal j3array[1], cj3array[1], sj3array[1];
9148bool j3valid[1]={false};
9149_nj3 = 1;
9150CheckValue<IkReal> x699=IKPowWithIntegerCheck(new_r01,-1);
9151if(!x699.valid){
9152continue;
9153}
9154IkReal x698=x699.value;
9155CheckValue<IkReal> x700=IKPowWithIntegerCheck(sj5,-1);
9156if(!x700.valid){
9157continue;
9158}
9159CheckValue<IkReal> x701=IKPowWithIntegerCheck(x698,-2);
9160if(!x701.valid){
9161continue;
9162}
9163if( IKabs(((-1.0)*cj5*x698)) < IKFAST_ATAN2_MAGTHRESH && IKabs((x698*(x700.value)*(((((-1.0)*(x701.value)))+(cj5*cj5))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*x698))+IKsqr((x698*(x700.value)*(((((-1.0)*(x701.value)))+(cj5*cj5)))))-1) <= IKFAST_SINCOS_THRESH )
9164 continue;
9165j3array[0]=IKatan2(((-1.0)*cj5*x698), (x698*(x700.value)*(((((-1.0)*(x701.value)))+(cj5*cj5)))));
9166sj3array[0]=IKsin(j3array[0]);
9167cj3array[0]=IKcos(j3array[0]);
9168if( j3array[0] > IKPI )
9169{
9170 j3array[0]-=IK2PI;
9171}
9172else if( j3array[0] < -IKPI )
9173{ j3array[0]+=IK2PI;
9174}
9175j3valid[0] = true;
9176for(int ij3 = 0; ij3 < 1; ++ij3)
9177{
9178if( !j3valid[ij3] )
9179{
9180 continue;
9181}
9182_ij3[0] = ij3; _ij3[1] = -1;
9183for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9184{
9185if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9186{
9187 j3valid[iij3]=false; _ij3[1] = iij3; break;
9188}
9189}
9190j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9191{
9192IkReal evalcond[7];
9193IkReal x702=IKcos(j3);
9194IkReal x703=IKsin(j3);
9195IkReal x704=((1.0)*cj5);
9196IkReal x705=(sj5*x702);
9197evalcond[0]=(cj5+((new_r01*x703)));
9198evalcond[1]=(sj5+((new_r01*x702)));
9199evalcond[2]=(sj5+(((-1.0)*new_r10*x702)));
9200evalcond[3]=((((-1.0)*x704))+((new_r10*x703)));
9201evalcond[4]=(((cj5*x703))+x705+new_r01);
9202evalcond[5]=((((-1.0)*x702*x704))+((sj5*x703)));
9203evalcond[6]=((((-1.0)*x705))+(((-1.0)*x703*x704))+new_r10);
9204if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9205{
9206continue;
9207}
9208}
9209
9210{
9211std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9212vinfos[0].jointtype = 1;
9213vinfos[0].foffset = j0;
9214vinfos[0].indices[0] = _ij0[0];
9215vinfos[0].indices[1] = _ij0[1];
9216vinfos[0].maxsolutions = _nj0;
9217vinfos[1].jointtype = 1;
9218vinfos[1].foffset = j1;
9219vinfos[1].indices[0] = _ij1[0];
9220vinfos[1].indices[1] = _ij1[1];
9221vinfos[1].maxsolutions = _nj1;
9222vinfos[2].jointtype = 1;
9223vinfos[2].foffset = j2;
9224vinfos[2].indices[0] = _ij2[0];
9225vinfos[2].indices[1] = _ij2[1];
9226vinfos[2].maxsolutions = _nj2;
9227vinfos[3].jointtype = 1;
9228vinfos[3].foffset = j3;
9229vinfos[3].indices[0] = _ij3[0];
9230vinfos[3].indices[1] = _ij3[1];
9231vinfos[3].maxsolutions = _nj3;
9232vinfos[4].jointtype = 1;
9233vinfos[4].foffset = j4;
9234vinfos[4].indices[0] = _ij4[0];
9235vinfos[4].indices[1] = _ij4[1];
9236vinfos[4].maxsolutions = _nj4;
9237vinfos[5].jointtype = 1;
9238vinfos[5].foffset = j5;
9239vinfos[5].indices[0] = _ij5[0];
9240vinfos[5].indices[1] = _ij5[1];
9241vinfos[5].maxsolutions = _nj5;
9242std::vector<int> vfree(0);
9243solutions.AddSolution(vinfos,vfree);
9244}
9245}
9246}
9247
9248}
9249
9250}
9251
9252} else
9253{
9254{
9255IkReal j3array[1], cj3array[1], sj3array[1];
9256bool j3valid[1]={false};
9257_nj3 = 1;
9258CheckValue<IkReal> x706=IKPowWithIntegerCheck(new_r01,-1);
9259if(!x706.valid){
9260continue;
9261}
9262CheckValue<IkReal> x707=IKPowWithIntegerCheck(new_r10,-1);
9263if(!x707.valid){
9264continue;
9265}
9266if( IKabs(((-1.0)*cj5*(x706.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((sj5*(x707.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*(x706.value)))+IKsqr((sj5*(x707.value)))-1) <= IKFAST_SINCOS_THRESH )
9267 continue;
9268j3array[0]=IKatan2(((-1.0)*cj5*(x706.value)), (sj5*(x707.value)));
9269sj3array[0]=IKsin(j3array[0]);
9270cj3array[0]=IKcos(j3array[0]);
9271if( j3array[0] > IKPI )
9272{
9273 j3array[0]-=IK2PI;
9274}
9275else if( j3array[0] < -IKPI )
9276{ j3array[0]+=IK2PI;
9277}
9278j3valid[0] = true;
9279for(int ij3 = 0; ij3 < 1; ++ij3)
9280{
9281if( !j3valid[ij3] )
9282{
9283 continue;
9284}
9285_ij3[0] = ij3; _ij3[1] = -1;
9286for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9287{
9288if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9289{
9290 j3valid[iij3]=false; _ij3[1] = iij3; break;
9291}
9292}
9293j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9294{
9295IkReal evalcond[7];
9296IkReal x708=IKcos(j3);
9297IkReal x709=IKsin(j3);
9298IkReal x710=((1.0)*cj5);
9299IkReal x711=(sj5*x708);
9300evalcond[0]=(cj5+((new_r01*x709)));
9301evalcond[1]=(sj5+((new_r01*x708)));
9302evalcond[2]=(sj5+(((-1.0)*new_r10*x708)));
9303evalcond[3]=((((-1.0)*x710))+((new_r10*x709)));
9304evalcond[4]=(((cj5*x709))+x711+new_r01);
9305evalcond[5]=(((sj5*x709))+(((-1.0)*x708*x710)));
9306evalcond[6]=((((-1.0)*x709*x710))+(((-1.0)*x711))+new_r10);
9307if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9308{
9309continue;
9310}
9311}
9312
9313{
9314std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9315vinfos[0].jointtype = 1;
9316vinfos[0].foffset = j0;
9317vinfos[0].indices[0] = _ij0[0];
9318vinfos[0].indices[1] = _ij0[1];
9319vinfos[0].maxsolutions = _nj0;
9320vinfos[1].jointtype = 1;
9321vinfos[1].foffset = j1;
9322vinfos[1].indices[0] = _ij1[0];
9323vinfos[1].indices[1] = _ij1[1];
9324vinfos[1].maxsolutions = _nj1;
9325vinfos[2].jointtype = 1;
9326vinfos[2].foffset = j2;
9327vinfos[2].indices[0] = _ij2[0];
9328vinfos[2].indices[1] = _ij2[1];
9329vinfos[2].maxsolutions = _nj2;
9330vinfos[3].jointtype = 1;
9331vinfos[3].foffset = j3;
9332vinfos[3].indices[0] = _ij3[0];
9333vinfos[3].indices[1] = _ij3[1];
9334vinfos[3].maxsolutions = _nj3;
9335vinfos[4].jointtype = 1;
9336vinfos[4].foffset = j4;
9337vinfos[4].indices[0] = _ij4[0];
9338vinfos[4].indices[1] = _ij4[1];
9339vinfos[4].maxsolutions = _nj4;
9340vinfos[5].jointtype = 1;
9341vinfos[5].foffset = j5;
9342vinfos[5].indices[0] = _ij5[0];
9343vinfos[5].indices[1] = _ij5[1];
9344vinfos[5].maxsolutions = _nj5;
9345std::vector<int> vfree(0);
9346solutions.AddSolution(vinfos,vfree);
9347}
9348}
9349}
9350
9351}
9352
9353}
9354
9355} else
9356{
9357{
9358IkReal j3array[1], cj3array[1], sj3array[1];
9359bool j3valid[1]={false};
9360_nj3 = 1;
9361CheckValue<IkReal> x712=IKPowWithIntegerCheck(IKsign(new_r01),-1);
9362if(!x712.valid){
9363continue;
9364}
9365CheckValue<IkReal> x713 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(((-1.0)*sj5)),IKFAST_ATAN2_MAGTHRESH);
9366if(!x713.valid){
9367continue;
9368}
9369j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x712.value)))+(x713.value));
9370sj3array[0]=IKsin(j3array[0]);
9371cj3array[0]=IKcos(j3array[0]);
9372if( j3array[0] > IKPI )
9373{
9374 j3array[0]-=IK2PI;
9375}
9376else if( j3array[0] < -IKPI )
9377{ j3array[0]+=IK2PI;
9378}
9379j3valid[0] = true;
9380for(int ij3 = 0; ij3 < 1; ++ij3)
9381{
9382if( !j3valid[ij3] )
9383{
9384 continue;
9385}
9386_ij3[0] = ij3; _ij3[1] = -1;
9387for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9388{
9389if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9390{
9391 j3valid[iij3]=false; _ij3[1] = iij3; break;
9392}
9393}
9394j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9395{
9396IkReal evalcond[7];
9397IkReal x714=IKcos(j3);
9398IkReal x715=IKsin(j3);
9399IkReal x716=((1.0)*cj5);
9400IkReal x717=(sj5*x714);
9401evalcond[0]=(cj5+((new_r01*x715)));
9402evalcond[1]=(sj5+((new_r01*x714)));
9403evalcond[2]=(sj5+(((-1.0)*new_r10*x714)));
9404evalcond[3]=((((-1.0)*x716))+((new_r10*x715)));
9405evalcond[4]=(((cj5*x715))+x717+new_r01);
9406evalcond[5]=((((-1.0)*x714*x716))+((sj5*x715)));
9407evalcond[6]=((((-1.0)*x717))+new_r10+(((-1.0)*x715*x716)));
9408if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9409{
9410continue;
9411}
9412}
9413
9414{
9415std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9416vinfos[0].jointtype = 1;
9417vinfos[0].foffset = j0;
9418vinfos[0].indices[0] = _ij0[0];
9419vinfos[0].indices[1] = _ij0[1];
9420vinfos[0].maxsolutions = _nj0;
9421vinfos[1].jointtype = 1;
9422vinfos[1].foffset = j1;
9423vinfos[1].indices[0] = _ij1[0];
9424vinfos[1].indices[1] = _ij1[1];
9425vinfos[1].maxsolutions = _nj1;
9426vinfos[2].jointtype = 1;
9427vinfos[2].foffset = j2;
9428vinfos[2].indices[0] = _ij2[0];
9429vinfos[2].indices[1] = _ij2[1];
9430vinfos[2].maxsolutions = _nj2;
9431vinfos[3].jointtype = 1;
9432vinfos[3].foffset = j3;
9433vinfos[3].indices[0] = _ij3[0];
9434vinfos[3].indices[1] = _ij3[1];
9435vinfos[3].maxsolutions = _nj3;
9436vinfos[4].jointtype = 1;
9437vinfos[4].foffset = j4;
9438vinfos[4].indices[0] = _ij4[0];
9439vinfos[4].indices[1] = _ij4[1];
9440vinfos[4].maxsolutions = _nj4;
9441vinfos[5].jointtype = 1;
9442vinfos[5].foffset = j5;
9443vinfos[5].indices[0] = _ij5[0];
9444vinfos[5].indices[1] = _ij5[1];
9445vinfos[5].maxsolutions = _nj5;
9446std::vector<int> vfree(0);
9447solutions.AddSolution(vinfos,vfree);
9448}
9449}
9450}
9451
9452}
9453
9454}
9455
9456}
9457} while(0);
9458if( bgotonextstatement )
9459{
9460bool bgotonextstatement = true;
9461do
9462{
9463evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
9464if( IKabs(evalcond[0]) < 0.0000050000000000 )
9465{
9466bgotonextstatement=false;
9467{
9468IkReal j3eval[1];
9469sj4=0;
9470cj4=1.0;
9471j4=0;
9472new_r11=0;
9473new_r01=0;
9474new_r22=0;
9475new_r20=0;
9476j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9477if( IKabs(j3eval[0]) < 0.0000010000000000 )
9478{
9479continue; // no branches [j3]
9480
9481} else
9482{
9483{
9484IkReal j3array[2], cj3array[2], sj3array[2];
9485bool j3valid[2]={false};
9486_nj3 = 2;
9487CheckValue<IkReal> x719 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
9488if(!x719.valid){
9489continue;
9490}
9491IkReal x718=x719.value;
9492j3array[0]=((-1.0)*x718);
9493sj3array[0]=IKsin(j3array[0]);
9494cj3array[0]=IKcos(j3array[0]);
9495j3array[1]=((3.14159265358979)+(((-1.0)*x718)));
9496sj3array[1]=IKsin(j3array[1]);
9497cj3array[1]=IKcos(j3array[1]);
9498if( j3array[0] > IKPI )
9499{
9500 j3array[0]-=IK2PI;
9501}
9502else if( j3array[0] < -IKPI )
9503{ j3array[0]+=IK2PI;
9504}
9505j3valid[0] = true;
9506if( j3array[1] > IKPI )
9507{
9508 j3array[1]-=IK2PI;
9509}
9510else if( j3array[1] < -IKPI )
9511{ j3array[1]+=IK2PI;
9512}
9513j3valid[1] = true;
9514for(int ij3 = 0; ij3 < 2; ++ij3)
9515{
9516if( !j3valid[ij3] )
9517{
9518 continue;
9519}
9520_ij3[0] = ij3; _ij3[1] = -1;
9521for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9522{
9523if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9524{
9525 j3valid[iij3]=false; _ij3[1] = iij3; break;
9526}
9527}
9528j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9529{
9530IkReal evalcond[1];
9531evalcond[0]=(((new_r00*(IKsin(j3))))+(((-1.0)*new_r10*(IKcos(j3)))));
9532if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9533{
9534continue;
9535}
9536}
9537
9538{
9539std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9540vinfos[0].jointtype = 1;
9541vinfos[0].foffset = j0;
9542vinfos[0].indices[0] = _ij0[0];
9543vinfos[0].indices[1] = _ij0[1];
9544vinfos[0].maxsolutions = _nj0;
9545vinfos[1].jointtype = 1;
9546vinfos[1].foffset = j1;
9547vinfos[1].indices[0] = _ij1[0];
9548vinfos[1].indices[1] = _ij1[1];
9549vinfos[1].maxsolutions = _nj1;
9550vinfos[2].jointtype = 1;
9551vinfos[2].foffset = j2;
9552vinfos[2].indices[0] = _ij2[0];
9553vinfos[2].indices[1] = _ij2[1];
9554vinfos[2].maxsolutions = _nj2;
9555vinfos[3].jointtype = 1;
9556vinfos[3].foffset = j3;
9557vinfos[3].indices[0] = _ij3[0];
9558vinfos[3].indices[1] = _ij3[1];
9559vinfos[3].maxsolutions = _nj3;
9560vinfos[4].jointtype = 1;
9561vinfos[4].foffset = j4;
9562vinfos[4].indices[0] = _ij4[0];
9563vinfos[4].indices[1] = _ij4[1];
9564vinfos[4].maxsolutions = _nj4;
9565vinfos[5].jointtype = 1;
9566vinfos[5].foffset = j5;
9567vinfos[5].indices[0] = _ij5[0];
9568vinfos[5].indices[1] = _ij5[1];
9569vinfos[5].maxsolutions = _nj5;
9570std::vector<int> vfree(0);
9571solutions.AddSolution(vinfos,vfree);
9572}
9573}
9574}
9575
9576}
9577
9578}
9579
9580}
9581} while(0);
9582if( bgotonextstatement )
9583{
9584bool bgotonextstatement = true;
9585do
9586{
9587evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
9588if( IKabs(evalcond[0]) < 0.0000050000000000 )
9589{
9590bgotonextstatement=false;
9591{
9592IkReal j3eval[1];
9593sj4=0;
9594cj4=1.0;
9595j4=0;
9596new_r00=0;
9597new_r10=0;
9598new_r21=0;
9599new_r22=0;
9600j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
9601if( IKabs(j3eval[0]) < 0.0000010000000000 )
9602{
9603continue; // no branches [j3]
9604
9605} else
9606{
9607{
9608IkReal j3array[2], cj3array[2], sj3array[2];
9609bool j3valid[2]={false};
9610_nj3 = 2;
9611CheckValue<IkReal> x721 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
9612if(!x721.valid){
9613continue;
9614}
9615IkReal x720=x721.value;
9616j3array[0]=((-1.0)*x720);
9617sj3array[0]=IKsin(j3array[0]);
9618cj3array[0]=IKcos(j3array[0]);
9619j3array[1]=((3.14159265358979)+(((-1.0)*x720)));
9620sj3array[1]=IKsin(j3array[1]);
9621cj3array[1]=IKcos(j3array[1]);
9622if( j3array[0] > IKPI )
9623{
9624 j3array[0]-=IK2PI;
9625}
9626else if( j3array[0] < -IKPI )
9627{ j3array[0]+=IK2PI;
9628}
9629j3valid[0] = true;
9630if( j3array[1] > IKPI )
9631{
9632 j3array[1]-=IK2PI;
9633}
9634else if( j3array[1] < -IKPI )
9635{ j3array[1]+=IK2PI;
9636}
9637j3valid[1] = true;
9638for(int ij3 = 0; ij3 < 2; ++ij3)
9639{
9640if( !j3valid[ij3] )
9641{
9642 continue;
9643}
9644_ij3[0] = ij3; _ij3[1] = -1;
9645for(int iij3 = ij3+1; iij3 < 2; ++iij3)
9646{
9647if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9648{
9649 j3valid[iij3]=false; _ij3[1] = iij3; break;
9650}
9651}
9652j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9653{
9654IkReal evalcond[1];
9655evalcond[0]=(((new_r01*(IKsin(j3))))+(((-1.0)*new_r11*(IKcos(j3)))));
9656if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
9657{
9658continue;
9659}
9660}
9661
9662{
9663std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9664vinfos[0].jointtype = 1;
9665vinfos[0].foffset = j0;
9666vinfos[0].indices[0] = _ij0[0];
9667vinfos[0].indices[1] = _ij0[1];
9668vinfos[0].maxsolutions = _nj0;
9669vinfos[1].jointtype = 1;
9670vinfos[1].foffset = j1;
9671vinfos[1].indices[0] = _ij1[0];
9672vinfos[1].indices[1] = _ij1[1];
9673vinfos[1].maxsolutions = _nj1;
9674vinfos[2].jointtype = 1;
9675vinfos[2].foffset = j2;
9676vinfos[2].indices[0] = _ij2[0];
9677vinfos[2].indices[1] = _ij2[1];
9678vinfos[2].maxsolutions = _nj2;
9679vinfos[3].jointtype = 1;
9680vinfos[3].foffset = j3;
9681vinfos[3].indices[0] = _ij3[0];
9682vinfos[3].indices[1] = _ij3[1];
9683vinfos[3].maxsolutions = _nj3;
9684vinfos[4].jointtype = 1;
9685vinfos[4].foffset = j4;
9686vinfos[4].indices[0] = _ij4[0];
9687vinfos[4].indices[1] = _ij4[1];
9688vinfos[4].maxsolutions = _nj4;
9689vinfos[5].jointtype = 1;
9690vinfos[5].foffset = j5;
9691vinfos[5].indices[0] = _ij5[0];
9692vinfos[5].indices[1] = _ij5[1];
9693vinfos[5].maxsolutions = _nj5;
9694std::vector<int> vfree(0);
9695solutions.AddSolution(vinfos,vfree);
9696}
9697}
9698}
9699
9700}
9701
9702}
9703
9704}
9705} while(0);
9706if( bgotonextstatement )
9707{
9708bool bgotonextstatement = true;
9709do
9710{
9711evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
9712if( IKabs(evalcond[0]) < 0.0000050000000000 )
9713{
9714bgotonextstatement=false;
9715{
9716IkReal j3eval[3];
9717sj4=0;
9718cj4=1.0;
9719j4=0;
9720new_r01=0;
9721new_r10=0;
9722j3eval[0]=new_r11;
9723j3eval[1]=IKsign(new_r11);
9724j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
9725if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9726{
9727{
9728IkReal j3eval[3];
9729sj4=0;
9730cj4=1.0;
9731j4=0;
9732new_r01=0;
9733new_r10=0;
9734j3eval[0]=new_r00;
9735j3eval[1]=((IKabs(cj5))+(IKabs(sj5)));
9736j3eval[2]=IKsign(new_r00);
9737if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
9738{
9739{
9740IkReal j3eval[2];
9741sj4=0;
9742cj4=1.0;
9743j4=0;
9744new_r01=0;
9745new_r10=0;
9746j3eval[0]=new_r00;
9747j3eval[1]=new_r11;
9748if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
9749{
9750continue; // no branches [j3]
9751
9752} else
9753{
9754{
9755IkReal j3array[1], cj3array[1], sj3array[1];
9756bool j3valid[1]={false};
9757_nj3 = 1;
9758CheckValue<IkReal> x722=IKPowWithIntegerCheck(new_r00,-1);
9759if(!x722.valid){
9760continue;
9761}
9762CheckValue<IkReal> x723=IKPowWithIntegerCheck(new_r11,-1);
9763if(!x723.valid){
9764continue;
9765}
9766if( IKabs(((-1.0)*sj5*(x722.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((cj5*(x723.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*sj5*(x722.value)))+IKsqr((cj5*(x723.value)))-1) <= IKFAST_SINCOS_THRESH )
9767 continue;
9768j3array[0]=IKatan2(((-1.0)*sj5*(x722.value)), (cj5*(x723.value)));
9769sj3array[0]=IKsin(j3array[0]);
9770cj3array[0]=IKcos(j3array[0]);
9771if( j3array[0] > IKPI )
9772{
9773 j3array[0]-=IK2PI;
9774}
9775else if( j3array[0] < -IKPI )
9776{ j3array[0]+=IK2PI;
9777}
9778j3valid[0] = true;
9779for(int ij3 = 0; ij3 < 1; ++ij3)
9780{
9781if( !j3valid[ij3] )
9782{
9783 continue;
9784}
9785_ij3[0] = ij3; _ij3[1] = -1;
9786for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9787{
9788if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9789{
9790 j3valid[iij3]=false; _ij3[1] = iij3; break;
9791}
9792}
9793j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9794{
9795IkReal evalcond[7];
9796IkReal x724=IKsin(j3);
9797IkReal x725=IKcos(j3);
9798IkReal x726=(sj5*x724);
9799IkReal x727=((1.0)*x725);
9800IkReal x728=(cj5*x727);
9801evalcond[0]=(sj5+((new_r00*x724)));
9802evalcond[1]=(sj5+((new_r11*x724)));
9803evalcond[2]=(cj5+(((-1.0)*new_r11*x727)));
9804evalcond[3]=(((new_r00*x725))+(((-1.0)*cj5)));
9805evalcond[4]=(((cj5*x724))+((sj5*x725)));
9806evalcond[5]=((((-1.0)*x728))+x726+new_r00);
9807evalcond[6]=((((-1.0)*x728))+x726+new_r11);
9808if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9809{
9810continue;
9811}
9812}
9813
9814{
9815std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9816vinfos[0].jointtype = 1;
9817vinfos[0].foffset = j0;
9818vinfos[0].indices[0] = _ij0[0];
9819vinfos[0].indices[1] = _ij0[1];
9820vinfos[0].maxsolutions = _nj0;
9821vinfos[1].jointtype = 1;
9822vinfos[1].foffset = j1;
9823vinfos[1].indices[0] = _ij1[0];
9824vinfos[1].indices[1] = _ij1[1];
9825vinfos[1].maxsolutions = _nj1;
9826vinfos[2].jointtype = 1;
9827vinfos[2].foffset = j2;
9828vinfos[2].indices[0] = _ij2[0];
9829vinfos[2].indices[1] = _ij2[1];
9830vinfos[2].maxsolutions = _nj2;
9831vinfos[3].jointtype = 1;
9832vinfos[3].foffset = j3;
9833vinfos[3].indices[0] = _ij3[0];
9834vinfos[3].indices[1] = _ij3[1];
9835vinfos[3].maxsolutions = _nj3;
9836vinfos[4].jointtype = 1;
9837vinfos[4].foffset = j4;
9838vinfos[4].indices[0] = _ij4[0];
9839vinfos[4].indices[1] = _ij4[1];
9840vinfos[4].maxsolutions = _nj4;
9841vinfos[5].jointtype = 1;
9842vinfos[5].foffset = j5;
9843vinfos[5].indices[0] = _ij5[0];
9844vinfos[5].indices[1] = _ij5[1];
9845vinfos[5].maxsolutions = _nj5;
9846std::vector<int> vfree(0);
9847solutions.AddSolution(vinfos,vfree);
9848}
9849}
9850}
9851
9852}
9853
9854}
9855
9856} else
9857{
9858{
9859IkReal j3array[1], cj3array[1], sj3array[1];
9860bool j3valid[1]={false};
9861_nj3 = 1;
9862CheckValue<IkReal> x729=IKPowWithIntegerCheck(IKsign(new_r00),-1);
9863if(!x729.valid){
9864continue;
9865}
9866CheckValue<IkReal> x730 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(cj5),IKFAST_ATAN2_MAGTHRESH);
9867if(!x730.valid){
9868continue;
9869}
9870j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x729.value)))+(x730.value));
9871sj3array[0]=IKsin(j3array[0]);
9872cj3array[0]=IKcos(j3array[0]);
9873if( j3array[0] > IKPI )
9874{
9875 j3array[0]-=IK2PI;
9876}
9877else if( j3array[0] < -IKPI )
9878{ j3array[0]+=IK2PI;
9879}
9880j3valid[0] = true;
9881for(int ij3 = 0; ij3 < 1; ++ij3)
9882{
9883if( !j3valid[ij3] )
9884{
9885 continue;
9886}
9887_ij3[0] = ij3; _ij3[1] = -1;
9888for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9889{
9890if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9891{
9892 j3valid[iij3]=false; _ij3[1] = iij3; break;
9893}
9894}
9895j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9896{
9897IkReal evalcond[7];
9898IkReal x731=IKsin(j3);
9899IkReal x732=IKcos(j3);
9900IkReal x733=(sj5*x731);
9901IkReal x734=((1.0)*x732);
9902IkReal x735=(cj5*x734);
9903evalcond[0]=(sj5+((new_r00*x731)));
9904evalcond[1]=(sj5+((new_r11*x731)));
9905evalcond[2]=(cj5+(((-1.0)*new_r11*x734)));
9906evalcond[3]=(((new_r00*x732))+(((-1.0)*cj5)));
9907evalcond[4]=(((cj5*x731))+((sj5*x732)));
9908evalcond[5]=((((-1.0)*x735))+x733+new_r00);
9909evalcond[6]=((((-1.0)*x735))+x733+new_r11);
9910if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
9911{
9912continue;
9913}
9914}
9915
9916{
9917std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9918vinfos[0].jointtype = 1;
9919vinfos[0].foffset = j0;
9920vinfos[0].indices[0] = _ij0[0];
9921vinfos[0].indices[1] = _ij0[1];
9922vinfos[0].maxsolutions = _nj0;
9923vinfos[1].jointtype = 1;
9924vinfos[1].foffset = j1;
9925vinfos[1].indices[0] = _ij1[0];
9926vinfos[1].indices[1] = _ij1[1];
9927vinfos[1].maxsolutions = _nj1;
9928vinfos[2].jointtype = 1;
9929vinfos[2].foffset = j2;
9930vinfos[2].indices[0] = _ij2[0];
9931vinfos[2].indices[1] = _ij2[1];
9932vinfos[2].maxsolutions = _nj2;
9933vinfos[3].jointtype = 1;
9934vinfos[3].foffset = j3;
9935vinfos[3].indices[0] = _ij3[0];
9936vinfos[3].indices[1] = _ij3[1];
9937vinfos[3].maxsolutions = _nj3;
9938vinfos[4].jointtype = 1;
9939vinfos[4].foffset = j4;
9940vinfos[4].indices[0] = _ij4[0];
9941vinfos[4].indices[1] = _ij4[1];
9942vinfos[4].maxsolutions = _nj4;
9943vinfos[5].jointtype = 1;
9944vinfos[5].foffset = j5;
9945vinfos[5].indices[0] = _ij5[0];
9946vinfos[5].indices[1] = _ij5[1];
9947vinfos[5].maxsolutions = _nj5;
9948std::vector<int> vfree(0);
9949solutions.AddSolution(vinfos,vfree);
9950}
9951}
9952}
9953
9954}
9955
9956}
9957
9958} else
9959{
9960{
9961IkReal j3array[1], cj3array[1], sj3array[1];
9962bool j3valid[1]={false};
9963_nj3 = 1;
9964CheckValue<IkReal> x736=IKPowWithIntegerCheck(IKsign(new_r11),-1);
9965if(!x736.valid){
9966continue;
9967}
9968CheckValue<IkReal> x737 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(cj5),IKFAST_ATAN2_MAGTHRESH);
9969if(!x737.valid){
9970continue;
9971}
9972j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x736.value)))+(x737.value));
9973sj3array[0]=IKsin(j3array[0]);
9974cj3array[0]=IKcos(j3array[0]);
9975if( j3array[0] > IKPI )
9976{
9977 j3array[0]-=IK2PI;
9978}
9979else if( j3array[0] < -IKPI )
9980{ j3array[0]+=IK2PI;
9981}
9982j3valid[0] = true;
9983for(int ij3 = 0; ij3 < 1; ++ij3)
9984{
9985if( !j3valid[ij3] )
9986{
9987 continue;
9988}
9989_ij3[0] = ij3; _ij3[1] = -1;
9990for(int iij3 = ij3+1; iij3 < 1; ++iij3)
9991{
9992if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
9993{
9994 j3valid[iij3]=false; _ij3[1] = iij3; break;
9995}
9996}
9997j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
9998{
9999IkReal evalcond[7];
10000IkReal x738=IKsin(j3);
10001IkReal x739=IKcos(j3);
10002IkReal x740=(sj5*x738);
10003IkReal x741=((1.0)*x739);
10004IkReal x742=(cj5*x741);
10005evalcond[0]=(sj5+((new_r00*x738)));
10006evalcond[1]=(sj5+((new_r11*x738)));
10007evalcond[2]=(cj5+(((-1.0)*new_r11*x741)));
10008evalcond[3]=(((new_r00*x739))+(((-1.0)*cj5)));
10009evalcond[4]=(((cj5*x738))+((sj5*x739)));
10010evalcond[5]=(x740+new_r00+(((-1.0)*x742)));
10011evalcond[6]=(x740+new_r11+(((-1.0)*x742)));
10012if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
10013{
10014continue;
10015}
10016}
10017
10018{
10019std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10020vinfos[0].jointtype = 1;
10021vinfos[0].foffset = j0;
10022vinfos[0].indices[0] = _ij0[0];
10023vinfos[0].indices[1] = _ij0[1];
10024vinfos[0].maxsolutions = _nj0;
10025vinfos[1].jointtype = 1;
10026vinfos[1].foffset = j1;
10027vinfos[1].indices[0] = _ij1[0];
10028vinfos[1].indices[1] = _ij1[1];
10029vinfos[1].maxsolutions = _nj1;
10030vinfos[2].jointtype = 1;
10031vinfos[2].foffset = j2;
10032vinfos[2].indices[0] = _ij2[0];
10033vinfos[2].indices[1] = _ij2[1];
10034vinfos[2].maxsolutions = _nj2;
10035vinfos[3].jointtype = 1;
10036vinfos[3].foffset = j3;
10037vinfos[3].indices[0] = _ij3[0];
10038vinfos[3].indices[1] = _ij3[1];
10039vinfos[3].maxsolutions = _nj3;
10040vinfos[4].jointtype = 1;
10041vinfos[4].foffset = j4;
10042vinfos[4].indices[0] = _ij4[0];
10043vinfos[4].indices[1] = _ij4[1];
10044vinfos[4].maxsolutions = _nj4;
10045vinfos[5].jointtype = 1;
10046vinfos[5].foffset = j5;
10047vinfos[5].indices[0] = _ij5[0];
10048vinfos[5].indices[1] = _ij5[1];
10049vinfos[5].maxsolutions = _nj5;
10050std::vector<int> vfree(0);
10051solutions.AddSolution(vinfos,vfree);
10052}
10053}
10054}
10055
10056}
10057
10058}
10059
10060}
10061} while(0);
10062if( bgotonextstatement )
10063{
10064bool bgotonextstatement = true;
10065do
10066{
10067if( 1 )
10068{
10069bgotonextstatement=false;
10070continue; // branch miss [j3]
10071
10072}
10073} while(0);
10074if( bgotonextstatement )
10075{
10076}
10077}
10078}
10079}
10080}
10081}
10082}
10083}
10084}
10085}
10086
10087} else
10088{
10089{
10090IkReal j3array[1], cj3array[1], sj3array[1];
10091bool j3valid[1]={false};
10092_nj3 = 1;
10093IkReal x743=((1.0)*new_r11);
10094CheckValue<IkReal> x744 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((cj5*sj5)))),IkReal(((1.0)+(((-1.0)*new_r00*x743))+(((-1.0)*(cj5*cj5))))),IKFAST_ATAN2_MAGTHRESH);
10095if(!x744.valid){
10096continue;
10097}
10098CheckValue<IkReal> x745=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r01*sj5))+(((-1.0)*cj5*x743)))),-1);
10099if(!x745.valid){
10100continue;
10101}
10102j3array[0]=((-1.5707963267949)+(x744.value)+(((1.5707963267949)*(x745.value))));
10103sj3array[0]=IKsin(j3array[0]);
10104cj3array[0]=IKcos(j3array[0]);
10105if( j3array[0] > IKPI )
10106{
10107 j3array[0]-=IK2PI;
10108}
10109else if( j3array[0] < -IKPI )
10110{ j3array[0]+=IK2PI;
10111}
10112j3valid[0] = true;
10113for(int ij3 = 0; ij3 < 1; ++ij3)
10114{
10115if( !j3valid[ij3] )
10116{
10117 continue;
10118}
10119_ij3[0] = ij3; _ij3[1] = -1;
10120for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10121{
10122if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10123{
10124 j3valid[iij3]=false; _ij3[1] = iij3; break;
10125}
10126}
10127j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10128{
10129IkReal evalcond[8];
10130IkReal x746=IKcos(j3);
10131IkReal x747=IKsin(j3);
10132IkReal x748=(sj5*x747);
10133IkReal x749=(cj5*x747);
10134IkReal x750=(sj5*x746);
10135IkReal x751=((1.0)*x746);
10136IkReal x752=(cj5*x751);
10137evalcond[0]=(sj5+((new_r01*x746))+((new_r11*x747)));
10138evalcond[1]=(x750+x749+new_r01);
10139evalcond[2]=(sj5+(((-1.0)*new_r10*x751))+((new_r00*x747)));
10140evalcond[3]=(cj5+(((-1.0)*new_r11*x751))+((new_r01*x747)));
10141evalcond[4]=(x748+new_r00+(((-1.0)*x752)));
10142evalcond[5]=(x748+new_r11+(((-1.0)*x752)));
10143evalcond[6]=(((new_r10*x747))+((new_r00*x746))+(((-1.0)*cj5)));
10144evalcond[7]=((((-1.0)*x750))+(((-1.0)*x749))+new_r10);
10145if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10146{
10147continue;
10148}
10149}
10150
10151{
10152std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10153vinfos[0].jointtype = 1;
10154vinfos[0].foffset = j0;
10155vinfos[0].indices[0] = _ij0[0];
10156vinfos[0].indices[1] = _ij0[1];
10157vinfos[0].maxsolutions = _nj0;
10158vinfos[1].jointtype = 1;
10159vinfos[1].foffset = j1;
10160vinfos[1].indices[0] = _ij1[0];
10161vinfos[1].indices[1] = _ij1[1];
10162vinfos[1].maxsolutions = _nj1;
10163vinfos[2].jointtype = 1;
10164vinfos[2].foffset = j2;
10165vinfos[2].indices[0] = _ij2[0];
10166vinfos[2].indices[1] = _ij2[1];
10167vinfos[2].maxsolutions = _nj2;
10168vinfos[3].jointtype = 1;
10169vinfos[3].foffset = j3;
10170vinfos[3].indices[0] = _ij3[0];
10171vinfos[3].indices[1] = _ij3[1];
10172vinfos[3].maxsolutions = _nj3;
10173vinfos[4].jointtype = 1;
10174vinfos[4].foffset = j4;
10175vinfos[4].indices[0] = _ij4[0];
10176vinfos[4].indices[1] = _ij4[1];
10177vinfos[4].maxsolutions = _nj4;
10178vinfos[5].jointtype = 1;
10179vinfos[5].foffset = j5;
10180vinfos[5].indices[0] = _ij5[0];
10181vinfos[5].indices[1] = _ij5[1];
10182vinfos[5].maxsolutions = _nj5;
10183std::vector<int> vfree(0);
10184solutions.AddSolution(vinfos,vfree);
10185}
10186}
10187}
10188
10189}
10190
10191}
10192
10193} else
10194{
10195{
10196IkReal j3array[1], cj3array[1], sj3array[1];
10197bool j3valid[1]={false};
10198_nj3 = 1;
10199CheckValue<IkReal> x753 = IKatan2WithCheck(IkReal((((new_r11*sj5))+((cj5*new_r01)))),IkReal((((new_r01*sj5))+(((-1.0)*cj5*new_r11)))),IKFAST_ATAN2_MAGTHRESH);
10200if(!x753.valid){
10201continue;
10202}
10203CheckValue<IkReal> x754=IKPowWithIntegerCheck(IKsign(((((-1.0)*(new_r01*new_r01)))+(((-1.0)*(new_r11*new_r11))))),-1);
10204if(!x754.valid){
10205continue;
10206}
10207j3array[0]=((-1.5707963267949)+(x753.value)+(((1.5707963267949)*(x754.value))));
10208sj3array[0]=IKsin(j3array[0]);
10209cj3array[0]=IKcos(j3array[0]);
10210if( j3array[0] > IKPI )
10211{
10212 j3array[0]-=IK2PI;
10213}
10214else if( j3array[0] < -IKPI )
10215{ j3array[0]+=IK2PI;
10216}
10217j3valid[0] = true;
10218for(int ij3 = 0; ij3 < 1; ++ij3)
10219{
10220if( !j3valid[ij3] )
10221{
10222 continue;
10223}
10224_ij3[0] = ij3; _ij3[1] = -1;
10225for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10226{
10227if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10228{
10229 j3valid[iij3]=false; _ij3[1] = iij3; break;
10230}
10231}
10232j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10233{
10234IkReal evalcond[8];
10235IkReal x755=IKcos(j3);
10236IkReal x756=IKsin(j3);
10237IkReal x757=(sj5*x756);
10238IkReal x758=(cj5*x756);
10239IkReal x759=(sj5*x755);
10240IkReal x760=((1.0)*x755);
10241IkReal x761=(cj5*x760);
10242evalcond[0]=(sj5+((new_r11*x756))+((new_r01*x755)));
10243evalcond[1]=(x759+x758+new_r01);
10244evalcond[2]=(sj5+(((-1.0)*new_r10*x760))+((new_r00*x756)));
10245evalcond[3]=(cj5+(((-1.0)*new_r11*x760))+((new_r01*x756)));
10246evalcond[4]=((((-1.0)*x761))+x757+new_r00);
10247evalcond[5]=((((-1.0)*x761))+x757+new_r11);
10248evalcond[6]=(((new_r00*x755))+((new_r10*x756))+(((-1.0)*cj5)));
10249evalcond[7]=((((-1.0)*x759))+(((-1.0)*x758))+new_r10);
10250if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10251{
10252continue;
10253}
10254}
10255
10256{
10257std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10258vinfos[0].jointtype = 1;
10259vinfos[0].foffset = j0;
10260vinfos[0].indices[0] = _ij0[0];
10261vinfos[0].indices[1] = _ij0[1];
10262vinfos[0].maxsolutions = _nj0;
10263vinfos[1].jointtype = 1;
10264vinfos[1].foffset = j1;
10265vinfos[1].indices[0] = _ij1[0];
10266vinfos[1].indices[1] = _ij1[1];
10267vinfos[1].maxsolutions = _nj1;
10268vinfos[2].jointtype = 1;
10269vinfos[2].foffset = j2;
10270vinfos[2].indices[0] = _ij2[0];
10271vinfos[2].indices[1] = _ij2[1];
10272vinfos[2].maxsolutions = _nj2;
10273vinfos[3].jointtype = 1;
10274vinfos[3].foffset = j3;
10275vinfos[3].indices[0] = _ij3[0];
10276vinfos[3].indices[1] = _ij3[1];
10277vinfos[3].maxsolutions = _nj3;
10278vinfos[4].jointtype = 1;
10279vinfos[4].foffset = j4;
10280vinfos[4].indices[0] = _ij4[0];
10281vinfos[4].indices[1] = _ij4[1];
10282vinfos[4].maxsolutions = _nj4;
10283vinfos[5].jointtype = 1;
10284vinfos[5].foffset = j5;
10285vinfos[5].indices[0] = _ij5[0];
10286vinfos[5].indices[1] = _ij5[1];
10287vinfos[5].maxsolutions = _nj5;
10288std::vector<int> vfree(0);
10289solutions.AddSolution(vinfos,vfree);
10290}
10291}
10292}
10293
10294}
10295
10296}
10297
10298} else
10299{
10300{
10301IkReal j3array[1], cj3array[1], sj3array[1];
10302bool j3valid[1]={false};
10303_nj3 = 1;
10304IkReal x762=((1.0)*new_r11);
10305CheckValue<IkReal> x763 = IKatan2WithCheck(IkReal((((new_r10*sj5))+((new_r01*sj5)))),IkReal((((new_r00*sj5))+(((-1.0)*sj5*x762)))),IKFAST_ATAN2_MAGTHRESH);
10306if(!x763.valid){
10307continue;
10308}
10309CheckValue<IkReal> x764=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x762))+(((-1.0)*new_r00*new_r01)))),-1);
10310if(!x764.valid){
10311continue;
10312}
10313j3array[0]=((-1.5707963267949)+(x763.value)+(((1.5707963267949)*(x764.value))));
10314sj3array[0]=IKsin(j3array[0]);
10315cj3array[0]=IKcos(j3array[0]);
10316if( j3array[0] > IKPI )
10317{
10318 j3array[0]-=IK2PI;
10319}
10320else if( j3array[0] < -IKPI )
10321{ j3array[0]+=IK2PI;
10322}
10323j3valid[0] = true;
10324for(int ij3 = 0; ij3 < 1; ++ij3)
10325{
10326if( !j3valid[ij3] )
10327{
10328 continue;
10329}
10330_ij3[0] = ij3; _ij3[1] = -1;
10331for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10332{
10333if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10334{
10335 j3valid[iij3]=false; _ij3[1] = iij3; break;
10336}
10337}
10338j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10339{
10340IkReal evalcond[8];
10341IkReal x765=IKcos(j3);
10342IkReal x766=IKsin(j3);
10343IkReal x767=(sj5*x766);
10344IkReal x768=(cj5*x766);
10345IkReal x769=(sj5*x765);
10346IkReal x770=((1.0)*x765);
10347IkReal x771=(cj5*x770);
10348evalcond[0]=(sj5+((new_r11*x766))+((new_r01*x765)));
10349evalcond[1]=(x768+x769+new_r01);
10350evalcond[2]=(sj5+((new_r00*x766))+(((-1.0)*new_r10*x770)));
10351evalcond[3]=(cj5+((new_r01*x766))+(((-1.0)*new_r11*x770)));
10352evalcond[4]=(x767+(((-1.0)*x771))+new_r00);
10353evalcond[5]=(x767+(((-1.0)*x771))+new_r11);
10354evalcond[6]=(((new_r10*x766))+((new_r00*x765))+(((-1.0)*cj5)));
10355evalcond[7]=((((-1.0)*x769))+(((-1.0)*x768))+new_r10);
10356if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
10357{
10358continue;
10359}
10360}
10361
10362{
10363std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10364vinfos[0].jointtype = 1;
10365vinfos[0].foffset = j0;
10366vinfos[0].indices[0] = _ij0[0];
10367vinfos[0].indices[1] = _ij0[1];
10368vinfos[0].maxsolutions = _nj0;
10369vinfos[1].jointtype = 1;
10370vinfos[1].foffset = j1;
10371vinfos[1].indices[0] = _ij1[0];
10372vinfos[1].indices[1] = _ij1[1];
10373vinfos[1].maxsolutions = _nj1;
10374vinfos[2].jointtype = 1;
10375vinfos[2].foffset = j2;
10376vinfos[2].indices[0] = _ij2[0];
10377vinfos[2].indices[1] = _ij2[1];
10378vinfos[2].maxsolutions = _nj2;
10379vinfos[3].jointtype = 1;
10380vinfos[3].foffset = j3;
10381vinfos[3].indices[0] = _ij3[0];
10382vinfos[3].indices[1] = _ij3[1];
10383vinfos[3].maxsolutions = _nj3;
10384vinfos[4].jointtype = 1;
10385vinfos[4].foffset = j4;
10386vinfos[4].indices[0] = _ij4[0];
10387vinfos[4].indices[1] = _ij4[1];
10388vinfos[4].maxsolutions = _nj4;
10389vinfos[5].jointtype = 1;
10390vinfos[5].foffset = j5;
10391vinfos[5].indices[0] = _ij5[0];
10392vinfos[5].indices[1] = _ij5[1];
10393vinfos[5].maxsolutions = _nj5;
10394std::vector<int> vfree(0);
10395solutions.AddSolution(vinfos,vfree);
10396}
10397}
10398}
10399
10400}
10401
10402}
10403
10404}
10405} while(0);
10406if( bgotonextstatement )
10407{
10408bool bgotonextstatement = true;
10409do
10410{
10411evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j4)))), 6.28318530717959)));
10412evalcond[1]=new_r02;
10413evalcond[2]=new_r12;
10414evalcond[3]=new_r21;
10415evalcond[4]=new_r20;
10416if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 && IKabs(evalcond[2]) < 0.0000050000000000 && IKabs(evalcond[3]) < 0.0000050000000000 && IKabs(evalcond[4]) < 0.0000050000000000 )
10417{
10418bgotonextstatement=false;
10419{
10420IkReal j3eval[3];
10421sj4=0;
10422cj4=-1.0;
10423j4=3.14159265358979;
10424IkReal x772=((1.0)*new_r10);
10425IkReal x773=((((-1.0)*new_r11*x772))+(((-1.0)*new_r00*new_r01)));
10426j3eval[0]=x773;
10427j3eval[1]=((IKabs((((cj5*new_r11))+((cj5*new_r00)))))+(IKabs((((cj5*new_r01))+(((-1.0)*cj5*x772))))));
10428j3eval[2]=IKsign(x773);
10429if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10430{
10431{
10432IkReal j3eval[3];
10433sj4=0;
10434cj4=-1.0;
10435j4=3.14159265358979;
10436IkReal x774=((1.0)*new_r10);
10437IkReal x775=((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x774)));
10438j3eval[0]=x775;
10439j3eval[1]=IKsign(x775);
10440j3eval[2]=((IKabs((((new_r00*new_r01))+((cj5*sj5)))))+(IKabs(((((-1.0)*new_r01*x774))+(cj5*cj5)))));
10441if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10442{
10443{
10444IkReal j3eval[3];
10445sj4=0;
10446cj4=-1.0;
10447j4=3.14159265358979;
10448IkReal x776=((1.0)*new_r00);
10449IkReal x777=((((-1.0)*sj5*x776))+((cj5*new_r10)));
10450j3eval[0]=x777;
10451j3eval[1]=((IKabs((((cj5*sj5))+(((-1.0)*new_r10*x776)))))+(IKabs(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00)))));
10452j3eval[2]=IKsign(x777);
10453if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10454{
10455{
10456IkReal evalcond[1];
10457bool bgotonextstatement = true;
10458do
10459{
10460IkReal x778=((-1.0)*new_r00);
10461IkReal x780 = ((new_r10*new_r10)+(new_r00*new_r00));
10462if(IKabs(x780)==0){
10463continue;
10464}
10465IkReal x779=pow(x780,-0.5);
10466CheckValue<IkReal> x781 = IKatan2WithCheck(IkReal(new_r10),IkReal(x778),IKFAST_ATAN2_MAGTHRESH);
10467if(!x781.valid){
10468continue;
10469}
10470IkReal gconst6=((-1.0)*(x781.value));
10471IkReal gconst7=((-1.0)*new_r10*x779);
10472IkReal gconst8=(x778*x779);
10473CheckValue<IkReal> x782 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
10474if(!x782.valid){
10475continue;
10476}
10477evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs((j5+(x782.value))))), 6.28318530717959)));
10478if( IKabs(evalcond[0]) < 0.0000050000000000 )
10479{
10480bgotonextstatement=false;
10481{
10482IkReal j3eval[3];
10483IkReal x783=((-1.0)*new_r00);
10484CheckValue<IkReal> x786 = IKatan2WithCheck(IkReal(new_r10),IkReal(x783),IKFAST_ATAN2_MAGTHRESH);
10485if(!x786.valid){
10486continue;
10487}
10488IkReal x784=((-1.0)*(x786.value));
10489IkReal x785=x779;
10490sj4=0;
10491cj4=-1.0;
10492j4=3.14159265358979;
10493sj5=gconst7;
10494cj5=gconst8;
10495j5=x784;
10496IkReal gconst6=x784;
10497IkReal gconst7=((-1.0)*new_r10*x785);
10498IkReal gconst8=(x783*x785);
10499IkReal x787=new_r00*new_r00;
10500IkReal x788=((1.0)*new_r11);
10501IkReal x789=((1.0)*new_r00*new_r01);
10502IkReal x790=((((-1.0)*x789))+(((-1.0)*new_r10*x788)));
10503IkReal x791=x779;
10504IkReal x792=(new_r00*x791);
10505j3eval[0]=x790;
10506j3eval[1]=((IKabs(((((-1.0)*x788*x792))+(((-1.0)*x787*x791)))))+(IKabs((((new_r10*x792))+(((-1.0)*x789*x791))))));
10507j3eval[2]=IKsign(x790);
10508if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10509{
10510{
10511IkReal j3eval[1];
10512IkReal x793=((-1.0)*new_r00);
10513CheckValue<IkReal> x796 = IKatan2WithCheck(IkReal(new_r10),IkReal(x793),IKFAST_ATAN2_MAGTHRESH);
10514if(!x796.valid){
10515continue;
10516}
10517IkReal x794=((-1.0)*(x796.value));
10518IkReal x795=x779;
10519sj4=0;
10520cj4=-1.0;
10521j4=3.14159265358979;
10522sj5=gconst7;
10523cj5=gconst8;
10524j5=x794;
10525IkReal gconst6=x794;
10526IkReal gconst7=((-1.0)*new_r10*x795);
10527IkReal gconst8=(x793*x795);
10528IkReal x797=new_r10*new_r10;
10529IkReal x798=new_r00*new_r00;
10530CheckValue<IkReal> x801=IKPowWithIntegerCheck((x797+x798),-1);
10531if(!x801.valid){
10532continue;
10533}
10534IkReal x799=x801.value;
10535IkReal x800=(new_r00*x799);
10536j3eval[0]=((IKabs((((new_r01*x797*x800))+((new_r10*x800))+((new_r01*x800*(new_r00*new_r00))))))+(IKabs((((x798*x799))+(((-1.0)*new_r01*new_r10))))));
10537if( IKabs(j3eval[0]) < 0.0000010000000000 )
10538{
10539{
10540IkReal j3eval[1];
10541IkReal x802=((-1.0)*new_r00);
10542CheckValue<IkReal> x805 = IKatan2WithCheck(IkReal(new_r10),IkReal(x802),IKFAST_ATAN2_MAGTHRESH);
10543if(!x805.valid){
10544continue;
10545}
10546IkReal x803=((-1.0)*(x805.value));
10547IkReal x804=x779;
10548sj4=0;
10549cj4=-1.0;
10550j4=3.14159265358979;
10551sj5=gconst7;
10552cj5=gconst8;
10553j5=x803;
10554IkReal gconst6=x803;
10555IkReal gconst7=((-1.0)*new_r10*x804);
10556IkReal gconst8=(x802*x804);
10557IkReal x806=new_r00*new_r00;
10558IkReal x807=new_r10*new_r10;
10559CheckValue<IkReal> x811=IKPowWithIntegerCheck((x807+x806),-1);
10560if(!x811.valid){
10561continue;
10562}
10563IkReal x808=x811.value;
10564IkReal x809=(new_r10*x808);
10565IkReal x810=((1.0)*x808);
10566j3eval[0]=((IKabs((((new_r00*x809*(new_r10*new_r10)))+((new_r00*x809))+((x809*(new_r00*new_r00*new_r00))))))+(IKabs((((x806*x808))+(((-1.0)*x810*(x807*x807)))+(((-1.0)*x806*x807*x810))))));
10567if( IKabs(j3eval[0]) < 0.0000010000000000 )
10568{
10569{
10570IkReal evalcond[2];
10571bool bgotonextstatement = true;
10572do
10573{
10574evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
10575if( IKabs(evalcond[0]) < 0.0000050000000000 )
10576{
10577bgotonextstatement=false;
10578{
10579IkReal j3eval[1];
10580CheckValue<IkReal> x813 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
10581if(!x813.valid){
10582continue;
10583}
10584IkReal x812=((-1.0)*(x813.value));
10585sj4=0;
10586cj4=-1.0;
10587j4=3.14159265358979;
10588sj5=gconst7;
10589cj5=gconst8;
10590j5=x812;
10591new_r11=0;
10592new_r00=0;
10593IkReal gconst6=x812;
10594IkReal x814 = new_r10*new_r10;
10595if(IKabs(x814)==0){
10596continue;
10597}
10598IkReal gconst7=((-1.0)*new_r10*(pow(x814,-0.5)));
10599IkReal gconst8=0;
10600j3eval[0]=new_r10;
10601if( IKabs(j3eval[0]) < 0.0000010000000000 )
10602{
10603{
10604IkReal j3array[2], cj3array[2], sj3array[2];
10605bool j3valid[2]={false};
10606_nj3 = 2;
10607CheckValue<IkReal> x815=IKPowWithIntegerCheck(gconst7,-1);
10608if(!x815.valid){
10609continue;
10610}
10611cj3array[0]=(new_r01*(x815.value));
10612if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10613{
10614 j3valid[0] = j3valid[1] = true;
10615 j3array[0] = IKacos(cj3array[0]);
10616 sj3array[0] = IKsin(j3array[0]);
10617 cj3array[1] = cj3array[0];
10618 j3array[1] = -j3array[0];
10619 sj3array[1] = -sj3array[0];
10620}
10621else if( isnan(cj3array[0]) )
10622{
10623 // probably any value will work
10624 j3valid[0] = true;
10625 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10626}
10627for(int ij3 = 0; ij3 < 2; ++ij3)
10628{
10629if( !j3valid[ij3] )
10630{
10631 continue;
10632}
10633_ij3[0] = ij3; _ij3[1] = -1;
10634for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10635{
10636if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10637{
10638 j3valid[iij3]=false; _ij3[1] = iij3; break;
10639}
10640}
10641j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10642{
10643IkReal evalcond[6];
10644IkReal x816=IKsin(j3);
10645IkReal x817=IKcos(j3);
10646IkReal x818=((1.0)*gconst7);
10647evalcond[0]=(new_r01*x816);
10648evalcond[1]=(new_r10*x816);
10649evalcond[2]=(gconst7*x816);
10650evalcond[3]=((((-1.0)*new_r10*x817))+gconst7);
10651evalcond[4]=((((-1.0)*x817*x818))+new_r10);
10652evalcond[5]=(((new_r01*x817))+(((-1.0)*x818)));
10653if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10654{
10655continue;
10656}
10657}
10658
10659{
10660std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10661vinfos[0].jointtype = 1;
10662vinfos[0].foffset = j0;
10663vinfos[0].indices[0] = _ij0[0];
10664vinfos[0].indices[1] = _ij0[1];
10665vinfos[0].maxsolutions = _nj0;
10666vinfos[1].jointtype = 1;
10667vinfos[1].foffset = j1;
10668vinfos[1].indices[0] = _ij1[0];
10669vinfos[1].indices[1] = _ij1[1];
10670vinfos[1].maxsolutions = _nj1;
10671vinfos[2].jointtype = 1;
10672vinfos[2].foffset = j2;
10673vinfos[2].indices[0] = _ij2[0];
10674vinfos[2].indices[1] = _ij2[1];
10675vinfos[2].maxsolutions = _nj2;
10676vinfos[3].jointtype = 1;
10677vinfos[3].foffset = j3;
10678vinfos[3].indices[0] = _ij3[0];
10679vinfos[3].indices[1] = _ij3[1];
10680vinfos[3].maxsolutions = _nj3;
10681vinfos[4].jointtype = 1;
10682vinfos[4].foffset = j4;
10683vinfos[4].indices[0] = _ij4[0];
10684vinfos[4].indices[1] = _ij4[1];
10685vinfos[4].maxsolutions = _nj4;
10686vinfos[5].jointtype = 1;
10687vinfos[5].foffset = j5;
10688vinfos[5].indices[0] = _ij5[0];
10689vinfos[5].indices[1] = _ij5[1];
10690vinfos[5].maxsolutions = _nj5;
10691std::vector<int> vfree(0);
10692solutions.AddSolution(vinfos,vfree);
10693}
10694}
10695}
10696
10697} else
10698{
10699{
10700IkReal j3array[2], cj3array[2], sj3array[2];
10701bool j3valid[2]={false};
10702_nj3 = 2;
10703CheckValue<IkReal> x819=IKPowWithIntegerCheck(new_r10,-1);
10704if(!x819.valid){
10705continue;
10706}
10707cj3array[0]=(gconst7*(x819.value));
10708if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
10709{
10710 j3valid[0] = j3valid[1] = true;
10711 j3array[0] = IKacos(cj3array[0]);
10712 sj3array[0] = IKsin(j3array[0]);
10713 cj3array[1] = cj3array[0];
10714 j3array[1] = -j3array[0];
10715 sj3array[1] = -sj3array[0];
10716}
10717else if( isnan(cj3array[0]) )
10718{
10719 // probably any value will work
10720 j3valid[0] = true;
10721 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
10722}
10723for(int ij3 = 0; ij3 < 2; ++ij3)
10724{
10725if( !j3valid[ij3] )
10726{
10727 continue;
10728}
10729_ij3[0] = ij3; _ij3[1] = -1;
10730for(int iij3 = ij3+1; iij3 < 2; ++iij3)
10731{
10732if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10733{
10734 j3valid[iij3]=false; _ij3[1] = iij3; break;
10735}
10736}
10737j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10738{
10739IkReal evalcond[6];
10740IkReal x820=IKsin(j3);
10741IkReal x821=IKcos(j3);
10742IkReal x822=((1.0)*gconst7);
10743IkReal x823=(x821*x822);
10744evalcond[0]=(new_r01*x820);
10745evalcond[1]=(new_r10*x820);
10746evalcond[2]=(gconst7*x820);
10747evalcond[3]=((((-1.0)*x823))+new_r01);
10748evalcond[4]=((((-1.0)*x823))+new_r10);
10749evalcond[5]=(((new_r01*x821))+(((-1.0)*x822)));
10750if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10751{
10752continue;
10753}
10754}
10755
10756{
10757std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10758vinfos[0].jointtype = 1;
10759vinfos[0].foffset = j0;
10760vinfos[0].indices[0] = _ij0[0];
10761vinfos[0].indices[1] = _ij0[1];
10762vinfos[0].maxsolutions = _nj0;
10763vinfos[1].jointtype = 1;
10764vinfos[1].foffset = j1;
10765vinfos[1].indices[0] = _ij1[0];
10766vinfos[1].indices[1] = _ij1[1];
10767vinfos[1].maxsolutions = _nj1;
10768vinfos[2].jointtype = 1;
10769vinfos[2].foffset = j2;
10770vinfos[2].indices[0] = _ij2[0];
10771vinfos[2].indices[1] = _ij2[1];
10772vinfos[2].maxsolutions = _nj2;
10773vinfos[3].jointtype = 1;
10774vinfos[3].foffset = j3;
10775vinfos[3].indices[0] = _ij3[0];
10776vinfos[3].indices[1] = _ij3[1];
10777vinfos[3].maxsolutions = _nj3;
10778vinfos[4].jointtype = 1;
10779vinfos[4].foffset = j4;
10780vinfos[4].indices[0] = _ij4[0];
10781vinfos[4].indices[1] = _ij4[1];
10782vinfos[4].maxsolutions = _nj4;
10783vinfos[5].jointtype = 1;
10784vinfos[5].foffset = j5;
10785vinfos[5].indices[0] = _ij5[0];
10786vinfos[5].indices[1] = _ij5[1];
10787vinfos[5].maxsolutions = _nj5;
10788std::vector<int> vfree(0);
10789solutions.AddSolution(vinfos,vfree);
10790}
10791}
10792}
10793
10794}
10795
10796}
10797
10798}
10799} while(0);
10800if( bgotonextstatement )
10801{
10802bool bgotonextstatement = true;
10803do
10804{
10805evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
10806evalcond[1]=gconst8;
10807if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
10808{
10809bgotonextstatement=false;
10810{
10811IkReal j3eval[3];
10812IkReal x824=((-1.0)*new_r00);
10813CheckValue<IkReal> x826 = IKatan2WithCheck(IkReal(new_r10),IkReal(x824),IKFAST_ATAN2_MAGTHRESH);
10814if(!x826.valid){
10815continue;
10816}
10817IkReal x825=((-1.0)*(x826.value));
10818sj4=0;
10819cj4=-1.0;
10820j4=3.14159265358979;
10821sj5=gconst7;
10822cj5=gconst8;
10823j5=x825;
10824new_r11=0;
10825new_r01=0;
10826new_r22=0;
10827new_r20=0;
10828IkReal gconst6=x825;
10829IkReal gconst7=((-1.0)*new_r10);
10830IkReal gconst8=x824;
10831j3eval[0]=1.0;
10832j3eval[1]=1.0;
10833j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
10834if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10835{
10836{
10837IkReal j3eval[3];
10838IkReal x827=((-1.0)*new_r00);
10839CheckValue<IkReal> x829 = IKatan2WithCheck(IkReal(new_r10),IkReal(x827),IKFAST_ATAN2_MAGTHRESH);
10840if(!x829.valid){
10841continue;
10842}
10843IkReal x828=((-1.0)*(x829.value));
10844sj4=0;
10845cj4=-1.0;
10846j4=3.14159265358979;
10847sj5=gconst7;
10848cj5=gconst8;
10849j5=x828;
10850new_r11=0;
10851new_r01=0;
10852new_r22=0;
10853new_r20=0;
10854IkReal gconst6=x828;
10855IkReal gconst7=((-1.0)*new_r10);
10856IkReal gconst8=x827;
10857j3eval[0]=-1.0;
10858j3eval[1]=((IKabs(((-1.0)+(new_r10*new_r10))))+(IKabs((new_r00*new_r10))));
10859j3eval[2]=-1.0;
10860if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10861{
10862{
10863IkReal j3eval[3];
10864IkReal x830=((-1.0)*new_r00);
10865CheckValue<IkReal> x832 = IKatan2WithCheck(IkReal(new_r10),IkReal(x830),IKFAST_ATAN2_MAGTHRESH);
10866if(!x832.valid){
10867continue;
10868}
10869IkReal x831=((-1.0)*(x832.value));
10870sj4=0;
10871cj4=-1.0;
10872j4=3.14159265358979;
10873sj5=gconst7;
10874cj5=gconst8;
10875j5=x831;
10876new_r11=0;
10877new_r01=0;
10878new_r22=0;
10879new_r20=0;
10880IkReal gconst6=x831;
10881IkReal gconst7=((-1.0)*new_r10);
10882IkReal gconst8=x830;
10883j3eval[0]=1.0;
10884j3eval[1]=((((0.5)*(IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))))+(IKabs((new_r00*new_r10))));
10885j3eval[2]=1.0;
10886if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
10887{
10888continue; // 3 cases reached
10889
10890} else
10891{
10892{
10893IkReal j3array[1], cj3array[1], sj3array[1];
10894bool j3valid[1]={false};
10895_nj3 = 1;
10896CheckValue<IkReal> x833=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*new_r10))+(((-1.0)*gconst8*new_r00)))),-1);
10897if(!x833.valid){
10898continue;
10899}
10900CheckValue<IkReal> x834 = IKatan2WithCheck(IkReal((((new_r00*new_r10))+((gconst7*gconst8)))),IkReal(((gconst8*gconst8)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
10901if(!x834.valid){
10902continue;
10903}
10904j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x833.value)))+(x834.value));
10905sj3array[0]=IKsin(j3array[0]);
10906cj3array[0]=IKcos(j3array[0]);
10907if( j3array[0] > IKPI )
10908{
10909 j3array[0]-=IK2PI;
10910}
10911else if( j3array[0] < -IKPI )
10912{ j3array[0]+=IK2PI;
10913}
10914j3valid[0] = true;
10915for(int ij3 = 0; ij3 < 1; ++ij3)
10916{
10917if( !j3valid[ij3] )
10918{
10919 continue;
10920}
10921_ij3[0] = ij3; _ij3[1] = -1;
10922for(int iij3 = ij3+1; iij3 < 1; ++iij3)
10923{
10924if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
10925{
10926 j3valid[iij3]=false; _ij3[1] = iij3; break;
10927}
10928}
10929j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
10930{
10931IkReal evalcond[6];
10932IkReal x835=IKsin(j3);
10933IkReal x836=IKcos(j3);
10934IkReal x837=(gconst8*x835);
10935IkReal x838=(gconst7*x835);
10936IkReal x839=(gconst8*x836);
10937IkReal x840=((1.0)*x836);
10938IkReal x841=(gconst7*x840);
10939evalcond[0]=((((-1.0)*x841))+x837);
10940evalcond[1]=(gconst8+((new_r00*x836))+((new_r10*x835)));
10941evalcond[2]=(new_r00+x838+x839);
10942evalcond[3]=((((-1.0)*new_r10*x840))+gconst7+((new_r00*x835)));
10943evalcond[4]=((((-1.0)*x838))+(((-1.0)*x839)));
10944evalcond[5]=((((-1.0)*x841))+new_r10+x837);
10945if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
10946{
10947continue;
10948}
10949}
10950
10951{
10952std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10953vinfos[0].jointtype = 1;
10954vinfos[0].foffset = j0;
10955vinfos[0].indices[0] = _ij0[0];
10956vinfos[0].indices[1] = _ij0[1];
10957vinfos[0].maxsolutions = _nj0;
10958vinfos[1].jointtype = 1;
10959vinfos[1].foffset = j1;
10960vinfos[1].indices[0] = _ij1[0];
10961vinfos[1].indices[1] = _ij1[1];
10962vinfos[1].maxsolutions = _nj1;
10963vinfos[2].jointtype = 1;
10964vinfos[2].foffset = j2;
10965vinfos[2].indices[0] = _ij2[0];
10966vinfos[2].indices[1] = _ij2[1];
10967vinfos[2].maxsolutions = _nj2;
10968vinfos[3].jointtype = 1;
10969vinfos[3].foffset = j3;
10970vinfos[3].indices[0] = _ij3[0];
10971vinfos[3].indices[1] = _ij3[1];
10972vinfos[3].maxsolutions = _nj3;
10973vinfos[4].jointtype = 1;
10974vinfos[4].foffset = j4;
10975vinfos[4].indices[0] = _ij4[0];
10976vinfos[4].indices[1] = _ij4[1];
10977vinfos[4].maxsolutions = _nj4;
10978vinfos[5].jointtype = 1;
10979vinfos[5].foffset = j5;
10980vinfos[5].indices[0] = _ij5[0];
10981vinfos[5].indices[1] = _ij5[1];
10982vinfos[5].maxsolutions = _nj5;
10983std::vector<int> vfree(0);
10984solutions.AddSolution(vinfos,vfree);
10985}
10986}
10987}
10988
10989}
10990
10991}
10992
10993} else
10994{
10995{
10996IkReal j3array[1], cj3array[1], sj3array[1];
10997bool j3valid[1]={false};
10998_nj3 = 1;
10999CheckValue<IkReal> x842 = IKatan2WithCheck(IkReal((gconst7*new_r00)),IkReal((gconst8*new_r00)),IKFAST_ATAN2_MAGTHRESH);
11000if(!x842.valid){
11001continue;
11002}
11003CheckValue<IkReal> x843=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst8*gconst8)))+(((-1.0)*(gconst7*gconst7))))),-1);
11004if(!x843.valid){
11005continue;
11006}
11007j3array[0]=((-1.5707963267949)+(x842.value)+(((1.5707963267949)*(x843.value))));
11008sj3array[0]=IKsin(j3array[0]);
11009cj3array[0]=IKcos(j3array[0]);
11010if( j3array[0] > IKPI )
11011{
11012 j3array[0]-=IK2PI;
11013}
11014else if( j3array[0] < -IKPI )
11015{ j3array[0]+=IK2PI;
11016}
11017j3valid[0] = true;
11018for(int ij3 = 0; ij3 < 1; ++ij3)
11019{
11020if( !j3valid[ij3] )
11021{
11022 continue;
11023}
11024_ij3[0] = ij3; _ij3[1] = -1;
11025for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11026{
11027if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11028{
11029 j3valid[iij3]=false; _ij3[1] = iij3; break;
11030}
11031}
11032j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11033{
11034IkReal evalcond[6];
11035IkReal x844=IKsin(j3);
11036IkReal x845=IKcos(j3);
11037IkReal x846=(gconst8*x844);
11038IkReal x847=(gconst7*x844);
11039IkReal x848=(gconst8*x845);
11040IkReal x849=((1.0)*x845);
11041IkReal x850=(gconst7*x849);
11042evalcond[0]=((((-1.0)*x850))+x846);
11043evalcond[1]=(((new_r10*x844))+gconst8+((new_r00*x845)));
11044evalcond[2]=(new_r00+x847+x848);
11045evalcond[3]=((((-1.0)*new_r10*x849))+gconst7+((new_r00*x844)));
11046evalcond[4]=((((-1.0)*x848))+(((-1.0)*x847)));
11047evalcond[5]=((((-1.0)*x850))+new_r10+x846);
11048if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11049{
11050continue;
11051}
11052}
11053
11054{
11055std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11056vinfos[0].jointtype = 1;
11057vinfos[0].foffset = j0;
11058vinfos[0].indices[0] = _ij0[0];
11059vinfos[0].indices[1] = _ij0[1];
11060vinfos[0].maxsolutions = _nj0;
11061vinfos[1].jointtype = 1;
11062vinfos[1].foffset = j1;
11063vinfos[1].indices[0] = _ij1[0];
11064vinfos[1].indices[1] = _ij1[1];
11065vinfos[1].maxsolutions = _nj1;
11066vinfos[2].jointtype = 1;
11067vinfos[2].foffset = j2;
11068vinfos[2].indices[0] = _ij2[0];
11069vinfos[2].indices[1] = _ij2[1];
11070vinfos[2].maxsolutions = _nj2;
11071vinfos[3].jointtype = 1;
11072vinfos[3].foffset = j3;
11073vinfos[3].indices[0] = _ij3[0];
11074vinfos[3].indices[1] = _ij3[1];
11075vinfos[3].maxsolutions = _nj3;
11076vinfos[4].jointtype = 1;
11077vinfos[4].foffset = j4;
11078vinfos[4].indices[0] = _ij4[0];
11079vinfos[4].indices[1] = _ij4[1];
11080vinfos[4].maxsolutions = _nj4;
11081vinfos[5].jointtype = 1;
11082vinfos[5].foffset = j5;
11083vinfos[5].indices[0] = _ij5[0];
11084vinfos[5].indices[1] = _ij5[1];
11085vinfos[5].maxsolutions = _nj5;
11086std::vector<int> vfree(0);
11087solutions.AddSolution(vinfos,vfree);
11088}
11089}
11090}
11091
11092}
11093
11094}
11095
11096} else
11097{
11098{
11099IkReal j3array[1], cj3array[1], sj3array[1];
11100bool j3valid[1]={false};
11101_nj3 = 1;
11102CheckValue<IkReal> x851 = IKatan2WithCheck(IkReal((gconst7*gconst8)),IkReal(gconst8*gconst8),IKFAST_ATAN2_MAGTHRESH);
11103if(!x851.valid){
11104continue;
11105}
11106CheckValue<IkReal> x852=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*new_r10))+(((-1.0)*gconst8*new_r00)))),-1);
11107if(!x852.valid){
11108continue;
11109}
11110j3array[0]=((-1.5707963267949)+(x851.value)+(((1.5707963267949)*(x852.value))));
11111sj3array[0]=IKsin(j3array[0]);
11112cj3array[0]=IKcos(j3array[0]);
11113if( j3array[0] > IKPI )
11114{
11115 j3array[0]-=IK2PI;
11116}
11117else if( j3array[0] < -IKPI )
11118{ j3array[0]+=IK2PI;
11119}
11120j3valid[0] = true;
11121for(int ij3 = 0; ij3 < 1; ++ij3)
11122{
11123if( !j3valid[ij3] )
11124{
11125 continue;
11126}
11127_ij3[0] = ij3; _ij3[1] = -1;
11128for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11129{
11130if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11131{
11132 j3valid[iij3]=false; _ij3[1] = iij3; break;
11133}
11134}
11135j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11136{
11137IkReal evalcond[6];
11138IkReal x853=IKsin(j3);
11139IkReal x854=IKcos(j3);
11140IkReal x855=(gconst8*x853);
11141IkReal x856=(gconst7*x853);
11142IkReal x857=(gconst8*x854);
11143IkReal x858=((1.0)*x854);
11144IkReal x859=(gconst7*x858);
11145evalcond[0]=((((-1.0)*x859))+x855);
11146evalcond[1]=(gconst8+((new_r10*x853))+((new_r00*x854)));
11147evalcond[2]=(new_r00+x856+x857);
11148evalcond[3]=(gconst7+(((-1.0)*new_r10*x858))+((new_r00*x853)));
11149evalcond[4]=((((-1.0)*x856))+(((-1.0)*x857)));
11150evalcond[5]=((((-1.0)*x859))+new_r10+x855);
11151if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11152{
11153continue;
11154}
11155}
11156
11157{
11158std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11159vinfos[0].jointtype = 1;
11160vinfos[0].foffset = j0;
11161vinfos[0].indices[0] = _ij0[0];
11162vinfos[0].indices[1] = _ij0[1];
11163vinfos[0].maxsolutions = _nj0;
11164vinfos[1].jointtype = 1;
11165vinfos[1].foffset = j1;
11166vinfos[1].indices[0] = _ij1[0];
11167vinfos[1].indices[1] = _ij1[1];
11168vinfos[1].maxsolutions = _nj1;
11169vinfos[2].jointtype = 1;
11170vinfos[2].foffset = j2;
11171vinfos[2].indices[0] = _ij2[0];
11172vinfos[2].indices[1] = _ij2[1];
11173vinfos[2].maxsolutions = _nj2;
11174vinfos[3].jointtype = 1;
11175vinfos[3].foffset = j3;
11176vinfos[3].indices[0] = _ij3[0];
11177vinfos[3].indices[1] = _ij3[1];
11178vinfos[3].maxsolutions = _nj3;
11179vinfos[4].jointtype = 1;
11180vinfos[4].foffset = j4;
11181vinfos[4].indices[0] = _ij4[0];
11182vinfos[4].indices[1] = _ij4[1];
11183vinfos[4].maxsolutions = _nj4;
11184vinfos[5].jointtype = 1;
11185vinfos[5].foffset = j5;
11186vinfos[5].indices[0] = _ij5[0];
11187vinfos[5].indices[1] = _ij5[1];
11188vinfos[5].maxsolutions = _nj5;
11189std::vector<int> vfree(0);
11190solutions.AddSolution(vinfos,vfree);
11191}
11192}
11193}
11194
11195}
11196
11197}
11198
11199}
11200} while(0);
11201if( bgotonextstatement )
11202{
11203bool bgotonextstatement = true;
11204do
11205{
11206evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
11207if( IKabs(evalcond[0]) < 0.0000050000000000 )
11208{
11209bgotonextstatement=false;
11210{
11211IkReal j3eval[1];
11212IkReal x860=((-1.0)*new_r00);
11213CheckValue<IkReal> x862 = IKatan2WithCheck(IkReal(0),IkReal(x860),IKFAST_ATAN2_MAGTHRESH);
11214if(!x862.valid){
11215continue;
11216}
11217IkReal x861=((-1.0)*(x862.value));
11218sj4=0;
11219cj4=-1.0;
11220j4=3.14159265358979;
11221sj5=gconst7;
11222cj5=gconst8;
11223j5=x861;
11224new_r01=0;
11225new_r10=0;
11226IkReal gconst6=x861;
11227IkReal gconst7=0;
11228IkReal x863 = new_r00*new_r00;
11229if(IKabs(x863)==0){
11230continue;
11231}
11232IkReal gconst8=(x860*(pow(x863,-0.5)));
11233j3eval[0]=new_r11;
11234if( IKabs(j3eval[0]) < 0.0000010000000000 )
11235{
11236{
11237IkReal j3array[2], cj3array[2], sj3array[2];
11238bool j3valid[2]={false};
11239_nj3 = 2;
11240CheckValue<IkReal> x864=IKPowWithIntegerCheck(gconst8,-1);
11241if(!x864.valid){
11242continue;
11243}
11244cj3array[0]=(new_r11*(x864.value));
11245if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
11246{
11247 j3valid[0] = j3valid[1] = true;
11248 j3array[0] = IKacos(cj3array[0]);
11249 sj3array[0] = IKsin(j3array[0]);
11250 cj3array[1] = cj3array[0];
11251 j3array[1] = -j3array[0];
11252 sj3array[1] = -sj3array[0];
11253}
11254else if( isnan(cj3array[0]) )
11255{
11256 // probably any value will work
11257 j3valid[0] = true;
11258 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11259}
11260for(int ij3 = 0; ij3 < 2; ++ij3)
11261{
11262if( !j3valid[ij3] )
11263{
11264 continue;
11265}
11266_ij3[0] = ij3; _ij3[1] = -1;
11267for(int iij3 = ij3+1; iij3 < 2; ++iij3)
11268{
11269if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11270{
11271 j3valid[iij3]=false; _ij3[1] = iij3; break;
11272}
11273}
11274j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11275{
11276IkReal evalcond[6];
11277IkReal x865=IKsin(j3);
11278IkReal x866=IKcos(j3);
11279evalcond[0]=(new_r00*x865);
11280evalcond[1]=(new_r11*x865);
11281evalcond[2]=(gconst8*x865);
11282evalcond[3]=(((new_r00*x866))+gconst8);
11283evalcond[4]=(((gconst8*x866))+new_r00);
11284evalcond[5]=(gconst8+(((-1.0)*new_r11*x866)));
11285if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11286{
11287continue;
11288}
11289}
11290
11291{
11292std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11293vinfos[0].jointtype = 1;
11294vinfos[0].foffset = j0;
11295vinfos[0].indices[0] = _ij0[0];
11296vinfos[0].indices[1] = _ij0[1];
11297vinfos[0].maxsolutions = _nj0;
11298vinfos[1].jointtype = 1;
11299vinfos[1].foffset = j1;
11300vinfos[1].indices[0] = _ij1[0];
11301vinfos[1].indices[1] = _ij1[1];
11302vinfos[1].maxsolutions = _nj1;
11303vinfos[2].jointtype = 1;
11304vinfos[2].foffset = j2;
11305vinfos[2].indices[0] = _ij2[0];
11306vinfos[2].indices[1] = _ij2[1];
11307vinfos[2].maxsolutions = _nj2;
11308vinfos[3].jointtype = 1;
11309vinfos[3].foffset = j3;
11310vinfos[3].indices[0] = _ij3[0];
11311vinfos[3].indices[1] = _ij3[1];
11312vinfos[3].maxsolutions = _nj3;
11313vinfos[4].jointtype = 1;
11314vinfos[4].foffset = j4;
11315vinfos[4].indices[0] = _ij4[0];
11316vinfos[4].indices[1] = _ij4[1];
11317vinfos[4].maxsolutions = _nj4;
11318vinfos[5].jointtype = 1;
11319vinfos[5].foffset = j5;
11320vinfos[5].indices[0] = _ij5[0];
11321vinfos[5].indices[1] = _ij5[1];
11322vinfos[5].maxsolutions = _nj5;
11323std::vector<int> vfree(0);
11324solutions.AddSolution(vinfos,vfree);
11325}
11326}
11327}
11328
11329} else
11330{
11331{
11332IkReal j3array[2], cj3array[2], sj3array[2];
11333bool j3valid[2]={false};
11334_nj3 = 2;
11335CheckValue<IkReal> x867=IKPowWithIntegerCheck(new_r11,-1);
11336if(!x867.valid){
11337continue;
11338}
11339cj3array[0]=(gconst8*(x867.value));
11340if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
11341{
11342 j3valid[0] = j3valid[1] = true;
11343 j3array[0] = IKacos(cj3array[0]);
11344 sj3array[0] = IKsin(j3array[0]);
11345 cj3array[1] = cj3array[0];
11346 j3array[1] = -j3array[0];
11347 sj3array[1] = -sj3array[0];
11348}
11349else if( isnan(cj3array[0]) )
11350{
11351 // probably any value will work
11352 j3valid[0] = true;
11353 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
11354}
11355for(int ij3 = 0; ij3 < 2; ++ij3)
11356{
11357if( !j3valid[ij3] )
11358{
11359 continue;
11360}
11361_ij3[0] = ij3; _ij3[1] = -1;
11362for(int iij3 = ij3+1; iij3 < 2; ++iij3)
11363{
11364if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11365{
11366 j3valid[iij3]=false; _ij3[1] = iij3; break;
11367}
11368}
11369j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11370{
11371IkReal evalcond[6];
11372IkReal x868=IKsin(j3);
11373IkReal x869=IKcos(j3);
11374IkReal x870=(gconst8*x869);
11375evalcond[0]=(new_r00*x868);
11376evalcond[1]=(new_r11*x868);
11377evalcond[2]=(gconst8*x868);
11378evalcond[3]=(((new_r00*x869))+gconst8);
11379evalcond[4]=(new_r00+x870);
11380evalcond[5]=((((-1.0)*x870))+new_r11);
11381if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
11382{
11383continue;
11384}
11385}
11386
11387{
11388std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11389vinfos[0].jointtype = 1;
11390vinfos[0].foffset = j0;
11391vinfos[0].indices[0] = _ij0[0];
11392vinfos[0].indices[1] = _ij0[1];
11393vinfos[0].maxsolutions = _nj0;
11394vinfos[1].jointtype = 1;
11395vinfos[1].foffset = j1;
11396vinfos[1].indices[0] = _ij1[0];
11397vinfos[1].indices[1] = _ij1[1];
11398vinfos[1].maxsolutions = _nj1;
11399vinfos[2].jointtype = 1;
11400vinfos[2].foffset = j2;
11401vinfos[2].indices[0] = _ij2[0];
11402vinfos[2].indices[1] = _ij2[1];
11403vinfos[2].maxsolutions = _nj2;
11404vinfos[3].jointtype = 1;
11405vinfos[3].foffset = j3;
11406vinfos[3].indices[0] = _ij3[0];
11407vinfos[3].indices[1] = _ij3[1];
11408vinfos[3].maxsolutions = _nj3;
11409vinfos[4].jointtype = 1;
11410vinfos[4].foffset = j4;
11411vinfos[4].indices[0] = _ij4[0];
11412vinfos[4].indices[1] = _ij4[1];
11413vinfos[4].maxsolutions = _nj4;
11414vinfos[5].jointtype = 1;
11415vinfos[5].foffset = j5;
11416vinfos[5].indices[0] = _ij5[0];
11417vinfos[5].indices[1] = _ij5[1];
11418vinfos[5].maxsolutions = _nj5;
11419std::vector<int> vfree(0);
11420solutions.AddSolution(vinfos,vfree);
11421}
11422}
11423}
11424
11425}
11426
11427}
11428
11429}
11430} while(0);
11431if( bgotonextstatement )
11432{
11433bool bgotonextstatement = true;
11434do
11435{
11436evalcond[0]=IKabs(new_r00);
11437if( IKabs(evalcond[0]) < 0.0000050000000000 )
11438{
11439bgotonextstatement=false;
11440{
11441IkReal j3eval[1];
11442CheckValue<IkReal> x872 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11443if(!x872.valid){
11444continue;
11445}
11446IkReal x871=((-1.0)*(x872.value));
11447sj4=0;
11448cj4=-1.0;
11449j4=3.14159265358979;
11450sj5=gconst7;
11451cj5=gconst8;
11452j5=x871;
11453new_r00=0;
11454IkReal gconst6=x871;
11455IkReal x873 = new_r10*new_r10;
11456if(IKabs(x873)==0){
11457continue;
11458}
11459IkReal gconst7=((-1.0)*new_r10*(pow(x873,-0.5)));
11460IkReal gconst8=0;
11461j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
11462if( IKabs(j3eval[0]) < 0.0000010000000000 )
11463{
11464{
11465IkReal j3eval[1];
11466CheckValue<IkReal> x875 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11467if(!x875.valid){
11468continue;
11469}
11470IkReal x874=((-1.0)*(x875.value));
11471sj4=0;
11472cj4=-1.0;
11473j4=3.14159265358979;
11474sj5=gconst7;
11475cj5=gconst8;
11476j5=x874;
11477new_r00=0;
11478IkReal gconst6=x874;
11479IkReal x876 = new_r10*new_r10;
11480if(IKabs(x876)==0){
11481continue;
11482}
11483IkReal gconst7=((-1.0)*new_r10*(pow(x876,-0.5)));
11484IkReal gconst8=0;
11485j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
11486if( IKabs(j3eval[0]) < 0.0000010000000000 )
11487{
11488{
11489IkReal j3eval[1];
11490CheckValue<IkReal> x878 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
11491if(!x878.valid){
11492continue;
11493}
11494IkReal x877=((-1.0)*(x878.value));
11495sj4=0;
11496cj4=-1.0;
11497j4=3.14159265358979;
11498sj5=gconst7;
11499cj5=gconst8;
11500j5=x877;
11501new_r00=0;
11502IkReal gconst6=x877;
11503IkReal x879 = new_r10*new_r10;
11504if(IKabs(x879)==0){
11505continue;
11506}
11507IkReal gconst7=((-1.0)*new_r10*(pow(x879,-0.5)));
11508IkReal gconst8=0;
11509j3eval[0]=new_r10;
11510if( IKabs(j3eval[0]) < 0.0000010000000000 )
11511{
11512continue; // 3 cases reached
11513
11514} else
11515{
11516{
11517IkReal j3array[1], cj3array[1], sj3array[1];
11518bool j3valid[1]={false};
11519_nj3 = 1;
11520CheckValue<IkReal> x880=IKPowWithIntegerCheck(gconst7,-1);
11521if(!x880.valid){
11522continue;
11523}
11524CheckValue<IkReal> x881=IKPowWithIntegerCheck(new_r10,-1);
11525if(!x881.valid){
11526continue;
11527}
11528if( IKabs((new_r11*(x880.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst7*(x881.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r11*(x880.value)))+IKsqr((gconst7*(x881.value)))-1) <= IKFAST_SINCOS_THRESH )
11529 continue;
11530j3array[0]=IKatan2((new_r11*(x880.value)), (gconst7*(x881.value)));
11531sj3array[0]=IKsin(j3array[0]);
11532cj3array[0]=IKcos(j3array[0]);
11533if( j3array[0] > IKPI )
11534{
11535 j3array[0]-=IK2PI;
11536}
11537else if( j3array[0] < -IKPI )
11538{ j3array[0]+=IK2PI;
11539}
11540j3valid[0] = true;
11541for(int ij3 = 0; ij3 < 1; ++ij3)
11542{
11543if( !j3valid[ij3] )
11544{
11545 continue;
11546}
11547_ij3[0] = ij3; _ij3[1] = -1;
11548for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11549{
11550if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11551{
11552 j3valid[iij3]=false; _ij3[1] = iij3; break;
11553}
11554}
11555j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11556{
11557IkReal evalcond[8];
11558IkReal x882=IKsin(j3);
11559IkReal x883=IKcos(j3);
11560IkReal x884=((1.0)*gconst7);
11561IkReal x885=((1.0)*x883);
11562IkReal x886=(x883*x884);
11563evalcond[0]=(new_r10*x882);
11564evalcond[1]=(gconst7*x882);
11565evalcond[2]=((((-1.0)*new_r10*x885))+gconst7);
11566evalcond[3]=((((-1.0)*x886))+new_r01);
11567evalcond[4]=((((-1.0)*x882*x884))+new_r11);
11568evalcond[5]=((((-1.0)*x886))+new_r10);
11569evalcond[6]=(((new_r01*x882))+(((-1.0)*new_r11*x885)));
11570evalcond[7]=(((new_r01*x883))+((new_r11*x882))+(((-1.0)*x884)));
11571if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11572{
11573continue;
11574}
11575}
11576
11577{
11578std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11579vinfos[0].jointtype = 1;
11580vinfos[0].foffset = j0;
11581vinfos[0].indices[0] = _ij0[0];
11582vinfos[0].indices[1] = _ij0[1];
11583vinfos[0].maxsolutions = _nj0;
11584vinfos[1].jointtype = 1;
11585vinfos[1].foffset = j1;
11586vinfos[1].indices[0] = _ij1[0];
11587vinfos[1].indices[1] = _ij1[1];
11588vinfos[1].maxsolutions = _nj1;
11589vinfos[2].jointtype = 1;
11590vinfos[2].foffset = j2;
11591vinfos[2].indices[0] = _ij2[0];
11592vinfos[2].indices[1] = _ij2[1];
11593vinfos[2].maxsolutions = _nj2;
11594vinfos[3].jointtype = 1;
11595vinfos[3].foffset = j3;
11596vinfos[3].indices[0] = _ij3[0];
11597vinfos[3].indices[1] = _ij3[1];
11598vinfos[3].maxsolutions = _nj3;
11599vinfos[4].jointtype = 1;
11600vinfos[4].foffset = j4;
11601vinfos[4].indices[0] = _ij4[0];
11602vinfos[4].indices[1] = _ij4[1];
11603vinfos[4].maxsolutions = _nj4;
11604vinfos[5].jointtype = 1;
11605vinfos[5].foffset = j5;
11606vinfos[5].indices[0] = _ij5[0];
11607vinfos[5].indices[1] = _ij5[1];
11608vinfos[5].maxsolutions = _nj5;
11609std::vector<int> vfree(0);
11610solutions.AddSolution(vinfos,vfree);
11611}
11612}
11613}
11614
11615}
11616
11617}
11618
11619} else
11620{
11621{
11622IkReal j3array[1], cj3array[1], sj3array[1];
11623bool j3valid[1]={false};
11624_nj3 = 1;
11625CheckValue<IkReal> x887=IKPowWithIntegerCheck(IKsign(gconst7),-1);
11626if(!x887.valid){
11627continue;
11628}
11629CheckValue<IkReal> x888 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
11630if(!x888.valid){
11631continue;
11632}
11633j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x887.value)))+(x888.value));
11634sj3array[0]=IKsin(j3array[0]);
11635cj3array[0]=IKcos(j3array[0]);
11636if( j3array[0] > IKPI )
11637{
11638 j3array[0]-=IK2PI;
11639}
11640else if( j3array[0] < -IKPI )
11641{ j3array[0]+=IK2PI;
11642}
11643j3valid[0] = true;
11644for(int ij3 = 0; ij3 < 1; ++ij3)
11645{
11646if( !j3valid[ij3] )
11647{
11648 continue;
11649}
11650_ij3[0] = ij3; _ij3[1] = -1;
11651for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11652{
11653if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11654{
11655 j3valid[iij3]=false; _ij3[1] = iij3; break;
11656}
11657}
11658j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11659{
11660IkReal evalcond[8];
11661IkReal x889=IKsin(j3);
11662IkReal x890=IKcos(j3);
11663IkReal x891=((1.0)*gconst7);
11664IkReal x892=((1.0)*x890);
11665IkReal x893=(x890*x891);
11666evalcond[0]=(new_r10*x889);
11667evalcond[1]=(gconst7*x889);
11668evalcond[2]=(gconst7+(((-1.0)*new_r10*x892)));
11669evalcond[3]=((((-1.0)*x893))+new_r01);
11670evalcond[4]=((((-1.0)*x889*x891))+new_r11);
11671evalcond[5]=((((-1.0)*x893))+new_r10);
11672evalcond[6]=(((new_r01*x889))+(((-1.0)*new_r11*x892)));
11673evalcond[7]=(((new_r11*x889))+((new_r01*x890))+(((-1.0)*x891)));
11674if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11675{
11676continue;
11677}
11678}
11679
11680{
11681std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11682vinfos[0].jointtype = 1;
11683vinfos[0].foffset = j0;
11684vinfos[0].indices[0] = _ij0[0];
11685vinfos[0].indices[1] = _ij0[1];
11686vinfos[0].maxsolutions = _nj0;
11687vinfos[1].jointtype = 1;
11688vinfos[1].foffset = j1;
11689vinfos[1].indices[0] = _ij1[0];
11690vinfos[1].indices[1] = _ij1[1];
11691vinfos[1].maxsolutions = _nj1;
11692vinfos[2].jointtype = 1;
11693vinfos[2].foffset = j2;
11694vinfos[2].indices[0] = _ij2[0];
11695vinfos[2].indices[1] = _ij2[1];
11696vinfos[2].maxsolutions = _nj2;
11697vinfos[3].jointtype = 1;
11698vinfos[3].foffset = j3;
11699vinfos[3].indices[0] = _ij3[0];
11700vinfos[3].indices[1] = _ij3[1];
11701vinfos[3].maxsolutions = _nj3;
11702vinfos[4].jointtype = 1;
11703vinfos[4].foffset = j4;
11704vinfos[4].indices[0] = _ij4[0];
11705vinfos[4].indices[1] = _ij4[1];
11706vinfos[4].maxsolutions = _nj4;
11707vinfos[5].jointtype = 1;
11708vinfos[5].foffset = j5;
11709vinfos[5].indices[0] = _ij5[0];
11710vinfos[5].indices[1] = _ij5[1];
11711vinfos[5].maxsolutions = _nj5;
11712std::vector<int> vfree(0);
11713solutions.AddSolution(vinfos,vfree);
11714}
11715}
11716}
11717
11718}
11719
11720}
11721
11722} else
11723{
11724{
11725IkReal j3array[1], cj3array[1], sj3array[1];
11726bool j3valid[1]={false};
11727_nj3 = 1;
11728CheckValue<IkReal> x894=IKPowWithIntegerCheck(IKsign(gconst7),-1);
11729if(!x894.valid){
11730continue;
11731}
11732CheckValue<IkReal> x895 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
11733if(!x895.valid){
11734continue;
11735}
11736j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x894.value)))+(x895.value));
11737sj3array[0]=IKsin(j3array[0]);
11738cj3array[0]=IKcos(j3array[0]);
11739if( j3array[0] > IKPI )
11740{
11741 j3array[0]-=IK2PI;
11742}
11743else if( j3array[0] < -IKPI )
11744{ j3array[0]+=IK2PI;
11745}
11746j3valid[0] = true;
11747for(int ij3 = 0; ij3 < 1; ++ij3)
11748{
11749if( !j3valid[ij3] )
11750{
11751 continue;
11752}
11753_ij3[0] = ij3; _ij3[1] = -1;
11754for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11755{
11756if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11757{
11758 j3valid[iij3]=false; _ij3[1] = iij3; break;
11759}
11760}
11761j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11762{
11763IkReal evalcond[8];
11764IkReal x896=IKsin(j3);
11765IkReal x897=IKcos(j3);
11766IkReal x898=((1.0)*gconst7);
11767IkReal x899=((1.0)*x897);
11768IkReal x900=(x897*x898);
11769evalcond[0]=(new_r10*x896);
11770evalcond[1]=(gconst7*x896);
11771evalcond[2]=(gconst7+(((-1.0)*new_r10*x899)));
11772evalcond[3]=((((-1.0)*x900))+new_r01);
11773evalcond[4]=((((-1.0)*x896*x898))+new_r11);
11774evalcond[5]=((((-1.0)*x900))+new_r10);
11775evalcond[6]=(((new_r01*x896))+(((-1.0)*new_r11*x899)));
11776evalcond[7]=(((new_r11*x896))+((new_r01*x897))+(((-1.0)*x898)));
11777if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11778{
11779continue;
11780}
11781}
11782
11783{
11784std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11785vinfos[0].jointtype = 1;
11786vinfos[0].foffset = j0;
11787vinfos[0].indices[0] = _ij0[0];
11788vinfos[0].indices[1] = _ij0[1];
11789vinfos[0].maxsolutions = _nj0;
11790vinfos[1].jointtype = 1;
11791vinfos[1].foffset = j1;
11792vinfos[1].indices[0] = _ij1[0];
11793vinfos[1].indices[1] = _ij1[1];
11794vinfos[1].maxsolutions = _nj1;
11795vinfos[2].jointtype = 1;
11796vinfos[2].foffset = j2;
11797vinfos[2].indices[0] = _ij2[0];
11798vinfos[2].indices[1] = _ij2[1];
11799vinfos[2].maxsolutions = _nj2;
11800vinfos[3].jointtype = 1;
11801vinfos[3].foffset = j3;
11802vinfos[3].indices[0] = _ij3[0];
11803vinfos[3].indices[1] = _ij3[1];
11804vinfos[3].maxsolutions = _nj3;
11805vinfos[4].jointtype = 1;
11806vinfos[4].foffset = j4;
11807vinfos[4].indices[0] = _ij4[0];
11808vinfos[4].indices[1] = _ij4[1];
11809vinfos[4].maxsolutions = _nj4;
11810vinfos[5].jointtype = 1;
11811vinfos[5].foffset = j5;
11812vinfos[5].indices[0] = _ij5[0];
11813vinfos[5].indices[1] = _ij5[1];
11814vinfos[5].maxsolutions = _nj5;
11815std::vector<int> vfree(0);
11816solutions.AddSolution(vinfos,vfree);
11817}
11818}
11819}
11820
11821}
11822
11823}
11824
11825}
11826} while(0);
11827if( bgotonextstatement )
11828{
11829bool bgotonextstatement = true;
11830do
11831{
11832if( 1 )
11833{
11834bgotonextstatement=false;
11835continue; // branch miss [j3]
11836
11837}
11838} while(0);
11839if( bgotonextstatement )
11840{
11841}
11842}
11843}
11844}
11845}
11846}
11847
11848} else
11849{
11850{
11851IkReal j3array[1], cj3array[1], sj3array[1];
11852bool j3valid[1]={false};
11853_nj3 = 1;
11854CheckValue<IkReal> x901=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*new_r10))+(((-1.0)*gconst8*new_r00)))),-1);
11855if(!x901.valid){
11856continue;
11857}
11858CheckValue<IkReal> x902 = IKatan2WithCheck(IkReal((((new_r00*new_r10))+((gconst7*gconst8)))),IkReal(((gconst8*gconst8)+(((-1.0)*(new_r10*new_r10))))),IKFAST_ATAN2_MAGTHRESH);
11859if(!x902.valid){
11860continue;
11861}
11862j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x901.value)))+(x902.value));
11863sj3array[0]=IKsin(j3array[0]);
11864cj3array[0]=IKcos(j3array[0]);
11865if( j3array[0] > IKPI )
11866{
11867 j3array[0]-=IK2PI;
11868}
11869else if( j3array[0] < -IKPI )
11870{ j3array[0]+=IK2PI;
11871}
11872j3valid[0] = true;
11873for(int ij3 = 0; ij3 < 1; ++ij3)
11874{
11875if( !j3valid[ij3] )
11876{
11877 continue;
11878}
11879_ij3[0] = ij3; _ij3[1] = -1;
11880for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11881{
11882if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11883{
11884 j3valid[iij3]=false; _ij3[1] = iij3; break;
11885}
11886}
11887j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11888{
11889IkReal evalcond[8];
11890IkReal x903=IKcos(j3);
11891IkReal x904=IKsin(j3);
11892IkReal x905=(gconst8*x904);
11893IkReal x906=(gconst7*x904);
11894IkReal x907=((1.0)*x903);
11895IkReal x908=(gconst7*x907);
11896evalcond[0]=(gconst8+((new_r10*x904))+((new_r00*x903)));
11897evalcond[1]=(new_r00+((gconst8*x903))+x906);
11898evalcond[2]=(gconst7+((new_r00*x904))+(((-1.0)*new_r10*x907)));
11899evalcond[3]=((((-1.0)*new_r11*x907))+gconst8+((new_r01*x904)));
11900evalcond[4]=((((-1.0)*x908))+new_r01+x905);
11901evalcond[5]=((((-1.0)*x908))+new_r10+x905);
11902evalcond[6]=((((-1.0)*gconst7))+((new_r11*x904))+((new_r01*x903)));
11903evalcond[7]=((((-1.0)*gconst8*x907))+(((-1.0)*x906))+new_r11);
11904if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
11905{
11906continue;
11907}
11908}
11909
11910{
11911std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11912vinfos[0].jointtype = 1;
11913vinfos[0].foffset = j0;
11914vinfos[0].indices[0] = _ij0[0];
11915vinfos[0].indices[1] = _ij0[1];
11916vinfos[0].maxsolutions = _nj0;
11917vinfos[1].jointtype = 1;
11918vinfos[1].foffset = j1;
11919vinfos[1].indices[0] = _ij1[0];
11920vinfos[1].indices[1] = _ij1[1];
11921vinfos[1].maxsolutions = _nj1;
11922vinfos[2].jointtype = 1;
11923vinfos[2].foffset = j2;
11924vinfos[2].indices[0] = _ij2[0];
11925vinfos[2].indices[1] = _ij2[1];
11926vinfos[2].maxsolutions = _nj2;
11927vinfos[3].jointtype = 1;
11928vinfos[3].foffset = j3;
11929vinfos[3].indices[0] = _ij3[0];
11930vinfos[3].indices[1] = _ij3[1];
11931vinfos[3].maxsolutions = _nj3;
11932vinfos[4].jointtype = 1;
11933vinfos[4].foffset = j4;
11934vinfos[4].indices[0] = _ij4[0];
11935vinfos[4].indices[1] = _ij4[1];
11936vinfos[4].maxsolutions = _nj4;
11937vinfos[5].jointtype = 1;
11938vinfos[5].foffset = j5;
11939vinfos[5].indices[0] = _ij5[0];
11940vinfos[5].indices[1] = _ij5[1];
11941vinfos[5].maxsolutions = _nj5;
11942std::vector<int> vfree(0);
11943solutions.AddSolution(vinfos,vfree);
11944}
11945}
11946}
11947
11948}
11949
11950}
11951
11952} else
11953{
11954{
11955IkReal j3array[1], cj3array[1], sj3array[1];
11956bool j3valid[1]={false};
11957_nj3 = 1;
11958IkReal x909=((1.0)*new_r10);
11959CheckValue<IkReal> x910=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst7*x909))+(((-1.0)*gconst8*new_r00)))),-1);
11960if(!x910.valid){
11961continue;
11962}
11963CheckValue<IkReal> x911 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((gconst7*gconst8)))),IkReal(((gconst8*gconst8)+(((-1.0)*new_r01*x909)))),IKFAST_ATAN2_MAGTHRESH);
11964if(!x911.valid){
11965continue;
11966}
11967j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x910.value)))+(x911.value));
11968sj3array[0]=IKsin(j3array[0]);
11969cj3array[0]=IKcos(j3array[0]);
11970if( j3array[0] > IKPI )
11971{
11972 j3array[0]-=IK2PI;
11973}
11974else if( j3array[0] < -IKPI )
11975{ j3array[0]+=IK2PI;
11976}
11977j3valid[0] = true;
11978for(int ij3 = 0; ij3 < 1; ++ij3)
11979{
11980if( !j3valid[ij3] )
11981{
11982 continue;
11983}
11984_ij3[0] = ij3; _ij3[1] = -1;
11985for(int iij3 = ij3+1; iij3 < 1; ++iij3)
11986{
11987if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
11988{
11989 j3valid[iij3]=false; _ij3[1] = iij3; break;
11990}
11991}
11992j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
11993{
11994IkReal evalcond[8];
11995IkReal x912=IKcos(j3);
11996IkReal x913=IKsin(j3);
11997IkReal x914=(gconst8*x913);
11998IkReal x915=(gconst7*x913);
11999IkReal x916=((1.0)*x912);
12000IkReal x917=(gconst7*x916);
12001evalcond[0]=(gconst8+((new_r10*x913))+((new_r00*x912)));
12002evalcond[1]=(((gconst8*x912))+new_r00+x915);
12003evalcond[2]=(gconst7+((new_r00*x913))+(((-1.0)*new_r10*x916)));
12004evalcond[3]=(gconst8+((new_r01*x913))+(((-1.0)*new_r11*x916)));
12005evalcond[4]=((((-1.0)*x917))+new_r01+x914);
12006evalcond[5]=((((-1.0)*x917))+new_r10+x914);
12007evalcond[6]=(((new_r11*x913))+(((-1.0)*gconst7))+((new_r01*x912)));
12008evalcond[7]=((((-1.0)*gconst8*x916))+(((-1.0)*x915))+new_r11);
12009if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
12010{
12011continue;
12012}
12013}
12014
12015{
12016std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12017vinfos[0].jointtype = 1;
12018vinfos[0].foffset = j0;
12019vinfos[0].indices[0] = _ij0[0];
12020vinfos[0].indices[1] = _ij0[1];
12021vinfos[0].maxsolutions = _nj0;
12022vinfos[1].jointtype = 1;
12023vinfos[1].foffset = j1;
12024vinfos[1].indices[0] = _ij1[0];
12025vinfos[1].indices[1] = _ij1[1];
12026vinfos[1].maxsolutions = _nj1;
12027vinfos[2].jointtype = 1;
12028vinfos[2].foffset = j2;
12029vinfos[2].indices[0] = _ij2[0];
12030vinfos[2].indices[1] = _ij2[1];
12031vinfos[2].maxsolutions = _nj2;
12032vinfos[3].jointtype = 1;
12033vinfos[3].foffset = j3;
12034vinfos[3].indices[0] = _ij3[0];
12035vinfos[3].indices[1] = _ij3[1];
12036vinfos[3].maxsolutions = _nj3;
12037vinfos[4].jointtype = 1;
12038vinfos[4].foffset = j4;
12039vinfos[4].indices[0] = _ij4[0];
12040vinfos[4].indices[1] = _ij4[1];
12041vinfos[4].maxsolutions = _nj4;
12042vinfos[5].jointtype = 1;
12043vinfos[5].foffset = j5;
12044vinfos[5].indices[0] = _ij5[0];
12045vinfos[5].indices[1] = _ij5[1];
12046vinfos[5].maxsolutions = _nj5;
12047std::vector<int> vfree(0);
12048solutions.AddSolution(vinfos,vfree);
12049}
12050}
12051}
12052
12053}
12054
12055}
12056
12057} else
12058{
12059{
12060IkReal j3array[1], cj3array[1], sj3array[1];
12061bool j3valid[1]={false};
12062_nj3 = 1;
12063IkReal x918=((1.0)*new_r10);
12064CheckValue<IkReal> x919 = IKatan2WithCheck(IkReal((((gconst8*new_r11))+((gconst8*new_r00)))),IkReal((((gconst8*new_r01))+(((-1.0)*gconst8*x918)))),IKFAST_ATAN2_MAGTHRESH);
12065if(!x919.valid){
12066continue;
12067}
12068CheckValue<IkReal> x920=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x918)))),-1);
12069if(!x920.valid){
12070continue;
12071}
12072j3array[0]=((-1.5707963267949)+(x919.value)+(((1.5707963267949)*(x920.value))));
12073sj3array[0]=IKsin(j3array[0]);
12074cj3array[0]=IKcos(j3array[0]);
12075if( j3array[0] > IKPI )
12076{
12077 j3array[0]-=IK2PI;
12078}
12079else if( j3array[0] < -IKPI )
12080{ j3array[0]+=IK2PI;
12081}
12082j3valid[0] = true;
12083for(int ij3 = 0; ij3 < 1; ++ij3)
12084{
12085if( !j3valid[ij3] )
12086{
12087 continue;
12088}
12089_ij3[0] = ij3; _ij3[1] = -1;
12090for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12091{
12092if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12093{
12094 j3valid[iij3]=false; _ij3[1] = iij3; break;
12095}
12096}
12097j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12098{
12099IkReal evalcond[8];
12100IkReal x921=IKcos(j3);
12101IkReal x922=IKsin(j3);
12102IkReal x923=(gconst8*x922);
12103IkReal x924=(gconst7*x922);
12104IkReal x925=((1.0)*x921);
12105IkReal x926=(gconst7*x925);
12106evalcond[0]=(gconst8+((new_r00*x921))+((new_r10*x922)));
12107evalcond[1]=(((gconst8*x921))+new_r00+x924);
12108evalcond[2]=(gconst7+(((-1.0)*new_r10*x925))+((new_r00*x922)));
12109evalcond[3]=((((-1.0)*new_r11*x925))+gconst8+((new_r01*x922)));
12110evalcond[4]=((((-1.0)*x926))+new_r01+x923);
12111evalcond[5]=((((-1.0)*x926))+new_r10+x923);
12112evalcond[6]=((((-1.0)*gconst7))+((new_r01*x921))+((new_r11*x922)));
12113evalcond[7]=((((-1.0)*gconst8*x925))+(((-1.0)*x924))+new_r11);
12114if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
12115{
12116continue;
12117}
12118}
12119
12120{
12121std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12122vinfos[0].jointtype = 1;
12123vinfos[0].foffset = j0;
12124vinfos[0].indices[0] = _ij0[0];
12125vinfos[0].indices[1] = _ij0[1];
12126vinfos[0].maxsolutions = _nj0;
12127vinfos[1].jointtype = 1;
12128vinfos[1].foffset = j1;
12129vinfos[1].indices[0] = _ij1[0];
12130vinfos[1].indices[1] = _ij1[1];
12131vinfos[1].maxsolutions = _nj1;
12132vinfos[2].jointtype = 1;
12133vinfos[2].foffset = j2;
12134vinfos[2].indices[0] = _ij2[0];
12135vinfos[2].indices[1] = _ij2[1];
12136vinfos[2].maxsolutions = _nj2;
12137vinfos[3].jointtype = 1;
12138vinfos[3].foffset = j3;
12139vinfos[3].indices[0] = _ij3[0];
12140vinfos[3].indices[1] = _ij3[1];
12141vinfos[3].maxsolutions = _nj3;
12142vinfos[4].jointtype = 1;
12143vinfos[4].foffset = j4;
12144vinfos[4].indices[0] = _ij4[0];
12145vinfos[4].indices[1] = _ij4[1];
12146vinfos[4].maxsolutions = _nj4;
12147vinfos[5].jointtype = 1;
12148vinfos[5].foffset = j5;
12149vinfos[5].indices[0] = _ij5[0];
12150vinfos[5].indices[1] = _ij5[1];
12151vinfos[5].maxsolutions = _nj5;
12152std::vector<int> vfree(0);
12153solutions.AddSolution(vinfos,vfree);
12154}
12155}
12156}
12157
12158}
12159
12160}
12161
12162}
12163} while(0);
12164if( bgotonextstatement )
12165{
12166bool bgotonextstatement = true;
12167do
12168{
12169IkReal x929 = ((new_r10*new_r10)+(new_r00*new_r00));
12170if(IKabs(x929)==0){
12171continue;
12172}
12173IkReal x927=pow(x929,-0.5);
12174IkReal x928=((1.0)*x927);
12175CheckValue<IkReal> x930 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12176if(!x930.valid){
12177continue;
12178}
12179IkReal gconst9=((3.14159265358979)+(((-1.0)*(x930.value))));
12180IkReal gconst10=(new_r10*x928);
12181IkReal gconst11=(new_r00*x928);
12182CheckValue<IkReal> x931 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12183if(!x931.valid){
12184continue;
12185}
12186evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+j5+(x931.value))))), 6.28318530717959)));
12187if( IKabs(evalcond[0]) < 0.0000050000000000 )
12188{
12189bgotonextstatement=false;
12190{
12191IkReal j3eval[3];
12192CheckValue<IkReal> x935 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12193if(!x935.valid){
12194continue;
12195}
12196IkReal x932=((1.0)*(x935.value));
12197IkReal x933=x927;
12198IkReal x934=((1.0)*x933);
12199sj4=0;
12200cj4=-1.0;
12201j4=3.14159265358979;
12202sj5=gconst10;
12203cj5=gconst11;
12204j5=((3.14159265)+(((-1.0)*x932)));
12205IkReal gconst9=((3.14159265358979)+(((-1.0)*x932)));
12206IkReal gconst10=(new_r10*x934);
12207IkReal gconst11=(new_r00*x934);
12208IkReal x936=new_r00*new_r00;
12209IkReal x937=((1.0)*new_r00);
12210IkReal x938=((((-1.0)*new_r01*x937))+(((-1.0)*new_r10*new_r11)));
12211IkReal x939=x927;
12212IkReal x940=(new_r00*x939);
12213j3eval[0]=x938;
12214j3eval[1]=((IKabs((((new_r01*x940))+(((-1.0)*new_r10*x937*x939)))))+(IKabs((((x936*x939))+((new_r11*x940))))));
12215j3eval[2]=IKsign(x938);
12216if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12217{
12218{
12219IkReal j3eval[1];
12220CheckValue<IkReal> x944 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12221if(!x944.valid){
12222continue;
12223}
12224IkReal x941=((1.0)*(x944.value));
12225IkReal x942=x927;
12226IkReal x943=((1.0)*x942);
12227sj4=0;
12228cj4=-1.0;
12229j4=3.14159265358979;
12230sj5=gconst10;
12231cj5=gconst11;
12232j5=((3.14159265)+(((-1.0)*x941)));
12233IkReal gconst9=((3.14159265358979)+(((-1.0)*x941)));
12234IkReal gconst10=(new_r10*x943);
12235IkReal gconst11=(new_r00*x943);
12236IkReal x945=new_r10*new_r10;
12237IkReal x946=new_r00*new_r00;
12238IkReal x947=((1.0)*new_r01);
12239CheckValue<IkReal> x951=IKPowWithIntegerCheck((x945+x946),-1);
12240if(!x951.valid){
12241continue;
12242}
12243IkReal x948=x951.value;
12244IkReal x949=(new_r10*x948);
12245IkReal x950=(new_r01*x948);
12246j3eval[0]=((IKabs((((new_r00*x949))+((new_r00*x945*x950))+((x950*(new_r00*new_r00*new_r00))))))+(IKabs((((x946*x948))+(((-1.0)*x947*x949*(new_r10*new_r10)))+(((-1.0)*x946*x947*x949))))));
12247if( IKabs(j3eval[0]) < 0.0000010000000000 )
12248{
12249{
12250IkReal j3eval[1];
12251CheckValue<IkReal> x955 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12252if(!x955.valid){
12253continue;
12254}
12255IkReal x952=((1.0)*(x955.value));
12256IkReal x953=x927;
12257IkReal x954=((1.0)*x953);
12258sj4=0;
12259cj4=-1.0;
12260j4=3.14159265358979;
12261sj5=gconst10;
12262cj5=gconst11;
12263j5=((3.14159265)+(((-1.0)*x952)));
12264IkReal gconst9=((3.14159265358979)+(((-1.0)*x952)));
12265IkReal gconst10=(new_r10*x954);
12266IkReal gconst11=(new_r00*x954);
12267IkReal x956=new_r00*new_r00;
12268IkReal x957=new_r10*new_r10;
12269CheckValue<IkReal> x961=IKPowWithIntegerCheck((x957+x956),-1);
12270if(!x961.valid){
12271continue;
12272}
12273IkReal x958=x961.value;
12274IkReal x959=(new_r10*x958);
12275IkReal x960=((1.0)*x958);
12276j3eval[0]=((IKabs(((((-1.0)*x956*x957*x960))+((x956*x958))+(((-1.0)*x960*(x957*x957))))))+(IKabs((((new_r00*x959))+((x959*(new_r00*new_r00*new_r00)))+((new_r00*x959*(new_r10*new_r10)))))));
12277if( IKabs(j3eval[0]) < 0.0000010000000000 )
12278{
12279{
12280IkReal evalcond[2];
12281bool bgotonextstatement = true;
12282do
12283{
12284evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
12285if( IKabs(evalcond[0]) < 0.0000050000000000 )
12286{
12287bgotonextstatement=false;
12288{
12289IkReal j3eval[1];
12290CheckValue<IkReal> x963 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
12291if(!x963.valid){
12292continue;
12293}
12294IkReal x962=((1.0)*(x963.value));
12295sj4=0;
12296cj4=-1.0;
12297j4=3.14159265358979;
12298sj5=gconst10;
12299cj5=gconst11;
12300j5=((3.14159265)+(((-1.0)*x962)));
12301new_r11=0;
12302new_r00=0;
12303IkReal gconst9=((3.14159265358979)+(((-1.0)*x962)));
12304IkReal x964 = new_r10*new_r10;
12305if(IKabs(x964)==0){
12306continue;
12307}
12308IkReal gconst10=((1.0)*new_r10*(pow(x964,-0.5)));
12309IkReal gconst11=0;
12310j3eval[0]=new_r10;
12311if( IKabs(j3eval[0]) < 0.0000010000000000 )
12312{
12313{
12314IkReal j3array[2], cj3array[2], sj3array[2];
12315bool j3valid[2]={false};
12316_nj3 = 2;
12317CheckValue<IkReal> x965=IKPowWithIntegerCheck(gconst10,-1);
12318if(!x965.valid){
12319continue;
12320}
12321cj3array[0]=(new_r01*(x965.value));
12322if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12323{
12324 j3valid[0] = j3valid[1] = true;
12325 j3array[0] = IKacos(cj3array[0]);
12326 sj3array[0] = IKsin(j3array[0]);
12327 cj3array[1] = cj3array[0];
12328 j3array[1] = -j3array[0];
12329 sj3array[1] = -sj3array[0];
12330}
12331else if( isnan(cj3array[0]) )
12332{
12333 // probably any value will work
12334 j3valid[0] = true;
12335 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12336}
12337for(int ij3 = 0; ij3 < 2; ++ij3)
12338{
12339if( !j3valid[ij3] )
12340{
12341 continue;
12342}
12343_ij3[0] = ij3; _ij3[1] = -1;
12344for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12345{
12346if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12347{
12348 j3valid[iij3]=false; _ij3[1] = iij3; break;
12349}
12350}
12351j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12352{
12353IkReal evalcond[6];
12354IkReal x966=IKsin(j3);
12355IkReal x967=IKcos(j3);
12356IkReal x968=((1.0)*gconst10);
12357evalcond[0]=(new_r01*x966);
12358evalcond[1]=(new_r10*x966);
12359evalcond[2]=(gconst10*x966);
12360evalcond[3]=(gconst10+(((-1.0)*new_r10*x967)));
12361evalcond[4]=((((-1.0)*x967*x968))+new_r10);
12362evalcond[5]=(((new_r01*x967))+(((-1.0)*x968)));
12363if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12364{
12365continue;
12366}
12367}
12368
12369{
12370std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12371vinfos[0].jointtype = 1;
12372vinfos[0].foffset = j0;
12373vinfos[0].indices[0] = _ij0[0];
12374vinfos[0].indices[1] = _ij0[1];
12375vinfos[0].maxsolutions = _nj0;
12376vinfos[1].jointtype = 1;
12377vinfos[1].foffset = j1;
12378vinfos[1].indices[0] = _ij1[0];
12379vinfos[1].indices[1] = _ij1[1];
12380vinfos[1].maxsolutions = _nj1;
12381vinfos[2].jointtype = 1;
12382vinfos[2].foffset = j2;
12383vinfos[2].indices[0] = _ij2[0];
12384vinfos[2].indices[1] = _ij2[1];
12385vinfos[2].maxsolutions = _nj2;
12386vinfos[3].jointtype = 1;
12387vinfos[3].foffset = j3;
12388vinfos[3].indices[0] = _ij3[0];
12389vinfos[3].indices[1] = _ij3[1];
12390vinfos[3].maxsolutions = _nj3;
12391vinfos[4].jointtype = 1;
12392vinfos[4].foffset = j4;
12393vinfos[4].indices[0] = _ij4[0];
12394vinfos[4].indices[1] = _ij4[1];
12395vinfos[4].maxsolutions = _nj4;
12396vinfos[5].jointtype = 1;
12397vinfos[5].foffset = j5;
12398vinfos[5].indices[0] = _ij5[0];
12399vinfos[5].indices[1] = _ij5[1];
12400vinfos[5].maxsolutions = _nj5;
12401std::vector<int> vfree(0);
12402solutions.AddSolution(vinfos,vfree);
12403}
12404}
12405}
12406
12407} else
12408{
12409{
12410IkReal j3array[2], cj3array[2], sj3array[2];
12411bool j3valid[2]={false};
12412_nj3 = 2;
12413CheckValue<IkReal> x969=IKPowWithIntegerCheck(new_r10,-1);
12414if(!x969.valid){
12415continue;
12416}
12417cj3array[0]=(gconst10*(x969.value));
12418if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12419{
12420 j3valid[0] = j3valid[1] = true;
12421 j3array[0] = IKacos(cj3array[0]);
12422 sj3array[0] = IKsin(j3array[0]);
12423 cj3array[1] = cj3array[0];
12424 j3array[1] = -j3array[0];
12425 sj3array[1] = -sj3array[0];
12426}
12427else if( isnan(cj3array[0]) )
12428{
12429 // probably any value will work
12430 j3valid[0] = true;
12431 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12432}
12433for(int ij3 = 0; ij3 < 2; ++ij3)
12434{
12435if( !j3valid[ij3] )
12436{
12437 continue;
12438}
12439_ij3[0] = ij3; _ij3[1] = -1;
12440for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12441{
12442if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12443{
12444 j3valid[iij3]=false; _ij3[1] = iij3; break;
12445}
12446}
12447j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12448{
12449IkReal evalcond[6];
12450IkReal x970=IKsin(j3);
12451IkReal x971=IKcos(j3);
12452IkReal x972=((1.0)*gconst10);
12453IkReal x973=(x971*x972);
12454evalcond[0]=(new_r01*x970);
12455evalcond[1]=(new_r10*x970);
12456evalcond[2]=(gconst10*x970);
12457evalcond[3]=(new_r01+(((-1.0)*x973)));
12458evalcond[4]=(new_r10+(((-1.0)*x973)));
12459evalcond[5]=(((new_r01*x971))+(((-1.0)*x972)));
12460if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12461{
12462continue;
12463}
12464}
12465
12466{
12467std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12468vinfos[0].jointtype = 1;
12469vinfos[0].foffset = j0;
12470vinfos[0].indices[0] = _ij0[0];
12471vinfos[0].indices[1] = _ij0[1];
12472vinfos[0].maxsolutions = _nj0;
12473vinfos[1].jointtype = 1;
12474vinfos[1].foffset = j1;
12475vinfos[1].indices[0] = _ij1[0];
12476vinfos[1].indices[1] = _ij1[1];
12477vinfos[1].maxsolutions = _nj1;
12478vinfos[2].jointtype = 1;
12479vinfos[2].foffset = j2;
12480vinfos[2].indices[0] = _ij2[0];
12481vinfos[2].indices[1] = _ij2[1];
12482vinfos[2].maxsolutions = _nj2;
12483vinfos[3].jointtype = 1;
12484vinfos[3].foffset = j3;
12485vinfos[3].indices[0] = _ij3[0];
12486vinfos[3].indices[1] = _ij3[1];
12487vinfos[3].maxsolutions = _nj3;
12488vinfos[4].jointtype = 1;
12489vinfos[4].foffset = j4;
12490vinfos[4].indices[0] = _ij4[0];
12491vinfos[4].indices[1] = _ij4[1];
12492vinfos[4].maxsolutions = _nj4;
12493vinfos[5].jointtype = 1;
12494vinfos[5].foffset = j5;
12495vinfos[5].indices[0] = _ij5[0];
12496vinfos[5].indices[1] = _ij5[1];
12497vinfos[5].maxsolutions = _nj5;
12498std::vector<int> vfree(0);
12499solutions.AddSolution(vinfos,vfree);
12500}
12501}
12502}
12503
12504}
12505
12506}
12507
12508}
12509} while(0);
12510if( bgotonextstatement )
12511{
12512bool bgotonextstatement = true;
12513do
12514{
12515evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
12516evalcond[1]=gconst11;
12517if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
12518{
12519bgotonextstatement=false;
12520{
12521IkReal j3eval[3];
12522CheckValue<IkReal> x975 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12523if(!x975.valid){
12524continue;
12525}
12526IkReal x974=((1.0)*(x975.value));
12527sj4=0;
12528cj4=-1.0;
12529j4=3.14159265358979;
12530sj5=gconst10;
12531cj5=gconst11;
12532j5=((3.14159265)+(((-1.0)*x974)));
12533new_r11=0;
12534new_r01=0;
12535new_r22=0;
12536new_r20=0;
12537IkReal gconst9=((3.14159265358979)+(((-1.0)*x974)));
12538IkReal gconst10=((1.0)*new_r10);
12539IkReal gconst11=((1.0)*new_r00);
12540j3eval[0]=-1.0;
12541j3eval[1]=-1.0;
12542j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
12543if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12544{
12545{
12546IkReal j3eval[3];
12547CheckValue<IkReal> x977 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12548if(!x977.valid){
12549continue;
12550}
12551IkReal x976=((1.0)*(x977.value));
12552sj4=0;
12553cj4=-1.0;
12554j4=3.14159265358979;
12555sj5=gconst10;
12556cj5=gconst11;
12557j5=((3.14159265)+(((-1.0)*x976)));
12558new_r11=0;
12559new_r01=0;
12560new_r22=0;
12561new_r20=0;
12562IkReal gconst9=((3.14159265358979)+(((-1.0)*x976)));
12563IkReal gconst10=((1.0)*new_r10);
12564IkReal gconst11=((1.0)*new_r00);
12565j3eval[0]=-1.0;
12566j3eval[1]=-1.0;
12567j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
12568if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12569{
12570{
12571IkReal j3eval[3];
12572CheckValue<IkReal> x979 = IKatan2WithCheck(IkReal(new_r10),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12573if(!x979.valid){
12574continue;
12575}
12576IkReal x978=((1.0)*(x979.value));
12577sj4=0;
12578cj4=-1.0;
12579j4=3.14159265358979;
12580sj5=gconst10;
12581cj5=gconst11;
12582j5=((3.14159265)+(((-1.0)*x978)));
12583new_r11=0;
12584new_r01=0;
12585new_r22=0;
12586new_r20=0;
12587IkReal gconst9=((3.14159265358979)+(((-1.0)*x978)));
12588IkReal gconst10=((1.0)*new_r10);
12589IkReal gconst11=((1.0)*new_r00);
12590j3eval[0]=-1.0;
12591j3eval[1]=-1.0;
12592j3eval[2]=((IKabs(((1.0)+(((-2.0)*(new_r10*new_r10))))))+(IKabs(((2.0)*new_r00*new_r10))));
12593if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
12594{
12595continue; // 3 cases reached
12596
12597} else
12598{
12599{
12600IkReal j3array[1], cj3array[1], sj3array[1];
12601bool j3valid[1]={false};
12602_nj3 = 1;
12603CheckValue<IkReal> x980 = IKatan2WithCheck(IkReal((((gconst10*gconst11))+((new_r00*new_r10)))),IkReal(((((-1.0)*(new_r10*new_r10)))+(gconst11*gconst11))),IKFAST_ATAN2_MAGTHRESH);
12604if(!x980.valid){
12605continue;
12606}
12607CheckValue<IkReal> x981=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*new_r10))+(((-1.0)*gconst11*new_r00)))),-1);
12608if(!x981.valid){
12609continue;
12610}
12611j3array[0]=((-1.5707963267949)+(x980.value)+(((1.5707963267949)*(x981.value))));
12612sj3array[0]=IKsin(j3array[0]);
12613cj3array[0]=IKcos(j3array[0]);
12614if( j3array[0] > IKPI )
12615{
12616 j3array[0]-=IK2PI;
12617}
12618else if( j3array[0] < -IKPI )
12619{ j3array[0]+=IK2PI;
12620}
12621j3valid[0] = true;
12622for(int ij3 = 0; ij3 < 1; ++ij3)
12623{
12624if( !j3valid[ij3] )
12625{
12626 continue;
12627}
12628_ij3[0] = ij3; _ij3[1] = -1;
12629for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12630{
12631if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12632{
12633 j3valid[iij3]=false; _ij3[1] = iij3; break;
12634}
12635}
12636j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12637{
12638IkReal evalcond[6];
12639IkReal x982=IKsin(j3);
12640IkReal x983=IKcos(j3);
12641IkReal x984=(gconst11*x982);
12642IkReal x985=(gconst10*x982);
12643IkReal x986=(gconst11*x983);
12644IkReal x987=((1.0)*x983);
12645IkReal x988=(gconst10*x987);
12646evalcond[0]=((((-1.0)*x988))+x984);
12647evalcond[1]=(((new_r00*x983))+gconst11+((new_r10*x982)));
12648evalcond[2]=(new_r00+x985+x986);
12649evalcond[3]=(((new_r00*x982))+gconst10+(((-1.0)*new_r10*x987)));
12650evalcond[4]=((((-1.0)*x985))+(((-1.0)*x986)));
12651evalcond[5]=((((-1.0)*x988))+new_r10+x984);
12652if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12653{
12654continue;
12655}
12656}
12657
12658{
12659std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12660vinfos[0].jointtype = 1;
12661vinfos[0].foffset = j0;
12662vinfos[0].indices[0] = _ij0[0];
12663vinfos[0].indices[1] = _ij0[1];
12664vinfos[0].maxsolutions = _nj0;
12665vinfos[1].jointtype = 1;
12666vinfos[1].foffset = j1;
12667vinfos[1].indices[0] = _ij1[0];
12668vinfos[1].indices[1] = _ij1[1];
12669vinfos[1].maxsolutions = _nj1;
12670vinfos[2].jointtype = 1;
12671vinfos[2].foffset = j2;
12672vinfos[2].indices[0] = _ij2[0];
12673vinfos[2].indices[1] = _ij2[1];
12674vinfos[2].maxsolutions = _nj2;
12675vinfos[3].jointtype = 1;
12676vinfos[3].foffset = j3;
12677vinfos[3].indices[0] = _ij3[0];
12678vinfos[3].indices[1] = _ij3[1];
12679vinfos[3].maxsolutions = _nj3;
12680vinfos[4].jointtype = 1;
12681vinfos[4].foffset = j4;
12682vinfos[4].indices[0] = _ij4[0];
12683vinfos[4].indices[1] = _ij4[1];
12684vinfos[4].maxsolutions = _nj4;
12685vinfos[5].jointtype = 1;
12686vinfos[5].foffset = j5;
12687vinfos[5].indices[0] = _ij5[0];
12688vinfos[5].indices[1] = _ij5[1];
12689vinfos[5].maxsolutions = _nj5;
12690std::vector<int> vfree(0);
12691solutions.AddSolution(vinfos,vfree);
12692}
12693}
12694}
12695
12696}
12697
12698}
12699
12700} else
12701{
12702{
12703IkReal j3array[1], cj3array[1], sj3array[1];
12704bool j3valid[1]={false};
12705_nj3 = 1;
12706CheckValue<IkReal> x989=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst11*gconst11)))+(((-1.0)*(gconst10*gconst10))))),-1);
12707if(!x989.valid){
12708continue;
12709}
12710CheckValue<IkReal> x990 = IKatan2WithCheck(IkReal((gconst10*new_r00)),IkReal((gconst11*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12711if(!x990.valid){
12712continue;
12713}
12714j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x989.value)))+(x990.value));
12715sj3array[0]=IKsin(j3array[0]);
12716cj3array[0]=IKcos(j3array[0]);
12717if( j3array[0] > IKPI )
12718{
12719 j3array[0]-=IK2PI;
12720}
12721else if( j3array[0] < -IKPI )
12722{ j3array[0]+=IK2PI;
12723}
12724j3valid[0] = true;
12725for(int ij3 = 0; ij3 < 1; ++ij3)
12726{
12727if( !j3valid[ij3] )
12728{
12729 continue;
12730}
12731_ij3[0] = ij3; _ij3[1] = -1;
12732for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12733{
12734if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12735{
12736 j3valid[iij3]=false; _ij3[1] = iij3; break;
12737}
12738}
12739j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12740{
12741IkReal evalcond[6];
12742IkReal x991=IKsin(j3);
12743IkReal x992=IKcos(j3);
12744IkReal x993=(gconst11*x991);
12745IkReal x994=(gconst10*x991);
12746IkReal x995=(gconst11*x992);
12747IkReal x996=((1.0)*x992);
12748IkReal x997=(gconst10*x996);
12749evalcond[0]=((((-1.0)*x997))+x993);
12750evalcond[1]=(((new_r10*x991))+gconst11+((new_r00*x992)));
12751evalcond[2]=(new_r00+x995+x994);
12752evalcond[3]=((((-1.0)*new_r10*x996))+gconst10+((new_r00*x991)));
12753evalcond[4]=((((-1.0)*x994))+(((-1.0)*x995)));
12754evalcond[5]=((((-1.0)*x997))+new_r10+x993);
12755if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12756{
12757continue;
12758}
12759}
12760
12761{
12762std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12763vinfos[0].jointtype = 1;
12764vinfos[0].foffset = j0;
12765vinfos[0].indices[0] = _ij0[0];
12766vinfos[0].indices[1] = _ij0[1];
12767vinfos[0].maxsolutions = _nj0;
12768vinfos[1].jointtype = 1;
12769vinfos[1].foffset = j1;
12770vinfos[1].indices[0] = _ij1[0];
12771vinfos[1].indices[1] = _ij1[1];
12772vinfos[1].maxsolutions = _nj1;
12773vinfos[2].jointtype = 1;
12774vinfos[2].foffset = j2;
12775vinfos[2].indices[0] = _ij2[0];
12776vinfos[2].indices[1] = _ij2[1];
12777vinfos[2].maxsolutions = _nj2;
12778vinfos[3].jointtype = 1;
12779vinfos[3].foffset = j3;
12780vinfos[3].indices[0] = _ij3[0];
12781vinfos[3].indices[1] = _ij3[1];
12782vinfos[3].maxsolutions = _nj3;
12783vinfos[4].jointtype = 1;
12784vinfos[4].foffset = j4;
12785vinfos[4].indices[0] = _ij4[0];
12786vinfos[4].indices[1] = _ij4[1];
12787vinfos[4].maxsolutions = _nj4;
12788vinfos[5].jointtype = 1;
12789vinfos[5].foffset = j5;
12790vinfos[5].indices[0] = _ij5[0];
12791vinfos[5].indices[1] = _ij5[1];
12792vinfos[5].maxsolutions = _nj5;
12793std::vector<int> vfree(0);
12794solutions.AddSolution(vinfos,vfree);
12795}
12796}
12797}
12798
12799}
12800
12801}
12802
12803} else
12804{
12805{
12806IkReal j3array[1], cj3array[1], sj3array[1];
12807bool j3valid[1]={false};
12808_nj3 = 1;
12809CheckValue<IkReal> x998 = IKatan2WithCheck(IkReal((gconst10*gconst11)),IkReal(gconst11*gconst11),IKFAST_ATAN2_MAGTHRESH);
12810if(!x998.valid){
12811continue;
12812}
12813CheckValue<IkReal> x999=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*new_r10))+(((-1.0)*gconst11*new_r00)))),-1);
12814if(!x999.valid){
12815continue;
12816}
12817j3array[0]=((-1.5707963267949)+(x998.value)+(((1.5707963267949)*(x999.value))));
12818sj3array[0]=IKsin(j3array[0]);
12819cj3array[0]=IKcos(j3array[0]);
12820if( j3array[0] > IKPI )
12821{
12822 j3array[0]-=IK2PI;
12823}
12824else if( j3array[0] < -IKPI )
12825{ j3array[0]+=IK2PI;
12826}
12827j3valid[0] = true;
12828for(int ij3 = 0; ij3 < 1; ++ij3)
12829{
12830if( !j3valid[ij3] )
12831{
12832 continue;
12833}
12834_ij3[0] = ij3; _ij3[1] = -1;
12835for(int iij3 = ij3+1; iij3 < 1; ++iij3)
12836{
12837if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12838{
12839 j3valid[iij3]=false; _ij3[1] = iij3; break;
12840}
12841}
12842j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12843{
12844IkReal evalcond[6];
12845IkReal x1000=IKsin(j3);
12846IkReal x1001=IKcos(j3);
12847IkReal x1002=(gconst11*x1000);
12848IkReal x1003=(gconst10*x1000);
12849IkReal x1004=(gconst11*x1001);
12850IkReal x1005=((1.0)*x1001);
12851IkReal x1006=(gconst10*x1005);
12852evalcond[0]=(x1002+(((-1.0)*x1006)));
12853evalcond[1]=(gconst11+((new_r10*x1000))+((new_r00*x1001)));
12854evalcond[2]=(x1004+x1003+new_r00);
12855evalcond[3]=(gconst10+(((-1.0)*new_r10*x1005))+((new_r00*x1000)));
12856evalcond[4]=((((-1.0)*x1003))+(((-1.0)*x1004)));
12857evalcond[5]=(x1002+(((-1.0)*x1006))+new_r10);
12858if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12859{
12860continue;
12861}
12862}
12863
12864{
12865std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12866vinfos[0].jointtype = 1;
12867vinfos[0].foffset = j0;
12868vinfos[0].indices[0] = _ij0[0];
12869vinfos[0].indices[1] = _ij0[1];
12870vinfos[0].maxsolutions = _nj0;
12871vinfos[1].jointtype = 1;
12872vinfos[1].foffset = j1;
12873vinfos[1].indices[0] = _ij1[0];
12874vinfos[1].indices[1] = _ij1[1];
12875vinfos[1].maxsolutions = _nj1;
12876vinfos[2].jointtype = 1;
12877vinfos[2].foffset = j2;
12878vinfos[2].indices[0] = _ij2[0];
12879vinfos[2].indices[1] = _ij2[1];
12880vinfos[2].maxsolutions = _nj2;
12881vinfos[3].jointtype = 1;
12882vinfos[3].foffset = j3;
12883vinfos[3].indices[0] = _ij3[0];
12884vinfos[3].indices[1] = _ij3[1];
12885vinfos[3].maxsolutions = _nj3;
12886vinfos[4].jointtype = 1;
12887vinfos[4].foffset = j4;
12888vinfos[4].indices[0] = _ij4[0];
12889vinfos[4].indices[1] = _ij4[1];
12890vinfos[4].maxsolutions = _nj4;
12891vinfos[5].jointtype = 1;
12892vinfos[5].foffset = j5;
12893vinfos[5].indices[0] = _ij5[0];
12894vinfos[5].indices[1] = _ij5[1];
12895vinfos[5].maxsolutions = _nj5;
12896std::vector<int> vfree(0);
12897solutions.AddSolution(vinfos,vfree);
12898}
12899}
12900}
12901
12902}
12903
12904}
12905
12906}
12907} while(0);
12908if( bgotonextstatement )
12909{
12910bool bgotonextstatement = true;
12911do
12912{
12913evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
12914if( IKabs(evalcond[0]) < 0.0000050000000000 )
12915{
12916bgotonextstatement=false;
12917{
12918IkReal j3eval[1];
12919CheckValue<IkReal> x1008 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r00)),IKFAST_ATAN2_MAGTHRESH);
12920if(!x1008.valid){
12921continue;
12922}
12923IkReal x1007=((1.0)*(x1008.value));
12924sj4=0;
12925cj4=-1.0;
12926j4=3.14159265358979;
12927sj5=gconst10;
12928cj5=gconst11;
12929j5=((3.14159265)+(((-1.0)*x1007)));
12930new_r01=0;
12931new_r10=0;
12932IkReal gconst9=((3.14159265358979)+(((-1.0)*x1007)));
12933IkReal gconst10=0;
12934IkReal x1009 = new_r00*new_r00;
12935if(IKabs(x1009)==0){
12936continue;
12937}
12938IkReal gconst11=((1.0)*new_r00*(pow(x1009,-0.5)));
12939j3eval[0]=new_r11;
12940if( IKabs(j3eval[0]) < 0.0000010000000000 )
12941{
12942{
12943IkReal j3array[2], cj3array[2], sj3array[2];
12944bool j3valid[2]={false};
12945_nj3 = 2;
12946CheckValue<IkReal> x1010=IKPowWithIntegerCheck(gconst11,-1);
12947if(!x1010.valid){
12948continue;
12949}
12950cj3array[0]=(new_r11*(x1010.value));
12951if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
12952{
12953 j3valid[0] = j3valid[1] = true;
12954 j3array[0] = IKacos(cj3array[0]);
12955 sj3array[0] = IKsin(j3array[0]);
12956 cj3array[1] = cj3array[0];
12957 j3array[1] = -j3array[0];
12958 sj3array[1] = -sj3array[0];
12959}
12960else if( isnan(cj3array[0]) )
12961{
12962 // probably any value will work
12963 j3valid[0] = true;
12964 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
12965}
12966for(int ij3 = 0; ij3 < 2; ++ij3)
12967{
12968if( !j3valid[ij3] )
12969{
12970 continue;
12971}
12972_ij3[0] = ij3; _ij3[1] = -1;
12973for(int iij3 = ij3+1; iij3 < 2; ++iij3)
12974{
12975if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
12976{
12977 j3valid[iij3]=false; _ij3[1] = iij3; break;
12978}
12979}
12980j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
12981{
12982IkReal evalcond[6];
12983IkReal x1011=IKsin(j3);
12984IkReal x1012=IKcos(j3);
12985evalcond[0]=(new_r00*x1011);
12986evalcond[1]=(new_r11*x1011);
12987evalcond[2]=(gconst11*x1011);
12988evalcond[3]=(gconst11+((new_r00*x1012)));
12989evalcond[4]=(((gconst11*x1012))+new_r00);
12990evalcond[5]=((((-1.0)*new_r11*x1012))+gconst11);
12991if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
12992{
12993continue;
12994}
12995}
12996
12997{
12998std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12999vinfos[0].jointtype = 1;
13000vinfos[0].foffset = j0;
13001vinfos[0].indices[0] = _ij0[0];
13002vinfos[0].indices[1] = _ij0[1];
13003vinfos[0].maxsolutions = _nj0;
13004vinfos[1].jointtype = 1;
13005vinfos[1].foffset = j1;
13006vinfos[1].indices[0] = _ij1[0];
13007vinfos[1].indices[1] = _ij1[1];
13008vinfos[1].maxsolutions = _nj1;
13009vinfos[2].jointtype = 1;
13010vinfos[2].foffset = j2;
13011vinfos[2].indices[0] = _ij2[0];
13012vinfos[2].indices[1] = _ij2[1];
13013vinfos[2].maxsolutions = _nj2;
13014vinfos[3].jointtype = 1;
13015vinfos[3].foffset = j3;
13016vinfos[3].indices[0] = _ij3[0];
13017vinfos[3].indices[1] = _ij3[1];
13018vinfos[3].maxsolutions = _nj3;
13019vinfos[4].jointtype = 1;
13020vinfos[4].foffset = j4;
13021vinfos[4].indices[0] = _ij4[0];
13022vinfos[4].indices[1] = _ij4[1];
13023vinfos[4].maxsolutions = _nj4;
13024vinfos[5].jointtype = 1;
13025vinfos[5].foffset = j5;
13026vinfos[5].indices[0] = _ij5[0];
13027vinfos[5].indices[1] = _ij5[1];
13028vinfos[5].maxsolutions = _nj5;
13029std::vector<int> vfree(0);
13030solutions.AddSolution(vinfos,vfree);
13031}
13032}
13033}
13034
13035} else
13036{
13037{
13038IkReal j3array[2], cj3array[2], sj3array[2];
13039bool j3valid[2]={false};
13040_nj3 = 2;
13041CheckValue<IkReal> x1013=IKPowWithIntegerCheck(new_r11,-1);
13042if(!x1013.valid){
13043continue;
13044}
13045cj3array[0]=(gconst11*(x1013.value));
13046if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
13047{
13048 j3valid[0] = j3valid[1] = true;
13049 j3array[0] = IKacos(cj3array[0]);
13050 sj3array[0] = IKsin(j3array[0]);
13051 cj3array[1] = cj3array[0];
13052 j3array[1] = -j3array[0];
13053 sj3array[1] = -sj3array[0];
13054}
13055else if( isnan(cj3array[0]) )
13056{
13057 // probably any value will work
13058 j3valid[0] = true;
13059 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
13060}
13061for(int ij3 = 0; ij3 < 2; ++ij3)
13062{
13063if( !j3valid[ij3] )
13064{
13065 continue;
13066}
13067_ij3[0] = ij3; _ij3[1] = -1;
13068for(int iij3 = ij3+1; iij3 < 2; ++iij3)
13069{
13070if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13071{
13072 j3valid[iij3]=false; _ij3[1] = iij3; break;
13073}
13074}
13075j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13076{
13077IkReal evalcond[6];
13078IkReal x1014=IKsin(j3);
13079IkReal x1015=IKcos(j3);
13080IkReal x1016=(gconst11*x1015);
13081evalcond[0]=(new_r00*x1014);
13082evalcond[1]=(new_r11*x1014);
13083evalcond[2]=(gconst11*x1014);
13084evalcond[3]=(gconst11+((new_r00*x1015)));
13085evalcond[4]=(x1016+new_r00);
13086evalcond[5]=(new_r11+(((-1.0)*x1016)));
13087if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
13088{
13089continue;
13090}
13091}
13092
13093{
13094std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13095vinfos[0].jointtype = 1;
13096vinfos[0].foffset = j0;
13097vinfos[0].indices[0] = _ij0[0];
13098vinfos[0].indices[1] = _ij0[1];
13099vinfos[0].maxsolutions = _nj0;
13100vinfos[1].jointtype = 1;
13101vinfos[1].foffset = j1;
13102vinfos[1].indices[0] = _ij1[0];
13103vinfos[1].indices[1] = _ij1[1];
13104vinfos[1].maxsolutions = _nj1;
13105vinfos[2].jointtype = 1;
13106vinfos[2].foffset = j2;
13107vinfos[2].indices[0] = _ij2[0];
13108vinfos[2].indices[1] = _ij2[1];
13109vinfos[2].maxsolutions = _nj2;
13110vinfos[3].jointtype = 1;
13111vinfos[3].foffset = j3;
13112vinfos[3].indices[0] = _ij3[0];
13113vinfos[3].indices[1] = _ij3[1];
13114vinfos[3].maxsolutions = _nj3;
13115vinfos[4].jointtype = 1;
13116vinfos[4].foffset = j4;
13117vinfos[4].indices[0] = _ij4[0];
13118vinfos[4].indices[1] = _ij4[1];
13119vinfos[4].maxsolutions = _nj4;
13120vinfos[5].jointtype = 1;
13121vinfos[5].foffset = j5;
13122vinfos[5].indices[0] = _ij5[0];
13123vinfos[5].indices[1] = _ij5[1];
13124vinfos[5].maxsolutions = _nj5;
13125std::vector<int> vfree(0);
13126solutions.AddSolution(vinfos,vfree);
13127}
13128}
13129}
13130
13131}
13132
13133}
13134
13135}
13136} while(0);
13137if( bgotonextstatement )
13138{
13139bool bgotonextstatement = true;
13140do
13141{
13142evalcond[0]=IKabs(new_r00);
13143if( IKabs(evalcond[0]) < 0.0000050000000000 )
13144{
13145bgotonextstatement=false;
13146{
13147IkReal j3eval[1];
13148CheckValue<IkReal> x1018 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13149if(!x1018.valid){
13150continue;
13151}
13152IkReal x1017=((1.0)*(x1018.value));
13153sj4=0;
13154cj4=-1.0;
13155j4=3.14159265358979;
13156sj5=gconst10;
13157cj5=gconst11;
13158j5=((3.14159265)+(((-1.0)*x1017)));
13159new_r00=0;
13160IkReal gconst9=((3.14159265358979)+(((-1.0)*x1017)));
13161IkReal x1019 = new_r10*new_r10;
13162if(IKabs(x1019)==0){
13163continue;
13164}
13165IkReal gconst10=((1.0)*new_r10*(pow(x1019,-0.5)));
13166IkReal gconst11=0;
13167j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
13168if( IKabs(j3eval[0]) < 0.0000010000000000 )
13169{
13170{
13171IkReal j3eval[1];
13172CheckValue<IkReal> x1021 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13173if(!x1021.valid){
13174continue;
13175}
13176IkReal x1020=((1.0)*(x1021.value));
13177sj4=0;
13178cj4=-1.0;
13179j4=3.14159265358979;
13180sj5=gconst10;
13181cj5=gconst11;
13182j5=((3.14159265)+(((-1.0)*x1020)));
13183new_r00=0;
13184IkReal gconst9=((3.14159265358979)+(((-1.0)*x1020)));
13185IkReal x1022 = new_r10*new_r10;
13186if(IKabs(x1022)==0){
13187continue;
13188}
13189IkReal gconst10=((1.0)*new_r10*(pow(x1022,-0.5)));
13190IkReal gconst11=0;
13191j3eval[0]=((IKabs(new_r11))+(IKabs(new_r10)));
13192if( IKabs(j3eval[0]) < 0.0000010000000000 )
13193{
13194{
13195IkReal j3eval[1];
13196CheckValue<IkReal> x1024 = IKatan2WithCheck(IkReal(new_r10),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
13197if(!x1024.valid){
13198continue;
13199}
13200IkReal x1023=((1.0)*(x1024.value));
13201sj4=0;
13202cj4=-1.0;
13203j4=3.14159265358979;
13204sj5=gconst10;
13205cj5=gconst11;
13206j5=((3.14159265)+(((-1.0)*x1023)));
13207new_r00=0;
13208IkReal gconst9=((3.14159265358979)+(((-1.0)*x1023)));
13209IkReal x1025 = new_r10*new_r10;
13210if(IKabs(x1025)==0){
13211continue;
13212}
13213IkReal gconst10=((1.0)*new_r10*(pow(x1025,-0.5)));
13214IkReal gconst11=0;
13215j3eval[0]=new_r10;
13216if( IKabs(j3eval[0]) < 0.0000010000000000 )
13217{
13218continue; // 3 cases reached
13219
13220} else
13221{
13222{
13223IkReal j3array[1], cj3array[1], sj3array[1];
13224bool j3valid[1]={false};
13225_nj3 = 1;
13226CheckValue<IkReal> x1026=IKPowWithIntegerCheck(gconst10,-1);
13227if(!x1026.valid){
13228continue;
13229}
13230CheckValue<IkReal> x1027=IKPowWithIntegerCheck(new_r10,-1);
13231if(!x1027.valid){
13232continue;
13233}
13234if( IKabs((new_r11*(x1026.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst10*(x1027.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((new_r11*(x1026.value)))+IKsqr((gconst10*(x1027.value)))-1) <= IKFAST_SINCOS_THRESH )
13235 continue;
13236j3array[0]=IKatan2((new_r11*(x1026.value)), (gconst10*(x1027.value)));
13237sj3array[0]=IKsin(j3array[0]);
13238cj3array[0]=IKcos(j3array[0]);
13239if( j3array[0] > IKPI )
13240{
13241 j3array[0]-=IK2PI;
13242}
13243else if( j3array[0] < -IKPI )
13244{ j3array[0]+=IK2PI;
13245}
13246j3valid[0] = true;
13247for(int ij3 = 0; ij3 < 1; ++ij3)
13248{
13249if( !j3valid[ij3] )
13250{
13251 continue;
13252}
13253_ij3[0] = ij3; _ij3[1] = -1;
13254for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13255{
13256if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13257{
13258 j3valid[iij3]=false; _ij3[1] = iij3; break;
13259}
13260}
13261j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13262{
13263IkReal evalcond[8];
13264IkReal x1028=IKsin(j3);
13265IkReal x1029=IKcos(j3);
13266IkReal x1030=(gconst10*x1028);
13267IkReal x1031=((1.0)*x1029);
13268IkReal x1032=(gconst10*x1031);
13269evalcond[0]=(new_r10*x1028);
13270evalcond[1]=x1030;
13271evalcond[2]=(gconst10+(((-1.0)*new_r10*x1031)));
13272evalcond[3]=((((-1.0)*x1032))+new_r01);
13273evalcond[4]=((((-1.0)*x1030))+new_r11);
13274evalcond[5]=((((-1.0)*x1032))+new_r10);
13275evalcond[6]=((((-1.0)*new_r11*x1031))+((new_r01*x1028)));
13276evalcond[7]=(((new_r11*x1028))+(((-1.0)*gconst10))+((new_r01*x1029)));
13277if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13278{
13279continue;
13280}
13281}
13282
13283{
13284std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13285vinfos[0].jointtype = 1;
13286vinfos[0].foffset = j0;
13287vinfos[0].indices[0] = _ij0[0];
13288vinfos[0].indices[1] = _ij0[1];
13289vinfos[0].maxsolutions = _nj0;
13290vinfos[1].jointtype = 1;
13291vinfos[1].foffset = j1;
13292vinfos[1].indices[0] = _ij1[0];
13293vinfos[1].indices[1] = _ij1[1];
13294vinfos[1].maxsolutions = _nj1;
13295vinfos[2].jointtype = 1;
13296vinfos[2].foffset = j2;
13297vinfos[2].indices[0] = _ij2[0];
13298vinfos[2].indices[1] = _ij2[1];
13299vinfos[2].maxsolutions = _nj2;
13300vinfos[3].jointtype = 1;
13301vinfos[3].foffset = j3;
13302vinfos[3].indices[0] = _ij3[0];
13303vinfos[3].indices[1] = _ij3[1];
13304vinfos[3].maxsolutions = _nj3;
13305vinfos[4].jointtype = 1;
13306vinfos[4].foffset = j4;
13307vinfos[4].indices[0] = _ij4[0];
13308vinfos[4].indices[1] = _ij4[1];
13309vinfos[4].maxsolutions = _nj4;
13310vinfos[5].jointtype = 1;
13311vinfos[5].foffset = j5;
13312vinfos[5].indices[0] = _ij5[0];
13313vinfos[5].indices[1] = _ij5[1];
13314vinfos[5].maxsolutions = _nj5;
13315std::vector<int> vfree(0);
13316solutions.AddSolution(vinfos,vfree);
13317}
13318}
13319}
13320
13321}
13322
13323}
13324
13325} else
13326{
13327{
13328IkReal j3array[1], cj3array[1], sj3array[1];
13329bool j3valid[1]={false};
13330_nj3 = 1;
13331CheckValue<IkReal> x1033=IKPowWithIntegerCheck(IKsign(gconst10),-1);
13332if(!x1033.valid){
13333continue;
13334}
13335CheckValue<IkReal> x1034 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
13336if(!x1034.valid){
13337continue;
13338}
13339j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1033.value)))+(x1034.value));
13340sj3array[0]=IKsin(j3array[0]);
13341cj3array[0]=IKcos(j3array[0]);
13342if( j3array[0] > IKPI )
13343{
13344 j3array[0]-=IK2PI;
13345}
13346else if( j3array[0] < -IKPI )
13347{ j3array[0]+=IK2PI;
13348}
13349j3valid[0] = true;
13350for(int ij3 = 0; ij3 < 1; ++ij3)
13351{
13352if( !j3valid[ij3] )
13353{
13354 continue;
13355}
13356_ij3[0] = ij3; _ij3[1] = -1;
13357for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13358{
13359if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13360{
13361 j3valid[iij3]=false; _ij3[1] = iij3; break;
13362}
13363}
13364j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13365{
13366IkReal evalcond[8];
13367IkReal x1035=IKsin(j3);
13368IkReal x1036=IKcos(j3);
13369IkReal x1037=(gconst10*x1035);
13370IkReal x1038=((1.0)*x1036);
13371IkReal x1039=(gconst10*x1038);
13372evalcond[0]=(new_r10*x1035);
13373evalcond[1]=x1037;
13374evalcond[2]=(gconst10+(((-1.0)*new_r10*x1038)));
13375evalcond[3]=((((-1.0)*x1039))+new_r01);
13376evalcond[4]=((((-1.0)*x1037))+new_r11);
13377evalcond[5]=((((-1.0)*x1039))+new_r10);
13378evalcond[6]=((((-1.0)*new_r11*x1038))+((new_r01*x1035)));
13379evalcond[7]=(((new_r11*x1035))+(((-1.0)*gconst10))+((new_r01*x1036)));
13380if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13381{
13382continue;
13383}
13384}
13385
13386{
13387std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13388vinfos[0].jointtype = 1;
13389vinfos[0].foffset = j0;
13390vinfos[0].indices[0] = _ij0[0];
13391vinfos[0].indices[1] = _ij0[1];
13392vinfos[0].maxsolutions = _nj0;
13393vinfos[1].jointtype = 1;
13394vinfos[1].foffset = j1;
13395vinfos[1].indices[0] = _ij1[0];
13396vinfos[1].indices[1] = _ij1[1];
13397vinfos[1].maxsolutions = _nj1;
13398vinfos[2].jointtype = 1;
13399vinfos[2].foffset = j2;
13400vinfos[2].indices[0] = _ij2[0];
13401vinfos[2].indices[1] = _ij2[1];
13402vinfos[2].maxsolutions = _nj2;
13403vinfos[3].jointtype = 1;
13404vinfos[3].foffset = j3;
13405vinfos[3].indices[0] = _ij3[0];
13406vinfos[3].indices[1] = _ij3[1];
13407vinfos[3].maxsolutions = _nj3;
13408vinfos[4].jointtype = 1;
13409vinfos[4].foffset = j4;
13410vinfos[4].indices[0] = _ij4[0];
13411vinfos[4].indices[1] = _ij4[1];
13412vinfos[4].maxsolutions = _nj4;
13413vinfos[5].jointtype = 1;
13414vinfos[5].foffset = j5;
13415vinfos[5].indices[0] = _ij5[0];
13416vinfos[5].indices[1] = _ij5[1];
13417vinfos[5].maxsolutions = _nj5;
13418std::vector<int> vfree(0);
13419solutions.AddSolution(vinfos,vfree);
13420}
13421}
13422}
13423
13424}
13425
13426}
13427
13428} else
13429{
13430{
13431IkReal j3array[1], cj3array[1], sj3array[1];
13432bool j3valid[1]={false};
13433_nj3 = 1;
13434CheckValue<IkReal> x1040=IKPowWithIntegerCheck(IKsign(gconst10),-1);
13435if(!x1040.valid){
13436continue;
13437}
13438CheckValue<IkReal> x1041 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
13439if(!x1041.valid){
13440continue;
13441}
13442j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1040.value)))+(x1041.value));
13443sj3array[0]=IKsin(j3array[0]);
13444cj3array[0]=IKcos(j3array[0]);
13445if( j3array[0] > IKPI )
13446{
13447 j3array[0]-=IK2PI;
13448}
13449else if( j3array[0] < -IKPI )
13450{ j3array[0]+=IK2PI;
13451}
13452j3valid[0] = true;
13453for(int ij3 = 0; ij3 < 1; ++ij3)
13454{
13455if( !j3valid[ij3] )
13456{
13457 continue;
13458}
13459_ij3[0] = ij3; _ij3[1] = -1;
13460for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13461{
13462if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13463{
13464 j3valid[iij3]=false; _ij3[1] = iij3; break;
13465}
13466}
13467j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13468{
13469IkReal evalcond[8];
13470IkReal x1042=IKsin(j3);
13471IkReal x1043=IKcos(j3);
13472IkReal x1044=(gconst10*x1042);
13473IkReal x1045=((1.0)*x1043);
13474IkReal x1046=(gconst10*x1045);
13475evalcond[0]=(new_r10*x1042);
13476evalcond[1]=x1044;
13477evalcond[2]=(gconst10+(((-1.0)*new_r10*x1045)));
13478evalcond[3]=(new_r01+(((-1.0)*x1046)));
13479evalcond[4]=((((-1.0)*x1044))+new_r11);
13480evalcond[5]=(new_r10+(((-1.0)*x1046)));
13481evalcond[6]=(((new_r01*x1042))+(((-1.0)*new_r11*x1045)));
13482evalcond[7]=(((new_r11*x1042))+((new_r01*x1043))+(((-1.0)*gconst10)));
13483if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13484{
13485continue;
13486}
13487}
13488
13489{
13490std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13491vinfos[0].jointtype = 1;
13492vinfos[0].foffset = j0;
13493vinfos[0].indices[0] = _ij0[0];
13494vinfos[0].indices[1] = _ij0[1];
13495vinfos[0].maxsolutions = _nj0;
13496vinfos[1].jointtype = 1;
13497vinfos[1].foffset = j1;
13498vinfos[1].indices[0] = _ij1[0];
13499vinfos[1].indices[1] = _ij1[1];
13500vinfos[1].maxsolutions = _nj1;
13501vinfos[2].jointtype = 1;
13502vinfos[2].foffset = j2;
13503vinfos[2].indices[0] = _ij2[0];
13504vinfos[2].indices[1] = _ij2[1];
13505vinfos[2].maxsolutions = _nj2;
13506vinfos[3].jointtype = 1;
13507vinfos[3].foffset = j3;
13508vinfos[3].indices[0] = _ij3[0];
13509vinfos[3].indices[1] = _ij3[1];
13510vinfos[3].maxsolutions = _nj3;
13511vinfos[4].jointtype = 1;
13512vinfos[4].foffset = j4;
13513vinfos[4].indices[0] = _ij4[0];
13514vinfos[4].indices[1] = _ij4[1];
13515vinfos[4].maxsolutions = _nj4;
13516vinfos[5].jointtype = 1;
13517vinfos[5].foffset = j5;
13518vinfos[5].indices[0] = _ij5[0];
13519vinfos[5].indices[1] = _ij5[1];
13520vinfos[5].maxsolutions = _nj5;
13521std::vector<int> vfree(0);
13522solutions.AddSolution(vinfos,vfree);
13523}
13524}
13525}
13526
13527}
13528
13529}
13530
13531}
13532} while(0);
13533if( bgotonextstatement )
13534{
13535bool bgotonextstatement = true;
13536do
13537{
13538if( 1 )
13539{
13540bgotonextstatement=false;
13541continue; // branch miss [j3]
13542
13543}
13544} while(0);
13545if( bgotonextstatement )
13546{
13547}
13548}
13549}
13550}
13551}
13552}
13553
13554} else
13555{
13556{
13557IkReal j3array[1], cj3array[1], sj3array[1];
13558bool j3valid[1]={false};
13559_nj3 = 1;
13560CheckValue<IkReal> x1047 = IKatan2WithCheck(IkReal((((gconst10*gconst11))+((new_r00*new_r10)))),IkReal(((((-1.0)*(new_r10*new_r10)))+(gconst11*gconst11))),IKFAST_ATAN2_MAGTHRESH);
13561if(!x1047.valid){
13562continue;
13563}
13564CheckValue<IkReal> x1048=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*new_r10))+(((-1.0)*gconst11*new_r00)))),-1);
13565if(!x1048.valid){
13566continue;
13567}
13568j3array[0]=((-1.5707963267949)+(x1047.value)+(((1.5707963267949)*(x1048.value))));
13569sj3array[0]=IKsin(j3array[0]);
13570cj3array[0]=IKcos(j3array[0]);
13571if( j3array[0] > IKPI )
13572{
13573 j3array[0]-=IK2PI;
13574}
13575else if( j3array[0] < -IKPI )
13576{ j3array[0]+=IK2PI;
13577}
13578j3valid[0] = true;
13579for(int ij3 = 0; ij3 < 1; ++ij3)
13580{
13581if( !j3valid[ij3] )
13582{
13583 continue;
13584}
13585_ij3[0] = ij3; _ij3[1] = -1;
13586for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13587{
13588if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13589{
13590 j3valid[iij3]=false; _ij3[1] = iij3; break;
13591}
13592}
13593j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13594{
13595IkReal evalcond[8];
13596IkReal x1049=IKcos(j3);
13597IkReal x1050=IKsin(j3);
13598IkReal x1051=((1.0)*gconst10);
13599IkReal x1052=(gconst11*x1050);
13600IkReal x1053=(gconst10*x1050);
13601IkReal x1054=(gconst11*x1049);
13602IkReal x1055=((1.0)*x1049);
13603IkReal x1056=(x1049*x1051);
13604evalcond[0]=(gconst11+((new_r00*x1049))+((new_r10*x1050)));
13605evalcond[1]=(x1053+x1054+new_r00);
13606evalcond[2]=(gconst10+((new_r00*x1050))+(((-1.0)*new_r10*x1055)));
13607evalcond[3]=(gconst11+((new_r01*x1050))+(((-1.0)*new_r11*x1055)));
13608evalcond[4]=((((-1.0)*x1056))+x1052+new_r01);
13609evalcond[5]=((((-1.0)*x1056))+x1052+new_r10);
13610evalcond[6]=((((-1.0)*x1051))+((new_r01*x1049))+((new_r11*x1050)));
13611evalcond[7]=((((-1.0)*x1054))+new_r11+(((-1.0)*x1050*x1051)));
13612if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13613{
13614continue;
13615}
13616}
13617
13618{
13619std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13620vinfos[0].jointtype = 1;
13621vinfos[0].foffset = j0;
13622vinfos[0].indices[0] = _ij0[0];
13623vinfos[0].indices[1] = _ij0[1];
13624vinfos[0].maxsolutions = _nj0;
13625vinfos[1].jointtype = 1;
13626vinfos[1].foffset = j1;
13627vinfos[1].indices[0] = _ij1[0];
13628vinfos[1].indices[1] = _ij1[1];
13629vinfos[1].maxsolutions = _nj1;
13630vinfos[2].jointtype = 1;
13631vinfos[2].foffset = j2;
13632vinfos[2].indices[0] = _ij2[0];
13633vinfos[2].indices[1] = _ij2[1];
13634vinfos[2].maxsolutions = _nj2;
13635vinfos[3].jointtype = 1;
13636vinfos[3].foffset = j3;
13637vinfos[3].indices[0] = _ij3[0];
13638vinfos[3].indices[1] = _ij3[1];
13639vinfos[3].maxsolutions = _nj3;
13640vinfos[4].jointtype = 1;
13641vinfos[4].foffset = j4;
13642vinfos[4].indices[0] = _ij4[0];
13643vinfos[4].indices[1] = _ij4[1];
13644vinfos[4].maxsolutions = _nj4;
13645vinfos[5].jointtype = 1;
13646vinfos[5].foffset = j5;
13647vinfos[5].indices[0] = _ij5[0];
13648vinfos[5].indices[1] = _ij5[1];
13649vinfos[5].maxsolutions = _nj5;
13650std::vector<int> vfree(0);
13651solutions.AddSolution(vinfos,vfree);
13652}
13653}
13654}
13655
13656}
13657
13658}
13659
13660} else
13661{
13662{
13663IkReal j3array[1], cj3array[1], sj3array[1];
13664bool j3valid[1]={false};
13665_nj3 = 1;
13666IkReal x1057=((1.0)*new_r10);
13667CheckValue<IkReal> x1058 = IKatan2WithCheck(IkReal((((gconst10*gconst11))+((new_r00*new_r01)))),IkReal(((((-1.0)*new_r01*x1057))+(gconst11*gconst11))),IKFAST_ATAN2_MAGTHRESH);
13668if(!x1058.valid){
13669continue;
13670}
13671CheckValue<IkReal> x1059=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst10*x1057))+(((-1.0)*gconst11*new_r00)))),-1);
13672if(!x1059.valid){
13673continue;
13674}
13675j3array[0]=((-1.5707963267949)+(x1058.value)+(((1.5707963267949)*(x1059.value))));
13676sj3array[0]=IKsin(j3array[0]);
13677cj3array[0]=IKcos(j3array[0]);
13678if( j3array[0] > IKPI )
13679{
13680 j3array[0]-=IK2PI;
13681}
13682else if( j3array[0] < -IKPI )
13683{ j3array[0]+=IK2PI;
13684}
13685j3valid[0] = true;
13686for(int ij3 = 0; ij3 < 1; ++ij3)
13687{
13688if( !j3valid[ij3] )
13689{
13690 continue;
13691}
13692_ij3[0] = ij3; _ij3[1] = -1;
13693for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13694{
13695if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13696{
13697 j3valid[iij3]=false; _ij3[1] = iij3; break;
13698}
13699}
13700j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13701{
13702IkReal evalcond[8];
13703IkReal x1060=IKcos(j3);
13704IkReal x1061=IKsin(j3);
13705IkReal x1062=((1.0)*gconst10);
13706IkReal x1063=(gconst11*x1061);
13707IkReal x1064=(gconst10*x1061);
13708IkReal x1065=(gconst11*x1060);
13709IkReal x1066=((1.0)*x1060);
13710IkReal x1067=(x1060*x1062);
13711evalcond[0]=(gconst11+((new_r00*x1060))+((new_r10*x1061)));
13712evalcond[1]=(x1065+x1064+new_r00);
13713evalcond[2]=((((-1.0)*new_r10*x1066))+gconst10+((new_r00*x1061)));
13714evalcond[3]=(gconst11+((new_r01*x1061))+(((-1.0)*new_r11*x1066)));
13715evalcond[4]=(x1063+new_r01+(((-1.0)*x1067)));
13716evalcond[5]=(x1063+new_r10+(((-1.0)*x1067)));
13717evalcond[6]=(((new_r01*x1060))+((new_r11*x1061))+(((-1.0)*x1062)));
13718evalcond[7]=((((-1.0)*x1061*x1062))+new_r11+(((-1.0)*x1065)));
13719if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13720{
13721continue;
13722}
13723}
13724
13725{
13726std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13727vinfos[0].jointtype = 1;
13728vinfos[0].foffset = j0;
13729vinfos[0].indices[0] = _ij0[0];
13730vinfos[0].indices[1] = _ij0[1];
13731vinfos[0].maxsolutions = _nj0;
13732vinfos[1].jointtype = 1;
13733vinfos[1].foffset = j1;
13734vinfos[1].indices[0] = _ij1[0];
13735vinfos[1].indices[1] = _ij1[1];
13736vinfos[1].maxsolutions = _nj1;
13737vinfos[2].jointtype = 1;
13738vinfos[2].foffset = j2;
13739vinfos[2].indices[0] = _ij2[0];
13740vinfos[2].indices[1] = _ij2[1];
13741vinfos[2].maxsolutions = _nj2;
13742vinfos[3].jointtype = 1;
13743vinfos[3].foffset = j3;
13744vinfos[3].indices[0] = _ij3[0];
13745vinfos[3].indices[1] = _ij3[1];
13746vinfos[3].maxsolutions = _nj3;
13747vinfos[4].jointtype = 1;
13748vinfos[4].foffset = j4;
13749vinfos[4].indices[0] = _ij4[0];
13750vinfos[4].indices[1] = _ij4[1];
13751vinfos[4].maxsolutions = _nj4;
13752vinfos[5].jointtype = 1;
13753vinfos[5].foffset = j5;
13754vinfos[5].indices[0] = _ij5[0];
13755vinfos[5].indices[1] = _ij5[1];
13756vinfos[5].maxsolutions = _nj5;
13757std::vector<int> vfree(0);
13758solutions.AddSolution(vinfos,vfree);
13759}
13760}
13761}
13762
13763}
13764
13765}
13766
13767} else
13768{
13769{
13770IkReal j3array[1], cj3array[1], sj3array[1];
13771bool j3valid[1]={false};
13772_nj3 = 1;
13773IkReal x1068=((1.0)*new_r10);
13774CheckValue<IkReal> x1069=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1068)))),-1);
13775if(!x1069.valid){
13776continue;
13777}
13778CheckValue<IkReal> x1070 = IKatan2WithCheck(IkReal((((gconst11*new_r00))+((gconst11*new_r11)))),IkReal((((gconst11*new_r01))+(((-1.0)*gconst11*x1068)))),IKFAST_ATAN2_MAGTHRESH);
13779if(!x1070.valid){
13780continue;
13781}
13782j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1069.value)))+(x1070.value));
13783sj3array[0]=IKsin(j3array[0]);
13784cj3array[0]=IKcos(j3array[0]);
13785if( j3array[0] > IKPI )
13786{
13787 j3array[0]-=IK2PI;
13788}
13789else if( j3array[0] < -IKPI )
13790{ j3array[0]+=IK2PI;
13791}
13792j3valid[0] = true;
13793for(int ij3 = 0; ij3 < 1; ++ij3)
13794{
13795if( !j3valid[ij3] )
13796{
13797 continue;
13798}
13799_ij3[0] = ij3; _ij3[1] = -1;
13800for(int iij3 = ij3+1; iij3 < 1; ++iij3)
13801{
13802if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
13803{
13804 j3valid[iij3]=false; _ij3[1] = iij3; break;
13805}
13806}
13807j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
13808{
13809IkReal evalcond[8];
13810IkReal x1071=IKcos(j3);
13811IkReal x1072=IKsin(j3);
13812IkReal x1073=((1.0)*gconst10);
13813IkReal x1074=(gconst11*x1072);
13814IkReal x1075=(gconst10*x1072);
13815IkReal x1076=(gconst11*x1071);
13816IkReal x1077=((1.0)*x1071);
13817IkReal x1078=(x1071*x1073);
13818evalcond[0]=(gconst11+((new_r00*x1071))+((new_r10*x1072)));
13819evalcond[1]=(x1076+x1075+new_r00);
13820evalcond[2]=(gconst10+((new_r00*x1072))+(((-1.0)*new_r10*x1077)));
13821evalcond[3]=((((-1.0)*new_r11*x1077))+gconst11+((new_r01*x1072)));
13822evalcond[4]=(x1074+new_r01+(((-1.0)*x1078)));
13823evalcond[5]=(x1074+new_r10+(((-1.0)*x1078)));
13824evalcond[6]=(((new_r01*x1071))+((new_r11*x1072))+(((-1.0)*x1073)));
13825evalcond[7]=((((-1.0)*x1076))+(((-1.0)*x1072*x1073))+new_r11);
13826if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
13827{
13828continue;
13829}
13830}
13831
13832{
13833std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13834vinfos[0].jointtype = 1;
13835vinfos[0].foffset = j0;
13836vinfos[0].indices[0] = _ij0[0];
13837vinfos[0].indices[1] = _ij0[1];
13838vinfos[0].maxsolutions = _nj0;
13839vinfos[1].jointtype = 1;
13840vinfos[1].foffset = j1;
13841vinfos[1].indices[0] = _ij1[0];
13842vinfos[1].indices[1] = _ij1[1];
13843vinfos[1].maxsolutions = _nj1;
13844vinfos[2].jointtype = 1;
13845vinfos[2].foffset = j2;
13846vinfos[2].indices[0] = _ij2[0];
13847vinfos[2].indices[1] = _ij2[1];
13848vinfos[2].maxsolutions = _nj2;
13849vinfos[3].jointtype = 1;
13850vinfos[3].foffset = j3;
13851vinfos[3].indices[0] = _ij3[0];
13852vinfos[3].indices[1] = _ij3[1];
13853vinfos[3].maxsolutions = _nj3;
13854vinfos[4].jointtype = 1;
13855vinfos[4].foffset = j4;
13856vinfos[4].indices[0] = _ij4[0];
13857vinfos[4].indices[1] = _ij4[1];
13858vinfos[4].maxsolutions = _nj4;
13859vinfos[5].jointtype = 1;
13860vinfos[5].foffset = j5;
13861vinfos[5].indices[0] = _ij5[0];
13862vinfos[5].indices[1] = _ij5[1];
13863vinfos[5].maxsolutions = _nj5;
13864std::vector<int> vfree(0);
13865solutions.AddSolution(vinfos,vfree);
13866}
13867}
13868}
13869
13870}
13871
13872}
13873
13874}
13875} while(0);
13876if( bgotonextstatement )
13877{
13878bool bgotonextstatement = true;
13879do
13880{
13881IkReal x1079=((-1.0)*new_r10);
13882IkReal x1081 = ((new_r10*new_r10)+(new_r00*new_r00));
13883if(IKabs(x1081)==0){
13884continue;
13885}
13886IkReal x1080=pow(x1081,-0.5);
13887CheckValue<IkReal> x1082 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1079),IKFAST_ATAN2_MAGTHRESH);
13888if(!x1082.valid){
13889continue;
13890}
13891IkReal gconst12=((-1.0)*(x1082.value));
13892IkReal gconst13=(new_r00*x1080);
13893IkReal gconst14=(x1079*x1080);
13894CheckValue<IkReal> x1083 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
13895if(!x1083.valid){
13896continue;
13897}
13898evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((x1083.value)+j5)))), 6.28318530717959)));
13899if( IKabs(evalcond[0]) < 0.0000050000000000 )
13900{
13901bgotonextstatement=false;
13902{
13903IkReal j3eval[3];
13904IkReal x1084=((-1.0)*new_r10);
13905CheckValue<IkReal> x1087 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1084),IKFAST_ATAN2_MAGTHRESH);
13906if(!x1087.valid){
13907continue;
13908}
13909IkReal x1085=((-1.0)*(x1087.value));
13910IkReal x1086=x1080;
13911sj4=0;
13912cj4=-1.0;
13913j4=3.14159265358979;
13914sj5=gconst13;
13915cj5=gconst14;
13916j5=x1085;
13917IkReal gconst12=x1085;
13918IkReal gconst13=(new_r00*x1086);
13919IkReal gconst14=(x1084*x1086);
13920IkReal x1088=new_r10*new_r10;
13921IkReal x1089=((1.0)*new_r00);
13922IkReal x1090=((1.0)*new_r10*new_r11);
13923IkReal x1091=((((-1.0)*x1090))+(((-1.0)*new_r01*x1089)));
13924IkReal x1092=x1080;
13925IkReal x1093=(new_r10*x1092);
13926j3eval[0]=x1091;
13927j3eval[1]=((IKabs((((x1088*x1092))+(((-1.0)*new_r01*x1093)))))+(IKabs(((((-1.0)*x1090*x1092))+(((-1.0)*x1089*x1093))))));
13928j3eval[2]=IKsign(x1091);
13929if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
13930{
13931{
13932IkReal j3eval[1];
13933IkReal x1094=((-1.0)*new_r10);
13934CheckValue<IkReal> x1097 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1094),IKFAST_ATAN2_MAGTHRESH);
13935if(!x1097.valid){
13936continue;
13937}
13938IkReal x1095=((-1.0)*(x1097.value));
13939IkReal x1096=x1080;
13940sj4=0;
13941cj4=-1.0;
13942j4=3.14159265358979;
13943sj5=gconst13;
13944cj5=gconst14;
13945j5=x1095;
13946IkReal gconst12=x1095;
13947IkReal gconst13=(new_r00*x1096);
13948IkReal gconst14=(x1094*x1096);
13949IkReal x1098=new_r10*new_r10;
13950CheckValue<IkReal> x1101=IKPowWithIntegerCheck((x1098+(new_r00*new_r00)),-1);
13951if(!x1101.valid){
13952continue;
13953}
13954IkReal x1099=x1101.value;
13955IkReal x1100=(new_r00*x1099);
13956j3eval[0]=((IKabs((((new_r00*new_r11))+((x1098*x1099)))))+(IKabs((((new_r01*x1098*x1100))+((new_r10*x1100))+((new_r01*x1100*(new_r00*new_r00)))))));
13957if( IKabs(j3eval[0]) < 0.0000010000000000 )
13958{
13959{
13960IkReal j3eval[1];
13961IkReal x1102=((-1.0)*new_r10);
13962CheckValue<IkReal> x1105 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1102),IKFAST_ATAN2_MAGTHRESH);
13963if(!x1105.valid){
13964continue;
13965}
13966IkReal x1103=((-1.0)*(x1105.value));
13967IkReal x1104=x1080;
13968sj4=0;
13969cj4=-1.0;
13970j4=3.14159265358979;
13971sj5=gconst13;
13972cj5=gconst14;
13973j5=x1103;
13974IkReal gconst12=x1103;
13975IkReal gconst13=(new_r00*x1104);
13976IkReal gconst14=(x1102*x1104);
13977IkReal x1106=new_r10*new_r10;
13978IkReal x1107=new_r00*new_r00;
13979CheckValue<IkReal> x1111=IKPowWithIntegerCheck((x1106+x1107),-1);
13980if(!x1111.valid){
13981continue;
13982}
13983IkReal x1108=x1111.value;
13984IkReal x1109=(new_r10*x1108);
13985IkReal x1110=(x1106*x1108);
13986j3eval[0]=((IKabs((x1110+(((-1.0)*x1108*(x1107*x1107)))+(((-1.0)*x1107*x1110)))))+(IKabs((((x1109*(new_r00*new_r00*new_r00)))+((new_r00*x1109))+((new_r00*x1109*(new_r10*new_r10)))))));
13987if( IKabs(j3eval[0]) < 0.0000010000000000 )
13988{
13989{
13990IkReal evalcond[2];
13991bool bgotonextstatement = true;
13992do
13993{
13994evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
13995if( IKabs(evalcond[0]) < 0.0000050000000000 )
13996{
13997bgotonextstatement=false;
13998{
13999IkReal j3eval[1];
14000IkReal x1112=((-1.0)*new_r10);
14001CheckValue<IkReal> x1114 = IKatan2WithCheck(IkReal(0),IkReal(x1112),IKFAST_ATAN2_MAGTHRESH);
14002if(!x1114.valid){
14003continue;
14004}
14005IkReal x1113=((-1.0)*(x1114.value));
14006sj4=0;
14007cj4=-1.0;
14008j4=3.14159265358979;
14009sj5=gconst13;
14010cj5=gconst14;
14011j5=x1113;
14012new_r11=0;
14013new_r00=0;
14014IkReal gconst12=x1113;
14015IkReal gconst13=0;
14016IkReal x1115 = new_r10*new_r10;
14017if(IKabs(x1115)==0){
14018continue;
14019}
14020IkReal gconst14=(x1112*(pow(x1115,-0.5)));
14021j3eval[0]=new_r01;
14022if( IKabs(j3eval[0]) < 0.0000010000000000 )
14023{
14024{
14025IkReal j3array[2], cj3array[2], sj3array[2];
14026bool j3valid[2]={false};
14027_nj3 = 2;
14028CheckValue<IkReal> x1116=IKPowWithIntegerCheck(gconst14,-1);
14029if(!x1116.valid){
14030continue;
14031}
14032sj3array[0]=((-1.0)*new_r01*(x1116.value));
14033if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14034{
14035 j3valid[0] = j3valid[1] = true;
14036 j3array[0] = IKasin(sj3array[0]);
14037 cj3array[0] = IKcos(j3array[0]);
14038 sj3array[1] = sj3array[0];
14039 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14040 cj3array[1] = -cj3array[0];
14041}
14042else if( isnan(sj3array[0]) )
14043{
14044 // probably any value will work
14045 j3valid[0] = true;
14046 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14047}
14048for(int ij3 = 0; ij3 < 2; ++ij3)
14049{
14050if( !j3valid[ij3] )
14051{
14052 continue;
14053}
14054_ij3[0] = ij3; _ij3[1] = -1;
14055for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14056{
14057if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14058{
14059 j3valid[iij3]=false; _ij3[1] = iij3; break;
14060}
14061}
14062j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14063{
14064IkReal evalcond[6];
14065IkReal x1117=IKcos(j3);
14066IkReal x1118=IKsin(j3);
14067evalcond[0]=(new_r01*x1117);
14068evalcond[1]=(gconst14*x1117);
14069evalcond[2]=((-1.0)*new_r10*x1117);
14070evalcond[3]=(gconst14+((new_r01*x1118)));
14071evalcond[4]=(gconst14+((new_r10*x1118)));
14072evalcond[5]=(((gconst14*x1118))+new_r10);
14073if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14074{
14075continue;
14076}
14077}
14078
14079{
14080std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14081vinfos[0].jointtype = 1;
14082vinfos[0].foffset = j0;
14083vinfos[0].indices[0] = _ij0[0];
14084vinfos[0].indices[1] = _ij0[1];
14085vinfos[0].maxsolutions = _nj0;
14086vinfos[1].jointtype = 1;
14087vinfos[1].foffset = j1;
14088vinfos[1].indices[0] = _ij1[0];
14089vinfos[1].indices[1] = _ij1[1];
14090vinfos[1].maxsolutions = _nj1;
14091vinfos[2].jointtype = 1;
14092vinfos[2].foffset = j2;
14093vinfos[2].indices[0] = _ij2[0];
14094vinfos[2].indices[1] = _ij2[1];
14095vinfos[2].maxsolutions = _nj2;
14096vinfos[3].jointtype = 1;
14097vinfos[3].foffset = j3;
14098vinfos[3].indices[0] = _ij3[0];
14099vinfos[3].indices[1] = _ij3[1];
14100vinfos[3].maxsolutions = _nj3;
14101vinfos[4].jointtype = 1;
14102vinfos[4].foffset = j4;
14103vinfos[4].indices[0] = _ij4[0];
14104vinfos[4].indices[1] = _ij4[1];
14105vinfos[4].maxsolutions = _nj4;
14106vinfos[5].jointtype = 1;
14107vinfos[5].foffset = j5;
14108vinfos[5].indices[0] = _ij5[0];
14109vinfos[5].indices[1] = _ij5[1];
14110vinfos[5].maxsolutions = _nj5;
14111std::vector<int> vfree(0);
14112solutions.AddSolution(vinfos,vfree);
14113}
14114}
14115}
14116
14117} else
14118{
14119{
14120IkReal j3array[2], cj3array[2], sj3array[2];
14121bool j3valid[2]={false};
14122_nj3 = 2;
14123CheckValue<IkReal> x1119=IKPowWithIntegerCheck(new_r01,-1);
14124if(!x1119.valid){
14125continue;
14126}
14127sj3array[0]=((-1.0)*gconst14*(x1119.value));
14128if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14129{
14130 j3valid[0] = j3valid[1] = true;
14131 j3array[0] = IKasin(sj3array[0]);
14132 cj3array[0] = IKcos(j3array[0]);
14133 sj3array[1] = sj3array[0];
14134 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14135 cj3array[1] = -cj3array[0];
14136}
14137else if( isnan(sj3array[0]) )
14138{
14139 // probably any value will work
14140 j3valid[0] = true;
14141 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14142}
14143for(int ij3 = 0; ij3 < 2; ++ij3)
14144{
14145if( !j3valid[ij3] )
14146{
14147 continue;
14148}
14149_ij3[0] = ij3; _ij3[1] = -1;
14150for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14151{
14152if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14153{
14154 j3valid[iij3]=false; _ij3[1] = iij3; break;
14155}
14156}
14157j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14158{
14159IkReal evalcond[6];
14160IkReal x1120=IKcos(j3);
14161IkReal x1121=IKsin(j3);
14162IkReal x1122=(gconst14*x1121);
14163evalcond[0]=(new_r01*x1120);
14164evalcond[1]=(gconst14*x1120);
14165evalcond[2]=((-1.0)*new_r10*x1120);
14166evalcond[3]=(x1122+new_r01);
14167evalcond[4]=(gconst14+((new_r10*x1121)));
14168evalcond[5]=(x1122+new_r10);
14169if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14170{
14171continue;
14172}
14173}
14174
14175{
14176std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14177vinfos[0].jointtype = 1;
14178vinfos[0].foffset = j0;
14179vinfos[0].indices[0] = _ij0[0];
14180vinfos[0].indices[1] = _ij0[1];
14181vinfos[0].maxsolutions = _nj0;
14182vinfos[1].jointtype = 1;
14183vinfos[1].foffset = j1;
14184vinfos[1].indices[0] = _ij1[0];
14185vinfos[1].indices[1] = _ij1[1];
14186vinfos[1].maxsolutions = _nj1;
14187vinfos[2].jointtype = 1;
14188vinfos[2].foffset = j2;
14189vinfos[2].indices[0] = _ij2[0];
14190vinfos[2].indices[1] = _ij2[1];
14191vinfos[2].maxsolutions = _nj2;
14192vinfos[3].jointtype = 1;
14193vinfos[3].foffset = j3;
14194vinfos[3].indices[0] = _ij3[0];
14195vinfos[3].indices[1] = _ij3[1];
14196vinfos[3].maxsolutions = _nj3;
14197vinfos[4].jointtype = 1;
14198vinfos[4].foffset = j4;
14199vinfos[4].indices[0] = _ij4[0];
14200vinfos[4].indices[1] = _ij4[1];
14201vinfos[4].maxsolutions = _nj4;
14202vinfos[5].jointtype = 1;
14203vinfos[5].foffset = j5;
14204vinfos[5].indices[0] = _ij5[0];
14205vinfos[5].indices[1] = _ij5[1];
14206vinfos[5].maxsolutions = _nj5;
14207std::vector<int> vfree(0);
14208solutions.AddSolution(vinfos,vfree);
14209}
14210}
14211}
14212
14213}
14214
14215}
14216
14217}
14218} while(0);
14219if( bgotonextstatement )
14220{
14221bool bgotonextstatement = true;
14222do
14223{
14224evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
14225evalcond[1]=gconst14;
14226if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
14227{
14228bgotonextstatement=false;
14229{
14230IkReal j3eval[3];
14231IkReal x1123=((-1.0)*new_r10);
14232CheckValue<IkReal> x1125 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1123),IKFAST_ATAN2_MAGTHRESH);
14233if(!x1125.valid){
14234continue;
14235}
14236IkReal x1124=((-1.0)*(x1125.value));
14237sj4=0;
14238cj4=-1.0;
14239j4=3.14159265358979;
14240sj5=gconst13;
14241cj5=gconst14;
14242j5=x1124;
14243new_r11=0;
14244new_r01=0;
14245new_r22=0;
14246new_r20=0;
14247IkReal gconst12=x1124;
14248IkReal gconst13=new_r00;
14249IkReal gconst14=x1123;
14250j3eval[0]=-1.0;
14251j3eval[1]=-1.0;
14252j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
14253if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14254{
14255{
14256IkReal j3eval[3];
14257IkReal x1126=((-1.0)*new_r10);
14258CheckValue<IkReal> x1128 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1126),IKFAST_ATAN2_MAGTHRESH);
14259if(!x1128.valid){
14260continue;
14261}
14262IkReal x1127=((-1.0)*(x1128.value));
14263sj4=0;
14264cj4=-1.0;
14265j4=3.14159265358979;
14266sj5=gconst13;
14267cj5=gconst14;
14268j5=x1127;
14269new_r11=0;
14270new_r01=0;
14271new_r22=0;
14272new_r20=0;
14273IkReal gconst12=x1127;
14274IkReal gconst13=new_r00;
14275IkReal gconst14=x1126;
14276j3eval[0]=-1.0;
14277j3eval[1]=-1.0;
14278j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs((new_r00*new_r10))));
14279if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14280{
14281{
14282IkReal j3eval[3];
14283IkReal x1129=((-1.0)*new_r10);
14284CheckValue<IkReal> x1131 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(x1129),IKFAST_ATAN2_MAGTHRESH);
14285if(!x1131.valid){
14286continue;
14287}
14288IkReal x1130=((-1.0)*(x1131.value));
14289sj4=0;
14290cj4=-1.0;
14291j4=3.14159265358979;
14292sj5=gconst13;
14293cj5=gconst14;
14294j5=x1130;
14295new_r11=0;
14296new_r01=0;
14297new_r22=0;
14298new_r20=0;
14299IkReal gconst12=x1130;
14300IkReal gconst13=new_r00;
14301IkReal gconst14=x1129;
14302j3eval[0]=1.0;
14303j3eval[1]=1.0;
14304j3eval[2]=((((0.5)*(IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))))+(IKabs((new_r00*new_r10))));
14305if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
14306{
14307continue; // 3 cases reached
14308
14309} else
14310{
14311{
14312IkReal j3array[1], cj3array[1], sj3array[1];
14313bool j3valid[1]={false};
14314_nj3 = 1;
14315IkReal x1132=((1.0)*gconst14);
14316CheckValue<IkReal> x1133=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r10*x1132))+((gconst13*new_r00)))),-1);
14317if(!x1133.valid){
14318continue;
14319}
14320CheckValue<IkReal> x1134 = IKatan2WithCheck(IkReal(((gconst14*gconst14)+(((-1.0)*(new_r00*new_r00))))),IkReal(((((-1.0)*gconst13*x1132))+((new_r00*new_r10)))),IKFAST_ATAN2_MAGTHRESH);
14321if(!x1134.valid){
14322continue;
14323}
14324j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1133.value)))+(x1134.value));
14325sj3array[0]=IKsin(j3array[0]);
14326cj3array[0]=IKcos(j3array[0]);
14327if( j3array[0] > IKPI )
14328{
14329 j3array[0]-=IK2PI;
14330}
14331else if( j3array[0] < -IKPI )
14332{ j3array[0]+=IK2PI;
14333}
14334j3valid[0] = true;
14335for(int ij3 = 0; ij3 < 1; ++ij3)
14336{
14337if( !j3valid[ij3] )
14338{
14339 continue;
14340}
14341_ij3[0] = ij3; _ij3[1] = -1;
14342for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14343{
14344if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14345{
14346 j3valid[iij3]=false; _ij3[1] = iij3; break;
14347}
14348}
14349j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14350{
14351IkReal evalcond[6];
14352IkReal x1135=IKsin(j3);
14353IkReal x1136=IKcos(j3);
14354IkReal x1137=((1.0)*gconst13);
14355IkReal x1138=(gconst14*x1135);
14356IkReal x1139=(gconst14*x1136);
14357IkReal x1140=(x1136*x1137);
14358evalcond[0]=(x1138+(((-1.0)*x1140)));
14359evalcond[1]=(gconst14+((new_r00*x1136))+((new_r10*x1135)));
14360evalcond[2]=(x1139+((gconst13*x1135))+new_r00);
14361evalcond[3]=(gconst13+((new_r00*x1135))+(((-1.0)*new_r10*x1136)));
14362evalcond[4]=((((-1.0)*x1139))+(((-1.0)*x1135*x1137)));
14363evalcond[5]=(x1138+(((-1.0)*x1140))+new_r10);
14364if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14365{
14366continue;
14367}
14368}
14369
14370{
14371std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14372vinfos[0].jointtype = 1;
14373vinfos[0].foffset = j0;
14374vinfos[0].indices[0] = _ij0[0];
14375vinfos[0].indices[1] = _ij0[1];
14376vinfos[0].maxsolutions = _nj0;
14377vinfos[1].jointtype = 1;
14378vinfos[1].foffset = j1;
14379vinfos[1].indices[0] = _ij1[0];
14380vinfos[1].indices[1] = _ij1[1];
14381vinfos[1].maxsolutions = _nj1;
14382vinfos[2].jointtype = 1;
14383vinfos[2].foffset = j2;
14384vinfos[2].indices[0] = _ij2[0];
14385vinfos[2].indices[1] = _ij2[1];
14386vinfos[2].maxsolutions = _nj2;
14387vinfos[3].jointtype = 1;
14388vinfos[3].foffset = j3;
14389vinfos[3].indices[0] = _ij3[0];
14390vinfos[3].indices[1] = _ij3[1];
14391vinfos[3].maxsolutions = _nj3;
14392vinfos[4].jointtype = 1;
14393vinfos[4].foffset = j4;
14394vinfos[4].indices[0] = _ij4[0];
14395vinfos[4].indices[1] = _ij4[1];
14396vinfos[4].maxsolutions = _nj4;
14397vinfos[5].jointtype = 1;
14398vinfos[5].foffset = j5;
14399vinfos[5].indices[0] = _ij5[0];
14400vinfos[5].indices[1] = _ij5[1];
14401vinfos[5].maxsolutions = _nj5;
14402std::vector<int> vfree(0);
14403solutions.AddSolution(vinfos,vfree);
14404}
14405}
14406}
14407
14408}
14409
14410}
14411
14412} else
14413{
14414{
14415IkReal j3array[1], cj3array[1], sj3array[1];
14416bool j3valid[1]={false};
14417_nj3 = 1;
14418CheckValue<IkReal> x1141=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst14*gconst14)))+(((-1.0)*(gconst13*gconst13))))),-1);
14419if(!x1141.valid){
14420continue;
14421}
14422CheckValue<IkReal> x1142 = IKatan2WithCheck(IkReal((gconst13*new_r00)),IkReal((gconst14*new_r00)),IKFAST_ATAN2_MAGTHRESH);
14423if(!x1142.valid){
14424continue;
14425}
14426j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1141.value)))+(x1142.value));
14427sj3array[0]=IKsin(j3array[0]);
14428cj3array[0]=IKcos(j3array[0]);
14429if( j3array[0] > IKPI )
14430{
14431 j3array[0]-=IK2PI;
14432}
14433else if( j3array[0] < -IKPI )
14434{ j3array[0]+=IK2PI;
14435}
14436j3valid[0] = true;
14437for(int ij3 = 0; ij3 < 1; ++ij3)
14438{
14439if( !j3valid[ij3] )
14440{
14441 continue;
14442}
14443_ij3[0] = ij3; _ij3[1] = -1;
14444for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14445{
14446if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14447{
14448 j3valid[iij3]=false; _ij3[1] = iij3; break;
14449}
14450}
14451j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14452{
14453IkReal evalcond[6];
14454IkReal x1143=IKsin(j3);
14455IkReal x1144=IKcos(j3);
14456IkReal x1145=((1.0)*gconst13);
14457IkReal x1146=(gconst14*x1143);
14458IkReal x1147=(gconst14*x1144);
14459IkReal x1148=(x1144*x1145);
14460evalcond[0]=(x1146+(((-1.0)*x1148)));
14461evalcond[1]=(((new_r00*x1144))+gconst14+((new_r10*x1143)));
14462evalcond[2]=(x1147+((gconst13*x1143))+new_r00);
14463evalcond[3]=((((-1.0)*new_r10*x1144))+((new_r00*x1143))+gconst13);
14464evalcond[4]=((((-1.0)*x1143*x1145))+(((-1.0)*x1147)));
14465evalcond[5]=(x1146+(((-1.0)*x1148))+new_r10);
14466if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14467{
14468continue;
14469}
14470}
14471
14472{
14473std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14474vinfos[0].jointtype = 1;
14475vinfos[0].foffset = j0;
14476vinfos[0].indices[0] = _ij0[0];
14477vinfos[0].indices[1] = _ij0[1];
14478vinfos[0].maxsolutions = _nj0;
14479vinfos[1].jointtype = 1;
14480vinfos[1].foffset = j1;
14481vinfos[1].indices[0] = _ij1[0];
14482vinfos[1].indices[1] = _ij1[1];
14483vinfos[1].maxsolutions = _nj1;
14484vinfos[2].jointtype = 1;
14485vinfos[2].foffset = j2;
14486vinfos[2].indices[0] = _ij2[0];
14487vinfos[2].indices[1] = _ij2[1];
14488vinfos[2].maxsolutions = _nj2;
14489vinfos[3].jointtype = 1;
14490vinfos[3].foffset = j3;
14491vinfos[3].indices[0] = _ij3[0];
14492vinfos[3].indices[1] = _ij3[1];
14493vinfos[3].maxsolutions = _nj3;
14494vinfos[4].jointtype = 1;
14495vinfos[4].foffset = j4;
14496vinfos[4].indices[0] = _ij4[0];
14497vinfos[4].indices[1] = _ij4[1];
14498vinfos[4].maxsolutions = _nj4;
14499vinfos[5].jointtype = 1;
14500vinfos[5].foffset = j5;
14501vinfos[5].indices[0] = _ij5[0];
14502vinfos[5].indices[1] = _ij5[1];
14503vinfos[5].maxsolutions = _nj5;
14504std::vector<int> vfree(0);
14505solutions.AddSolution(vinfos,vfree);
14506}
14507}
14508}
14509
14510}
14511
14512}
14513
14514} else
14515{
14516{
14517IkReal j3array[1], cj3array[1], sj3array[1];
14518bool j3valid[1]={false};
14519_nj3 = 1;
14520CheckValue<IkReal> x1149 = IKatan2WithCheck(IkReal(gconst13*gconst13),IkReal((gconst13*gconst14)),IKFAST_ATAN2_MAGTHRESH);
14521if(!x1149.valid){
14522continue;
14523}
14524CheckValue<IkReal> x1150=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r00))+((gconst14*new_r10)))),-1);
14525if(!x1150.valid){
14526continue;
14527}
14528j3array[0]=((-1.5707963267949)+(x1149.value)+(((1.5707963267949)*(x1150.value))));
14529sj3array[0]=IKsin(j3array[0]);
14530cj3array[0]=IKcos(j3array[0]);
14531if( j3array[0] > IKPI )
14532{
14533 j3array[0]-=IK2PI;
14534}
14535else if( j3array[0] < -IKPI )
14536{ j3array[0]+=IK2PI;
14537}
14538j3valid[0] = true;
14539for(int ij3 = 0; ij3 < 1; ++ij3)
14540{
14541if( !j3valid[ij3] )
14542{
14543 continue;
14544}
14545_ij3[0] = ij3; _ij3[1] = -1;
14546for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14547{
14548if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14549{
14550 j3valid[iij3]=false; _ij3[1] = iij3; break;
14551}
14552}
14553j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14554{
14555IkReal evalcond[6];
14556IkReal x1151=IKsin(j3);
14557IkReal x1152=IKcos(j3);
14558IkReal x1153=((1.0)*gconst13);
14559IkReal x1154=(gconst14*x1151);
14560IkReal x1155=(gconst14*x1152);
14561IkReal x1156=(x1152*x1153);
14562evalcond[0]=(x1154+(((-1.0)*x1156)));
14563evalcond[1]=(((new_r10*x1151))+gconst14+((new_r00*x1152)));
14564evalcond[2]=(x1155+((gconst13*x1151))+new_r00);
14565evalcond[3]=(gconst13+((new_r00*x1151))+(((-1.0)*new_r10*x1152)));
14566evalcond[4]=((((-1.0)*x1155))+(((-1.0)*x1151*x1153)));
14567evalcond[5]=(x1154+(((-1.0)*x1156))+new_r10);
14568if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14569{
14570continue;
14571}
14572}
14573
14574{
14575std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14576vinfos[0].jointtype = 1;
14577vinfos[0].foffset = j0;
14578vinfos[0].indices[0] = _ij0[0];
14579vinfos[0].indices[1] = _ij0[1];
14580vinfos[0].maxsolutions = _nj0;
14581vinfos[1].jointtype = 1;
14582vinfos[1].foffset = j1;
14583vinfos[1].indices[0] = _ij1[0];
14584vinfos[1].indices[1] = _ij1[1];
14585vinfos[1].maxsolutions = _nj1;
14586vinfos[2].jointtype = 1;
14587vinfos[2].foffset = j2;
14588vinfos[2].indices[0] = _ij2[0];
14589vinfos[2].indices[1] = _ij2[1];
14590vinfos[2].maxsolutions = _nj2;
14591vinfos[3].jointtype = 1;
14592vinfos[3].foffset = j3;
14593vinfos[3].indices[0] = _ij3[0];
14594vinfos[3].indices[1] = _ij3[1];
14595vinfos[3].maxsolutions = _nj3;
14596vinfos[4].jointtype = 1;
14597vinfos[4].foffset = j4;
14598vinfos[4].indices[0] = _ij4[0];
14599vinfos[4].indices[1] = _ij4[1];
14600vinfos[4].maxsolutions = _nj4;
14601vinfos[5].jointtype = 1;
14602vinfos[5].foffset = j5;
14603vinfos[5].indices[0] = _ij5[0];
14604vinfos[5].indices[1] = _ij5[1];
14605vinfos[5].maxsolutions = _nj5;
14606std::vector<int> vfree(0);
14607solutions.AddSolution(vinfos,vfree);
14608}
14609}
14610}
14611
14612}
14613
14614}
14615
14616}
14617} while(0);
14618if( bgotonextstatement )
14619{
14620bool bgotonextstatement = true;
14621do
14622{
14623evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
14624if( IKabs(evalcond[0]) < 0.0000050000000000 )
14625{
14626bgotonextstatement=false;
14627{
14628IkReal j3array[2], cj3array[2], sj3array[2];
14629bool j3valid[2]={false};
14630_nj3 = 2;
14631CheckValue<IkReal> x1157=IKPowWithIntegerCheck(gconst13,-1);
14632if(!x1157.valid){
14633continue;
14634}
14635sj3array[0]=(new_r11*(x1157.value));
14636if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
14637{
14638 j3valid[0] = j3valid[1] = true;
14639 j3array[0] = IKasin(sj3array[0]);
14640 cj3array[0] = IKcos(j3array[0]);
14641 sj3array[1] = sj3array[0];
14642 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
14643 cj3array[1] = -cj3array[0];
14644}
14645else if( isnan(sj3array[0]) )
14646{
14647 // probably any value will work
14648 j3valid[0] = true;
14649 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
14650}
14651for(int ij3 = 0; ij3 < 2; ++ij3)
14652{
14653if( !j3valid[ij3] )
14654{
14655 continue;
14656}
14657_ij3[0] = ij3; _ij3[1] = -1;
14658for(int iij3 = ij3+1; iij3 < 2; ++iij3)
14659{
14660if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14661{
14662 j3valid[iij3]=false; _ij3[1] = iij3; break;
14663}
14664}
14665j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14666{
14667IkReal evalcond[6];
14668IkReal x1158=IKcos(j3);
14669IkReal x1159=IKsin(j3);
14670IkReal x1160=((-1.0)*x1158);
14671evalcond[0]=(new_r00*x1158);
14672evalcond[1]=(new_r11*x1160);
14673evalcond[2]=(gconst13*x1160);
14674evalcond[3]=(gconst13+((new_r00*x1159)));
14675evalcond[4]=(((gconst13*x1159))+new_r00);
14676evalcond[5]=(((new_r11*x1159))+(((-1.0)*gconst13)));
14677if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
14678{
14679continue;
14680}
14681}
14682
14683{
14684std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14685vinfos[0].jointtype = 1;
14686vinfos[0].foffset = j0;
14687vinfos[0].indices[0] = _ij0[0];
14688vinfos[0].indices[1] = _ij0[1];
14689vinfos[0].maxsolutions = _nj0;
14690vinfos[1].jointtype = 1;
14691vinfos[1].foffset = j1;
14692vinfos[1].indices[0] = _ij1[0];
14693vinfos[1].indices[1] = _ij1[1];
14694vinfos[1].maxsolutions = _nj1;
14695vinfos[2].jointtype = 1;
14696vinfos[2].foffset = j2;
14697vinfos[2].indices[0] = _ij2[0];
14698vinfos[2].indices[1] = _ij2[1];
14699vinfos[2].maxsolutions = _nj2;
14700vinfos[3].jointtype = 1;
14701vinfos[3].foffset = j3;
14702vinfos[3].indices[0] = _ij3[0];
14703vinfos[3].indices[1] = _ij3[1];
14704vinfos[3].maxsolutions = _nj3;
14705vinfos[4].jointtype = 1;
14706vinfos[4].foffset = j4;
14707vinfos[4].indices[0] = _ij4[0];
14708vinfos[4].indices[1] = _ij4[1];
14709vinfos[4].maxsolutions = _nj4;
14710vinfos[5].jointtype = 1;
14711vinfos[5].foffset = j5;
14712vinfos[5].indices[0] = _ij5[0];
14713vinfos[5].indices[1] = _ij5[1];
14714vinfos[5].maxsolutions = _nj5;
14715std::vector<int> vfree(0);
14716solutions.AddSolution(vinfos,vfree);
14717}
14718}
14719}
14720
14721}
14722} while(0);
14723if( bgotonextstatement )
14724{
14725bool bgotonextstatement = true;
14726do
14727{
14728evalcond[0]=((IKabs(new_r11))+(IKabs(new_r10)));
14729if( IKabs(evalcond[0]) < 0.0000050000000000 )
14730{
14731bgotonextstatement=false;
14732{
14733IkReal j3eval[1];
14734CheckValue<IkReal> x1162 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14735if(!x1162.valid){
14736continue;
14737}
14738IkReal x1161=((-1.0)*(x1162.value));
14739sj4=0;
14740cj4=-1.0;
14741j4=3.14159265358979;
14742sj5=gconst13;
14743cj5=gconst14;
14744j5=x1161;
14745new_r11=0;
14746new_r10=0;
14747new_r22=0;
14748new_r02=0;
14749IkReal gconst12=x1161;
14750IkReal x1163 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14751if(IKabs(x1163)==0){
14752continue;
14753}
14754IkReal gconst13=(new_r00*(pow(x1163,-0.5)));
14755IkReal gconst14=0;
14756j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
14757if( IKabs(j3eval[0]) < 0.0000010000000000 )
14758{
14759{
14760IkReal j3eval[1];
14761CheckValue<IkReal> x1165 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14762if(!x1165.valid){
14763continue;
14764}
14765IkReal x1164=((-1.0)*(x1165.value));
14766sj4=0;
14767cj4=-1.0;
14768j4=3.14159265358979;
14769sj5=gconst13;
14770cj5=gconst14;
14771j5=x1164;
14772new_r11=0;
14773new_r10=0;
14774new_r22=0;
14775new_r02=0;
14776IkReal gconst12=x1164;
14777IkReal x1166 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14778if(IKabs(x1166)==0){
14779continue;
14780}
14781IkReal gconst13=(new_r00*(pow(x1166,-0.5)));
14782IkReal gconst14=0;
14783j3eval[0]=new_r00;
14784if( IKabs(j3eval[0]) < 0.0000010000000000 )
14785{
14786{
14787IkReal j3eval[2];
14788CheckValue<IkReal> x1168 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
14789if(!x1168.valid){
14790continue;
14791}
14792IkReal x1167=((-1.0)*(x1168.value));
14793sj4=0;
14794cj4=-1.0;
14795j4=3.14159265358979;
14796sj5=gconst13;
14797cj5=gconst14;
14798j5=x1167;
14799new_r11=0;
14800new_r10=0;
14801new_r22=0;
14802new_r02=0;
14803IkReal gconst12=x1167;
14804IkReal x1169 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
14805if(IKabs(x1169)==0){
14806continue;
14807}
14808IkReal gconst13=(new_r00*(pow(x1169,-0.5)));
14809IkReal gconst14=0;
14810j3eval[0]=new_r00;
14811j3eval[1]=new_r01;
14812if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
14813{
14814continue; // 3 cases reached
14815
14816} else
14817{
14818{
14819IkReal j3array[1], cj3array[1], sj3array[1];
14820bool j3valid[1]={false};
14821_nj3 = 1;
14822CheckValue<IkReal> x1170=IKPowWithIntegerCheck(new_r00,-1);
14823if(!x1170.valid){
14824continue;
14825}
14826CheckValue<IkReal> x1171=IKPowWithIntegerCheck(new_r01,-1);
14827if(!x1171.valid){
14828continue;
14829}
14830if( IKabs(((-1.0)*gconst13*(x1170.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst13*(x1171.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst13*(x1170.value)))+IKsqr((gconst13*(x1171.value)))-1) <= IKFAST_SINCOS_THRESH )
14831 continue;
14832j3array[0]=IKatan2(((-1.0)*gconst13*(x1170.value)), (gconst13*(x1171.value)));
14833sj3array[0]=IKsin(j3array[0]);
14834cj3array[0]=IKcos(j3array[0]);
14835if( j3array[0] > IKPI )
14836{
14837 j3array[0]-=IK2PI;
14838}
14839else if( j3array[0] < -IKPI )
14840{ j3array[0]+=IK2PI;
14841}
14842j3valid[0] = true;
14843for(int ij3 = 0; ij3 < 1; ++ij3)
14844{
14845if( !j3valid[ij3] )
14846{
14847 continue;
14848}
14849_ij3[0] = ij3; _ij3[1] = -1;
14850for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14851{
14852if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14853{
14854 j3valid[iij3]=false; _ij3[1] = iij3; break;
14855}
14856}
14857j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14858{
14859IkReal evalcond[8];
14860IkReal x1172=IKsin(j3);
14861IkReal x1173=IKcos(j3);
14862IkReal x1174=(gconst13*x1173);
14863IkReal x1175=(gconst13*x1172);
14864evalcond[0]=(new_r01*x1172);
14865evalcond[1]=(new_r00*x1173);
14866evalcond[2]=((-1.0)*x1175);
14867evalcond[3]=((-1.0)*x1174);
14868evalcond[4]=(gconst13+((new_r00*x1172)));
14869evalcond[5]=(x1175+new_r00);
14870evalcond[6]=(new_r01+(((-1.0)*x1174)));
14871evalcond[7]=((((-1.0)*gconst13))+((new_r01*x1173)));
14872if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14873{
14874continue;
14875}
14876}
14877
14878{
14879std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14880vinfos[0].jointtype = 1;
14881vinfos[0].foffset = j0;
14882vinfos[0].indices[0] = _ij0[0];
14883vinfos[0].indices[1] = _ij0[1];
14884vinfos[0].maxsolutions = _nj0;
14885vinfos[1].jointtype = 1;
14886vinfos[1].foffset = j1;
14887vinfos[1].indices[0] = _ij1[0];
14888vinfos[1].indices[1] = _ij1[1];
14889vinfos[1].maxsolutions = _nj1;
14890vinfos[2].jointtype = 1;
14891vinfos[2].foffset = j2;
14892vinfos[2].indices[0] = _ij2[0];
14893vinfos[2].indices[1] = _ij2[1];
14894vinfos[2].maxsolutions = _nj2;
14895vinfos[3].jointtype = 1;
14896vinfos[3].foffset = j3;
14897vinfos[3].indices[0] = _ij3[0];
14898vinfos[3].indices[1] = _ij3[1];
14899vinfos[3].maxsolutions = _nj3;
14900vinfos[4].jointtype = 1;
14901vinfos[4].foffset = j4;
14902vinfos[4].indices[0] = _ij4[0];
14903vinfos[4].indices[1] = _ij4[1];
14904vinfos[4].maxsolutions = _nj4;
14905vinfos[5].jointtype = 1;
14906vinfos[5].foffset = j5;
14907vinfos[5].indices[0] = _ij5[0];
14908vinfos[5].indices[1] = _ij5[1];
14909vinfos[5].maxsolutions = _nj5;
14910std::vector<int> vfree(0);
14911solutions.AddSolution(vinfos,vfree);
14912}
14913}
14914}
14915
14916}
14917
14918}
14919
14920} else
14921{
14922{
14923IkReal j3array[1], cj3array[1], sj3array[1];
14924bool j3valid[1]={false};
14925_nj3 = 1;
14926CheckValue<IkReal> x1176=IKPowWithIntegerCheck(new_r00,-1);
14927if(!x1176.valid){
14928continue;
14929}
14930CheckValue<IkReal> x1177=IKPowWithIntegerCheck(gconst13,-1);
14931if(!x1177.valid){
14932continue;
14933}
14934if( IKabs(((-1.0)*gconst13*(x1176.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1177.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst13*(x1176.value)))+IKsqr((new_r01*(x1177.value)))-1) <= IKFAST_SINCOS_THRESH )
14935 continue;
14936j3array[0]=IKatan2(((-1.0)*gconst13*(x1176.value)), (new_r01*(x1177.value)));
14937sj3array[0]=IKsin(j3array[0]);
14938cj3array[0]=IKcos(j3array[0]);
14939if( j3array[0] > IKPI )
14940{
14941 j3array[0]-=IK2PI;
14942}
14943else if( j3array[0] < -IKPI )
14944{ j3array[0]+=IK2PI;
14945}
14946j3valid[0] = true;
14947for(int ij3 = 0; ij3 < 1; ++ij3)
14948{
14949if( !j3valid[ij3] )
14950{
14951 continue;
14952}
14953_ij3[0] = ij3; _ij3[1] = -1;
14954for(int iij3 = ij3+1; iij3 < 1; ++iij3)
14955{
14956if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
14957{
14958 j3valid[iij3]=false; _ij3[1] = iij3; break;
14959}
14960}
14961j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
14962{
14963IkReal evalcond[8];
14964IkReal x1178=IKsin(j3);
14965IkReal x1179=IKcos(j3);
14966IkReal x1180=(gconst13*x1179);
14967IkReal x1181=(gconst13*x1178);
14968evalcond[0]=(new_r01*x1178);
14969evalcond[1]=(new_r00*x1179);
14970evalcond[2]=((-1.0)*x1181);
14971evalcond[3]=((-1.0)*x1180);
14972evalcond[4]=(gconst13+((new_r00*x1178)));
14973evalcond[5]=(x1181+new_r00);
14974evalcond[6]=(new_r01+(((-1.0)*x1180)));
14975evalcond[7]=((((-1.0)*gconst13))+((new_r01*x1179)));
14976if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
14977{
14978continue;
14979}
14980}
14981
14982{
14983std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14984vinfos[0].jointtype = 1;
14985vinfos[0].foffset = j0;
14986vinfos[0].indices[0] = _ij0[0];
14987vinfos[0].indices[1] = _ij0[1];
14988vinfos[0].maxsolutions = _nj0;
14989vinfos[1].jointtype = 1;
14990vinfos[1].foffset = j1;
14991vinfos[1].indices[0] = _ij1[0];
14992vinfos[1].indices[1] = _ij1[1];
14993vinfos[1].maxsolutions = _nj1;
14994vinfos[2].jointtype = 1;
14995vinfos[2].foffset = j2;
14996vinfos[2].indices[0] = _ij2[0];
14997vinfos[2].indices[1] = _ij2[1];
14998vinfos[2].maxsolutions = _nj2;
14999vinfos[3].jointtype = 1;
15000vinfos[3].foffset = j3;
15001vinfos[3].indices[0] = _ij3[0];
15002vinfos[3].indices[1] = _ij3[1];
15003vinfos[3].maxsolutions = _nj3;
15004vinfos[4].jointtype = 1;
15005vinfos[4].foffset = j4;
15006vinfos[4].indices[0] = _ij4[0];
15007vinfos[4].indices[1] = _ij4[1];
15008vinfos[4].maxsolutions = _nj4;
15009vinfos[5].jointtype = 1;
15010vinfos[5].foffset = j5;
15011vinfos[5].indices[0] = _ij5[0];
15012vinfos[5].indices[1] = _ij5[1];
15013vinfos[5].maxsolutions = _nj5;
15014std::vector<int> vfree(0);
15015solutions.AddSolution(vinfos,vfree);
15016}
15017}
15018}
15019
15020}
15021
15022}
15023
15024} else
15025{
15026{
15027IkReal j3array[1], cj3array[1], sj3array[1];
15028bool j3valid[1]={false};
15029_nj3 = 1;
15030CheckValue<IkReal> x1182 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15031if(!x1182.valid){
15032continue;
15033}
15034CheckValue<IkReal> x1183=IKPowWithIntegerCheck(IKsign(gconst13),-1);
15035if(!x1183.valid){
15036continue;
15037}
15038j3array[0]=((-1.5707963267949)+(x1182.value)+(((1.5707963267949)*(x1183.value))));
15039sj3array[0]=IKsin(j3array[0]);
15040cj3array[0]=IKcos(j3array[0]);
15041if( j3array[0] > IKPI )
15042{
15043 j3array[0]-=IK2PI;
15044}
15045else if( j3array[0] < -IKPI )
15046{ j3array[0]+=IK2PI;
15047}
15048j3valid[0] = true;
15049for(int ij3 = 0; ij3 < 1; ++ij3)
15050{
15051if( !j3valid[ij3] )
15052{
15053 continue;
15054}
15055_ij3[0] = ij3; _ij3[1] = -1;
15056for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15057{
15058if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15059{
15060 j3valid[iij3]=false; _ij3[1] = iij3; break;
15061}
15062}
15063j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15064{
15065IkReal evalcond[8];
15066IkReal x1184=IKsin(j3);
15067IkReal x1185=IKcos(j3);
15068IkReal x1186=(gconst13*x1185);
15069IkReal x1187=(gconst13*x1184);
15070evalcond[0]=(new_r01*x1184);
15071evalcond[1]=(new_r00*x1185);
15072evalcond[2]=((-1.0)*x1187);
15073evalcond[3]=((-1.0)*x1186);
15074evalcond[4]=(gconst13+((new_r00*x1184)));
15075evalcond[5]=(x1187+new_r00);
15076evalcond[6]=(new_r01+(((-1.0)*x1186)));
15077evalcond[7]=(((new_r01*x1185))+(((-1.0)*gconst13)));
15078if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15079{
15080continue;
15081}
15082}
15083
15084{
15085std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15086vinfos[0].jointtype = 1;
15087vinfos[0].foffset = j0;
15088vinfos[0].indices[0] = _ij0[0];
15089vinfos[0].indices[1] = _ij0[1];
15090vinfos[0].maxsolutions = _nj0;
15091vinfos[1].jointtype = 1;
15092vinfos[1].foffset = j1;
15093vinfos[1].indices[0] = _ij1[0];
15094vinfos[1].indices[1] = _ij1[1];
15095vinfos[1].maxsolutions = _nj1;
15096vinfos[2].jointtype = 1;
15097vinfos[2].foffset = j2;
15098vinfos[2].indices[0] = _ij2[0];
15099vinfos[2].indices[1] = _ij2[1];
15100vinfos[2].maxsolutions = _nj2;
15101vinfos[3].jointtype = 1;
15102vinfos[3].foffset = j3;
15103vinfos[3].indices[0] = _ij3[0];
15104vinfos[3].indices[1] = _ij3[1];
15105vinfos[3].maxsolutions = _nj3;
15106vinfos[4].jointtype = 1;
15107vinfos[4].foffset = j4;
15108vinfos[4].indices[0] = _ij4[0];
15109vinfos[4].indices[1] = _ij4[1];
15110vinfos[4].maxsolutions = _nj4;
15111vinfos[5].jointtype = 1;
15112vinfos[5].foffset = j5;
15113vinfos[5].indices[0] = _ij5[0];
15114vinfos[5].indices[1] = _ij5[1];
15115vinfos[5].maxsolutions = _nj5;
15116std::vector<int> vfree(0);
15117solutions.AddSolution(vinfos,vfree);
15118}
15119}
15120}
15121
15122}
15123
15124}
15125
15126}
15127} while(0);
15128if( bgotonextstatement )
15129{
15130bool bgotonextstatement = true;
15131do
15132{
15133evalcond[0]=IKabs(new_r10);
15134if( IKabs(evalcond[0]) < 0.0000050000000000 )
15135{
15136bgotonextstatement=false;
15137{
15138IkReal j3eval[1];
15139CheckValue<IkReal> x1189 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15140if(!x1189.valid){
15141continue;
15142}
15143IkReal x1188=((-1.0)*(x1189.value));
15144sj4=0;
15145cj4=-1.0;
15146j4=3.14159265358979;
15147sj5=gconst13;
15148cj5=gconst14;
15149j5=x1188;
15150new_r10=0;
15151IkReal gconst12=x1188;
15152IkReal x1190 = new_r00*new_r00;
15153if(IKabs(x1190)==0){
15154continue;
15155}
15156IkReal gconst13=(new_r00*(pow(x1190,-0.5)));
15157IkReal gconst14=0;
15158j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
15159if( IKabs(j3eval[0]) < 0.0000010000000000 )
15160{
15161{
15162IkReal j3eval[1];
15163CheckValue<IkReal> x1192 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15164if(!x1192.valid){
15165continue;
15166}
15167IkReal x1191=((-1.0)*(x1192.value));
15168sj4=0;
15169cj4=-1.0;
15170j4=3.14159265358979;
15171sj5=gconst13;
15172cj5=gconst14;
15173j5=x1191;
15174new_r10=0;
15175IkReal gconst12=x1191;
15176IkReal x1193 = new_r00*new_r00;
15177if(IKabs(x1193)==0){
15178continue;
15179}
15180IkReal gconst13=(new_r00*(pow(x1193,-0.5)));
15181IkReal gconst14=0;
15182j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
15183if( IKabs(j3eval[0]) < 0.0000010000000000 )
15184{
15185{
15186IkReal j3eval[1];
15187CheckValue<IkReal> x1195 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
15188if(!x1195.valid){
15189continue;
15190}
15191IkReal x1194=((-1.0)*(x1195.value));
15192sj4=0;
15193cj4=-1.0;
15194j4=3.14159265358979;
15195sj5=gconst13;
15196cj5=gconst14;
15197j5=x1194;
15198new_r10=0;
15199IkReal gconst12=x1194;
15200IkReal x1196 = new_r00*new_r00;
15201if(IKabs(x1196)==0){
15202continue;
15203}
15204IkReal gconst13=(new_r00*(pow(x1196,-0.5)));
15205IkReal gconst14=0;
15206j3eval[0]=new_r00;
15207if( IKabs(j3eval[0]) < 0.0000010000000000 )
15208{
15209continue; // 3 cases reached
15210
15211} else
15212{
15213{
15214IkReal j3array[1], cj3array[1], sj3array[1];
15215bool j3valid[1]={false};
15216_nj3 = 1;
15217CheckValue<IkReal> x1197=IKPowWithIntegerCheck(new_r00,-1);
15218if(!x1197.valid){
15219continue;
15220}
15221CheckValue<IkReal> x1198=IKPowWithIntegerCheck(gconst13,-1);
15222if(!x1198.valid){
15223continue;
15224}
15225if( IKabs(((-1.0)*gconst13*(x1197.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1198.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst13*(x1197.value)))+IKsqr((new_r01*(x1198.value)))-1) <= IKFAST_SINCOS_THRESH )
15226 continue;
15227j3array[0]=IKatan2(((-1.0)*gconst13*(x1197.value)), (new_r01*(x1198.value)));
15228sj3array[0]=IKsin(j3array[0]);
15229cj3array[0]=IKcos(j3array[0]);
15230if( j3array[0] > IKPI )
15231{
15232 j3array[0]-=IK2PI;
15233}
15234else if( j3array[0] < -IKPI )
15235{ j3array[0]+=IK2PI;
15236}
15237j3valid[0] = true;
15238for(int ij3 = 0; ij3 < 1; ++ij3)
15239{
15240if( !j3valid[ij3] )
15241{
15242 continue;
15243}
15244_ij3[0] = ij3; _ij3[1] = -1;
15245for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15246{
15247if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15248{
15249 j3valid[iij3]=false; _ij3[1] = iij3; break;
15250}
15251}
15252j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15253{
15254IkReal evalcond[8];
15255IkReal x1199=IKcos(j3);
15256IkReal x1200=IKsin(j3);
15257IkReal x1201=(gconst13*x1199);
15258IkReal x1202=(gconst13*x1200);
15259evalcond[0]=(new_r00*x1199);
15260evalcond[1]=((-1.0)*x1201);
15261evalcond[2]=(((new_r00*x1200))+gconst13);
15262evalcond[3]=(x1202+new_r00);
15263evalcond[4]=((((-1.0)*x1201))+new_r01);
15264evalcond[5]=((((-1.0)*x1202))+new_r11);
15265evalcond[6]=(((new_r01*x1200))+(((-1.0)*new_r11*x1199)));
15266evalcond[7]=(((new_r11*x1200))+(((-1.0)*gconst13))+((new_r01*x1199)));
15267if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15268{
15269continue;
15270}
15271}
15272
15273{
15274std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15275vinfos[0].jointtype = 1;
15276vinfos[0].foffset = j0;
15277vinfos[0].indices[0] = _ij0[0];
15278vinfos[0].indices[1] = _ij0[1];
15279vinfos[0].maxsolutions = _nj0;
15280vinfos[1].jointtype = 1;
15281vinfos[1].foffset = j1;
15282vinfos[1].indices[0] = _ij1[0];
15283vinfos[1].indices[1] = _ij1[1];
15284vinfos[1].maxsolutions = _nj1;
15285vinfos[2].jointtype = 1;
15286vinfos[2].foffset = j2;
15287vinfos[2].indices[0] = _ij2[0];
15288vinfos[2].indices[1] = _ij2[1];
15289vinfos[2].maxsolutions = _nj2;
15290vinfos[3].jointtype = 1;
15291vinfos[3].foffset = j3;
15292vinfos[3].indices[0] = _ij3[0];
15293vinfos[3].indices[1] = _ij3[1];
15294vinfos[3].maxsolutions = _nj3;
15295vinfos[4].jointtype = 1;
15296vinfos[4].foffset = j4;
15297vinfos[4].indices[0] = _ij4[0];
15298vinfos[4].indices[1] = _ij4[1];
15299vinfos[4].maxsolutions = _nj4;
15300vinfos[5].jointtype = 1;
15301vinfos[5].foffset = j5;
15302vinfos[5].indices[0] = _ij5[0];
15303vinfos[5].indices[1] = _ij5[1];
15304vinfos[5].maxsolutions = _nj5;
15305std::vector<int> vfree(0);
15306solutions.AddSolution(vinfos,vfree);
15307}
15308}
15309}
15310
15311}
15312
15313}
15314
15315} else
15316{
15317{
15318IkReal j3array[1], cj3array[1], sj3array[1];
15319bool j3valid[1]={false};
15320_nj3 = 1;
15321CheckValue<IkReal> x1203 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15322if(!x1203.valid){
15323continue;
15324}
15325CheckValue<IkReal> x1204=IKPowWithIntegerCheck(IKsign(gconst13),-1);
15326if(!x1204.valid){
15327continue;
15328}
15329j3array[0]=((-1.5707963267949)+(x1203.value)+(((1.5707963267949)*(x1204.value))));
15330sj3array[0]=IKsin(j3array[0]);
15331cj3array[0]=IKcos(j3array[0]);
15332if( j3array[0] > IKPI )
15333{
15334 j3array[0]-=IK2PI;
15335}
15336else if( j3array[0] < -IKPI )
15337{ j3array[0]+=IK2PI;
15338}
15339j3valid[0] = true;
15340for(int ij3 = 0; ij3 < 1; ++ij3)
15341{
15342if( !j3valid[ij3] )
15343{
15344 continue;
15345}
15346_ij3[0] = ij3; _ij3[1] = -1;
15347for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15348{
15349if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15350{
15351 j3valid[iij3]=false; _ij3[1] = iij3; break;
15352}
15353}
15354j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15355{
15356IkReal evalcond[8];
15357IkReal x1205=IKcos(j3);
15358IkReal x1206=IKsin(j3);
15359IkReal x1207=(gconst13*x1205);
15360IkReal x1208=(gconst13*x1206);
15361evalcond[0]=(new_r00*x1205);
15362evalcond[1]=((-1.0)*x1207);
15363evalcond[2]=(((new_r00*x1206))+gconst13);
15364evalcond[3]=(x1208+new_r00);
15365evalcond[4]=((((-1.0)*x1207))+new_r01);
15366evalcond[5]=((((-1.0)*x1208))+new_r11);
15367evalcond[6]=(((new_r01*x1206))+(((-1.0)*new_r11*x1205)));
15368evalcond[7]=(((new_r11*x1206))+((new_r01*x1205))+(((-1.0)*gconst13)));
15369if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15370{
15371continue;
15372}
15373}
15374
15375{
15376std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15377vinfos[0].jointtype = 1;
15378vinfos[0].foffset = j0;
15379vinfos[0].indices[0] = _ij0[0];
15380vinfos[0].indices[1] = _ij0[1];
15381vinfos[0].maxsolutions = _nj0;
15382vinfos[1].jointtype = 1;
15383vinfos[1].foffset = j1;
15384vinfos[1].indices[0] = _ij1[0];
15385vinfos[1].indices[1] = _ij1[1];
15386vinfos[1].maxsolutions = _nj1;
15387vinfos[2].jointtype = 1;
15388vinfos[2].foffset = j2;
15389vinfos[2].indices[0] = _ij2[0];
15390vinfos[2].indices[1] = _ij2[1];
15391vinfos[2].maxsolutions = _nj2;
15392vinfos[3].jointtype = 1;
15393vinfos[3].foffset = j3;
15394vinfos[3].indices[0] = _ij3[0];
15395vinfos[3].indices[1] = _ij3[1];
15396vinfos[3].maxsolutions = _nj3;
15397vinfos[4].jointtype = 1;
15398vinfos[4].foffset = j4;
15399vinfos[4].indices[0] = _ij4[0];
15400vinfos[4].indices[1] = _ij4[1];
15401vinfos[4].maxsolutions = _nj4;
15402vinfos[5].jointtype = 1;
15403vinfos[5].foffset = j5;
15404vinfos[5].indices[0] = _ij5[0];
15405vinfos[5].indices[1] = _ij5[1];
15406vinfos[5].maxsolutions = _nj5;
15407std::vector<int> vfree(0);
15408solutions.AddSolution(vinfos,vfree);
15409}
15410}
15411}
15412
15413}
15414
15415}
15416
15417} else
15418{
15419{
15420IkReal j3array[1], cj3array[1], sj3array[1];
15421bool j3valid[1]={false};
15422_nj3 = 1;
15423CheckValue<IkReal> x1209 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
15424if(!x1209.valid){
15425continue;
15426}
15427CheckValue<IkReal> x1210=IKPowWithIntegerCheck(IKsign(gconst13),-1);
15428if(!x1210.valid){
15429continue;
15430}
15431j3array[0]=((-1.5707963267949)+(x1209.value)+(((1.5707963267949)*(x1210.value))));
15432sj3array[0]=IKsin(j3array[0]);
15433cj3array[0]=IKcos(j3array[0]);
15434if( j3array[0] > IKPI )
15435{
15436 j3array[0]-=IK2PI;
15437}
15438else if( j3array[0] < -IKPI )
15439{ j3array[0]+=IK2PI;
15440}
15441j3valid[0] = true;
15442for(int ij3 = 0; ij3 < 1; ++ij3)
15443{
15444if( !j3valid[ij3] )
15445{
15446 continue;
15447}
15448_ij3[0] = ij3; _ij3[1] = -1;
15449for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15450{
15451if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15452{
15453 j3valid[iij3]=false; _ij3[1] = iij3; break;
15454}
15455}
15456j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15457{
15458IkReal evalcond[8];
15459IkReal x1211=IKcos(j3);
15460IkReal x1212=IKsin(j3);
15461IkReal x1213=(gconst13*x1211);
15462IkReal x1214=(gconst13*x1212);
15463evalcond[0]=(new_r00*x1211);
15464evalcond[1]=((-1.0)*x1213);
15465evalcond[2]=(gconst13+((new_r00*x1212)));
15466evalcond[3]=(x1214+new_r00);
15467evalcond[4]=(new_r01+(((-1.0)*x1213)));
15468evalcond[5]=(new_r11+(((-1.0)*x1214)));
15469evalcond[6]=(((new_r01*x1212))+(((-1.0)*new_r11*x1211)));
15470evalcond[7]=(((new_r11*x1212))+((new_r01*x1211))+(((-1.0)*gconst13)));
15471if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15472{
15473continue;
15474}
15475}
15476
15477{
15478std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15479vinfos[0].jointtype = 1;
15480vinfos[0].foffset = j0;
15481vinfos[0].indices[0] = _ij0[0];
15482vinfos[0].indices[1] = _ij0[1];
15483vinfos[0].maxsolutions = _nj0;
15484vinfos[1].jointtype = 1;
15485vinfos[1].foffset = j1;
15486vinfos[1].indices[0] = _ij1[0];
15487vinfos[1].indices[1] = _ij1[1];
15488vinfos[1].maxsolutions = _nj1;
15489vinfos[2].jointtype = 1;
15490vinfos[2].foffset = j2;
15491vinfos[2].indices[0] = _ij2[0];
15492vinfos[2].indices[1] = _ij2[1];
15493vinfos[2].maxsolutions = _nj2;
15494vinfos[3].jointtype = 1;
15495vinfos[3].foffset = j3;
15496vinfos[3].indices[0] = _ij3[0];
15497vinfos[3].indices[1] = _ij3[1];
15498vinfos[3].maxsolutions = _nj3;
15499vinfos[4].jointtype = 1;
15500vinfos[4].foffset = j4;
15501vinfos[4].indices[0] = _ij4[0];
15502vinfos[4].indices[1] = _ij4[1];
15503vinfos[4].maxsolutions = _nj4;
15504vinfos[5].jointtype = 1;
15505vinfos[5].foffset = j5;
15506vinfos[5].indices[0] = _ij5[0];
15507vinfos[5].indices[1] = _ij5[1];
15508vinfos[5].maxsolutions = _nj5;
15509std::vector<int> vfree(0);
15510solutions.AddSolution(vinfos,vfree);
15511}
15512}
15513}
15514
15515}
15516
15517}
15518
15519}
15520} while(0);
15521if( bgotonextstatement )
15522{
15523bool bgotonextstatement = true;
15524do
15525{
15526if( 1 )
15527{
15528bgotonextstatement=false;
15529continue; // branch miss [j3]
15530
15531}
15532} while(0);
15533if( bgotonextstatement )
15534{
15535}
15536}
15537}
15538}
15539}
15540}
15541}
15542
15543} else
15544{
15545{
15546IkReal j3array[1], cj3array[1], sj3array[1];
15547bool j3valid[1]={false};
15548_nj3 = 1;
15549IkReal x1215=((1.0)*gconst14);
15550CheckValue<IkReal> x1216=IKPowWithIntegerCheck(IKsign((((gconst13*new_r00))+(((-1.0)*new_r10*x1215)))),-1);
15551if(!x1216.valid){
15552continue;
15553}
15554CheckValue<IkReal> x1217 = IKatan2WithCheck(IkReal(((gconst14*gconst14)+(((-1.0)*(new_r00*new_r00))))),IkReal((((new_r00*new_r10))+(((-1.0)*gconst13*x1215)))),IKFAST_ATAN2_MAGTHRESH);
15555if(!x1217.valid){
15556continue;
15557}
15558j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1216.value)))+(x1217.value));
15559sj3array[0]=IKsin(j3array[0]);
15560cj3array[0]=IKcos(j3array[0]);
15561if( j3array[0] > IKPI )
15562{
15563 j3array[0]-=IK2PI;
15564}
15565else if( j3array[0] < -IKPI )
15566{ j3array[0]+=IK2PI;
15567}
15568j3valid[0] = true;
15569for(int ij3 = 0; ij3 < 1; ++ij3)
15570{
15571if( !j3valid[ij3] )
15572{
15573 continue;
15574}
15575_ij3[0] = ij3; _ij3[1] = -1;
15576for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15577{
15578if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15579{
15580 j3valid[iij3]=false; _ij3[1] = iij3; break;
15581}
15582}
15583j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15584{
15585IkReal evalcond[8];
15586IkReal x1218=IKsin(j3);
15587IkReal x1219=IKcos(j3);
15588IkReal x1220=(gconst14*x1218);
15589IkReal x1221=((1.0)*x1219);
15590IkReal x1222=(gconst13*x1218);
15591IkReal x1223=(gconst13*x1221);
15592evalcond[0]=(((new_r10*x1218))+gconst14+((new_r00*x1219)));
15593evalcond[1]=(x1222+((gconst14*x1219))+new_r00);
15594evalcond[2]=(gconst13+((new_r00*x1218))+(((-1.0)*new_r10*x1221)));
15595evalcond[3]=(gconst14+((new_r01*x1218))+(((-1.0)*new_r11*x1221)));
15596evalcond[4]=(x1220+(((-1.0)*x1223))+new_r01);
15597evalcond[5]=(x1220+(((-1.0)*x1223))+new_r10);
15598evalcond[6]=(((new_r11*x1218))+((new_r01*x1219))+(((-1.0)*gconst13)));
15599evalcond[7]=((((-1.0)*x1222))+new_r11+(((-1.0)*gconst14*x1221)));
15600if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15601{
15602continue;
15603}
15604}
15605
15606{
15607std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15608vinfos[0].jointtype = 1;
15609vinfos[0].foffset = j0;
15610vinfos[0].indices[0] = _ij0[0];
15611vinfos[0].indices[1] = _ij0[1];
15612vinfos[0].maxsolutions = _nj0;
15613vinfos[1].jointtype = 1;
15614vinfos[1].foffset = j1;
15615vinfos[1].indices[0] = _ij1[0];
15616vinfos[1].indices[1] = _ij1[1];
15617vinfos[1].maxsolutions = _nj1;
15618vinfos[2].jointtype = 1;
15619vinfos[2].foffset = j2;
15620vinfos[2].indices[0] = _ij2[0];
15621vinfos[2].indices[1] = _ij2[1];
15622vinfos[2].maxsolutions = _nj2;
15623vinfos[3].jointtype = 1;
15624vinfos[3].foffset = j3;
15625vinfos[3].indices[0] = _ij3[0];
15626vinfos[3].indices[1] = _ij3[1];
15627vinfos[3].maxsolutions = _nj3;
15628vinfos[4].jointtype = 1;
15629vinfos[4].foffset = j4;
15630vinfos[4].indices[0] = _ij4[0];
15631vinfos[4].indices[1] = _ij4[1];
15632vinfos[4].maxsolutions = _nj4;
15633vinfos[5].jointtype = 1;
15634vinfos[5].foffset = j5;
15635vinfos[5].indices[0] = _ij5[0];
15636vinfos[5].indices[1] = _ij5[1];
15637vinfos[5].maxsolutions = _nj5;
15638std::vector<int> vfree(0);
15639solutions.AddSolution(vinfos,vfree);
15640}
15641}
15642}
15643
15644}
15645
15646}
15647
15648} else
15649{
15650{
15651IkReal j3array[1], cj3array[1], sj3array[1];
15652bool j3valid[1]={false};
15653_nj3 = 1;
15654IkReal x1224=((1.0)*gconst14);
15655CheckValue<IkReal> x1225 = IKatan2WithCheck(IkReal(((gconst14*gconst14)+((new_r00*new_r11)))),IkReal((((new_r00*new_r01))+(((-1.0)*gconst13*x1224)))),IKFAST_ATAN2_MAGTHRESH);
15656if(!x1225.valid){
15657continue;
15658}
15659CheckValue<IkReal> x1226=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst13*new_r11))+(((-1.0)*new_r01*x1224)))),-1);
15660if(!x1226.valid){
15661continue;
15662}
15663j3array[0]=((-1.5707963267949)+(x1225.value)+(((1.5707963267949)*(x1226.value))));
15664sj3array[0]=IKsin(j3array[0]);
15665cj3array[0]=IKcos(j3array[0]);
15666if( j3array[0] > IKPI )
15667{
15668 j3array[0]-=IK2PI;
15669}
15670else if( j3array[0] < -IKPI )
15671{ j3array[0]+=IK2PI;
15672}
15673j3valid[0] = true;
15674for(int ij3 = 0; ij3 < 1; ++ij3)
15675{
15676if( !j3valid[ij3] )
15677{
15678 continue;
15679}
15680_ij3[0] = ij3; _ij3[1] = -1;
15681for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15682{
15683if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15684{
15685 j3valid[iij3]=false; _ij3[1] = iij3; break;
15686}
15687}
15688j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15689{
15690IkReal evalcond[8];
15691IkReal x1227=IKsin(j3);
15692IkReal x1228=IKcos(j3);
15693IkReal x1229=(gconst14*x1227);
15694IkReal x1230=((1.0)*x1228);
15695IkReal x1231=(gconst13*x1227);
15696IkReal x1232=(gconst13*x1230);
15697evalcond[0]=(gconst14+((new_r00*x1228))+((new_r10*x1227)));
15698evalcond[1]=(x1231+new_r00+((gconst14*x1228)));
15699evalcond[2]=(gconst13+((new_r00*x1227))+(((-1.0)*new_r10*x1230)));
15700evalcond[3]=(gconst14+((new_r01*x1227))+(((-1.0)*new_r11*x1230)));
15701evalcond[4]=(x1229+(((-1.0)*x1232))+new_r01);
15702evalcond[5]=(x1229+(((-1.0)*x1232))+new_r10);
15703evalcond[6]=(((new_r11*x1227))+((new_r01*x1228))+(((-1.0)*gconst13)));
15704evalcond[7]=((((-1.0)*x1231))+new_r11+(((-1.0)*gconst14*x1230)));
15705if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15706{
15707continue;
15708}
15709}
15710
15711{
15712std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15713vinfos[0].jointtype = 1;
15714vinfos[0].foffset = j0;
15715vinfos[0].indices[0] = _ij0[0];
15716vinfos[0].indices[1] = _ij0[1];
15717vinfos[0].maxsolutions = _nj0;
15718vinfos[1].jointtype = 1;
15719vinfos[1].foffset = j1;
15720vinfos[1].indices[0] = _ij1[0];
15721vinfos[1].indices[1] = _ij1[1];
15722vinfos[1].maxsolutions = _nj1;
15723vinfos[2].jointtype = 1;
15724vinfos[2].foffset = j2;
15725vinfos[2].indices[0] = _ij2[0];
15726vinfos[2].indices[1] = _ij2[1];
15727vinfos[2].maxsolutions = _nj2;
15728vinfos[3].jointtype = 1;
15729vinfos[3].foffset = j3;
15730vinfos[3].indices[0] = _ij3[0];
15731vinfos[3].indices[1] = _ij3[1];
15732vinfos[3].maxsolutions = _nj3;
15733vinfos[4].jointtype = 1;
15734vinfos[4].foffset = j4;
15735vinfos[4].indices[0] = _ij4[0];
15736vinfos[4].indices[1] = _ij4[1];
15737vinfos[4].maxsolutions = _nj4;
15738vinfos[5].jointtype = 1;
15739vinfos[5].foffset = j5;
15740vinfos[5].indices[0] = _ij5[0];
15741vinfos[5].indices[1] = _ij5[1];
15742vinfos[5].maxsolutions = _nj5;
15743std::vector<int> vfree(0);
15744solutions.AddSolution(vinfos,vfree);
15745}
15746}
15747}
15748
15749}
15750
15751}
15752
15753} else
15754{
15755{
15756IkReal j3array[1], cj3array[1], sj3array[1];
15757bool j3valid[1]={false};
15758_nj3 = 1;
15759IkReal x1233=((1.0)*new_r10);
15760CheckValue<IkReal> x1234=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1233)))),-1);
15761if(!x1234.valid){
15762continue;
15763}
15764CheckValue<IkReal> x1235 = IKatan2WithCheck(IkReal((((gconst14*new_r00))+((gconst14*new_r11)))),IkReal((((gconst14*new_r01))+(((-1.0)*gconst14*x1233)))),IKFAST_ATAN2_MAGTHRESH);
15765if(!x1235.valid){
15766continue;
15767}
15768j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1234.value)))+(x1235.value));
15769sj3array[0]=IKsin(j3array[0]);
15770cj3array[0]=IKcos(j3array[0]);
15771if( j3array[0] > IKPI )
15772{
15773 j3array[0]-=IK2PI;
15774}
15775else if( j3array[0] < -IKPI )
15776{ j3array[0]+=IK2PI;
15777}
15778j3valid[0] = true;
15779for(int ij3 = 0; ij3 < 1; ++ij3)
15780{
15781if( !j3valid[ij3] )
15782{
15783 continue;
15784}
15785_ij3[0] = ij3; _ij3[1] = -1;
15786for(int iij3 = ij3+1; iij3 < 1; ++iij3)
15787{
15788if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
15789{
15790 j3valid[iij3]=false; _ij3[1] = iij3; break;
15791}
15792}
15793j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
15794{
15795IkReal evalcond[8];
15796IkReal x1236=IKsin(j3);
15797IkReal x1237=IKcos(j3);
15798IkReal x1238=(gconst14*x1236);
15799IkReal x1239=((1.0)*x1237);
15800IkReal x1240=(gconst13*x1236);
15801IkReal x1241=(gconst13*x1239);
15802evalcond[0]=(gconst14+((new_r10*x1236))+((new_r00*x1237)));
15803evalcond[1]=(((gconst14*x1237))+x1240+new_r00);
15804evalcond[2]=(gconst13+((new_r00*x1236))+(((-1.0)*new_r10*x1239)));
15805evalcond[3]=(gconst14+((new_r01*x1236))+(((-1.0)*new_r11*x1239)));
15806evalcond[4]=(x1238+(((-1.0)*x1241))+new_r01);
15807evalcond[5]=(x1238+(((-1.0)*x1241))+new_r10);
15808evalcond[6]=(((new_r01*x1237))+((new_r11*x1236))+(((-1.0)*gconst13)));
15809evalcond[7]=((((-1.0)*x1240))+new_r11+(((-1.0)*gconst14*x1239)));
15810if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
15811{
15812continue;
15813}
15814}
15815
15816{
15817std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15818vinfos[0].jointtype = 1;
15819vinfos[0].foffset = j0;
15820vinfos[0].indices[0] = _ij0[0];
15821vinfos[0].indices[1] = _ij0[1];
15822vinfos[0].maxsolutions = _nj0;
15823vinfos[1].jointtype = 1;
15824vinfos[1].foffset = j1;
15825vinfos[1].indices[0] = _ij1[0];
15826vinfos[1].indices[1] = _ij1[1];
15827vinfos[1].maxsolutions = _nj1;
15828vinfos[2].jointtype = 1;
15829vinfos[2].foffset = j2;
15830vinfos[2].indices[0] = _ij2[0];
15831vinfos[2].indices[1] = _ij2[1];
15832vinfos[2].maxsolutions = _nj2;
15833vinfos[3].jointtype = 1;
15834vinfos[3].foffset = j3;
15835vinfos[3].indices[0] = _ij3[0];
15836vinfos[3].indices[1] = _ij3[1];
15837vinfos[3].maxsolutions = _nj3;
15838vinfos[4].jointtype = 1;
15839vinfos[4].foffset = j4;
15840vinfos[4].indices[0] = _ij4[0];
15841vinfos[4].indices[1] = _ij4[1];
15842vinfos[4].maxsolutions = _nj4;
15843vinfos[5].jointtype = 1;
15844vinfos[5].foffset = j5;
15845vinfos[5].indices[0] = _ij5[0];
15846vinfos[5].indices[1] = _ij5[1];
15847vinfos[5].maxsolutions = _nj5;
15848std::vector<int> vfree(0);
15849solutions.AddSolution(vinfos,vfree);
15850}
15851}
15852}
15853
15854}
15855
15856}
15857
15858}
15859} while(0);
15860if( bgotonextstatement )
15861{
15862bool bgotonextstatement = true;
15863do
15864{
15865IkReal x1242=((-1.0)*new_r00);
15866IkReal x1244 = ((new_r10*new_r10)+(new_r00*new_r00));
15867if(IKabs(x1244)==0){
15868continue;
15869}
15870IkReal x1243=pow(x1244,-0.5);
15871CheckValue<IkReal> x1245 = IKatan2WithCheck(IkReal(x1242),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15872if(!x1245.valid){
15873continue;
15874}
15875IkReal gconst15=((3.14159265358979)+(((-1.0)*(x1245.value))));
15876IkReal gconst16=(x1242*x1243);
15877IkReal gconst17=((1.0)*new_r10*x1243);
15878CheckValue<IkReal> x1246 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15879if(!x1246.valid){
15880continue;
15881}
15882evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-3.14159265358979)+(x1246.value)+j5)))), 6.28318530717959)));
15883if( IKabs(evalcond[0]) < 0.0000050000000000 )
15884{
15885bgotonextstatement=false;
15886{
15887IkReal j3eval[3];
15888IkReal x1247=((-1.0)*new_r00);
15889CheckValue<IkReal> x1250 = IKatan2WithCheck(IkReal(x1247),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15890if(!x1250.valid){
15891continue;
15892}
15893IkReal x1248=((1.0)*(x1250.value));
15894IkReal x1249=x1243;
15895sj4=0;
15896cj4=-1.0;
15897j4=3.14159265358979;
15898sj5=gconst16;
15899cj5=gconst17;
15900j5=((3.14159265)+(((-1.0)*x1248)));
15901IkReal gconst15=((3.14159265358979)+(((-1.0)*x1248)));
15902IkReal gconst16=(x1247*x1249);
15903IkReal gconst17=((1.0)*new_r10*x1249);
15904IkReal x1251=new_r10*new_r10;
15905IkReal x1252=(new_r10*new_r11);
15906IkReal x1253=((((-1.0)*new_r00*new_r01))+(((-1.0)*x1252)));
15907IkReal x1254=x1243;
15908IkReal x1255=(new_r10*x1254);
15909j3eval[0]=x1253;
15910j3eval[1]=((IKabs((((new_r01*x1255))+(((-1.0)*x1251*x1254)))))+(IKabs((((new_r00*x1255))+((x1252*x1254))))));
15911j3eval[2]=IKsign(x1253);
15912if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
15913{
15914{
15915IkReal j3eval[1];
15916IkReal x1256=((-1.0)*new_r00);
15917CheckValue<IkReal> x1259 = IKatan2WithCheck(IkReal(x1256),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15918if(!x1259.valid){
15919continue;
15920}
15921IkReal x1257=((1.0)*(x1259.value));
15922IkReal x1258=x1243;
15923sj4=0;
15924cj4=-1.0;
15925j4=3.14159265358979;
15926sj5=gconst16;
15927cj5=gconst17;
15928j5=((3.14159265)+(((-1.0)*x1257)));
15929IkReal gconst15=((3.14159265358979)+(((-1.0)*x1257)));
15930IkReal gconst16=(x1256*x1258);
15931IkReal gconst17=((1.0)*new_r10*x1258);
15932IkReal x1260=new_r10*new_r10;
15933IkReal x1261=new_r00*new_r00*new_r00;
15934CheckValue<IkReal> x1265=IKPowWithIntegerCheck((x1260+(new_r00*new_r00)),-1);
15935if(!x1265.valid){
15936continue;
15937}
15938IkReal x1262=x1265.value;
15939IkReal x1263=(x1260*x1262);
15940IkReal x1264=(x1261*x1262);
15941j3eval[0]=((IKabs((((new_r00*new_r10*x1262))+((new_r01*x1264))+((new_r00*new_r01*x1263)))))+(IKabs((x1263+((new_r11*x1264))+((new_r00*new_r11*x1263))))));
15942if( IKabs(j3eval[0]) < 0.0000010000000000 )
15943{
15944{
15945IkReal j3eval[1];
15946IkReal x1266=((-1.0)*new_r00);
15947CheckValue<IkReal> x1269 = IKatan2WithCheck(IkReal(x1266),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15948if(!x1269.valid){
15949continue;
15950}
15951IkReal x1267=((1.0)*(x1269.value));
15952IkReal x1268=x1243;
15953sj4=0;
15954cj4=-1.0;
15955j4=3.14159265358979;
15956sj5=gconst16;
15957cj5=gconst17;
15958j5=((3.14159265)+(((-1.0)*x1267)));
15959IkReal gconst15=((3.14159265358979)+(((-1.0)*x1267)));
15960IkReal gconst16=(x1266*x1268);
15961IkReal gconst17=((1.0)*new_r10*x1268);
15962IkReal x1270=new_r10*new_r10;
15963IkReal x1271=new_r00*new_r00;
15964CheckValue<IkReal> x1275=IKPowWithIntegerCheck((x1270+x1271),-1);
15965if(!x1275.valid){
15966continue;
15967}
15968IkReal x1272=x1275.value;
15969IkReal x1273=(new_r10*x1272);
15970IkReal x1274=(x1270*x1272);
15971j3eval[0]=((IKabs(((((-1.0)*x1271*x1274))+x1274+(((-1.0)*x1272*(x1271*x1271))))))+(IKabs((((new_r00*x1273))+((x1273*(new_r00*new_r00*new_r00)))+((new_r00*x1273*(new_r10*new_r10)))))));
15972if( IKabs(j3eval[0]) < 0.0000010000000000 )
15973{
15974{
15975IkReal evalcond[2];
15976bool bgotonextstatement = true;
15977do
15978{
15979evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
15980if( IKabs(evalcond[0]) < 0.0000050000000000 )
15981{
15982bgotonextstatement=false;
15983{
15984IkReal j3eval[1];
15985CheckValue<IkReal> x1277 = IKatan2WithCheck(IkReal(0),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
15986if(!x1277.valid){
15987continue;
15988}
15989IkReal x1276=((1.0)*(x1277.value));
15990sj4=0;
15991cj4=-1.0;
15992j4=3.14159265358979;
15993sj5=gconst16;
15994cj5=gconst17;
15995j5=((3.14159265)+(((-1.0)*x1276)));
15996new_r11=0;
15997new_r00=0;
15998IkReal gconst15=((3.14159265358979)+(((-1.0)*x1276)));
15999IkReal gconst16=0;
16000IkReal x1278 = new_r10*new_r10;
16001if(IKabs(x1278)==0){
16002continue;
16003}
16004IkReal gconst17=((1.0)*new_r10*(pow(x1278,-0.5)));
16005j3eval[0]=new_r01;
16006if( IKabs(j3eval[0]) < 0.0000010000000000 )
16007{
16008{
16009IkReal j3array[2], cj3array[2], sj3array[2];
16010bool j3valid[2]={false};
16011_nj3 = 2;
16012CheckValue<IkReal> x1279=IKPowWithIntegerCheck(gconst17,-1);
16013if(!x1279.valid){
16014continue;
16015}
16016sj3array[0]=((-1.0)*new_r01*(x1279.value));
16017if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16018{
16019 j3valid[0] = j3valid[1] = true;
16020 j3array[0] = IKasin(sj3array[0]);
16021 cj3array[0] = IKcos(j3array[0]);
16022 sj3array[1] = sj3array[0];
16023 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16024 cj3array[1] = -cj3array[0];
16025}
16026else if( isnan(sj3array[0]) )
16027{
16028 // probably any value will work
16029 j3valid[0] = true;
16030 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16031}
16032for(int ij3 = 0; ij3 < 2; ++ij3)
16033{
16034if( !j3valid[ij3] )
16035{
16036 continue;
16037}
16038_ij3[0] = ij3; _ij3[1] = -1;
16039for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16040{
16041if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16042{
16043 j3valid[iij3]=false; _ij3[1] = iij3; break;
16044}
16045}
16046j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16047{
16048IkReal evalcond[6];
16049IkReal x1280=IKcos(j3);
16050IkReal x1281=IKsin(j3);
16051evalcond[0]=(new_r01*x1280);
16052evalcond[1]=(gconst17*x1280);
16053evalcond[2]=((-1.0)*new_r10*x1280);
16054evalcond[3]=(gconst17+((new_r01*x1281)));
16055evalcond[4]=(((new_r10*x1281))+gconst17);
16056evalcond[5]=(new_r10+((gconst17*x1281)));
16057if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16058{
16059continue;
16060}
16061}
16062
16063{
16064std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16065vinfos[0].jointtype = 1;
16066vinfos[0].foffset = j0;
16067vinfos[0].indices[0] = _ij0[0];
16068vinfos[0].indices[1] = _ij0[1];
16069vinfos[0].maxsolutions = _nj0;
16070vinfos[1].jointtype = 1;
16071vinfos[1].foffset = j1;
16072vinfos[1].indices[0] = _ij1[0];
16073vinfos[1].indices[1] = _ij1[1];
16074vinfos[1].maxsolutions = _nj1;
16075vinfos[2].jointtype = 1;
16076vinfos[2].foffset = j2;
16077vinfos[2].indices[0] = _ij2[0];
16078vinfos[2].indices[1] = _ij2[1];
16079vinfos[2].maxsolutions = _nj2;
16080vinfos[3].jointtype = 1;
16081vinfos[3].foffset = j3;
16082vinfos[3].indices[0] = _ij3[0];
16083vinfos[3].indices[1] = _ij3[1];
16084vinfos[3].maxsolutions = _nj3;
16085vinfos[4].jointtype = 1;
16086vinfos[4].foffset = j4;
16087vinfos[4].indices[0] = _ij4[0];
16088vinfos[4].indices[1] = _ij4[1];
16089vinfos[4].maxsolutions = _nj4;
16090vinfos[5].jointtype = 1;
16091vinfos[5].foffset = j5;
16092vinfos[5].indices[0] = _ij5[0];
16093vinfos[5].indices[1] = _ij5[1];
16094vinfos[5].maxsolutions = _nj5;
16095std::vector<int> vfree(0);
16096solutions.AddSolution(vinfos,vfree);
16097}
16098}
16099}
16100
16101} else
16102{
16103{
16104IkReal j3array[2], cj3array[2], sj3array[2];
16105bool j3valid[2]={false};
16106_nj3 = 2;
16107CheckValue<IkReal> x1282=IKPowWithIntegerCheck(new_r01,-1);
16108if(!x1282.valid){
16109continue;
16110}
16111sj3array[0]=((-1.0)*gconst17*(x1282.value));
16112if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16113{
16114 j3valid[0] = j3valid[1] = true;
16115 j3array[0] = IKasin(sj3array[0]);
16116 cj3array[0] = IKcos(j3array[0]);
16117 sj3array[1] = sj3array[0];
16118 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16119 cj3array[1] = -cj3array[0];
16120}
16121else if( isnan(sj3array[0]) )
16122{
16123 // probably any value will work
16124 j3valid[0] = true;
16125 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16126}
16127for(int ij3 = 0; ij3 < 2; ++ij3)
16128{
16129if( !j3valid[ij3] )
16130{
16131 continue;
16132}
16133_ij3[0] = ij3; _ij3[1] = -1;
16134for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16135{
16136if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16137{
16138 j3valid[iij3]=false; _ij3[1] = iij3; break;
16139}
16140}
16141j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16142{
16143IkReal evalcond[6];
16144IkReal x1283=IKcos(j3);
16145IkReal x1284=IKsin(j3);
16146IkReal x1285=(gconst17*x1284);
16147evalcond[0]=(new_r01*x1283);
16148evalcond[1]=(gconst17*x1283);
16149evalcond[2]=((-1.0)*new_r10*x1283);
16150evalcond[3]=(x1285+new_r01);
16151evalcond[4]=(((new_r10*x1284))+gconst17);
16152evalcond[5]=(x1285+new_r10);
16153if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16154{
16155continue;
16156}
16157}
16158
16159{
16160std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16161vinfos[0].jointtype = 1;
16162vinfos[0].foffset = j0;
16163vinfos[0].indices[0] = _ij0[0];
16164vinfos[0].indices[1] = _ij0[1];
16165vinfos[0].maxsolutions = _nj0;
16166vinfos[1].jointtype = 1;
16167vinfos[1].foffset = j1;
16168vinfos[1].indices[0] = _ij1[0];
16169vinfos[1].indices[1] = _ij1[1];
16170vinfos[1].maxsolutions = _nj1;
16171vinfos[2].jointtype = 1;
16172vinfos[2].foffset = j2;
16173vinfos[2].indices[0] = _ij2[0];
16174vinfos[2].indices[1] = _ij2[1];
16175vinfos[2].maxsolutions = _nj2;
16176vinfos[3].jointtype = 1;
16177vinfos[3].foffset = j3;
16178vinfos[3].indices[0] = _ij3[0];
16179vinfos[3].indices[1] = _ij3[1];
16180vinfos[3].maxsolutions = _nj3;
16181vinfos[4].jointtype = 1;
16182vinfos[4].foffset = j4;
16183vinfos[4].indices[0] = _ij4[0];
16184vinfos[4].indices[1] = _ij4[1];
16185vinfos[4].maxsolutions = _nj4;
16186vinfos[5].jointtype = 1;
16187vinfos[5].foffset = j5;
16188vinfos[5].indices[0] = _ij5[0];
16189vinfos[5].indices[1] = _ij5[1];
16190vinfos[5].maxsolutions = _nj5;
16191std::vector<int> vfree(0);
16192solutions.AddSolution(vinfos,vfree);
16193}
16194}
16195}
16196
16197}
16198
16199}
16200
16201}
16202} while(0);
16203if( bgotonextstatement )
16204{
16205bool bgotonextstatement = true;
16206do
16207{
16208evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
16209evalcond[1]=gconst17;
16210if( IKabs(evalcond[0]) < 0.0000050000000000 && IKabs(evalcond[1]) < 0.0000050000000000 )
16211{
16212bgotonextstatement=false;
16213{
16214IkReal j3eval[3];
16215IkReal x1286=((-1.0)*new_r00);
16216CheckValue<IkReal> x1288 = IKatan2WithCheck(IkReal(x1286),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16217if(!x1288.valid){
16218continue;
16219}
16220IkReal x1287=((1.0)*(x1288.value));
16221sj4=0;
16222cj4=-1.0;
16223j4=3.14159265358979;
16224sj5=gconst16;
16225cj5=gconst17;
16226j5=((3.14159265)+(((-1.0)*x1287)));
16227new_r11=0;
16228new_r01=0;
16229new_r22=0;
16230new_r20=0;
16231IkReal gconst15=((3.14159265358979)+(((-1.0)*x1287)));
16232IkReal gconst16=x1286;
16233IkReal gconst17=((1.0)*new_r10);
16234j3eval[0]=1.0;
16235j3eval[1]=1.0;
16236j3eval[2]=((IKabs(((1.0)+(((-1.0)*(new_r10*new_r10))))))+(IKabs(((1.0)*new_r00*new_r10))));
16237if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16238{
16239{
16240IkReal j3eval[3];
16241IkReal x1289=((-1.0)*new_r00);
16242CheckValue<IkReal> x1291 = IKatan2WithCheck(IkReal(x1289),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16243if(!x1291.valid){
16244continue;
16245}
16246IkReal x1290=((1.0)*(x1291.value));
16247sj4=0;
16248cj4=-1.0;
16249j4=3.14159265358979;
16250sj5=gconst16;
16251cj5=gconst17;
16252j5=((3.14159265)+(((-1.0)*x1290)));
16253new_r11=0;
16254new_r01=0;
16255new_r22=0;
16256new_r20=0;
16257IkReal gconst15=((3.14159265358979)+(((-1.0)*x1290)));
16258IkReal gconst16=x1289;
16259IkReal gconst17=((1.0)*new_r10);
16260j3eval[0]=-1.0;
16261j3eval[1]=-1.0;
16262j3eval[2]=((IKabs(((1.0)*new_r00*new_r10)))+(IKabs(((-1.0)+(new_r10*new_r10)))));
16263if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16264{
16265{
16266IkReal j3eval[3];
16267IkReal x1292=((-1.0)*new_r00);
16268CheckValue<IkReal> x1294 = IKatan2WithCheck(IkReal(x1292),IkReal(((-1.0)*new_r10)),IKFAST_ATAN2_MAGTHRESH);
16269if(!x1294.valid){
16270continue;
16271}
16272IkReal x1293=((1.0)*(x1294.value));
16273sj4=0;
16274cj4=-1.0;
16275j4=3.14159265358979;
16276sj5=gconst16;
16277cj5=gconst17;
16278j5=((3.14159265)+(((-1.0)*x1293)));
16279new_r11=0;
16280new_r01=0;
16281new_r22=0;
16282new_r20=0;
16283IkReal gconst15=((3.14159265358979)+(((-1.0)*x1293)));
16284IkReal gconst16=x1292;
16285IkReal gconst17=((1.0)*new_r10);
16286j3eval[0]=-1.0;
16287j3eval[1]=-1.0;
16288j3eval[2]=((IKabs(((-1.0)+(((2.0)*(new_r10*new_r10))))))+(IKabs(((2.0)*new_r00*new_r10))));
16289if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
16290{
16291continue; // 3 cases reached
16292
16293} else
16294{
16295{
16296IkReal j3array[1], cj3array[1], sj3array[1];
16297bool j3valid[1]={false};
16298_nj3 = 1;
16299IkReal x1295=((1.0)*gconst17);
16300CheckValue<IkReal> x1296 = IKatan2WithCheck(IkReal(((((-1.0)*(new_r00*new_r00)))+(gconst17*gconst17))),IkReal(((((-1.0)*gconst16*x1295))+((new_r00*new_r10)))),IKFAST_ATAN2_MAGTHRESH);
16301if(!x1296.valid){
16302continue;
16303}
16304CheckValue<IkReal> x1297=IKPowWithIntegerCheck(IKsign((((gconst16*new_r00))+(((-1.0)*new_r10*x1295)))),-1);
16305if(!x1297.valid){
16306continue;
16307}
16308j3array[0]=((-1.5707963267949)+(x1296.value)+(((1.5707963267949)*(x1297.value))));
16309sj3array[0]=IKsin(j3array[0]);
16310cj3array[0]=IKcos(j3array[0]);
16311if( j3array[0] > IKPI )
16312{
16313 j3array[0]-=IK2PI;
16314}
16315else if( j3array[0] < -IKPI )
16316{ j3array[0]+=IK2PI;
16317}
16318j3valid[0] = true;
16319for(int ij3 = 0; ij3 < 1; ++ij3)
16320{
16321if( !j3valid[ij3] )
16322{
16323 continue;
16324}
16325_ij3[0] = ij3; _ij3[1] = -1;
16326for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16327{
16328if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16329{
16330 j3valid[iij3]=false; _ij3[1] = iij3; break;
16331}
16332}
16333j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16334{
16335IkReal evalcond[6];
16336IkReal x1298=IKsin(j3);
16337IkReal x1299=IKcos(j3);
16338IkReal x1300=(gconst17*x1298);
16339IkReal x1301=(gconst16*x1298);
16340IkReal x1302=(gconst17*x1299);
16341IkReal x1303=((1.0)*x1299);
16342IkReal x1304=(gconst16*x1303);
16343evalcond[0]=(x1300+(((-1.0)*x1304)));
16344evalcond[1]=(gconst17+((new_r10*x1298))+((new_r00*x1299)));
16345evalcond[2]=(x1301+x1302+new_r00);
16346evalcond[3]=((((-1.0)*new_r10*x1303))+gconst16+((new_r00*x1298)));
16347evalcond[4]=((((-1.0)*x1301))+(((-1.0)*x1302)));
16348evalcond[5]=(x1300+(((-1.0)*x1304))+new_r10);
16349if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16350{
16351continue;
16352}
16353}
16354
16355{
16356std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16357vinfos[0].jointtype = 1;
16358vinfos[0].foffset = j0;
16359vinfos[0].indices[0] = _ij0[0];
16360vinfos[0].indices[1] = _ij0[1];
16361vinfos[0].maxsolutions = _nj0;
16362vinfos[1].jointtype = 1;
16363vinfos[1].foffset = j1;
16364vinfos[1].indices[0] = _ij1[0];
16365vinfos[1].indices[1] = _ij1[1];
16366vinfos[1].maxsolutions = _nj1;
16367vinfos[2].jointtype = 1;
16368vinfos[2].foffset = j2;
16369vinfos[2].indices[0] = _ij2[0];
16370vinfos[2].indices[1] = _ij2[1];
16371vinfos[2].maxsolutions = _nj2;
16372vinfos[3].jointtype = 1;
16373vinfos[3].foffset = j3;
16374vinfos[3].indices[0] = _ij3[0];
16375vinfos[3].indices[1] = _ij3[1];
16376vinfos[3].maxsolutions = _nj3;
16377vinfos[4].jointtype = 1;
16378vinfos[4].foffset = j4;
16379vinfos[4].indices[0] = _ij4[0];
16380vinfos[4].indices[1] = _ij4[1];
16381vinfos[4].maxsolutions = _nj4;
16382vinfos[5].jointtype = 1;
16383vinfos[5].foffset = j5;
16384vinfos[5].indices[0] = _ij5[0];
16385vinfos[5].indices[1] = _ij5[1];
16386vinfos[5].maxsolutions = _nj5;
16387std::vector<int> vfree(0);
16388solutions.AddSolution(vinfos,vfree);
16389}
16390}
16391}
16392
16393}
16394
16395}
16396
16397} else
16398{
16399{
16400IkReal j3array[1], cj3array[1], sj3array[1];
16401bool j3valid[1]={false};
16402_nj3 = 1;
16403CheckValue<IkReal> x1305=IKPowWithIntegerCheck(IKsign(((((-1.0)*(gconst17*gconst17)))+(((-1.0)*(gconst16*gconst16))))),-1);
16404if(!x1305.valid){
16405continue;
16406}
16407CheckValue<IkReal> x1306 = IKatan2WithCheck(IkReal((gconst16*new_r00)),IkReal((gconst17*new_r00)),IKFAST_ATAN2_MAGTHRESH);
16408if(!x1306.valid){
16409continue;
16410}
16411j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1305.value)))+(x1306.value));
16412sj3array[0]=IKsin(j3array[0]);
16413cj3array[0]=IKcos(j3array[0]);
16414if( j3array[0] > IKPI )
16415{
16416 j3array[0]-=IK2PI;
16417}
16418else if( j3array[0] < -IKPI )
16419{ j3array[0]+=IK2PI;
16420}
16421j3valid[0] = true;
16422for(int ij3 = 0; ij3 < 1; ++ij3)
16423{
16424if( !j3valid[ij3] )
16425{
16426 continue;
16427}
16428_ij3[0] = ij3; _ij3[1] = -1;
16429for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16430{
16431if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16432{
16433 j3valid[iij3]=false; _ij3[1] = iij3; break;
16434}
16435}
16436j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16437{
16438IkReal evalcond[6];
16439IkReal x1307=IKsin(j3);
16440IkReal x1308=IKcos(j3);
16441IkReal x1309=(gconst17*x1307);
16442IkReal x1310=(gconst16*x1307);
16443IkReal x1311=(gconst17*x1308);
16444IkReal x1312=((1.0)*x1308);
16445IkReal x1313=(gconst16*x1312);
16446evalcond[0]=(x1309+(((-1.0)*x1313)));
16447evalcond[1]=(gconst17+((new_r10*x1307))+((new_r00*x1308)));
16448evalcond[2]=(x1311+x1310+new_r00);
16449evalcond[3]=((((-1.0)*new_r10*x1312))+gconst16+((new_r00*x1307)));
16450evalcond[4]=((((-1.0)*x1310))+(((-1.0)*x1311)));
16451evalcond[5]=(x1309+(((-1.0)*x1313))+new_r10);
16452if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16453{
16454continue;
16455}
16456}
16457
16458{
16459std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16460vinfos[0].jointtype = 1;
16461vinfos[0].foffset = j0;
16462vinfos[0].indices[0] = _ij0[0];
16463vinfos[0].indices[1] = _ij0[1];
16464vinfos[0].maxsolutions = _nj0;
16465vinfos[1].jointtype = 1;
16466vinfos[1].foffset = j1;
16467vinfos[1].indices[0] = _ij1[0];
16468vinfos[1].indices[1] = _ij1[1];
16469vinfos[1].maxsolutions = _nj1;
16470vinfos[2].jointtype = 1;
16471vinfos[2].foffset = j2;
16472vinfos[2].indices[0] = _ij2[0];
16473vinfos[2].indices[1] = _ij2[1];
16474vinfos[2].maxsolutions = _nj2;
16475vinfos[3].jointtype = 1;
16476vinfos[3].foffset = j3;
16477vinfos[3].indices[0] = _ij3[0];
16478vinfos[3].indices[1] = _ij3[1];
16479vinfos[3].maxsolutions = _nj3;
16480vinfos[4].jointtype = 1;
16481vinfos[4].foffset = j4;
16482vinfos[4].indices[0] = _ij4[0];
16483vinfos[4].indices[1] = _ij4[1];
16484vinfos[4].maxsolutions = _nj4;
16485vinfos[5].jointtype = 1;
16486vinfos[5].foffset = j5;
16487vinfos[5].indices[0] = _ij5[0];
16488vinfos[5].indices[1] = _ij5[1];
16489vinfos[5].maxsolutions = _nj5;
16490std::vector<int> vfree(0);
16491solutions.AddSolution(vinfos,vfree);
16492}
16493}
16494}
16495
16496}
16497
16498}
16499
16500} else
16501{
16502{
16503IkReal j3array[1], cj3array[1], sj3array[1];
16504bool j3valid[1]={false};
16505_nj3 = 1;
16506CheckValue<IkReal> x1314=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r00))+((gconst17*new_r10)))),-1);
16507if(!x1314.valid){
16508continue;
16509}
16510CheckValue<IkReal> x1315 = IKatan2WithCheck(IkReal(gconst16*gconst16),IkReal((gconst16*gconst17)),IKFAST_ATAN2_MAGTHRESH);
16511if(!x1315.valid){
16512continue;
16513}
16514j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1314.value)))+(x1315.value));
16515sj3array[0]=IKsin(j3array[0]);
16516cj3array[0]=IKcos(j3array[0]);
16517if( j3array[0] > IKPI )
16518{
16519 j3array[0]-=IK2PI;
16520}
16521else if( j3array[0] < -IKPI )
16522{ j3array[0]+=IK2PI;
16523}
16524j3valid[0] = true;
16525for(int ij3 = 0; ij3 < 1; ++ij3)
16526{
16527if( !j3valid[ij3] )
16528{
16529 continue;
16530}
16531_ij3[0] = ij3; _ij3[1] = -1;
16532for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16533{
16534if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16535{
16536 j3valid[iij3]=false; _ij3[1] = iij3; break;
16537}
16538}
16539j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16540{
16541IkReal evalcond[6];
16542IkReal x1316=IKsin(j3);
16543IkReal x1317=IKcos(j3);
16544IkReal x1318=(gconst17*x1316);
16545IkReal x1319=(gconst16*x1316);
16546IkReal x1320=(gconst17*x1317);
16547IkReal x1321=((1.0)*x1317);
16548IkReal x1322=(gconst16*x1321);
16549evalcond[0]=(x1318+(((-1.0)*x1322)));
16550evalcond[1]=(((new_r00*x1317))+gconst17+((new_r10*x1316)));
16551evalcond[2]=(x1319+x1320+new_r00);
16552evalcond[3]=(((new_r00*x1316))+(((-1.0)*new_r10*x1321))+gconst16);
16553evalcond[4]=((((-1.0)*x1319))+(((-1.0)*x1320)));
16554evalcond[5]=(x1318+(((-1.0)*x1322))+new_r10);
16555if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16556{
16557continue;
16558}
16559}
16560
16561{
16562std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16563vinfos[0].jointtype = 1;
16564vinfos[0].foffset = j0;
16565vinfos[0].indices[0] = _ij0[0];
16566vinfos[0].indices[1] = _ij0[1];
16567vinfos[0].maxsolutions = _nj0;
16568vinfos[1].jointtype = 1;
16569vinfos[1].foffset = j1;
16570vinfos[1].indices[0] = _ij1[0];
16571vinfos[1].indices[1] = _ij1[1];
16572vinfos[1].maxsolutions = _nj1;
16573vinfos[2].jointtype = 1;
16574vinfos[2].foffset = j2;
16575vinfos[2].indices[0] = _ij2[0];
16576vinfos[2].indices[1] = _ij2[1];
16577vinfos[2].maxsolutions = _nj2;
16578vinfos[3].jointtype = 1;
16579vinfos[3].foffset = j3;
16580vinfos[3].indices[0] = _ij3[0];
16581vinfos[3].indices[1] = _ij3[1];
16582vinfos[3].maxsolutions = _nj3;
16583vinfos[4].jointtype = 1;
16584vinfos[4].foffset = j4;
16585vinfos[4].indices[0] = _ij4[0];
16586vinfos[4].indices[1] = _ij4[1];
16587vinfos[4].maxsolutions = _nj4;
16588vinfos[5].jointtype = 1;
16589vinfos[5].foffset = j5;
16590vinfos[5].indices[0] = _ij5[0];
16591vinfos[5].indices[1] = _ij5[1];
16592vinfos[5].maxsolutions = _nj5;
16593std::vector<int> vfree(0);
16594solutions.AddSolution(vinfos,vfree);
16595}
16596}
16597}
16598
16599}
16600
16601}
16602
16603}
16604} while(0);
16605if( bgotonextstatement )
16606{
16607bool bgotonextstatement = true;
16608do
16609{
16610evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
16611if( IKabs(evalcond[0]) < 0.0000050000000000 )
16612{
16613bgotonextstatement=false;
16614{
16615IkReal j3array[2], cj3array[2], sj3array[2];
16616bool j3valid[2]={false};
16617_nj3 = 2;
16618CheckValue<IkReal> x1323=IKPowWithIntegerCheck(gconst16,-1);
16619if(!x1323.valid){
16620continue;
16621}
16622sj3array[0]=(new_r11*(x1323.value));
16623if( sj3array[0] >= -1-IKFAST_SINCOS_THRESH && sj3array[0] <= 1+IKFAST_SINCOS_THRESH )
16624{
16625 j3valid[0] = j3valid[1] = true;
16626 j3array[0] = IKasin(sj3array[0]);
16627 cj3array[0] = IKcos(j3array[0]);
16628 sj3array[1] = sj3array[0];
16629 j3array[1] = j3array[0] > 0 ? (IKPI-j3array[0]) : (-IKPI-j3array[0]);
16630 cj3array[1] = -cj3array[0];
16631}
16632else if( isnan(sj3array[0]) )
16633{
16634 // probably any value will work
16635 j3valid[0] = true;
16636 cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
16637}
16638for(int ij3 = 0; ij3 < 2; ++ij3)
16639{
16640if( !j3valid[ij3] )
16641{
16642 continue;
16643}
16644_ij3[0] = ij3; _ij3[1] = -1;
16645for(int iij3 = ij3+1; iij3 < 2; ++iij3)
16646{
16647if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16648{
16649 j3valid[iij3]=false; _ij3[1] = iij3; break;
16650}
16651}
16652j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16653{
16654IkReal evalcond[6];
16655IkReal x1324=IKcos(j3);
16656IkReal x1325=IKsin(j3);
16657IkReal x1326=((-1.0)*x1324);
16658evalcond[0]=(new_r00*x1324);
16659evalcond[1]=(new_r11*x1326);
16660evalcond[2]=(gconst16*x1326);
16661evalcond[3]=(((new_r00*x1325))+gconst16);
16662evalcond[4]=(((gconst16*x1325))+new_r00);
16663evalcond[5]=(((new_r11*x1325))+(((-1.0)*gconst16)));
16664if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH )
16665{
16666continue;
16667}
16668}
16669
16670{
16671std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16672vinfos[0].jointtype = 1;
16673vinfos[0].foffset = j0;
16674vinfos[0].indices[0] = _ij0[0];
16675vinfos[0].indices[1] = _ij0[1];
16676vinfos[0].maxsolutions = _nj0;
16677vinfos[1].jointtype = 1;
16678vinfos[1].foffset = j1;
16679vinfos[1].indices[0] = _ij1[0];
16680vinfos[1].indices[1] = _ij1[1];
16681vinfos[1].maxsolutions = _nj1;
16682vinfos[2].jointtype = 1;
16683vinfos[2].foffset = j2;
16684vinfos[2].indices[0] = _ij2[0];
16685vinfos[2].indices[1] = _ij2[1];
16686vinfos[2].maxsolutions = _nj2;
16687vinfos[3].jointtype = 1;
16688vinfos[3].foffset = j3;
16689vinfos[3].indices[0] = _ij3[0];
16690vinfos[3].indices[1] = _ij3[1];
16691vinfos[3].maxsolutions = _nj3;
16692vinfos[4].jointtype = 1;
16693vinfos[4].foffset = j4;
16694vinfos[4].indices[0] = _ij4[0];
16695vinfos[4].indices[1] = _ij4[1];
16696vinfos[4].maxsolutions = _nj4;
16697vinfos[5].jointtype = 1;
16698vinfos[5].foffset = j5;
16699vinfos[5].indices[0] = _ij5[0];
16700vinfos[5].indices[1] = _ij5[1];
16701vinfos[5].maxsolutions = _nj5;
16702std::vector<int> vfree(0);
16703solutions.AddSolution(vinfos,vfree);
16704}
16705}
16706}
16707
16708}
16709} while(0);
16710if( bgotonextstatement )
16711{
16712bool bgotonextstatement = true;
16713do
16714{
16715evalcond[0]=((IKabs(new_r11))+(IKabs(new_r10)));
16716if( IKabs(evalcond[0]) < 0.0000050000000000 )
16717{
16718bgotonextstatement=false;
16719{
16720IkReal j3eval[1];
16721IkReal x1327=((-1.0)*new_r00);
16722CheckValue<IkReal> x1329 = IKatan2WithCheck(IkReal(x1327),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16723if(!x1329.valid){
16724continue;
16725}
16726IkReal x1328=((1.0)*(x1329.value));
16727sj4=0;
16728cj4=-1.0;
16729j4=3.14159265358979;
16730sj5=gconst16;
16731cj5=gconst17;
16732j5=((3.14159265)+(((-1.0)*x1328)));
16733new_r11=0;
16734new_r10=0;
16735new_r22=0;
16736new_r02=0;
16737IkReal gconst15=((3.14159265358979)+(((-1.0)*x1328)));
16738IkReal x1330 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16739if(IKabs(x1330)==0){
16740continue;
16741}
16742IkReal gconst16=(x1327*(pow(x1330,-0.5)));
16743IkReal gconst17=0;
16744j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
16745if( IKabs(j3eval[0]) < 0.0000010000000000 )
16746{
16747{
16748IkReal j3eval[1];
16749IkReal x1331=((-1.0)*new_r00);
16750CheckValue<IkReal> x1333 = IKatan2WithCheck(IkReal(x1331),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16751if(!x1333.valid){
16752continue;
16753}
16754IkReal x1332=((1.0)*(x1333.value));
16755sj4=0;
16756cj4=-1.0;
16757j4=3.14159265358979;
16758sj5=gconst16;
16759cj5=gconst17;
16760j5=((3.14159265)+(((-1.0)*x1332)));
16761new_r11=0;
16762new_r10=0;
16763new_r22=0;
16764new_r02=0;
16765IkReal gconst15=((3.14159265358979)+(((-1.0)*x1332)));
16766IkReal x1334 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16767if(IKabs(x1334)==0){
16768continue;
16769}
16770IkReal gconst16=(x1331*(pow(x1334,-0.5)));
16771IkReal gconst17=0;
16772j3eval[0]=new_r00;
16773if( IKabs(j3eval[0]) < 0.0000010000000000 )
16774{
16775{
16776IkReal j3eval[2];
16777IkReal x1335=((-1.0)*new_r00);
16778CheckValue<IkReal> x1337 = IKatan2WithCheck(IkReal(x1335),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
16779if(!x1337.valid){
16780continue;
16781}
16782IkReal x1336=((1.0)*(x1337.value));
16783sj4=0;
16784cj4=-1.0;
16785j4=3.14159265358979;
16786sj5=gconst16;
16787cj5=gconst17;
16788j5=((3.14159265)+(((-1.0)*x1336)));
16789new_r11=0;
16790new_r10=0;
16791new_r22=0;
16792new_r02=0;
16793IkReal gconst15=((3.14159265358979)+(((-1.0)*x1336)));
16794IkReal x1338 = ((1.0)+(((-1.0)*(new_r01*new_r01))));
16795if(IKabs(x1338)==0){
16796continue;
16797}
16798IkReal gconst16=(x1335*(pow(x1338,-0.5)));
16799IkReal gconst17=0;
16800j3eval[0]=new_r00;
16801j3eval[1]=new_r01;
16802if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
16803{
16804continue; // 3 cases reached
16805
16806} else
16807{
16808{
16809IkReal j3array[1], cj3array[1], sj3array[1];
16810bool j3valid[1]={false};
16811_nj3 = 1;
16812CheckValue<IkReal> x1339=IKPowWithIntegerCheck(new_r00,-1);
16813if(!x1339.valid){
16814continue;
16815}
16816CheckValue<IkReal> x1340=IKPowWithIntegerCheck(new_r01,-1);
16817if(!x1340.valid){
16818continue;
16819}
16820if( IKabs(((-1.0)*gconst16*(x1339.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((gconst16*(x1340.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst16*(x1339.value)))+IKsqr((gconst16*(x1340.value)))-1) <= IKFAST_SINCOS_THRESH )
16821 continue;
16822j3array[0]=IKatan2(((-1.0)*gconst16*(x1339.value)), (gconst16*(x1340.value)));
16823sj3array[0]=IKsin(j3array[0]);
16824cj3array[0]=IKcos(j3array[0]);
16825if( j3array[0] > IKPI )
16826{
16827 j3array[0]-=IK2PI;
16828}
16829else if( j3array[0] < -IKPI )
16830{ j3array[0]+=IK2PI;
16831}
16832j3valid[0] = true;
16833for(int ij3 = 0; ij3 < 1; ++ij3)
16834{
16835if( !j3valid[ij3] )
16836{
16837 continue;
16838}
16839_ij3[0] = ij3; _ij3[1] = -1;
16840for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16841{
16842if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16843{
16844 j3valid[iij3]=false; _ij3[1] = iij3; break;
16845}
16846}
16847j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16848{
16849IkReal evalcond[8];
16850IkReal x1341=IKsin(j3);
16851IkReal x1342=IKcos(j3);
16852IkReal x1343=((1.0)*gconst16);
16853IkReal x1344=((-1.0)*gconst16);
16854evalcond[0]=(new_r01*x1341);
16855evalcond[1]=(new_r00*x1342);
16856evalcond[2]=(x1341*x1344);
16857evalcond[3]=(x1342*x1344);
16858evalcond[4]=(gconst16+((new_r00*x1341)));
16859evalcond[5]=(((gconst16*x1341))+new_r00);
16860evalcond[6]=((((-1.0)*x1342*x1343))+new_r01);
16861evalcond[7]=((((-1.0)*x1343))+((new_r01*x1342)));
16862if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
16863{
16864continue;
16865}
16866}
16867
16868{
16869std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16870vinfos[0].jointtype = 1;
16871vinfos[0].foffset = j0;
16872vinfos[0].indices[0] = _ij0[0];
16873vinfos[0].indices[1] = _ij0[1];
16874vinfos[0].maxsolutions = _nj0;
16875vinfos[1].jointtype = 1;
16876vinfos[1].foffset = j1;
16877vinfos[1].indices[0] = _ij1[0];
16878vinfos[1].indices[1] = _ij1[1];
16879vinfos[1].maxsolutions = _nj1;
16880vinfos[2].jointtype = 1;
16881vinfos[2].foffset = j2;
16882vinfos[2].indices[0] = _ij2[0];
16883vinfos[2].indices[1] = _ij2[1];
16884vinfos[2].maxsolutions = _nj2;
16885vinfos[3].jointtype = 1;
16886vinfos[3].foffset = j3;
16887vinfos[3].indices[0] = _ij3[0];
16888vinfos[3].indices[1] = _ij3[1];
16889vinfos[3].maxsolutions = _nj3;
16890vinfos[4].jointtype = 1;
16891vinfos[4].foffset = j4;
16892vinfos[4].indices[0] = _ij4[0];
16893vinfos[4].indices[1] = _ij4[1];
16894vinfos[4].maxsolutions = _nj4;
16895vinfos[5].jointtype = 1;
16896vinfos[5].foffset = j5;
16897vinfos[5].indices[0] = _ij5[0];
16898vinfos[5].indices[1] = _ij5[1];
16899vinfos[5].maxsolutions = _nj5;
16900std::vector<int> vfree(0);
16901solutions.AddSolution(vinfos,vfree);
16902}
16903}
16904}
16905
16906}
16907
16908}
16909
16910} else
16911{
16912{
16913IkReal j3array[1], cj3array[1], sj3array[1];
16914bool j3valid[1]={false};
16915_nj3 = 1;
16916CheckValue<IkReal> x1345=IKPowWithIntegerCheck(new_r00,-1);
16917if(!x1345.valid){
16918continue;
16919}
16920CheckValue<IkReal> x1346=IKPowWithIntegerCheck(gconst16,-1);
16921if(!x1346.valid){
16922continue;
16923}
16924if( IKabs(((-1.0)*gconst16*(x1345.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1346.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst16*(x1345.value)))+IKsqr((new_r01*(x1346.value)))-1) <= IKFAST_SINCOS_THRESH )
16925 continue;
16926j3array[0]=IKatan2(((-1.0)*gconst16*(x1345.value)), (new_r01*(x1346.value)));
16927sj3array[0]=IKsin(j3array[0]);
16928cj3array[0]=IKcos(j3array[0]);
16929if( j3array[0] > IKPI )
16930{
16931 j3array[0]-=IK2PI;
16932}
16933else if( j3array[0] < -IKPI )
16934{ j3array[0]+=IK2PI;
16935}
16936j3valid[0] = true;
16937for(int ij3 = 0; ij3 < 1; ++ij3)
16938{
16939if( !j3valid[ij3] )
16940{
16941 continue;
16942}
16943_ij3[0] = ij3; _ij3[1] = -1;
16944for(int iij3 = ij3+1; iij3 < 1; ++iij3)
16945{
16946if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
16947{
16948 j3valid[iij3]=false; _ij3[1] = iij3; break;
16949}
16950}
16951j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
16952{
16953IkReal evalcond[8];
16954IkReal x1347=IKsin(j3);
16955IkReal x1348=IKcos(j3);
16956IkReal x1349=((1.0)*gconst16);
16957IkReal x1350=((-1.0)*gconst16);
16958evalcond[0]=(new_r01*x1347);
16959evalcond[1]=(new_r00*x1348);
16960evalcond[2]=(x1347*x1350);
16961evalcond[3]=(x1348*x1350);
16962evalcond[4]=(gconst16+((new_r00*x1347)));
16963evalcond[5]=(((gconst16*x1347))+new_r00);
16964evalcond[6]=((((-1.0)*x1348*x1349))+new_r01);
16965evalcond[7]=((((-1.0)*x1349))+((new_r01*x1348)));
16966if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
16967{
16968continue;
16969}
16970}
16971
16972{
16973std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16974vinfos[0].jointtype = 1;
16975vinfos[0].foffset = j0;
16976vinfos[0].indices[0] = _ij0[0];
16977vinfos[0].indices[1] = _ij0[1];
16978vinfos[0].maxsolutions = _nj0;
16979vinfos[1].jointtype = 1;
16980vinfos[1].foffset = j1;
16981vinfos[1].indices[0] = _ij1[0];
16982vinfos[1].indices[1] = _ij1[1];
16983vinfos[1].maxsolutions = _nj1;
16984vinfos[2].jointtype = 1;
16985vinfos[2].foffset = j2;
16986vinfos[2].indices[0] = _ij2[0];
16987vinfos[2].indices[1] = _ij2[1];
16988vinfos[2].maxsolutions = _nj2;
16989vinfos[3].jointtype = 1;
16990vinfos[3].foffset = j3;
16991vinfos[3].indices[0] = _ij3[0];
16992vinfos[3].indices[1] = _ij3[1];
16993vinfos[3].maxsolutions = _nj3;
16994vinfos[4].jointtype = 1;
16995vinfos[4].foffset = j4;
16996vinfos[4].indices[0] = _ij4[0];
16997vinfos[4].indices[1] = _ij4[1];
16998vinfos[4].maxsolutions = _nj4;
16999vinfos[5].jointtype = 1;
17000vinfos[5].foffset = j5;
17001vinfos[5].indices[0] = _ij5[0];
17002vinfos[5].indices[1] = _ij5[1];
17003vinfos[5].maxsolutions = _nj5;
17004std::vector<int> vfree(0);
17005solutions.AddSolution(vinfos,vfree);
17006}
17007}
17008}
17009
17010}
17011
17012}
17013
17014} else
17015{
17016{
17017IkReal j3array[1], cj3array[1], sj3array[1];
17018bool j3valid[1]={false};
17019_nj3 = 1;
17020CheckValue<IkReal> x1351 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17021if(!x1351.valid){
17022continue;
17023}
17024CheckValue<IkReal> x1352=IKPowWithIntegerCheck(IKsign(gconst16),-1);
17025if(!x1352.valid){
17026continue;
17027}
17028j3array[0]=((-1.5707963267949)+(x1351.value)+(((1.5707963267949)*(x1352.value))));
17029sj3array[0]=IKsin(j3array[0]);
17030cj3array[0]=IKcos(j3array[0]);
17031if( j3array[0] > IKPI )
17032{
17033 j3array[0]-=IK2PI;
17034}
17035else if( j3array[0] < -IKPI )
17036{ j3array[0]+=IK2PI;
17037}
17038j3valid[0] = true;
17039for(int ij3 = 0; ij3 < 1; ++ij3)
17040{
17041if( !j3valid[ij3] )
17042{
17043 continue;
17044}
17045_ij3[0] = ij3; _ij3[1] = -1;
17046for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17047{
17048if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17049{
17050 j3valid[iij3]=false; _ij3[1] = iij3; break;
17051}
17052}
17053j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17054{
17055IkReal evalcond[8];
17056IkReal x1353=IKsin(j3);
17057IkReal x1354=IKcos(j3);
17058IkReal x1355=((1.0)*gconst16);
17059IkReal x1356=((-1.0)*gconst16);
17060evalcond[0]=(new_r01*x1353);
17061evalcond[1]=(new_r00*x1354);
17062evalcond[2]=(x1353*x1356);
17063evalcond[3]=(x1354*x1356);
17064evalcond[4]=(gconst16+((new_r00*x1353)));
17065evalcond[5]=(new_r00+((gconst16*x1353)));
17066evalcond[6]=((((-1.0)*x1354*x1355))+new_r01);
17067evalcond[7]=((((-1.0)*x1355))+((new_r01*x1354)));
17068if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17069{
17070continue;
17071}
17072}
17073
17074{
17075std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17076vinfos[0].jointtype = 1;
17077vinfos[0].foffset = j0;
17078vinfos[0].indices[0] = _ij0[0];
17079vinfos[0].indices[1] = _ij0[1];
17080vinfos[0].maxsolutions = _nj0;
17081vinfos[1].jointtype = 1;
17082vinfos[1].foffset = j1;
17083vinfos[1].indices[0] = _ij1[0];
17084vinfos[1].indices[1] = _ij1[1];
17085vinfos[1].maxsolutions = _nj1;
17086vinfos[2].jointtype = 1;
17087vinfos[2].foffset = j2;
17088vinfos[2].indices[0] = _ij2[0];
17089vinfos[2].indices[1] = _ij2[1];
17090vinfos[2].maxsolutions = _nj2;
17091vinfos[3].jointtype = 1;
17092vinfos[3].foffset = j3;
17093vinfos[3].indices[0] = _ij3[0];
17094vinfos[3].indices[1] = _ij3[1];
17095vinfos[3].maxsolutions = _nj3;
17096vinfos[4].jointtype = 1;
17097vinfos[4].foffset = j4;
17098vinfos[4].indices[0] = _ij4[0];
17099vinfos[4].indices[1] = _ij4[1];
17100vinfos[4].maxsolutions = _nj4;
17101vinfos[5].jointtype = 1;
17102vinfos[5].foffset = j5;
17103vinfos[5].indices[0] = _ij5[0];
17104vinfos[5].indices[1] = _ij5[1];
17105vinfos[5].maxsolutions = _nj5;
17106std::vector<int> vfree(0);
17107solutions.AddSolution(vinfos,vfree);
17108}
17109}
17110}
17111
17112}
17113
17114}
17115
17116}
17117} while(0);
17118if( bgotonextstatement )
17119{
17120bool bgotonextstatement = true;
17121do
17122{
17123evalcond[0]=IKabs(new_r10);
17124if( IKabs(evalcond[0]) < 0.0000050000000000 )
17125{
17126bgotonextstatement=false;
17127{
17128IkReal j3eval[1];
17129IkReal x1357=((-1.0)*new_r00);
17130CheckValue<IkReal> x1359 = IKatan2WithCheck(IkReal(x1357),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17131if(!x1359.valid){
17132continue;
17133}
17134IkReal x1358=((1.0)*(x1359.value));
17135sj4=0;
17136cj4=-1.0;
17137j4=3.14159265358979;
17138sj5=gconst16;
17139cj5=gconst17;
17140j5=((3.14159265)+(((-1.0)*x1358)));
17141new_r10=0;
17142IkReal gconst15=((3.14159265358979)+(((-1.0)*x1358)));
17143IkReal x1360 = new_r00*new_r00;
17144if(IKabs(x1360)==0){
17145continue;
17146}
17147IkReal gconst16=(x1357*(pow(x1360,-0.5)));
17148IkReal gconst17=0;
17149j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
17150if( IKabs(j3eval[0]) < 0.0000010000000000 )
17151{
17152{
17153IkReal j3eval[1];
17154IkReal x1361=((-1.0)*new_r00);
17155CheckValue<IkReal> x1363 = IKatan2WithCheck(IkReal(x1361),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17156if(!x1363.valid){
17157continue;
17158}
17159IkReal x1362=((1.0)*(x1363.value));
17160sj4=0;
17161cj4=-1.0;
17162j4=3.14159265358979;
17163sj5=gconst16;
17164cj5=gconst17;
17165j5=((3.14159265)+(((-1.0)*x1362)));
17166new_r10=0;
17167IkReal gconst15=((3.14159265358979)+(((-1.0)*x1362)));
17168IkReal x1364 = new_r00*new_r00;
17169if(IKabs(x1364)==0){
17170continue;
17171}
17172IkReal gconst16=(x1361*(pow(x1364,-0.5)));
17173IkReal gconst17=0;
17174j3eval[0]=((IKabs(new_r00))+(IKabs(new_r01)));
17175if( IKabs(j3eval[0]) < 0.0000010000000000 )
17176{
17177{
17178IkReal j3eval[1];
17179IkReal x1365=((-1.0)*new_r00);
17180CheckValue<IkReal> x1367 = IKatan2WithCheck(IkReal(x1365),IkReal(0),IKFAST_ATAN2_MAGTHRESH);
17181if(!x1367.valid){
17182continue;
17183}
17184IkReal x1366=((1.0)*(x1367.value));
17185sj4=0;
17186cj4=-1.0;
17187j4=3.14159265358979;
17188sj5=gconst16;
17189cj5=gconst17;
17190j5=((3.14159265)+(((-1.0)*x1366)));
17191new_r10=0;
17192IkReal gconst15=((3.14159265358979)+(((-1.0)*x1366)));
17193IkReal x1368 = new_r00*new_r00;
17194if(IKabs(x1368)==0){
17195continue;
17196}
17197IkReal gconst16=(x1365*(pow(x1368,-0.5)));
17198IkReal gconst17=0;
17199j3eval[0]=new_r00;
17200if( IKabs(j3eval[0]) < 0.0000010000000000 )
17201{
17202continue; // 3 cases reached
17203
17204} else
17205{
17206{
17207IkReal j3array[1], cj3array[1], sj3array[1];
17208bool j3valid[1]={false};
17209_nj3 = 1;
17210CheckValue<IkReal> x1369=IKPowWithIntegerCheck(new_r00,-1);
17211if(!x1369.valid){
17212continue;
17213}
17214CheckValue<IkReal> x1370=IKPowWithIntegerCheck(gconst16,-1);
17215if(!x1370.valid){
17216continue;
17217}
17218if( IKabs(((-1.0)*gconst16*(x1369.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((new_r01*(x1370.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*gconst16*(x1369.value)))+IKsqr((new_r01*(x1370.value)))-1) <= IKFAST_SINCOS_THRESH )
17219 continue;
17220j3array[0]=IKatan2(((-1.0)*gconst16*(x1369.value)), (new_r01*(x1370.value)));
17221sj3array[0]=IKsin(j3array[0]);
17222cj3array[0]=IKcos(j3array[0]);
17223if( j3array[0] > IKPI )
17224{
17225 j3array[0]-=IK2PI;
17226}
17227else if( j3array[0] < -IKPI )
17228{ j3array[0]+=IK2PI;
17229}
17230j3valid[0] = true;
17231for(int ij3 = 0; ij3 < 1; ++ij3)
17232{
17233if( !j3valid[ij3] )
17234{
17235 continue;
17236}
17237_ij3[0] = ij3; _ij3[1] = -1;
17238for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17239{
17240if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17241{
17242 j3valid[iij3]=false; _ij3[1] = iij3; break;
17243}
17244}
17245j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17246{
17247IkReal evalcond[8];
17248IkReal x1371=IKcos(j3);
17249IkReal x1372=IKsin(j3);
17250IkReal x1373=((1.0)*gconst16);
17251evalcond[0]=(new_r00*x1371);
17252evalcond[1]=((-1.0)*gconst16*x1371);
17253evalcond[2]=(gconst16+((new_r00*x1372)));
17254evalcond[3]=(new_r00+((gconst16*x1372)));
17255evalcond[4]=((((-1.0)*x1371*x1373))+new_r01);
17256evalcond[5]=((((-1.0)*x1372*x1373))+new_r11);
17257evalcond[6]=(((new_r01*x1372))+(((-1.0)*new_r11*x1371)));
17258evalcond[7]=((((-1.0)*x1373))+((new_r11*x1372))+((new_r01*x1371)));
17259if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17260{
17261continue;
17262}
17263}
17264
17265{
17266std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17267vinfos[0].jointtype = 1;
17268vinfos[0].foffset = j0;
17269vinfos[0].indices[0] = _ij0[0];
17270vinfos[0].indices[1] = _ij0[1];
17271vinfos[0].maxsolutions = _nj0;
17272vinfos[1].jointtype = 1;
17273vinfos[1].foffset = j1;
17274vinfos[1].indices[0] = _ij1[0];
17275vinfos[1].indices[1] = _ij1[1];
17276vinfos[1].maxsolutions = _nj1;
17277vinfos[2].jointtype = 1;
17278vinfos[2].foffset = j2;
17279vinfos[2].indices[0] = _ij2[0];
17280vinfos[2].indices[1] = _ij2[1];
17281vinfos[2].maxsolutions = _nj2;
17282vinfos[3].jointtype = 1;
17283vinfos[3].foffset = j3;
17284vinfos[3].indices[0] = _ij3[0];
17285vinfos[3].indices[1] = _ij3[1];
17286vinfos[3].maxsolutions = _nj3;
17287vinfos[4].jointtype = 1;
17288vinfos[4].foffset = j4;
17289vinfos[4].indices[0] = _ij4[0];
17290vinfos[4].indices[1] = _ij4[1];
17291vinfos[4].maxsolutions = _nj4;
17292vinfos[5].jointtype = 1;
17293vinfos[5].foffset = j5;
17294vinfos[5].indices[0] = _ij5[0];
17295vinfos[5].indices[1] = _ij5[1];
17296vinfos[5].maxsolutions = _nj5;
17297std::vector<int> vfree(0);
17298solutions.AddSolution(vinfos,vfree);
17299}
17300}
17301}
17302
17303}
17304
17305}
17306
17307} else
17308{
17309{
17310IkReal j3array[1], cj3array[1], sj3array[1];
17311bool j3valid[1]={false};
17312_nj3 = 1;
17313CheckValue<IkReal> x1374 = IKatan2WithCheck(IkReal(((-1.0)*new_r00)),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17314if(!x1374.valid){
17315continue;
17316}
17317CheckValue<IkReal> x1375=IKPowWithIntegerCheck(IKsign(gconst16),-1);
17318if(!x1375.valid){
17319continue;
17320}
17321j3array[0]=((-1.5707963267949)+(x1374.value)+(((1.5707963267949)*(x1375.value))));
17322sj3array[0]=IKsin(j3array[0]);
17323cj3array[0]=IKcos(j3array[0]);
17324if( j3array[0] > IKPI )
17325{
17326 j3array[0]-=IK2PI;
17327}
17328else if( j3array[0] < -IKPI )
17329{ j3array[0]+=IK2PI;
17330}
17331j3valid[0] = true;
17332for(int ij3 = 0; ij3 < 1; ++ij3)
17333{
17334if( !j3valid[ij3] )
17335{
17336 continue;
17337}
17338_ij3[0] = ij3; _ij3[1] = -1;
17339for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17340{
17341if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17342{
17343 j3valid[iij3]=false; _ij3[1] = iij3; break;
17344}
17345}
17346j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17347{
17348IkReal evalcond[8];
17349IkReal x1376=IKcos(j3);
17350IkReal x1377=IKsin(j3);
17351IkReal x1378=((1.0)*gconst16);
17352evalcond[0]=(new_r00*x1376);
17353evalcond[1]=((-1.0)*gconst16*x1376);
17354evalcond[2]=(gconst16+((new_r00*x1377)));
17355evalcond[3]=(new_r00+((gconst16*x1377)));
17356evalcond[4]=((((-1.0)*x1376*x1378))+new_r01);
17357evalcond[5]=((((-1.0)*x1377*x1378))+new_r11);
17358evalcond[6]=(((new_r01*x1377))+(((-1.0)*new_r11*x1376)));
17359evalcond[7]=((((-1.0)*x1378))+((new_r11*x1377))+((new_r01*x1376)));
17360if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17361{
17362continue;
17363}
17364}
17365
17366{
17367std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17368vinfos[0].jointtype = 1;
17369vinfos[0].foffset = j0;
17370vinfos[0].indices[0] = _ij0[0];
17371vinfos[0].indices[1] = _ij0[1];
17372vinfos[0].maxsolutions = _nj0;
17373vinfos[1].jointtype = 1;
17374vinfos[1].foffset = j1;
17375vinfos[1].indices[0] = _ij1[0];
17376vinfos[1].indices[1] = _ij1[1];
17377vinfos[1].maxsolutions = _nj1;
17378vinfos[2].jointtype = 1;
17379vinfos[2].foffset = j2;
17380vinfos[2].indices[0] = _ij2[0];
17381vinfos[2].indices[1] = _ij2[1];
17382vinfos[2].maxsolutions = _nj2;
17383vinfos[3].jointtype = 1;
17384vinfos[3].foffset = j3;
17385vinfos[3].indices[0] = _ij3[0];
17386vinfos[3].indices[1] = _ij3[1];
17387vinfos[3].maxsolutions = _nj3;
17388vinfos[4].jointtype = 1;
17389vinfos[4].foffset = j4;
17390vinfos[4].indices[0] = _ij4[0];
17391vinfos[4].indices[1] = _ij4[1];
17392vinfos[4].maxsolutions = _nj4;
17393vinfos[5].jointtype = 1;
17394vinfos[5].foffset = j5;
17395vinfos[5].indices[0] = _ij5[0];
17396vinfos[5].indices[1] = _ij5[1];
17397vinfos[5].maxsolutions = _nj5;
17398std::vector<int> vfree(0);
17399solutions.AddSolution(vinfos,vfree);
17400}
17401}
17402}
17403
17404}
17405
17406}
17407
17408} else
17409{
17410{
17411IkReal j3array[1], cj3array[1], sj3array[1];
17412bool j3valid[1]={false};
17413_nj3 = 1;
17414CheckValue<IkReal> x1379=IKPowWithIntegerCheck(IKsign(gconst16),-1);
17415if(!x1379.valid){
17416continue;
17417}
17418CheckValue<IkReal> x1380 = IKatan2WithCheck(IkReal(new_r11),IkReal(new_r01),IKFAST_ATAN2_MAGTHRESH);
17419if(!x1380.valid){
17420continue;
17421}
17422j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1379.value)))+(x1380.value));
17423sj3array[0]=IKsin(j3array[0]);
17424cj3array[0]=IKcos(j3array[0]);
17425if( j3array[0] > IKPI )
17426{
17427 j3array[0]-=IK2PI;
17428}
17429else if( j3array[0] < -IKPI )
17430{ j3array[0]+=IK2PI;
17431}
17432j3valid[0] = true;
17433for(int ij3 = 0; ij3 < 1; ++ij3)
17434{
17435if( !j3valid[ij3] )
17436{
17437 continue;
17438}
17439_ij3[0] = ij3; _ij3[1] = -1;
17440for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17441{
17442if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17443{
17444 j3valid[iij3]=false; _ij3[1] = iij3; break;
17445}
17446}
17447j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17448{
17449IkReal evalcond[8];
17450IkReal x1381=IKcos(j3);
17451IkReal x1382=IKsin(j3);
17452IkReal x1383=((1.0)*gconst16);
17453evalcond[0]=(new_r00*x1381);
17454evalcond[1]=((-1.0)*gconst16*x1381);
17455evalcond[2]=(gconst16+((new_r00*x1382)));
17456evalcond[3]=(((gconst16*x1382))+new_r00);
17457evalcond[4]=((((-1.0)*x1381*x1383))+new_r01);
17458evalcond[5]=((((-1.0)*x1382*x1383))+new_r11);
17459evalcond[6]=((((-1.0)*new_r11*x1381))+((new_r01*x1382)));
17460evalcond[7]=((((-1.0)*x1383))+((new_r11*x1382))+((new_r01*x1381)));
17461if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17462{
17463continue;
17464}
17465}
17466
17467{
17468std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17469vinfos[0].jointtype = 1;
17470vinfos[0].foffset = j0;
17471vinfos[0].indices[0] = _ij0[0];
17472vinfos[0].indices[1] = _ij0[1];
17473vinfos[0].maxsolutions = _nj0;
17474vinfos[1].jointtype = 1;
17475vinfos[1].foffset = j1;
17476vinfos[1].indices[0] = _ij1[0];
17477vinfos[1].indices[1] = _ij1[1];
17478vinfos[1].maxsolutions = _nj1;
17479vinfos[2].jointtype = 1;
17480vinfos[2].foffset = j2;
17481vinfos[2].indices[0] = _ij2[0];
17482vinfos[2].indices[1] = _ij2[1];
17483vinfos[2].maxsolutions = _nj2;
17484vinfos[3].jointtype = 1;
17485vinfos[3].foffset = j3;
17486vinfos[3].indices[0] = _ij3[0];
17487vinfos[3].indices[1] = _ij3[1];
17488vinfos[3].maxsolutions = _nj3;
17489vinfos[4].jointtype = 1;
17490vinfos[4].foffset = j4;
17491vinfos[4].indices[0] = _ij4[0];
17492vinfos[4].indices[1] = _ij4[1];
17493vinfos[4].maxsolutions = _nj4;
17494vinfos[5].jointtype = 1;
17495vinfos[5].foffset = j5;
17496vinfos[5].indices[0] = _ij5[0];
17497vinfos[5].indices[1] = _ij5[1];
17498vinfos[5].maxsolutions = _nj5;
17499std::vector<int> vfree(0);
17500solutions.AddSolution(vinfos,vfree);
17501}
17502}
17503}
17504
17505}
17506
17507}
17508
17509}
17510} while(0);
17511if( bgotonextstatement )
17512{
17513bool bgotonextstatement = true;
17514do
17515{
17516if( 1 )
17517{
17518bgotonextstatement=false;
17519continue; // branch miss [j3]
17520
17521}
17522} while(0);
17523if( bgotonextstatement )
17524{
17525}
17526}
17527}
17528}
17529}
17530}
17531}
17532
17533} else
17534{
17535{
17536IkReal j3array[1], cj3array[1], sj3array[1];
17537bool j3valid[1]={false};
17538_nj3 = 1;
17539IkReal x1384=((1.0)*gconst17);
17540CheckValue<IkReal> x1385=IKPowWithIntegerCheck(IKsign((((gconst16*new_r00))+(((-1.0)*new_r10*x1384)))),-1);
17541if(!x1385.valid){
17542continue;
17543}
17544CheckValue<IkReal> x1386 = IKatan2WithCheck(IkReal(((((-1.0)*(new_r00*new_r00)))+(gconst17*gconst17))),IkReal((((new_r00*new_r10))+(((-1.0)*gconst16*x1384)))),IKFAST_ATAN2_MAGTHRESH);
17545if(!x1386.valid){
17546continue;
17547}
17548j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1385.value)))+(x1386.value));
17549sj3array[0]=IKsin(j3array[0]);
17550cj3array[0]=IKcos(j3array[0]);
17551if( j3array[0] > IKPI )
17552{
17553 j3array[0]-=IK2PI;
17554}
17555else if( j3array[0] < -IKPI )
17556{ j3array[0]+=IK2PI;
17557}
17558j3valid[0] = true;
17559for(int ij3 = 0; ij3 < 1; ++ij3)
17560{
17561if( !j3valid[ij3] )
17562{
17563 continue;
17564}
17565_ij3[0] = ij3; _ij3[1] = -1;
17566for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17567{
17568if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17569{
17570 j3valid[iij3]=false; _ij3[1] = iij3; break;
17571}
17572}
17573j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17574{
17575IkReal evalcond[8];
17576IkReal x1387=IKsin(j3);
17577IkReal x1388=IKcos(j3);
17578IkReal x1389=((1.0)*gconst16);
17579IkReal x1390=(gconst17*x1387);
17580IkReal x1391=(gconst17*x1388);
17581IkReal x1392=((1.0)*x1388);
17582IkReal x1393=(x1388*x1389);
17583evalcond[0]=(((new_r10*x1387))+gconst17+((new_r00*x1388)));
17584evalcond[1]=(x1391+((gconst16*x1387))+new_r00);
17585evalcond[2]=(gconst16+((new_r00*x1387))+(((-1.0)*new_r10*x1392)));
17586evalcond[3]=(gconst17+((new_r01*x1387))+(((-1.0)*new_r11*x1392)));
17587evalcond[4]=(x1390+new_r01+(((-1.0)*x1393)));
17588evalcond[5]=(x1390+new_r10+(((-1.0)*x1393)));
17589evalcond[6]=((((-1.0)*x1389))+((new_r11*x1387))+((new_r01*x1388)));
17590evalcond[7]=((((-1.0)*x1391))+new_r11+(((-1.0)*x1387*x1389)));
17591if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17592{
17593continue;
17594}
17595}
17596
17597{
17598std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17599vinfos[0].jointtype = 1;
17600vinfos[0].foffset = j0;
17601vinfos[0].indices[0] = _ij0[0];
17602vinfos[0].indices[1] = _ij0[1];
17603vinfos[0].maxsolutions = _nj0;
17604vinfos[1].jointtype = 1;
17605vinfos[1].foffset = j1;
17606vinfos[1].indices[0] = _ij1[0];
17607vinfos[1].indices[1] = _ij1[1];
17608vinfos[1].maxsolutions = _nj1;
17609vinfos[2].jointtype = 1;
17610vinfos[2].foffset = j2;
17611vinfos[2].indices[0] = _ij2[0];
17612vinfos[2].indices[1] = _ij2[1];
17613vinfos[2].maxsolutions = _nj2;
17614vinfos[3].jointtype = 1;
17615vinfos[3].foffset = j3;
17616vinfos[3].indices[0] = _ij3[0];
17617vinfos[3].indices[1] = _ij3[1];
17618vinfos[3].maxsolutions = _nj3;
17619vinfos[4].jointtype = 1;
17620vinfos[4].foffset = j4;
17621vinfos[4].indices[0] = _ij4[0];
17622vinfos[4].indices[1] = _ij4[1];
17623vinfos[4].maxsolutions = _nj4;
17624vinfos[5].jointtype = 1;
17625vinfos[5].foffset = j5;
17626vinfos[5].indices[0] = _ij5[0];
17627vinfos[5].indices[1] = _ij5[1];
17628vinfos[5].maxsolutions = _nj5;
17629std::vector<int> vfree(0);
17630solutions.AddSolution(vinfos,vfree);
17631}
17632}
17633}
17634
17635}
17636
17637}
17638
17639} else
17640{
17641{
17642IkReal j3array[1], cj3array[1], sj3array[1];
17643bool j3valid[1]={false};
17644_nj3 = 1;
17645IkReal x1394=((1.0)*gconst17);
17646CheckValue<IkReal> x1395=IKPowWithIntegerCheck(IKsign(((((-1.0)*gconst16*new_r11))+(((-1.0)*new_r01*x1394)))),-1);
17647if(!x1395.valid){
17648continue;
17649}
17650CheckValue<IkReal> x1396 = IKatan2WithCheck(IkReal((((new_r00*new_r11))+(gconst17*gconst17))),IkReal(((((-1.0)*gconst16*x1394))+((new_r00*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
17651if(!x1396.valid){
17652continue;
17653}
17654j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1395.value)))+(x1396.value));
17655sj3array[0]=IKsin(j3array[0]);
17656cj3array[0]=IKcos(j3array[0]);
17657if( j3array[0] > IKPI )
17658{
17659 j3array[0]-=IK2PI;
17660}
17661else if( j3array[0] < -IKPI )
17662{ j3array[0]+=IK2PI;
17663}
17664j3valid[0] = true;
17665for(int ij3 = 0; ij3 < 1; ++ij3)
17666{
17667if( !j3valid[ij3] )
17668{
17669 continue;
17670}
17671_ij3[0] = ij3; _ij3[1] = -1;
17672for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17673{
17674if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17675{
17676 j3valid[iij3]=false; _ij3[1] = iij3; break;
17677}
17678}
17679j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17680{
17681IkReal evalcond[8];
17682IkReal x1397=IKsin(j3);
17683IkReal x1398=IKcos(j3);
17684IkReal x1399=((1.0)*gconst16);
17685IkReal x1400=(gconst17*x1397);
17686IkReal x1401=(gconst17*x1398);
17687IkReal x1402=((1.0)*x1398);
17688IkReal x1403=(x1398*x1399);
17689evalcond[0]=(((new_r10*x1397))+gconst17+((new_r00*x1398)));
17690evalcond[1]=(x1401+new_r00+((gconst16*x1397)));
17691evalcond[2]=(gconst16+((new_r00*x1397))+(((-1.0)*new_r10*x1402)));
17692evalcond[3]=(gconst17+((new_r01*x1397))+(((-1.0)*new_r11*x1402)));
17693evalcond[4]=((((-1.0)*x1403))+x1400+new_r01);
17694evalcond[5]=((((-1.0)*x1403))+x1400+new_r10);
17695evalcond[6]=(((new_r11*x1397))+((new_r01*x1398))+(((-1.0)*x1399)));
17696evalcond[7]=((((-1.0)*x1401))+(((-1.0)*x1397*x1399))+new_r11);
17697if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17698{
17699continue;
17700}
17701}
17702
17703{
17704std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17705vinfos[0].jointtype = 1;
17706vinfos[0].foffset = j0;
17707vinfos[0].indices[0] = _ij0[0];
17708vinfos[0].indices[1] = _ij0[1];
17709vinfos[0].maxsolutions = _nj0;
17710vinfos[1].jointtype = 1;
17711vinfos[1].foffset = j1;
17712vinfos[1].indices[0] = _ij1[0];
17713vinfos[1].indices[1] = _ij1[1];
17714vinfos[1].maxsolutions = _nj1;
17715vinfos[2].jointtype = 1;
17716vinfos[2].foffset = j2;
17717vinfos[2].indices[0] = _ij2[0];
17718vinfos[2].indices[1] = _ij2[1];
17719vinfos[2].maxsolutions = _nj2;
17720vinfos[3].jointtype = 1;
17721vinfos[3].foffset = j3;
17722vinfos[3].indices[0] = _ij3[0];
17723vinfos[3].indices[1] = _ij3[1];
17724vinfos[3].maxsolutions = _nj3;
17725vinfos[4].jointtype = 1;
17726vinfos[4].foffset = j4;
17727vinfos[4].indices[0] = _ij4[0];
17728vinfos[4].indices[1] = _ij4[1];
17729vinfos[4].maxsolutions = _nj4;
17730vinfos[5].jointtype = 1;
17731vinfos[5].foffset = j5;
17732vinfos[5].indices[0] = _ij5[0];
17733vinfos[5].indices[1] = _ij5[1];
17734vinfos[5].maxsolutions = _nj5;
17735std::vector<int> vfree(0);
17736solutions.AddSolution(vinfos,vfree);
17737}
17738}
17739}
17740
17741}
17742
17743}
17744
17745} else
17746{
17747{
17748IkReal j3array[1], cj3array[1], sj3array[1];
17749bool j3valid[1]={false};
17750_nj3 = 1;
17751IkReal x1404=((1.0)*new_r10);
17752CheckValue<IkReal> x1405 = IKatan2WithCheck(IkReal((((gconst17*new_r11))+((gconst17*new_r00)))),IkReal(((((-1.0)*gconst17*x1404))+((gconst17*new_r01)))),IKFAST_ATAN2_MAGTHRESH);
17753if(!x1405.valid){
17754continue;
17755}
17756CheckValue<IkReal> x1406=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r00*new_r01))+(((-1.0)*new_r11*x1404)))),-1);
17757if(!x1406.valid){
17758continue;
17759}
17760j3array[0]=((-1.5707963267949)+(x1405.value)+(((1.5707963267949)*(x1406.value))));
17761sj3array[0]=IKsin(j3array[0]);
17762cj3array[0]=IKcos(j3array[0]);
17763if( j3array[0] > IKPI )
17764{
17765 j3array[0]-=IK2PI;
17766}
17767else if( j3array[0] < -IKPI )
17768{ j3array[0]+=IK2PI;
17769}
17770j3valid[0] = true;
17771for(int ij3 = 0; ij3 < 1; ++ij3)
17772{
17773if( !j3valid[ij3] )
17774{
17775 continue;
17776}
17777_ij3[0] = ij3; _ij3[1] = -1;
17778for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17779{
17780if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17781{
17782 j3valid[iij3]=false; _ij3[1] = iij3; break;
17783}
17784}
17785j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17786{
17787IkReal evalcond[8];
17788IkReal x1407=IKsin(j3);
17789IkReal x1408=IKcos(j3);
17790IkReal x1409=((1.0)*gconst16);
17791IkReal x1410=(gconst17*x1407);
17792IkReal x1411=(gconst17*x1408);
17793IkReal x1412=((1.0)*x1408);
17794IkReal x1413=(x1408*x1409);
17795evalcond[0]=(gconst17+((new_r10*x1407))+((new_r00*x1408)));
17796evalcond[1]=(((gconst16*x1407))+x1411+new_r00);
17797evalcond[2]=((((-1.0)*new_r10*x1412))+gconst16+((new_r00*x1407)));
17798evalcond[3]=((((-1.0)*new_r11*x1412))+((new_r01*x1407))+gconst17);
17799evalcond[4]=((((-1.0)*x1413))+x1410+new_r01);
17800evalcond[5]=((((-1.0)*x1413))+x1410+new_r10);
17801evalcond[6]=(((new_r01*x1408))+((new_r11*x1407))+(((-1.0)*x1409)));
17802evalcond[7]=((((-1.0)*x1407*x1409))+new_r11+(((-1.0)*x1411)));
17803if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17804{
17805continue;
17806}
17807}
17808
17809{
17810std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17811vinfos[0].jointtype = 1;
17812vinfos[0].foffset = j0;
17813vinfos[0].indices[0] = _ij0[0];
17814vinfos[0].indices[1] = _ij0[1];
17815vinfos[0].maxsolutions = _nj0;
17816vinfos[1].jointtype = 1;
17817vinfos[1].foffset = j1;
17818vinfos[1].indices[0] = _ij1[0];
17819vinfos[1].indices[1] = _ij1[1];
17820vinfos[1].maxsolutions = _nj1;
17821vinfos[2].jointtype = 1;
17822vinfos[2].foffset = j2;
17823vinfos[2].indices[0] = _ij2[0];
17824vinfos[2].indices[1] = _ij2[1];
17825vinfos[2].maxsolutions = _nj2;
17826vinfos[3].jointtype = 1;
17827vinfos[3].foffset = j3;
17828vinfos[3].indices[0] = _ij3[0];
17829vinfos[3].indices[1] = _ij3[1];
17830vinfos[3].maxsolutions = _nj3;
17831vinfos[4].jointtype = 1;
17832vinfos[4].foffset = j4;
17833vinfos[4].indices[0] = _ij4[0];
17834vinfos[4].indices[1] = _ij4[1];
17835vinfos[4].maxsolutions = _nj4;
17836vinfos[5].jointtype = 1;
17837vinfos[5].foffset = j5;
17838vinfos[5].indices[0] = _ij5[0];
17839vinfos[5].indices[1] = _ij5[1];
17840vinfos[5].maxsolutions = _nj5;
17841std::vector<int> vfree(0);
17842solutions.AddSolution(vinfos,vfree);
17843}
17844}
17845}
17846
17847}
17848
17849}
17850
17851}
17852} while(0);
17853if( bgotonextstatement )
17854{
17855bool bgotonextstatement = true;
17856do
17857{
17858evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((-1.5707963267949)+j5)))), 6.28318530717959)));
17859if( IKabs(evalcond[0]) < 0.0000050000000000 )
17860{
17861bgotonextstatement=false;
17862{
17863IkReal j3array[1], cj3array[1], sj3array[1];
17864bool j3valid[1]={false};
17865_nj3 = 1;
17866if( IKabs(((-1.0)*new_r00)) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r01) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r00))+IKsqr(new_r01)-1) <= IKFAST_SINCOS_THRESH )
17867 continue;
17868j3array[0]=IKatan2(((-1.0)*new_r00), new_r01);
17869sj3array[0]=IKsin(j3array[0]);
17870cj3array[0]=IKcos(j3array[0]);
17871if( j3array[0] > IKPI )
17872{
17873 j3array[0]-=IK2PI;
17874}
17875else if( j3array[0] < -IKPI )
17876{ j3array[0]+=IK2PI;
17877}
17878j3valid[0] = true;
17879for(int ij3 = 0; ij3 < 1; ++ij3)
17880{
17881if( !j3valid[ij3] )
17882{
17883 continue;
17884}
17885_ij3[0] = ij3; _ij3[1] = -1;
17886for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17887{
17888if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17889{
17890 j3valid[iij3]=false; _ij3[1] = iij3; break;
17891}
17892}
17893j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17894{
17895IkReal evalcond[8];
17896IkReal x1414=IKsin(j3);
17897IkReal x1415=IKcos(j3);
17898IkReal x1416=((1.0)*x1415);
17899evalcond[0]=(x1414+new_r00);
17900evalcond[1]=((((-1.0)*x1416))+new_r01);
17901evalcond[2]=(new_r11+(((-1.0)*x1414)));
17902evalcond[3]=((((-1.0)*x1416))+new_r10);
17903evalcond[4]=(((new_r00*x1415))+((new_r10*x1414)));
17904evalcond[5]=((((-1.0)*new_r11*x1416))+((new_r01*x1414)));
17905evalcond[6]=((-1.0)+((new_r01*x1415))+((new_r11*x1414)));
17906evalcond[7]=((1.0)+(((-1.0)*new_r10*x1416))+((new_r00*x1414)));
17907if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
17908{
17909continue;
17910}
17911}
17912
17913{
17914std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
17915vinfos[0].jointtype = 1;
17916vinfos[0].foffset = j0;
17917vinfos[0].indices[0] = _ij0[0];
17918vinfos[0].indices[1] = _ij0[1];
17919vinfos[0].maxsolutions = _nj0;
17920vinfos[1].jointtype = 1;
17921vinfos[1].foffset = j1;
17922vinfos[1].indices[0] = _ij1[0];
17923vinfos[1].indices[1] = _ij1[1];
17924vinfos[1].maxsolutions = _nj1;
17925vinfos[2].jointtype = 1;
17926vinfos[2].foffset = j2;
17927vinfos[2].indices[0] = _ij2[0];
17928vinfos[2].indices[1] = _ij2[1];
17929vinfos[2].maxsolutions = _nj2;
17930vinfos[3].jointtype = 1;
17931vinfos[3].foffset = j3;
17932vinfos[3].indices[0] = _ij3[0];
17933vinfos[3].indices[1] = _ij3[1];
17934vinfos[3].maxsolutions = _nj3;
17935vinfos[4].jointtype = 1;
17936vinfos[4].foffset = j4;
17937vinfos[4].indices[0] = _ij4[0];
17938vinfos[4].indices[1] = _ij4[1];
17939vinfos[4].maxsolutions = _nj4;
17940vinfos[5].jointtype = 1;
17941vinfos[5].foffset = j5;
17942vinfos[5].indices[0] = _ij5[0];
17943vinfos[5].indices[1] = _ij5[1];
17944vinfos[5].maxsolutions = _nj5;
17945std::vector<int> vfree(0);
17946solutions.AddSolution(vinfos,vfree);
17947}
17948}
17949}
17950
17951}
17952} while(0);
17953if( bgotonextstatement )
17954{
17955bool bgotonextstatement = true;
17956do
17957{
17958evalcond[0]=((-3.14159265358979)+(IKfmod(((3.14159265358979)+(IKabs(((1.5707963267949)+j5)))), 6.28318530717959)));
17959if( IKabs(evalcond[0]) < 0.0000050000000000 )
17960{
17961bgotonextstatement=false;
17962{
17963IkReal j3array[1], cj3array[1], sj3array[1];
17964bool j3valid[1]={false};
17965_nj3 = 1;
17966if( IKabs(((-1.0)*new_r11)) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r01)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*new_r11))+IKsqr(((-1.0)*new_r01))-1) <= IKFAST_SINCOS_THRESH )
17967 continue;
17968j3array[0]=IKatan2(((-1.0)*new_r11), ((-1.0)*new_r01));
17969sj3array[0]=IKsin(j3array[0]);
17970cj3array[0]=IKcos(j3array[0]);
17971if( j3array[0] > IKPI )
17972{
17973 j3array[0]-=IK2PI;
17974}
17975else if( j3array[0] < -IKPI )
17976{ j3array[0]+=IK2PI;
17977}
17978j3valid[0] = true;
17979for(int ij3 = 0; ij3 < 1; ++ij3)
17980{
17981if( !j3valid[ij3] )
17982{
17983 continue;
17984}
17985_ij3[0] = ij3; _ij3[1] = -1;
17986for(int iij3 = ij3+1; iij3 < 1; ++iij3)
17987{
17988if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
17989{
17990 j3valid[iij3]=false; _ij3[1] = iij3; break;
17991}
17992}
17993j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
17994{
17995IkReal evalcond[8];
17996IkReal x1417=IKcos(j3);
17997IkReal x1418=IKsin(j3);
17998IkReal x1419=((1.0)*x1417);
17999evalcond[0]=(x1417+new_r01);
18000evalcond[1]=(x1418+new_r11);
18001evalcond[2]=(x1417+new_r10);
18002evalcond[3]=(new_r00+(((-1.0)*x1418)));
18003evalcond[4]=(((new_r00*x1417))+((new_r10*x1418)));
18004evalcond[5]=((((-1.0)*new_r11*x1419))+((new_r01*x1418)));
18005evalcond[6]=((1.0)+((new_r01*x1417))+((new_r11*x1418)));
18006evalcond[7]=((-1.0)+(((-1.0)*new_r10*x1419))+((new_r00*x1418)));
18007if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
18008{
18009continue;
18010}
18011}
18012
18013{
18014std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18015vinfos[0].jointtype = 1;
18016vinfos[0].foffset = j0;
18017vinfos[0].indices[0] = _ij0[0];
18018vinfos[0].indices[1] = _ij0[1];
18019vinfos[0].maxsolutions = _nj0;
18020vinfos[1].jointtype = 1;
18021vinfos[1].foffset = j1;
18022vinfos[1].indices[0] = _ij1[0];
18023vinfos[1].indices[1] = _ij1[1];
18024vinfos[1].maxsolutions = _nj1;
18025vinfos[2].jointtype = 1;
18026vinfos[2].foffset = j2;
18027vinfos[2].indices[0] = _ij2[0];
18028vinfos[2].indices[1] = _ij2[1];
18029vinfos[2].maxsolutions = _nj2;
18030vinfos[3].jointtype = 1;
18031vinfos[3].foffset = j3;
18032vinfos[3].indices[0] = _ij3[0];
18033vinfos[3].indices[1] = _ij3[1];
18034vinfos[3].maxsolutions = _nj3;
18035vinfos[4].jointtype = 1;
18036vinfos[4].foffset = j4;
18037vinfos[4].indices[0] = _ij4[0];
18038vinfos[4].indices[1] = _ij4[1];
18039vinfos[4].maxsolutions = _nj4;
18040vinfos[5].jointtype = 1;
18041vinfos[5].foffset = j5;
18042vinfos[5].indices[0] = _ij5[0];
18043vinfos[5].indices[1] = _ij5[1];
18044vinfos[5].maxsolutions = _nj5;
18045std::vector<int> vfree(0);
18046solutions.AddSolution(vinfos,vfree);
18047}
18048}
18049}
18050
18051}
18052} while(0);
18053if( bgotonextstatement )
18054{
18055bool bgotonextstatement = true;
18056do
18057{
18058evalcond[0]=((IKabs(new_r11))+(IKabs(new_r00)));
18059if( IKabs(evalcond[0]) < 0.0000050000000000 )
18060{
18061bgotonextstatement=false;
18062{
18063IkReal j3eval[3];
18064sj4=0;
18065cj4=-1.0;
18066j4=3.14159265358979;
18067new_r11=0;
18068new_r00=0;
18069j3eval[0]=new_r10;
18070j3eval[1]=IKsign(new_r10);
18071j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18072if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18073{
18074{
18075IkReal j3eval[3];
18076sj4=0;
18077cj4=-1.0;
18078j4=3.14159265358979;
18079new_r11=0;
18080new_r00=0;
18081j3eval[0]=new_r01;
18082j3eval[1]=IKsign(new_r01);
18083j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18084if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18085{
18086{
18087IkReal j3eval[2];
18088sj4=0;
18089cj4=-1.0;
18090j4=3.14159265358979;
18091new_r11=0;
18092new_r00=0;
18093j3eval[0]=new_r01;
18094j3eval[1]=new_r10;
18095if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
18096{
18097continue; // no branches [j3]
18098
18099} else
18100{
18101{
18102IkReal j3array[1], cj3array[1], sj3array[1];
18103bool j3valid[1]={false};
18104_nj3 = 1;
18105CheckValue<IkReal> x1420=IKPowWithIntegerCheck(new_r01,-1);
18106if(!x1420.valid){
18107continue;
18108}
18109CheckValue<IkReal> x1421=IKPowWithIntegerCheck(new_r10,-1);
18110if(!x1421.valid){
18111continue;
18112}
18113if( IKabs(((-1.0)*cj5*(x1420.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((sj5*(x1421.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*cj5*(x1420.value)))+IKsqr((sj5*(x1421.value)))-1) <= IKFAST_SINCOS_THRESH )
18114 continue;
18115j3array[0]=IKatan2(((-1.0)*cj5*(x1420.value)), (sj5*(x1421.value)));
18116sj3array[0]=IKsin(j3array[0]);
18117cj3array[0]=IKcos(j3array[0]);
18118if( j3array[0] > IKPI )
18119{
18120 j3array[0]-=IK2PI;
18121}
18122else if( j3array[0] < -IKPI )
18123{ j3array[0]+=IK2PI;
18124}
18125j3valid[0] = true;
18126for(int ij3 = 0; ij3 < 1; ++ij3)
18127{
18128if( !j3valid[ij3] )
18129{
18130 continue;
18131}
18132_ij3[0] = ij3; _ij3[1] = -1;
18133for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18134{
18135if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18136{
18137 j3valid[iij3]=false; _ij3[1] = iij3; break;
18138}
18139}
18140j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18141{
18142IkReal evalcond[7];
18143IkReal x1422=IKsin(j3);
18144IkReal x1423=IKcos(j3);
18145IkReal x1424=((1.0)*sj5);
18146IkReal x1425=(cj5*x1422);
18147IkReal x1426=(x1423*x1424);
18148evalcond[0]=(cj5+((new_r01*x1422)));
18149evalcond[1]=(cj5+((new_r10*x1422)));
18150evalcond[2]=(sj5+(((-1.0)*new_r10*x1423)));
18151evalcond[3]=(((new_r01*x1423))+(((-1.0)*x1424)));
18152evalcond[4]=(((sj5*x1422))+((cj5*x1423)));
18153evalcond[5]=(x1425+new_r01+(((-1.0)*x1426)));
18154evalcond[6]=(x1425+new_r10+(((-1.0)*x1426)));
18155if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18156{
18157continue;
18158}
18159}
18160
18161{
18162std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18163vinfos[0].jointtype = 1;
18164vinfos[0].foffset = j0;
18165vinfos[0].indices[0] = _ij0[0];
18166vinfos[0].indices[1] = _ij0[1];
18167vinfos[0].maxsolutions = _nj0;
18168vinfos[1].jointtype = 1;
18169vinfos[1].foffset = j1;
18170vinfos[1].indices[0] = _ij1[0];
18171vinfos[1].indices[1] = _ij1[1];
18172vinfos[1].maxsolutions = _nj1;
18173vinfos[2].jointtype = 1;
18174vinfos[2].foffset = j2;
18175vinfos[2].indices[0] = _ij2[0];
18176vinfos[2].indices[1] = _ij2[1];
18177vinfos[2].maxsolutions = _nj2;
18178vinfos[3].jointtype = 1;
18179vinfos[3].foffset = j3;
18180vinfos[3].indices[0] = _ij3[0];
18181vinfos[3].indices[1] = _ij3[1];
18182vinfos[3].maxsolutions = _nj3;
18183vinfos[4].jointtype = 1;
18184vinfos[4].foffset = j4;
18185vinfos[4].indices[0] = _ij4[0];
18186vinfos[4].indices[1] = _ij4[1];
18187vinfos[4].maxsolutions = _nj4;
18188vinfos[5].jointtype = 1;
18189vinfos[5].foffset = j5;
18190vinfos[5].indices[0] = _ij5[0];
18191vinfos[5].indices[1] = _ij5[1];
18192vinfos[5].maxsolutions = _nj5;
18193std::vector<int> vfree(0);
18194solutions.AddSolution(vinfos,vfree);
18195}
18196}
18197}
18198
18199}
18200
18201}
18202
18203} else
18204{
18205{
18206IkReal j3array[1], cj3array[1], sj3array[1];
18207bool j3valid[1]={false};
18208_nj3 = 1;
18209CheckValue<IkReal> x1427=IKPowWithIntegerCheck(IKsign(new_r01),-1);
18210if(!x1427.valid){
18211continue;
18212}
18213CheckValue<IkReal> x1428 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(sj5),IKFAST_ATAN2_MAGTHRESH);
18214if(!x1428.valid){
18215continue;
18216}
18217j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1427.value)))+(x1428.value));
18218sj3array[0]=IKsin(j3array[0]);
18219cj3array[0]=IKcos(j3array[0]);
18220if( j3array[0] > IKPI )
18221{
18222 j3array[0]-=IK2PI;
18223}
18224else if( j3array[0] < -IKPI )
18225{ j3array[0]+=IK2PI;
18226}
18227j3valid[0] = true;
18228for(int ij3 = 0; ij3 < 1; ++ij3)
18229{
18230if( !j3valid[ij3] )
18231{
18232 continue;
18233}
18234_ij3[0] = ij3; _ij3[1] = -1;
18235for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18236{
18237if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18238{
18239 j3valid[iij3]=false; _ij3[1] = iij3; break;
18240}
18241}
18242j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18243{
18244IkReal evalcond[7];
18245IkReal x1429=IKsin(j3);
18246IkReal x1430=IKcos(j3);
18247IkReal x1431=((1.0)*sj5);
18248IkReal x1432=(cj5*x1429);
18249IkReal x1433=(x1430*x1431);
18250evalcond[0]=(cj5+((new_r01*x1429)));
18251evalcond[1]=(cj5+((new_r10*x1429)));
18252evalcond[2]=(sj5+(((-1.0)*new_r10*x1430)));
18253evalcond[3]=(((new_r01*x1430))+(((-1.0)*x1431)));
18254evalcond[4]=(((cj5*x1430))+((sj5*x1429)));
18255evalcond[5]=((((-1.0)*x1433))+x1432+new_r01);
18256evalcond[6]=((((-1.0)*x1433))+x1432+new_r10);
18257if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18258{
18259continue;
18260}
18261}
18262
18263{
18264std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18265vinfos[0].jointtype = 1;
18266vinfos[0].foffset = j0;
18267vinfos[0].indices[0] = _ij0[0];
18268vinfos[0].indices[1] = _ij0[1];
18269vinfos[0].maxsolutions = _nj0;
18270vinfos[1].jointtype = 1;
18271vinfos[1].foffset = j1;
18272vinfos[1].indices[0] = _ij1[0];
18273vinfos[1].indices[1] = _ij1[1];
18274vinfos[1].maxsolutions = _nj1;
18275vinfos[2].jointtype = 1;
18276vinfos[2].foffset = j2;
18277vinfos[2].indices[0] = _ij2[0];
18278vinfos[2].indices[1] = _ij2[1];
18279vinfos[2].maxsolutions = _nj2;
18280vinfos[3].jointtype = 1;
18281vinfos[3].foffset = j3;
18282vinfos[3].indices[0] = _ij3[0];
18283vinfos[3].indices[1] = _ij3[1];
18284vinfos[3].maxsolutions = _nj3;
18285vinfos[4].jointtype = 1;
18286vinfos[4].foffset = j4;
18287vinfos[4].indices[0] = _ij4[0];
18288vinfos[4].indices[1] = _ij4[1];
18289vinfos[4].maxsolutions = _nj4;
18290vinfos[5].jointtype = 1;
18291vinfos[5].foffset = j5;
18292vinfos[5].indices[0] = _ij5[0];
18293vinfos[5].indices[1] = _ij5[1];
18294vinfos[5].maxsolutions = _nj5;
18295std::vector<int> vfree(0);
18296solutions.AddSolution(vinfos,vfree);
18297}
18298}
18299}
18300
18301}
18302
18303}
18304
18305} else
18306{
18307{
18308IkReal j3array[1], cj3array[1], sj3array[1];
18309bool j3valid[1]={false};
18310_nj3 = 1;
18311CheckValue<IkReal> x1434=IKPowWithIntegerCheck(IKsign(new_r10),-1);
18312if(!x1434.valid){
18313continue;
18314}
18315CheckValue<IkReal> x1435 = IKatan2WithCheck(IkReal(((-1.0)*cj5)),IkReal(sj5),IKFAST_ATAN2_MAGTHRESH);
18316if(!x1435.valid){
18317continue;
18318}
18319j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1434.value)))+(x1435.value));
18320sj3array[0]=IKsin(j3array[0]);
18321cj3array[0]=IKcos(j3array[0]);
18322if( j3array[0] > IKPI )
18323{
18324 j3array[0]-=IK2PI;
18325}
18326else if( j3array[0] < -IKPI )
18327{ j3array[0]+=IK2PI;
18328}
18329j3valid[0] = true;
18330for(int ij3 = 0; ij3 < 1; ++ij3)
18331{
18332if( !j3valid[ij3] )
18333{
18334 continue;
18335}
18336_ij3[0] = ij3; _ij3[1] = -1;
18337for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18338{
18339if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18340{
18341 j3valid[iij3]=false; _ij3[1] = iij3; break;
18342}
18343}
18344j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18345{
18346IkReal evalcond[7];
18347IkReal x1436=IKsin(j3);
18348IkReal x1437=IKcos(j3);
18349IkReal x1438=((1.0)*sj5);
18350IkReal x1439=(cj5*x1436);
18351IkReal x1440=(x1437*x1438);
18352evalcond[0]=(cj5+((new_r01*x1436)));
18353evalcond[1]=(cj5+((new_r10*x1436)));
18354evalcond[2]=(sj5+(((-1.0)*new_r10*x1437)));
18355evalcond[3]=(((new_r01*x1437))+(((-1.0)*x1438)));
18356evalcond[4]=(((sj5*x1436))+((cj5*x1437)));
18357evalcond[5]=(x1439+(((-1.0)*x1440))+new_r01);
18358evalcond[6]=(x1439+(((-1.0)*x1440))+new_r10);
18359if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18360{
18361continue;
18362}
18363}
18364
18365{
18366std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18367vinfos[0].jointtype = 1;
18368vinfos[0].foffset = j0;
18369vinfos[0].indices[0] = _ij0[0];
18370vinfos[0].indices[1] = _ij0[1];
18371vinfos[0].maxsolutions = _nj0;
18372vinfos[1].jointtype = 1;
18373vinfos[1].foffset = j1;
18374vinfos[1].indices[0] = _ij1[0];
18375vinfos[1].indices[1] = _ij1[1];
18376vinfos[1].maxsolutions = _nj1;
18377vinfos[2].jointtype = 1;
18378vinfos[2].foffset = j2;
18379vinfos[2].indices[0] = _ij2[0];
18380vinfos[2].indices[1] = _ij2[1];
18381vinfos[2].maxsolutions = _nj2;
18382vinfos[3].jointtype = 1;
18383vinfos[3].foffset = j3;
18384vinfos[3].indices[0] = _ij3[0];
18385vinfos[3].indices[1] = _ij3[1];
18386vinfos[3].maxsolutions = _nj3;
18387vinfos[4].jointtype = 1;
18388vinfos[4].foffset = j4;
18389vinfos[4].indices[0] = _ij4[0];
18390vinfos[4].indices[1] = _ij4[1];
18391vinfos[4].maxsolutions = _nj4;
18392vinfos[5].jointtype = 1;
18393vinfos[5].foffset = j5;
18394vinfos[5].indices[0] = _ij5[0];
18395vinfos[5].indices[1] = _ij5[1];
18396vinfos[5].maxsolutions = _nj5;
18397std::vector<int> vfree(0);
18398solutions.AddSolution(vinfos,vfree);
18399}
18400}
18401}
18402
18403}
18404
18405}
18406
18407}
18408} while(0);
18409if( bgotonextstatement )
18410{
18411bool bgotonextstatement = true;
18412do
18413{
18414evalcond[0]=((IKabs(new_r11))+(IKabs(new_r01)));
18415if( IKabs(evalcond[0]) < 0.0000050000000000 )
18416{
18417bgotonextstatement=false;
18418{
18419IkReal j3eval[1];
18420sj4=0;
18421cj4=-1.0;
18422j4=3.14159265358979;
18423new_r11=0;
18424new_r01=0;
18425new_r22=0;
18426new_r20=0;
18427j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
18428if( IKabs(j3eval[0]) < 0.0000010000000000 )
18429{
18430continue; // no branches [j3]
18431
18432} else
18433{
18434{
18435IkReal j3array[2], cj3array[2], sj3array[2];
18436bool j3valid[2]={false};
18437_nj3 = 2;
18438CheckValue<IkReal> x1442 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
18439if(!x1442.valid){
18440continue;
18441}
18442IkReal x1441=x1442.value;
18443j3array[0]=((-1.0)*x1441);
18444sj3array[0]=IKsin(j3array[0]);
18445cj3array[0]=IKcos(j3array[0]);
18446j3array[1]=((3.14159265358979)+(((-1.0)*x1441)));
18447sj3array[1]=IKsin(j3array[1]);
18448cj3array[1]=IKcos(j3array[1]);
18449if( j3array[0] > IKPI )
18450{
18451 j3array[0]-=IK2PI;
18452}
18453else if( j3array[0] < -IKPI )
18454{ j3array[0]+=IK2PI;
18455}
18456j3valid[0] = true;
18457if( j3array[1] > IKPI )
18458{
18459 j3array[1]-=IK2PI;
18460}
18461else if( j3array[1] < -IKPI )
18462{ j3array[1]+=IK2PI;
18463}
18464j3valid[1] = true;
18465for(int ij3 = 0; ij3 < 2; ++ij3)
18466{
18467if( !j3valid[ij3] )
18468{
18469 continue;
18470}
18471_ij3[0] = ij3; _ij3[1] = -1;
18472for(int iij3 = ij3+1; iij3 < 2; ++iij3)
18473{
18474if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18475{
18476 j3valid[iij3]=false; _ij3[1] = iij3; break;
18477}
18478}
18479j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18480{
18481IkReal evalcond[1];
18482evalcond[0]=(((new_r00*(IKsin(j3))))+(((-1.0)*new_r10*(IKcos(j3)))));
18483if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
18484{
18485continue;
18486}
18487}
18488
18489{
18490std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18491vinfos[0].jointtype = 1;
18492vinfos[0].foffset = j0;
18493vinfos[0].indices[0] = _ij0[0];
18494vinfos[0].indices[1] = _ij0[1];
18495vinfos[0].maxsolutions = _nj0;
18496vinfos[1].jointtype = 1;
18497vinfos[1].foffset = j1;
18498vinfos[1].indices[0] = _ij1[0];
18499vinfos[1].indices[1] = _ij1[1];
18500vinfos[1].maxsolutions = _nj1;
18501vinfos[2].jointtype = 1;
18502vinfos[2].foffset = j2;
18503vinfos[2].indices[0] = _ij2[0];
18504vinfos[2].indices[1] = _ij2[1];
18505vinfos[2].maxsolutions = _nj2;
18506vinfos[3].jointtype = 1;
18507vinfos[3].foffset = j3;
18508vinfos[3].indices[0] = _ij3[0];
18509vinfos[3].indices[1] = _ij3[1];
18510vinfos[3].maxsolutions = _nj3;
18511vinfos[4].jointtype = 1;
18512vinfos[4].foffset = j4;
18513vinfos[4].indices[0] = _ij4[0];
18514vinfos[4].indices[1] = _ij4[1];
18515vinfos[4].maxsolutions = _nj4;
18516vinfos[5].jointtype = 1;
18517vinfos[5].foffset = j5;
18518vinfos[5].indices[0] = _ij5[0];
18519vinfos[5].indices[1] = _ij5[1];
18520vinfos[5].maxsolutions = _nj5;
18521std::vector<int> vfree(0);
18522solutions.AddSolution(vinfos,vfree);
18523}
18524}
18525}
18526
18527}
18528
18529}
18530
18531}
18532} while(0);
18533if( bgotonextstatement )
18534{
18535bool bgotonextstatement = true;
18536do
18537{
18538evalcond[0]=((IKabs(new_r10))+(IKabs(new_r00)));
18539if( IKabs(evalcond[0]) < 0.0000050000000000 )
18540{
18541bgotonextstatement=false;
18542{
18543IkReal j3eval[1];
18544sj4=0;
18545cj4=-1.0;
18546j4=3.14159265358979;
18547new_r00=0;
18548new_r10=0;
18549new_r21=0;
18550new_r22=0;
18551j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
18552if( IKabs(j3eval[0]) < 0.0000010000000000 )
18553{
18554continue; // no branches [j3]
18555
18556} else
18557{
18558{
18559IkReal j3array[2], cj3array[2], sj3array[2];
18560bool j3valid[2]={false};
18561_nj3 = 2;
18562CheckValue<IkReal> x1444 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
18563if(!x1444.valid){
18564continue;
18565}
18566IkReal x1443=x1444.value;
18567j3array[0]=((-1.0)*x1443);
18568sj3array[0]=IKsin(j3array[0]);
18569cj3array[0]=IKcos(j3array[0]);
18570j3array[1]=((3.14159265358979)+(((-1.0)*x1443)));
18571sj3array[1]=IKsin(j3array[1]);
18572cj3array[1]=IKcos(j3array[1]);
18573if( j3array[0] > IKPI )
18574{
18575 j3array[0]-=IK2PI;
18576}
18577else if( j3array[0] < -IKPI )
18578{ j3array[0]+=IK2PI;
18579}
18580j3valid[0] = true;
18581if( j3array[1] > IKPI )
18582{
18583 j3array[1]-=IK2PI;
18584}
18585else if( j3array[1] < -IKPI )
18586{ j3array[1]+=IK2PI;
18587}
18588j3valid[1] = true;
18589for(int ij3 = 0; ij3 < 2; ++ij3)
18590{
18591if( !j3valid[ij3] )
18592{
18593 continue;
18594}
18595_ij3[0] = ij3; _ij3[1] = -1;
18596for(int iij3 = ij3+1; iij3 < 2; ++iij3)
18597{
18598if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18599{
18600 j3valid[iij3]=false; _ij3[1] = iij3; break;
18601}
18602}
18603j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18604{
18605IkReal evalcond[1];
18606evalcond[0]=(((new_r01*(IKsin(j3))))+(((-1.0)*new_r11*(IKcos(j3)))));
18607if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH )
18608{
18609continue;
18610}
18611}
18612
18613{
18614std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18615vinfos[0].jointtype = 1;
18616vinfos[0].foffset = j0;
18617vinfos[0].indices[0] = _ij0[0];
18618vinfos[0].indices[1] = _ij0[1];
18619vinfos[0].maxsolutions = _nj0;
18620vinfos[1].jointtype = 1;
18621vinfos[1].foffset = j1;
18622vinfos[1].indices[0] = _ij1[0];
18623vinfos[1].indices[1] = _ij1[1];
18624vinfos[1].maxsolutions = _nj1;
18625vinfos[2].jointtype = 1;
18626vinfos[2].foffset = j2;
18627vinfos[2].indices[0] = _ij2[0];
18628vinfos[2].indices[1] = _ij2[1];
18629vinfos[2].maxsolutions = _nj2;
18630vinfos[3].jointtype = 1;
18631vinfos[3].foffset = j3;
18632vinfos[3].indices[0] = _ij3[0];
18633vinfos[3].indices[1] = _ij3[1];
18634vinfos[3].maxsolutions = _nj3;
18635vinfos[4].jointtype = 1;
18636vinfos[4].foffset = j4;
18637vinfos[4].indices[0] = _ij4[0];
18638vinfos[4].indices[1] = _ij4[1];
18639vinfos[4].maxsolutions = _nj4;
18640vinfos[5].jointtype = 1;
18641vinfos[5].foffset = j5;
18642vinfos[5].indices[0] = _ij5[0];
18643vinfos[5].indices[1] = _ij5[1];
18644vinfos[5].maxsolutions = _nj5;
18645std::vector<int> vfree(0);
18646solutions.AddSolution(vinfos,vfree);
18647}
18648}
18649}
18650
18651}
18652
18653}
18654
18655}
18656} while(0);
18657if( bgotonextstatement )
18658{
18659bool bgotonextstatement = true;
18660do
18661{
18662evalcond[0]=((IKabs(new_r10))+(IKabs(new_r01)));
18663if( IKabs(evalcond[0]) < 0.0000050000000000 )
18664{
18665bgotonextstatement=false;
18666{
18667IkReal j3eval[3];
18668sj4=0;
18669cj4=-1.0;
18670j4=3.14159265358979;
18671new_r01=0;
18672new_r10=0;
18673j3eval[0]=new_r00;
18674j3eval[1]=IKsign(new_r00);
18675j3eval[2]=((IKabs(cj5))+(IKabs(sj5)));
18676if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 || IKabs(j3eval[2]) < 0.0000010000000000 )
18677{
18678{
18679IkReal j3eval[2];
18680sj4=0;
18681cj4=-1.0;
18682j4=3.14159265358979;
18683new_r01=0;
18684new_r10=0;
18685j3eval[0]=new_r00;
18686j3eval[1]=new_r11;
18687if( IKabs(j3eval[0]) < 0.0000010000000000 || IKabs(j3eval[1]) < 0.0000010000000000 )
18688{
18689continue; // no branches [j3]
18690
18691} else
18692{
18693{
18694IkReal j3array[1], cj3array[1], sj3array[1];
18695bool j3valid[1]={false};
18696_nj3 = 1;
18697CheckValue<IkReal> x1445=IKPowWithIntegerCheck(new_r00,-1);
18698if(!x1445.valid){
18699continue;
18700}
18701CheckValue<IkReal> x1446=IKPowWithIntegerCheck(new_r11,-1);
18702if(!x1446.valid){
18703continue;
18704}
18705if( IKabs(((-1.0)*sj5*(x1445.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs((cj5*(x1446.value))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((-1.0)*sj5*(x1445.value)))+IKsqr((cj5*(x1446.value)))-1) <= IKFAST_SINCOS_THRESH )
18706 continue;
18707j3array[0]=IKatan2(((-1.0)*sj5*(x1445.value)), (cj5*(x1446.value)));
18708sj3array[0]=IKsin(j3array[0]);
18709cj3array[0]=IKcos(j3array[0]);
18710if( j3array[0] > IKPI )
18711{
18712 j3array[0]-=IK2PI;
18713}
18714else if( j3array[0] < -IKPI )
18715{ j3array[0]+=IK2PI;
18716}
18717j3valid[0] = true;
18718for(int ij3 = 0; ij3 < 1; ++ij3)
18719{
18720if( !j3valid[ij3] )
18721{
18722 continue;
18723}
18724_ij3[0] = ij3; _ij3[1] = -1;
18725for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18726{
18727if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18728{
18729 j3valid[iij3]=false; _ij3[1] = iij3; break;
18730}
18731}
18732j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18733{
18734IkReal evalcond[7];
18735IkReal x1447=IKcos(j3);
18736IkReal x1448=IKsin(j3);
18737IkReal x1449=((1.0)*sj5);
18738IkReal x1450=(cj5*x1447);
18739IkReal x1451=((1.0)*x1447);
18740evalcond[0]=(sj5+((new_r00*x1448)));
18741evalcond[1]=(cj5+((new_r00*x1447)));
18742evalcond[2]=(cj5+(((-1.0)*new_r11*x1451)));
18743evalcond[3]=(((new_r11*x1448))+(((-1.0)*x1449)));
18744evalcond[4]=((((-1.0)*x1447*x1449))+((cj5*x1448)));
18745evalcond[5]=(((sj5*x1448))+x1450+new_r00);
18746evalcond[6]=((((-1.0)*x1450))+new_r11+(((-1.0)*x1448*x1449)));
18747if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18748{
18749continue;
18750}
18751}
18752
18753{
18754std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18755vinfos[0].jointtype = 1;
18756vinfos[0].foffset = j0;
18757vinfos[0].indices[0] = _ij0[0];
18758vinfos[0].indices[1] = _ij0[1];
18759vinfos[0].maxsolutions = _nj0;
18760vinfos[1].jointtype = 1;
18761vinfos[1].foffset = j1;
18762vinfos[1].indices[0] = _ij1[0];
18763vinfos[1].indices[1] = _ij1[1];
18764vinfos[1].maxsolutions = _nj1;
18765vinfos[2].jointtype = 1;
18766vinfos[2].foffset = j2;
18767vinfos[2].indices[0] = _ij2[0];
18768vinfos[2].indices[1] = _ij2[1];
18769vinfos[2].maxsolutions = _nj2;
18770vinfos[3].jointtype = 1;
18771vinfos[3].foffset = j3;
18772vinfos[3].indices[0] = _ij3[0];
18773vinfos[3].indices[1] = _ij3[1];
18774vinfos[3].maxsolutions = _nj3;
18775vinfos[4].jointtype = 1;
18776vinfos[4].foffset = j4;
18777vinfos[4].indices[0] = _ij4[0];
18778vinfos[4].indices[1] = _ij4[1];
18779vinfos[4].maxsolutions = _nj4;
18780vinfos[5].jointtype = 1;
18781vinfos[5].foffset = j5;
18782vinfos[5].indices[0] = _ij5[0];
18783vinfos[5].indices[1] = _ij5[1];
18784vinfos[5].maxsolutions = _nj5;
18785std::vector<int> vfree(0);
18786solutions.AddSolution(vinfos,vfree);
18787}
18788}
18789}
18790
18791}
18792
18793}
18794
18795} else
18796{
18797{
18798IkReal j3array[1], cj3array[1], sj3array[1];
18799bool j3valid[1]={false};
18800_nj3 = 1;
18801CheckValue<IkReal> x1452 = IKatan2WithCheck(IkReal(((-1.0)*sj5)),IkReal(((-1.0)*cj5)),IKFAST_ATAN2_MAGTHRESH);
18802if(!x1452.valid){
18803continue;
18804}
18805CheckValue<IkReal> x1453=IKPowWithIntegerCheck(IKsign(new_r00),-1);
18806if(!x1453.valid){
18807continue;
18808}
18809j3array[0]=((-1.5707963267949)+(x1452.value)+(((1.5707963267949)*(x1453.value))));
18810sj3array[0]=IKsin(j3array[0]);
18811cj3array[0]=IKcos(j3array[0]);
18812if( j3array[0] > IKPI )
18813{
18814 j3array[0]-=IK2PI;
18815}
18816else if( j3array[0] < -IKPI )
18817{ j3array[0]+=IK2PI;
18818}
18819j3valid[0] = true;
18820for(int ij3 = 0; ij3 < 1; ++ij3)
18821{
18822if( !j3valid[ij3] )
18823{
18824 continue;
18825}
18826_ij3[0] = ij3; _ij3[1] = -1;
18827for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18828{
18829if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18830{
18831 j3valid[iij3]=false; _ij3[1] = iij3; break;
18832}
18833}
18834j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18835{
18836IkReal evalcond[7];
18837IkReal x1454=IKcos(j3);
18838IkReal x1455=IKsin(j3);
18839IkReal x1456=((1.0)*sj5);
18840IkReal x1457=(cj5*x1454);
18841IkReal x1458=((1.0)*x1454);
18842evalcond[0]=(sj5+((new_r00*x1455)));
18843evalcond[1]=(cj5+((new_r00*x1454)));
18844evalcond[2]=(cj5+(((-1.0)*new_r11*x1458)));
18845evalcond[3]=((((-1.0)*x1456))+((new_r11*x1455)));
18846evalcond[4]=(((cj5*x1455))+(((-1.0)*x1454*x1456)));
18847evalcond[5]=(((sj5*x1455))+x1457+new_r00);
18848evalcond[6]=((((-1.0)*x1457))+(((-1.0)*x1455*x1456))+new_r11);
18849if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH )
18850{
18851continue;
18852}
18853}
18854
18855{
18856std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18857vinfos[0].jointtype = 1;
18858vinfos[0].foffset = j0;
18859vinfos[0].indices[0] = _ij0[0];
18860vinfos[0].indices[1] = _ij0[1];
18861vinfos[0].maxsolutions = _nj0;
18862vinfos[1].jointtype = 1;
18863vinfos[1].foffset = j1;
18864vinfos[1].indices[0] = _ij1[0];
18865vinfos[1].indices[1] = _ij1[1];
18866vinfos[1].maxsolutions = _nj1;
18867vinfos[2].jointtype = 1;
18868vinfos[2].foffset = j2;
18869vinfos[2].indices[0] = _ij2[0];
18870vinfos[2].indices[1] = _ij2[1];
18871vinfos[2].maxsolutions = _nj2;
18872vinfos[3].jointtype = 1;
18873vinfos[3].foffset = j3;
18874vinfos[3].indices[0] = _ij3[0];
18875vinfos[3].indices[1] = _ij3[1];
18876vinfos[3].maxsolutions = _nj3;
18877vinfos[4].jointtype = 1;
18878vinfos[4].foffset = j4;
18879vinfos[4].indices[0] = _ij4[0];
18880vinfos[4].indices[1] = _ij4[1];
18881vinfos[4].maxsolutions = _nj4;
18882vinfos[5].jointtype = 1;
18883vinfos[5].foffset = j5;
18884vinfos[5].indices[0] = _ij5[0];
18885vinfos[5].indices[1] = _ij5[1];
18886vinfos[5].maxsolutions = _nj5;
18887std::vector<int> vfree(0);
18888solutions.AddSolution(vinfos,vfree);
18889}
18890}
18891}
18892
18893}
18894
18895}
18896
18897}
18898} while(0);
18899if( bgotonextstatement )
18900{
18901bool bgotonextstatement = true;
18902do
18903{
18904if( 1 )
18905{
18906bgotonextstatement=false;
18907continue; // branch miss [j3]
18908
18909}
18910} while(0);
18911if( bgotonextstatement )
18912{
18913}
18914}
18915}
18916}
18917}
18918}
18919}
18920}
18921}
18922}
18923}
18924}
18925
18926} else
18927{
18928{
18929IkReal j3array[1], cj3array[1], sj3array[1];
18930bool j3valid[1]={false};
18931_nj3 = 1;
18932IkReal x1459=((1.0)*new_r00);
18933CheckValue<IkReal> x1460 = IKatan2WithCheck(IkReal(((((-1.0)*(cj5*cj5)))+(new_r00*new_r00))),IkReal(((((-1.0)*new_r10*x1459))+((cj5*sj5)))),IKFAST_ATAN2_MAGTHRESH);
18934if(!x1460.valid){
18935continue;
18936}
18937CheckValue<IkReal> x1461=IKPowWithIntegerCheck(IKsign((((cj5*new_r10))+(((-1.0)*sj5*x1459)))),-1);
18938if(!x1461.valid){
18939continue;
18940}
18941j3array[0]=((-1.5707963267949)+(x1460.value)+(((1.5707963267949)*(x1461.value))));
18942sj3array[0]=IKsin(j3array[0]);
18943cj3array[0]=IKcos(j3array[0]);
18944if( j3array[0] > IKPI )
18945{
18946 j3array[0]-=IK2PI;
18947}
18948else if( j3array[0] < -IKPI )
18949{ j3array[0]+=IK2PI;
18950}
18951j3valid[0] = true;
18952for(int ij3 = 0; ij3 < 1; ++ij3)
18953{
18954if( !j3valid[ij3] )
18955{
18956 continue;
18957}
18958_ij3[0] = ij3; _ij3[1] = -1;
18959for(int iij3 = ij3+1; iij3 < 1; ++iij3)
18960{
18961if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
18962{
18963 j3valid[iij3]=false; _ij3[1] = iij3; break;
18964}
18965}
18966j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
18967{
18968IkReal evalcond[8];
18969IkReal x1462=IKsin(j3);
18970IkReal x1463=IKcos(j3);
18971IkReal x1464=((1.0)*sj5);
18972IkReal x1465=(cj5*x1462);
18973IkReal x1466=((1.0)*x1463);
18974IkReal x1467=(x1463*x1464);
18975evalcond[0]=(((new_r00*x1463))+cj5+((new_r10*x1462)));
18976evalcond[1]=(((cj5*x1463))+new_r00+((sj5*x1462)));
18977evalcond[2]=((((-1.0)*new_r10*x1466))+((new_r00*x1462))+sj5);
18978evalcond[3]=(((new_r01*x1462))+cj5+(((-1.0)*new_r11*x1466)));
18979evalcond[4]=((((-1.0)*x1467))+x1465+new_r01);
18980evalcond[5]=((((-1.0)*x1467))+x1465+new_r10);
18981evalcond[6]=(((new_r01*x1463))+(((-1.0)*x1464))+((new_r11*x1462)));
18982evalcond[7]=((((-1.0)*cj5*x1466))+(((-1.0)*x1462*x1464))+new_r11);
18983if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
18984{
18985continue;
18986}
18987}
18988
18989{
18990std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
18991vinfos[0].jointtype = 1;
18992vinfos[0].foffset = j0;
18993vinfos[0].indices[0] = _ij0[0];
18994vinfos[0].indices[1] = _ij0[1];
18995vinfos[0].maxsolutions = _nj0;
18996vinfos[1].jointtype = 1;
18997vinfos[1].foffset = j1;
18998vinfos[1].indices[0] = _ij1[0];
18999vinfos[1].indices[1] = _ij1[1];
19000vinfos[1].maxsolutions = _nj1;
19001vinfos[2].jointtype = 1;
19002vinfos[2].foffset = j2;
19003vinfos[2].indices[0] = _ij2[0];
19004vinfos[2].indices[1] = _ij2[1];
19005vinfos[2].maxsolutions = _nj2;
19006vinfos[3].jointtype = 1;
19007vinfos[3].foffset = j3;
19008vinfos[3].indices[0] = _ij3[0];
19009vinfos[3].indices[1] = _ij3[1];
19010vinfos[3].maxsolutions = _nj3;
19011vinfos[4].jointtype = 1;
19012vinfos[4].foffset = j4;
19013vinfos[4].indices[0] = _ij4[0];
19014vinfos[4].indices[1] = _ij4[1];
19015vinfos[4].maxsolutions = _nj4;
19016vinfos[5].jointtype = 1;
19017vinfos[5].foffset = j5;
19018vinfos[5].indices[0] = _ij5[0];
19019vinfos[5].indices[1] = _ij5[1];
19020vinfos[5].maxsolutions = _nj5;
19021std::vector<int> vfree(0);
19022solutions.AddSolution(vinfos,vfree);
19023}
19024}
19025}
19026
19027}
19028
19029}
19030
19031} else
19032{
19033{
19034IkReal j3array[1], cj3array[1], sj3array[1];
19035bool j3valid[1]={false};
19036_nj3 = 1;
19037IkReal x1468=((1.0)*new_r10);
19038CheckValue<IkReal> x1469 = IKatan2WithCheck(IkReal((((new_r00*new_r01))+((cj5*sj5)))),IkReal(((((-1.0)*new_r01*x1468))+(cj5*cj5))),IKFAST_ATAN2_MAGTHRESH);
19039if(!x1469.valid){
19040continue;
19041}
19042CheckValue<IkReal> x1470=IKPowWithIntegerCheck(IKsign(((((-1.0)*cj5*new_r00))+(((-1.0)*sj5*x1468)))),-1);
19043if(!x1470.valid){
19044continue;
19045}
19046j3array[0]=((-1.5707963267949)+(x1469.value)+(((1.5707963267949)*(x1470.value))));
19047sj3array[0]=IKsin(j3array[0]);
19048cj3array[0]=IKcos(j3array[0]);
19049if( j3array[0] > IKPI )
19050{
19051 j3array[0]-=IK2PI;
19052}
19053else if( j3array[0] < -IKPI )
19054{ j3array[0]+=IK2PI;
19055}
19056j3valid[0] = true;
19057for(int ij3 = 0; ij3 < 1; ++ij3)
19058{
19059if( !j3valid[ij3] )
19060{
19061 continue;
19062}
19063_ij3[0] = ij3; _ij3[1] = -1;
19064for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19065{
19066if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19067{
19068 j3valid[iij3]=false; _ij3[1] = iij3; break;
19069}
19070}
19071j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19072{
19073IkReal evalcond[8];
19074IkReal x1471=IKsin(j3);
19075IkReal x1472=IKcos(j3);
19076IkReal x1473=((1.0)*sj5);
19077IkReal x1474=(cj5*x1471);
19078IkReal x1475=((1.0)*x1472);
19079IkReal x1476=(x1472*x1473);
19080evalcond[0]=(((new_r10*x1471))+cj5+((new_r00*x1472)));
19081evalcond[1]=(((cj5*x1472))+((sj5*x1471))+new_r00);
19082evalcond[2]=(sj5+((new_r00*x1471))+(((-1.0)*new_r10*x1475)));
19083evalcond[3]=(cj5+(((-1.0)*new_r11*x1475))+((new_r01*x1471)));
19084evalcond[4]=(x1474+(((-1.0)*x1476))+new_r01);
19085evalcond[5]=(x1474+(((-1.0)*x1476))+new_r10);
19086evalcond[6]=(((new_r11*x1471))+((new_r01*x1472))+(((-1.0)*x1473)));
19087evalcond[7]=((((-1.0)*x1471*x1473))+(((-1.0)*cj5*x1475))+new_r11);
19088if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
19089{
19090continue;
19091}
19092}
19093
19094{
19095std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19096vinfos[0].jointtype = 1;
19097vinfos[0].foffset = j0;
19098vinfos[0].indices[0] = _ij0[0];
19099vinfos[0].indices[1] = _ij0[1];
19100vinfos[0].maxsolutions = _nj0;
19101vinfos[1].jointtype = 1;
19102vinfos[1].foffset = j1;
19103vinfos[1].indices[0] = _ij1[0];
19104vinfos[1].indices[1] = _ij1[1];
19105vinfos[1].maxsolutions = _nj1;
19106vinfos[2].jointtype = 1;
19107vinfos[2].foffset = j2;
19108vinfos[2].indices[0] = _ij2[0];
19109vinfos[2].indices[1] = _ij2[1];
19110vinfos[2].maxsolutions = _nj2;
19111vinfos[3].jointtype = 1;
19112vinfos[3].foffset = j3;
19113vinfos[3].indices[0] = _ij3[0];
19114vinfos[3].indices[1] = _ij3[1];
19115vinfos[3].maxsolutions = _nj3;
19116vinfos[4].jointtype = 1;
19117vinfos[4].foffset = j4;
19118vinfos[4].indices[0] = _ij4[0];
19119vinfos[4].indices[1] = _ij4[1];
19120vinfos[4].maxsolutions = _nj4;
19121vinfos[5].jointtype = 1;
19122vinfos[5].foffset = j5;
19123vinfos[5].indices[0] = _ij5[0];
19124vinfos[5].indices[1] = _ij5[1];
19125vinfos[5].maxsolutions = _nj5;
19126std::vector<int> vfree(0);
19127solutions.AddSolution(vinfos,vfree);
19128}
19129}
19130}
19131
19132}
19133
19134}
19135
19136} else
19137{
19138{
19139IkReal j3array[1], cj3array[1], sj3array[1];
19140bool j3valid[1]={false};
19141_nj3 = 1;
19142IkReal x1477=((1.0)*new_r10);
19143CheckValue<IkReal> x1478 = IKatan2WithCheck(IkReal((((cj5*new_r11))+((cj5*new_r00)))),IkReal((((cj5*new_r01))+(((-1.0)*cj5*x1477)))),IKFAST_ATAN2_MAGTHRESH);
19144if(!x1478.valid){
19145continue;
19146}
19147CheckValue<IkReal> x1479=IKPowWithIntegerCheck(IKsign(((((-1.0)*new_r11*x1477))+(((-1.0)*new_r00*new_r01)))),-1);
19148if(!x1479.valid){
19149continue;
19150}
19151j3array[0]=((-1.5707963267949)+(x1478.value)+(((1.5707963267949)*(x1479.value))));
19152sj3array[0]=IKsin(j3array[0]);
19153cj3array[0]=IKcos(j3array[0]);
19154if( j3array[0] > IKPI )
19155{
19156 j3array[0]-=IK2PI;
19157}
19158else if( j3array[0] < -IKPI )
19159{ j3array[0]+=IK2PI;
19160}
19161j3valid[0] = true;
19162for(int ij3 = 0; ij3 < 1; ++ij3)
19163{
19164if( !j3valid[ij3] )
19165{
19166 continue;
19167}
19168_ij3[0] = ij3; _ij3[1] = -1;
19169for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19170{
19171if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19172{
19173 j3valid[iij3]=false; _ij3[1] = iij3; break;
19174}
19175}
19176j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19177{
19178IkReal evalcond[8];
19179IkReal x1480=IKsin(j3);
19180IkReal x1481=IKcos(j3);
19181IkReal x1482=((1.0)*sj5);
19182IkReal x1483=(cj5*x1480);
19183IkReal x1484=((1.0)*x1481);
19184IkReal x1485=(x1481*x1482);
19185evalcond[0]=(cj5+((new_r00*x1481))+((new_r10*x1480)));
19186evalcond[1]=(((cj5*x1481))+new_r00+((sj5*x1480)));
19187evalcond[2]=(sj5+(((-1.0)*new_r10*x1484))+((new_r00*x1480)));
19188evalcond[3]=(cj5+(((-1.0)*new_r11*x1484))+((new_r01*x1480)));
19189evalcond[4]=((((-1.0)*x1485))+x1483+new_r01);
19190evalcond[5]=((((-1.0)*x1485))+x1483+new_r10);
19191evalcond[6]=((((-1.0)*x1482))+((new_r11*x1480))+((new_r01*x1481)));
19192evalcond[7]=((((-1.0)*x1480*x1482))+new_r11+(((-1.0)*cj5*x1484)));
19193if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH )
19194{
19195continue;
19196}
19197}
19198
19199{
19200std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19201vinfos[0].jointtype = 1;
19202vinfos[0].foffset = j0;
19203vinfos[0].indices[0] = _ij0[0];
19204vinfos[0].indices[1] = _ij0[1];
19205vinfos[0].maxsolutions = _nj0;
19206vinfos[1].jointtype = 1;
19207vinfos[1].foffset = j1;
19208vinfos[1].indices[0] = _ij1[0];
19209vinfos[1].indices[1] = _ij1[1];
19210vinfos[1].maxsolutions = _nj1;
19211vinfos[2].jointtype = 1;
19212vinfos[2].foffset = j2;
19213vinfos[2].indices[0] = _ij2[0];
19214vinfos[2].indices[1] = _ij2[1];
19215vinfos[2].maxsolutions = _nj2;
19216vinfos[3].jointtype = 1;
19217vinfos[3].foffset = j3;
19218vinfos[3].indices[0] = _ij3[0];
19219vinfos[3].indices[1] = _ij3[1];
19220vinfos[3].maxsolutions = _nj3;
19221vinfos[4].jointtype = 1;
19222vinfos[4].foffset = j4;
19223vinfos[4].indices[0] = _ij4[0];
19224vinfos[4].indices[1] = _ij4[1];
19225vinfos[4].maxsolutions = _nj4;
19226vinfos[5].jointtype = 1;
19227vinfos[5].foffset = j5;
19228vinfos[5].indices[0] = _ij5[0];
19229vinfos[5].indices[1] = _ij5[1];
19230vinfos[5].maxsolutions = _nj5;
19231std::vector<int> vfree(0);
19232solutions.AddSolution(vinfos,vfree);
19233}
19234}
19235}
19236
19237}
19238
19239}
19240
19241}
19242} while(0);
19243if( bgotonextstatement )
19244{
19245bool bgotonextstatement = true;
19246do
19247{
19248evalcond[0]=((IKabs(new_r12))+(IKabs(new_r02)));
19249if( IKabs(evalcond[0]) < 0.0000050000000000 )
19250{
19251bgotonextstatement=false;
19252{
19253IkReal j3eval[1];
19254new_r02=0;
19255new_r12=0;
19256new_r20=0;
19257new_r21=0;
19258j3eval[0]=((IKabs(new_r11))+(IKabs(new_r01)));
19259if( IKabs(j3eval[0]) < 0.0000010000000000 )
19260{
19261{
19262IkReal j3eval[1];
19263new_r02=0;
19264new_r12=0;
19265new_r20=0;
19266new_r21=0;
19267j3eval[0]=((IKabs(new_r10))+(IKabs(new_r00)));
19268if( IKabs(j3eval[0]) < 0.0000010000000000 )
19269{
19270{
19271IkReal j3eval[1];
19272new_r02=0;
19273new_r12=0;
19274new_r20=0;
19275new_r21=0;
19276j3eval[0]=((IKabs((new_r10*new_r22)))+(IKabs((new_r00*new_r22))));
19277if( IKabs(j3eval[0]) < 0.0000010000000000 )
19278{
19279continue; // no branches [j3]
19280
19281} else
19282{
19283{
19284IkReal j3array[2], cj3array[2], sj3array[2];
19285bool j3valid[2]={false};
19286_nj3 = 2;
19287IkReal x1486=((-1.0)*new_r22);
19288CheckValue<IkReal> x1488 = IKatan2WithCheck(IkReal((new_r00*x1486)),IkReal((new_r10*x1486)),IKFAST_ATAN2_MAGTHRESH);
19289if(!x1488.valid){
19290continue;
19291}
19292IkReal x1487=x1488.value;
19293j3array[0]=((-1.0)*x1487);
19294sj3array[0]=IKsin(j3array[0]);
19295cj3array[0]=IKcos(j3array[0]);
19296j3array[1]=((3.14159265358979)+(((-1.0)*x1487)));
19297sj3array[1]=IKsin(j3array[1]);
19298cj3array[1]=IKcos(j3array[1]);
19299if( j3array[0] > IKPI )
19300{
19301 j3array[0]-=IK2PI;
19302}
19303else if( j3array[0] < -IKPI )
19304{ j3array[0]+=IK2PI;
19305}
19306j3valid[0] = true;
19307if( j3array[1] > IKPI )
19308{
19309 j3array[1]-=IK2PI;
19310}
19311else if( j3array[1] < -IKPI )
19312{ j3array[1]+=IK2PI;
19313}
19314j3valid[1] = true;
19315for(int ij3 = 0; ij3 < 2; ++ij3)
19316{
19317if( !j3valid[ij3] )
19318{
19319 continue;
19320}
19321_ij3[0] = ij3; _ij3[1] = -1;
19322for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19323{
19324if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19325{
19326 j3valid[iij3]=false; _ij3[1] = iij3; break;
19327}
19328}
19329j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19330{
19331IkReal evalcond[5];
19332IkReal x1489=IKsin(j3);
19333IkReal x1490=IKcos(j3);
19334IkReal x1491=((1.0)*new_r11);
19335IkReal x1492=(new_r01*x1490);
19336evalcond[0]=(((new_r11*x1489))+x1492);
19337evalcond[1]=(((new_r00*x1490))+((new_r10*x1489)));
19338evalcond[2]=(((new_r00*x1489))+(((-1.0)*new_r10*x1490)));
19339evalcond[3]=(((new_r01*x1489))+(((-1.0)*x1490*x1491)));
19340evalcond[4]=((((-1.0)*new_r22*x1492))+(((-1.0)*new_r22*x1489*x1491)));
19341if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19342{
19343continue;
19344}
19345}
19346
19347{
19348std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19349vinfos[0].jointtype = 1;
19350vinfos[0].foffset = j0;
19351vinfos[0].indices[0] = _ij0[0];
19352vinfos[0].indices[1] = _ij0[1];
19353vinfos[0].maxsolutions = _nj0;
19354vinfos[1].jointtype = 1;
19355vinfos[1].foffset = j1;
19356vinfos[1].indices[0] = _ij1[0];
19357vinfos[1].indices[1] = _ij1[1];
19358vinfos[1].maxsolutions = _nj1;
19359vinfos[2].jointtype = 1;
19360vinfos[2].foffset = j2;
19361vinfos[2].indices[0] = _ij2[0];
19362vinfos[2].indices[1] = _ij2[1];
19363vinfos[2].maxsolutions = _nj2;
19364vinfos[3].jointtype = 1;
19365vinfos[3].foffset = j3;
19366vinfos[3].indices[0] = _ij3[0];
19367vinfos[3].indices[1] = _ij3[1];
19368vinfos[3].maxsolutions = _nj3;
19369vinfos[4].jointtype = 1;
19370vinfos[4].foffset = j4;
19371vinfos[4].indices[0] = _ij4[0];
19372vinfos[4].indices[1] = _ij4[1];
19373vinfos[4].maxsolutions = _nj4;
19374vinfos[5].jointtype = 1;
19375vinfos[5].foffset = j5;
19376vinfos[5].indices[0] = _ij5[0];
19377vinfos[5].indices[1] = _ij5[1];
19378vinfos[5].maxsolutions = _nj5;
19379std::vector<int> vfree(0);
19380solutions.AddSolution(vinfos,vfree);
19381}
19382}
19383}
19384
19385}
19386
19387}
19388
19389} else
19390{
19391{
19392IkReal j3array[2], cj3array[2], sj3array[2];
19393bool j3valid[2]={false};
19394_nj3 = 2;
19395CheckValue<IkReal> x1494 = IKatan2WithCheck(IkReal(new_r00),IkReal(new_r10),IKFAST_ATAN2_MAGTHRESH);
19396if(!x1494.valid){
19397continue;
19398}
19399IkReal x1493=x1494.value;
19400j3array[0]=((-1.0)*x1493);
19401sj3array[0]=IKsin(j3array[0]);
19402cj3array[0]=IKcos(j3array[0]);
19403j3array[1]=((3.14159265358979)+(((-1.0)*x1493)));
19404sj3array[1]=IKsin(j3array[1]);
19405cj3array[1]=IKcos(j3array[1]);
19406if( j3array[0] > IKPI )
19407{
19408 j3array[0]-=IK2PI;
19409}
19410else if( j3array[0] < -IKPI )
19411{ j3array[0]+=IK2PI;
19412}
19413j3valid[0] = true;
19414if( j3array[1] > IKPI )
19415{
19416 j3array[1]-=IK2PI;
19417}
19418else if( j3array[1] < -IKPI )
19419{ j3array[1]+=IK2PI;
19420}
19421j3valid[1] = true;
19422for(int ij3 = 0; ij3 < 2; ++ij3)
19423{
19424if( !j3valid[ij3] )
19425{
19426 continue;
19427}
19428_ij3[0] = ij3; _ij3[1] = -1;
19429for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19430{
19431if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19432{
19433 j3valid[iij3]=false; _ij3[1] = iij3; break;
19434}
19435}
19436j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19437{
19438IkReal evalcond[5];
19439IkReal x1495=IKsin(j3);
19440IkReal x1496=IKcos(j3);
19441IkReal x1497=((1.0)*x1496);
19442IkReal x1498=((1.0)*new_r22*x1495);
19443evalcond[0]=(((new_r01*x1496))+((new_r11*x1495)));
19444evalcond[1]=((((-1.0)*new_r10*x1497))+((new_r00*x1495)));
19445evalcond[2]=(((new_r01*x1495))+(((-1.0)*new_r11*x1497)));
19446evalcond[3]=((((-1.0)*new_r10*x1498))+(((-1.0)*new_r00*new_r22*x1497)));
19447evalcond[4]=((((-1.0)*new_r11*x1498))+(((-1.0)*new_r01*new_r22*x1497)));
19448if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19449{
19450continue;
19451}
19452}
19453
19454{
19455std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19456vinfos[0].jointtype = 1;
19457vinfos[0].foffset = j0;
19458vinfos[0].indices[0] = _ij0[0];
19459vinfos[0].indices[1] = _ij0[1];
19460vinfos[0].maxsolutions = _nj0;
19461vinfos[1].jointtype = 1;
19462vinfos[1].foffset = j1;
19463vinfos[1].indices[0] = _ij1[0];
19464vinfos[1].indices[1] = _ij1[1];
19465vinfos[1].maxsolutions = _nj1;
19466vinfos[2].jointtype = 1;
19467vinfos[2].foffset = j2;
19468vinfos[2].indices[0] = _ij2[0];
19469vinfos[2].indices[1] = _ij2[1];
19470vinfos[2].maxsolutions = _nj2;
19471vinfos[3].jointtype = 1;
19472vinfos[3].foffset = j3;
19473vinfos[3].indices[0] = _ij3[0];
19474vinfos[3].indices[1] = _ij3[1];
19475vinfos[3].maxsolutions = _nj3;
19476vinfos[4].jointtype = 1;
19477vinfos[4].foffset = j4;
19478vinfos[4].indices[0] = _ij4[0];
19479vinfos[4].indices[1] = _ij4[1];
19480vinfos[4].maxsolutions = _nj4;
19481vinfos[5].jointtype = 1;
19482vinfos[5].foffset = j5;
19483vinfos[5].indices[0] = _ij5[0];
19484vinfos[5].indices[1] = _ij5[1];
19485vinfos[5].maxsolutions = _nj5;
19486std::vector<int> vfree(0);
19487solutions.AddSolution(vinfos,vfree);
19488}
19489}
19490}
19491
19492}
19493
19494}
19495
19496} else
19497{
19498{
19499IkReal j3array[2], cj3array[2], sj3array[2];
19500bool j3valid[2]={false};
19501_nj3 = 2;
19502CheckValue<IkReal> x1500 = IKatan2WithCheck(IkReal(new_r01),IkReal(new_r11),IKFAST_ATAN2_MAGTHRESH);
19503if(!x1500.valid){
19504continue;
19505}
19506IkReal x1499=x1500.value;
19507j3array[0]=((-1.0)*x1499);
19508sj3array[0]=IKsin(j3array[0]);
19509cj3array[0]=IKcos(j3array[0]);
19510j3array[1]=((3.14159265358979)+(((-1.0)*x1499)));
19511sj3array[1]=IKsin(j3array[1]);
19512cj3array[1]=IKcos(j3array[1]);
19513if( j3array[0] > IKPI )
19514{
19515 j3array[0]-=IK2PI;
19516}
19517else if( j3array[0] < -IKPI )
19518{ j3array[0]+=IK2PI;
19519}
19520j3valid[0] = true;
19521if( j3array[1] > IKPI )
19522{
19523 j3array[1]-=IK2PI;
19524}
19525else if( j3array[1] < -IKPI )
19526{ j3array[1]+=IK2PI;
19527}
19528j3valid[1] = true;
19529for(int ij3 = 0; ij3 < 2; ++ij3)
19530{
19531if( !j3valid[ij3] )
19532{
19533 continue;
19534}
19535_ij3[0] = ij3; _ij3[1] = -1;
19536for(int iij3 = ij3+1; iij3 < 2; ++iij3)
19537{
19538if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19539{
19540 j3valid[iij3]=false; _ij3[1] = iij3; break;
19541}
19542}
19543j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19544{
19545IkReal evalcond[5];
19546IkReal x1501=IKsin(j3);
19547IkReal x1502=IKcos(j3);
19548IkReal x1503=((1.0)*new_r22);
19549IkReal x1504=(new_r10*x1501);
19550IkReal x1505=((1.0)*x1502);
19551IkReal x1506=(new_r00*x1502);
19552evalcond[0]=(x1506+x1504);
19553evalcond[1]=((((-1.0)*new_r10*x1505))+((new_r00*x1501)));
19554evalcond[2]=((((-1.0)*new_r11*x1505))+((new_r01*x1501)));
19555evalcond[3]=((((-1.0)*x1503*x1506))+(((-1.0)*x1503*x1504)));
19556evalcond[4]=((((-1.0)*new_r11*x1501*x1503))+(((-1.0)*new_r01*x1502*x1503)));
19557if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH )
19558{
19559continue;
19560}
19561}
19562
19563{
19564std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19565vinfos[0].jointtype = 1;
19566vinfos[0].foffset = j0;
19567vinfos[0].indices[0] = _ij0[0];
19568vinfos[0].indices[1] = _ij0[1];
19569vinfos[0].maxsolutions = _nj0;
19570vinfos[1].jointtype = 1;
19571vinfos[1].foffset = j1;
19572vinfos[1].indices[0] = _ij1[0];
19573vinfos[1].indices[1] = _ij1[1];
19574vinfos[1].maxsolutions = _nj1;
19575vinfos[2].jointtype = 1;
19576vinfos[2].foffset = j2;
19577vinfos[2].indices[0] = _ij2[0];
19578vinfos[2].indices[1] = _ij2[1];
19579vinfos[2].maxsolutions = _nj2;
19580vinfos[3].jointtype = 1;
19581vinfos[3].foffset = j3;
19582vinfos[3].indices[0] = _ij3[0];
19583vinfos[3].indices[1] = _ij3[1];
19584vinfos[3].maxsolutions = _nj3;
19585vinfos[4].jointtype = 1;
19586vinfos[4].foffset = j4;
19587vinfos[4].indices[0] = _ij4[0];
19588vinfos[4].indices[1] = _ij4[1];
19589vinfos[4].maxsolutions = _nj4;
19590vinfos[5].jointtype = 1;
19591vinfos[5].foffset = j5;
19592vinfos[5].indices[0] = _ij5[0];
19593vinfos[5].indices[1] = _ij5[1];
19594vinfos[5].maxsolutions = _nj5;
19595std::vector<int> vfree(0);
19596solutions.AddSolution(vinfos,vfree);
19597}
19598}
19599}
19600
19601}
19602
19603}
19604
19605}
19606} while(0);
19607if( bgotonextstatement )
19608{
19609bool bgotonextstatement = true;
19610do
19611{
19612if( 1 )
19613{
19614bgotonextstatement=false;
19615continue; // branch miss [j3]
19616
19617}
19618} while(0);
19619if( bgotonextstatement )
19620{
19621}
19622}
19623}
19624}
19625}
19626
19627} else
19628{
19629{
19630IkReal j3array[1], cj3array[1], sj3array[1];
19631bool j3valid[1]={false};
19632_nj3 = 1;
19633CheckValue<IkReal> x1508=IKPowWithIntegerCheck(sj4,-1);
19634if(!x1508.valid){
19635continue;
19636}
19637IkReal x1507=x1508.value;
19638CheckValue<IkReal> x1509=IKPowWithIntegerCheck(new_r12,-1);
19639if(!x1509.valid){
19640continue;
19641}
19642if( IKabs((x1507*(x1509.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((-1.0)*new_r02*x1507)) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr((x1507*(x1509.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))))+IKsqr(((-1.0)*new_r02*x1507))-1) <= IKFAST_SINCOS_THRESH )
19643 continue;
19644j3array[0]=IKatan2((x1507*(x1509.value)*(((-1.0)+(new_r02*new_r02)+(cj4*cj4)))), ((-1.0)*new_r02*x1507));
19645sj3array[0]=IKsin(j3array[0]);
19646cj3array[0]=IKcos(j3array[0]);
19647if( j3array[0] > IKPI )
19648{
19649 j3array[0]-=IK2PI;
19650}
19651else if( j3array[0] < -IKPI )
19652{ j3array[0]+=IK2PI;
19653}
19654j3valid[0] = true;
19655for(int ij3 = 0; ij3 < 1; ++ij3)
19656{
19657if( !j3valid[ij3] )
19658{
19659 continue;
19660}
19661_ij3[0] = ij3; _ij3[1] = -1;
19662for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19663{
19664if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19665{
19666 j3valid[iij3]=false; _ij3[1] = iij3; break;
19667}
19668}
19669j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19670{
19671IkReal evalcond[18];
19672IkReal x1510=IKcos(j3);
19673IkReal x1511=IKsin(j3);
19674IkReal x1512=(cj4*cj5);
19675IkReal x1513=(cj4*sj5);
19676IkReal x1514=((1.0)*new_r20);
19677IkReal x1515=((1.0)*cj4);
19678IkReal x1516=((1.0)*sj4);
19679IkReal x1517=(sj4*x1511);
19680IkReal x1518=((1.0)*x1510);
19681IkReal x1519=(sj4*x1510);
19682IkReal x1520=(x1511*x1515);
19683evalcond[0]=(x1519+new_r02);
19684evalcond[1]=(x1517+new_r12);
19685evalcond[2]=((((-1.0)*new_r02*x1511))+((new_r12*x1510)));
19686evalcond[3]=(sj4+((new_r02*x1510))+((new_r12*x1511)));
19687evalcond[4]=(sj5+(((-1.0)*new_r10*x1518))+((new_r00*x1511)));
19688evalcond[5]=((((-1.0)*new_r11*x1518))+cj5+((new_r01*x1511)));
19689evalcond[6]=(((x1510*x1513))+((cj5*x1511))+new_r01);
19690evalcond[7]=(x1513+((new_r01*x1510))+((new_r11*x1511)));
19691evalcond[8]=(((sj5*x1511))+(((-1.0)*x1512*x1518))+new_r00);
19692evalcond[9]=((((-1.0)*cj5*x1518))+new_r11+((x1511*x1513)));
19693evalcond[10]=(((new_r10*x1511))+(((-1.0)*x1512))+((new_r00*x1510)));
19694evalcond[11]=((((-1.0)*x1511*x1512))+(((-1.0)*sj5*x1518))+new_r10);
19695evalcond[12]=(((new_r10*x1517))+(((-1.0)*cj4*x1514))+((new_r00*x1519)));
19696evalcond[13]=((((-1.0)*new_r21*x1515))+((new_r01*x1519))+((new_r11*x1517)));
19697evalcond[14]=((1.0)+(((-1.0)*new_r22*x1515))+((new_r02*x1519))+((new_r12*x1517)));
19698evalcond[15]=((((-1.0)*new_r22*x1516))+(((-1.0)*new_r12*x1520))+(((-1.0)*new_r02*x1510*x1515)));
19699evalcond[16]=(cj5+(((-1.0)*new_r00*x1510*x1515))+(((-1.0)*sj4*x1514))+(((-1.0)*new_r10*x1520)));
19700evalcond[17]=((((-1.0)*sj5))+(((-1.0)*new_r11*x1520))+(((-1.0)*new_r21*x1516))+(((-1.0)*new_r01*x1510*x1515)));
19701if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
19702{
19703continue;
19704}
19705}
19706
19707{
19708std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19709vinfos[0].jointtype = 1;
19710vinfos[0].foffset = j0;
19711vinfos[0].indices[0] = _ij0[0];
19712vinfos[0].indices[1] = _ij0[1];
19713vinfos[0].maxsolutions = _nj0;
19714vinfos[1].jointtype = 1;
19715vinfos[1].foffset = j1;
19716vinfos[1].indices[0] = _ij1[0];
19717vinfos[1].indices[1] = _ij1[1];
19718vinfos[1].maxsolutions = _nj1;
19719vinfos[2].jointtype = 1;
19720vinfos[2].foffset = j2;
19721vinfos[2].indices[0] = _ij2[0];
19722vinfos[2].indices[1] = _ij2[1];
19723vinfos[2].maxsolutions = _nj2;
19724vinfos[3].jointtype = 1;
19725vinfos[3].foffset = j3;
19726vinfos[3].indices[0] = _ij3[0];
19727vinfos[3].indices[1] = _ij3[1];
19728vinfos[3].maxsolutions = _nj3;
19729vinfos[4].jointtype = 1;
19730vinfos[4].foffset = j4;
19731vinfos[4].indices[0] = _ij4[0];
19732vinfos[4].indices[1] = _ij4[1];
19733vinfos[4].maxsolutions = _nj4;
19734vinfos[5].jointtype = 1;
19735vinfos[5].foffset = j5;
19736vinfos[5].indices[0] = _ij5[0];
19737vinfos[5].indices[1] = _ij5[1];
19738vinfos[5].maxsolutions = _nj5;
19739std::vector<int> vfree(0);
19740solutions.AddSolution(vinfos,vfree);
19741}
19742}
19743}
19744
19745}
19746
19747}
19748
19749} else
19750{
19751{
19752IkReal j3array[1], cj3array[1], sj3array[1];
19753bool j3valid[1]={false};
19754_nj3 = 1;
19755CheckValue<IkReal> x1521=IKPowWithIntegerCheck(IKsign(sj4),-1);
19756if(!x1521.valid){
19757continue;
19758}
19759CheckValue<IkReal> x1522 = IKatan2WithCheck(IkReal(((-1.0)*new_r12)),IkReal(((-1.0)*new_r02)),IKFAST_ATAN2_MAGTHRESH);
19760if(!x1522.valid){
19761continue;
19762}
19763j3array[0]=((-1.5707963267949)+(((1.5707963267949)*(x1521.value)))+(x1522.value));
19764sj3array[0]=IKsin(j3array[0]);
19765cj3array[0]=IKcos(j3array[0]);
19766if( j3array[0] > IKPI )
19767{
19768 j3array[0]-=IK2PI;
19769}
19770else if( j3array[0] < -IKPI )
19771{ j3array[0]+=IK2PI;
19772}
19773j3valid[0] = true;
19774for(int ij3 = 0; ij3 < 1; ++ij3)
19775{
19776if( !j3valid[ij3] )
19777{
19778 continue;
19779}
19780_ij3[0] = ij3; _ij3[1] = -1;
19781for(int iij3 = ij3+1; iij3 < 1; ++iij3)
19782{
19783if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
19784{
19785 j3valid[iij3]=false; _ij3[1] = iij3; break;
19786}
19787}
19788j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
19789{
19790IkReal evalcond[18];
19791IkReal x1523=IKcos(j3);
19792IkReal x1524=IKsin(j3);
19793IkReal x1525=(cj4*cj5);
19794IkReal x1526=(cj4*sj5);
19795IkReal x1527=((1.0)*new_r20);
19796IkReal x1528=((1.0)*cj4);
19797IkReal x1529=((1.0)*sj4);
19798IkReal x1530=(sj4*x1524);
19799IkReal x1531=((1.0)*x1523);
19800IkReal x1532=(sj4*x1523);
19801IkReal x1533=(x1524*x1528);
19802evalcond[0]=(x1532+new_r02);
19803evalcond[1]=(x1530+new_r12);
19804evalcond[2]=(((new_r12*x1523))+(((-1.0)*new_r02*x1524)));
19805evalcond[3]=(((new_r12*x1524))+sj4+((new_r02*x1523)));
19806evalcond[4]=((((-1.0)*new_r10*x1531))+sj5+((new_r00*x1524)));
19807evalcond[5]=(cj5+((new_r01*x1524))+(((-1.0)*new_r11*x1531)));
19808evalcond[6]=(((x1523*x1526))+((cj5*x1524))+new_r01);
19809evalcond[7]=(((new_r01*x1523))+x1526+((new_r11*x1524)));
19810evalcond[8]=(((sj5*x1524))+(((-1.0)*x1525*x1531))+new_r00);
19811evalcond[9]=(((x1524*x1526))+(((-1.0)*cj5*x1531))+new_r11);
19812evalcond[10]=(((new_r00*x1523))+((new_r10*x1524))+(((-1.0)*x1525)));
19813evalcond[11]=((((-1.0)*x1524*x1525))+new_r10+(((-1.0)*sj5*x1531)));
19814evalcond[12]=((((-1.0)*cj4*x1527))+((new_r00*x1532))+((new_r10*x1530)));
19815evalcond[13]=((((-1.0)*new_r21*x1528))+((new_r01*x1532))+((new_r11*x1530)));
19816evalcond[14]=((1.0)+((new_r02*x1532))+(((-1.0)*new_r22*x1528))+((new_r12*x1530)));
19817evalcond[15]=((((-1.0)*new_r02*x1523*x1528))+(((-1.0)*new_r22*x1529))+(((-1.0)*new_r12*x1533)));
19818evalcond[16]=((((-1.0)*new_r10*x1533))+(((-1.0)*sj4*x1527))+cj5+(((-1.0)*new_r00*x1523*x1528)));
19819evalcond[17]=((((-1.0)*sj5))+(((-1.0)*new_r21*x1529))+(((-1.0)*new_r01*x1523*x1528))+(((-1.0)*new_r11*x1533)));
19820if( IKabs(evalcond[0]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[1]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[2]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[3]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[4]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[5]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[6]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[7]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[8]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[9]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[10]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[11]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[12]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[13]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[14]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[15]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[16]) > IKFAST_EVALCOND_THRESH || IKabs(evalcond[17]) > IKFAST_EVALCOND_THRESH )
19821{
19822continue;
19823}
19824}
19825
19826{
19827std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
19828vinfos[0].jointtype = 1;
19829vinfos[0].foffset = j0;
19830vinfos[0].indices[0] = _ij0[0];
19831vinfos[0].indices[1] = _ij0[1];
19832vinfos[0].maxsolutions = _nj0;
19833vinfos[1].jointtype = 1;
19834vinfos[1].foffset = j1;
19835vinfos[1].indices[0] = _ij1[0];
19836vinfos[1].indices[1] = _ij1[1];
19837vinfos[1].maxsolutions = _nj1;
19838vinfos[2].jointtype = 1;
19839vinfos[2].foffset = j2;
19840vinfos[2].indices[0] = _ij2[0];
19841vinfos[2].indices[1] = _ij2[1];
19842vinfos[2].maxsolutions = _nj2;
19843vinfos[3].jointtype = 1;
19844vinfos[3].foffset = j3;
19845vinfos[3].indices[0] = _ij3[0];
19846vinfos[3].indices[1] = _ij3[1];
19847vinfos[3].maxsolutions = _nj3;
19848vinfos[4].jointtype = 1;
19849vinfos[4].foffset = j4;
19850vinfos[4].indices[0] = _ij4[0];
19851vinfos[4].indices[1] = _ij4[1];
19852vinfos[4].maxsolutions = _nj4;
19853vinfos[5].jointtype = 1;
19854vinfos[5].foffset = j5;
19855vinfos[5].indices[0] = _ij5[0];
19856vinfos[5].indices[1] = _ij5[1];
19857vinfos[5].maxsolutions = _nj5;
19858std::vector<int> vfree(0);
19859solutions.AddSolution(vinfos,vfree);
19860}
19861}
19862}
19863
19864}
19865
19866}
19867}
19868}
19869
19870}
19871
19872}
19873}
19874}
19875}
19876}static inline void polyroots3(IkReal rawcoeffs[3+1], IkReal rawroots[3], int& numroots)
19877{
19878 using std::complex;
19879 if( rawcoeffs[0] == 0 ) {
19880 // solve with one reduced degree
19881 polyroots2(&rawcoeffs[1], &rawroots[0], numroots);
19882 return;
19883 }
19884 IKFAST_ASSERT(rawcoeffs[0] != 0);
19885 const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19886 const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
19887 complex<IkReal> coeffs[3];
19888 const int maxsteps = 110;
19889 for(int i = 0; i < 3; ++i) {
19890 coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19891 }
19892 complex<IkReal> roots[3];
19893 IkReal err[3];
19894 roots[0] = complex<IkReal>(1,0);
19895 roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
19896 err[0] = 1.0;
19897 err[1] = 1.0;
19898 for(int i = 2; i < 3; ++i) {
19899 roots[i] = roots[i-1]*roots[1];
19900 err[i] = 1.0;
19901 }
19902 for(int step = 0; step < maxsteps; ++step) {
19903 bool changed = false;
19904 for(int i = 0; i < 3; ++i) {
19905 if ( err[i] >= tol ) {
19906 changed = true;
19907 // evaluate
19908 complex<IkReal> x = roots[i] + coeffs[0];
19909 for(int j = 1; j < 3; ++j) {
19910 x = roots[i] * x + coeffs[j];
19911 }
19912 for(int j = 0; j < 3; ++j) {
19913 if( i != j ) {
19914 if( roots[i] != roots[j] ) {
19915 x /= (roots[i] - roots[j]);
19916 }
19917 }
19918 }
19919 roots[i] -= x;
19920 err[i] = abs(x);
19921 }
19922 }
19923 if( !changed ) {
19924 break;
19925 }
19926 }
19927
19928 numroots = 0;
19929 bool visited[3] = {false};
19930 for(int i = 0; i < 3; ++i) {
19931 if( !visited[i] ) {
19932 // might be a multiple root, in which case it will have more error than the other roots
19933 // find any neighboring roots, and take the average
19934 complex<IkReal> newroot=roots[i];
19935 int n = 1;
19936 for(int j = i+1; j < 3; ++j) {
19937 // care about error in real much more than imaginary
19938 if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
19939 newroot += roots[j];
19940 n += 1;
19941 visited[j] = true;
19942 }
19943 }
19944 if( n > 1 ) {
19945 newroot /= n;
19946 }
19947 // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
19948 if( IKabs(imag(newroot)) < tolsqrt ) {
19949 rawroots[numroots++] = real(newroot);
19950 }
19951 }
19952 }
19953}
19954static inline void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int& numroots) {
19955 IkReal det = rawcoeffs[1]*rawcoeffs[1]-4*rawcoeffs[0]*rawcoeffs[2];
19956 if( det < 0 ) {
19957 numroots=0;
19958 }
19959 else if( det == 0 ) {
19960 rawroots[0] = -0.5*rawcoeffs[1]/rawcoeffs[0];
19961 numroots = 1;
19962 }
19963 else {
19964 det = IKsqrt(det);
19965 rawroots[0] = (-rawcoeffs[1]+det)/(2*rawcoeffs[0]);
19966 rawroots[1] = (-rawcoeffs[1]-det)/(2*rawcoeffs[0]);//rawcoeffs[2]/(rawcoeffs[0]*rawroots[0]);
19967 numroots = 2;
19968 }
19969}
19970static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
19971{
19972 using std::complex;
19973 if( rawcoeffs[0] == 0 ) {
19974 // solve with one reduced degree
19975 polyroots3(&rawcoeffs[1], &rawroots[0], numroots);
19976 return;
19977 }
19978 IKFAST_ASSERT(rawcoeffs[0] != 0);
19979 const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
19980 const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
19981 complex<IkReal> coeffs[4];
19982 const int maxsteps = 110;
19983 for(int i = 0; i < 4; ++i) {
19984 coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
19985 }
19986 complex<IkReal> roots[4];
19987 IkReal err[4];
19988 roots[0] = complex<IkReal>(1,0);
19989 roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
19990 err[0] = 1.0;
19991 err[1] = 1.0;
19992 for(int i = 2; i < 4; ++i) {
19993 roots[i] = roots[i-1]*roots[1];
19994 err[i] = 1.0;
19995 }
19996 for(int step = 0; step < maxsteps; ++step) {
19997 bool changed = false;
19998 for(int i = 0; i < 4; ++i) {
19999 if ( err[i] >= tol ) {
20000 changed = true;
20001 // evaluate
20002 complex<IkReal> x = roots[i] + coeffs[0];
20003 for(int j = 1; j < 4; ++j) {
20004 x = roots[i] * x + coeffs[j];
20005 }
20006 for(int j = 0; j < 4; ++j) {
20007 if( i != j ) {
20008 if( roots[i] != roots[j] ) {
20009 x /= (roots[i] - roots[j]);
20010 }
20011 }
20012 }
20013 roots[i] -= x;
20014 err[i] = abs(x);
20015 }
20016 }
20017 if( !changed ) {
20018 break;
20019 }
20020 }
20021
20022 numroots = 0;
20023 bool visited[4] = {false};
20024 for(int i = 0; i < 4; ++i) {
20025 if( !visited[i] ) {
20026 // might be a multiple root, in which case it will have more error than the other roots
20027 // find any neighboring roots, and take the average
20028 complex<IkReal> newroot=roots[i];
20029 int n = 1;
20030 for(int j = i+1; j < 4; ++j) {
20031 // care about error in real much more than imaginary
20032 if( abs(real(roots[i])-real(roots[j])) < tolsqrt && abs(imag(roots[i])-imag(roots[j])) < 0.002 ) {
20033 newroot += roots[j];
20034 n += 1;
20035 visited[j] = true;
20036 }
20037 }
20038 if( n > 1 ) {
20039 newroot /= n;
20040 }
20041 // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
20042 if( IKabs(imag(newroot)) < tolsqrt ) {
20043 rawroots[numroots++] = real(newroot);
20044 }
20045 }
20046 }
20047}
20048};
20049
20050
20053IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
20054IKSolver solver;
20055return solver.ComputeIk(eetrans,eerot,pfree,solutions);
20056}
20057
20058IKFAST_API bool ComputeIk2(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions, void* /* unused */) {
20059IKSolver solver;
20060return solver.ComputeIk(eetrans,eerot,pfree,solutions);
20061}
20062
20063IKFAST_API const char* GetKinematicsHash() { return "<robot:GenericRobot - prbt (11ad975bd60aa3f28e8d228465d0f213)>"; }
20064
20065IKFAST_API const char* GetIkFastVersion() { return "0x1000004a"; }
20066
20067#ifdef IKFAST_NAMESPACE
20068} // end namespace
20069#endif
20070
20071#ifndef IKFAST_NO_MAIN
20072#include <stdio.h>
20073#include <stdlib.h>
20074#ifdef IKFAST_NAMESPACE
20075using namespace IKFAST_NAMESPACE;
20076#endif
20077int main(int argc, char** argv)
20078{
20079 if( argc != 12+GetNumFreeParameters()+1 ) {
20080 printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
20081 "Returns the ik solutions given the transformation of the end effector specified by\n"
20082 "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
20083 "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
20084 return 1;
20085 }
20086
20087 IkSolutionList<IkReal> solutions;
20088 std::vector<IkReal> vfree(GetNumFreeParameters());
20089 IkReal eerot[9],eetrans[3];
20090 eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
20091 eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
20092 eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
20093 for(std::size_t i = 0; i < vfree.size(); ++i)
20094 vfree[i] = atof(argv[13+i]);
20095 bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : nullptr, solutions);
20096
20097 if( !bSuccess ) {
20098 fprintf(stderr,"Failed to get ik solution\n");
20099 return -1;
20100 }
20101
20102 printf("Found %d ik solutions:\n", static_cast<int>(solutions.GetNumSolutions()));
20103 std::vector<IkReal> solvalues(GetNumJoints());
20104 for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
20105 const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
20106 printf("sol%d (free=%d): ", static_cast<int>(i), static_cast<int>(sol.GetFree().size()));
20107 std::vector<IkReal> vsolfree(sol.GetFree().size());
20108 sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:nullptr);
20109 for( std::size_t j = 0; j < solvalues.size(); ++j)
20110 printf("%.15f, ", solvalues[j]);
20111 printf("\n");
20112 }
20113 return 0;
20114}
20115
20116#endif
IKSolver::rotationfunction0
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
Definition prbt_manipulator_ikfast_solver.cpp:866
IKSolver::polyroots2
static void polyroots2(IkReal rawcoeffs[2+1], IkReal rawroots[2], int &numroots)
Definition prbt_manipulator_ikfast_solver.cpp:19954
IKSolver::polyroots4
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
Definition prbt_manipulator_ikfast_solver.cpp:19970
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *, IkSolutionListBase< IkReal > &solutions)
Definition prbt_manipulator_ikfast_solver.cpp:372
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition ikfast.h:73
ikfast::IkSolutionBase::GetFree
virtual const std::vector< int > & GetFree() const =0
Gets the indices of the configuration space that have to be preset before a full solution can be retu...
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
ikfast::IkSolutionListBase
manages all the solutions
Definition ikfast.h:103
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
ikfast::IkSolutionListBase::AddSolution
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition ikfast.h:277
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition ikfast.h:286
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition ikfast.h:297
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition ikfast.h:45
IKPI
#define IKPI
Definition prbt_manipulator_ikfast_solver.cpp:63
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition prbt_manipulator_ikfast_solver.cpp:100
ComputeIk2
IKFAST_API bool ComputeIk2(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions, void *)
Definition prbt_manipulator_ikfast_solver.cpp:20058
IKatan2WithCheck
CheckValue< T > IKatan2WithCheck(T fy, T fx, T)
Definition prbt_manipulator_ikfast_solver.cpp:204
IKasin
float IKasin(float f)
Definition prbt_manipulator_ikfast_solver.cpp:114
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition prbt_manipulator_ikfast_solver.cpp:358
IKatan2Simple
float IKatan2Simple(float fy, float fx)
Definition prbt_manipulator_ikfast_solver.cpp:169
main
int main(int argc, char **argv)
Definition prbt_manipulator_ikfast_solver.cpp:20077
GetNumJoints
IKFAST_API int GetNumJoints()
Definition prbt_manipulator_ikfast_solver.cpp:359
IKPI_2
#define IKPI_2
Definition prbt_manipulator_ikfast_solver.cpp:64
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition prbt_manipulator_ikfast_solver.cpp:20065
IKatan2
float IKatan2(float fy, float fx)
Definition prbt_manipulator_ikfast_solver.cpp:172
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition prbt_manipulator_ikfast_solver.cpp:95
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition prbt_manipulator_ikfast_solver.cpp:361
IKacos
float IKacos(float f)
Definition prbt_manipulator_ikfast_solver.cpp:147
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition prbt_manipulator_ikfast_solver.cpp:20063
IKFAST_EVALCOND_THRESH
#define IKFAST_EVALCOND_THRESH
Definition prbt_manipulator_ikfast_solver.cpp:110
IKsqrt
float IKsqrt(float f)
Definition prbt_manipulator_ikfast_solver.cpp:167
IKsin
float IKsin(float f)
Definition prbt_manipulator_ikfast_solver.cpp:161
IKsign
float IKsign(float f)
Definition prbt_manipulator_ikfast_solver.cpp:218
IKfmod
float IKfmod(float x, float y)
Definition prbt_manipulator_ikfast_solver.cpp:130
IKcos
float IKcos(float f)
Definition prbt_manipulator_ikfast_solver.cpp:163
IKsqr
float IKsqr(float f)
Definition prbt_manipulator_ikfast_solver.cpp:87
GetIkType
IKFAST_API int GetIkType()
Definition prbt_manipulator_ikfast_solver.cpp:363
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition prbt_manipulator_ikfast_solver.cpp:292
IKtan
float IKtan(float f)
Definition prbt_manipulator_ikfast_solver.cpp:165
IK2PI
#define IK2PI
Definition prbt_manipulator_ikfast_solver.cpp:62
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition prbt_manipulator_ikfast_solver.cpp:20053
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition prbt_manipulator_ikfast_solver.cpp:105
IKPowWithIntegerCheck
CheckValue< T > IKPowWithIntegerCheck(T f, int n)
Definition prbt_manipulator_ikfast_solver.cpp:239
IKlog
float IKlog(float f)
Definition prbt_manipulator_ikfast_solver.cpp:90
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition prbt_manipulator_ikfast_solver.cpp:357
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition prbt_manipulator_ikfast_solver.cpp:52
IKabs
float IKabs(float f)
Definition prbt_manipulator_ikfast_solver.cpp:84
Generated by doxygen 1.9.8