fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
21 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
22 using namespace ikfast;
23 
24 // check if the included ikfast version matches what this file was compiled with
25 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
26 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
27 
28 #include <cmath>
29 #include <vector>
30 #include <limits>
31 #include <algorithm>
32 #include <complex>
33 
34 #define IKFAST_STRINGIZE2(s) #s
35 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
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 #endif // _MSC_VER
71 
72 // lapack routines
73 extern "C" {
74  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
75  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
76  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
77  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
78  void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
79  void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
80 }
81 
82 using namespace std; // necessary to get std math routines
83 
84 #ifdef IKFAST_NAMESPACE
85 namespace IKFAST_NAMESPACE {
86 #endif
87 
88 inline float IKabs(float f) { return fabsf(f); }
89 inline double IKabs(double f) { return fabs(f); }
90 
91 inline float IKsqr(float f) { return f*f; }
92 inline double IKsqr(double f) { return f*f; }
93 
94 inline float IKlog(float f) { return logf(f); }
95 inline double IKlog(double f) { return log(f); }
96 
97 // allows asin and acos to exceed 1
98 #ifndef IKFAST_SINCOS_THRESH
99 #define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
100 #endif
101 
102 // used to check input to atan2 for degenerate cases
103 #ifndef IKFAST_ATAN2_MAGTHRESH
104 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
105 #endif
106 
107 // minimum distance of separate solutions
108 #ifndef IKFAST_SOLUTION_THRESH
109 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
110 #endif
111 
112 inline float IKasin(float f)
113 {
114 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
115 if( f <= -1 ) return float(-IKPI_2);
116 else if( f >= 1 ) return float(IKPI_2);
117 return asinf(f);
118 }
119 inline double IKasin(double f)
120 {
121 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
122 if( f <= -1 ) return -IKPI_2;
123 else if( f >= 1 ) return IKPI_2;
124 return asin(f);
125 }
126 
127 // return positive value in [0,y)
128 inline float IKfmod(float x, float y)
129 {
130  while(x < 0) {
131  x += y;
132  }
133  return fmodf(x,y);
134 }
135 
136 // return positive value in [0,y)
137 inline double IKfmod(double x, double y)
138 {
139  while(x < 0) {
140  x += y;
141  }
142  return fmod(x,y);
143 }
144 
145 inline float IKacos(float f)
146 {
147 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
148 if( f <= -1 ) return float(IKPI);
149 else if( f >= 1 ) return float(0);
150 return acosf(f);
151 }
152 inline double IKacos(double f)
153 {
154 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
155 if( f <= -1 ) return IKPI;
156 else if( f >= 1 ) return 0;
157 return acos(f);
158 }
159 inline float IKsin(float f) { return sinf(f); }
160 inline double IKsin(double f) { return sin(f); }
161 inline float IKcos(float f) { return cosf(f); }
162 inline double IKcos(double f) { return cos(f); }
163 inline float IKtan(float f) { return tanf(f); }
164 inline double IKtan(double f) { return tan(f); }
165 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
166 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
167 inline float IKatan2(float fy, float fx) {
168  if( isnan(fy) ) {
169  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
170  return float(IKPI_2);
171  }
172  else if( isnan(fx) ) {
173  return 0;
174  }
175  return atan2f(fy,fx);
176 }
177 inline double IKatan2(double fy, double fx) {
178  if( isnan(fy) ) {
179  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
180  return IKPI_2;
181  }
182  else if( isnan(fx) ) {
183  return 0;
184  }
185  return atan2(fy,fx);
186 }
187 
188 inline float IKsign(float f) {
189  if( f > 0 ) {
190  return float(1);
191  }
192  else if( f < 0 ) {
193  return float(-1);
194  }
195  return 0;
196 }
197 
198 inline double IKsign(double f) {
199  if( f > 0 ) {
200  return 1.0;
201  }
202  else if( f < 0 ) {
203  return -1.0;
204  }
205  return 0;
206 }
207 
210 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
211 IkReal 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;
212 x0=IKcos(j[0]);
213 x1=IKsin(j[1]);
214 x2=IKcos(j[2]);
215 x3=IKcos(j[1]);
216 x4=IKsin(j[2]);
217 x5=IKsin(j[3]);
218 x6=IKsin(j[0]);
219 x7=IKcos(j[3]);
220 x8=IKcos(j[4]);
221 x9=IKsin(j[4]);
222 x10=((IkReal(0.0750000000000000))*(x0));
223 x11=((IkReal(0.0800000000000000))*(x6));
224 x12=((IkReal(1.00000000000000))*(x0));
225 x13=((IkReal(1.00000000000000))*(x6));
226 x14=((IkReal(0.327500000000000))*(x0));
227 x15=((IkReal(0.327500000000000))*(x6));
228 x16=((IkReal(0.300000000000000))*(x1));
229 x17=((IkReal(0.0800000000000000))*(x0));
230 x18=((IkReal(0.0750000000000000))*(x6));
231 x19=((x3)*(x4));
232 x20=((x1)*(x2));
233 x21=((x2)*(x3));
234 x22=((x1)*(x4));
235 x23=((IkReal(0.0750000000000000))*(x20));
236 x24=((x1)*(x11));
237 IkReal x25=((IkReal(1.00000000000000))*(x19));
238 eetrans[0]=((((x0)*(x16)))+(((IkReal(-1.00000000000000))*(x10)*(x25)))+(x10)+(((x10)*(x20)))+(((x7)*(((((x17)*(x22)))+(((x17)*(x21)))))))+(((x14)*(x21)))+(((x14)*(x22)))+(((x5)*(((((IkReal(-1.00000000000000))*(x17)*(x25)))+(((x17)*(x20))))))));
239 IkReal x26=((IkReal(1.00000000000000))*(x19));
240 eetrans[1]=((((x7)*(((((x11)*(x21)))+(((x11)*(x22)))))))+(((x15)*(x21)))+(((x15)*(x22)))+(x18)+(((IkReal(-1.00000000000000))*(x18)*(x26)))+(((x16)*(x6)))+(((x5)*(((((x11)*(x20)))+(((IkReal(-1.00000000000000))*(x11)*(x26)))))))+(((x18)*(x20))));
241 eetrans[2]=((IkReal(0.330000000000000))+(((x7)*(((((IkReal(0.0800000000000000))*(x19)))+(((IkReal(-0.0800000000000000))*(x20)))))))+(((IkReal(0.327500000000000))*(x19)))+(((IkReal(-0.327500000000000))*(x20)))+(((IkReal(0.0750000000000000))*(x21)))+(((IkReal(0.300000000000000))*(x3)))+(((IkReal(0.0750000000000000))*(x22)))+(((x5)*(((((IkReal(0.0800000000000000))*(x21)))+(((IkReal(0.0800000000000000))*(x22))))))));
242 IkReal x27=((IkReal(1.00000000000000))*(x12));
243 eerot[0]=((((IkReal(-1.00000000000000))*(x13)*(x9)))+(((x8)*(((((x5)*(((((IkReal(-1.00000000000000))*(x21)*(x27)))+(((IkReal(-1.00000000000000))*(x22)*(x27)))))))+(((x7)*(((((x0)*(x20)))+(((IkReal(-1.00000000000000))*(x19)*(x27))))))))))));
244 IkReal x28=((IkReal(1.00000000000000))*(x13));
245 eerot[1]=((((x8)*(((((x5)*(((((IkReal(-1.00000000000000))*(x21)*(x28)))+(((IkReal(-1.00000000000000))*(x22)*(x28)))))))+(((x7)*(((((x20)*(x6)))+(((IkReal(-1.00000000000000))*(x19)*(x28)))))))))))+(((x0)*(x9))));
246 eerot[2]=((x8)*(((((x5)*(((((IkReal(1.00000000000000))*(x20)))+(((IkReal(-1.00000000000000))*(x19)))))))+(((x7)*(((x21)+(x22))))))));
247 }
248 
249 IKFAST_API int GetNumFreeParameters() { return 0; }
250 IKFAST_API int* GetFreeParameters() { return NULL; }
251 IKFAST_API int GetNumJoints() { return 5; }
252 
253 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
254 
255 IKFAST_API int GetIkType() { return 0x56000007; }
256 
257 class IKSolver {
258 public:
259 IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
260 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4;
261 
262 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
263 j0=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;
264 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
265  solutions.Clear();
266 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
267 
268 r00 = eerot[0];
269 r01 = eerot[1];
270 r02 = eerot[2];
271 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
272 new_r00=r00;
273 new_px=px;
274 new_r01=r01;
275 new_py=py;
276 new_r02=r02;
277 new_pz=((IkReal(-0.330000000000000))+(pz));
278 r00 = new_r00; r01 = new_r01; r02 = new_r02; px = new_px; py = new_py; pz = new_pz;
279 
280 pp=(((px)*(px))+((pz)*(pz))+((py)*(py)));
281 {
282 IkReal j0array[2], cj0array[2], sj0array[2];
283 bool j0valid[2]={false};
284 _nj0 = 2;
285 if( IKabs(((IkReal(-1.00000000000000))*(py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH )
286  continue;
287 IkReal x29=IKatan2(((IkReal(-1.00000000000000))*(py)), px);
288 j0array[0]=((IkReal(-1.00000000000000))*(x29));
289 sj0array[0]=IKsin(j0array[0]);
290 cj0array[0]=IKcos(j0array[0]);
291 j0array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x29))));
292 sj0array[1]=IKsin(j0array[1]);
293 cj0array[1]=IKcos(j0array[1]);
294 if( j0array[0] > IKPI )
295 {
296  j0array[0]-=IK2PI;
297 }
298 else if( j0array[0] < -IKPI )
299 { j0array[0]+=IK2PI;
300 }
301 j0valid[0] = true;
302 if( j0array[1] > IKPI )
303 {
304  j0array[1]-=IK2PI;
305 }
306 else if( j0array[1] < -IKPI )
307 { j0array[1]+=IK2PI;
308 }
309 j0valid[1] = true;
310 for(int ij0 = 0; ij0 < 2; ++ij0)
311 {
312 if( !j0valid[ij0] )
313 {
314  continue;
315 }
316 _ij0[0] = ij0; _ij0[1] = -1;
317 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
318 {
319 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
320 {
321  j0valid[iij0]=false; _ij0[1] = iij0; break;
322 }
323 }
324 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
325 
326 IkReal op[4+1], zeror[4];
327 int numroots;
328 op[0]=((((IkReal(-0.00325687500000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-2.40000000000000))*(px)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.676575000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(0.763425000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(1.20000000000000))*(pp)*(px)*(r00)*(r02)))+(((IkReal(-2.40000000000000))*(pz)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.763425000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.180000000000000))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.600000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.0567750000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(1.20000000000000))*(cj0)*(pp)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(-4.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-4.80000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.138050000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.0283875000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(0.0130275000000000))*(pz)*((r02)*(r02))))+(((IkReal(-0.381712500000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.180000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-8.00000000000000))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.00709687500000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(0.180000000000000))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-0.382500000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(-8.00000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-0.600000000000000))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.180000000000000))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.600000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(4.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.763425000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-4.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(2.61418359375000e-5))*((r02)*(r02))))+(((IkReal(0.600000000000000))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.600000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(0.625375000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-2.40000000000000))*(cj0)*(pp)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.00325687500000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(0.0690250000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-1.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(-0.0283875000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-0.765000000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(-0.00709687500000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(1.20000000000000))*(pp)*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.600000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-4.80000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.0130275000000000))*(py)*(r01)*(r02)))+(((IkReal(4.00000000000000))*(pp)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-2.40000000000000))*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.763425000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(2.40000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-16.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(0.600000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-2.40000000000000))*(pz)*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-4.80000000000000))*(py)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-0.180000000000000))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(0.00384000000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(0.00115200000000000))*(r01)*(r02)*(sj0)))+(((IkReal(2.40000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(2.40000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(-0.600000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-2.40000000000000))*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-16.0000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-4.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.600000000000000))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.00325687500000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-8.00000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.00325687500000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(0.0690250000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.180000000000000))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(0.00218614183593750))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(0.00437228367187500))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(0.600000000000000))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-2.40000000000000))*(pz)*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(1.20000000000000))*(pp)*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(1.20000000000000))*(pp)*(py)*(r01)*(r02)))+(((IkReal(0.600000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(2.40000000000000))*(py)*(r01)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.300000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(0.00218614183593750))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.180000000000000))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(0.600000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.180000000000000))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.180000000000000))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(0.763425000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-4.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)))+(((IkReal(0.138050000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-8.00000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-0.381712500000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(2.40000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(2.40000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-1.20000000000000))*(pp)*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(1.20000000000000))*(pp)*(pz)*((r02)*(r02))))+(((IkReal(0.0283875000000000))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(1.20000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(2.40000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(0.763425000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.180000000000000))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(-0.294075000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.00384000000000000))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-2.00000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pp)*(pp))))+(((IkReal(0.600000000000000))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(2.40000000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(0.180000000000000))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(-4.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-1.20000000000000))*(pp)*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.625375000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.382500000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(-2.40000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.0283875000000000))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.763425000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.300000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((IkReal(0.763425000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.0690250000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.300000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-8.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-2.40000000000000))*((pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0442125000000000))*(pp)*((r02)*(r02))))+(((IkReal(-0.300000000000000))*(pp)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.600000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(2.40000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-2.40000000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.180000000000000))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(0.180000000000000))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(1.20000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.00115200000000000))*(cj0)*(r00)*(r02)))+(((IkReal(0.0690250000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0283875000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.625375000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.625375000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.180000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-0.676575000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.763425000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(2.40000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.0130275000000000))*(px)*(r00)*(r02)))+(((IkReal(0.0283875000000000))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.300000000000000))*(cj0)*(pp)*(py)*(r00)*(r01)))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.300000000000000))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(2.40000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(1.20000000000000))*(cj0)*(pp)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(0.600000000000000))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(2.40000000000000))*(pz)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.600000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-4.80000000000000))*(px)*(r00)*(r02)*((pz)*(pz)))));
329 op[1]=((((IkReal(0.360000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(4.80000000000000))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.360000000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.000945000000000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-0.108000000000000))*(px)*(r00)*(r02)))+(((IkReal(-4.80000000000000))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.0307200000000000))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(2.40000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-2.40000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((IkReal(-0.360000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.180000000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.00921600000000000))*(r01)*(r02)*(sj0)))+(((IkReal(4.80000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(9.60000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.108000000000000))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.360000000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(0.360000000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.360000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.108000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(0.360000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.180000000000000))*(pp)*((r02)*(r02))))+(((IkReal(-0.720000000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(1.44000000000000))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(1.44000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-0.360000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-1.44000000000000))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(0.00390825000000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(4.80000000000000))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.360000000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(1.44000000000000))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-0.360000000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-2.40000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(0.000945000000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(-0.360000000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.000945000000000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-0.720000000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.360000000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-4.80000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(1.44000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(1.44000000000000))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(4.80000000000000))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.00195412500000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.360000000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.00921600000000000))*(cj0)*(r00)*(r02)))+(((IkReal(4.80000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(0.108000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-1.44000000000000))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(0.108000000000000))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.0307200000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(1.44000000000000))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.360000000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(4.80000000000000))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(4.80000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.360000000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(9.60000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(-0.360000000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-2.40000000000000))*(pp)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.0297750000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(1.44000000000000))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(0.00195412500000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.360000000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.360000000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(2.40000000000000))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(1.44000000000000))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(4.80000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(1.44000000000000))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(4.80000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(1.44000000000000))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(-0.0297750000000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(-2.40000000000000))*(cj0)*(pp)*(py)*(r00)*(r01)))+(((IkReal(1.44000000000000))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(-0.720000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.360000000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(0.00425812500000000))*((r02)*(r02))))+(((IkReal(0.108000000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.108000000000000))*(py)*(r01)*(r02)))+(((IkReal(0.360000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(-0.360000000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(-0.108000000000000))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(0.000945000000000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(4.80000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.216000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.108000000000000))*(pz)*((r02)*(r02))))+(((IkReal(0.180000000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(1.44000000000000))*(cj0)*(px)*(pz)*((r00)*(r00)))));
330 op[2]=((((IkReal(-16.0000000000000))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.0129437163281250))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(0.201806250000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(0.276100000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-1.62925000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(1.20000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(1.20000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(0.676575000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(8.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.52685000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(-32.0000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-16.0000000000000))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-1.35315000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-8.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-1.20000000000000))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.138050000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.209486250000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(0.600000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(8.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.209486250000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-16.0000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.676575000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.00768000000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(0.209486250000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-1.62925000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-2.00000000000000))*((pp)*(pp))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.20000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-1.35315000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-2.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(0.209486250000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(1.20000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(1.35315000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(8.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(8.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(8.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(-32.0000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-2.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(-1.35315000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-16.0000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-4.00000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pp)*(pp))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.276100000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(1.20000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.765000000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(1.20000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-1.62925000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-16.0000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-1.35315000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.00116828367187500))*((r02)*(r02))))+(((IkReal(-1.52842500000000))*(pp)*((r02)*(r02))))+(((IkReal(1.20000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-1.53000000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-1.35315000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(0.201806250000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(2.40000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(-0.765000000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(1.52685000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(-16.0000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(0.138050000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.600000000000000))*(pp)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-8.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.20000000000000))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(8.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(1.20000000000000))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(2.29185000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.600000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-1.35315000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-32.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(-0.0129437163281250))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.20000000000000))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(1.20000000000000))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.600000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((IkReal(-1.62925000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-8.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.138050000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(8.00000000000000))*(pp)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-1.35315000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.600000000000000))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.00768000000000000))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-1.35315000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.20000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-1.20000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(0.138050000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(8.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-8.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.600000000000000))*(cj0)*(pp)*(py)*(r00)*(r01)))+(((IkReal(1.20000000000000))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(2.40000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.0258874326562500))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(8.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(1.20000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02))));
331 op[3]=((((IkReal(0.360000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(4.80000000000000))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.360000000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.000945000000000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-4.80000000000000))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.108000000000000))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-1.44000000000000))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.0307200000000000))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(2.40000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-2.40000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((IkReal(-0.360000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.180000000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(9.60000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.360000000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(0.360000000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.360000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.108000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(0.360000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.180000000000000))*(pp)*((r02)*(r02))))+(((IkReal(0.108000000000000))*(px)*(r00)*(r02)))+(((IkReal(-0.720000000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(0.216000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.360000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-1.44000000000000))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-1.44000000000000))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(0.108000000000000))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.00390825000000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(4.80000000000000))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.108000000000000))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(1.44000000000000))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(-0.360000000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.360000000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-2.40000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(0.000945000000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(-0.360000000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.000945000000000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-0.720000000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(1.44000000000000))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(-0.360000000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-4.80000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-1.44000000000000))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(4.80000000000000))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.00195412500000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-1.44000000000000))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(-0.360000000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-1.44000000000000))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(4.80000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.00921600000000000))*(cj0)*(r00)*(r02)))+(((IkReal(-1.44000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.0307200000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(-0.108000000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.360000000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(4.80000000000000))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(4.80000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.360000000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(9.60000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(-0.360000000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-2.40000000000000))*(pp)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.108000000000000))*(py)*(r01)*(r02)))+(((IkReal(-1.44000000000000))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-0.0297750000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-1.44000000000000))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.00195412500000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.360000000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-1.44000000000000))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(-0.360000000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(2.40000000000000))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(4.80000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-0.108000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(4.80000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-0.0297750000000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(-2.40000000000000))*(cj0)*(pp)*(py)*(r00)*(r01)))+(((IkReal(-0.720000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.360000000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(0.00425812500000000))*((r02)*(r02))))+(((IkReal(-0.00921600000000000))*(r01)*(r02)*(sj0)))+(((IkReal(0.360000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(-0.360000000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.000945000000000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.108000000000000))*(pz)*((r02)*(r02))))+(((IkReal(4.80000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-1.44000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-1.44000000000000))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(0.180000000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00)))));
332 op[4]=((((IkReal(-0.00325687500000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.676575000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-1.20000000000000))*(pp)*(py)*(r01)*(r02)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(0.763425000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(2.40000000000000))*(px)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.0130275000000000))*(pz)*((r02)*(r02))))+(((IkReal(-0.0567750000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.763425000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-2.40000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(4.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(0.600000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(0.138050000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-2.40000000000000))*(pz)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.381712500000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-8.00000000000000))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-1.20000000000000))*(pp)*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.00709687500000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(-0.180000000000000))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-0.382500000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.180000000000000))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-2.40000000000000))*(py)*(r01)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-0.600000000000000))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(4.80000000000000))*(py)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.180000000000000))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.600000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(2.40000000000000))*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(4.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.763425000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.180000000000000))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(-4.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(2.61418359375000e-5))*((r02)*(r02))))+(((IkReal(4.80000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.600000000000000))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.600000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(-2.40000000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(0.625375000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-1.20000000000000))*(pp)*(pz)*((r02)*(r02))))+(((IkReal(-0.00325687500000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(0.0690250000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-1.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(2.40000000000000))*(pz)*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.765000000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(-0.00709687500000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(0.600000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(2.40000000000000))*((pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(4.00000000000000))*(pp)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.180000000000000))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(-0.00115200000000000))*(cj0)*(r00)*(r02)))+(((IkReal(1.20000000000000))*(pp)*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.763425000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(0.0283875000000000))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.0283875000000000))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(0.600000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(4.80000000000000))*(px)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(4.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0130275000000000))*(px)*(r00)*(r02)))+(((IkReal(0.00384000000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(-0.600000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-0.180000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-16.0000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-2.40000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-4.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.600000000000000))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.00325687500000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-8.00000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.00325687500000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-1.20000000000000))*(cj0)*(pp)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-2.40000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.0690250000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.00218614183593750))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.180000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(0.00437228367187500))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(0.600000000000000))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(0.600000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.300000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-2.40000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(2.40000000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.00218614183593750))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(0.600000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(4.80000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.763425000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.138050000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-8.00000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-0.381712500000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.180000000000000))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-2.40000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(1.20000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(-2.40000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(0.763425000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-2.40000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.180000000000000))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(0.0283875000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-0.294075000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-1.20000000000000))*(cj0)*(pp)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.00384000000000000))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-2.00000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pp)*(pp))))+(((IkReal(0.600000000000000))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(-4.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0130275000000000))*(py)*(r01)*(r02)))+(((IkReal(0.625375000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.382500000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(2.40000000000000))*(pz)*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.0283875000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(0.763425000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.300000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((IkReal(0.763425000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(2.40000000000000))*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(2.40000000000000))*(pz)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.180000000000000))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(0.0690250000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.180000000000000))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(-0.300000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-8.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0442125000000000))*(pp)*((r02)*(r02))))+(((IkReal(-1.20000000000000))*(pp)*(px)*(r00)*(r02)))+(((IkReal(2.40000000000000))*(cj0)*(pp)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-1.20000000000000))*(pp)*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.300000000000000))*(pp)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.600000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.20000000000000))*(pp)*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.0283875000000000))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(1.20000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.0690250000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.180000000000000))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(0.625375000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.180000000000000))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.625375000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.676575000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.0283875000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.763425000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-2.40000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.300000000000000))*(cj0)*(pp)*(py)*(r00)*(r01)))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.300000000000000))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(0.600000000000000))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.180000000000000))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(0.600000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-2.40000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(4.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-0.00115200000000000))*(r01)*(r02)*(sj0)))+(((IkReal(2.40000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r02)*(r02)))));
333 polyroots4(op,zeror,numroots);
334 IkReal j1array[4], cj1array[4], sj1array[4], tempj1array[1];
335 int numsolutions = 0;
336 for(int ij1 = 0; ij1 < numroots; ++ij1)
337 {
338 IkReal htj1 = zeror[ij1];
339 tempj1array[0]=((IkReal(2.00000000000000))*(atan(htj1)));
340 for(int kj1 = 0; kj1 < 1; ++kj1)
341 {
342 j1array[numsolutions] = tempj1array[kj1];
343 if( j1array[numsolutions] > IKPI )
344 {
345  j1array[numsolutions]-=IK2PI;
346 }
347 else if( j1array[numsolutions] < -IKPI )
348 {
349  j1array[numsolutions]+=IK2PI;
350 }
351 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
352 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
353 numsolutions++;
354 }
355 }
356 bool j1valid[4]={true,true,true,true};
357 _nj1 = 4;
358 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
359  {
360 if( !j1valid[ij1] )
361 {
362  continue;
363 }
364  j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
365 htj1 = IKtan(j1/2);
366 
367 _ij1[0] = ij1; _ij1[1] = -1;
368 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
369 {
370 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
371 {
372  j1valid[iij1]=false; _ij1[1] = iij1; break;
373 }
374 }
375 {
376 IkReal j4array[2], cj4array[2], sj4array[2];
377 bool j4valid[2]={false};
378 _nj4 = 2;
379 sj4array[0]=((((IkReal(-1.00000000000000))*(r00)*(sj0)))+(((cj0)*(r01))));
380 if( sj4array[0] >= -1-IKFAST_SINCOS_THRESH && sj4array[0] <= 1+IKFAST_SINCOS_THRESH )
381 {
382  j4valid[0] = j4valid[1] = true;
383  j4array[0] = IKasin(sj4array[0]);
384  cj4array[0] = IKcos(j4array[0]);
385  sj4array[1] = sj4array[0];
386  j4array[1] = j4array[0] > 0 ? (IKPI-j4array[0]) : (-IKPI-j4array[0]);
387  cj4array[1] = -cj4array[0];
388 }
389 else if( isnan(sj4array[0]) )
390 {
391  // probably any value will work
392  j4valid[0] = true;
393  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
394 }
395 for(int ij4 = 0; ij4 < 2; ++ij4)
396 {
397 if( !j4valid[ij4] )
398 {
399  continue;
400 }
401 _ij4[0] = ij4; _ij4[1] = -1;
402 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
403 {
404 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
405 {
406  j4valid[iij4]=false; _ij4[1] = iij4; break;
407 }
408 }
409 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
410 
411 {
412 IkReal dummyeval[1];
413 IkReal gconst0;
414 gconst0=IKsign(cj4);
415 dummyeval[0]=cj4;
416 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
417 {
418 {
419 IkReal dummyeval[1];
420 IkReal gconst1;
421 gconst1=IKsign(cj4);
422 dummyeval[0]=cj4;
423 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
424 {
425 {
426 IkReal evalcond[9];
427 IkReal x30=(py)*(py);
428 IkReal x31=(px)*(px);
429 IkReal x32=(pz)*(pz);
430 IkReal x33=((r02)*(sj1));
431 IkReal x34=((cj1)*(pz));
432 IkReal x35=((IkReal(0.150000000000000))*(r00));
433 IkReal x36=((IkReal(0.600000000000000))*(r02));
434 IkReal x37=((cj0)*(px));
435 IkReal x38=((r01)*(sj0));
436 IkReal x39=((IkReal(1.00000000000000))*(cj1));
437 IkReal x40=((py)*(sj0));
438 IkReal x41=((py)*(r01));
439 IkReal x42=((IkReal(0.150000000000000))*(sj1));
440 IkReal x43=((IkReal(1.00000000000000))*(pz));
441 IkReal x44=((px)*(sj1));
442 IkReal x45=((IkReal(0.600000000000000))*(r00));
443 IkReal x46=((cj0)*(r00));
444 IkReal x47=((IkReal(0.0843750000000000))*(cj1));
445 IkReal x48=((IkReal(2.00000000000000))*(pz));
446 IkReal x49=((cj1)*(px));
447 IkReal x50=((IkReal(1.00000000000000))*(px));
448 IkReal x51=((IkReal(0.300000000000000))*(cj1));
449 IkReal x52=((IkReal(0.0956250000000000))*(sj1));
450 IkReal x53=((IkReal(1.00000000000000))*(sj1));
451 IkReal x54=((cj0)*(pz));
452 IkReal x55=((IkReal(0.150000000000000))*(cj1));
453 IkReal x56=((IkReal(2.00000000000000))*(sj1));
454 IkReal x57=((IkReal(2.00000000000000))*(cj1));
455 IkReal x58=((IkReal(2.00000000000000))*(r00));
456 IkReal x59=((IkReal(0.300000000000000))*(sj1));
457 IkReal x60=((r02)*(x40));
458 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
459 evalcond[1]=((((cj0)*(py)))+(((IkReal(-1.00000000000000))*(sj0)*(x50))));
460 evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r00)*(sj0)))+(((cj0)*(r01))));
461 evalcond[3]=((((IkReal(-1.00000000000000))*(x38)*(x39)))+(x33)+(((IkReal(-1.00000000000000))*(x39)*(x46))));
462 evalcond[4]=((((IkReal(-1.00000000000000))*(r02)*(x39)))+(((IkReal(-1.00000000000000))*(x38)*(x53)))+(((IkReal(-1.00000000000000))*(x46)*(x53))));
463 evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(x43)))+(((r02)*(x51)))+(((IkReal(0.0750000000000000))*(x38)))+(((IkReal(-1.00000000000000))*(r00)*(x50)))+(((IkReal(-1.00000000000000))*(x41)))+(((x46)*(x59)))+(((x38)*(x59)))+(((IkReal(0.0750000000000000))*(x46))));
464 evalcond[6]=((((x46)*(x51)))+(x60)+(((IkReal(-0.300000000000000))*(x33)))+(((IkReal(-1.00000000000000))*(x43)*(x46)))+(((x38)*(x51)))+(((IkReal(-1.00000000000000))*(x38)*(x43)))+(((r02)*(x37)))+(((IkReal(-0.0750000000000000))*(r02))));
465 evalcond[7]=((((x40)*(x44)*(x58)))+(((IkReal(-1.00000000000000))*(pp)*(r02)*(x39)))+(((IkReal(-0.150000000000000))*(pz)*(x33)))+(((x33)*(x37)*(x48)))+(((IkReal(-1.00000000000000))*(px)*(x45)))+(((IkReal(-1.00000000000000))*(pz)*(x36)))+(((IkReal(-1.00000000000000))*(x41)*(x42)))+(((x31)*(x46)*(x56)))+(((x37)*(x41)*(x56)))+(((px)*(x34)*(x58)))+(((x30)*(x38)*(x56)))+(((x38)*(x52)))+(((x55)*(x60)))+(((x46)*(x52)))+(((r02)*(x32)*(x57)))+(((r02)*(x37)*(x55)))+(((IkReal(-1.00000000000000))*(pp)*(x46)*(x53)))+(((IkReal(0.0450000000000000))*(x38)))+(((IkReal(2.00000000000000))*(x34)*(x41)))+(((IkReal(-1.00000000000000))*(pp)*(x38)*(x53)))+(((IkReal(-1.00000000000000))*(x35)*(x44)))+(((x33)*(x40)*(x48)))+(((IkReal(-0.600000000000000))*(x41)))+(((r02)*(x47)))+(((IkReal(-0.150000000000000))*(x34)*(x38)))+(((IkReal(-1.00000000000000))*(cj0)*(x34)*(x35)))+(((IkReal(0.0450000000000000))*(x46))));
466 evalcond[8]=((((IkReal(-0.150000000000000))*(r02)*(x34)))+(((IkReal(-1.00000000000000))*(pp)*(x38)*(x39)))+(((IkReal(2.00000000000000))*(r02)*(x34)*(x37)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x35)*(x49)))+(((pp)*(x33)))+(((x31)*(x46)*(x57)))+(((x45)*(x54)))+(((IkReal(-0.150000000000000))*(x33)*(x37)))+(((IkReal(-0.150000000000000))*(x33)*(x40)))+(((IkReal(2.00000000000000))*(x34)*(x60)))+(((x30)*(x38)*(x57)))+(((IkReal(0.600000000000000))*(pz)*(x38)))+(((IkReal(-1.00000000000000))*(x36)*(x37)))+(((x40)*(x49)*(x58)))+(((IkReal(-1.00000000000000))*(sj1)*(x41)*(x48)))+(((IkReal(-2.00000000000000))*(x32)*(x33)))+(((IkReal(-1.00000000000000))*(x38)*(x47)))+(((IkReal(0.0956250000000000))*(x33)))+(((x37)*(x41)*(x57)))+(((sj1)*(x35)*(x54)))+(((IkReal(-1.00000000000000))*(x41)*(x55)))+(((IkReal(-1.00000000000000))*(r00)*(x44)*(x48)))+(((IkReal(-1.00000000000000))*(pp)*(x39)*(x46)))+(((IkReal(-1.00000000000000))*(x36)*(x40)))+(((pz)*(x38)*(x42)))+(((IkReal(-1.00000000000000))*(x46)*(x47))));
467 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 )
468 {
469 {
470 IkReal j3array[2], cj3array[2], sj3array[2];
471 bool j3valid[2]={false};
472 _nj3 = 2;
473 IkReal x61=((IkReal(11.1614434376257))*(sj1));
474 IkReal x62=((py)*(sj0));
475 IkReal x63=((cj0)*(px));
476 if( (((IkReal(0.440063160535554))+(((IkReal(-0.837108257821925))*(sj1)))+(((x61)*(x62)))+(((IkReal(11.1614434376257))*(cj1)*(pz)))+(((IkReal(-18.6024057293761))*(pp)))+(((IkReal(2.79036085940642))*(x62)))+(((IkReal(2.79036085940642))*(x63)))+(((x61)*(x63))))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.440063160535554))+(((IkReal(-0.837108257821925))*(sj1)))+(((x61)*(x62)))+(((IkReal(11.1614434376257))*(cj1)*(pz)))+(((IkReal(-18.6024057293761))*(pp)))+(((IkReal(2.79036085940642))*(x62)))+(((IkReal(2.79036085940642))*(x63)))+(((x61)*(x63))))) > 1+IKFAST_SINCOS_THRESH )
477  continue;
478 IkReal x64=IKasin(((IkReal(0.440063160535554))+(((IkReal(-0.837108257821925))*(sj1)))+(((x61)*(x62)))+(((IkReal(11.1614434376257))*(cj1)*(pz)))+(((IkReal(-18.6024057293761))*(pp)))+(((IkReal(2.79036085940642))*(x62)))+(((IkReal(2.79036085940642))*(x63)))+(((x61)*(x63)))));
479 j3array[0]=((IkReal(-1.34567065063197))+(((IkReal(-1.00000000000000))*(x64))));
480 sj3array[0]=IKsin(j3array[0]);
481 cj3array[0]=IKcos(j3array[0]);
482 j3array[1]=((IkReal(1.79592200295782))+(x64));
483 sj3array[1]=IKsin(j3array[1]);
484 cj3array[1]=IKcos(j3array[1]);
485 if( j3array[0] > IKPI )
486 {
487  j3array[0]-=IK2PI;
488 }
489 else if( j3array[0] < -IKPI )
490 { j3array[0]+=IK2PI;
491 }
492 j3valid[0] = true;
493 if( j3array[1] > IKPI )
494 {
495  j3array[1]-=IK2PI;
496 }
497 else if( j3array[1] < -IKPI )
498 { j3array[1]+=IK2PI;
499 }
500 j3valid[1] = true;
501 for(int ij3 = 0; ij3 < 2; ++ij3)
502 {
503 if( !j3valid[ij3] )
504 {
505  continue;
506 }
507 _ij3[0] = ij3; _ij3[1] = -1;
508 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
509 {
510 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
511 {
512  j3valid[iij3]=false; _ij3[1] = iij3; break;
513 }
514 }
515 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
516 {
517 IkReal evalcond[1];
518 IkReal x65=((IkReal(2.00000000000000))*(sj0));
519 IkReal x66=((px)*(py));
520 IkReal x67=((r00)*(sj0));
521 IkReal x68=((cj0)*(r01));
522 IkReal x69=((py)*(r00));
523 IkReal x70=((IkReal(2.00000000000000))*(cj0));
524 IkReal x71=((px)*(r01));
525 IkReal x72=((IkReal(0.600000000000000))*(cj1));
526 IkReal x73=((py)*(r02));
527 IkReal x74=((IkReal(0.600000000000000))*(sj1));
528 IkReal x75=((IkReal(0.0450000000000000))*(sj1));
529 IkReal x76=((px)*(r02));
530 evalcond[0]=((IkReal(-0.119281250000000))+(((IkReal(-1.00000000000000))*(x67)*(x75)))+(((r01)*(x65)*(x66)))+(((IkReal(-0.0956250000000000))*(x67)))+(((x69)*(x74)))+(((cj0)*(x72)*(x73)))+(((IkReal(-1.00000000000000))*(pz)*(x68)*(x72)))+(((pp)*(x68)))+(((IkReal(-1.00000000000000))*(pp)*(x67)))+(((IkReal(0.0956250000000000))*(x68)))+(((IkReal(-2.00000000000000))*(x68)*((py)*(py))))+(((IkReal(-0.150000000000000))*(x71)))+(((IkReal(-1.00000000000000))*(r00)*(x66)*(x70)))+(((IkReal(-0.0120000000000000))*(IKsin(j3))))+(((IkReal(-1.00000000000000))*(x71)*(x74)))+(((IkReal(-1.00000000000000))*(sj0)*(x72)*(x76)))+(((pz)*(x67)*(x72)))+(((pz)*(x65)*(x76)))+(((IkReal(-0.0524000000000000))*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(pz)*(x70)*(x73)))+(((IkReal(0.150000000000000))*(x69)))+(((r00)*(x65)*((px)*(px))))+(((x68)*(x75))));
531 if( IKabs(evalcond[0]) > 0.000001 )
532 {
533 continue;
534 }
535 }
536 
537 {
538 IkReal dummyeval[1];
539 IkReal gconst8;
540 gconst8=IKsign(((IkReal(0.112881250000000))+(((IkReal(0.0524000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
541 dummyeval[0]=((IkReal(17.6376953125000))+(((IkReal(8.18750000000000))*(cj3)))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3)));
542 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
543 {
544 {
545 IkReal dummyeval[1];
546 IkReal gconst9;
547 gconst9=IKsign(((IkReal(0.112881250000000))+(((IkReal(0.0524000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
548 dummyeval[0]=((IkReal(17.6376953125000))+(((IkReal(8.18750000000000))*(cj3)))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3)));
549 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
550 {
551 continue;
552 
553 } else
554 {
555 {
556 IkReal j2array[1], cj2array[1], sj2array[1];
557 bool j2valid[1]={false};
558 _nj2 = 1;
559 IkReal x77=((pz)*(sj1));
560 IkReal x78=((cj0)*(r01));
561 IkReal x79=((IkReal(0.0800000000000000))*(sj3));
562 IkReal x80=((r00)*(sj0));
563 IkReal x81=((IkReal(0.00600000000000000))*(cj1));
564 IkReal x82=((IkReal(0.0750000000000000))*(cj1));
565 IkReal x83=((IkReal(0.0245625000000000))*(cj1));
566 IkReal x84=((px)*(r01));
567 IkReal x85=((IkReal(0.00562500000000000))*(cj1));
568 IkReal x86=((IkReal(0.0800000000000000))*(cj3));
569 IkReal x87=((px)*(sj1));
570 IkReal x88=((cj1)*(pz));
571 IkReal x89=((IkReal(0.0750000000000000))*(sj0));
572 IkReal x90=((IkReal(0.327500000000000))*(cj0));
573 IkReal x91=((py)*(sj1));
574 IkReal x92=((cj0)*(r02));
575 IkReal x93=((IkReal(0.00600000000000000))*(sj1));
576 IkReal x94=((IkReal(0.327500000000000))*(sj0));
577 IkReal x95=((cj1)*(py)*(r00));
578 IkReal x96=((IkReal(0.0800000000000000))*(r02)*(sj0)*(x87));
579 if( IKabs(((gconst9)*(((IkReal(-0.0982500000000000))+(((x79)*(x95)))+(((py)*(r00)*(x82)))+(((r02)*(x87)*(x89)))+(((IkReal(-1.00000000000000))*(x80)*(x85)))+(((IkReal(0.0750000000000000))*(x77)*(x78)))+(((cj0)*(x86)*(x87)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(cj1)*(x79)*(x84)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x79)*(x91)*(x92)))+(((IkReal(-1.00000000000000))*(x77)*(x79)*(x80)))+(((IkReal(-1.00000000000000))*(x82)*(x84)))+(((IkReal(0.327500000000000))*(x88)))+(((IkReal(-0.0750000000000000))*(x91)*(x92)))+(((IkReal(-1.00000000000000))*(sj3)*(x80)*(x81)))+(((r02)*(sj0)*(x79)*(x87)))+(((IkReal(-1.00000000000000))*(cj3)*(x93)))+(((x78)*(x85)))+(((sj3)*(x78)*(x81)))+(((x77)*(x78)*(x79)))+(((x86)*(x88)))+(((x87)*(x90)))+(((x91)*(x94)))+(((sj0)*(x86)*(x91)))+(((IkReal(-0.0750000000000000))*(x77)*(x80))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(0.327500000000000))*(cj1)*(x84)))+(((IkReal(0.0750000000000000))*(cj0)*(x87)))+(((IkReal(-1.00000000000000))*(sj3)*(x93)))+(((x89)*(x91)))+(((IkReal(-0.327500000000000))*(x95)))+(((IkReal(-1.00000000000000))*(x77)*(x78)*(x86)))+(((x77)*(x80)*(x86)))+(((cj1)*(x84)*(x86)))+(((IkReal(-1.00000000000000))*(r02)*(x87)*(x94)))+(((cj0)*(x79)*(x87)))+(((x80)*(x83)))+(((pz)*(x82)))+(((r02)*(x90)*(x91)))+(((x86)*(x91)*(x92)))+(((x79)*(x88)))+(((cj3)*(x80)*(x81)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x86)*(x87)))+(((IkReal(0.327500000000000))*(x77)*(x80)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((sj0)*(x79)*(x91)))+(((IkReal(-1.00000000000000))*(x78)*(x83)))+(((IkReal(-1.00000000000000))*(x86)*(x95)))+(((IkReal(-0.327500000000000))*(x77)*(x78)))+(((IkReal(-1.00000000000000))*(cj3)*(x78)*(x81))))))) < IKFAST_ATAN2_MAGTHRESH )
580  continue;
581 j2array[0]=IKatan2(((gconst9)*(((IkReal(-0.0982500000000000))+(((x79)*(x95)))+(((py)*(r00)*(x82)))+(((r02)*(x87)*(x89)))+(((IkReal(-1.00000000000000))*(x80)*(x85)))+(((IkReal(0.0750000000000000))*(x77)*(x78)))+(((cj0)*(x86)*(x87)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(cj1)*(x79)*(x84)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x79)*(x91)*(x92)))+(((IkReal(-1.00000000000000))*(x77)*(x79)*(x80)))+(((IkReal(-1.00000000000000))*(x82)*(x84)))+(((IkReal(0.327500000000000))*(x88)))+(((IkReal(-0.0750000000000000))*(x91)*(x92)))+(((IkReal(-1.00000000000000))*(sj3)*(x80)*(x81)))+(((r02)*(sj0)*(x79)*(x87)))+(((IkReal(-1.00000000000000))*(cj3)*(x93)))+(((x78)*(x85)))+(((sj3)*(x78)*(x81)))+(((x77)*(x78)*(x79)))+(((x86)*(x88)))+(((x87)*(x90)))+(((x91)*(x94)))+(((sj0)*(x86)*(x91)))+(((IkReal(-0.0750000000000000))*(x77)*(x80)))))), ((gconst9)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(0.327500000000000))*(cj1)*(x84)))+(((IkReal(0.0750000000000000))*(cj0)*(x87)))+(((IkReal(-1.00000000000000))*(sj3)*(x93)))+(((x89)*(x91)))+(((IkReal(-0.327500000000000))*(x95)))+(((IkReal(-1.00000000000000))*(x77)*(x78)*(x86)))+(((x77)*(x80)*(x86)))+(((cj1)*(x84)*(x86)))+(((IkReal(-1.00000000000000))*(r02)*(x87)*(x94)))+(((cj0)*(x79)*(x87)))+(((x80)*(x83)))+(((pz)*(x82)))+(((r02)*(x90)*(x91)))+(((x86)*(x91)*(x92)))+(((x79)*(x88)))+(((cj3)*(x80)*(x81)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x86)*(x87)))+(((IkReal(0.327500000000000))*(x77)*(x80)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((sj0)*(x79)*(x91)))+(((IkReal(-1.00000000000000))*(x78)*(x83)))+(((IkReal(-1.00000000000000))*(x86)*(x95)))+(((IkReal(-0.327500000000000))*(x77)*(x78)))+(((IkReal(-1.00000000000000))*(cj3)*(x78)*(x81)))))));
582 sj2array[0]=IKsin(j2array[0]);
583 cj2array[0]=IKcos(j2array[0]);
584 if( j2array[0] > IKPI )
585 {
586  j2array[0]-=IK2PI;
587 }
588 else if( j2array[0] < -IKPI )
589 { j2array[0]+=IK2PI;
590 }
591 j2valid[0] = true;
592 for(int ij2 = 0; ij2 < 1; ++ij2)
593 {
594 if( !j2valid[ij2] )
595 {
596  continue;
597 }
598 _ij2[0] = ij2; _ij2[1] = -1;
599 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
600 {
601 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
602 {
603  j2valid[iij2]=false; _ij2[1] = iij2; break;
604 }
605 }
606 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
607 {
608 IkReal evalcond[4];
609 IkReal x97=IKcos(j2);
610 IkReal x98=IKsin(j2);
611 IkReal x99=((IkReal(0.0800000000000000))*(sj3));
612 IkReal x100=((cj0)*(r01));
613 IkReal x101=((IkReal(1.00000000000000))*(px));
614 IkReal x102=((py)*(sj1));
615 IkReal x103=((cj0)*(r02));
616 IkReal x104=((IkReal(0.0750000000000000))*(cj1));
617 IkReal x105=((r02)*(sj0));
618 IkReal x106=((IkReal(0.0750000000000000))*(sj1));
619 IkReal x107=((cj1)*(pz));
620 IkReal x108=((r00)*(sj0));
621 IkReal x109=((IkReal(0.0800000000000000))*(cj3));
622 IkReal x110=((pz)*(sj1));
623 IkReal x111=((IkReal(1.00000000000000))*(sj0));
624 IkReal x112=((cj1)*(py));
625 IkReal x113=((IkReal(0.0750000000000000))*(x98));
626 IkReal x114=((IkReal(0.327500000000000))*(x97));
627 IkReal x115=((IkReal(0.0750000000000000))*(x97));
628 IkReal x116=((IkReal(0.327500000000000))*(x98));
629 IkReal x117=((sj1)*(x108));
630 IkReal x118=((x98)*(x99));
631 IkReal x119=((x109)*(x97));
632 IkReal x120=((x97)*(x99));
633 IkReal x121=((x109)*(x98));
634 IkReal x122=((x113)+(x118));
635 IkReal x123=((x114)+(x119));
636 IkReal x124=((x120)+(x121)+(x116)+(x115));
637 evalcond[0]=((((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x101)))+(((IkReal(-1.00000000000000))*(x111)*(x112)))+(x123)+(((IkReal(-1.00000000000000))*(x122)))+(x104)+(x110));
638 evalcond[1]=((IkReal(0.300000000000000))+(x124)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x101)))+(x106)+(((IkReal(-1.00000000000000))*(x102)*(x111)))+(((IkReal(-1.00000000000000))*(x107))));
639 evalcond[2]=((((x108)*(x110)))+(((x102)*(x103)))+(((cj1)*(px)*(r01)))+(x122)+(((IkReal(-1.00000000000000))*(x100)*(x104)))+(((IkReal(-1.00000000000000))*(x100)*(x110)))+(((IkReal(-1.00000000000000))*(sj1)*(x101)*(x105)))+(((x104)*(x108)))+(((IkReal(-1.00000000000000))*(x123)))+(((IkReal(-1.00000000000000))*(r00)*(x112))));
640 evalcond[3]=((((IkReal(-0.300000000000000))*(x108)))+(((IkReal(-1.00000000000000))*(x106)*(x108)))+(((IkReal(-1.00000000000000))*(cj1)*(x101)*(x105)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x101)))+(((x100)*(x106)))+(((r00)*(x102)))+(((IkReal(0.300000000000000))*(x100)))+(((IkReal(-1.00000000000000))*(x100)*(x107)))+(x124)+(((x107)*(x108)))+(((x103)*(x112))));
641 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
642 {
643 continue;
644 }
645 }
646 
647 {
648 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
649 vinfos[0].jointtype = 1;
650 vinfos[0].foffset = j0;
651 vinfos[0].indices[0] = _ij0[0];
652 vinfos[0].indices[1] = _ij0[1];
653 vinfos[0].maxsolutions = _nj0;
654 vinfos[1].jointtype = 1;
655 vinfos[1].foffset = j1;
656 vinfos[1].indices[0] = _ij1[0];
657 vinfos[1].indices[1] = _ij1[1];
658 vinfos[1].maxsolutions = _nj1;
659 vinfos[2].jointtype = 1;
660 vinfos[2].foffset = j2;
661 vinfos[2].indices[0] = _ij2[0];
662 vinfos[2].indices[1] = _ij2[1];
663 vinfos[2].maxsolutions = _nj2;
664 vinfos[3].jointtype = 1;
665 vinfos[3].foffset = j3;
666 vinfos[3].indices[0] = _ij3[0];
667 vinfos[3].indices[1] = _ij3[1];
668 vinfos[3].maxsolutions = _nj3;
669 vinfos[4].jointtype = 1;
670 vinfos[4].foffset = j4;
671 vinfos[4].indices[0] = _ij4[0];
672 vinfos[4].indices[1] = _ij4[1];
673 vinfos[4].maxsolutions = _nj4;
674 std::vector<int> vfree(0);
675 solutions.AddSolution(vinfos,vfree);
676 }
677 }
678 }
679 
680 }
681 
682 }
683 
684 } else
685 {
686 {
687 IkReal j2array[1], cj2array[1], sj2array[1];
688 bool j2valid[1]={false};
689 _nj2 = 1;
690 IkReal x125=((IkReal(0.0800000000000000))*(cj1));
691 IkReal x126=((cj0)*(px));
692 IkReal x127=((py)*(sj0));
693 IkReal x128=((IkReal(0.0750000000000000))*(cj1));
694 IkReal x129=((IkReal(0.327500000000000))*(cj1));
695 IkReal x130=((IkReal(0.327500000000000))*(sj1));
696 IkReal x131=((IkReal(0.0750000000000000))*(sj1));
697 IkReal x132=((IkReal(0.00600000000000000))*(sj1));
698 IkReal x133=((IkReal(0.00600000000000000))*(cj1));
699 IkReal x134=((IkReal(0.0800000000000000))*(sj1)*(sj3));
700 IkReal x135=((IkReal(0.0800000000000000))*(cj3)*(sj1));
701 if( IKabs(((gconst8)*(((IkReal(-0.0982500000000000))+(((x126)*(x130)))+(((IkReal(-1.00000000000000))*(cj3)*(x132)))+(((pz)*(x134)))+(((pz)*(x131)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((sj3)*(x133)))+(((pz)*(x129)))+(((x127)*(x130)))+(((IkReal(-1.00000000000000))*(sj3)*(x125)*(x126)))+(((x126)*(x135)))+(((IkReal(-1.00000000000000))*(x126)*(x128)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x127)*(x128)))+(((x127)*(x135)))+(((IkReal(-1.00000000000000))*(sj3)*(x125)*(x127)))+(((cj3)*(pz)*(x125))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x132)))+(((IkReal(-1.00000000000000))*(pz)*(x135)))+(((IkReal(-1.00000000000000))*(cj3)*(x133)))+(((x127)*(x134)))+(((x126)*(x129)))+(((x126)*(x134)))+(((pz)*(x128)))+(((x127)*(x131)))+(((x126)*(x131)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((cj3)*(x125)*(x126)))+(((x127)*(x129)))+(((pz)*(sj3)*(x125)))+(((IkReal(-1.00000000000000))*(pz)*(x130)))+(((cj3)*(x125)*(x127))))))) < IKFAST_ATAN2_MAGTHRESH )
702  continue;
703 j2array[0]=IKatan2(((gconst8)*(((IkReal(-0.0982500000000000))+(((x126)*(x130)))+(((IkReal(-1.00000000000000))*(cj3)*(x132)))+(((pz)*(x134)))+(((pz)*(x131)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((sj3)*(x133)))+(((pz)*(x129)))+(((x127)*(x130)))+(((IkReal(-1.00000000000000))*(sj3)*(x125)*(x126)))+(((x126)*(x135)))+(((IkReal(-1.00000000000000))*(x126)*(x128)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x127)*(x128)))+(((x127)*(x135)))+(((IkReal(-1.00000000000000))*(sj3)*(x125)*(x127)))+(((cj3)*(pz)*(x125)))))), ((gconst8)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x132)))+(((IkReal(-1.00000000000000))*(pz)*(x135)))+(((IkReal(-1.00000000000000))*(cj3)*(x133)))+(((x127)*(x134)))+(((x126)*(x129)))+(((x126)*(x134)))+(((pz)*(x128)))+(((x127)*(x131)))+(((x126)*(x131)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((cj3)*(x125)*(x126)))+(((x127)*(x129)))+(((pz)*(sj3)*(x125)))+(((IkReal(-1.00000000000000))*(pz)*(x130)))+(((cj3)*(x125)*(x127)))))));
704 sj2array[0]=IKsin(j2array[0]);
705 cj2array[0]=IKcos(j2array[0]);
706 if( j2array[0] > IKPI )
707 {
708  j2array[0]-=IK2PI;
709 }
710 else if( j2array[0] < -IKPI )
711 { j2array[0]+=IK2PI;
712 }
713 j2valid[0] = true;
714 for(int ij2 = 0; ij2 < 1; ++ij2)
715 {
716 if( !j2valid[ij2] )
717 {
718  continue;
719 }
720 _ij2[0] = ij2; _ij2[1] = -1;
721 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
722 {
723 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
724 {
725  j2valid[iij2]=false; _ij2[1] = iij2; break;
726 }
727 }
728 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
729 {
730 IkReal evalcond[4];
731 IkReal x136=IKcos(j2);
732 IkReal x137=IKsin(j2);
733 IkReal x138=((IkReal(0.0800000000000000))*(sj3));
734 IkReal x139=((cj0)*(r01));
735 IkReal x140=((IkReal(1.00000000000000))*(px));
736 IkReal x141=((py)*(sj1));
737 IkReal x142=((cj0)*(r02));
738 IkReal x143=((IkReal(0.0750000000000000))*(cj1));
739 IkReal x144=((r02)*(sj0));
740 IkReal x145=((IkReal(0.0750000000000000))*(sj1));
741 IkReal x146=((cj1)*(pz));
742 IkReal x147=((r00)*(sj0));
743 IkReal x148=((IkReal(0.0800000000000000))*(cj3));
744 IkReal x149=((pz)*(sj1));
745 IkReal x150=((IkReal(1.00000000000000))*(sj0));
746 IkReal x151=((cj1)*(py));
747 IkReal x152=((IkReal(0.0750000000000000))*(x137));
748 IkReal x153=((IkReal(0.327500000000000))*(x136));
749 IkReal x154=((IkReal(0.0750000000000000))*(x136));
750 IkReal x155=((IkReal(0.327500000000000))*(x137));
751 IkReal x156=((sj1)*(x147));
752 IkReal x157=((x137)*(x138));
753 IkReal x158=((x136)*(x148));
754 IkReal x159=((x136)*(x138));
755 IkReal x160=((x137)*(x148));
756 IkReal x161=((x157)+(x152));
757 IkReal x162=((x153)+(x158));
758 IkReal x163=((x155)+(x154)+(x159)+(x160));
759 evalcond[0]=((((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x140)))+(((IkReal(-1.00000000000000))*(x150)*(x151)))+(x162)+(((IkReal(-1.00000000000000))*(x161)))+(x143)+(x149));
760 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x141)*(x150)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x140)))+(((IkReal(-1.00000000000000))*(x146)))+(x163)+(x145));
761 evalcond[2]=((((cj1)*(px)*(r01)))+(x161)+(((IkReal(-1.00000000000000))*(x139)*(x149)))+(((IkReal(-1.00000000000000))*(sj1)*(x140)*(x144)))+(((x143)*(x147)))+(((IkReal(-1.00000000000000))*(r00)*(x151)))+(((IkReal(-1.00000000000000))*(x139)*(x143)))+(((x147)*(x149)))+(((x141)*(x142)))+(((IkReal(-1.00000000000000))*(x162))));
762 evalcond[3]=((((IkReal(-1.00000000000000))*(x139)*(x146)))+(((x139)*(x145)))+(((IkReal(-0.300000000000000))*(x147)))+(((r00)*(x141)))+(((IkReal(-1.00000000000000))*(cj1)*(x140)*(x144)))+(x163)+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x140)))+(((x142)*(x151)))+(((IkReal(-1.00000000000000))*(x145)*(x147)))+(((IkReal(0.300000000000000))*(x139)))+(((x146)*(x147))));
763 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
764 {
765 continue;
766 }
767 }
768 
769 {
770 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
771 vinfos[0].jointtype = 1;
772 vinfos[0].foffset = j0;
773 vinfos[0].indices[0] = _ij0[0];
774 vinfos[0].indices[1] = _ij0[1];
775 vinfos[0].maxsolutions = _nj0;
776 vinfos[1].jointtype = 1;
777 vinfos[1].foffset = j1;
778 vinfos[1].indices[0] = _ij1[0];
779 vinfos[1].indices[1] = _ij1[1];
780 vinfos[1].maxsolutions = _nj1;
781 vinfos[2].jointtype = 1;
782 vinfos[2].foffset = j2;
783 vinfos[2].indices[0] = _ij2[0];
784 vinfos[2].indices[1] = _ij2[1];
785 vinfos[2].maxsolutions = _nj2;
786 vinfos[3].jointtype = 1;
787 vinfos[3].foffset = j3;
788 vinfos[3].indices[0] = _ij3[0];
789 vinfos[3].indices[1] = _ij3[1];
790 vinfos[3].maxsolutions = _nj3;
791 vinfos[4].jointtype = 1;
792 vinfos[4].foffset = j4;
793 vinfos[4].indices[0] = _ij4[0];
794 vinfos[4].indices[1] = _ij4[1];
795 vinfos[4].maxsolutions = _nj4;
796 std::vector<int> vfree(0);
797 solutions.AddSolution(vinfos,vfree);
798 }
799 }
800 }
801 
802 }
803 
804 }
805 }
806 }
807 
808 } else
809 {
810 IkReal x164=(py)*(py);
811 IkReal x165=(px)*(px);
812 IkReal x166=(pz)*(pz);
813 IkReal x167=((r02)*(sj1));
814 IkReal x168=((cj1)*(pz));
815 IkReal x169=((IkReal(0.150000000000000))*(r00));
816 IkReal x170=((IkReal(0.600000000000000))*(r02));
817 IkReal x171=((cj0)*(px));
818 IkReal x172=((r01)*(sj0));
819 IkReal x173=((IkReal(1.00000000000000))*(cj1));
820 IkReal x174=((py)*(sj0));
821 IkReal x175=((py)*(r01));
822 IkReal x176=((IkReal(0.150000000000000))*(sj1));
823 IkReal x177=((IkReal(1.00000000000000))*(pz));
824 IkReal x178=((px)*(sj1));
825 IkReal x179=((IkReal(0.600000000000000))*(r00));
826 IkReal x180=((cj0)*(r00));
827 IkReal x181=((IkReal(0.0843750000000000))*(cj1));
828 IkReal x182=((IkReal(2.00000000000000))*(pz));
829 IkReal x183=((cj1)*(px));
830 IkReal x184=((IkReal(1.00000000000000))*(px));
831 IkReal x185=((IkReal(0.300000000000000))*(cj1));
832 IkReal x186=((IkReal(0.0956250000000000))*(sj1));
833 IkReal x187=((IkReal(1.00000000000000))*(sj1));
834 IkReal x188=((cj0)*(pz));
835 IkReal x189=((IkReal(0.150000000000000))*(cj1));
836 IkReal x190=((IkReal(2.00000000000000))*(sj1));
837 IkReal x191=((IkReal(2.00000000000000))*(cj1));
838 IkReal x192=((IkReal(2.00000000000000))*(r00));
839 IkReal x193=((IkReal(0.300000000000000))*(sj1));
840 IkReal x194=((r02)*(x174));
841 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
842 evalcond[1]=((((IkReal(-1.00000000000000))*(sj0)*(x184)))+(((cj0)*(py))));
843 evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r00)*(sj0)))+(((cj0)*(r01))));
844 evalcond[3]=((((IkReal(-1.00000000000000))*(x173)*(x180)))+(x167)+(((IkReal(-1.00000000000000))*(x172)*(x173))));
845 evalcond[4]=((((IkReal(-1.00000000000000))*(x180)*(x187)))+(((IkReal(-1.00000000000000))*(x172)*(x187)))+(((IkReal(-1.00000000000000))*(r02)*(x173))));
846 evalcond[5]=((((x172)*(x193)))+(((IkReal(-1.00000000000000))*(x175)))+(((r02)*(x185)))+(((IkReal(-1.00000000000000))*(r02)*(x177)))+(((IkReal(0.0750000000000000))*(x172)))+(((x180)*(x193)))+(((IkReal(-1.00000000000000))*(r00)*(x184)))+(((IkReal(0.0750000000000000))*(x180))));
847 evalcond[6]=((((IkReal(-1.00000000000000))*(x172)*(x177)))+(((IkReal(-1.00000000000000))*(x177)*(x180)))+(x194)+(((IkReal(-0.300000000000000))*(x167)))+(((x180)*(x185)))+(((x172)*(x185)))+(((r02)*(x171)))+(((IkReal(-0.0750000000000000))*(r02))));
848 evalcond[7]=((((x167)*(x174)*(x182)))+(((IkReal(2.00000000000000))*(x168)*(x175)))+(((IkReal(0.0450000000000000))*(x172)))+(((IkReal(-1.00000000000000))*(x169)*(x178)))+(((IkReal(0.0450000000000000))*(x180)))+(((IkReal(-1.00000000000000))*(pp)*(r02)*(x173)))+(((x174)*(x178)*(x192)))+(((r02)*(x166)*(x191)))+(((px)*(x168)*(x192)))+(((IkReal(-0.150000000000000))*(x168)*(x172)))+(((x165)*(x180)*(x190)))+(((IkReal(-1.00000000000000))*(pz)*(x170)))+(((IkReal(-1.00000000000000))*(pp)*(x180)*(x187)))+(((IkReal(-1.00000000000000))*(x175)*(x176)))+(((IkReal(-1.00000000000000))*(cj0)*(x168)*(x169)))+(((IkReal(-1.00000000000000))*(px)*(x179)))+(((IkReal(-0.150000000000000))*(pz)*(x167)))+(((x172)*(x186)))+(((r02)*(x171)*(x189)))+(((x167)*(x171)*(x182)))+(((IkReal(-1.00000000000000))*(pp)*(x172)*(x187)))+(((r02)*(x181)))+(((x164)*(x172)*(x190)))+(((x171)*(x175)*(x190)))+(((x189)*(x194)))+(((IkReal(-0.600000000000000))*(x175)))+(((x180)*(x186))));
849 evalcond[8]=((((IkReal(-1.00000000000000))*(x172)*(x181)))+(((IkReal(-0.150000000000000))*(x167)*(x174)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(0.600000000000000))*(pz)*(x172)))+(((IkReal(-1.00000000000000))*(x180)*(x181)))+(((pp)*(x167)))+(((pz)*(x172)*(x176)))+(((IkReal(-0.150000000000000))*(x167)*(x171)))+(((x179)*(x188)))+(((IkReal(-2.00000000000000))*(x166)*(x167)))+(((IkReal(-1.00000000000000))*(r00)*(x178)*(x182)))+(((IkReal(-1.00000000000000))*(x169)*(x183)))+(((IkReal(2.00000000000000))*(r02)*(x168)*(x171)))+(((IkReal(-0.150000000000000))*(r02)*(x168)))+(((IkReal(0.0956250000000000))*(x167)))+(((IkReal(-1.00000000000000))*(pp)*(x173)*(x180)))+(((x164)*(x172)*(x191)))+(((IkReal(-1.00000000000000))*(x170)*(x171)))+(((IkReal(-1.00000000000000))*(x175)*(x189)))+(((IkReal(-1.00000000000000))*(sj1)*(x175)*(x182)))+(((IkReal(2.00000000000000))*(x168)*(x194)))+(((IkReal(-1.00000000000000))*(x170)*(x174)))+(((sj1)*(x169)*(x188)))+(((x165)*(x180)*(x191)))+(((x171)*(x175)*(x191)))+(((IkReal(-1.00000000000000))*(pp)*(x172)*(x173)))+(((x174)*(x183)*(x192))));
850 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 )
851 {
852 {
853 IkReal j3array[2], cj3array[2], sj3array[2];
854 bool j3valid[2]={false};
855 _nj3 = 2;
856 IkReal x195=((IkReal(11.1614434376257))*(sj1));
857 IkReal x196=((py)*(sj0));
858 IkReal x197=((cj0)*(px));
859 if( (((IkReal(0.440063160535554))+(((IkReal(-0.837108257821925))*(sj1)))+(((x195)*(x196)))+(((IkReal(11.1614434376257))*(cj1)*(pz)))+(((IkReal(-18.6024057293761))*(pp)))+(((IkReal(2.79036085940642))*(x196)))+(((IkReal(2.79036085940642))*(x197)))+(((x195)*(x197))))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.440063160535554))+(((IkReal(-0.837108257821925))*(sj1)))+(((x195)*(x196)))+(((IkReal(11.1614434376257))*(cj1)*(pz)))+(((IkReal(-18.6024057293761))*(pp)))+(((IkReal(2.79036085940642))*(x196)))+(((IkReal(2.79036085940642))*(x197)))+(((x195)*(x197))))) > 1+IKFAST_SINCOS_THRESH )
860  continue;
861 IkReal x198=IKasin(((IkReal(0.440063160535554))+(((IkReal(-0.837108257821925))*(sj1)))+(((x195)*(x196)))+(((IkReal(11.1614434376257))*(cj1)*(pz)))+(((IkReal(-18.6024057293761))*(pp)))+(((IkReal(2.79036085940642))*(x196)))+(((IkReal(2.79036085940642))*(x197)))+(((x195)*(x197)))));
862 j3array[0]=((IkReal(-1.34567065063197))+(((IkReal(-1.00000000000000))*(x198))));
863 sj3array[0]=IKsin(j3array[0]);
864 cj3array[0]=IKcos(j3array[0]);
865 j3array[1]=((IkReal(1.79592200295782))+(x198));
866 sj3array[1]=IKsin(j3array[1]);
867 cj3array[1]=IKcos(j3array[1]);
868 if( j3array[0] > IKPI )
869 {
870  j3array[0]-=IK2PI;
871 }
872 else if( j3array[0] < -IKPI )
873 { j3array[0]+=IK2PI;
874 }
875 j3valid[0] = true;
876 if( j3array[1] > IKPI )
877 {
878  j3array[1]-=IK2PI;
879 }
880 else if( j3array[1] < -IKPI )
881 { j3array[1]+=IK2PI;
882 }
883 j3valid[1] = true;
884 for(int ij3 = 0; ij3 < 2; ++ij3)
885 {
886 if( !j3valid[ij3] )
887 {
888  continue;
889 }
890 _ij3[0] = ij3; _ij3[1] = -1;
891 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
892 {
893 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
894 {
895  j3valid[iij3]=false; _ij3[1] = iij3; break;
896 }
897 }
898 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
899 {
900 IkReal evalcond[1];
901 IkReal x199=((IkReal(2.00000000000000))*(sj0));
902 IkReal x200=((px)*(py));
903 IkReal x201=((r00)*(sj0));
904 IkReal x202=((cj0)*(r01));
905 IkReal x203=((py)*(r00));
906 IkReal x204=((IkReal(2.00000000000000))*(cj0));
907 IkReal x205=((px)*(r01));
908 IkReal x206=((IkReal(0.600000000000000))*(cj1));
909 IkReal x207=((py)*(r02));
910 IkReal x208=((IkReal(0.600000000000000))*(sj1));
911 IkReal x209=((IkReal(0.0450000000000000))*(sj1));
912 IkReal x210=((px)*(r02));
913 evalcond[0]=((IkReal(0.119281250000000))+(((x203)*(x208)))+(((pp)*(x202)))+(((r01)*(x199)*(x200)))+(((IkReal(-1.00000000000000))*(pp)*(x201)))+(((r00)*(x199)*((px)*(px))))+(((IkReal(0.0956250000000000))*(x202)))+(((IkReal(-1.00000000000000))*(r00)*(x200)*(x204)))+(((IkReal(-2.00000000000000))*(x202)*((py)*(py))))+(((IkReal(-1.00000000000000))*(pz)*(x204)*(x207)))+(((IkReal(-1.00000000000000))*(pz)*(x202)*(x206)))+(((IkReal(-0.0956250000000000))*(x201)))+(((IkReal(-1.00000000000000))*(sj0)*(x206)*(x210)))+(((pz)*(x201)*(x206)))+(((x202)*(x209)))+(((IkReal(0.150000000000000))*(x203)))+(((pz)*(x199)*(x210)))+(((IkReal(-1.00000000000000))*(x201)*(x209)))+(((IkReal(0.0120000000000000))*(IKsin(j3))))+(((IkReal(0.0524000000000000))*(IKcos(j3))))+(((IkReal(-0.150000000000000))*(x205)))+(((IkReal(-1.00000000000000))*(x205)*(x208)))+(((cj0)*(x206)*(x207))));
914 if( IKabs(evalcond[0]) > 0.000001 )
915 {
916 continue;
917 }
918 }
919 
920 {
921 IkReal dummyeval[1];
922 IkReal gconst10;
923 gconst10=IKsign(((IkReal(0.112881250000000))+(((IkReal(0.0524000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
924 dummyeval[0]=((IkReal(17.6376953125000))+(((IkReal(8.18750000000000))*(cj3)))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3)));
925 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
926 {
927 {
928 IkReal dummyeval[1];
929 IkReal gconst11;
930 gconst11=IKsign(((IkReal(-0.112881250000000))+(((IkReal(-0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(-0.0120000000000000))*(sj3)))+(((IkReal(-0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(-0.0524000000000000))*(cj3)))));
931 dummyeval[0]=((IkReal(-17.6376953125000))+(((IkReal(-1.87500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-8.18750000000000))*(cj3)))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
932 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
933 {
934 continue;
935 
936 } else
937 {
938 {
939 IkReal j2array[1], cj2array[1], sj2array[1];
940 bool j2valid[1]={false};
941 _nj2 = 1;
942 IkReal x211=((pz)*(r01));
943 IkReal x212=((py)*(r02));
944 IkReal x213=((cj1)*(r01));
945 IkReal x214=((IkReal(0.00600000000000000))*(cj0));
946 IkReal x215=((IkReal(0.0750000000000000))*(sj1));
947 IkReal x216=((py)*(sj0));
948 IkReal x217=((cj1)*(pz));
949 IkReal x218=((r00)*(sj0));
950 IkReal x219=((px)*(sj3));
951 IkReal x220=((IkReal(0.0800000000000000))*(sj3));
952 IkReal x221=((IkReal(0.0800000000000000))*(cj3));
953 IkReal x222=((IkReal(0.00600000000000000))*(sj3));
954 IkReal x223=((IkReal(0.00600000000000000))*(cj3));
955 IkReal x224=((IkReal(0.327500000000000))*(sj1));
956 IkReal x225=((IkReal(0.0800000000000000))*(cj0)*(sj1));
957 IkReal x226=((cj0)*(x224));
958 IkReal x227=((sj1)*(x221));
959 IkReal x228=((px)*(r02)*(sj0));
960 IkReal x229=((cj1)*(py)*(r00));
961 IkReal x230=((IkReal(0.0800000000000000))*(sj1)*(x228));
962 if( IKabs(((gconst11)*(((IkReal(0.0982500000000000))+(((IkReal(0.00562500000000000))*(cj0)*(x213)))+(((x220)*(x229)))+(((IkReal(-1.00000000000000))*(cj1)*(x218)*(x222)))+(((IkReal(-1.00000000000000))*(cj0)*(x212)*(x215)))+(((cj0)*(sj1)*(x211)*(x220)))+(((sj3)*(x213)*(x214)))+(((IkReal(0.0750000000000000))*(x229)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x227)))+(((IkReal(0.0245625000000000))*(sj1)))+(((IkReal(-0.327500000000000))*(x217)))+(((IkReal(-1.00000000000000))*(x217)*(x221)))+(((IkReal(0.0800000000000000))*(r02)*(sj0)*(sj1)*(x219)))+(((IkReal(-0.00562500000000000))*(cj1)*(x218)))+(((IkReal(-0.0750000000000000))*(px)*(x213)))+(((IkReal(-1.00000000000000))*(pz)*(sj1)*(x218)*(x220)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x212)*(x220)))+(((x215)*(x228)))+(((IkReal(-1.00000000000000))*(x216)*(x224)))+(((IkReal(-1.00000000000000))*(x216)*(x227)))+(((IkReal(-1.00000000000000))*(pz)*(x215)*(x218)))+(((IkReal(-1.00000000000000))*(px)*(x226)))+(((cj0)*(x211)*(x215)))+(((sj1)*(x223)))+(((IkReal(0.0240000000000000))*(cj3)))+(((IkReal(-0.0800000000000000))*(x213)*(x219))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((IkReal(0.0225000000000000))+(((cj1)*(x218)*(x223)))+(((IkReal(-1.00000000000000))*(x221)*(x229)))+(((IkReal(-0.0245625000000000))*(cj0)*(x213)))+(((IkReal(0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x211)*(x226)))+(((IkReal(-0.0750000000000000))*(x217)))+(((IkReal(-1.00000000000000))*(x217)*(x220)))+(((IkReal(-1.00000000000000))*(x224)*(x228)))+(((pz)*(x218)*(x227)))+(((IkReal(-1.00000000000000))*(cj0)*(x211)*(x227)))+(((IkReal(-1.00000000000000))*(x219)*(x225)))+(((IkReal(-0.327500000000000))*(x229)))+(((IkReal(0.327500000000000))*(px)*(x213)))+(((IkReal(-1.00000000000000))*(x215)*(x216)))+(((IkReal(-1.00000000000000))*(sj1)*(x216)*(x220)))+(((IkReal(-1.00000000000000))*(x227)*(x228)))+(((px)*(x213)*(x221)))+(((IkReal(0.0245625000000000))*(cj1)*(x218)))+(((x212)*(x226)))+(((sj1)*(x222)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x215)))+(((IkReal(-1.00000000000000))*(cj3)*(x213)*(x214)))+(((pz)*(x218)*(x224)))+(((IkReal(0.0240000000000000))*(sj3)))+(((cj0)*(x212)*(x227))))))) < IKFAST_ATAN2_MAGTHRESH )
963  continue;
964 j2array[0]=IKatan2(((gconst11)*(((IkReal(0.0982500000000000))+(((IkReal(0.00562500000000000))*(cj0)*(x213)))+(((x220)*(x229)))+(((IkReal(-1.00000000000000))*(cj1)*(x218)*(x222)))+(((IkReal(-1.00000000000000))*(cj0)*(x212)*(x215)))+(((cj0)*(sj1)*(x211)*(x220)))+(((sj3)*(x213)*(x214)))+(((IkReal(0.0750000000000000))*(x229)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x227)))+(((IkReal(0.0245625000000000))*(sj1)))+(((IkReal(-0.327500000000000))*(x217)))+(((IkReal(-1.00000000000000))*(x217)*(x221)))+(((IkReal(0.0800000000000000))*(r02)*(sj0)*(sj1)*(x219)))+(((IkReal(-0.00562500000000000))*(cj1)*(x218)))+(((IkReal(-0.0750000000000000))*(px)*(x213)))+(((IkReal(-1.00000000000000))*(pz)*(sj1)*(x218)*(x220)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x212)*(x220)))+(((x215)*(x228)))+(((IkReal(-1.00000000000000))*(x216)*(x224)))+(((IkReal(-1.00000000000000))*(x216)*(x227)))+(((IkReal(-1.00000000000000))*(pz)*(x215)*(x218)))+(((IkReal(-1.00000000000000))*(px)*(x226)))+(((cj0)*(x211)*(x215)))+(((sj1)*(x223)))+(((IkReal(0.0240000000000000))*(cj3)))+(((IkReal(-0.0800000000000000))*(x213)*(x219)))))), ((gconst11)*(((IkReal(0.0225000000000000))+(((cj1)*(x218)*(x223)))+(((IkReal(-1.00000000000000))*(x221)*(x229)))+(((IkReal(-0.0245625000000000))*(cj0)*(x213)))+(((IkReal(0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x211)*(x226)))+(((IkReal(-0.0750000000000000))*(x217)))+(((IkReal(-1.00000000000000))*(x217)*(x220)))+(((IkReal(-1.00000000000000))*(x224)*(x228)))+(((pz)*(x218)*(x227)))+(((IkReal(-1.00000000000000))*(cj0)*(x211)*(x227)))+(((IkReal(-1.00000000000000))*(x219)*(x225)))+(((IkReal(-0.327500000000000))*(x229)))+(((IkReal(0.327500000000000))*(px)*(x213)))+(((IkReal(-1.00000000000000))*(x215)*(x216)))+(((IkReal(-1.00000000000000))*(sj1)*(x216)*(x220)))+(((IkReal(-1.00000000000000))*(x227)*(x228)))+(((px)*(x213)*(x221)))+(((IkReal(0.0245625000000000))*(cj1)*(x218)))+(((x212)*(x226)))+(((sj1)*(x222)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x215)))+(((IkReal(-1.00000000000000))*(cj3)*(x213)*(x214)))+(((pz)*(x218)*(x224)))+(((IkReal(0.0240000000000000))*(sj3)))+(((cj0)*(x212)*(x227)))))));
965 sj2array[0]=IKsin(j2array[0]);
966 cj2array[0]=IKcos(j2array[0]);
967 if( j2array[0] > IKPI )
968 {
969  j2array[0]-=IK2PI;
970 }
971 else if( j2array[0] < -IKPI )
972 { j2array[0]+=IK2PI;
973 }
974 j2valid[0] = true;
975 for(int ij2 = 0; ij2 < 1; ++ij2)
976 {
977 if( !j2valid[ij2] )
978 {
979  continue;
980 }
981 _ij2[0] = ij2; _ij2[1] = -1;
982 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
983 {
984 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
985 {
986  j2valid[iij2]=false; _ij2[1] = iij2; break;
987 }
988 }
989 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
990 {
991 IkReal evalcond[4];
992 IkReal x231=IKcos(j2);
993 IkReal x232=IKsin(j2);
994 IkReal x233=((IkReal(0.0800000000000000))*(sj3));
995 IkReal x234=((cj0)*(r01));
996 IkReal x235=((IkReal(1.00000000000000))*(px));
997 IkReal x236=((py)*(sj1));
998 IkReal x237=((cj0)*(r02));
999 IkReal x238=((IkReal(0.0750000000000000))*(cj1));
1000 IkReal x239=((r02)*(sj0));
1001 IkReal x240=((IkReal(0.0750000000000000))*(sj1));
1002 IkReal x241=((cj1)*(pz));
1003 IkReal x242=((r00)*(sj0));
1004 IkReal x243=((IkReal(0.0800000000000000))*(cj3));
1005 IkReal x244=((pz)*(sj1));
1006 IkReal x245=((IkReal(1.00000000000000))*(sj0));
1007 IkReal x246=((cj1)*(py));
1008 IkReal x247=((IkReal(0.327500000000000))*(x231));
1009 IkReal x248=((IkReal(0.0750000000000000))*(x232));
1010 IkReal x249=((IkReal(0.0750000000000000))*(x231));
1011 IkReal x250=((IkReal(0.327500000000000))*(x232));
1012 IkReal x251=((sj1)*(x242));
1013 IkReal x252=((x231)*(x243));
1014 IkReal x253=((x232)*(x233));
1015 IkReal x254=((x231)*(x233));
1016 IkReal x255=((x232)*(x243));
1017 IkReal x256=((x253)+(x248));
1018 IkReal x257=((x252)+(x247));
1019 IkReal x258=((x254)+(x255)+(x250)+(x249));
1020 evalcond[0]=((((IkReal(-1.00000000000000))*(x256)))+(((IkReal(-1.00000000000000))*(x245)*(x246)))+(x238)+(x257)+(x244)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x235))));
1021 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x236)*(x245)))+(x258)+(x240)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x235)))+(((IkReal(-1.00000000000000))*(x241))));
1022 evalcond[2]=((((x242)*(x244)))+(((IkReal(-1.00000000000000))*(x256)))+(x257)+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(sj1)*(x235)*(x239)))+(((x238)*(x242)))+(((IkReal(-1.00000000000000))*(x234)*(x238)))+(((x236)*(x237)))+(((IkReal(-1.00000000000000))*(r00)*(x246)))+(((IkReal(-1.00000000000000))*(x234)*(x244))));
1023 evalcond[3]=((((x237)*(x246)))+(((IkReal(-1.00000000000000))*(cj1)*(x235)*(x239)))+(((IkReal(-1.00000000000000))*(x258)))+(((IkReal(-0.300000000000000))*(x242)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x235)))+(((x241)*(x242)))+(((x234)*(x240)))+(((IkReal(-1.00000000000000))*(x234)*(x241)))+(((IkReal(0.300000000000000))*(x234)))+(((IkReal(-1.00000000000000))*(x240)*(x242)))+(((r00)*(x236))));
1024 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1025 {
1026 continue;
1027 }
1028 }
1029 
1030 {
1031 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1032 vinfos[0].jointtype = 1;
1033 vinfos[0].foffset = j0;
1034 vinfos[0].indices[0] = _ij0[0];
1035 vinfos[0].indices[1] = _ij0[1];
1036 vinfos[0].maxsolutions = _nj0;
1037 vinfos[1].jointtype = 1;
1038 vinfos[1].foffset = j1;
1039 vinfos[1].indices[0] = _ij1[0];
1040 vinfos[1].indices[1] = _ij1[1];
1041 vinfos[1].maxsolutions = _nj1;
1042 vinfos[2].jointtype = 1;
1043 vinfos[2].foffset = j2;
1044 vinfos[2].indices[0] = _ij2[0];
1045 vinfos[2].indices[1] = _ij2[1];
1046 vinfos[2].maxsolutions = _nj2;
1047 vinfos[3].jointtype = 1;
1048 vinfos[3].foffset = j3;
1049 vinfos[3].indices[0] = _ij3[0];
1050 vinfos[3].indices[1] = _ij3[1];
1051 vinfos[3].maxsolutions = _nj3;
1052 vinfos[4].jointtype = 1;
1053 vinfos[4].foffset = j4;
1054 vinfos[4].indices[0] = _ij4[0];
1055 vinfos[4].indices[1] = _ij4[1];
1056 vinfos[4].maxsolutions = _nj4;
1057 std::vector<int> vfree(0);
1058 solutions.AddSolution(vinfos,vfree);
1059 }
1060 }
1061 }
1062 
1063 }
1064 
1065 }
1066 
1067 } else
1068 {
1069 {
1070 IkReal j2array[1], cj2array[1], sj2array[1];
1071 bool j2valid[1]={false};
1072 _nj2 = 1;
1073 IkReal x259=((IkReal(0.0800000000000000))*(cj1));
1074 IkReal x260=((cj0)*(px));
1075 IkReal x261=((py)*(sj0));
1076 IkReal x262=((IkReal(0.0750000000000000))*(cj1));
1077 IkReal x263=((IkReal(0.327500000000000))*(cj1));
1078 IkReal x264=((IkReal(0.327500000000000))*(sj1));
1079 IkReal x265=((IkReal(0.0750000000000000))*(sj1));
1080 IkReal x266=((IkReal(0.00600000000000000))*(sj1));
1081 IkReal x267=((IkReal(0.00600000000000000))*(cj1));
1082 IkReal x268=((IkReal(0.0800000000000000))*(sj1)*(sj3));
1083 IkReal x269=((IkReal(0.0800000000000000))*(cj3)*(sj1));
1084 if( IKabs(((gconst10)*(((IkReal(-0.0982500000000000))+(((IkReal(-1.00000000000000))*(x261)*(x262)))+(((pz)*(x263)))+(((pz)*(x268)))+(((cj3)*(pz)*(x259)))+(((IkReal(-1.00000000000000))*(sj3)*(x259)*(x261)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x260)*(x262)))+(((x261)*(x269)))+(((IkReal(-1.00000000000000))*(cj3)*(x266)))+(((sj3)*(x267)))+(((x261)*(x264)))+(((IkReal(-1.00000000000000))*(sj3)*(x259)*(x260)))+(((x260)*(x264)))+(((x260)*(x269)))+(((IkReal(0.00562500000000000))*(cj1)))+(((pz)*(x265))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((pz)*(sj3)*(x259)))+(((cj3)*(x259)*(x260)))+(((x261)*(x265)))+(((x261)*(x263)))+(((x260)*(x265)))+(((IkReal(-1.00000000000000))*(pz)*(x269)))+(((IkReal(-1.00000000000000))*(sj3)*(x266)))+(((x261)*(x268)))+(((x260)*(x263)))+(((IkReal(-1.00000000000000))*(pz)*(x264)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((cj3)*(x259)*(x261)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(cj3)*(x267)))+(((x260)*(x268)))+(((pz)*(x262))))))) < IKFAST_ATAN2_MAGTHRESH )
1085  continue;
1086 j2array[0]=IKatan2(((gconst10)*(((IkReal(-0.0982500000000000))+(((IkReal(-1.00000000000000))*(x261)*(x262)))+(((pz)*(x263)))+(((pz)*(x268)))+(((cj3)*(pz)*(x259)))+(((IkReal(-1.00000000000000))*(sj3)*(x259)*(x261)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x260)*(x262)))+(((x261)*(x269)))+(((IkReal(-1.00000000000000))*(cj3)*(x266)))+(((sj3)*(x267)))+(((x261)*(x264)))+(((IkReal(-1.00000000000000))*(sj3)*(x259)*(x260)))+(((x260)*(x264)))+(((x260)*(x269)))+(((IkReal(0.00562500000000000))*(cj1)))+(((pz)*(x265)))))), ((gconst10)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((pz)*(sj3)*(x259)))+(((cj3)*(x259)*(x260)))+(((x261)*(x265)))+(((x261)*(x263)))+(((x260)*(x265)))+(((IkReal(-1.00000000000000))*(pz)*(x269)))+(((IkReal(-1.00000000000000))*(sj3)*(x266)))+(((x261)*(x268)))+(((x260)*(x263)))+(((IkReal(-1.00000000000000))*(pz)*(x264)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((cj3)*(x259)*(x261)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(cj3)*(x267)))+(((x260)*(x268)))+(((pz)*(x262)))))));
1087 sj2array[0]=IKsin(j2array[0]);
1088 cj2array[0]=IKcos(j2array[0]);
1089 if( j2array[0] > IKPI )
1090 {
1091  j2array[0]-=IK2PI;
1092 }
1093 else if( j2array[0] < -IKPI )
1094 { j2array[0]+=IK2PI;
1095 }
1096 j2valid[0] = true;
1097 for(int ij2 = 0; ij2 < 1; ++ij2)
1098 {
1099 if( !j2valid[ij2] )
1100 {
1101  continue;
1102 }
1103 _ij2[0] = ij2; _ij2[1] = -1;
1104 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
1105 {
1106 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
1107 {
1108  j2valid[iij2]=false; _ij2[1] = iij2; break;
1109 }
1110 }
1111 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
1112 {
1113 IkReal evalcond[4];
1114 IkReal x270=IKcos(j2);
1115 IkReal x271=IKsin(j2);
1116 IkReal x272=((IkReal(0.0800000000000000))*(sj3));
1117 IkReal x273=((cj0)*(r01));
1118 IkReal x274=((IkReal(1.00000000000000))*(px));
1119 IkReal x275=((py)*(sj1));
1120 IkReal x276=((cj0)*(r02));
1121 IkReal x277=((IkReal(0.0750000000000000))*(cj1));
1122 IkReal x278=((r02)*(sj0));
1123 IkReal x279=((IkReal(0.0750000000000000))*(sj1));
1124 IkReal x280=((cj1)*(pz));
1125 IkReal x281=((r00)*(sj0));
1126 IkReal x282=((IkReal(0.0800000000000000))*(cj3));
1127 IkReal x283=((pz)*(sj1));
1128 IkReal x284=((IkReal(1.00000000000000))*(sj0));
1129 IkReal x285=((cj1)*(py));
1130 IkReal x286=((IkReal(0.327500000000000))*(x270));
1131 IkReal x287=((IkReal(0.0750000000000000))*(x271));
1132 IkReal x288=((IkReal(0.0750000000000000))*(x270));
1133 IkReal x289=((IkReal(0.327500000000000))*(x271));
1134 IkReal x290=((sj1)*(x281));
1135 IkReal x291=((x270)*(x282));
1136 IkReal x292=((x271)*(x272));
1137 IkReal x293=((x270)*(x272));
1138 IkReal x294=((x271)*(x282));
1139 IkReal x295=((x287)+(x292));
1140 IkReal x296=((x286)+(x291));
1141 IkReal x297=((x289)+(x288)+(x293)+(x294));
1142 evalcond[0]=((((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x274)))+(x283)+(x296)+(((IkReal(-1.00000000000000))*(x295)))+(x277)+(((IkReal(-1.00000000000000))*(x284)*(x285))));
1143 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x280)))+(((IkReal(-1.00000000000000))*(x275)*(x284)))+(x297)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x274)))+(x279));
1144 evalcond[2]=((((x281)*(x283)))+(((IkReal(-1.00000000000000))*(sj1)*(x274)*(x278)))+(((IkReal(-1.00000000000000))*(x273)*(x277)))+(x296)+(((cj1)*(px)*(r01)))+(((x275)*(x276)))+(((IkReal(-1.00000000000000))*(x295)))+(((IkReal(-1.00000000000000))*(x273)*(x283)))+(((x277)*(x281)))+(((IkReal(-1.00000000000000))*(r00)*(x285))));
1145 evalcond[3]=((((x280)*(x281)))+(((IkReal(-1.00000000000000))*(x279)*(x281)))+(((r00)*(x275)))+(((x273)*(x279)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x274)))+(((IkReal(0.300000000000000))*(x273)))+(((IkReal(-1.00000000000000))*(x297)))+(((IkReal(-0.300000000000000))*(x281)))+(((x276)*(x285)))+(((IkReal(-1.00000000000000))*(x273)*(x280)))+(((IkReal(-1.00000000000000))*(cj1)*(x274)*(x278))));
1146 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1147 {
1148 continue;
1149 }
1150 }
1151 
1152 {
1153 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1154 vinfos[0].jointtype = 1;
1155 vinfos[0].foffset = j0;
1156 vinfos[0].indices[0] = _ij0[0];
1157 vinfos[0].indices[1] = _ij0[1];
1158 vinfos[0].maxsolutions = _nj0;
1159 vinfos[1].jointtype = 1;
1160 vinfos[1].foffset = j1;
1161 vinfos[1].indices[0] = _ij1[0];
1162 vinfos[1].indices[1] = _ij1[1];
1163 vinfos[1].maxsolutions = _nj1;
1164 vinfos[2].jointtype = 1;
1165 vinfos[2].foffset = j2;
1166 vinfos[2].indices[0] = _ij2[0];
1167 vinfos[2].indices[1] = _ij2[1];
1168 vinfos[2].maxsolutions = _nj2;
1169 vinfos[3].jointtype = 1;
1170 vinfos[3].foffset = j3;
1171 vinfos[3].indices[0] = _ij3[0];
1172 vinfos[3].indices[1] = _ij3[1];
1173 vinfos[3].maxsolutions = _nj3;
1174 vinfos[4].jointtype = 1;
1175 vinfos[4].foffset = j4;
1176 vinfos[4].indices[0] = _ij4[0];
1177 vinfos[4].indices[1] = _ij4[1];
1178 vinfos[4].maxsolutions = _nj4;
1179 std::vector<int> vfree(0);
1180 solutions.AddSolution(vinfos,vfree);
1181 }
1182 }
1183 }
1184 
1185 }
1186 
1187 }
1188 }
1189 }
1190 
1191 } else
1192 {
1193 if( 1 )
1194 {
1195 continue;
1196 
1197 } else
1198 {
1199 }
1200 }
1201 }
1202 }
1203 
1204 } else
1205 {
1206 {
1207 IkReal j3array[1], cj3array[1], sj3array[1];
1208 bool j3valid[1]={false};
1209 _nj3 = 1;
1210 IkReal x298=((px)*(r00));
1211 IkReal x299=((cj0)*(r00));
1212 IkReal x300=((IkReal(15720.0000000000))*(cj1));
1213 IkReal x301=((IkReal(3600.00000000000))*(sj1));
1214 IkReal x302=((r01)*(sj0));
1215 IkReal x303=((IkReal(3600.00000000000))*(cj1));
1216 IkReal x304=((py)*(r01));
1217 IkReal x305=((pz)*(r02));
1218 IkReal x306=((IkReal(12000.0000000000))*(pz));
1219 IkReal x307=((IkReal(52400.0000000000))*(r02));
1220 IkReal x308=((cj0)*(px));
1221 IkReal x309=((py)*(sj0));
1222 IkReal x310=((IkReal(52400.0000000000))*(pz));
1223 IkReal x311=((IkReal(15720.0000000000))*(sj1));
1224 IkReal x312=((IkReal(12000.0000000000))*(r02));
1225 IkReal x313=((sj1)*(x302));
1226 if( IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(x302)*(x306)))+(((IkReal(-900.000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x299)*(x306)))+(((x299)*(x303)))+(((IkReal(-52400.0000000000))*(x305)))+(((IkReal(-1.00000000000000))*(r02)*(x301)))+(((x308)*(x312)))+(((x299)*(x311)))+(((x302)*(x303)))+(((x309)*(x312)))+(((IkReal(3930.00000000000))*(x302)))+(((x302)*(x311)))+(((IkReal(-52400.0000000000))*(x304)))+(((r02)*(x300)))+(((IkReal(-960.000000000000))*(cj4)))+(((IkReal(3930.00000000000))*(x299)))+(((IkReal(-52400.0000000000))*(x298))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(x299)*(x301)))+(((IkReal(-900.000000000000))*(x302)))+(((IkReal(-1.00000000000000))*(r02)*(x311)))+(((IkReal(12000.0000000000))*(x304)))+(((IkReal(-1.00000000000000))*(r02)*(x303)))+(((IkReal(-3930.00000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x302)*(x310)))+(((IkReal(12000.0000000000))*(x305)))+(((IkReal(-1.00000000000000))*(x299)*(x310)))+(((x307)*(x309)))+(((IkReal(12000.0000000000))*(x298)))+(((x300)*(x302)))+(((x299)*(x300)))+(((x307)*(x308)))+(((IkReal(-900.000000000000))*(x299)))+(((IkReal(-4192.00000000000))*(cj4)))+(((IkReal(-1.00000000000000))*(x301)*(x302))))))) < IKFAST_ATAN2_MAGTHRESH )
1227  continue;
1228 j3array[0]=IKatan2(((gconst1)*(((((IkReal(-1.00000000000000))*(x302)*(x306)))+(((IkReal(-900.000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x299)*(x306)))+(((x299)*(x303)))+(((IkReal(-52400.0000000000))*(x305)))+(((IkReal(-1.00000000000000))*(r02)*(x301)))+(((x308)*(x312)))+(((x299)*(x311)))+(((x302)*(x303)))+(((x309)*(x312)))+(((IkReal(3930.00000000000))*(x302)))+(((x302)*(x311)))+(((IkReal(-52400.0000000000))*(x304)))+(((r02)*(x300)))+(((IkReal(-960.000000000000))*(cj4)))+(((IkReal(3930.00000000000))*(x299)))+(((IkReal(-52400.0000000000))*(x298)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(x299)*(x301)))+(((IkReal(-900.000000000000))*(x302)))+(((IkReal(-1.00000000000000))*(r02)*(x311)))+(((IkReal(12000.0000000000))*(x304)))+(((IkReal(-1.00000000000000))*(r02)*(x303)))+(((IkReal(-3930.00000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x302)*(x310)))+(((IkReal(12000.0000000000))*(x305)))+(((IkReal(-1.00000000000000))*(x299)*(x310)))+(((x307)*(x309)))+(((IkReal(12000.0000000000))*(x298)))+(((x300)*(x302)))+(((x299)*(x300)))+(((x307)*(x308)))+(((IkReal(-900.000000000000))*(x299)))+(((IkReal(-4192.00000000000))*(cj4)))+(((IkReal(-1.00000000000000))*(x301)*(x302)))))));
1229 sj3array[0]=IKsin(j3array[0]);
1230 cj3array[0]=IKcos(j3array[0]);
1231 if( j3array[0] > IKPI )
1232 {
1233  j3array[0]-=IK2PI;
1234 }
1235 else if( j3array[0] < -IKPI )
1236 { j3array[0]+=IK2PI;
1237 }
1238 j3valid[0] = true;
1239 for(int ij3 = 0; ij3 < 1; ++ij3)
1240 {
1241 if( !j3valid[ij3] )
1242 {
1243  continue;
1244 }
1245 _ij3[0] = ij3; _ij3[1] = -1;
1246 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
1247 {
1248 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1249 {
1250  j3valid[iij3]=false; _ij3[1] = iij3; break;
1251 }
1252 }
1253 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1254 {
1255 IkReal evalcond[4];
1256 IkReal x314=IKcos(j3);
1257 IkReal x315=IKsin(j3);
1258 IkReal x316=((r01)*(sj0));
1259 IkReal x317=((IkReal(0.300000000000000))*(r02));
1260 IkReal x318=((r00)*(sj0));
1261 IkReal x319=((py)*(sj0));
1262 IkReal x320=((IkReal(0.600000000000000))*(cj1));
1263 IkReal x321=((IkReal(0.150000000000000))*(px));
1264 IkReal x322=((IkReal(1.00000000000000))*(pz));
1265 IkReal x323=((py)*(r00));
1266 IkReal x324=((IkReal(0.300000000000000))*(cj1));
1267 IkReal x325=((IkReal(1.00000000000000))*(pp));
1268 IkReal x326=((cj0)*(r00));
1269 IkReal x327=((IkReal(0.0450000000000000))*(sj1));
1270 IkReal x328=((IkReal(0.600000000000000))*(sj1));
1271 IkReal x329=((cj0)*(r01));
1272 IkReal x330=((IkReal(2.00000000000000))*(pz));
1273 IkReal x331=((cj0)*(px));
1274 IkReal x332=((IkReal(0.300000000000000))*(sj1));
1275 IkReal x333=((IkReal(2.00000000000000))*(px)*(py));
1276 IkReal x334=((IkReal(0.0120000000000000))*(x315));
1277 IkReal x335=((cj4)*(x315));
1278 IkReal x336=((IkReal(0.0524000000000000))*(x314));
1279 IkReal x337=((cj4)*(x314));
1280 IkReal x338=((cj0)*(py)*(r02));
1281 IkReal x339=((px)*(r02)*(sj0));
1282 evalcond[0]=((IkReal(0.0236562500000000))+(((x319)*(x328)))+(((IkReal(-1.00000000000000))*(x325)))+(((IkReal(-1.00000000000000))*(x327)))+(((x328)*(x331)))+(((cj0)*(x321)))+(x336)+(x334)+(((pz)*(x320)))+(((IkReal(0.150000000000000))*(x319))));
1283 evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((x326)*(x332)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(0.0750000000000000))*(x316)))+(((IkReal(0.0750000000000000))*(x326)))+(((cj1)*(x317)))+(((IkReal(0.0750000000000000))*(x337)))+(((IkReal(-0.327500000000000))*(x335)))+(((x316)*(x332)))+(((IkReal(-1.00000000000000))*(r02)*(x322))));
1284 evalcond[2]=((((IkReal(-0.0800000000000000))*(cj4)))+(((IkReal(-1.00000000000000))*(x316)*(x322)))+(((r02)*(x319)))+(((x324)*(x326)))+(((x316)*(x324)))+(((IkReal(-0.0750000000000000))*(x335)))+(((IkReal(-1.00000000000000))*(x322)*(x326)))+(((IkReal(-0.0750000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(sj1)*(x317)))+(((r02)*(x331)))+(((IkReal(-0.327500000000000))*(x337))));
1285 evalcond[3]=((((x330)*(x339)))+(((IkReal(-0.119281250000000))*(sj4)))+(((IkReal(-1.00000000000000))*(sj4)*(x334)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x328)))+(((IkReal(-2.00000000000000))*(x329)*((py)*(py))))+(((IkReal(-1.00000000000000))*(sj4)*(x336)))+(((x316)*(x333)))+(((IkReal(-1.00000000000000))*(pz)*(x320)*(x329)))+(((IkReal(-1.00000000000000))*(r01)*(x321)))+(((x327)*(x329)))+(((x320)*(x338)))+(((IkReal(-1.00000000000000))*(x318)*(x327)))+(((pp)*(x329)))+(((IkReal(0.150000000000000))*(x323)))+(((IkReal(-0.0956250000000000))*(x318)))+(((IkReal(-2.00000000000000))*(x323)*(x331)))+(((IkReal(-1.00000000000000))*(x330)*(x338)))+(((IkReal(-1.00000000000000))*(x320)*(x339)))+(((pz)*(x318)*(x320)))+(((IkReal(0.0956250000000000))*(x329)))+(((IkReal(-1.00000000000000))*(x318)*(x325)))+(((IkReal(2.00000000000000))*(x318)*((px)*(px))))+(((x323)*(x328))));
1286 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1287 {
1288 continue;
1289 }
1290 }
1291 
1292 {
1293 IkReal dummyeval[1];
1294 IkReal gconst2;
1295 gconst2=IKsign(((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3))))));
1296 dummyeval[0]=((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3)))));
1297 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1298 {
1299 {
1300 IkReal dummyeval[1];
1301 IkReal gconst3;
1302 IkReal x340=((IkReal(0.0800000000000000))*(cj4));
1303 gconst3=IKsign(((((x340)*((sj3)*(sj3))))+(((x340)*((cj3)*(cj3))))+(((IkReal(0.0750000000000000))*(cj4)*(sj3)))+(((IkReal(0.327500000000000))*(cj3)*(cj4)))));
1304 IkReal x341=((IkReal(1.06666666666667))*(cj4));
1305 dummyeval[0]=((((IkReal(4.36666666666667))*(cj3)*(cj4)))+(((x341)*((sj3)*(sj3))))+(((cj4)*(sj3)))+(((x341)*((cj3)*(cj3)))));
1306 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1307 {
1308 {
1309 IkReal evalcond[11];
1310 IkReal x342=((IkReal(0.0524000000000000))*(cj3));
1311 IkReal x343=((IkReal(0.0120000000000000))*(sj3));
1312 IkReal x344=(py)*(py);
1313 IkReal x345=(px)*(px);
1314 IkReal x346=(pz)*(pz);
1315 IkReal x347=((r01)*(sj0));
1316 IkReal x348=((py)*(r00));
1317 IkReal x349=((pz)*(sj1));
1318 IkReal x350=((py)*(r01));
1319 IkReal x351=((px)*(sj0));
1320 IkReal x352=((IkReal(0.600000000000000))*(r02));
1321 IkReal x353=((IkReal(0.150000000000000))*(cj1));
1322 IkReal x354=((cj0)*(sj1));
1323 IkReal x355=((IkReal(0.150000000000000))*(px));
1324 IkReal x356=((IkReal(2.00000000000000))*(cj1));
1325 IkReal x357=((cj0)*(r01));
1326 IkReal x358=((r02)*(sj1));
1327 IkReal x359=((px)*(r00));
1328 IkReal x360=((IkReal(0.300000000000000))*(r00));
1329 IkReal x361=((IkReal(1.00000000000000))*(pz));
1330 IkReal x362=((r00)*(sj1));
1331 IkReal x363=((cj0)*(r00));
1332 IkReal x364=((cj0)*(cj1));
1333 IkReal x365=((IkReal(1.00000000000000))*(sj1));
1334 IkReal x366=((IkReal(0.0956250000000000))*(r00));
1335 IkReal x367=((IkReal(0.600000000000000))*(pz));
1336 IkReal x368=((IkReal(0.600000000000000))*(sj1));
1337 IkReal x369=((IkReal(2.00000000000000))*(px));
1338 IkReal x370=((IkReal(2.00000000000000))*(sj1));
1339 IkReal x371=((IkReal(0.150000000000000))*(sj1));
1340 IkReal x372=((cj1)*(r02));
1341 IkReal x373=((cj0)*(px));
1342 IkReal x374=((IkReal(0.0843750000000000))*(cj1));
1343 IkReal x375=((py)*(sj0));
1344 IkReal x376=((pz)*(r02));
1345 IkReal x377=((IkReal(1.00000000000000))*(cj1));
1346 IkReal x378=((cj0)*(py));
1347 IkReal x379=((r00)*(sj0));
1348 IkReal x380=((r02)*(x375));
1349 IkReal x381=((pp)*(x377));
1350 IkReal x382=((IkReal(1.00000000000000))*(pp)*(r00));
1351 IkReal x383=((x342)+(x343));
1352 IkReal x384=((IkReal(2.00000000000000))*(r00)*(x345));
1353 IkReal x385=((cj0)*(x369)*(x376));
1354 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
1355 evalcond[1]=((x378)+(((IkReal(-1.00000000000000))*(x351))));
1356 evalcond[2]=((IkReal(-1.00000000000000))+(x357)+(((IkReal(-1.00000000000000))*(x379))));
1357 evalcond[3]=((((IkReal(-1.00000000000000))*(x363)*(x377)))+(x358)+(((IkReal(-1.00000000000000))*(x347)*(x377))));
1358 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x354)))+(((IkReal(-1.00000000000000))*(x372)))+(((IkReal(-1.00000000000000))*(x347)*(x365))));
1359 evalcond[5]=((IkReal(0.0236562500000000))+(x383)+(((IkReal(0.150000000000000))*(x375)))+(((cj0)*(x355)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(0.600000000000000))*(px)*(x354)))+(((x368)*(x375)))+(((cj1)*(x367)))+(((IkReal(-0.0450000000000000))*(sj1))));
1360 evalcond[6]=((((IkReal(0.300000000000000))*(x372)))+(((IkReal(-1.00000000000000))*(r02)*(x361)))+(((x354)*(x360)))+(((IkReal(-1.00000000000000))*(x350)))+(((IkReal(-1.00000000000000))*(x359)))+(((IkReal(0.0750000000000000))*(x363)))+(((IkReal(0.0750000000000000))*(x347)))+(((IkReal(0.300000000000000))*(sj1)*(x347))));
1361 evalcond[7]=((((r02)*(x373)))+(((IkReal(-1.00000000000000))*(x361)*(x363)))+(x380)+(((IkReal(-1.00000000000000))*(x347)*(x361)))+(((IkReal(0.300000000000000))*(cj1)*(x347)))+(((x360)*(x364)))+(((IkReal(-0.0750000000000000))*(r02)))+(((IkReal(-0.300000000000000))*(x358))));
1362 evalcond[8]=((IkReal(-0.119281250000000))+(((IkReal(2.00000000000000))*(x351)*(x376)))+(((IkReal(-2.00000000000000))*(x376)*(x378)))+(((IkReal(-1.00000000000000))*(x383)))+(((IkReal(-0.0450000000000000))*(sj0)*(x362)))+(((IkReal(-1.00000000000000))*(cj0)*(x348)*(x369)))+(((IkReal(-2.00000000000000))*(x344)*(x357)))+(((py)*(x352)*(x364)))+(((IkReal(-1.00000000000000))*(sj0)*(x366)))+(((IkReal(0.0450000000000000))*(r01)*(x354)))+(((x348)*(x368)))+(((IkReal(-1.00000000000000))*(cj1)*(x351)*(x352)))+(((IkReal(-1.00000000000000))*(cj1)*(x357)*(x367)))+(((IkReal(-1.00000000000000))*(r01)*(x355)))+(((IkReal(0.150000000000000))*(x348)))+(((IkReal(2.00000000000000))*(x345)*(x379)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x368)))+(((cj1)*(x367)*(x379)))+(((py)*(x347)*(x369)))+(((IkReal(-1.00000000000000))*(pp)*(x379)))+(((pp)*(x357)))+(((IkReal(0.0956250000000000))*(x357))));
1363 evalcond[9]=((((IkReal(2.00000000000000))*(x349)*(x380)))+(((IkReal(-1.00000000000000))*(x354)*(x382)))+(((IkReal(0.0956250000000000))*(sj1)*(x347)))+(((IkReal(-1.00000000000000))*(pp)*(x347)*(x365)))+(((IkReal(0.0843750000000000))*(x372)))+(((IkReal(-0.600000000000000))*(x359)))+(((IkReal(0.0450000000000000))*(x347)))+(((IkReal(-1.00000000000000))*(pz)*(x347)*(x353)))+(((r02)*(x353)*(x373)))+(((IkReal(-1.00000000000000))*(pz)*(x352)))+(((r02)*(x346)*(x356)))+(((IkReal(0.0450000000000000))*(x363)))+(((IkReal(-1.00000000000000))*(x350)*(x371)))+(((x348)*(x351)*(x370)))+(((IkReal(-0.150000000000000))*(r02)*(x349)))+(((IkReal(-0.600000000000000))*(x350)))+(((x344)*(x347)*(x370)))+(((x354)*(x366)))+(((IkReal(-1.00000000000000))*(pp)*(x372)))+(((IkReal(-1.00000000000000))*(x355)*(x362)))+(((x354)*(x384)))+(((pz)*(x356)*(x359)))+(((x353)*(x380)))+(((pz)*(x350)*(x356)))+(((x350)*(x354)*(x369)))+(((IkReal(-1.00000000000000))*(pz)*(x353)*(x363)))+(((cj0)*(r02)*(x349)*(x369))));
1364 evalcond[10]=((((IkReal(-1.00000000000000))*(x353)*(x359)))+(((x356)*(x375)*(x376)))+(((IkReal(-1.00000000000000))*(x363)*(x381)))+(((IkReal(-2.00000000000000))*(x349)*(x350)))+(((IkReal(-0.150000000000000))*(x358)*(x375)))+(((x363)*(x367)))+(((IkReal(-1.00000000000000))*(x353)*(x376)))+(((x345)*(x356)*(x363)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-2.00000000000000))*(x346)*(x358)))+(((IkReal(-1.00000000000000))*(x352)*(x375)))+(((IkReal(0.150000000000000))*(x347)*(x349)))+(((pp)*(x358)))+(((IkReal(0.0956250000000000))*(x358)))+(((x344)*(x347)*(x356)))+(((x356)*(x373)*(x376)))+(((x348)*(x351)*(x356)))+(((IkReal(-1.00000000000000))*(x363)*(x374)))+(((IkReal(0.150000000000000))*(x349)*(x363)))+(((IkReal(-1.00000000000000))*(x352)*(x373)))+(((IkReal(-1.00000000000000))*(x347)*(x374)))+(((x347)*(x367)))+(((IkReal(-1.00000000000000))*(r02)*(x354)*(x355)))+(((IkReal(-1.00000000000000))*(x347)*(x381)))+(((x350)*(x356)*(x373)))+(((IkReal(-1.00000000000000))*(x350)*(x353)))+(((IkReal(-2.00000000000000))*(x349)*(x359))));
1365 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 )
1366 {
1367 {
1368 IkReal dummyeval[1];
1369 IkReal gconst4;
1370 gconst4=IKsign(((IkReal(0.112881250000000))+(((IkReal(0.0524000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
1371 dummyeval[0]=((IkReal(17.6376953125000))+(((IkReal(8.18750000000000))*(cj3)))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3)));
1372 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1373 {
1374 {
1375 IkReal dummyeval[1];
1376 IkReal gconst5;
1377 gconst5=IKsign(((IkReal(0.112881250000000))+(((IkReal(0.0524000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
1378 dummyeval[0]=((IkReal(17.6376953125000))+(((IkReal(8.18750000000000))*(cj3)))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3)));
1379 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1380 {
1381 continue;
1382 
1383 } else
1384 {
1385 {
1386 IkReal j2array[1], cj2array[1], sj2array[1];
1387 bool j2valid[1]={false};
1388 _nj2 = 1;
1389 IkReal x386=((pz)*(r01));
1390 IkReal x387=((py)*(r02));
1391 IkReal x388=((cj1)*(r01));
1392 IkReal x389=((IkReal(0.00600000000000000))*(cj0));
1393 IkReal x390=((cj1)*(pz));
1394 IkReal x391=((r00)*(sj0));
1395 IkReal x392=((IkReal(0.0750000000000000))*(px));
1396 IkReal x393=((cj0)*(sj1));
1397 IkReal x394=((px)*(sj3));
1398 IkReal x395=((IkReal(0.0800000000000000))*(sj3));
1399 IkReal x396=((sj0)*(sj1));
1400 IkReal x397=((IkReal(0.327500000000000))*(px));
1401 IkReal x398=((IkReal(0.00600000000000000))*(cj1));
1402 IkReal x399=((IkReal(0.0750000000000000))*(py));
1403 IkReal x400=((IkReal(0.0800000000000000))*(cj3));
1404 IkReal x401=((pz)*(sj1));
1405 IkReal x402=((IkReal(0.00600000000000000))*(sj1));
1406 IkReal x403=((IkReal(0.0800000000000000))*(x393));
1407 IkReal x404=((sj1)*(x400));
1408 IkReal x405=((cj1)*(py)*(r00));
1409 IkReal x406=((IkReal(0.0800000000000000))*(px)*(r02)*(x396));
1410 if( IKabs(((gconst5)*(((IkReal(-0.0982500000000000))+(((IkReal(0.0800000000000000))*(r02)*(x394)*(x396)))+(((px)*(x393)*(x400)))+(((IkReal(-0.0800000000000000))*(x388)*(x394)))+(((IkReal(-0.0750000000000000))*(x391)*(x401)))+(((sj3)*(x388)*(x389)))+(((IkReal(-1.00000000000000))*(x388)*(x392)))+(((x393)*(x397)))+(((IkReal(-0.00562500000000000))*(cj1)*(x391)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x391)*(x398)))+(((IkReal(0.327500000000000))*(py)*(x396)))+(((x395)*(x405)))+(((cj1)*(r00)*(x399)))+(((IkReal(-0.0750000000000000))*(x387)*(x393)))+(((IkReal(0.327500000000000))*(x390)))+(((x390)*(x400)))+(((IkReal(-1.00000000000000))*(x387)*(x393)*(x395)))+(((IkReal(0.00562500000000000))*(cj0)*(x388)))+(((py)*(x396)*(x400)))+(((r02)*(x392)*(x396)))+(((IkReal(0.0750000000000000))*(x386)*(x393)))+(((x386)*(x393)*(x395)))+(((IkReal(-1.00000000000000))*(cj3)*(x402)))+(((IkReal(-1.00000000000000))*(x391)*(x395)*(x401))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((x387)*(x393)*(x400)))+(((px)*(x388)*(x400)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x396)*(x400)))+(((IkReal(-0.327500000000000))*(x386)*(x393)))+(((IkReal(0.327500000000000))*(x391)*(x401)))+(((x391)*(x400)*(x401)))+(((IkReal(-1.00000000000000))*(r02)*(x396)*(x397)))+(((IkReal(0.0750000000000000))*(x390)))+(((IkReal(-1.00000000000000))*(x400)*(x405)))+(((IkReal(-0.327500000000000))*(x405)))+(((IkReal(-1.00000000000000))*(x386)*(x393)*(x400)))+(((x392)*(x393)))+(((cj3)*(x391)*(x398)))+(((x396)*(x399)))+(((IkReal(-1.00000000000000))*(sj3)*(x402)))+(((IkReal(0.0245625000000000))*(cj1)*(x391)))+(((x390)*(x395)))+(((py)*(x395)*(x396)))+(((IkReal(0.327500000000000))*(x387)*(x393)))+(((x394)*(x403)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x388)*(x397)))+(((IkReal(-0.0245625000000000))*(cj0)*(x388)))+(((IkReal(-1.00000000000000))*(cj3)*(x388)*(x389))))))) < IKFAST_ATAN2_MAGTHRESH )
1411  continue;
1412 j2array[0]=IKatan2(((gconst5)*(((IkReal(-0.0982500000000000))+(((IkReal(0.0800000000000000))*(r02)*(x394)*(x396)))+(((px)*(x393)*(x400)))+(((IkReal(-0.0800000000000000))*(x388)*(x394)))+(((IkReal(-0.0750000000000000))*(x391)*(x401)))+(((sj3)*(x388)*(x389)))+(((IkReal(-1.00000000000000))*(x388)*(x392)))+(((x393)*(x397)))+(((IkReal(-0.00562500000000000))*(cj1)*(x391)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x391)*(x398)))+(((IkReal(0.327500000000000))*(py)*(x396)))+(((x395)*(x405)))+(((cj1)*(r00)*(x399)))+(((IkReal(-0.0750000000000000))*(x387)*(x393)))+(((IkReal(0.327500000000000))*(x390)))+(((x390)*(x400)))+(((IkReal(-1.00000000000000))*(x387)*(x393)*(x395)))+(((IkReal(0.00562500000000000))*(cj0)*(x388)))+(((py)*(x396)*(x400)))+(((r02)*(x392)*(x396)))+(((IkReal(0.0750000000000000))*(x386)*(x393)))+(((x386)*(x393)*(x395)))+(((IkReal(-1.00000000000000))*(cj3)*(x402)))+(((IkReal(-1.00000000000000))*(x391)*(x395)*(x401)))))), ((gconst5)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((x387)*(x393)*(x400)))+(((px)*(x388)*(x400)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x396)*(x400)))+(((IkReal(-0.327500000000000))*(x386)*(x393)))+(((IkReal(0.327500000000000))*(x391)*(x401)))+(((x391)*(x400)*(x401)))+(((IkReal(-1.00000000000000))*(r02)*(x396)*(x397)))+(((IkReal(0.0750000000000000))*(x390)))+(((IkReal(-1.00000000000000))*(x400)*(x405)))+(((IkReal(-0.327500000000000))*(x405)))+(((IkReal(-1.00000000000000))*(x386)*(x393)*(x400)))+(((x392)*(x393)))+(((cj3)*(x391)*(x398)))+(((x396)*(x399)))+(((IkReal(-1.00000000000000))*(sj3)*(x402)))+(((IkReal(0.0245625000000000))*(cj1)*(x391)))+(((x390)*(x395)))+(((py)*(x395)*(x396)))+(((IkReal(0.327500000000000))*(x387)*(x393)))+(((x394)*(x403)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x388)*(x397)))+(((IkReal(-0.0245625000000000))*(cj0)*(x388)))+(((IkReal(-1.00000000000000))*(cj3)*(x388)*(x389)))))));
1413 sj2array[0]=IKsin(j2array[0]);
1414 cj2array[0]=IKcos(j2array[0]);
1415 if( j2array[0] > IKPI )
1416 {
1417  j2array[0]-=IK2PI;
1418 }
1419 else if( j2array[0] < -IKPI )
1420 { j2array[0]+=IK2PI;
1421 }
1422 j2valid[0] = true;
1423 for(int ij2 = 0; ij2 < 1; ++ij2)
1424 {
1425 if( !j2valid[ij2] )
1426 {
1427  continue;
1428 }
1429 _ij2[0] = ij2; _ij2[1] = -1;
1430 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
1431 {
1432 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
1433 {
1434  j2valid[iij2]=false; _ij2[1] = iij2; break;
1435 }
1436 }
1437 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
1438 {
1439 IkReal evalcond[4];
1440 IkReal x407=IKcos(j2);
1441 IkReal x408=IKsin(j2);
1442 IkReal x409=((IkReal(0.0800000000000000))*(sj3));
1443 IkReal x410=((cj0)*(r01));
1444 IkReal x411=((IkReal(1.00000000000000))*(px));
1445 IkReal x412=((py)*(sj1));
1446 IkReal x413=((cj0)*(r02));
1447 IkReal x414=((IkReal(0.0750000000000000))*(cj1));
1448 IkReal x415=((r02)*(sj0));
1449 IkReal x416=((IkReal(0.0750000000000000))*(sj1));
1450 IkReal x417=((cj1)*(pz));
1451 IkReal x418=((r00)*(sj0));
1452 IkReal x419=((IkReal(0.0800000000000000))*(cj3));
1453 IkReal x420=((pz)*(sj1));
1454 IkReal x421=((IkReal(1.00000000000000))*(sj0));
1455 IkReal x422=((cj1)*(py));
1456 IkReal x423=((IkReal(0.0750000000000000))*(x408));
1457 IkReal x424=((IkReal(0.327500000000000))*(x407));
1458 IkReal x425=((IkReal(0.0750000000000000))*(x407));
1459 IkReal x426=((IkReal(0.327500000000000))*(x408));
1460 IkReal x427=((sj1)*(x418));
1461 IkReal x428=((x408)*(x409));
1462 IkReal x429=((x407)*(x419));
1463 IkReal x430=((x407)*(x409));
1464 IkReal x431=((x408)*(x419));
1465 IkReal x432=((x428)+(x423));
1466 IkReal x433=((x429)+(x424));
1467 IkReal x434=((x430)+(x431)+(x426)+(x425));
1468 evalcond[0]=((x433)+(x420)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x411)))+(((IkReal(-1.00000000000000))*(x421)*(x422)))+(((IkReal(-1.00000000000000))*(x432)))+(x414));
1469 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x412)*(x421)))+(x434)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x411)))+(((IkReal(-1.00000000000000))*(x417)))+(x416));
1470 evalcond[2]=((((IkReal(-1.00000000000000))*(x410)*(x414)))+(((x412)*(x413)))+(x432)+(((x418)*(x420)))+(((IkReal(-1.00000000000000))*(r00)*(x422)))+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(x410)*(x420)))+(((x414)*(x418)))+(((IkReal(-1.00000000000000))*(x433)))+(((IkReal(-1.00000000000000))*(sj1)*(x411)*(x415))));
1471 evalcond[3]=((x434)+(((IkReal(-0.300000000000000))*(x418)))+(((IkReal(-1.00000000000000))*(x416)*(x418)))+(((x413)*(x422)))+(((IkReal(-1.00000000000000))*(x410)*(x417)))+(((IkReal(0.300000000000000))*(x410)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x411)))+(((x417)*(x418)))+(((IkReal(-1.00000000000000))*(cj1)*(x411)*(x415)))+(((r00)*(x412)))+(((x410)*(x416))));
1472 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1473 {
1474 continue;
1475 }
1476 }
1477 
1478 {
1479 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1480 vinfos[0].jointtype = 1;
1481 vinfos[0].foffset = j0;
1482 vinfos[0].indices[0] = _ij0[0];
1483 vinfos[0].indices[1] = _ij0[1];
1484 vinfos[0].maxsolutions = _nj0;
1485 vinfos[1].jointtype = 1;
1486 vinfos[1].foffset = j1;
1487 vinfos[1].indices[0] = _ij1[0];
1488 vinfos[1].indices[1] = _ij1[1];
1489 vinfos[1].maxsolutions = _nj1;
1490 vinfos[2].jointtype = 1;
1491 vinfos[2].foffset = j2;
1492 vinfos[2].indices[0] = _ij2[0];
1493 vinfos[2].indices[1] = _ij2[1];
1494 vinfos[2].maxsolutions = _nj2;
1495 vinfos[3].jointtype = 1;
1496 vinfos[3].foffset = j3;
1497 vinfos[3].indices[0] = _ij3[0];
1498 vinfos[3].indices[1] = _ij3[1];
1499 vinfos[3].maxsolutions = _nj3;
1500 vinfos[4].jointtype = 1;
1501 vinfos[4].foffset = j4;
1502 vinfos[4].indices[0] = _ij4[0];
1503 vinfos[4].indices[1] = _ij4[1];
1504 vinfos[4].maxsolutions = _nj4;
1505 std::vector<int> vfree(0);
1506 solutions.AddSolution(vinfos,vfree);
1507 }
1508 }
1509 }
1510 
1511 }
1512 
1513 }
1514 
1515 } else
1516 {
1517 {
1518 IkReal j2array[1], cj2array[1], sj2array[1];
1519 bool j2valid[1]={false};
1520 _nj2 = 1;
1521 IkReal x435=((IkReal(0.0800000000000000))*(cj1));
1522 IkReal x436=((cj0)*(px));
1523 IkReal x437=((py)*(sj0));
1524 IkReal x438=((IkReal(0.0750000000000000))*(cj1));
1525 IkReal x439=((IkReal(0.327500000000000))*(cj1));
1526 IkReal x440=((IkReal(0.327500000000000))*(sj1));
1527 IkReal x441=((IkReal(0.0750000000000000))*(sj1));
1528 IkReal x442=((IkReal(0.00600000000000000))*(sj1));
1529 IkReal x443=((IkReal(0.00600000000000000))*(cj1));
1530 IkReal x444=((IkReal(0.0800000000000000))*(sj1)*(sj3));
1531 IkReal x445=((IkReal(0.0800000000000000))*(cj3)*(sj1));
1532 if( IKabs(((gconst4)*(((IkReal(-0.0982500000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x442)))+(((pz)*(x439)))+(((x437)*(x440)))+(((x437)*(x445)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((pz)*(x441)))+(((x436)*(x440)))+(((IkReal(-1.00000000000000))*(x436)*(x438)))+(((IkReal(-1.00000000000000))*(x437)*(x438)))+(((cj3)*(pz)*(x435)))+(((IkReal(-1.00000000000000))*(sj3)*(x435)*(x436)))+(((IkReal(-1.00000000000000))*(sj3)*(x435)*(x437)))+(((IkReal(0.00562500000000000))*(cj1)))+(((pz)*(x444)))+(((x436)*(x445)))+(((sj3)*(x443))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((pz)*(sj3)*(x435)))+(((x436)*(x441)))+(((x436)*(x439)))+(((cj3)*(x435)*(x437)))+(((x436)*(x444)))+(((IkReal(-1.00000000000000))*(pz)*(x440)))+(((IkReal(-1.00000000000000))*(pz)*(x445)))+(((IkReal(-1.00000000000000))*(cj3)*(x443)))+(((cj3)*(x435)*(x436)))+(((x437)*(x444)))+(((IkReal(-1.00000000000000))*(sj3)*(x442)))+(((x437)*(x439)))+(((pz)*(x438)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x437)*(x441))))))) < IKFAST_ATAN2_MAGTHRESH )
1533  continue;
1534 j2array[0]=IKatan2(((gconst4)*(((IkReal(-0.0982500000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x442)))+(((pz)*(x439)))+(((x437)*(x440)))+(((x437)*(x445)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((pz)*(x441)))+(((x436)*(x440)))+(((IkReal(-1.00000000000000))*(x436)*(x438)))+(((IkReal(-1.00000000000000))*(x437)*(x438)))+(((cj3)*(pz)*(x435)))+(((IkReal(-1.00000000000000))*(sj3)*(x435)*(x436)))+(((IkReal(-1.00000000000000))*(sj3)*(x435)*(x437)))+(((IkReal(0.00562500000000000))*(cj1)))+(((pz)*(x444)))+(((x436)*(x445)))+(((sj3)*(x443)))))), ((gconst4)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((pz)*(sj3)*(x435)))+(((x436)*(x441)))+(((x436)*(x439)))+(((cj3)*(x435)*(x437)))+(((x436)*(x444)))+(((IkReal(-1.00000000000000))*(pz)*(x440)))+(((IkReal(-1.00000000000000))*(pz)*(x445)))+(((IkReal(-1.00000000000000))*(cj3)*(x443)))+(((cj3)*(x435)*(x436)))+(((x437)*(x444)))+(((IkReal(-1.00000000000000))*(sj3)*(x442)))+(((x437)*(x439)))+(((pz)*(x438)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x437)*(x441)))))));
1535 sj2array[0]=IKsin(j2array[0]);
1536 cj2array[0]=IKcos(j2array[0]);
1537 if( j2array[0] > IKPI )
1538 {
1539  j2array[0]-=IK2PI;
1540 }
1541 else if( j2array[0] < -IKPI )
1542 { j2array[0]+=IK2PI;
1543 }
1544 j2valid[0] = true;
1545 for(int ij2 = 0; ij2 < 1; ++ij2)
1546 {
1547 if( !j2valid[ij2] )
1548 {
1549  continue;
1550 }
1551 _ij2[0] = ij2; _ij2[1] = -1;
1552 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
1553 {
1554 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
1555 {
1556  j2valid[iij2]=false; _ij2[1] = iij2; break;
1557 }
1558 }
1559 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
1560 {
1561 IkReal evalcond[4];
1562 IkReal x446=IKcos(j2);
1563 IkReal x447=IKsin(j2);
1564 IkReal x448=((IkReal(0.0800000000000000))*(sj3));
1565 IkReal x449=((cj0)*(r01));
1566 IkReal x450=((IkReal(1.00000000000000))*(px));
1567 IkReal x451=((py)*(sj1));
1568 IkReal x452=((cj0)*(r02));
1569 IkReal x453=((IkReal(0.0750000000000000))*(cj1));
1570 IkReal x454=((r02)*(sj0));
1571 IkReal x455=((IkReal(0.0750000000000000))*(sj1));
1572 IkReal x456=((cj1)*(pz));
1573 IkReal x457=((r00)*(sj0));
1574 IkReal x458=((IkReal(0.0800000000000000))*(cj3));
1575 IkReal x459=((pz)*(sj1));
1576 IkReal x460=((IkReal(1.00000000000000))*(sj0));
1577 IkReal x461=((cj1)*(py));
1578 IkReal x462=((IkReal(0.0750000000000000))*(x447));
1579 IkReal x463=((IkReal(0.327500000000000))*(x446));
1580 IkReal x464=((IkReal(0.0750000000000000))*(x446));
1581 IkReal x465=((IkReal(0.327500000000000))*(x447));
1582 IkReal x466=((sj1)*(x457));
1583 IkReal x467=((x447)*(x448));
1584 IkReal x468=((x446)*(x458));
1585 IkReal x469=((x446)*(x448));
1586 IkReal x470=((x447)*(x458));
1587 IkReal x471=((x462)+(x467));
1588 IkReal x472=((x468)+(x463));
1589 IkReal x473=((x470)+(x469)+(x465)+(x464));
1590 evalcond[0]=((((IkReal(-1.00000000000000))*(x460)*(x461)))+(x472)+(x459)+(x453)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x450)))+(((IkReal(-1.00000000000000))*(x471))));
1591 evalcond[1]=((IkReal(0.300000000000000))+(x473)+(((IkReal(-1.00000000000000))*(x456)))+(x455)+(((IkReal(-1.00000000000000))*(x451)*(x460)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x450))));
1592 evalcond[2]=((x471)+(((IkReal(-1.00000000000000))*(x449)*(x453)))+(((IkReal(-1.00000000000000))*(sj1)*(x450)*(x454)))+(((IkReal(-1.00000000000000))*(x449)*(x459)))+(((cj1)*(px)*(r01)))+(((x457)*(x459)))+(((x453)*(x457)))+(((x451)*(x452)))+(((IkReal(-1.00000000000000))*(r00)*(x461)))+(((IkReal(-1.00000000000000))*(x472))));
1593 evalcond[3]=((((x452)*(x461)))+(x473)+(((x456)*(x457)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x450)))+(((IkReal(-0.300000000000000))*(x457)))+(((IkReal(-1.00000000000000))*(cj1)*(x450)*(x454)))+(((r00)*(x451)))+(((x449)*(x455)))+(((IkReal(-1.00000000000000))*(x455)*(x457)))+(((IkReal(-1.00000000000000))*(x449)*(x456)))+(((IkReal(0.300000000000000))*(x449))));
1594 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1595 {
1596 continue;
1597 }
1598 }
1599 
1600 {
1601 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1602 vinfos[0].jointtype = 1;
1603 vinfos[0].foffset = j0;
1604 vinfos[0].indices[0] = _ij0[0];
1605 vinfos[0].indices[1] = _ij0[1];
1606 vinfos[0].maxsolutions = _nj0;
1607 vinfos[1].jointtype = 1;
1608 vinfos[1].foffset = j1;
1609 vinfos[1].indices[0] = _ij1[0];
1610 vinfos[1].indices[1] = _ij1[1];
1611 vinfos[1].maxsolutions = _nj1;
1612 vinfos[2].jointtype = 1;
1613 vinfos[2].foffset = j2;
1614 vinfos[2].indices[0] = _ij2[0];
1615 vinfos[2].indices[1] = _ij2[1];
1616 vinfos[2].maxsolutions = _nj2;
1617 vinfos[3].jointtype = 1;
1618 vinfos[3].foffset = j3;
1619 vinfos[3].indices[0] = _ij3[0];
1620 vinfos[3].indices[1] = _ij3[1];
1621 vinfos[3].maxsolutions = _nj3;
1622 vinfos[4].jointtype = 1;
1623 vinfos[4].foffset = j4;
1624 vinfos[4].indices[0] = _ij4[0];
1625 vinfos[4].indices[1] = _ij4[1];
1626 vinfos[4].maxsolutions = _nj4;
1627 std::vector<int> vfree(0);
1628 solutions.AddSolution(vinfos,vfree);
1629 }
1630 }
1631 }
1632 
1633 }
1634 
1635 }
1636 
1637 } else
1638 {
1639 IkReal x474=((IkReal(0.0524000000000000))*(cj3));
1640 IkReal x475=((IkReal(0.0120000000000000))*(sj3));
1641 IkReal x476=(py)*(py);
1642 IkReal x477=(px)*(px);
1643 IkReal x478=(pz)*(pz);
1644 IkReal x479=((r01)*(sj0));
1645 IkReal x480=((py)*(r00));
1646 IkReal x481=((pz)*(sj1));
1647 IkReal x482=((py)*(r01));
1648 IkReal x483=((px)*(sj0));
1649 IkReal x484=((IkReal(0.600000000000000))*(r02));
1650 IkReal x485=((IkReal(0.150000000000000))*(cj1));
1651 IkReal x486=((cj0)*(sj1));
1652 IkReal x487=((IkReal(0.150000000000000))*(px));
1653 IkReal x488=((IkReal(2.00000000000000))*(cj1));
1654 IkReal x489=((cj0)*(r01));
1655 IkReal x490=((r02)*(sj1));
1656 IkReal x491=((px)*(r00));
1657 IkReal x492=((IkReal(0.300000000000000))*(r00));
1658 IkReal x493=((IkReal(1.00000000000000))*(pz));
1659 IkReal x494=((r00)*(sj1));
1660 IkReal x495=((cj0)*(r00));
1661 IkReal x496=((cj0)*(cj1));
1662 IkReal x497=((IkReal(1.00000000000000))*(sj1));
1663 IkReal x498=((IkReal(0.0956250000000000))*(r00));
1664 IkReal x499=((IkReal(0.600000000000000))*(pz));
1665 IkReal x500=((IkReal(0.600000000000000))*(sj1));
1666 IkReal x501=((IkReal(2.00000000000000))*(px));
1667 IkReal x502=((IkReal(2.00000000000000))*(sj1));
1668 IkReal x503=((IkReal(0.150000000000000))*(sj1));
1669 IkReal x504=((cj1)*(r02));
1670 IkReal x505=((cj0)*(px));
1671 IkReal x506=((IkReal(0.0843750000000000))*(cj1));
1672 IkReal x507=((py)*(sj0));
1673 IkReal x508=((pz)*(r02));
1674 IkReal x509=((IkReal(1.00000000000000))*(cj1));
1675 IkReal x510=((cj0)*(py));
1676 IkReal x511=((r00)*(sj0));
1677 IkReal x512=((r02)*(x507));
1678 IkReal x513=((pp)*(x509));
1679 IkReal x514=((IkReal(1.00000000000000))*(pp)*(r00));
1680 IkReal x515=((x474)+(x475));
1681 IkReal x516=((IkReal(2.00000000000000))*(r00)*(x477));
1682 IkReal x517=((cj0)*(x501)*(x508));
1683 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
1684 evalcond[1]=((x510)+(((IkReal(-1.00000000000000))*(x483))));
1685 evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x511)))+(x489));
1686 evalcond[3]=((((IkReal(-1.00000000000000))*(x495)*(x509)))+(((IkReal(-1.00000000000000))*(x479)*(x509)))+(x490));
1687 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x486)))+(((IkReal(-1.00000000000000))*(x504)))+(((IkReal(-1.00000000000000))*(x479)*(x497))));
1688 evalcond[5]=((IkReal(0.0236562500000000))+(((x500)*(x507)))+(((IkReal(0.600000000000000))*(px)*(x486)))+(x515)+(((cj1)*(x499)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(0.150000000000000))*(x507)))+(((cj0)*(x487)))+(((IkReal(-0.0450000000000000))*(sj1))));
1689 evalcond[6]=((((IkReal(0.300000000000000))*(sj1)*(x479)))+(((IkReal(0.0750000000000000))*(x495)))+(((IkReal(-1.00000000000000))*(r02)*(x493)))+(((x486)*(x492)))+(((IkReal(-1.00000000000000))*(x482)))+(((IkReal(0.300000000000000))*(x504)))+(((IkReal(0.0750000000000000))*(x479)))+(((IkReal(-1.00000000000000))*(x491))));
1690 evalcond[7]=((x512)+(((IkReal(-1.00000000000000))*(x479)*(x493)))+(((x492)*(x496)))+(((r02)*(x505)))+(((IkReal(-0.300000000000000))*(x490)))+(((IkReal(-1.00000000000000))*(x493)*(x495)))+(((IkReal(-0.0750000000000000))*(r02)))+(((IkReal(0.300000000000000))*(cj1)*(x479))));
1691 evalcond[8]=((IkReal(0.119281250000000))+(((x480)*(x500)))+(((pp)*(x489)))+(((IkReal(-1.00000000000000))*(pp)*(x511)))+(((IkReal(2.00000000000000))*(x477)*(x511)))+(x515)+(((IkReal(-1.00000000000000))*(cj1)*(x489)*(x499)))+(((py)*(x484)*(x496)))+(((py)*(x479)*(x501)))+(((IkReal(2.00000000000000))*(x483)*(x508)))+(((IkReal(-1.00000000000000))*(cj0)*(x480)*(x501)))+(((IkReal(0.0956250000000000))*(x489)))+(((IkReal(0.0450000000000000))*(r01)*(x486)))+(((IkReal(-1.00000000000000))*(r01)*(x487)))+(((IkReal(-1.00000000000000))*(cj1)*(x483)*(x484)))+(((IkReal(-0.0450000000000000))*(sj0)*(x494)))+(((IkReal(-2.00000000000000))*(x476)*(x489)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x500)))+(((IkReal(0.150000000000000))*(x480)))+(((IkReal(-1.00000000000000))*(sj0)*(x498)))+(((cj1)*(x499)*(x511)))+(((IkReal(-2.00000000000000))*(x508)*(x510))));
1692 evalcond[9]=((((IkReal(-0.150000000000000))*(r02)*(x481)))+(((x486)*(x516)))+(((r02)*(x485)*(x505)))+(((IkReal(-1.00000000000000))*(pz)*(x485)*(x495)))+(((x485)*(x512)))+(((pz)*(x488)*(x491)))+(((x480)*(x483)*(x502)))+(((IkReal(2.00000000000000))*(x481)*(x512)))+(((x486)*(x498)))+(((IkReal(-0.600000000000000))*(x482)))+(((IkReal(-1.00000000000000))*(pz)*(x484)))+(((r02)*(x478)*(x488)))+(((x482)*(x486)*(x501)))+(((IkReal(-1.00000000000000))*(pz)*(x479)*(x485)))+(((IkReal(-1.00000000000000))*(pp)*(x504)))+(((IkReal(0.0450000000000000))*(x479)))+(((IkReal(-1.00000000000000))*(x486)*(x514)))+(((x476)*(x479)*(x502)))+(((IkReal(0.0956250000000000))*(sj1)*(x479)))+(((cj0)*(r02)*(x481)*(x501)))+(((IkReal(-0.600000000000000))*(x491)))+(((IkReal(0.0450000000000000))*(x495)))+(((pz)*(x482)*(x488)))+(((IkReal(-1.00000000000000))*(x482)*(x503)))+(((IkReal(0.0843750000000000))*(x504)))+(((IkReal(-1.00000000000000))*(pp)*(x479)*(x497)))+(((IkReal(-1.00000000000000))*(x487)*(x494))));
1693 evalcond[10]=((((x477)*(x488)*(x495)))+(((IkReal(-1.00000000000000))*(x485)*(x508)))+(((x476)*(x479)*(x488)))+(((x495)*(x499)))+(((x480)*(x483)*(x488)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(0.150000000000000))*(x481)*(x495)))+(((IkReal(-1.00000000000000))*(x479)*(x513)))+(((IkReal(-2.00000000000000))*(x478)*(x490)))+(((IkReal(-1.00000000000000))*(x479)*(x506)))+(((IkReal(-1.00000000000000))*(x484)*(x507)))+(((IkReal(-1.00000000000000))*(x484)*(x505)))+(((IkReal(0.0956250000000000))*(x490)))+(((IkReal(0.150000000000000))*(x479)*(x481)))+(((IkReal(-1.00000000000000))*(x485)*(x491)))+(((IkReal(-0.150000000000000))*(x490)*(x507)))+(((pp)*(x490)))+(((IkReal(-1.00000000000000))*(r02)*(x486)*(x487)))+(((IkReal(-2.00000000000000))*(x481)*(x482)))+(((x479)*(x499)))+(((x482)*(x488)*(x505)))+(((IkReal(-2.00000000000000))*(x481)*(x491)))+(((IkReal(-1.00000000000000))*(x495)*(x506)))+(((x488)*(x507)*(x508)))+(((IkReal(-1.00000000000000))*(x482)*(x485)))+(((IkReal(-1.00000000000000))*(x495)*(x513)))+(((x488)*(x505)*(x508))));
1694 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 )
1695 {
1696 {
1697 IkReal dummyeval[1];
1698 IkReal gconst6;
1699 gconst6=IKsign(((IkReal(0.112881250000000))+(((IkReal(0.0524000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
1700 dummyeval[0]=((IkReal(17.6376953125000))+(((IkReal(8.18750000000000))*(cj3)))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3)));
1701 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1702 {
1703 {
1704 IkReal dummyeval[1];
1705 IkReal gconst7;
1706 gconst7=IKsign(((IkReal(-0.112881250000000))+(((IkReal(-0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(-0.0120000000000000))*(sj3)))+(((IkReal(-0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(-0.0524000000000000))*(cj3)))));
1707 dummyeval[0]=((IkReal(-17.6376953125000))+(((IkReal(-1.87500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-8.18750000000000))*(cj3)))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
1708 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1709 {
1710 continue;
1711 
1712 } else
1713 {
1714 {
1715 IkReal j2array[1], cj2array[1], sj2array[1];
1716 bool j2valid[1]={false};
1717 _nj2 = 1;
1718 IkReal x518=((cj0)*(r01));
1719 IkReal x519=((pz)*(sj1));
1720 IkReal x520=((IkReal(0.0800000000000000))*(sj3));
1721 IkReal x521=((IkReal(0.00600000000000000))*(cj1));
1722 IkReal x522=((IkReal(0.0750000000000000))*(cj1));
1723 IkReal x523=((r00)*(sj0));
1724 IkReal x524=((IkReal(0.0245625000000000))*(cj1));
1725 IkReal x525=((px)*(r01));
1726 IkReal x526=((IkReal(0.00562500000000000))*(cj1));
1727 IkReal x527=((IkReal(0.0800000000000000))*(cj3));
1728 IkReal x528=((cj1)*(pz));
1729 IkReal x529=((px)*(r02));
1730 IkReal x530=((py)*(sj1));
1731 IkReal x531=((cj0)*(r02));
1732 IkReal x532=((IkReal(0.00600000000000000))*(sj1));
1733 IkReal x533=((IkReal(0.327500000000000))*(sj0));
1734 IkReal x534=((IkReal(0.0750000000000000))*(sj0)*(sj1));
1735 IkReal x535=((cj0)*(px)*(sj1));
1736 IkReal x536=((cj1)*(py)*(r00));
1737 IkReal x537=((IkReal(0.0800000000000000))*(sj0)*(sj1)*(x529));
1738 if( IKabs(((gconst7)*(((IkReal(0.0982500000000000))+(((IkReal(-1.00000000000000))*(sj0)*(x527)*(x530)))+(((sj0)*(sj1)*(x520)*(x529)))+(((IkReal(-0.327500000000000))*(x528)))+(((IkReal(-1.00000000000000))*(x522)*(x525)))+(((IkReal(-1.00000000000000))*(x527)*(x528)))+(((IkReal(-1.00000000000000))*(x520)*(x530)*(x531)))+(((IkReal(-1.00000000000000))*(x523)*(x526)))+(((IkReal(-1.00000000000000))*(x527)*(x535)))+(((IkReal(-0.327500000000000))*(x535)))+(((IkReal(-0.0750000000000000))*(x519)*(x523)))+(((IkReal(-0.0750000000000000))*(x530)*(x531)))+(((IkReal(-1.00000000000000))*(cj1)*(x520)*(x525)))+(((IkReal(-1.00000000000000))*(x519)*(x520)*(x523)))+(((IkReal(0.0245625000000000))*(sj1)))+(((py)*(r00)*(x522)))+(((sj3)*(x518)*(x521)))+(((IkReal(-1.00000000000000))*(x530)*(x533)))+(((x529)*(x534)))+(((IkReal(-1.00000000000000))*(sj3)*(x521)*(x523)))+(((cj3)*(x532)))+(((x518)*(x519)*(x520)))+(((x518)*(x526)))+(((IkReal(0.0240000000000000))*(cj3)))+(((x520)*(x536)))+(((IkReal(0.0750000000000000))*(x518)*(x519))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((IkReal(0.0225000000000000))+(((IkReal(-0.0750000000000000))*(sj0)*(x530)))+(((x527)*(x530)*(x531)))+(((IkReal(-1.00000000000000))*(sj1)*(x529)*(x533)))+(((IkReal(0.00562500000000000))*(sj1)))+(((IkReal(0.327500000000000))*(x519)*(x523)))+(((IkReal(-1.00000000000000))*(sj0)*(x520)*(x530)))+(((IkReal(-1.00000000000000))*(x520)*(x528)))+(((IkReal(-0.327500000000000))*(x536)))+(((cj1)*(x525)*(x527)))+(((IkReal(-1.00000000000000))*(pz)*(x522)))+(((IkReal(0.327500000000000))*(cj1)*(x525)))+(((x523)*(x524)))+(((IkReal(0.327500000000000))*(x530)*(x531)))+(((IkReal(-1.00000000000000))*(sj0)*(sj1)*(x527)*(x529)))+(((cj3)*(x521)*(x523)))+(((IkReal(-0.0750000000000000))*(x535)))+(((IkReal(-1.00000000000000))*(x527)*(x536)))+(((IkReal(-1.00000000000000))*(cj3)*(x518)*(x521)))+(((IkReal(-1.00000000000000))*(x518)*(x519)*(x527)))+(((sj3)*(x532)))+(((IkReal(-1.00000000000000))*(x518)*(x524)))+(((IkReal(-0.327500000000000))*(x518)*(x519)))+(((IkReal(-1.00000000000000))*(x520)*(x535)))+(((IkReal(0.0240000000000000))*(sj3)))+(((x519)*(x523)*(x527))))))) < IKFAST_ATAN2_MAGTHRESH )
1739  continue;
1740 j2array[0]=IKatan2(((gconst7)*(((IkReal(0.0982500000000000))+(((IkReal(-1.00000000000000))*(sj0)*(x527)*(x530)))+(((sj0)*(sj1)*(x520)*(x529)))+(((IkReal(-0.327500000000000))*(x528)))+(((IkReal(-1.00000000000000))*(x522)*(x525)))+(((IkReal(-1.00000000000000))*(x527)*(x528)))+(((IkReal(-1.00000000000000))*(x520)*(x530)*(x531)))+(((IkReal(-1.00000000000000))*(x523)*(x526)))+(((IkReal(-1.00000000000000))*(x527)*(x535)))+(((IkReal(-0.327500000000000))*(x535)))+(((IkReal(-0.0750000000000000))*(x519)*(x523)))+(((IkReal(-0.0750000000000000))*(x530)*(x531)))+(((IkReal(-1.00000000000000))*(cj1)*(x520)*(x525)))+(((IkReal(-1.00000000000000))*(x519)*(x520)*(x523)))+(((IkReal(0.0245625000000000))*(sj1)))+(((py)*(r00)*(x522)))+(((sj3)*(x518)*(x521)))+(((IkReal(-1.00000000000000))*(x530)*(x533)))+(((x529)*(x534)))+(((IkReal(-1.00000000000000))*(sj3)*(x521)*(x523)))+(((cj3)*(x532)))+(((x518)*(x519)*(x520)))+(((x518)*(x526)))+(((IkReal(0.0240000000000000))*(cj3)))+(((x520)*(x536)))+(((IkReal(0.0750000000000000))*(x518)*(x519)))))), ((gconst7)*(((IkReal(0.0225000000000000))+(((IkReal(-0.0750000000000000))*(sj0)*(x530)))+(((x527)*(x530)*(x531)))+(((IkReal(-1.00000000000000))*(sj1)*(x529)*(x533)))+(((IkReal(0.00562500000000000))*(sj1)))+(((IkReal(0.327500000000000))*(x519)*(x523)))+(((IkReal(-1.00000000000000))*(sj0)*(x520)*(x530)))+(((IkReal(-1.00000000000000))*(x520)*(x528)))+(((IkReal(-0.327500000000000))*(x536)))+(((cj1)*(x525)*(x527)))+(((IkReal(-1.00000000000000))*(pz)*(x522)))+(((IkReal(0.327500000000000))*(cj1)*(x525)))+(((x523)*(x524)))+(((IkReal(0.327500000000000))*(x530)*(x531)))+(((IkReal(-1.00000000000000))*(sj0)*(sj1)*(x527)*(x529)))+(((cj3)*(x521)*(x523)))+(((IkReal(-0.0750000000000000))*(x535)))+(((IkReal(-1.00000000000000))*(x527)*(x536)))+(((IkReal(-1.00000000000000))*(cj3)*(x518)*(x521)))+(((IkReal(-1.00000000000000))*(x518)*(x519)*(x527)))+(((sj3)*(x532)))+(((IkReal(-1.00000000000000))*(x518)*(x524)))+(((IkReal(-0.327500000000000))*(x518)*(x519)))+(((IkReal(-1.00000000000000))*(x520)*(x535)))+(((IkReal(0.0240000000000000))*(sj3)))+(((x519)*(x523)*(x527)))))));
1741 sj2array[0]=IKsin(j2array[0]);
1742 cj2array[0]=IKcos(j2array[0]);
1743 if( j2array[0] > IKPI )
1744 {
1745  j2array[0]-=IK2PI;
1746 }
1747 else if( j2array[0] < -IKPI )
1748 { j2array[0]+=IK2PI;
1749 }
1750 j2valid[0] = true;
1751 for(int ij2 = 0; ij2 < 1; ++ij2)
1752 {
1753 if( !j2valid[ij2] )
1754 {
1755  continue;
1756 }
1757 _ij2[0] = ij2; _ij2[1] = -1;
1758 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
1759 {
1760 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
1761 {
1762  j2valid[iij2]=false; _ij2[1] = iij2; break;
1763 }
1764 }
1765 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
1766 {
1767 IkReal evalcond[4];
1768 IkReal x538=IKcos(j2);
1769 IkReal x539=IKsin(j2);
1770 IkReal x540=((IkReal(0.0800000000000000))*(sj3));
1771 IkReal x541=((cj0)*(r01));
1772 IkReal x542=((IkReal(1.00000000000000))*(px));
1773 IkReal x543=((py)*(sj1));
1774 IkReal x544=((cj0)*(r02));
1775 IkReal x545=((IkReal(0.0750000000000000))*(cj1));
1776 IkReal x546=((r02)*(sj0));
1777 IkReal x547=((IkReal(0.0750000000000000))*(sj1));
1778 IkReal x548=((cj1)*(pz));
1779 IkReal x549=((r00)*(sj0));
1780 IkReal x550=((IkReal(0.0800000000000000))*(cj3));
1781 IkReal x551=((pz)*(sj1));
1782 IkReal x552=((IkReal(1.00000000000000))*(sj0));
1783 IkReal x553=((cj1)*(py));
1784 IkReal x554=((IkReal(0.327500000000000))*(x538));
1785 IkReal x555=((IkReal(0.0750000000000000))*(x539));
1786 IkReal x556=((IkReal(0.0750000000000000))*(x538));
1787 IkReal x557=((IkReal(0.327500000000000))*(x539));
1788 IkReal x558=((sj1)*(x549));
1789 IkReal x559=((x538)*(x550));
1790 IkReal x560=((x539)*(x540));
1791 IkReal x561=((x538)*(x540));
1792 IkReal x562=((x539)*(x550));
1793 IkReal x563=((x560)+(x555));
1794 IkReal x564=((x559)+(x554));
1795 IkReal x565=((x562)+(x561)+(x557)+(x556));
1796 evalcond[0]=((((IkReal(-1.00000000000000))*(x552)*(x553)))+(x545)+(x564)+(((IkReal(-1.00000000000000))*(x563)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x542)))+(x551));
1797 evalcond[1]=((IkReal(0.300000000000000))+(x547)+(((IkReal(-1.00000000000000))*(x543)*(x552)))+(x565)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x542)))+(((IkReal(-1.00000000000000))*(x548))));
1798 evalcond[2]=((((x549)*(x551)))+(((x545)*(x549)))+(x564)+(((IkReal(-1.00000000000000))*(x541)*(x545)))+(((IkReal(-1.00000000000000))*(x563)))+(((cj1)*(px)*(r01)))+(((x543)*(x544)))+(((IkReal(-1.00000000000000))*(sj1)*(x542)*(x546)))+(((IkReal(-1.00000000000000))*(r00)*(x553)))+(((IkReal(-1.00000000000000))*(x541)*(x551))));
1799 evalcond[3]=((((IkReal(0.300000000000000))*(x541)))+(((IkReal(-1.00000000000000))*(x547)*(x549)))+(((IkReal(-0.300000000000000))*(x549)))+(((x548)*(x549)))+(((IkReal(-1.00000000000000))*(x541)*(x548)))+(((IkReal(-1.00000000000000))*(x565)))+(((IkReal(-1.00000000000000))*(cj1)*(x542)*(x546)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x542)))+(((r00)*(x543)))+(((x541)*(x547)))+(((x544)*(x553))));
1800 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1801 {
1802 continue;
1803 }
1804 }
1805 
1806 {
1807 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1808 vinfos[0].jointtype = 1;
1809 vinfos[0].foffset = j0;
1810 vinfos[0].indices[0] = _ij0[0];
1811 vinfos[0].indices[1] = _ij0[1];
1812 vinfos[0].maxsolutions = _nj0;
1813 vinfos[1].jointtype = 1;
1814 vinfos[1].foffset = j1;
1815 vinfos[1].indices[0] = _ij1[0];
1816 vinfos[1].indices[1] = _ij1[1];
1817 vinfos[1].maxsolutions = _nj1;
1818 vinfos[2].jointtype = 1;
1819 vinfos[2].foffset = j2;
1820 vinfos[2].indices[0] = _ij2[0];
1821 vinfos[2].indices[1] = _ij2[1];
1822 vinfos[2].maxsolutions = _nj2;
1823 vinfos[3].jointtype = 1;
1824 vinfos[3].foffset = j3;
1825 vinfos[3].indices[0] = _ij3[0];
1826 vinfos[3].indices[1] = _ij3[1];
1827 vinfos[3].maxsolutions = _nj3;
1828 vinfos[4].jointtype = 1;
1829 vinfos[4].foffset = j4;
1830 vinfos[4].indices[0] = _ij4[0];
1831 vinfos[4].indices[1] = _ij4[1];
1832 vinfos[4].maxsolutions = _nj4;
1833 std::vector<int> vfree(0);
1834 solutions.AddSolution(vinfos,vfree);
1835 }
1836 }
1837 }
1838 
1839 }
1840 
1841 }
1842 
1843 } else
1844 {
1845 {
1846 IkReal j2array[1], cj2array[1], sj2array[1];
1847 bool j2valid[1]={false};
1848 _nj2 = 1;
1849 IkReal x566=((IkReal(0.0800000000000000))*(cj1));
1850 IkReal x567=((cj0)*(px));
1851 IkReal x568=((py)*(sj0));
1852 IkReal x569=((IkReal(0.0750000000000000))*(cj1));
1853 IkReal x570=((IkReal(0.327500000000000))*(cj1));
1854 IkReal x571=((IkReal(0.327500000000000))*(sj1));
1855 IkReal x572=((IkReal(0.0750000000000000))*(sj1));
1856 IkReal x573=((IkReal(0.00600000000000000))*(sj1));
1857 IkReal x574=((IkReal(0.00600000000000000))*(cj1));
1858 IkReal x575=((IkReal(0.0800000000000000))*(sj1)*(sj3));
1859 IkReal x576=((IkReal(0.0800000000000000))*(cj3)*(sj1));
1860 if( IKabs(((gconst6)*(((IkReal(-0.0982500000000000))+(((x568)*(x576)))+(((IkReal(-1.00000000000000))*(x568)*(x569)))+(((IkReal(-1.00000000000000))*(cj3)*(x573)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((x567)*(x576)))+(((pz)*(x575)))+(((pz)*(x570)))+(((sj3)*(x574)))+(((x567)*(x571)))+(((IkReal(-1.00000000000000))*(sj3)*(x566)*(x568)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x567)*(x569)))+(((x568)*(x571)))+(((cj3)*(pz)*(x566)))+(((pz)*(x572)))+(((IkReal(-1.00000000000000))*(sj3)*(x566)*(x567))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((x568)*(x572)))+(((IkReal(-1.00000000000000))*(pz)*(x571)))+(((x567)*(x572)))+(((IkReal(-1.00000000000000))*(sj3)*(x573)))+(((pz)*(x569)))+(((pz)*(sj3)*(x566)))+(((IkReal(-1.00000000000000))*(pz)*(x576)))+(((x568)*(x570)))+(((x568)*(x575)))+(((x567)*(x570)))+(((cj3)*(x566)*(x567)))+(((IkReal(-1.00000000000000))*(cj3)*(x574)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x567)*(x575)))+(((cj3)*(x566)*(x568))))))) < IKFAST_ATAN2_MAGTHRESH )
1861  continue;
1862 j2array[0]=IKatan2(((gconst6)*(((IkReal(-0.0982500000000000))+(((x568)*(x576)))+(((IkReal(-1.00000000000000))*(x568)*(x569)))+(((IkReal(-1.00000000000000))*(cj3)*(x573)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((x567)*(x576)))+(((pz)*(x575)))+(((pz)*(x570)))+(((sj3)*(x574)))+(((x567)*(x571)))+(((IkReal(-1.00000000000000))*(sj3)*(x566)*(x568)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x567)*(x569)))+(((x568)*(x571)))+(((cj3)*(pz)*(x566)))+(((pz)*(x572)))+(((IkReal(-1.00000000000000))*(sj3)*(x566)*(x567)))))), ((gconst6)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((x568)*(x572)))+(((IkReal(-1.00000000000000))*(pz)*(x571)))+(((x567)*(x572)))+(((IkReal(-1.00000000000000))*(sj3)*(x573)))+(((pz)*(x569)))+(((pz)*(sj3)*(x566)))+(((IkReal(-1.00000000000000))*(pz)*(x576)))+(((x568)*(x570)))+(((x568)*(x575)))+(((x567)*(x570)))+(((cj3)*(x566)*(x567)))+(((IkReal(-1.00000000000000))*(cj3)*(x574)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x567)*(x575)))+(((cj3)*(x566)*(x568)))))));
1863 sj2array[0]=IKsin(j2array[0]);
1864 cj2array[0]=IKcos(j2array[0]);
1865 if( j2array[0] > IKPI )
1866 {
1867  j2array[0]-=IK2PI;
1868 }
1869 else if( j2array[0] < -IKPI )
1870 { j2array[0]+=IK2PI;
1871 }
1872 j2valid[0] = true;
1873 for(int ij2 = 0; ij2 < 1; ++ij2)
1874 {
1875 if( !j2valid[ij2] )
1876 {
1877  continue;
1878 }
1879 _ij2[0] = ij2; _ij2[1] = -1;
1880 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
1881 {
1882 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
1883 {
1884  j2valid[iij2]=false; _ij2[1] = iij2; break;
1885 }
1886 }
1887 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
1888 {
1889 IkReal evalcond[4];
1890 IkReal x577=IKcos(j2);
1891 IkReal x578=IKsin(j2);
1892 IkReal x579=((IkReal(0.0800000000000000))*(sj3));
1893 IkReal x580=((cj0)*(r01));
1894 IkReal x581=((IkReal(1.00000000000000))*(px));
1895 IkReal x582=((py)*(sj1));
1896 IkReal x583=((cj0)*(r02));
1897 IkReal x584=((IkReal(0.0750000000000000))*(cj1));
1898 IkReal x585=((r02)*(sj0));
1899 IkReal x586=((IkReal(0.0750000000000000))*(sj1));
1900 IkReal x587=((cj1)*(pz));
1901 IkReal x588=((r00)*(sj0));
1902 IkReal x589=((IkReal(0.0800000000000000))*(cj3));
1903 IkReal x590=((pz)*(sj1));
1904 IkReal x591=((IkReal(1.00000000000000))*(sj0));
1905 IkReal x592=((cj1)*(py));
1906 IkReal x593=((IkReal(0.327500000000000))*(x577));
1907 IkReal x594=((IkReal(0.0750000000000000))*(x578));
1908 IkReal x595=((IkReal(0.0750000000000000))*(x577));
1909 IkReal x596=((IkReal(0.327500000000000))*(x578));
1910 IkReal x597=((sj1)*(x588));
1911 IkReal x598=((x577)*(x589));
1912 IkReal x599=((x578)*(x579));
1913 IkReal x600=((x577)*(x579));
1914 IkReal x601=((x578)*(x589));
1915 IkReal x602=((x594)+(x599));
1916 IkReal x603=((x593)+(x598));
1917 IkReal x604=((x596)+(x595)+(x601)+(x600));
1918 evalcond[0]=((((IkReal(-1.00000000000000))*(x591)*(x592)))+(((IkReal(-1.00000000000000))*(x602)))+(x590)+(x584)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x581)))+(x603));
1919 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x582)*(x591)))+(((IkReal(-1.00000000000000))*(x587)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x581)))+(x586)+(x604));
1920 evalcond[2]=((((IkReal(-1.00000000000000))*(sj1)*(x581)*(x585)))+(((IkReal(-1.00000000000000))*(r00)*(x592)))+(((x584)*(x588)))+(((x588)*(x590)))+(((IkReal(-1.00000000000000))*(x580)*(x584)))+(((cj1)*(px)*(r01)))+(((x582)*(x583)))+(((IkReal(-1.00000000000000))*(x602)))+(((IkReal(-1.00000000000000))*(x580)*(x590)))+(x603));
1921 evalcond[3]=((((IkReal(-1.00000000000000))*(x580)*(x587)))+(((IkReal(-0.300000000000000))*(x588)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x581)))+(((x583)*(x592)))+(((x587)*(x588)))+(((x580)*(x586)))+(((IkReal(-1.00000000000000))*(cj1)*(x581)*(x585)))+(((IkReal(-1.00000000000000))*(x604)))+(((r00)*(x582)))+(((IkReal(0.300000000000000))*(x580)))+(((IkReal(-1.00000000000000))*(x586)*(x588))));
1922 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1923 {
1924 continue;
1925 }
1926 }
1927 
1928 {
1929 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1930 vinfos[0].jointtype = 1;
1931 vinfos[0].foffset = j0;
1932 vinfos[0].indices[0] = _ij0[0];
1933 vinfos[0].indices[1] = _ij0[1];
1934 vinfos[0].maxsolutions = _nj0;
1935 vinfos[1].jointtype = 1;
1936 vinfos[1].foffset = j1;
1937 vinfos[1].indices[0] = _ij1[0];
1938 vinfos[1].indices[1] = _ij1[1];
1939 vinfos[1].maxsolutions = _nj1;
1940 vinfos[2].jointtype = 1;
1941 vinfos[2].foffset = j2;
1942 vinfos[2].indices[0] = _ij2[0];
1943 vinfos[2].indices[1] = _ij2[1];
1944 vinfos[2].maxsolutions = _nj2;
1945 vinfos[3].jointtype = 1;
1946 vinfos[3].foffset = j3;
1947 vinfos[3].indices[0] = _ij3[0];
1948 vinfos[3].indices[1] = _ij3[1];
1949 vinfos[3].maxsolutions = _nj3;
1950 vinfos[4].jointtype = 1;
1951 vinfos[4].foffset = j4;
1952 vinfos[4].indices[0] = _ij4[0];
1953 vinfos[4].indices[1] = _ij4[1];
1954 vinfos[4].maxsolutions = _nj4;
1955 std::vector<int> vfree(0);
1956 solutions.AddSolution(vinfos,vfree);
1957 }
1958 }
1959 }
1960 
1961 }
1962 
1963 }
1964 
1965 } else
1966 {
1967 if( 1 )
1968 {
1969 continue;
1970 
1971 } else
1972 {
1973 }
1974 }
1975 }
1976 }
1977 
1978 } else
1979 {
1980 {
1981 IkReal j2array[1], cj2array[1], sj2array[1];
1982 bool j2valid[1]={false};
1983 _nj2 = 1;
1984 IkReal x605=((r02)*(sj1));
1985 IkReal x606=((cj0)*(px));
1986 IkReal x607=((IkReal(0.0800000000000000))*(sj3));
1987 IkReal x608=((cj4)*(sj3));
1988 IkReal x609=((pz)*(sj1));
1989 IkReal x610=((IkReal(0.0800000000000000))*(cj3));
1990 IkReal x611=((IkReal(1.00000000000000))*(cj1));
1991 IkReal x612=((py)*(sj0));
1992 IkReal x613=((cj1)*(r01)*(sj0));
1993 IkReal x614=((cj1)*(cj3)*(cj4));
1994 IkReal x615=((cj0)*(cj1)*(r00));
1995 IkReal x616=((IkReal(0.0800000000000000))*(x615));
1996 if( IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(x610)*(x615)))+(((IkReal(-0.327500000000000))*(x615)))+(((x605)*(x610)))+(((x608)*(x609)))+(((IkReal(-0.327500000000000))*(x613)))+(((IkReal(-1.00000000000000))*(x610)*(x613)))+(((IkReal(-1.00000000000000))*(x606)*(x608)*(x611)))+(((IkReal(-1.00000000000000))*(x608)*(x611)*(x612)))+(((IkReal(0.327500000000000))*(x605)))+(((IkReal(0.0750000000000000))*(cj1)*(x608))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((IkReal(-0.0750000000000000))*(x615)))+(((IkReal(-1.00000000000000))*(x607)*(x613)))+(((x606)*(x614)))+(((x612)*(x614)))+(((x605)*(x607)))+(((IkReal(-0.0750000000000000))*(x614)))+(((IkReal(-1.00000000000000))*(x607)*(x615)))+(((IkReal(0.0750000000000000))*(x605)))+(((IkReal(-1.00000000000000))*(cj3)*(cj4)*(x609)))+(((IkReal(-0.0750000000000000))*(x613))))))) < IKFAST_ATAN2_MAGTHRESH )
1997  continue;
1998 j2array[0]=IKatan2(((gconst3)*(((((IkReal(-1.00000000000000))*(x610)*(x615)))+(((IkReal(-0.327500000000000))*(x615)))+(((x605)*(x610)))+(((x608)*(x609)))+(((IkReal(-0.327500000000000))*(x613)))+(((IkReal(-1.00000000000000))*(x610)*(x613)))+(((IkReal(-1.00000000000000))*(x606)*(x608)*(x611)))+(((IkReal(-1.00000000000000))*(x608)*(x611)*(x612)))+(((IkReal(0.327500000000000))*(x605)))+(((IkReal(0.0750000000000000))*(cj1)*(x608)))))), ((gconst3)*(((((IkReal(-0.0750000000000000))*(x615)))+(((IkReal(-1.00000000000000))*(x607)*(x613)))+(((x606)*(x614)))+(((x612)*(x614)))+(((x605)*(x607)))+(((IkReal(-0.0750000000000000))*(x614)))+(((IkReal(-1.00000000000000))*(x607)*(x615)))+(((IkReal(0.0750000000000000))*(x605)))+(((IkReal(-1.00000000000000))*(cj3)*(cj4)*(x609)))+(((IkReal(-0.0750000000000000))*(x613)))))));
1999 sj2array[0]=IKsin(j2array[0]);
2000 cj2array[0]=IKcos(j2array[0]);
2001 if( j2array[0] > IKPI )
2002 {
2003  j2array[0]-=IK2PI;
2004 }
2005 else if( j2array[0] < -IKPI )
2006 { j2array[0]+=IK2PI;
2007 }
2008 j2valid[0] = true;
2009 for(int ij2 = 0; ij2 < 1; ++ij2)
2010 {
2011 if( !j2valid[ij2] )
2012 {
2013  continue;
2014 }
2015 _ij2[0] = ij2; _ij2[1] = -1;
2016 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2017 {
2018 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2019 {
2020  j2valid[iij2]=false; _ij2[1] = iij2; break;
2021 }
2022 }
2023 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2024 {
2025 IkReal evalcond[8];
2026 IkReal x617=IKcos(j2);
2027 IkReal x618=IKsin(j2);
2028 IkReal x619=(py)*(py);
2029 IkReal x620=(px)*(px);
2030 IkReal x621=(pz)*(pz);
2031 IkReal x622=((sj0)*(sj1));
2032 IkReal x623=((IkReal(1.00000000000000))*(r01));
2033 IkReal x624=((r00)*(sj1));
2034 IkReal x625=((IkReal(0.0750000000000000))*(r00));
2035 IkReal x626=((IkReal(0.0491250000000000))*(sj3));
2036 IkReal x627=((cj1)*(r01));
2037 IkReal x628=((IkReal(0.150000000000000))*(py));
2038 IkReal x629=((pz)*(r02));
2039 IkReal x630=((px)*(r02));
2040 IkReal x631=((IkReal(0.0750000000000000))*(sj4));
2041 IkReal x632=((cj1)*(sj0));
2042 IkReal x633=((pz)*(r00));
2043 IkReal x634=((IkReal(0.0750000000000000))*(cj0));
2044 IkReal x635=((IkReal(2.00000000000000))*(cj0));
2045 IkReal x636=((cj1)*(r02));
2046 IkReal x637=((IkReal(0.150000000000000))*(sj1));
2047 IkReal x638=((px)*(r00));
2048 IkReal x639=((IkReal(2.00000000000000))*(py));
2049 IkReal x640=((r02)*(sj1));
2050 IkReal x641=((IkReal(0.150000000000000))*(cj1));
2051 IkReal x642=((IkReal(0.108031250000000))*(cj3));
2052 IkReal x643=((cj0)*(py));
2053 IkReal x644=((r01)*(sj1));
2054 IkReal x645=((IkReal(0.0800000000000000))*(cj3));
2055 IkReal x646=((IkReal(0.150000000000000))*(pz));
2056 IkReal x647=((IkReal(0.0491250000000000))*(cj3));
2057 IkReal x648=((cj1)*(pz));
2058 IkReal x649=((IkReal(0.0952312500000000))*(sj3));
2059 IkReal x650=((IkReal(0.600000000000000))*(py));
2060 IkReal x651=((IkReal(1.00000000000000))*(py));
2061 IkReal x652=((px)*(sj1));
2062 IkReal x653=((IkReal(1.00000000000000))*(cj0));
2063 IkReal x654=((cj0)*(r00));
2064 IkReal x655=((IkReal(0.600000000000000))*(cj0));
2065 IkReal x656=((IkReal(1.00000000000000))*(sj3));
2066 IkReal x657=((pz)*(sj1));
2067 IkReal x658=((IkReal(0.0800000000000000))*(sj3));
2068 IkReal x659=((px)*(py));
2069 IkReal x660=((cj1)*(r00));
2070 IkReal x661=((r01)*(sj0));
2071 IkReal x662=((cj1)*(px));
2072 IkReal x663=((cj4)*(x618));
2073 IkReal x664=((cj4)*(x617));
2074 IkReal x665=((sj4)*(x618));
2075 IkReal x666=((IkReal(2.00000000000000))*(x621));
2076 IkReal x667=((sj4)*(x617));
2077 IkReal x668=((IkReal(2.00000000000000))*(x619));
2078 IkReal x669=((x617)*(x658));
2079 IkReal x670=((px)*(x629)*(x635));
2080 evalcond[0]=((((IkReal(-1.00000000000000))*(cj3)*(x663)))+(x640)+(((IkReal(-1.00000000000000))*(x623)*(x632)))+(((IkReal(-1.00000000000000))*(x653)*(x660)))+(((IkReal(-1.00000000000000))*(x656)*(x664))));
2081 evalcond[1]=((((IkReal(-1.00000000000000))*(x656)*(x663)))+(((IkReal(-1.00000000000000))*(x622)*(x623)))+(((IkReal(-1.00000000000000))*(x624)*(x653)))+(((IkReal(-1.00000000000000))*(x636)))+(((cj3)*(x664))));
2082 evalcond[2]=((x657)+(((IkReal(0.327500000000000))*(x617)))+(((IkReal(-0.0750000000000000))*(x618)))+(((x617)*(x645)))+(((IkReal(-1.00000000000000))*(x632)*(x651)))+(((IkReal(-1.00000000000000))*(x653)*(x662)))+(((IkReal(-1.00000000000000))*(x618)*(x658)))+(((IkReal(0.0750000000000000))*(cj1))));
2083 evalcond[3]=((IkReal(0.300000000000000))+(x669)+(((x618)*(x645)))+(((IkReal(0.0750000000000000))*(x617)))+(((IkReal(-1.00000000000000))*(x652)*(x653)))+(((IkReal(0.327500000000000))*(x618)))+(((IkReal(-1.00000000000000))*(x622)*(x651)))+(((IkReal(-1.00000000000000))*(x648)))+(((IkReal(0.0750000000000000))*(sj1))));
2084 evalcond[4]=((((x625)*(x632)))+(((IkReal(-1.00000000000000))*(x651)*(x660)))+(((x658)*(x665)))+(((px)*(x627)))+(((IkReal(-1.00000000000000))*(x627)*(x634)))+(((IkReal(-1.00000000000000))*(x622)*(x630)))+(((IkReal(-1.00000000000000))*(cj0)*(x623)*(x657)))+(((IkReal(-0.327500000000000))*(x667)))+(((x640)*(x643)))+(((x622)*(x633)))+(((IkReal(-1.00000000000000))*(x645)*(x667)))+(((x618)*(x631))));
2085 evalcond[5]=((((IkReal(-1.00000000000000))*(x630)*(x632)))+(((x658)*(x667)))+(((IkReal(-1.00000000000000))*(cj0)*(x623)*(x648)))+(((x632)*(x633)))+(((x645)*(x665)))+(((IkReal(-0.300000000000000))*(r00)*(sj0)))+(((IkReal(-1.00000000000000))*(x622)*(x625)))+(((IkReal(-1.00000000000000))*(x623)*(x652)))+(((x636)*(x643)))+(((x617)*(x631)))+(((IkReal(0.300000000000000))*(cj0)*(r01)))+(((py)*(x624)))+(((x634)*(x644)))+(((IkReal(0.327500000000000))*(x665))));
2086 evalcond[6]=((((IkReal(0.0843750000000000))*(x636)))+(((IkReal(-1.00000000000000))*(x647)*(x663)))+(((IkReal(0.0450000000000000))*(x654)))+(((IkReal(-0.600000000000000))*(x629)))+(((IkReal(-0.150000000000000))*(px)*(x624)))+(((IkReal(-1.00000000000000))*(x628)*(x644)))+(((IkReal(-1.00000000000000))*(sj0)*(x627)*(x646)))+(((x642)*(x664)))+(((IkReal(2.00000000000000))*(x633)*(x662)))+(((IkReal(-1.00000000000000))*(x629)*(x637)))+(((x636)*(x666)))+(((IkReal(-1.00000000000000))*(pp)*(x636)))+(((x649)*(x663)))+(((x620)*(x624)*(x635)))+(((IkReal(0.0956250000000000))*(cj0)*(x624)))+(((pz)*(x627)*(x639)))+(((r01)*(x622)*(x668)))+(((IkReal(0.0450000000000000))*(x661)))+(((r02)*(x628)*(x632)))+(((x626)*(x664)))+(((IkReal(-0.0120000000000000))*(x663)))+(((IkReal(0.0524000000000000))*(x664)))+(((IkReal(-0.600000000000000))*(x638)))+(((x635)*(x644)*(x659)))+(((cj0)*(x630)*(x641)))+(((IkReal(0.0956250000000000))*(r01)*(x622)))+(((IkReal(-1.00000000000000))*(pp)*(x624)*(x653)))+(((IkReal(-1.00000000000000))*(pp)*(x622)*(x623)))+(((IkReal(-1.00000000000000))*(r01)*(x650)))+(((IkReal(-1.00000000000000))*(cj0)*(x633)*(x641)))+(((x622)*(x629)*(x639)))+(((x622)*(x638)*(x639)))+(((x629)*(x635)*(x652))));
2087 evalcond[7]=((((IkReal(-1.00000000000000))*(x627)*(x628)))+(((r01)*(x622)*(x646)))+(((x632)*(x638)*(x639)))+(((IkReal(-1.00000000000000))*(x638)*(x641)))+(((x620)*(x635)*(x660)))+(((IkReal(-0.0843750000000000))*(sj0)*(x627)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(cj0)*(x630)*(x637)))+(((IkReal(-0.0120000000000000))*(x664)))+(((IkReal(0.0956250000000000))*(x640)))+(((IkReal(-1.00000000000000))*(pp)*(x623)*(x632)))+(((IkReal(-2.00000000000000))*(px)*(pz)*(x624)))+(((IkReal(-0.0843750000000000))*(cj1)*(x654)))+(((IkReal(0.600000000000000))*(pz)*(x661)))+(((IkReal(-1.00000000000000))*(pz)*(x639)*(x644)))+(((x629)*(x635)*(x662)))+(((x633)*(x655)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x650)))+(((IkReal(-1.00000000000000))*(x629)*(x641)))+(((IkReal(-1.00000000000000))*(x640)*(x666)))+(((IkReal(-0.0524000000000000))*(x663)))+(((x627)*(x635)*(x659)))+(((cj0)*(x624)*(x646)))+(((IkReal(-1.00000000000000))*(pp)*(x653)*(x660)))+(((x649)*(x664)))+(((IkReal(-1.00000000000000))*(x647)*(x664)))+(((IkReal(-1.00000000000000))*(x630)*(x655)))+(((pp)*(x640)))+(((IkReal(-1.00000000000000))*(x626)*(x663)))+(((sj0)*(x627)*(x668)))+(((IkReal(-1.00000000000000))*(r02)*(x622)*(x628)))+(((x629)*(x632)*(x639)))+(((IkReal(-1.00000000000000))*(x642)*(x663))));
2088 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 )
2089 {
2090 continue;
2091 }
2092 }
2093 
2094 {
2095 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
2096 vinfos[0].jointtype = 1;
2097 vinfos[0].foffset = j0;
2098 vinfos[0].indices[0] = _ij0[0];
2099 vinfos[0].indices[1] = _ij0[1];
2100 vinfos[0].maxsolutions = _nj0;
2101 vinfos[1].jointtype = 1;
2102 vinfos[1].foffset = j1;
2103 vinfos[1].indices[0] = _ij1[0];
2104 vinfos[1].indices[1] = _ij1[1];
2105 vinfos[1].maxsolutions = _nj1;
2106 vinfos[2].jointtype = 1;
2107 vinfos[2].foffset = j2;
2108 vinfos[2].indices[0] = _ij2[0];
2109 vinfos[2].indices[1] = _ij2[1];
2110 vinfos[2].maxsolutions = _nj2;
2111 vinfos[3].jointtype = 1;
2112 vinfos[3].foffset = j3;
2113 vinfos[3].indices[0] = _ij3[0];
2114 vinfos[3].indices[1] = _ij3[1];
2115 vinfos[3].maxsolutions = _nj3;
2116 vinfos[4].jointtype = 1;
2117 vinfos[4].foffset = j4;
2118 vinfos[4].indices[0] = _ij4[0];
2119 vinfos[4].indices[1] = _ij4[1];
2120 vinfos[4].maxsolutions = _nj4;
2121 std::vector<int> vfree(0);
2122 solutions.AddSolution(vinfos,vfree);
2123 }
2124 }
2125 }
2126 
2127 }
2128 
2129 }
2130 
2131 } else
2132 {
2133 {
2134 IkReal j2array[1], cj2array[1], sj2array[1];
2135 bool j2valid[1]={false};
2136 _nj2 = 1;
2137 IkReal x671=((IkReal(1.00000000000000))*(cj1));
2138 IkReal x672=((cj0)*(r00));
2139 IkReal x673=((cj3)*(r02));
2140 IkReal x674=((sj1)*(sj3));
2141 IkReal x675=((r01)*(sj0));
2142 IkReal x676=((cj3)*(x675));
2143 if( IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(x671)*(x676)))+(((IkReal(-1.00000000000000))*(x672)*(x674)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x671)))+(((sj1)*(x673)))+(((IkReal(-1.00000000000000))*(cj3)*(x671)*(x672)))+(((IkReal(-1.00000000000000))*(x674)*(x675))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(sj3)*(x671)*(x672)))+(((cj3)*(sj1)*(x672)))+(((cj1)*(x673)))+(((sj1)*(x676)))+(((IkReal(-1.00000000000000))*(sj3)*(x671)*(x675)))+(((r02)*(x674))))))) < IKFAST_ATAN2_MAGTHRESH )
2144  continue;
2145 j2array[0]=IKatan2(((gconst2)*(((((IkReal(-1.00000000000000))*(x671)*(x676)))+(((IkReal(-1.00000000000000))*(x672)*(x674)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x671)))+(((sj1)*(x673)))+(((IkReal(-1.00000000000000))*(cj3)*(x671)*(x672)))+(((IkReal(-1.00000000000000))*(x674)*(x675)))))), ((gconst2)*(((((IkReal(-1.00000000000000))*(sj3)*(x671)*(x672)))+(((cj3)*(sj1)*(x672)))+(((cj1)*(x673)))+(((sj1)*(x676)))+(((IkReal(-1.00000000000000))*(sj3)*(x671)*(x675)))+(((r02)*(x674)))))));
2146 sj2array[0]=IKsin(j2array[0]);
2147 cj2array[0]=IKcos(j2array[0]);
2148 if( j2array[0] > IKPI )
2149 {
2150  j2array[0]-=IK2PI;
2151 }
2152 else if( j2array[0] < -IKPI )
2153 { j2array[0]+=IK2PI;
2154 }
2155 j2valid[0] = true;
2156 for(int ij2 = 0; ij2 < 1; ++ij2)
2157 {
2158 if( !j2valid[ij2] )
2159 {
2160  continue;
2161 }
2162 _ij2[0] = ij2; _ij2[1] = -1;
2163 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2164 {
2165 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2166 {
2167  j2valid[iij2]=false; _ij2[1] = iij2; break;
2168 }
2169 }
2170 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2171 {
2172 IkReal evalcond[8];
2173 IkReal x677=IKcos(j2);
2174 IkReal x678=IKsin(j2);
2175 IkReal x679=(py)*(py);
2176 IkReal x680=(px)*(px);
2177 IkReal x681=(pz)*(pz);
2178 IkReal x682=((sj0)*(sj1));
2179 IkReal x683=((IkReal(1.00000000000000))*(r01));
2180 IkReal x684=((r00)*(sj1));
2181 IkReal x685=((IkReal(0.0750000000000000))*(r00));
2182 IkReal x686=((IkReal(0.0491250000000000))*(sj3));
2183 IkReal x687=((cj1)*(r01));
2184 IkReal x688=((IkReal(0.150000000000000))*(py));
2185 IkReal x689=((pz)*(r02));
2186 IkReal x690=((px)*(r02));
2187 IkReal x691=((IkReal(0.0750000000000000))*(sj4));
2188 IkReal x692=((cj1)*(sj0));
2189 IkReal x693=((pz)*(r00));
2190 IkReal x694=((IkReal(0.0750000000000000))*(cj0));
2191 IkReal x695=((IkReal(2.00000000000000))*(cj0));
2192 IkReal x696=((cj1)*(r02));
2193 IkReal x697=((IkReal(0.150000000000000))*(sj1));
2194 IkReal x698=((px)*(r00));
2195 IkReal x699=((IkReal(2.00000000000000))*(py));
2196 IkReal x700=((r02)*(sj1));
2197 IkReal x701=((IkReal(0.150000000000000))*(cj1));
2198 IkReal x702=((IkReal(0.108031250000000))*(cj3));
2199 IkReal x703=((cj0)*(py));
2200 IkReal x704=((r01)*(sj1));
2201 IkReal x705=((IkReal(0.0800000000000000))*(cj3));
2202 IkReal x706=((IkReal(0.150000000000000))*(pz));
2203 IkReal x707=((IkReal(0.0491250000000000))*(cj3));
2204 IkReal x708=((cj1)*(pz));
2205 IkReal x709=((IkReal(0.0952312500000000))*(sj3));
2206 IkReal x710=((IkReal(0.600000000000000))*(py));
2207 IkReal x711=((IkReal(1.00000000000000))*(py));
2208 IkReal x712=((px)*(sj1));
2209 IkReal x713=((IkReal(1.00000000000000))*(cj0));
2210 IkReal x714=((cj0)*(r00));
2211 IkReal x715=((IkReal(0.600000000000000))*(cj0));
2212 IkReal x716=((IkReal(1.00000000000000))*(sj3));
2213 IkReal x717=((pz)*(sj1));
2214 IkReal x718=((IkReal(0.0800000000000000))*(sj3));
2215 IkReal x719=((px)*(py));
2216 IkReal x720=((cj1)*(r00));
2217 IkReal x721=((r01)*(sj0));
2218 IkReal x722=((cj1)*(px));
2219 IkReal x723=((cj4)*(x678));
2220 IkReal x724=((cj4)*(x677));
2221 IkReal x725=((sj4)*(x678));
2222 IkReal x726=((IkReal(2.00000000000000))*(x681));
2223 IkReal x727=((sj4)*(x677));
2224 IkReal x728=((IkReal(2.00000000000000))*(x679));
2225 IkReal x729=((x677)*(x718));
2226 IkReal x730=((px)*(x689)*(x695));
2227 evalcond[0]=((((IkReal(-1.00000000000000))*(cj3)*(x723)))+(((IkReal(-1.00000000000000))*(x716)*(x724)))+(x700)+(((IkReal(-1.00000000000000))*(x683)*(x692)))+(((IkReal(-1.00000000000000))*(x713)*(x720))));
2228 evalcond[1]=((((IkReal(-1.00000000000000))*(x716)*(x723)))+(((IkReal(-1.00000000000000))*(x682)*(x683)))+(((IkReal(-1.00000000000000))*(x696)))+(((IkReal(-1.00000000000000))*(x684)*(x713)))+(((cj3)*(x724))));
2229 evalcond[2]=((((IkReal(-1.00000000000000))*(x713)*(x722)))+(((IkReal(-1.00000000000000))*(x678)*(x718)))+(((IkReal(-0.0750000000000000))*(x678)))+(((IkReal(-1.00000000000000))*(x692)*(x711)))+(((IkReal(0.327500000000000))*(x677)))+(x717)+(((IkReal(0.0750000000000000))*(cj1)))+(((x677)*(x705))));
2230 evalcond[3]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x682)*(x711)))+(((IkReal(-1.00000000000000))*(x708)))+(x729)+(((x678)*(x705)))+(((IkReal(0.0750000000000000))*(x677)))+(((IkReal(0.327500000000000))*(x678)))+(((IkReal(-1.00000000000000))*(x712)*(x713)))+(((IkReal(0.0750000000000000))*(sj1))));
2231 evalcond[4]=((((x685)*(x692)))+(((IkReal(-1.00000000000000))*(cj0)*(x683)*(x717)))+(((IkReal(-1.00000000000000))*(x682)*(x690)))+(((x700)*(x703)))+(((IkReal(-0.327500000000000))*(x727)))+(((px)*(x687)))+(((IkReal(-1.00000000000000))*(x705)*(x727)))+(((x678)*(x691)))+(((IkReal(-1.00000000000000))*(x687)*(x694)))+(((IkReal(-1.00000000000000))*(x711)*(x720)))+(((x682)*(x693)))+(((x718)*(x725))));
2232 evalcond[5]=((((IkReal(-1.00000000000000))*(x683)*(x712)))+(((x692)*(x693)))+(((x696)*(x703)))+(((IkReal(-0.300000000000000))*(r00)*(sj0)))+(((py)*(x684)))+(((IkReal(-1.00000000000000))*(x682)*(x685)))+(((IkReal(-1.00000000000000))*(x690)*(x692)))+(((x694)*(x704)))+(((x718)*(x727)))+(((x705)*(x725)))+(((IkReal(-1.00000000000000))*(cj0)*(x683)*(x708)))+(((IkReal(0.300000000000000))*(cj0)*(r01)))+(((IkReal(0.327500000000000))*(x725)))+(((x677)*(x691))));
2233 evalcond[6]=((((r02)*(x688)*(x692)))+(((IkReal(-0.150000000000000))*(px)*(x684)))+(((IkReal(-0.600000000000000))*(x698)))+(((IkReal(0.0450000000000000))*(x721)))+(((x709)*(x723)))+(((x682)*(x689)*(x699)))+(((x695)*(x704)*(x719)))+(((IkReal(-0.600000000000000))*(x689)))+(((IkReal(2.00000000000000))*(x693)*(x722)))+(((IkReal(-1.00000000000000))*(pp)*(x682)*(x683)))+(((IkReal(0.0956250000000000))*(cj0)*(x684)))+(((IkReal(-1.00000000000000))*(pp)*(x696)))+(((IkReal(0.0524000000000000))*(x724)))+(((IkReal(-0.0120000000000000))*(x723)))+(((IkReal(-1.00000000000000))*(x689)*(x697)))+(((x680)*(x684)*(x695)))+(((IkReal(-1.00000000000000))*(pp)*(x684)*(x713)))+(((x702)*(x724)))+(((cj0)*(x690)*(x701)))+(((IkReal(0.0843750000000000))*(x696)))+(((IkReal(-1.00000000000000))*(sj0)*(x687)*(x706)))+(((IkReal(-1.00000000000000))*(x707)*(x723)))+(((IkReal(-1.00000000000000))*(r01)*(x710)))+(((x686)*(x724)))+(((r01)*(x682)*(x728)))+(((x682)*(x698)*(x699)))+(((x689)*(x695)*(x712)))+(((pz)*(x687)*(x699)))+(((IkReal(-1.00000000000000))*(cj0)*(x693)*(x701)))+(((x696)*(x726)))+(((IkReal(0.0956250000000000))*(r01)*(x682)))+(((IkReal(-1.00000000000000))*(x688)*(x704)))+(((IkReal(0.0450000000000000))*(x714))));
2234 evalcond[7]=((((IkReal(-1.00000000000000))*(pp)*(x713)*(x720)))+(((IkReal(-1.00000000000000))*(pp)*(x683)*(x692)))+(((IkReal(0.600000000000000))*(pz)*(x721)))+(((sj0)*(x687)*(x728)))+(((x689)*(x695)*(x722)))+(((IkReal(-1.00000000000000))*(x702)*(x723)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(r02)*(x682)*(x688)))+(((IkReal(-0.0843750000000000))*(cj1)*(x714)))+(((x709)*(x724)))+(((IkReal(-1.00000000000000))*(x698)*(x701)))+(((IkReal(-0.0524000000000000))*(x723)))+(((cj0)*(x684)*(x706)))+(((x692)*(x698)*(x699)))+(((IkReal(-1.00000000000000))*(x707)*(x724)))+(((x687)*(x695)*(x719)))+(((IkReal(-0.0120000000000000))*(x724)))+(((IkReal(0.0956250000000000))*(x700)))+(((r01)*(x682)*(x706)))+(((IkReal(-0.0843750000000000))*(sj0)*(x687)))+(((x689)*(x692)*(x699)))+(((IkReal(-2.00000000000000))*(px)*(pz)*(x684)))+(((IkReal(-1.00000000000000))*(x687)*(x688)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x710)))+(((IkReal(-1.00000000000000))*(x686)*(x723)))+(((IkReal(-1.00000000000000))*(x689)*(x701)))+(((x680)*(x695)*(x720)))+(((x693)*(x715)))+(((IkReal(-1.00000000000000))*(pz)*(x699)*(x704)))+(((IkReal(-1.00000000000000))*(cj0)*(x690)*(x697)))+(((pp)*(x700)))+(((IkReal(-1.00000000000000))*(x690)*(x715)))+(((IkReal(-1.00000000000000))*(x700)*(x726))));
2235 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 )
2236 {
2237 continue;
2238 }
2239 }
2240 
2241 {
2242 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
2243 vinfos[0].jointtype = 1;
2244 vinfos[0].foffset = j0;
2245 vinfos[0].indices[0] = _ij0[0];
2246 vinfos[0].indices[1] = _ij0[1];
2247 vinfos[0].maxsolutions = _nj0;
2248 vinfos[1].jointtype = 1;
2249 vinfos[1].foffset = j1;
2250 vinfos[1].indices[0] = _ij1[0];
2251 vinfos[1].indices[1] = _ij1[1];
2252 vinfos[1].maxsolutions = _nj1;
2253 vinfos[2].jointtype = 1;
2254 vinfos[2].foffset = j2;
2255 vinfos[2].indices[0] = _ij2[0];
2256 vinfos[2].indices[1] = _ij2[1];
2257 vinfos[2].maxsolutions = _nj2;
2258 vinfos[3].jointtype = 1;
2259 vinfos[3].foffset = j3;
2260 vinfos[3].indices[0] = _ij3[0];
2261 vinfos[3].indices[1] = _ij3[1];
2262 vinfos[3].maxsolutions = _nj3;
2263 vinfos[4].jointtype = 1;
2264 vinfos[4].foffset = j4;
2265 vinfos[4].indices[0] = _ij4[0];
2266 vinfos[4].indices[1] = _ij4[1];
2267 vinfos[4].maxsolutions = _nj4;
2268 std::vector<int> vfree(0);
2269 solutions.AddSolution(vinfos,vfree);
2270 }
2271 }
2272 }
2273 
2274 }
2275 
2276 }
2277 }
2278 }
2279 
2280 }
2281 
2282 }
2283 
2284 } else
2285 {
2286 {
2287 IkReal j3array[1], cj3array[1], sj3array[1];
2288 bool j3valid[1]={false};
2289 _nj3 = 1;
2290 IkReal x731=((r01)*(sj0));
2291 IkReal x732=((IkReal(15720.0000000000))*(sj1));
2292 IkReal x733=((px)*(r00));
2293 IkReal x734=((IkReal(3600.00000000000))*(sj1));
2294 IkReal x735=((cj0)*(r00));
2295 IkReal x736=((cj1)*(r02));
2296 IkReal x737=((py)*(r01));
2297 IkReal x738=((cj4)*(sj1));
2298 IkReal x739=((pz)*(r02));
2299 IkReal x740=((cj4)*(pp));
2300 IkReal x741=((cj0)*(cj4)*(px));
2301 IkReal x742=((cj1)*(cj4)*(pz));
2302 IkReal x743=((cj4)*(py)*(sj0));
2303 IkReal x744=((py)*(sj0)*(x738));
2304 if( IKabs(((gconst0)*(((((IkReal(-11250.0000000000))*(x741)))+(((IkReal(-11250.0000000000))*(x743)))+(((x732)*(x735)))+(((IkReal(3930.00000000000))*(x731)))+(((x731)*(x732)))+(((IkReal(-45000.0000000000))*(x744)))+(((IkReal(-45000.0000000000))*(cj0)*(px)*(x738)))+(((IkReal(-45000.0000000000))*(x742)))+(((IkReal(3930.00000000000))*(x735)))+(((IkReal(-52400.0000000000))*(x739)))+(((IkReal(3375.00000000000))*(x738)))+(((IkReal(-52400.0000000000))*(x733)))+(((IkReal(15720.0000000000))*(x736)))+(((IkReal(-1774.21875000000))*(cj4)))+(((IkReal(-52400.0000000000))*(x737)))+(((IkReal(75000.0000000000))*(x740))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-49125.0000000000))*(x743)))+(((IkReal(-1.00000000000000))*(x734)*(x735)))+(((IkReal(-196500.000000000))*(x742)))+(((IkReal(-3600.00000000000))*(x736)))+(((IkReal(-900.000000000000))*(x731)))+(((IkReal(12000.0000000000))*(x737)))+(((IkReal(-196500.000000000))*(x744)))+(((IkReal(-196500.000000000))*(cj0)*(px)*(x738)))+(((IkReal(-1.00000000000000))*(x731)*(x734)))+(((IkReal(12000.0000000000))*(x739)))+(((IkReal(12000.0000000000))*(x733)))+(((IkReal(327500.000000000))*(x740)))+(((IkReal(-7747.42187500000))*(cj4)))+(((IkReal(-900.000000000000))*(x735)))+(((IkReal(14737.5000000000))*(x738)))+(((IkReal(-49125.0000000000))*(x741))))))) < IKFAST_ATAN2_MAGTHRESH )
2305  continue;
2306 j3array[0]=IKatan2(((gconst0)*(((((IkReal(-11250.0000000000))*(x741)))+(((IkReal(-11250.0000000000))*(x743)))+(((x732)*(x735)))+(((IkReal(3930.00000000000))*(x731)))+(((x731)*(x732)))+(((IkReal(-45000.0000000000))*(x744)))+(((IkReal(-45000.0000000000))*(cj0)*(px)*(x738)))+(((IkReal(-45000.0000000000))*(x742)))+(((IkReal(3930.00000000000))*(x735)))+(((IkReal(-52400.0000000000))*(x739)))+(((IkReal(3375.00000000000))*(x738)))+(((IkReal(-52400.0000000000))*(x733)))+(((IkReal(15720.0000000000))*(x736)))+(((IkReal(-1774.21875000000))*(cj4)))+(((IkReal(-52400.0000000000))*(x737)))+(((IkReal(75000.0000000000))*(x740)))))), ((gconst0)*(((((IkReal(-49125.0000000000))*(x743)))+(((IkReal(-1.00000000000000))*(x734)*(x735)))+(((IkReal(-196500.000000000))*(x742)))+(((IkReal(-3600.00000000000))*(x736)))+(((IkReal(-900.000000000000))*(x731)))+(((IkReal(12000.0000000000))*(x737)))+(((IkReal(-196500.000000000))*(x744)))+(((IkReal(-196500.000000000))*(cj0)*(px)*(x738)))+(((IkReal(-1.00000000000000))*(x731)*(x734)))+(((IkReal(12000.0000000000))*(x739)))+(((IkReal(12000.0000000000))*(x733)))+(((IkReal(327500.000000000))*(x740)))+(((IkReal(-7747.42187500000))*(cj4)))+(((IkReal(-900.000000000000))*(x735)))+(((IkReal(14737.5000000000))*(x738)))+(((IkReal(-49125.0000000000))*(x741)))))));
2307 sj3array[0]=IKsin(j3array[0]);
2308 cj3array[0]=IKcos(j3array[0]);
2309 if( j3array[0] > IKPI )
2310 {
2311  j3array[0]-=IK2PI;
2312 }
2313 else if( j3array[0] < -IKPI )
2314 { j3array[0]+=IK2PI;
2315 }
2316 j3valid[0] = true;
2317 for(int ij3 = 0; ij3 < 1; ++ij3)
2318 {
2319 if( !j3valid[ij3] )
2320 {
2321  continue;
2322 }
2323 _ij3[0] = ij3; _ij3[1] = -1;
2324 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2325 {
2326 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2327 {
2328  j3valid[iij3]=false; _ij3[1] = iij3; break;
2329 }
2330 }
2331 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2332 {
2333 IkReal evalcond[4];
2334 IkReal x745=IKcos(j3);
2335 IkReal x746=IKsin(j3);
2336 IkReal x747=((r01)*(sj0));
2337 IkReal x748=((IkReal(0.300000000000000))*(r02));
2338 IkReal x749=((r00)*(sj0));
2339 IkReal x750=((py)*(sj0));
2340 IkReal x751=((IkReal(0.600000000000000))*(cj1));
2341 IkReal x752=((IkReal(0.150000000000000))*(px));
2342 IkReal x753=((IkReal(1.00000000000000))*(pz));
2343 IkReal x754=((py)*(r00));
2344 IkReal x755=((IkReal(0.300000000000000))*(cj1));
2345 IkReal x756=((IkReal(1.00000000000000))*(pp));
2346 IkReal x757=((cj0)*(r00));
2347 IkReal x758=((IkReal(0.0450000000000000))*(sj1));
2348 IkReal x759=((IkReal(0.600000000000000))*(sj1));
2349 IkReal x760=((cj0)*(r01));
2350 IkReal x761=((IkReal(2.00000000000000))*(pz));
2351 IkReal x762=((cj0)*(px));
2352 IkReal x763=((IkReal(0.300000000000000))*(sj1));
2353 IkReal x764=((IkReal(2.00000000000000))*(px)*(py));
2354 IkReal x765=((IkReal(0.0120000000000000))*(x746));
2355 IkReal x766=((cj4)*(x746));
2356 IkReal x767=((IkReal(0.0524000000000000))*(x745));
2357 IkReal x768=((cj4)*(x745));
2358 IkReal x769=((cj0)*(py)*(r02));
2359 IkReal x770=((px)*(r02)*(sj0));
2360 evalcond[0]=((IkReal(0.0236562500000000))+(((x750)*(x759)))+(((cj0)*(x752)))+(((IkReal(-1.00000000000000))*(x756)))+(((IkReal(-1.00000000000000))*(x758)))+(x765)+(x767)+(((pz)*(x751)))+(((x759)*(x762)))+(((IkReal(0.150000000000000))*(x750))));
2361 evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((IkReal(-0.327500000000000))*(x766)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((cj1)*(x748)))+(((IkReal(-1.00000000000000))*(r02)*(x753)))+(((IkReal(0.0750000000000000))*(x757)))+(((IkReal(0.0750000000000000))*(x768)))+(((x757)*(x763)))+(((x747)*(x763)))+(((IkReal(0.0750000000000000))*(x747))));
2362 evalcond[2]=((((x747)*(x755)))+(((IkReal(-0.0800000000000000))*(cj4)))+(((IkReal(-1.00000000000000))*(x753)*(x757)))+(((IkReal(-0.0750000000000000))*(x766)))+(((r02)*(x750)))+(((r02)*(x762)))+(((IkReal(-0.0750000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(sj1)*(x748)))+(((x755)*(x757)))+(((IkReal(-0.327500000000000))*(x768)))+(((IkReal(-1.00000000000000))*(x747)*(x753))));
2363 evalcond[3]=((((IkReal(-1.00000000000000))*(x761)*(x769)))+(((IkReal(-1.00000000000000))*(sj4)*(x767)))+(((IkReal(-1.00000000000000))*(pz)*(x751)*(x760)))+(((IkReal(-0.119281250000000))*(sj4)))+(((IkReal(-0.0956250000000000))*(x749)))+(((IkReal(-2.00000000000000))*(x760)*((py)*(py))))+(((IkReal(-1.00000000000000))*(r01)*(x752)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x759)))+(((IkReal(-1.00000000000000))*(x749)*(x758)))+(((x761)*(x770)))+(((x754)*(x759)))+(((pz)*(x749)*(x751)))+(((IkReal(0.150000000000000))*(x754)))+(((IkReal(-2.00000000000000))*(x754)*(x762)))+(((x747)*(x764)))+(((IkReal(-1.00000000000000))*(x751)*(x770)))+(((x758)*(x760)))+(((IkReal(2.00000000000000))*(x749)*((px)*(px))))+(((x751)*(x769)))+(((IkReal(0.0956250000000000))*(x760)))+(((IkReal(-1.00000000000000))*(sj4)*(x765)))+(((pp)*(x760)))+(((IkReal(-1.00000000000000))*(x749)*(x756))));
2364 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2365 {
2366 continue;
2367 }
2368 }
2369 
2370 {
2371 IkReal dummyeval[1];
2372 IkReal gconst2;
2373 gconst2=IKsign(((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3))))));
2374 dummyeval[0]=((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3)))));
2375 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2376 {
2377 {
2378 IkReal dummyeval[1];
2379 IkReal gconst3;
2380 IkReal x771=((IkReal(0.0800000000000000))*(cj4));
2381 gconst3=IKsign(((((IkReal(0.0750000000000000))*(cj4)*(sj3)))+(((x771)*((sj3)*(sj3))))+(((x771)*((cj3)*(cj3))))+(((IkReal(0.327500000000000))*(cj3)*(cj4)))));
2382 IkReal x772=((IkReal(1.06666666666667))*(cj4));
2383 dummyeval[0]=((((x772)*((cj3)*(cj3))))+(((x772)*((sj3)*(sj3))))+(((IkReal(4.36666666666667))*(cj3)*(cj4)))+(((cj4)*(sj3))));
2384 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2385 {
2386 {
2387 IkReal evalcond[11];
2388 IkReal x773=((IkReal(0.0524000000000000))*(cj3));
2389 IkReal x774=((IkReal(0.0120000000000000))*(sj3));
2390 IkReal x775=(py)*(py);
2391 IkReal x776=(px)*(px);
2392 IkReal x777=(pz)*(pz);
2393 IkReal x778=((r01)*(sj0));
2394 IkReal x779=((py)*(r00));
2395 IkReal x780=((pz)*(sj1));
2396 IkReal x781=((py)*(r01));
2397 IkReal x782=((px)*(sj0));
2398 IkReal x783=((IkReal(0.600000000000000))*(r02));
2399 IkReal x784=((IkReal(0.150000000000000))*(cj1));
2400 IkReal x785=((cj0)*(sj1));
2401 IkReal x786=((IkReal(0.150000000000000))*(px));
2402 IkReal x787=((IkReal(2.00000000000000))*(cj1));
2403 IkReal x788=((cj0)*(r01));
2404 IkReal x789=((r02)*(sj1));
2405 IkReal x790=((px)*(r00));
2406 IkReal x791=((IkReal(0.300000000000000))*(r00));
2407 IkReal x792=((IkReal(1.00000000000000))*(pz));
2408 IkReal x793=((r00)*(sj1));
2409 IkReal x794=((cj0)*(r00));
2410 IkReal x795=((cj0)*(cj1));
2411 IkReal x796=((IkReal(1.00000000000000))*(sj1));
2412 IkReal x797=((IkReal(0.0956250000000000))*(r00));
2413 IkReal x798=((IkReal(0.600000000000000))*(pz));
2414 IkReal x799=((IkReal(0.600000000000000))*(sj1));
2415 IkReal x800=((IkReal(2.00000000000000))*(px));
2416 IkReal x801=((IkReal(2.00000000000000))*(sj1));
2417 IkReal x802=((IkReal(0.150000000000000))*(sj1));
2418 IkReal x803=((cj1)*(r02));
2419 IkReal x804=((cj0)*(px));
2420 IkReal x805=((IkReal(0.0843750000000000))*(cj1));
2421 IkReal x806=((py)*(sj0));
2422 IkReal x807=((pz)*(r02));
2423 IkReal x808=((IkReal(1.00000000000000))*(cj1));
2424 IkReal x809=((cj0)*(py));
2425 IkReal x810=((r00)*(sj0));
2426 IkReal x811=((r02)*(x806));
2427 IkReal x812=((pp)*(x808));
2428 IkReal x813=((IkReal(1.00000000000000))*(pp)*(r00));
2429 IkReal x814=((x773)+(x774));
2430 IkReal x815=((IkReal(2.00000000000000))*(r00)*(x776));
2431 IkReal x816=((cj0)*(x800)*(x807));
2432 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
2433 evalcond[1]=((x809)+(((IkReal(-1.00000000000000))*(x782))));
2434 evalcond[2]=((IkReal(-1.00000000000000))+(x788)+(((IkReal(-1.00000000000000))*(x810))));
2435 evalcond[3]=((x789)+(((IkReal(-1.00000000000000))*(x778)*(x808)))+(((IkReal(-1.00000000000000))*(x794)*(x808))));
2436 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x785)))+(((IkReal(-1.00000000000000))*(x778)*(x796)))+(((IkReal(-1.00000000000000))*(x803))));
2437 evalcond[5]=((IkReal(0.0236562500000000))+(((x799)*(x806)))+(((IkReal(-1.00000000000000))*(pp)))+(((cj1)*(x798)))+(((IkReal(0.150000000000000))*(x806)))+(((IkReal(0.600000000000000))*(px)*(x785)))+(x814)+(((IkReal(-0.0450000000000000))*(sj1)))+(((cj0)*(x786))));
2438 evalcond[6]=((((IkReal(0.0750000000000000))*(x794)))+(((IkReal(0.300000000000000))*(x803)))+(((IkReal(-1.00000000000000))*(r02)*(x792)))+(((IkReal(0.0750000000000000))*(x778)))+(((IkReal(-1.00000000000000))*(x790)))+(((IkReal(0.300000000000000))*(sj1)*(x778)))+(((IkReal(-1.00000000000000))*(x781)))+(((x785)*(x791))));
2439 evalcond[7]=((((IkReal(-1.00000000000000))*(x792)*(x794)))+(((IkReal(0.300000000000000))*(cj1)*(x778)))+(((r02)*(x804)))+(((IkReal(-0.300000000000000))*(x789)))+(((IkReal(-1.00000000000000))*(x778)*(x792)))+(((IkReal(-0.0750000000000000))*(r02)))+(x811)+(((x791)*(x795))));
2440 evalcond[8]=((IkReal(-0.119281250000000))+(((IkReal(-1.00000000000000))*(sj0)*(x797)))+(((py)*(x778)*(x800)))+(((IkReal(2.00000000000000))*(x776)*(x810)))+(((pp)*(x788)))+(((IkReal(-2.00000000000000))*(x807)*(x809)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x799)))+(((IkReal(-1.00000000000000))*(r01)*(x786)))+(((cj1)*(x798)*(x810)))+(((IkReal(0.0956250000000000))*(x788)))+(((IkReal(2.00000000000000))*(x782)*(x807)))+(((IkReal(-1.00000000000000))*(x814)))+(((IkReal(-2.00000000000000))*(x775)*(x788)))+(((IkReal(-1.00000000000000))*(cj1)*(x782)*(x783)))+(((IkReal(-1.00000000000000))*(pp)*(x810)))+(((IkReal(-0.0450000000000000))*(sj0)*(x793)))+(((x779)*(x799)))+(((py)*(x783)*(x795)))+(((IkReal(0.0450000000000000))*(r01)*(x785)))+(((IkReal(-1.00000000000000))*(cj0)*(x779)*(x800)))+(((IkReal(-1.00000000000000))*(cj1)*(x788)*(x798)))+(((IkReal(0.150000000000000))*(x779))));
2441 evalcond[9]=((((r02)*(x777)*(x787)))+(((pz)*(x781)*(x787)))+(((x785)*(x797)))+(((IkReal(0.0450000000000000))*(x778)))+(((IkReal(-1.00000000000000))*(pp)*(x778)*(x796)))+(((IkReal(-1.00000000000000))*(x781)*(x802)))+(((r02)*(x784)*(x804)))+(((IkReal(0.0956250000000000))*(sj1)*(x778)))+(((IkReal(-1.00000000000000))*(pz)*(x778)*(x784)))+(((IkReal(-1.00000000000000))*(pz)*(x784)*(x794)))+(((IkReal(-1.00000000000000))*(pp)*(x803)))+(((x785)*(x815)))+(((IkReal(-0.600000000000000))*(x790)))+(((IkReal(0.0843750000000000))*(x803)))+(((IkReal(-1.00000000000000))*(pz)*(x783)))+(((IkReal(0.0450000000000000))*(x794)))+(((x775)*(x778)*(x801)))+(((cj0)*(r02)*(x780)*(x800)))+(((x784)*(x811)))+(((IkReal(-1.00000000000000))*(x786)*(x793)))+(((x781)*(x785)*(x800)))+(((x779)*(x782)*(x801)))+(((IkReal(-0.600000000000000))*(x781)))+(((IkReal(-0.150000000000000))*(r02)*(x780)))+(((IkReal(2.00000000000000))*(x780)*(x811)))+(((IkReal(-1.00000000000000))*(x785)*(x813)))+(((pz)*(x787)*(x790))));
2442 evalcond[10]=((((IkReal(-1.00000000000000))*(r02)*(x785)*(x786)))+(((IkReal(-1.00000000000000))*(x781)*(x784)))+(((IkReal(-1.00000000000000))*(x783)*(x804)))+(((IkReal(-1.00000000000000))*(x778)*(x805)))+(((IkReal(-2.00000000000000))*(x777)*(x789)))+(((IkReal(0.0956250000000000))*(x789)))+(((IkReal(0.150000000000000))*(x780)*(x794)))+(((x776)*(x787)*(x794)))+(((IkReal(-1.00000000000000))*(x778)*(x812)))+(((IkReal(-1.00000000000000))*(x794)*(x812)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x783)*(x806)))+(((x775)*(x778)*(x787)))+(((IkReal(-2.00000000000000))*(x780)*(x790)))+(((IkReal(-1.00000000000000))*(x794)*(x805)))+(((IkReal(-2.00000000000000))*(x780)*(x781)))+(((x778)*(x798)))+(((IkReal(-1.00000000000000))*(x784)*(x807)))+(((IkReal(-0.150000000000000))*(x789)*(x806)))+(((x794)*(x798)))+(((IkReal(-1.00000000000000))*(x784)*(x790)))+(((x779)*(x782)*(x787)))+(((x787)*(x804)*(x807)))+(((pp)*(x789)))+(((x787)*(x806)*(x807)))+(((x781)*(x787)*(x804)))+(((IkReal(0.150000000000000))*(x778)*(x780))));
2443 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 )
2444 {
2445 {
2446 IkReal dummyeval[1];
2447 IkReal gconst4;
2448 gconst4=IKsign(((IkReal(0.112881250000000))+(((IkReal(0.0524000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
2449 dummyeval[0]=((IkReal(17.6376953125000))+(((IkReal(8.18750000000000))*(cj3)))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3)));
2450 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2451 {
2452 {
2453 IkReal dummyeval[1];
2454 IkReal gconst5;
2455 gconst5=IKsign(((IkReal(0.112881250000000))+(((IkReal(0.0524000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
2456 dummyeval[0]=((IkReal(17.6376953125000))+(((IkReal(8.18750000000000))*(cj3)))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3)));
2457 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2458 {
2459 continue;
2460 
2461 } else
2462 {
2463 {
2464 IkReal j2array[1], cj2array[1], sj2array[1];
2465 bool j2valid[1]={false};
2466 _nj2 = 1;
2467 IkReal x817=((pz)*(r01));
2468 IkReal x818=((py)*(r02));
2469 IkReal x819=((cj1)*(r01));
2470 IkReal x820=((IkReal(0.00600000000000000))*(cj0));
2471 IkReal x821=((cj1)*(pz));
2472 IkReal x822=((r00)*(sj0));
2473 IkReal x823=((IkReal(0.0750000000000000))*(px));
2474 IkReal x824=((cj0)*(sj1));
2475 IkReal x825=((px)*(sj3));
2476 IkReal x826=((IkReal(0.0800000000000000))*(sj3));
2477 IkReal x827=((sj0)*(sj1));
2478 IkReal x828=((IkReal(0.327500000000000))*(px));
2479 IkReal x829=((IkReal(0.00600000000000000))*(cj1));
2480 IkReal x830=((IkReal(0.0750000000000000))*(py));
2481 IkReal x831=((IkReal(0.0800000000000000))*(cj3));
2482 IkReal x832=((pz)*(sj1));
2483 IkReal x833=((IkReal(0.00600000000000000))*(sj1));
2484 IkReal x834=((IkReal(0.0800000000000000))*(x824));
2485 IkReal x835=((sj1)*(x831));
2486 IkReal x836=((cj1)*(py)*(r00));
2487 IkReal x837=((IkReal(0.0800000000000000))*(px)*(r02)*(x827));
2488 if( IKabs(((gconst5)*(((IkReal(-0.0982500000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x833)))+(((IkReal(0.327500000000000))*(x821)))+(((x821)*(x831)))+(((IkReal(-1.00000000000000))*(x822)*(x826)*(x832)))+(((IkReal(0.0800000000000000))*(r02)*(x825)*(x827)))+(((IkReal(-0.0750000000000000))*(x818)*(x824)))+(((py)*(x827)*(x831)))+(((sj3)*(x819)*(x820)))+(((IkReal(-1.00000000000000))*(x818)*(x824)*(x826)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x822)*(x829)))+(((IkReal(0.00562500000000000))*(cj0)*(x819)))+(((x817)*(x824)*(x826)))+(((x826)*(x836)))+(((IkReal(-0.00562500000000000))*(cj1)*(x822)))+(((px)*(x824)*(x831)))+(((IkReal(-0.0800000000000000))*(x819)*(x825)))+(((r02)*(x823)*(x827)))+(((IkReal(-1.00000000000000))*(x819)*(x823)))+(((cj1)*(r00)*(x830)))+(((x824)*(x828)))+(((IkReal(0.327500000000000))*(py)*(x827)))+(((IkReal(-0.0750000000000000))*(x822)*(x832)))+(((IkReal(0.0750000000000000))*(x817)*(x824))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(-0.0225000000000000))+(((IkReal(0.0750000000000000))*(x821)))+(((IkReal(-1.00000000000000))*(sj3)*(x833)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-0.327500000000000))*(x836)))+(((x822)*(x831)*(x832)))+(((IkReal(-1.00000000000000))*(x831)*(x836)))+(((x818)*(x824)*(x831)))+(((x827)*(x830)))+(((IkReal(-1.00000000000000))*(cj3)*(x819)*(x820)))+(((x819)*(x828)))+(((IkReal(-0.327500000000000))*(x817)*(x824)))+(((x821)*(x826)))+(((x823)*(x824)))+(((IkReal(0.327500000000000))*(x822)*(x832)))+(((IkReal(0.0245625000000000))*(cj1)*(x822)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x827)*(x831)))+(((IkReal(-1.00000000000000))*(r02)*(x827)*(x828)))+(((IkReal(0.327500000000000))*(x818)*(x824)))+(((cj3)*(x822)*(x829)))+(((py)*(x826)*(x827)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x825)*(x834)))+(((IkReal(-0.0245625000000000))*(cj0)*(x819)))+(((px)*(x819)*(x831)))+(((IkReal(-1.00000000000000))*(x817)*(x824)*(x831))))))) < IKFAST_ATAN2_MAGTHRESH )
2489  continue;
2490 j2array[0]=IKatan2(((gconst5)*(((IkReal(-0.0982500000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x833)))+(((IkReal(0.327500000000000))*(x821)))+(((x821)*(x831)))+(((IkReal(-1.00000000000000))*(x822)*(x826)*(x832)))+(((IkReal(0.0800000000000000))*(r02)*(x825)*(x827)))+(((IkReal(-0.0750000000000000))*(x818)*(x824)))+(((py)*(x827)*(x831)))+(((sj3)*(x819)*(x820)))+(((IkReal(-1.00000000000000))*(x818)*(x824)*(x826)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x822)*(x829)))+(((IkReal(0.00562500000000000))*(cj0)*(x819)))+(((x817)*(x824)*(x826)))+(((x826)*(x836)))+(((IkReal(-0.00562500000000000))*(cj1)*(x822)))+(((px)*(x824)*(x831)))+(((IkReal(-0.0800000000000000))*(x819)*(x825)))+(((r02)*(x823)*(x827)))+(((IkReal(-1.00000000000000))*(x819)*(x823)))+(((cj1)*(r00)*(x830)))+(((x824)*(x828)))+(((IkReal(0.327500000000000))*(py)*(x827)))+(((IkReal(-0.0750000000000000))*(x822)*(x832)))+(((IkReal(0.0750000000000000))*(x817)*(x824)))))), ((gconst5)*(((IkReal(-0.0225000000000000))+(((IkReal(0.0750000000000000))*(x821)))+(((IkReal(-1.00000000000000))*(sj3)*(x833)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-0.327500000000000))*(x836)))+(((x822)*(x831)*(x832)))+(((IkReal(-1.00000000000000))*(x831)*(x836)))+(((x818)*(x824)*(x831)))+(((x827)*(x830)))+(((IkReal(-1.00000000000000))*(cj3)*(x819)*(x820)))+(((x819)*(x828)))+(((IkReal(-0.327500000000000))*(x817)*(x824)))+(((x821)*(x826)))+(((x823)*(x824)))+(((IkReal(0.327500000000000))*(x822)*(x832)))+(((IkReal(0.0245625000000000))*(cj1)*(x822)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x827)*(x831)))+(((IkReal(-1.00000000000000))*(r02)*(x827)*(x828)))+(((IkReal(0.327500000000000))*(x818)*(x824)))+(((cj3)*(x822)*(x829)))+(((py)*(x826)*(x827)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x825)*(x834)))+(((IkReal(-0.0245625000000000))*(cj0)*(x819)))+(((px)*(x819)*(x831)))+(((IkReal(-1.00000000000000))*(x817)*(x824)*(x831)))))));
2491 sj2array[0]=IKsin(j2array[0]);
2492 cj2array[0]=IKcos(j2array[0]);
2493 if( j2array[0] > IKPI )
2494 {
2495  j2array[0]-=IK2PI;
2496 }
2497 else if( j2array[0] < -IKPI )
2498 { j2array[0]+=IK2PI;
2499 }
2500 j2valid[0] = true;
2501 for(int ij2 = 0; ij2 < 1; ++ij2)
2502 {
2503 if( !j2valid[ij2] )
2504 {
2505  continue;
2506 }
2507 _ij2[0] = ij2; _ij2[1] = -1;
2508 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2509 {
2510 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2511 {
2512  j2valid[iij2]=false; _ij2[1] = iij2; break;
2513 }
2514 }
2515 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2516 {
2517 IkReal evalcond[4];
2518 IkReal x838=IKcos(j2);
2519 IkReal x839=IKsin(j2);
2520 IkReal x840=((IkReal(0.0800000000000000))*(sj3));
2521 IkReal x841=((cj0)*(r01));
2522 IkReal x842=((IkReal(1.00000000000000))*(px));
2523 IkReal x843=((py)*(sj1));
2524 IkReal x844=((cj0)*(r02));
2525 IkReal x845=((IkReal(0.0750000000000000))*(cj1));
2526 IkReal x846=((r02)*(sj0));
2527 IkReal x847=((IkReal(0.0750000000000000))*(sj1));
2528 IkReal x848=((cj1)*(pz));
2529 IkReal x849=((r00)*(sj0));
2530 IkReal x850=((IkReal(0.0800000000000000))*(cj3));
2531 IkReal x851=((pz)*(sj1));
2532 IkReal x852=((IkReal(1.00000000000000))*(sj0));
2533 IkReal x853=((cj1)*(py));
2534 IkReal x854=((IkReal(0.0750000000000000))*(x839));
2535 IkReal x855=((IkReal(0.327500000000000))*(x838));
2536 IkReal x856=((IkReal(0.0750000000000000))*(x838));
2537 IkReal x857=((IkReal(0.327500000000000))*(x839));
2538 IkReal x858=((sj1)*(x849));
2539 IkReal x859=((x839)*(x840));
2540 IkReal x860=((x838)*(x850));
2541 IkReal x861=((x838)*(x840));
2542 IkReal x862=((x839)*(x850));
2543 IkReal x863=((x854)+(x859));
2544 IkReal x864=((x860)+(x855));
2545 IkReal x865=((x862)+(x861)+(x856)+(x857));
2546 evalcond[0]=((((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x842)))+(x864)+(x845)+(((IkReal(-1.00000000000000))*(x863)))+(x851)+(((IkReal(-1.00000000000000))*(x852)*(x853))));
2547 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x842)))+(((IkReal(-1.00000000000000))*(x843)*(x852)))+(x865)+(x847)+(((IkReal(-1.00000000000000))*(x848))));
2548 evalcond[2]=((((x843)*(x844)))+(((IkReal(-1.00000000000000))*(x864)))+(((IkReal(-1.00000000000000))*(x841)*(x851)))+(x863)+(((x849)*(x851)))+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(x841)*(x845)))+(((IkReal(-1.00000000000000))*(r00)*(x853)))+(((x845)*(x849)))+(((IkReal(-1.00000000000000))*(sj1)*(x842)*(x846))));
2549 evalcond[3]=((((IkReal(-1.00000000000000))*(x847)*(x849)))+(((IkReal(-0.300000000000000))*(x849)))+(((x848)*(x849)))+(((r00)*(x843)))+(((x844)*(x853)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x842)))+(x865)+(((IkReal(-1.00000000000000))*(x841)*(x848)))+(((x841)*(x847)))+(((IkReal(0.300000000000000))*(x841)))+(((IkReal(-1.00000000000000))*(cj1)*(x842)*(x846))));
2550 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2551 {
2552 continue;
2553 }
2554 }
2555 
2556 {
2557 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
2558 vinfos[0].jointtype = 1;
2559 vinfos[0].foffset = j0;
2560 vinfos[0].indices[0] = _ij0[0];
2561 vinfos[0].indices[1] = _ij0[1];
2562 vinfos[0].maxsolutions = _nj0;
2563 vinfos[1].jointtype = 1;
2564 vinfos[1].foffset = j1;
2565 vinfos[1].indices[0] = _ij1[0];
2566 vinfos[1].indices[1] = _ij1[1];
2567 vinfos[1].maxsolutions = _nj1;
2568 vinfos[2].jointtype = 1;
2569 vinfos[2].foffset = j2;
2570 vinfos[2].indices[0] = _ij2[0];
2571 vinfos[2].indices[1] = _ij2[1];
2572 vinfos[2].maxsolutions = _nj2;
2573 vinfos[3].jointtype = 1;
2574 vinfos[3].foffset = j3;
2575 vinfos[3].indices[0] = _ij3[0];
2576 vinfos[3].indices[1] = _ij3[1];
2577 vinfos[3].maxsolutions = _nj3;
2578 vinfos[4].jointtype = 1;
2579 vinfos[4].foffset = j4;
2580 vinfos[4].indices[0] = _ij4[0];
2581 vinfos[4].indices[1] = _ij4[1];
2582 vinfos[4].maxsolutions = _nj4;
2583 std::vector<int> vfree(0);
2584 solutions.AddSolution(vinfos,vfree);
2585 }
2586 }
2587 }
2588 
2589 }
2590 
2591 }
2592 
2593 } else
2594 {
2595 {
2596 IkReal j2array[1], cj2array[1], sj2array[1];
2597 bool j2valid[1]={false};
2598 _nj2 = 1;
2599 IkReal x866=((IkReal(0.0800000000000000))*(cj1));
2600 IkReal x867=((cj0)*(px));
2601 IkReal x868=((py)*(sj0));
2602 IkReal x869=((IkReal(0.0750000000000000))*(cj1));
2603 IkReal x870=((IkReal(0.327500000000000))*(cj1));
2604 IkReal x871=((IkReal(0.327500000000000))*(sj1));
2605 IkReal x872=((IkReal(0.0750000000000000))*(sj1));
2606 IkReal x873=((IkReal(0.00600000000000000))*(sj1));
2607 IkReal x874=((IkReal(0.00600000000000000))*(cj1));
2608 IkReal x875=((IkReal(0.0800000000000000))*(sj1)*(sj3));
2609 IkReal x876=((IkReal(0.0800000000000000))*(cj3)*(sj1));
2610 if( IKabs(((gconst4)*(((IkReal(-0.0982500000000000))+(((sj3)*(x874)))+(((x868)*(x871)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((x867)*(x871)))+(((IkReal(-1.00000000000000))*(x867)*(x869)))+(((pz)*(x870)))+(((IkReal(-1.00000000000000))*(sj3)*(x866)*(x868)))+(((x868)*(x876)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(cj3)*(x873)))+(((IkReal(-1.00000000000000))*(x868)*(x869)))+(((pz)*(x872)))+(((x867)*(x876)))+(((IkReal(-1.00000000000000))*(sj3)*(x866)*(x867)))+(((pz)*(x875)))+(((cj3)*(pz)*(x866))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((cj3)*(x866)*(x868)))+(((x868)*(x870)))+(((IkReal(-1.00000000000000))*(sj3)*(x873)))+(((x868)*(x875)))+(((x867)*(x875)))+(((x867)*(x870)))+(((x868)*(x872)))+(((pz)*(x869)))+(((cj3)*(x866)*(x867)))+(((pz)*(sj3)*(x866)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(pz)*(x871)))+(((IkReal(-1.00000000000000))*(pz)*(x876)))+(((IkReal(-1.00000000000000))*(cj3)*(x874)))+(((x867)*(x872))))))) < IKFAST_ATAN2_MAGTHRESH )
2611  continue;
2612 j2array[0]=IKatan2(((gconst4)*(((IkReal(-0.0982500000000000))+(((sj3)*(x874)))+(((x868)*(x871)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((x867)*(x871)))+(((IkReal(-1.00000000000000))*(x867)*(x869)))+(((pz)*(x870)))+(((IkReal(-1.00000000000000))*(sj3)*(x866)*(x868)))+(((x868)*(x876)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(cj3)*(x873)))+(((IkReal(-1.00000000000000))*(x868)*(x869)))+(((pz)*(x872)))+(((x867)*(x876)))+(((IkReal(-1.00000000000000))*(sj3)*(x866)*(x867)))+(((pz)*(x875)))+(((cj3)*(pz)*(x866)))))), ((gconst4)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((cj3)*(x866)*(x868)))+(((x868)*(x870)))+(((IkReal(-1.00000000000000))*(sj3)*(x873)))+(((x868)*(x875)))+(((x867)*(x875)))+(((x867)*(x870)))+(((x868)*(x872)))+(((pz)*(x869)))+(((cj3)*(x866)*(x867)))+(((pz)*(sj3)*(x866)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(pz)*(x871)))+(((IkReal(-1.00000000000000))*(pz)*(x876)))+(((IkReal(-1.00000000000000))*(cj3)*(x874)))+(((x867)*(x872)))))));
2613 sj2array[0]=IKsin(j2array[0]);
2614 cj2array[0]=IKcos(j2array[0]);
2615 if( j2array[0] > IKPI )
2616 {
2617  j2array[0]-=IK2PI;
2618 }
2619 else if( j2array[0] < -IKPI )
2620 { j2array[0]+=IK2PI;
2621 }
2622 j2valid[0] = true;
2623 for(int ij2 = 0; ij2 < 1; ++ij2)
2624 {
2625 if( !j2valid[ij2] )
2626 {
2627  continue;
2628 }
2629 _ij2[0] = ij2; _ij2[1] = -1;
2630 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2631 {
2632 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2633 {
2634  j2valid[iij2]=false; _ij2[1] = iij2; break;
2635 }
2636 }
2637 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2638 {
2639 IkReal evalcond[4];
2640 IkReal x877=IKcos(j2);
2641 IkReal x878=IKsin(j2);
2642 IkReal x879=((IkReal(0.0800000000000000))*(sj3));
2643 IkReal x880=((cj0)*(r01));
2644 IkReal x881=((IkReal(1.00000000000000))*(px));
2645 IkReal x882=((py)*(sj1));
2646 IkReal x883=((cj0)*(r02));
2647 IkReal x884=((IkReal(0.0750000000000000))*(cj1));
2648 IkReal x885=((r02)*(sj0));
2649 IkReal x886=((IkReal(0.0750000000000000))*(sj1));
2650 IkReal x887=((cj1)*(pz));
2651 IkReal x888=((r00)*(sj0));
2652 IkReal x889=((IkReal(0.0800000000000000))*(cj3));
2653 IkReal x890=((pz)*(sj1));
2654 IkReal x891=((IkReal(1.00000000000000))*(sj0));
2655 IkReal x892=((cj1)*(py));
2656 IkReal x893=((IkReal(0.0750000000000000))*(x878));
2657 IkReal x894=((IkReal(0.327500000000000))*(x877));
2658 IkReal x895=((IkReal(0.0750000000000000))*(x877));
2659 IkReal x896=((IkReal(0.327500000000000))*(x878));
2660 IkReal x897=((sj1)*(x888));
2661 IkReal x898=((x878)*(x879));
2662 IkReal x899=((x877)*(x889));
2663 IkReal x900=((x877)*(x879));
2664 IkReal x901=((x878)*(x889));
2665 IkReal x902=((x893)+(x898));
2666 IkReal x903=((x894)+(x899));
2667 IkReal x904=((x900)+(x901)+(x896)+(x895));
2668 evalcond[0]=((((IkReal(-1.00000000000000))*(x891)*(x892)))+(x884)+(((IkReal(-1.00000000000000))*(x902)))+(x903)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x881)))+(x890));
2669 evalcond[1]=((IkReal(0.300000000000000))+(x886)+(((IkReal(-1.00000000000000))*(x887)))+(x904)+(((IkReal(-1.00000000000000))*(x882)*(x891)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x881))));
2670 evalcond[2]=((((cj1)*(px)*(r01)))+(x902)+(((IkReal(-1.00000000000000))*(x903)))+(((IkReal(-1.00000000000000))*(r00)*(x892)))+(((IkReal(-1.00000000000000))*(x880)*(x890)))+(((IkReal(-1.00000000000000))*(x880)*(x884)))+(((IkReal(-1.00000000000000))*(sj1)*(x881)*(x885)))+(((x884)*(x888)))+(((x882)*(x883)))+(((x888)*(x890))));
2671 evalcond[3]=((((IkReal(0.300000000000000))*(x880)))+(((x887)*(x888)))+(((IkReal(-1.00000000000000))*(x880)*(x887)))+(((r00)*(x882)))+(((x883)*(x892)))+(((x880)*(x886)))+(((IkReal(-1.00000000000000))*(x886)*(x888)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x881)))+(((IkReal(-0.300000000000000))*(x888)))+(x904)+(((IkReal(-1.00000000000000))*(cj1)*(x881)*(x885))));
2672 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2673 {
2674 continue;
2675 }
2676 }
2677 
2678 {
2679 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
2680 vinfos[0].jointtype = 1;
2681 vinfos[0].foffset = j0;
2682 vinfos[0].indices[0] = _ij0[0];
2683 vinfos[0].indices[1] = _ij0[1];
2684 vinfos[0].maxsolutions = _nj0;
2685 vinfos[1].jointtype = 1;
2686 vinfos[1].foffset = j1;
2687 vinfos[1].indices[0] = _ij1[0];
2688 vinfos[1].indices[1] = _ij1[1];
2689 vinfos[1].maxsolutions = _nj1;
2690 vinfos[2].jointtype = 1;
2691 vinfos[2].foffset = j2;
2692 vinfos[2].indices[0] = _ij2[0];
2693 vinfos[2].indices[1] = _ij2[1];
2694 vinfos[2].maxsolutions = _nj2;
2695 vinfos[3].jointtype = 1;
2696 vinfos[3].foffset = j3;
2697 vinfos[3].indices[0] = _ij3[0];
2698 vinfos[3].indices[1] = _ij3[1];
2699 vinfos[3].maxsolutions = _nj3;
2700 vinfos[4].jointtype = 1;
2701 vinfos[4].foffset = j4;
2702 vinfos[4].indices[0] = _ij4[0];
2703 vinfos[4].indices[1] = _ij4[1];
2704 vinfos[4].maxsolutions = _nj4;
2705 std::vector<int> vfree(0);
2706 solutions.AddSolution(vinfos,vfree);
2707 }
2708 }
2709 }
2710 
2711 }
2712 
2713 }
2714 
2715 } else
2716 {
2717 IkReal x905=((IkReal(0.0524000000000000))*(cj3));
2718 IkReal x906=((IkReal(0.0120000000000000))*(sj3));
2719 IkReal x907=(py)*(py);
2720 IkReal x908=(px)*(px);
2721 IkReal x909=(pz)*(pz);
2722 IkReal x910=((r01)*(sj0));
2723 IkReal x911=((py)*(r00));
2724 IkReal x912=((pz)*(sj1));
2725 IkReal x913=((py)*(r01));
2726 IkReal x914=((px)*(sj0));
2727 IkReal x915=((IkReal(0.600000000000000))*(r02));
2728 IkReal x916=((IkReal(0.150000000000000))*(cj1));
2729 IkReal x917=((cj0)*(sj1));
2730 IkReal x918=((IkReal(0.150000000000000))*(px));
2731 IkReal x919=((IkReal(2.00000000000000))*(cj1));
2732 IkReal x920=((cj0)*(r01));
2733 IkReal x921=((r02)*(sj1));
2734 IkReal x922=((px)*(r00));
2735 IkReal x923=((IkReal(0.300000000000000))*(r00));
2736 IkReal x924=((IkReal(1.00000000000000))*(pz));
2737 IkReal x925=((r00)*(sj1));
2738 IkReal x926=((cj0)*(r00));
2739 IkReal x927=((cj0)*(cj1));
2740 IkReal x928=((IkReal(1.00000000000000))*(sj1));
2741 IkReal x929=((IkReal(0.0956250000000000))*(r00));
2742 IkReal x930=((IkReal(0.600000000000000))*(pz));
2743 IkReal x931=((IkReal(0.600000000000000))*(sj1));
2744 IkReal x932=((IkReal(2.00000000000000))*(px));
2745 IkReal x933=((IkReal(2.00000000000000))*(sj1));
2746 IkReal x934=((IkReal(0.150000000000000))*(sj1));
2747 IkReal x935=((cj1)*(r02));
2748 IkReal x936=((cj0)*(px));
2749 IkReal x937=((IkReal(0.0843750000000000))*(cj1));
2750 IkReal x938=((py)*(sj0));
2751 IkReal x939=((pz)*(r02));
2752 IkReal x940=((IkReal(1.00000000000000))*(cj1));
2753 IkReal x941=((cj0)*(py));
2754 IkReal x942=((r00)*(sj0));
2755 IkReal x943=((r02)*(x938));
2756 IkReal x944=((pp)*(x940));
2757 IkReal x945=((IkReal(1.00000000000000))*(pp)*(r00));
2758 IkReal x946=((x905)+(x906));
2759 IkReal x947=((IkReal(2.00000000000000))*(r00)*(x908));
2760 IkReal x948=((cj0)*(x932)*(x939));
2761 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
2762 evalcond[1]=((((IkReal(-1.00000000000000))*(x914)))+(x941));
2763 evalcond[2]=((IkReal(1.00000000000000))+(x920)+(((IkReal(-1.00000000000000))*(x942))));
2764 evalcond[3]=((x921)+(((IkReal(-1.00000000000000))*(x910)*(x940)))+(((IkReal(-1.00000000000000))*(x926)*(x940))));
2765 evalcond[4]=((((IkReal(-1.00000000000000))*(x910)*(x928)))+(((IkReal(-1.00000000000000))*(x935)))+(((IkReal(-1.00000000000000))*(r00)*(x917))));
2766 evalcond[5]=((IkReal(0.0236562500000000))+(((IkReal(-1.00000000000000))*(pp)))+(x946)+(((IkReal(0.150000000000000))*(x938)))+(((IkReal(0.600000000000000))*(px)*(x917)))+(((cj0)*(x918)))+(((x931)*(x938)))+(((IkReal(-0.0450000000000000))*(sj1)))+(((cj1)*(x930))));
2767 evalcond[6]=((((x917)*(x923)))+(((IkReal(-1.00000000000000))*(x913)))+(((IkReal(0.300000000000000))*(x935)))+(((IkReal(0.300000000000000))*(sj1)*(x910)))+(((IkReal(-1.00000000000000))*(r02)*(x924)))+(((IkReal(0.0750000000000000))*(x926)))+(((IkReal(-1.00000000000000))*(x922)))+(((IkReal(0.0750000000000000))*(x910))));
2768 evalcond[7]=((((IkReal(0.300000000000000))*(cj1)*(x910)))+(x943)+(((IkReal(-0.300000000000000))*(x921)))+(((IkReal(-1.00000000000000))*(x924)*(x926)))+(((x923)*(x927)))+(((IkReal(-1.00000000000000))*(x910)*(x924)))+(((IkReal(-0.0750000000000000))*(r02)))+(((r02)*(x936))));
2769 evalcond[8]=((IkReal(0.119281250000000))+(((cj1)*(x930)*(x942)))+(((IkReal(-2.00000000000000))*(x939)*(x941)))+(((pp)*(x920)))+(((IkReal(0.0956250000000000))*(x920)))+(((IkReal(0.0450000000000000))*(r01)*(x917)))+(((IkReal(-1.00000000000000))*(r01)*(x918)))+(x946)+(((IkReal(-1.00000000000000))*(px)*(r01)*(x931)))+(((IkReal(2.00000000000000))*(x908)*(x942)))+(((IkReal(0.150000000000000))*(x911)))+(((IkReal(-2.00000000000000))*(x907)*(x920)))+(((py)*(x915)*(x927)))+(((IkReal(-1.00000000000000))*(cj0)*(x911)*(x932)))+(((IkReal(-1.00000000000000))*(cj1)*(x914)*(x915)))+(((IkReal(2.00000000000000))*(x914)*(x939)))+(((x911)*(x931)))+(((IkReal(-1.00000000000000))*(cj1)*(x920)*(x930)))+(((py)*(x910)*(x932)))+(((IkReal(-1.00000000000000))*(sj0)*(x929)))+(((IkReal(-1.00000000000000))*(pp)*(x942)))+(((IkReal(-0.0450000000000000))*(sj0)*(x925))));
2770 evalcond[9]=((((x917)*(x947)))+(((IkReal(0.0450000000000000))*(x926)))+(((r02)*(x916)*(x936)))+(((IkReal(-0.150000000000000))*(r02)*(x912)))+(((IkReal(-1.00000000000000))*(pz)*(x910)*(x916)))+(((IkReal(-1.00000000000000))*(pp)*(x910)*(x928)))+(((x911)*(x914)*(x933)))+(((IkReal(-1.00000000000000))*(pp)*(x935)))+(((pz)*(x913)*(x919)))+(((x916)*(x943)))+(((IkReal(-0.600000000000000))*(x922)))+(((IkReal(-0.600000000000000))*(x913)))+(((IkReal(0.0843750000000000))*(x935)))+(((IkReal(2.00000000000000))*(x912)*(x943)))+(((IkReal(-1.00000000000000))*(x917)*(x945)))+(((x917)*(x929)))+(((pz)*(x919)*(x922)))+(((IkReal(-1.00000000000000))*(x913)*(x934)))+(((IkReal(-1.00000000000000))*(pz)*(x916)*(x926)))+(((r02)*(x909)*(x919)))+(((IkReal(0.0956250000000000))*(sj1)*(x910)))+(((x907)*(x910)*(x933)))+(((IkReal(-1.00000000000000))*(x918)*(x925)))+(((IkReal(-1.00000000000000))*(pz)*(x915)))+(((x913)*(x917)*(x932)))+(((cj0)*(r02)*(x912)*(x932)))+(((IkReal(0.0450000000000000))*(x910))));
2771 evalcond[10]=((((IkReal(0.0956250000000000))*(x921)))+(((x910)*(x930)))+(((x911)*(x914)*(x919)))+(((IkReal(0.0450000000000000))*(r02)))+(((x919)*(x938)*(x939)))+(((IkReal(-1.00000000000000))*(x910)*(x944)))+(((IkReal(-1.00000000000000))*(x910)*(x937)))+(((x919)*(x936)*(x939)))+(((IkReal(-1.00000000000000))*(x915)*(x938)))+(((IkReal(-1.00000000000000))*(x916)*(x922)))+(((x908)*(x919)*(x926)))+(((IkReal(-1.00000000000000))*(x913)*(x916)))+(((IkReal(-1.00000000000000))*(x915)*(x936)))+(((IkReal(-1.00000000000000))*(x916)*(x939)))+(((x913)*(x919)*(x936)))+(((IkReal(-1.00000000000000))*(x926)*(x944)))+(((IkReal(-2.00000000000000))*(x912)*(x913)))+(((IkReal(-0.150000000000000))*(x921)*(x938)))+(((IkReal(0.150000000000000))*(x910)*(x912)))+(((pp)*(x921)))+(((IkReal(-2.00000000000000))*(x909)*(x921)))+(((IkReal(0.150000000000000))*(x912)*(x926)))+(((IkReal(-2.00000000000000))*(x912)*(x922)))+(((IkReal(-1.00000000000000))*(x926)*(x937)))+(((x926)*(x930)))+(((x907)*(x910)*(x919)))+(((IkReal(-1.00000000000000))*(r02)*(x917)*(x918))));
2772 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 )
2773 {
2774 {
2775 IkReal dummyeval[1];
2776 IkReal gconst6;
2777 gconst6=IKsign(((IkReal(0.112881250000000))+(((IkReal(0.0524000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
2778 dummyeval[0]=((IkReal(17.6376953125000))+(((IkReal(8.18750000000000))*(cj3)))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3)));
2779 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2780 {
2781 {
2782 IkReal dummyeval[1];
2783 IkReal gconst7;
2784 gconst7=IKsign(((IkReal(-0.112881250000000))+(((IkReal(-0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(-0.0120000000000000))*(sj3)))+(((IkReal(-0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(-0.0524000000000000))*(cj3)))));
2785 dummyeval[0]=((IkReal(-17.6376953125000))+(((IkReal(-1.87500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-8.18750000000000))*(cj3)))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
2786 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2787 {
2788 continue;
2789 
2790 } else
2791 {
2792 {
2793 IkReal j2array[1], cj2array[1], sj2array[1];
2794 bool j2valid[1]={false};
2795 _nj2 = 1;
2796 IkReal x949=((cj0)*(r01));
2797 IkReal x950=((pz)*(sj1));
2798 IkReal x951=((IkReal(0.0800000000000000))*(sj3));
2799 IkReal x952=((IkReal(0.00600000000000000))*(cj1));
2800 IkReal x953=((IkReal(0.0750000000000000))*(cj1));
2801 IkReal x954=((r00)*(sj0));
2802 IkReal x955=((IkReal(0.0245625000000000))*(cj1));
2803 IkReal x956=((px)*(r01));
2804 IkReal x957=((IkReal(0.00562500000000000))*(cj1));
2805 IkReal x958=((IkReal(0.0800000000000000))*(cj3));
2806 IkReal x959=((cj1)*(pz));
2807 IkReal x960=((px)*(r02));
2808 IkReal x961=((py)*(sj1));
2809 IkReal x962=((cj0)*(r02));
2810 IkReal x963=((IkReal(0.00600000000000000))*(sj1));
2811 IkReal x964=((IkReal(0.327500000000000))*(sj0));
2812 IkReal x965=((IkReal(0.0750000000000000))*(sj0)*(sj1));
2813 IkReal x966=((cj0)*(px)*(sj1));
2814 IkReal x967=((cj1)*(py)*(r00));
2815 IkReal x968=((IkReal(0.0800000000000000))*(sj0)*(sj1)*(x960));
2816 if( IKabs(((gconst7)*(((IkReal(0.0982500000000000))+(((IkReal(-1.00000000000000))*(x951)*(x961)*(x962)))+(((IkReal(-1.00000000000000))*(cj1)*(x951)*(x956)))+(((sj3)*(x949)*(x952)))+(((IkReal(-1.00000000000000))*(x958)*(x966)))+(((IkReal(-1.00000000000000))*(sj3)*(x952)*(x954)))+(((IkReal(-0.327500000000000))*(x966)))+(((IkReal(-0.0750000000000000))*(x961)*(x962)))+(((IkReal(-1.00000000000000))*(sj0)*(x958)*(x961)))+(((IkReal(-0.327500000000000))*(x959)))+(((IkReal(-1.00000000000000))*(x954)*(x957)))+(((py)*(r00)*(x953)))+(((IkReal(-1.00000000000000))*(x961)*(x964)))+(((IkReal(0.0245625000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x950)*(x951)*(x954)))+(((x949)*(x957)))+(((cj3)*(x963)))+(((x949)*(x950)*(x951)))+(((IkReal(0.0750000000000000))*(x949)*(x950)))+(((IkReal(-1.00000000000000))*(x953)*(x956)))+(((IkReal(-0.0750000000000000))*(x950)*(x954)))+(((sj0)*(sj1)*(x951)*(x960)))+(((IkReal(0.0240000000000000))*(cj3)))+(((x951)*(x967)))+(((IkReal(-1.00000000000000))*(x958)*(x959)))+(((x960)*(x965))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((IkReal(0.0225000000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x949)*(x952)))+(((IkReal(-0.0750000000000000))*(sj0)*(x961)))+(((IkReal(-1.00000000000000))*(x951)*(x959)))+(((IkReal(0.00562500000000000))*(sj1)))+(((IkReal(-0.327500000000000))*(x967)))+(((IkReal(-1.00000000000000))*(x951)*(x966)))+(((x950)*(x954)*(x958)))+(((IkReal(0.327500000000000))*(x961)*(x962)))+(((cj3)*(x952)*(x954)))+(((IkReal(-1.00000000000000))*(pz)*(x953)))+(((IkReal(-1.00000000000000))*(sj0)*(x951)*(x961)))+(((IkReal(-0.0750000000000000))*(x966)))+(((IkReal(-1.00000000000000))*(sj0)*(sj1)*(x958)*(x960)))+(((IkReal(-0.327500000000000))*(x949)*(x950)))+(((IkReal(-1.00000000000000))*(x958)*(x967)))+(((IkReal(0.327500000000000))*(x950)*(x954)))+(((sj3)*(x963)))+(((x958)*(x961)*(x962)))+(((cj1)*(x956)*(x958)))+(((IkReal(-1.00000000000000))*(sj1)*(x960)*(x964)))+(((IkReal(-1.00000000000000))*(x949)*(x950)*(x958)))+(((IkReal(-1.00000000000000))*(x949)*(x955)))+(((IkReal(0.327500000000000))*(cj1)*(x956)))+(((IkReal(0.0240000000000000))*(sj3)))+(((x954)*(x955))))))) < IKFAST_ATAN2_MAGTHRESH )
2817  continue;
2818 j2array[0]=IKatan2(((gconst7)*(((IkReal(0.0982500000000000))+(((IkReal(-1.00000000000000))*(x951)*(x961)*(x962)))+(((IkReal(-1.00000000000000))*(cj1)*(x951)*(x956)))+(((sj3)*(x949)*(x952)))+(((IkReal(-1.00000000000000))*(x958)*(x966)))+(((IkReal(-1.00000000000000))*(sj3)*(x952)*(x954)))+(((IkReal(-0.327500000000000))*(x966)))+(((IkReal(-0.0750000000000000))*(x961)*(x962)))+(((IkReal(-1.00000000000000))*(sj0)*(x958)*(x961)))+(((IkReal(-0.327500000000000))*(x959)))+(((IkReal(-1.00000000000000))*(x954)*(x957)))+(((py)*(r00)*(x953)))+(((IkReal(-1.00000000000000))*(x961)*(x964)))+(((IkReal(0.0245625000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x950)*(x951)*(x954)))+(((x949)*(x957)))+(((cj3)*(x963)))+(((x949)*(x950)*(x951)))+(((IkReal(0.0750000000000000))*(x949)*(x950)))+(((IkReal(-1.00000000000000))*(x953)*(x956)))+(((IkReal(-0.0750000000000000))*(x950)*(x954)))+(((sj0)*(sj1)*(x951)*(x960)))+(((IkReal(0.0240000000000000))*(cj3)))+(((x951)*(x967)))+(((IkReal(-1.00000000000000))*(x958)*(x959)))+(((x960)*(x965)))))), ((gconst7)*(((IkReal(0.0225000000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x949)*(x952)))+(((IkReal(-0.0750000000000000))*(sj0)*(x961)))+(((IkReal(-1.00000000000000))*(x951)*(x959)))+(((IkReal(0.00562500000000000))*(sj1)))+(((IkReal(-0.327500000000000))*(x967)))+(((IkReal(-1.00000000000000))*(x951)*(x966)))+(((x950)*(x954)*(x958)))+(((IkReal(0.327500000000000))*(x961)*(x962)))+(((cj3)*(x952)*(x954)))+(((IkReal(-1.00000000000000))*(pz)*(x953)))+(((IkReal(-1.00000000000000))*(sj0)*(x951)*(x961)))+(((IkReal(-0.0750000000000000))*(x966)))+(((IkReal(-1.00000000000000))*(sj0)*(sj1)*(x958)*(x960)))+(((IkReal(-0.327500000000000))*(x949)*(x950)))+(((IkReal(-1.00000000000000))*(x958)*(x967)))+(((IkReal(0.327500000000000))*(x950)*(x954)))+(((sj3)*(x963)))+(((x958)*(x961)*(x962)))+(((cj1)*(x956)*(x958)))+(((IkReal(-1.00000000000000))*(sj1)*(x960)*(x964)))+(((IkReal(-1.00000000000000))*(x949)*(x950)*(x958)))+(((IkReal(-1.00000000000000))*(x949)*(x955)))+(((IkReal(0.327500000000000))*(cj1)*(x956)))+(((IkReal(0.0240000000000000))*(sj3)))+(((x954)*(x955)))))));
2819 sj2array[0]=IKsin(j2array[0]);
2820 cj2array[0]=IKcos(j2array[0]);
2821 if( j2array[0] > IKPI )
2822 {
2823  j2array[0]-=IK2PI;
2824 }
2825 else if( j2array[0] < -IKPI )
2826 { j2array[0]+=IK2PI;
2827 }
2828 j2valid[0] = true;
2829 for(int ij2 = 0; ij2 < 1; ++ij2)
2830 {
2831 if( !j2valid[ij2] )
2832 {
2833  continue;
2834 }
2835 _ij2[0] = ij2; _ij2[1] = -1;
2836 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2837 {
2838 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2839 {
2840  j2valid[iij2]=false; _ij2[1] = iij2; break;
2841 }
2842 }
2843 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2844 {
2845 IkReal evalcond[4];
2846 IkReal x969=IKcos(j2);
2847 IkReal x970=IKsin(j2);
2848 IkReal x971=((IkReal(0.0800000000000000))*(sj3));
2849 IkReal x972=((cj0)*(r01));
2850 IkReal x973=((IkReal(1.00000000000000))*(px));
2851 IkReal x974=((py)*(sj1));
2852 IkReal x975=((cj0)*(r02));
2853 IkReal x976=((IkReal(0.0750000000000000))*(cj1));
2854 IkReal x977=((r02)*(sj0));
2855 IkReal x978=((IkReal(0.0750000000000000))*(sj1));
2856 IkReal x979=((cj1)*(pz));
2857 IkReal x980=((r00)*(sj0));
2858 IkReal x981=((IkReal(0.0800000000000000))*(cj3));
2859 IkReal x982=((pz)*(sj1));
2860 IkReal x983=((IkReal(1.00000000000000))*(sj0));
2861 IkReal x984=((cj1)*(py));
2862 IkReal x985=((IkReal(0.327500000000000))*(x969));
2863 IkReal x986=((IkReal(0.0750000000000000))*(x970));
2864 IkReal x987=((IkReal(0.0750000000000000))*(x969));
2865 IkReal x988=((IkReal(0.327500000000000))*(x970));
2866 IkReal x989=((sj1)*(x980));
2867 IkReal x990=((x969)*(x981));
2868 IkReal x991=((x970)*(x971));
2869 IkReal x992=((x969)*(x971));
2870 IkReal x993=((x970)*(x981));
2871 IkReal x994=((x986)+(x991));
2872 IkReal x995=((x985)+(x990));
2873 IkReal x996=((x988)+(x987)+(x993)+(x992));
2874 evalcond[0]=((((IkReal(-1.00000000000000))*(x994)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x973)))+(x976)+(x982)+(((IkReal(-1.00000000000000))*(x983)*(x984)))+(x995));
2875 evalcond[1]=((IkReal(0.300000000000000))+(x978)+(((IkReal(-1.00000000000000))*(x979)))+(((IkReal(-1.00000000000000))*(x974)*(x983)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x973)))+(x996));
2876 evalcond[2]=((((IkReal(-1.00000000000000))*(x972)*(x976)))+(((IkReal(-1.00000000000000))*(x994)))+(((x980)*(x982)))+(((IkReal(-1.00000000000000))*(r00)*(x984)))+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(x972)*(x982)))+(((x976)*(x980)))+(((x974)*(x975)))+(((IkReal(-1.00000000000000))*(sj1)*(x973)*(x977)))+(x995));
2877 evalcond[3]=((((x979)*(x980)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x973)))+(((IkReal(-0.300000000000000))*(x980)))+(((x972)*(x978)))+(((IkReal(0.300000000000000))*(x972)))+(((IkReal(-1.00000000000000))*(x978)*(x980)))+(((IkReal(-1.00000000000000))*(x996)))+(((IkReal(-1.00000000000000))*(cj1)*(x973)*(x977)))+(((IkReal(-1.00000000000000))*(x972)*(x979)))+(((x975)*(x984)))+(((r00)*(x974))));
2878 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2879 {
2880 continue;
2881 }
2882 }
2883 
2884 {
2885 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
2886 vinfos[0].jointtype = 1;
2887 vinfos[0].foffset = j0;
2888 vinfos[0].indices[0] = _ij0[0];
2889 vinfos[0].indices[1] = _ij0[1];
2890 vinfos[0].maxsolutions = _nj0;
2891 vinfos[1].jointtype = 1;
2892 vinfos[1].foffset = j1;
2893 vinfos[1].indices[0] = _ij1[0];
2894 vinfos[1].indices[1] = _ij1[1];
2895 vinfos[1].maxsolutions = _nj1;
2896 vinfos[2].jointtype = 1;
2897 vinfos[2].foffset = j2;
2898 vinfos[2].indices[0] = _ij2[0];
2899 vinfos[2].indices[1] = _ij2[1];
2900 vinfos[2].maxsolutions = _nj2;
2901 vinfos[3].jointtype = 1;
2902 vinfos[3].foffset = j3;
2903 vinfos[3].indices[0] = _ij3[0];
2904 vinfos[3].indices[1] = _ij3[1];
2905 vinfos[3].maxsolutions = _nj3;
2906 vinfos[4].jointtype = 1;
2907 vinfos[4].foffset = j4;
2908 vinfos[4].indices[0] = _ij4[0];
2909 vinfos[4].indices[1] = _ij4[1];
2910 vinfos[4].maxsolutions = _nj4;
2911 std::vector<int> vfree(0);
2912 solutions.AddSolution(vinfos,vfree);
2913 }
2914 }
2915 }
2916 
2917 }
2918 
2919 }
2920 
2921 } else
2922 {
2923 {
2924 IkReal j2array[1], cj2array[1], sj2array[1];
2925 bool j2valid[1]={false};
2926 _nj2 = 1;
2927 IkReal x997=((IkReal(0.0800000000000000))*(cj1));
2928 IkReal x998=((cj0)*(px));
2929 IkReal x999=((py)*(sj0));
2930 IkReal x1000=((IkReal(0.0750000000000000))*(cj1));
2931 IkReal x1001=((IkReal(0.327500000000000))*(cj1));
2932 IkReal x1002=((IkReal(0.327500000000000))*(sj1));
2933 IkReal x1003=((IkReal(0.0750000000000000))*(sj1));
2934 IkReal x1004=((IkReal(0.00600000000000000))*(sj1));
2935 IkReal x1005=((IkReal(0.00600000000000000))*(cj1));
2936 IkReal x1006=((IkReal(0.0800000000000000))*(sj1)*(sj3));
2937 IkReal x1007=((IkReal(0.0800000000000000))*(cj3)*(sj1));
2938 if( IKabs(((gconst6)*(((IkReal(-0.0982500000000000))+(((pz)*(x1003)))+(((IkReal(-1.00000000000000))*(x1000)*(x998)))+(((x1002)*(x998)))+(((pz)*(x1001)))+(((sj3)*(x1005)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((pz)*(x1006)))+(((cj3)*(pz)*(x997)))+(((IkReal(-1.00000000000000))*(sj3)*(x997)*(x998)))+(((x1007)*(x999)))+(((x1007)*(x998)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x1000)*(x999)))+(((IkReal(-1.00000000000000))*(cj3)*(x1004)))+(((IkReal(-1.00000000000000))*(sj3)*(x997)*(x999)))+(((x1002)*(x999))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(-0.0225000000000000))+(((IkReal(-1.00000000000000))*(pz)*(x1007)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((pz)*(sj3)*(x997)))+(((IkReal(-1.00000000000000))*(pz)*(x1002)))+(((x1003)*(x998)))+(((x1003)*(x999)))+(((cj3)*(x997)*(x998)))+(((IkReal(-1.00000000000000))*(sj3)*(x1004)))+(((IkReal(-1.00000000000000))*(cj3)*(x1005)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x1001)*(x998)))+(((cj3)*(x997)*(x999)))+(((x1001)*(x999)))+(((x1006)*(x998)))+(((pz)*(x1000)))+(((x1006)*(x999))))))) < IKFAST_ATAN2_MAGTHRESH )
2939  continue;
2940 j2array[0]=IKatan2(((gconst6)*(((IkReal(-0.0982500000000000))+(((pz)*(x1003)))+(((IkReal(-1.00000000000000))*(x1000)*(x998)))+(((x1002)*(x998)))+(((pz)*(x1001)))+(((sj3)*(x1005)))+(((IkReal(-0.0245625000000000))*(sj1)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((pz)*(x1006)))+(((cj3)*(pz)*(x997)))+(((IkReal(-1.00000000000000))*(sj3)*(x997)*(x998)))+(((x1007)*(x999)))+(((x1007)*(x998)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x1000)*(x999)))+(((IkReal(-1.00000000000000))*(cj3)*(x1004)))+(((IkReal(-1.00000000000000))*(sj3)*(x997)*(x999)))+(((x1002)*(x999)))))), ((gconst6)*(((IkReal(-0.0225000000000000))+(((IkReal(-1.00000000000000))*(pz)*(x1007)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((pz)*(sj3)*(x997)))+(((IkReal(-1.00000000000000))*(pz)*(x1002)))+(((x1003)*(x998)))+(((x1003)*(x999)))+(((cj3)*(x997)*(x998)))+(((IkReal(-1.00000000000000))*(sj3)*(x1004)))+(((IkReal(-1.00000000000000))*(cj3)*(x1005)))+(((IkReal(-0.0245625000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x1001)*(x998)))+(((cj3)*(x997)*(x999)))+(((x1001)*(x999)))+(((x1006)*(x998)))+(((pz)*(x1000)))+(((x1006)*(x999)))))));
2941 sj2array[0]=IKsin(j2array[0]);
2942 cj2array[0]=IKcos(j2array[0]);
2943 if( j2array[0] > IKPI )
2944 {
2945  j2array[0]-=IK2PI;
2946 }
2947 else if( j2array[0] < -IKPI )
2948 { j2array[0]+=IK2PI;
2949 }
2950 j2valid[0] = true;
2951 for(int ij2 = 0; ij2 < 1; ++ij2)
2952 {
2953 if( !j2valid[ij2] )
2954 {
2955  continue;
2956 }
2957 _ij2[0] = ij2; _ij2[1] = -1;
2958 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2959 {
2960 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2961 {
2962  j2valid[iij2]=false; _ij2[1] = iij2; break;
2963 }
2964 }
2965 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2966 {
2967 IkReal evalcond[4];
2968 IkReal x1008=IKcos(j2);
2969 IkReal x1009=IKsin(j2);
2970 IkReal x1010=((IkReal(0.0800000000000000))*(sj3));
2971 IkReal x1011=((cj0)*(r01));
2972 IkReal x1012=((IkReal(1.00000000000000))*(px));
2973 IkReal x1013=((py)*(sj1));
2974 IkReal x1014=((cj0)*(r02));
2975 IkReal x1015=((IkReal(0.0750000000000000))*(cj1));
2976 IkReal x1016=((r02)*(sj0));
2977 IkReal x1017=((IkReal(0.0750000000000000))*(sj1));
2978 IkReal x1018=((cj1)*(pz));
2979 IkReal x1019=((r00)*(sj0));
2980 IkReal x1020=((IkReal(0.0800000000000000))*(cj3));
2981 IkReal x1021=((pz)*(sj1));
2982 IkReal x1022=((IkReal(1.00000000000000))*(sj0));
2983 IkReal x1023=((cj1)*(py));
2984 IkReal x1024=((IkReal(0.327500000000000))*(x1008));
2985 IkReal x1025=((IkReal(0.0750000000000000))*(x1009));
2986 IkReal x1026=((IkReal(0.0750000000000000))*(x1008));
2987 IkReal x1027=((IkReal(0.327500000000000))*(x1009));
2988 IkReal x1028=((sj1)*(x1019));
2989 IkReal x1029=((x1008)*(x1020));
2990 IkReal x1030=((x1009)*(x1010));
2991 IkReal x1031=((x1008)*(x1010));
2992 IkReal x1032=((x1009)*(x1020));
2993 IkReal x1033=((x1030)+(x1025));
2994 IkReal x1034=((x1024)+(x1029));
2995 IkReal x1035=((x1032)+(x1031)+(x1027)+(x1026));
2996 evalcond[0]=((x1034)+(((IkReal(-1.00000000000000))*(x1033)))+(((IkReal(-1.00000000000000))*(x1022)*(x1023)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x1012)))+(x1015)+(x1021));
2997 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x1013)*(x1022)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x1012)))+(x1035)+(((IkReal(-1.00000000000000))*(x1018)))+(x1017));
2998 evalcond[2]=((((x1013)*(x1014)))+(((IkReal(-1.00000000000000))*(x1011)*(x1015)))+(((IkReal(-1.00000000000000))*(r00)*(x1023)))+(((IkReal(-1.00000000000000))*(x1011)*(x1021)))+(x1034)+(((IkReal(-1.00000000000000))*(x1033)))+(((IkReal(-1.00000000000000))*(sj1)*(x1012)*(x1016)))+(((cj1)*(px)*(r01)))+(((x1019)*(x1021)))+(((x1015)*(x1019))));
2999 evalcond[3]=((((r00)*(x1013)))+(((IkReal(-1.00000000000000))*(x1017)*(x1019)))+(((x1011)*(x1017)))+(((IkReal(-1.00000000000000))*(cj1)*(x1012)*(x1016)))+(((IkReal(-1.00000000000000))*(x1011)*(x1018)))+(((x1014)*(x1023)))+(((IkReal(-0.300000000000000))*(x1019)))+(((IkReal(-1.00000000000000))*(x1035)))+(((IkReal(0.300000000000000))*(x1011)))+(((x1018)*(x1019)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x1012))));
3000 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
3001 {
3002 continue;
3003 }
3004 }
3005 
3006 {
3007 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
3008 vinfos[0].jointtype = 1;
3009 vinfos[0].foffset = j0;
3010 vinfos[0].indices[0] = _ij0[0];
3011 vinfos[0].indices[1] = _ij0[1];
3012 vinfos[0].maxsolutions = _nj0;
3013 vinfos[1].jointtype = 1;
3014 vinfos[1].foffset = j1;
3015 vinfos[1].indices[0] = _ij1[0];
3016 vinfos[1].indices[1] = _ij1[1];
3017 vinfos[1].maxsolutions = _nj1;
3018 vinfos[2].jointtype = 1;
3019 vinfos[2].foffset = j2;
3020 vinfos[2].indices[0] = _ij2[0];
3021 vinfos[2].indices[1] = _ij2[1];
3022 vinfos[2].maxsolutions = _nj2;
3023 vinfos[3].jointtype = 1;
3024 vinfos[3].foffset = j3;
3025 vinfos[3].indices[0] = _ij3[0];
3026 vinfos[3].indices[1] = _ij3[1];
3027 vinfos[3].maxsolutions = _nj3;
3028 vinfos[4].jointtype = 1;
3029 vinfos[4].foffset = j4;
3030 vinfos[4].indices[0] = _ij4[0];
3031 vinfos[4].indices[1] = _ij4[1];
3032 vinfos[4].maxsolutions = _nj4;
3033 std::vector<int> vfree(0);
3034 solutions.AddSolution(vinfos,vfree);
3035 }
3036 }
3037 }
3038 
3039 }
3040 
3041 }
3042 
3043 } else
3044 {
3045 if( 1 )
3046 {
3047 continue;
3048 
3049 } else
3050 {
3051 }
3052 }
3053 }
3054 }
3055 
3056 } else
3057 {
3058 {
3059 IkReal j2array[1], cj2array[1], sj2array[1];
3060 bool j2valid[1]={false};
3061 _nj2 = 1;
3062 IkReal x1036=((r02)*(sj1));
3063 IkReal x1037=((cj0)*(px));
3064 IkReal x1038=((IkReal(0.0800000000000000))*(sj3));
3065 IkReal x1039=((cj4)*(sj3));
3066 IkReal x1040=((pz)*(sj1));
3067 IkReal x1041=((IkReal(0.0800000000000000))*(cj3));
3068 IkReal x1042=((IkReal(1.00000000000000))*(cj1));
3069 IkReal x1043=((py)*(sj0));
3070 IkReal x1044=((cj1)*(r01)*(sj0));
3071 IkReal x1045=((cj1)*(cj3)*(cj4));
3072 IkReal x1046=((cj0)*(cj1)*(r00));
3073 IkReal x1047=((IkReal(0.0800000000000000))*(x1046));
3074 if( IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(x1039)*(x1042)*(x1043)))+(((x1039)*(x1040)))+(((IkReal(-0.327500000000000))*(x1046)))+(((IkReal(-1.00000000000000))*(x1041)*(x1044)))+(((IkReal(-0.327500000000000))*(x1044)))+(((x1036)*(x1041)))+(((IkReal(-1.00000000000000))*(x1037)*(x1039)*(x1042)))+(((IkReal(0.0750000000000000))*(cj1)*(x1039)))+(((IkReal(-1.00000000000000))*(x1041)*(x1046)))+(((IkReal(0.327500000000000))*(x1036))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((IkReal(0.0750000000000000))*(x1036)))+(((IkReal(-1.00000000000000))*(x1038)*(x1044)))+(((x1043)*(x1045)))+(((IkReal(-0.0750000000000000))*(x1044)))+(((IkReal(-0.0750000000000000))*(x1045)))+(((IkReal(-1.00000000000000))*(x1038)*(x1046)))+(((x1036)*(x1038)))+(((IkReal(-1.00000000000000))*(cj3)*(cj4)*(x1040)))+(((IkReal(-0.0750000000000000))*(x1046)))+(((x1037)*(x1045))))))) < IKFAST_ATAN2_MAGTHRESH )
3075  continue;
3076 j2array[0]=IKatan2(((gconst3)*(((((IkReal(-1.00000000000000))*(x1039)*(x1042)*(x1043)))+(((x1039)*(x1040)))+(((IkReal(-0.327500000000000))*(x1046)))+(((IkReal(-1.00000000000000))*(x1041)*(x1044)))+(((IkReal(-0.327500000000000))*(x1044)))+(((x1036)*(x1041)))+(((IkReal(-1.00000000000000))*(x1037)*(x1039)*(x1042)))+(((IkReal(0.0750000000000000))*(cj1)*(x1039)))+(((IkReal(-1.00000000000000))*(x1041)*(x1046)))+(((IkReal(0.327500000000000))*(x1036)))))), ((gconst3)*(((((IkReal(0.0750000000000000))*(x1036)))+(((IkReal(-1.00000000000000))*(x1038)*(x1044)))+(((x1043)*(x1045)))+(((IkReal(-0.0750000000000000))*(x1044)))+(((IkReal(-0.0750000000000000))*(x1045)))+(((IkReal(-1.00000000000000))*(x1038)*(x1046)))+(((x1036)*(x1038)))+(((IkReal(-1.00000000000000))*(cj3)*(cj4)*(x1040)))+(((IkReal(-0.0750000000000000))*(x1046)))+(((x1037)*(x1045)))))));
3077 sj2array[0]=IKsin(j2array[0]);
3078 cj2array[0]=IKcos(j2array[0]);
3079 if( j2array[0] > IKPI )
3080 {
3081  j2array[0]-=IK2PI;
3082 }
3083 else if( j2array[0] < -IKPI )
3084 { j2array[0]+=IK2PI;
3085 }
3086 j2valid[0] = true;
3087 for(int ij2 = 0; ij2 < 1; ++ij2)
3088 {
3089 if( !j2valid[ij2] )
3090 {
3091  continue;
3092 }
3093 _ij2[0] = ij2; _ij2[1] = -1;
3094 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
3095 {
3096 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
3097 {
3098  j2valid[iij2]=false; _ij2[1] = iij2; break;
3099 }
3100 }
3101 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
3102 {
3103 IkReal evalcond[8];
3104 IkReal x1048=IKcos(j2);
3105 IkReal x1049=IKsin(j2);
3106 IkReal x1050=(py)*(py);
3107 IkReal x1051=(px)*(px);
3108 IkReal x1052=(pz)*(pz);
3109 IkReal x1053=((sj0)*(sj1));
3110 IkReal x1054=((IkReal(1.00000000000000))*(r01));
3111 IkReal x1055=((r00)*(sj1));
3112 IkReal x1056=((IkReal(0.0750000000000000))*(r00));
3113 IkReal x1057=((IkReal(0.0491250000000000))*(sj3));
3114 IkReal x1058=((cj1)*(r01));
3115 IkReal x1059=((IkReal(0.150000000000000))*(py));
3116 IkReal x1060=((pz)*(r02));
3117 IkReal x1061=((px)*(r02));
3118 IkReal x1062=((IkReal(0.0750000000000000))*(sj4));
3119 IkReal x1063=((cj1)*(sj0));
3120 IkReal x1064=((pz)*(r00));
3121 IkReal x1065=((IkReal(0.0750000000000000))*(cj0));
3122 IkReal x1066=((IkReal(2.00000000000000))*(cj0));
3123 IkReal x1067=((cj1)*(r02));
3124 IkReal x1068=((IkReal(0.150000000000000))*(sj1));
3125 IkReal x1069=((px)*(r00));
3126 IkReal x1070=((IkReal(2.00000000000000))*(py));
3127 IkReal x1071=((r02)*(sj1));
3128 IkReal x1072=((IkReal(0.150000000000000))*(cj1));
3129 IkReal x1073=((IkReal(0.108031250000000))*(cj3));
3130 IkReal x1074=((cj0)*(py));
3131 IkReal x1075=((r01)*(sj1));
3132 IkReal x1076=((IkReal(0.0800000000000000))*(cj3));
3133 IkReal x1077=((IkReal(0.150000000000000))*(pz));
3134 IkReal x1078=((IkReal(0.0491250000000000))*(cj3));
3135 IkReal x1079=((cj1)*(pz));
3136 IkReal x1080=((IkReal(0.0952312500000000))*(sj3));
3137 IkReal x1081=((IkReal(0.600000000000000))*(py));
3138 IkReal x1082=((IkReal(1.00000000000000))*(py));
3139 IkReal x1083=((px)*(sj1));
3140 IkReal x1084=((IkReal(1.00000000000000))*(cj0));
3141 IkReal x1085=((cj0)*(r00));
3142 IkReal x1086=((IkReal(0.600000000000000))*(cj0));
3143 IkReal x1087=((IkReal(1.00000000000000))*(sj3));
3144 IkReal x1088=((pz)*(sj1));
3145 IkReal x1089=((IkReal(0.0800000000000000))*(sj3));
3146 IkReal x1090=((px)*(py));
3147 IkReal x1091=((cj1)*(r00));
3148 IkReal x1092=((r01)*(sj0));
3149 IkReal x1093=((cj1)*(px));
3150 IkReal x1094=((cj4)*(x1049));
3151 IkReal x1095=((cj4)*(x1048));
3152 IkReal x1096=((sj4)*(x1049));
3153 IkReal x1097=((IkReal(2.00000000000000))*(x1052));
3154 IkReal x1098=((sj4)*(x1048));
3155 IkReal x1099=((IkReal(2.00000000000000))*(x1050));
3156 IkReal x1100=((x1048)*(x1089));
3157 IkReal x1101=((px)*(x1060)*(x1066));
3158 evalcond[0]=((((IkReal(-1.00000000000000))*(x1084)*(x1091)))+(((IkReal(-1.00000000000000))*(x1087)*(x1095)))+(((IkReal(-1.00000000000000))*(x1054)*(x1063)))+(((IkReal(-1.00000000000000))*(cj3)*(x1094)))+(x1071));
3159 evalcond[1]=((((IkReal(-1.00000000000000))*(x1067)))+(((IkReal(-1.00000000000000))*(x1087)*(x1094)))+(((IkReal(-1.00000000000000))*(x1053)*(x1054)))+(((cj3)*(x1095)))+(((IkReal(-1.00000000000000))*(x1055)*(x1084))));
3160 evalcond[2]=((x1088)+(((IkReal(-0.0750000000000000))*(x1049)))+(((x1048)*(x1076)))+(((IkReal(-1.00000000000000))*(x1084)*(x1093)))+(((IkReal(0.327500000000000))*(x1048)))+(((IkReal(-1.00000000000000))*(x1049)*(x1089)))+(((IkReal(-1.00000000000000))*(x1063)*(x1082)))+(((IkReal(0.0750000000000000))*(cj1))));
3161 evalcond[3]=((IkReal(0.300000000000000))+(x1100)+(((IkReal(0.327500000000000))*(x1049)))+(((IkReal(-1.00000000000000))*(x1083)*(x1084)))+(((IkReal(-1.00000000000000))*(x1079)))+(((IkReal(0.0750000000000000))*(x1048)))+(((IkReal(-1.00000000000000))*(x1053)*(x1082)))+(((IkReal(0.0750000000000000))*(sj1)))+(((x1049)*(x1076))));
3162 evalcond[4]=((((IkReal(-1.00000000000000))*(x1076)*(x1098)))+(((x1089)*(x1096)))+(((x1049)*(x1062)))+(((x1053)*(x1064)))+(((x1071)*(x1074)))+(((IkReal(-1.00000000000000))*(x1082)*(x1091)))+(((IkReal(-1.00000000000000))*(x1058)*(x1065)))+(((x1056)*(x1063)))+(((px)*(x1058)))+(((IkReal(-1.00000000000000))*(cj0)*(x1054)*(x1088)))+(((IkReal(-0.327500000000000))*(x1098)))+(((IkReal(-1.00000000000000))*(x1053)*(x1061))));
3163 evalcond[5]=((((py)*(x1055)))+(((IkReal(-1.00000000000000))*(x1061)*(x1063)))+(((IkReal(-1.00000000000000))*(x1054)*(x1083)))+(((x1089)*(x1098)))+(((x1063)*(x1064)))+(((x1065)*(x1075)))+(((x1076)*(x1096)))+(((IkReal(-0.300000000000000))*(r00)*(sj0)))+(((IkReal(-1.00000000000000))*(x1053)*(x1056)))+(((IkReal(0.300000000000000))*(cj0)*(r01)))+(((IkReal(-1.00000000000000))*(cj0)*(x1054)*(x1079)))+(((x1048)*(x1062)))+(((x1067)*(x1074)))+(((IkReal(0.327500000000000))*(x1096))));
3164 evalcond[6]=((((IkReal(0.0450000000000000))*(x1092)))+(((r01)*(x1053)*(x1099)))+(((IkReal(-1.00000000000000))*(x1078)*(x1094)))+(((r02)*(x1059)*(x1063)))+(((IkReal(0.0450000000000000))*(x1085)))+(((IkReal(2.00000000000000))*(x1064)*(x1093)))+(((pz)*(x1058)*(x1070)))+(((x1060)*(x1066)*(x1083)))+(((IkReal(-1.00000000000000))*(r01)*(x1081)))+(((cj0)*(x1061)*(x1072)))+(((IkReal(-1.00000000000000))*(cj0)*(x1064)*(x1072)))+(((x1080)*(x1094)))+(((IkReal(-0.600000000000000))*(x1060)))+(((x1057)*(x1095)))+(((IkReal(-1.00000000000000))*(pp)*(x1055)*(x1084)))+(((IkReal(-0.600000000000000))*(x1069)))+(((IkReal(0.0956250000000000))*(r01)*(x1053)))+(((IkReal(0.0843750000000000))*(x1067)))+(((IkReal(-1.00000000000000))*(pp)*(x1053)*(x1054)))+(((IkReal(-1.00000000000000))*(x1060)*(x1068)))+(((IkReal(0.0524000000000000))*(x1095)))+(((x1051)*(x1055)*(x1066)))+(((IkReal(-1.00000000000000))*(sj0)*(x1058)*(x1077)))+(((IkReal(-0.0120000000000000))*(x1094)))+(((x1053)*(x1060)*(x1070)))+(((IkReal(-1.00000000000000))*(pp)*(x1067)))+(((IkReal(-1.00000000000000))*(x1059)*(x1075)))+(((x1067)*(x1097)))+(((x1073)*(x1095)))+(((x1053)*(x1069)*(x1070)))+(((IkReal(-0.150000000000000))*(px)*(x1055)))+(((x1066)*(x1075)*(x1090)))+(((IkReal(0.0956250000000000))*(cj0)*(x1055))));
3165 evalcond[7]=((((x1060)*(x1063)*(x1070)))+(((IkReal(-1.00000000000000))*(x1058)*(x1059)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1081)))+(((IkReal(0.0956250000000000))*(x1071)))+(((IkReal(-1.00000000000000))*(x1060)*(x1072)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(pz)*(x1070)*(x1075)))+(((x1051)*(x1066)*(x1091)))+(((IkReal(0.600000000000000))*(pz)*(x1092)))+(((x1060)*(x1066)*(x1093)))+(((r01)*(x1053)*(x1077)))+(((pp)*(x1071)))+(((x1063)*(x1069)*(x1070)))+(((IkReal(-0.0524000000000000))*(x1094)))+(((IkReal(-1.00000000000000))*(x1057)*(x1094)))+(((x1064)*(x1086)))+(((IkReal(-1.00000000000000))*(cj0)*(x1061)*(x1068)))+(((IkReal(-0.0843750000000000))*(cj1)*(x1085)))+(((IkReal(-1.00000000000000))*(x1078)*(x1095)))+(((IkReal(-1.00000000000000))*(x1073)*(x1094)))+(((IkReal(-1.00000000000000))*(x1069)*(x1072)))+(((cj0)*(x1055)*(x1077)))+(((IkReal(-2.00000000000000))*(px)*(pz)*(x1055)))+(((IkReal(-1.00000000000000))*(r02)*(x1053)*(x1059)))+(((x1080)*(x1095)))+(((sj0)*(x1058)*(x1099)))+(((IkReal(-1.00000000000000))*(x1061)*(x1086)))+(((IkReal(-1.00000000000000))*(pp)*(x1054)*(x1063)))+(((x1058)*(x1066)*(x1090)))+(((IkReal(-1.00000000000000))*(x1071)*(x1097)))+(((IkReal(-0.0120000000000000))*(x1095)))+(((IkReal(-0.0843750000000000))*(sj0)*(x1058)))+(((IkReal(-1.00000000000000))*(pp)*(x1084)*(x1091))));
3166 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 )
3167 {
3168 continue;
3169 }
3170 }
3171 
3172 {
3173 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
3174 vinfos[0].jointtype = 1;
3175 vinfos[0].foffset = j0;
3176 vinfos[0].indices[0] = _ij0[0];
3177 vinfos[0].indices[1] = _ij0[1];
3178 vinfos[0].maxsolutions = _nj0;
3179 vinfos[1].jointtype = 1;
3180 vinfos[1].foffset = j1;
3181 vinfos[1].indices[0] = _ij1[0];
3182 vinfos[1].indices[1] = _ij1[1];
3183 vinfos[1].maxsolutions = _nj1;
3184 vinfos[2].jointtype = 1;
3185 vinfos[2].foffset = j2;
3186 vinfos[2].indices[0] = _ij2[0];
3187 vinfos[2].indices[1] = _ij2[1];
3188 vinfos[2].maxsolutions = _nj2;
3189 vinfos[3].jointtype = 1;
3190 vinfos[3].foffset = j3;
3191 vinfos[3].indices[0] = _ij3[0];
3192 vinfos[3].indices[1] = _ij3[1];
3193 vinfos[3].maxsolutions = _nj3;
3194 vinfos[4].jointtype = 1;
3195 vinfos[4].foffset = j4;
3196 vinfos[4].indices[0] = _ij4[0];
3197 vinfos[4].indices[1] = _ij4[1];
3198 vinfos[4].maxsolutions = _nj4;
3199 std::vector<int> vfree(0);
3200 solutions.AddSolution(vinfos,vfree);
3201 }
3202 }
3203 }
3204 
3205 }
3206 
3207 }
3208 
3209 } else
3210 {
3211 {
3212 IkReal j2array[1], cj2array[1], sj2array[1];
3213 bool j2valid[1]={false};
3214 _nj2 = 1;
3215 IkReal x1102=((IkReal(1.00000000000000))*(cj1));
3216 IkReal x1103=((cj0)*(r00));
3217 IkReal x1104=((cj3)*(r02));
3218 IkReal x1105=((sj1)*(sj3));
3219 IkReal x1106=((r01)*(sj0));
3220 IkReal x1107=((cj3)*(x1106));
3221 if( IKabs(((gconst2)*(((((sj1)*(x1104)))+(((IkReal(-1.00000000000000))*(x1103)*(x1105)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x1102)))+(((IkReal(-1.00000000000000))*(x1105)*(x1106)))+(((IkReal(-1.00000000000000))*(cj3)*(x1102)*(x1103)))+(((IkReal(-1.00000000000000))*(x1102)*(x1107))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((sj1)*(x1107)))+(((r02)*(x1105)))+(((cj1)*(x1104)))+(((cj3)*(sj1)*(x1103)))+(((IkReal(-1.00000000000000))*(sj3)*(x1102)*(x1103)))+(((IkReal(-1.00000000000000))*(sj3)*(x1102)*(x1106))))))) < IKFAST_ATAN2_MAGTHRESH )
3222  continue;
3223 j2array[0]=IKatan2(((gconst2)*(((((sj1)*(x1104)))+(((IkReal(-1.00000000000000))*(x1103)*(x1105)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x1102)))+(((IkReal(-1.00000000000000))*(x1105)*(x1106)))+(((IkReal(-1.00000000000000))*(cj3)*(x1102)*(x1103)))+(((IkReal(-1.00000000000000))*(x1102)*(x1107)))))), ((gconst2)*(((((sj1)*(x1107)))+(((r02)*(x1105)))+(((cj1)*(x1104)))+(((cj3)*(sj1)*(x1103)))+(((IkReal(-1.00000000000000))*(sj3)*(x1102)*(x1103)))+(((IkReal(-1.00000000000000))*(sj3)*(x1102)*(x1106)))))));
3224 sj2array[0]=IKsin(j2array[0]);
3225 cj2array[0]=IKcos(j2array[0]);
3226 if( j2array[0] > IKPI )
3227 {
3228  j2array[0]-=IK2PI;
3229 }
3230 else if( j2array[0] < -IKPI )
3231 { j2array[0]+=IK2PI;
3232 }
3233 j2valid[0] = true;
3234 for(int ij2 = 0; ij2 < 1; ++ij2)
3235 {
3236 if( !j2valid[ij2] )
3237 {
3238  continue;
3239 }
3240 _ij2[0] = ij2; _ij2[1] = -1;
3241 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
3242 {
3243 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
3244 {
3245  j2valid[iij2]=false; _ij2[1] = iij2; break;
3246 }
3247 }
3248 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
3249 {
3250 IkReal evalcond[8];
3251 IkReal x1108=IKcos(j2);
3252 IkReal x1109=IKsin(j2);
3253 IkReal x1110=(py)*(py);
3254 IkReal x1111=(px)*(px);
3255 IkReal x1112=(pz)*(pz);
3256 IkReal x1113=((sj0)*(sj1));
3257 IkReal x1114=((IkReal(1.00000000000000))*(r01));
3258 IkReal x1115=((r00)*(sj1));
3259 IkReal x1116=((IkReal(0.0750000000000000))*(r00));
3260 IkReal x1117=((IkReal(0.0491250000000000))*(sj3));
3261 IkReal x1118=((cj1)*(r01));
3262 IkReal x1119=((IkReal(0.150000000000000))*(py));
3263 IkReal x1120=((pz)*(r02));
3264 IkReal x1121=((px)*(r02));
3265 IkReal x1122=((IkReal(0.0750000000000000))*(sj4));
3266 IkReal x1123=((cj1)*(sj0));
3267 IkReal x1124=((pz)*(r00));
3268 IkReal x1125=((IkReal(0.0750000000000000))*(cj0));
3269 IkReal x1126=((IkReal(2.00000000000000))*(cj0));
3270 IkReal x1127=((cj1)*(r02));
3271 IkReal x1128=((IkReal(0.150000000000000))*(sj1));
3272 IkReal x1129=((px)*(r00));
3273 IkReal x1130=((IkReal(2.00000000000000))*(py));
3274 IkReal x1131=((r02)*(sj1));
3275 IkReal x1132=((IkReal(0.150000000000000))*(cj1));
3276 IkReal x1133=((IkReal(0.108031250000000))*(cj3));
3277 IkReal x1134=((cj0)*(py));
3278 IkReal x1135=((r01)*(sj1));
3279 IkReal x1136=((IkReal(0.0800000000000000))*(cj3));
3280 IkReal x1137=((IkReal(0.150000000000000))*(pz));
3281 IkReal x1138=((IkReal(0.0491250000000000))*(cj3));
3282 IkReal x1139=((cj1)*(pz));
3283 IkReal x1140=((IkReal(0.0952312500000000))*(sj3));
3284 IkReal x1141=((IkReal(0.600000000000000))*(py));
3285 IkReal x1142=((IkReal(1.00000000000000))*(py));
3286 IkReal x1143=((px)*(sj1));
3287 IkReal x1144=((IkReal(1.00000000000000))*(cj0));
3288 IkReal x1145=((cj0)*(r00));
3289 IkReal x1146=((IkReal(0.600000000000000))*(cj0));
3290 IkReal x1147=((IkReal(1.00000000000000))*(sj3));
3291 IkReal x1148=((pz)*(sj1));
3292 IkReal x1149=((IkReal(0.0800000000000000))*(sj3));
3293 IkReal x1150=((px)*(py));
3294 IkReal x1151=((cj1)*(r00));
3295 IkReal x1152=((r01)*(sj0));
3296 IkReal x1153=((cj1)*(px));
3297 IkReal x1154=((cj4)*(x1109));
3298 IkReal x1155=((cj4)*(x1108));
3299 IkReal x1156=((sj4)*(x1109));
3300 IkReal x1157=((IkReal(2.00000000000000))*(x1112));
3301 IkReal x1158=((sj4)*(x1108));
3302 IkReal x1159=((IkReal(2.00000000000000))*(x1110));
3303 IkReal x1160=((x1108)*(x1149));
3304 IkReal x1161=((px)*(x1120)*(x1126));
3305 evalcond[0]=((((IkReal(-1.00000000000000))*(x1147)*(x1155)))+(((IkReal(-1.00000000000000))*(cj3)*(x1154)))+(x1131)+(((IkReal(-1.00000000000000))*(x1144)*(x1151)))+(((IkReal(-1.00000000000000))*(x1114)*(x1123))));
3306 evalcond[1]=((((IkReal(-1.00000000000000))*(x1127)))+(((cj3)*(x1155)))+(((IkReal(-1.00000000000000))*(x1113)*(x1114)))+(((IkReal(-1.00000000000000))*(x1147)*(x1154)))+(((IkReal(-1.00000000000000))*(x1115)*(x1144))));
3307 evalcond[2]=((((IkReal(-0.0750000000000000))*(x1109)))+(x1148)+(((IkReal(-1.00000000000000))*(x1144)*(x1153)))+(((x1108)*(x1136)))+(((IkReal(-1.00000000000000))*(x1123)*(x1142)))+(((IkReal(-1.00000000000000))*(x1109)*(x1149)))+(((IkReal(0.327500000000000))*(x1108)))+(((IkReal(0.0750000000000000))*(cj1))));
3308 evalcond[3]=((IkReal(0.300000000000000))+(x1160)+(((IkReal(-1.00000000000000))*(x1139)))+(((IkReal(-1.00000000000000))*(x1113)*(x1142)))+(((IkReal(-1.00000000000000))*(x1143)*(x1144)))+(((IkReal(0.327500000000000))*(x1109)))+(((IkReal(0.0750000000000000))*(x1108)))+(((IkReal(0.0750000000000000))*(sj1)))+(((x1109)*(x1136))));
3309 evalcond[4]=((((x1131)*(x1134)))+(((x1116)*(x1123)))+(((IkReal(-1.00000000000000))*(x1136)*(x1158)))+(((IkReal(-1.00000000000000))*(cj0)*(x1114)*(x1148)))+(((x1149)*(x1156)))+(((x1109)*(x1122)))+(((x1113)*(x1124)))+(((IkReal(-0.327500000000000))*(x1158)))+(((IkReal(-1.00000000000000))*(x1142)*(x1151)))+(((IkReal(-1.00000000000000))*(x1118)*(x1125)))+(((IkReal(-1.00000000000000))*(x1113)*(x1121)))+(((px)*(x1118))));
3310 evalcond[5]=((((IkReal(-1.00000000000000))*(cj0)*(x1114)*(x1139)))+(((x1125)*(x1135)))+(((x1136)*(x1156)))+(((IkReal(-1.00000000000000))*(x1121)*(x1123)))+(((IkReal(-0.300000000000000))*(r00)*(sj0)))+(((x1108)*(x1122)))+(((py)*(x1115)))+(((x1123)*(x1124)))+(((IkReal(-1.00000000000000))*(x1113)*(x1116)))+(((IkReal(-1.00000000000000))*(x1114)*(x1143)))+(((x1127)*(x1134)))+(((IkReal(0.300000000000000))*(cj0)*(r01)))+(((IkReal(0.327500000000000))*(x1156)))+(((x1149)*(x1158))));
3311 evalcond[6]=((((x1133)*(x1155)))+(((IkReal(-0.600000000000000))*(x1129)))+(((pz)*(x1118)*(x1130)))+(((IkReal(-1.00000000000000))*(pp)*(x1115)*(x1144)))+(((IkReal(2.00000000000000))*(x1124)*(x1153)))+(((IkReal(0.0956250000000000))*(cj0)*(x1115)))+(((x1126)*(x1135)*(x1150)))+(((r02)*(x1119)*(x1123)))+(((IkReal(0.0450000000000000))*(x1145)))+(((IkReal(0.0843750000000000))*(x1127)))+(((IkReal(-1.00000000000000))*(pp)*(x1113)*(x1114)))+(((IkReal(-1.00000000000000))*(sj0)*(x1118)*(x1137)))+(((IkReal(-1.00000000000000))*(x1138)*(x1154)))+(((IkReal(0.0450000000000000))*(x1152)))+(((IkReal(-0.0120000000000000))*(x1154)))+(((IkReal(-1.00000000000000))*(cj0)*(x1124)*(x1132)))+(((IkReal(0.0524000000000000))*(x1155)))+(((x1120)*(x1126)*(x1143)))+(((r01)*(x1113)*(x1159)))+(((IkReal(-0.150000000000000))*(px)*(x1115)))+(((x1117)*(x1155)))+(((IkReal(-1.00000000000000))*(pp)*(x1127)))+(((x1113)*(x1120)*(x1130)))+(((IkReal(-0.600000000000000))*(x1120)))+(((IkReal(-1.00000000000000))*(x1119)*(x1135)))+(((x1127)*(x1157)))+(((x1113)*(x1129)*(x1130)))+(((x1111)*(x1115)*(x1126)))+(((cj0)*(x1121)*(x1132)))+(((IkReal(-1.00000000000000))*(x1120)*(x1128)))+(((IkReal(-1.00000000000000))*(r01)*(x1141)))+(((x1140)*(x1154)))+(((IkReal(0.0956250000000000))*(r01)*(x1113))));
3312 evalcond[7]=((((IkReal(0.0956250000000000))*(x1131)))+(((x1120)*(x1123)*(x1130)))+(((x1120)*(x1126)*(x1153)))+(((IkReal(-1.00000000000000))*(x1129)*(x1132)))+(((IkReal(-1.00000000000000))*(pp)*(x1144)*(x1151)))+(((x1118)*(x1126)*(x1150)))+(((IkReal(-1.00000000000000))*(pz)*(x1130)*(x1135)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(0.600000000000000))*(pz)*(x1152)))+(((IkReal(-1.00000000000000))*(r02)*(x1113)*(x1119)))+(((IkReal(-1.00000000000000))*(x1118)*(x1119)))+(((cj0)*(x1115)*(x1137)))+(((x1111)*(x1126)*(x1151)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1141)))+(((pp)*(x1131)))+(((IkReal(-1.00000000000000))*(x1121)*(x1146)))+(((IkReal(-1.00000000000000))*(x1131)*(x1157)))+(((IkReal(-0.0843750000000000))*(sj0)*(x1118)))+(((IkReal(-1.00000000000000))*(x1120)*(x1132)))+(((x1124)*(x1146)))+(((sj0)*(x1118)*(x1159)))+(((IkReal(-1.00000000000000))*(x1138)*(x1155)))+(((IkReal(-0.0120000000000000))*(x1155)))+(((IkReal(-1.00000000000000))*(pp)*(x1114)*(x1123)))+(((IkReal(-1.00000000000000))*(x1133)*(x1154)))+(((x1123)*(x1129)*(x1130)))+(((r01)*(x1113)*(x1137)))+(((IkReal(-1.00000000000000))*(cj0)*(x1121)*(x1128)))+(((IkReal(-0.0524000000000000))*(x1154)))+(((IkReal(-2.00000000000000))*(px)*(pz)*(x1115)))+(((x1140)*(x1155)))+(((IkReal(-1.00000000000000))*(x1117)*(x1154)))+(((IkReal(-0.0843750000000000))*(cj1)*(x1145))));
3313 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 )
3314 {
3315 continue;
3316 }
3317 }
3318 
3319 {
3320 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
3321 vinfos[0].jointtype = 1;
3322 vinfos[0].foffset = j0;
3323 vinfos[0].indices[0] = _ij0[0];
3324 vinfos[0].indices[1] = _ij0[1];
3325 vinfos[0].maxsolutions = _nj0;
3326 vinfos[1].jointtype = 1;
3327 vinfos[1].foffset = j1;
3328 vinfos[1].indices[0] = _ij1[0];
3329 vinfos[1].indices[1] = _ij1[1];
3330 vinfos[1].maxsolutions = _nj1;
3331 vinfos[2].jointtype = 1;
3332 vinfos[2].foffset = j2;
3333 vinfos[2].indices[0] = _ij2[0];
3334 vinfos[2].indices[1] = _ij2[1];
3335 vinfos[2].maxsolutions = _nj2;
3336 vinfos[3].jointtype = 1;
3337 vinfos[3].foffset = j3;
3338 vinfos[3].indices[0] = _ij3[0];
3339 vinfos[3].indices[1] = _ij3[1];
3340 vinfos[3].maxsolutions = _nj3;
3341 vinfos[4].jointtype = 1;
3342 vinfos[4].foffset = j4;
3343 vinfos[4].indices[0] = _ij4[0];
3344 vinfos[4].indices[1] = _ij4[1];
3345 vinfos[4].maxsolutions = _nj4;
3346 std::vector<int> vfree(0);
3347 solutions.AddSolution(vinfos,vfree);
3348 }
3349 }
3350 }
3351 
3352 }
3353 
3354 }
3355 }
3356 }
3357 
3358 }
3359 
3360 }
3361 }
3362 }
3363  }
3364 }
3365 }
3366 }
3367 return solutions.GetNumSolutions()>0;
3368 }
3369 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
3370 {
3371  using std::complex;
3372  IKFAST_ASSERT(rawcoeffs[0] != 0);
3373  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
3374  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
3375  complex<IkReal> coeffs[4];
3376  const int maxsteps = 110;
3377  for(int i = 0; i < 4; ++i) {
3378  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
3379  }
3380  complex<IkReal> roots[4];
3381  IkReal err[4];
3382  roots[0] = complex<IkReal>(1,0);
3383  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
3384  err[0] = 1.0;
3385  err[1] = 1.0;
3386  for(int i = 2; i < 4; ++i) {
3387  roots[i] = roots[i-1]*roots[1];
3388  err[i] = 1.0;
3389  }
3390  for(int step = 0; step < maxsteps; ++step) {
3391  bool changed = false;
3392  for(int i = 0; i < 4; ++i) {
3393  if ( err[i] >= tol ) {
3394  changed = true;
3395  // evaluate
3396  complex<IkReal> x = roots[i] + coeffs[0];
3397  for(int j = 1; j < 4; ++j) {
3398  x = roots[i] * x + coeffs[j];
3399  }
3400  for(int j = 0; j < 4; ++j) {
3401  if( i != j ) {
3402  if( roots[i] != roots[j] ) {
3403  x /= (roots[i] - roots[j]);
3404  }
3405  }
3406  }
3407  roots[i] -= x;
3408  err[i] = abs(x);
3409  }
3410  }
3411  if( !changed ) {
3412  break;
3413  }
3414  }
3415 
3416  numroots = 0;
3417  bool visited[4] = {false};
3418  for(int i = 0; i < 4; ++i) {
3419  if( !visited[i] ) {
3420  // might be a multiple root, in which case it will have more error than the other roots
3421  // find any neighboring roots, and take the average
3422  complex<IkReal> newroot=roots[i];
3423  int n = 1;
3424  for(int j = i+1; j < 4; ++j) {
3425  if( abs(roots[i]-roots[j]) < 8*tolsqrt ) {
3426  newroot += roots[j];
3427  n += 1;
3428  visited[j] = true;
3429  }
3430  }
3431  if( n > 1 ) {
3432  newroot /= n;
3433  }
3434  // 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
3435  if( IKabs(imag(newroot)) < tolsqrt ) {
3436  rawroots[numroots++] = real(newroot);
3437  }
3438  }
3439  }
3440 }
3441 };
3442 
3443 
3446 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
3447 IKSolver solver;
3448 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
3449 }
3450 
3451 IKFAST_API const char* GetKinematicsHash() { return "76eda3aa6f0ad735cb91d01eee207a60"; }
3452 
3453 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
3454 
3455 #ifdef IKFAST_NAMESPACE
3456 } // end namespace
3457 #endif
3458 
3459 #ifndef IKFAST_NO_MAIN
3460 #include <stdio.h>
3461 #include <stdlib.h>
3462 #ifdef IKFAST_NAMESPACE
3463 using namespace IKFAST_NAMESPACE;
3464 #endif
3465 int main(int argc, char** argv)
3466 {
3467  if( argc != 12+GetNumFreeParameters()+1 ) {
3468  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
3469  "Returns the ik solutions given the transformation of the end effector specified by\n"
3470  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
3471  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
3472  return 1;
3473  }
3474 
3475  IkSolutionList<IkReal> solutions;
3476  std::vector<IkReal> vfree(GetNumFreeParameters());
3477  IkReal eerot[9],eetrans[3];
3478  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
3479  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
3480  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
3481  for(std::size_t i = 0; i < vfree.size(); ++i)
3482  vfree[i] = atof(argv[13+i]);
3483  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
3484 
3485  if( !bSuccess ) {
3486  fprintf(stderr,"Failed to get ik solution\n");
3487  return -1;
3488  }
3489 
3490  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
3491  std::vector<IkReal> solvalues(GetNumJoints());
3492  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
3493  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
3494  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
3495  std::vector<IkReal> vsolfree(sol.GetFree().size());
3496  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
3497  for( std::size_t j = 0; j < solvalues.size(); ++j)
3498  printf("%.15f, ", solvalues[j]);
3499  printf("\n");
3500  }
3501  return 0;
3502 }
3503 
3504 #endif
IK2PI
#define IK2PI
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:62
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: lrmate200ic5f_kinematics/include/ikfast.h:239
main
int main(int argc, char **argv)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:3465
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
IKcos
float IKcos(float f)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:161
log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: lrmate200ic5f_kinematics/include/ikfast.h:249
IKsqrt
float IKsqrt(float f)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:165
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:210
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:250
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: lrmate200ic5f_kinematics/include/ikfast.h:65
IKsign
float IKsign(float f)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:188
IKsin
float IKsin(float f)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:159
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:3451
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:3446
IKasin
float IKasin(float f)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:112
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...
GetIkType
IKFAST_API int GetIkType()
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:255
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:109
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
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
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:249
atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
IKtan
float IKtan(float f)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:163
IKatan2
float IKatan2(float fy, float fx)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:167
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
IKlog
float IKlog(float f)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:94
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:104
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: lrmate200ic5f_kinematics/include/ikfast.h:41
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:52
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
IKabs
float IKabs(float f)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:88
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:262
IKSolver::polyroots4
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:3369
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:99
IKPI
#define IKPI
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:63
step
unsigned int step
IKfmod
float IKfmod(float x, float y)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:128
acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:253
IKsqr
float IKsqr(float f)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:91
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: lrmate200ic5f_kinematics/include/ikfast.h:229
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:3453
dgeev_
void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi, double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info)
IKFAST_STRINGIZE
#define IKFAST_STRINGIZE(s)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:35
IKacos
float IKacos(float f)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:145
cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
tan
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
IKPI_2
#define IKPI_2
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:64
x
double x
dgetrs_
void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:251
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: fanuc_lrmate200ic5f_manipulator_ikfast_solver.cpp:25

fanuc_lrmate200ic_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Sun Apr 4 2021 02:20:33