fanuc_lrmate200ic5h_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[0]);
218 x6=IKsin(j[3]);
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))*(x5));
224 x12=((IkReal(0.320000000000000))*(x0));
225 x13=((IkReal(0.0800000000000000))*(x1));
226 x14=((IkReal(1.00000000000000))*(x0));
227 x15=((IkReal(1.00000000000000))*(x5));
228 x16=((IkReal(0.0800000000000000))*(x0));
229 x17=((IkReal(0.0750000000000000))*(x5));
230 x18=((x3)*(x4));
231 x19=((x1)*(x5));
232 x20=((x2)*(x3));
233 x21=((x1)*(x4));
234 x22=((x1)*(x2));
235 x23=((IkReal(0.0750000000000000))*(x22));
236 x24=((x1)*(x11));
237 IkReal x25=((x0)*(x13));
238 IkReal x26=((IkReal(1.00000000000000))*(x18));
239 eetrans[0]=((((x10)*(x22)))+(x10)+(((x6)*(((((IkReal(-1.00000000000000))*(x16)*(x26)))+(((x2)*(x25)))))))+(((x12)*(x20)))+(((IkReal(0.300000000000000))*(x0)*(x1)))+(((x12)*(x21)))+(((x7)*(((((x16)*(x20)))+(((x25)*(x4)))))))+(((IkReal(-1.00000000000000))*(x10)*(x26))));
240 IkReal x27=((IkReal(1.00000000000000))*(x18));
241 eetrans[1]=((((IkReal(0.320000000000000))*(x19)*(x4)))+(((IkReal(0.320000000000000))*(x20)*(x5)))+(((x6)*(((((IkReal(-1.00000000000000))*(x11)*(x27)))+(((x11)*(x22)))))))+(((IkReal(-1.00000000000000))*(x17)*(x27)))+(((IkReal(0.300000000000000))*(x19)))+(x17)+(((x17)*(x22)))+(((x7)*(((((x11)*(x21)))+(((x11)*(x20))))))));
242 eetrans[2]=((IkReal(0.330000000000000))+(((x6)*(((((x13)*(x4)))+(((IkReal(0.0800000000000000))*(x20)))))))+(((IkReal(0.320000000000000))*(x18)))+(((IkReal(0.0750000000000000))*(x21)))+(((IkReal(0.300000000000000))*(x3)))+(((IkReal(-0.320000000000000))*(x22)))+(((x7)*(((((IkReal(0.0800000000000000))*(x18)))+(((IkReal(-1.00000000000000))*(x13)*(x2)))))))+(((IkReal(0.0750000000000000))*(x20))));
243 IkReal x28=((IkReal(1.00000000000000))*(x14));
244 eerot[0]=((((x8)*(((((x6)*(((((IkReal(-1.00000000000000))*(x21)*(x28)))+(((IkReal(-1.00000000000000))*(x20)*(x28)))))))+(((x7)*(((((IkReal(-1.00000000000000))*(x18)*(x28)))+(((x0)*(x22)))))))))))+(((IkReal(-1.00000000000000))*(x15)*(x9))));
245 IkReal x29=((IkReal(1.00000000000000))*(x15));
246 eerot[1]=((((x8)*(((((x6)*(((((IkReal(-1.00000000000000))*(x20)*(x29)))+(((IkReal(-1.00000000000000))*(x21)*(x29)))))))+(((x7)*(((((IkReal(-1.00000000000000))*(x18)*(x29)))+(((x19)*(x2)))))))))))+(((x0)*(x9))));
247 eerot[2]=((x8)*(((((x6)*(((((IkReal(-1.00000000000000))*(x18)))+(((IkReal(1.00000000000000))*(x22)))))))+(((x7)*(((x20)+(x21))))))));
248 }
249 
250 IKFAST_API int GetNumFreeParameters() { return 0; }
251 IKFAST_API int* GetFreeParameters() { return NULL; }
252 IKFAST_API int GetNumJoints() { return 5; }
253 
254 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
255 
256 IKFAST_API int GetIkType() { return 0x56000007; }
257 
258 class IKSolver {
259 public:
260 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;
261 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4;
262 
263 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
264 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;
265 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
266  solutions.Clear();
267 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
268 
269 r00 = eerot[0];
270 r01 = eerot[1];
271 r02 = eerot[2];
272 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
273 new_r00=r00;
274 new_px=px;
275 new_r01=r01;
276 new_py=py;
277 new_r02=r02;
278 new_pz=((IkReal(-0.330000000000000))+(pz));
279 r00 = new_r00; r01 = new_r01; r02 = new_r02; px = new_px; py = new_py; pz = new_pz;
280 
281 pp=(((px)*(px))+((pz)*(pz))+((py)*(py)));
282 {
283 IkReal j0array[2], cj0array[2], sj0array[2];
284 bool j0valid[2]={false};
285 _nj0 = 2;
286 if( IKabs(((IkReal(-1.00000000000000))*(py))) < IKFAST_ATAN2_MAGTHRESH && IKabs(px) < IKFAST_ATAN2_MAGTHRESH )
287  continue;
288 IkReal x30=IKatan2(((IkReal(-1.00000000000000))*(py)), px);
289 j0array[0]=((IkReal(-1.00000000000000))*(x30));
290 sj0array[0]=IKsin(j0array[0]);
291 cj0array[0]=IKcos(j0array[0]);
292 j0array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x30))));
293 sj0array[1]=IKsin(j0array[1]);
294 cj0array[1]=IKcos(j0array[1]);
295 if( j0array[0] > IKPI )
296 {
297  j0array[0]-=IK2PI;
298 }
299 else if( j0array[0] < -IKPI )
300 { j0array[0]+=IK2PI;
301 }
302 j0valid[0] = true;
303 if( j0array[1] > IKPI )
304 {
305  j0array[1]-=IK2PI;
306 }
307 else if( j0array[1] < -IKPI )
308 { j0array[1]+=IK2PI;
309 }
310 j0valid[1] = true;
311 for(int ij0 = 0; ij0 < 2; ++ij0)
312 {
313 if( !j0valid[ij0] )
314 {
315  continue;
316 }
317 _ij0[0] = ij0; _ij0[1] = -1;
318 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
319 {
320 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
321 {
322  j0valid[iij0]=false; _ij0[1] = iij0; break;
323 }
324 }
325 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
326 
327 IkReal op[4+1], zeror[4];
328 int numroots;
329 op[0]=((((IkReal(-2.40000000000000))*(px)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-0.313500000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.00564000000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.00180000000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(1.20000000000000))*(pp)*(px)*(r00)*(r02)))+(((IkReal(-2.40000000000000))*(pz)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.644800000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((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.0225600000000000))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.20000000000000))*(cj0)*(pp)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-0.0225600000000000))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(0.00720000000000000))*(pz)*((r02)*(r02))))+(((IkReal(-4.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-4.80000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.372000000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.0225600000000000))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(0.180000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-0.00564000000000000))*(cj0)*(px)*((r02)*(r02))))+(((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.0496000000000000))*((cj0)*(cj0))*((px)*(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.00720000000000000))*(py)*(r01)*(r02)))+(((IkReal(-0.600000000000000))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.644800000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(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(-4.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.600000000000000))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.600000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(0.00453600000000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(-2.40000000000000))*(cj0)*(pp)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.744000000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-1.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(-0.765000000000000))*(px)*(py)*(r00)*(r01)))+(((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.744000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((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(2.40000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.00180000000000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-0.372000000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((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(0.744000000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((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(-8.00000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.00720000000000000))*(px)*(r00)*(r02)))+(((IkReal(-0.180000000000000))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(-0.0345000000000000))*(pp)*((r02)*(r02))))+(((IkReal(0.000108000000000000))*((r02)*(r02))))+(((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(-0.00180000000000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(1.20000000000000))*(pp)*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.0451200000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((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.00180000000000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.744000000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((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.744000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(0.0992000000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-4.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.644800000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-8.00000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(2.40000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-0.696000000000000))*(py)*(pz)*(r01)*(r02)))+(((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(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.0225600000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(1.20000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(0.00226800000000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(2.40000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(0.180000000000000))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(-8.00000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(0.0496000000000000))*((py)*(py))*((r02)*(r02))*((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.382500000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(-2.40000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.300000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((IkReal(0.0496000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.0992000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.300000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(0.0496000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(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.696000000000000))*(px)*(pz)*(r00)*(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.644800000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.180000000000000))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(0.744000000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((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.744000000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.744000000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0225600000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(0.180000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(2.40000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((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(0.00226800000000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-4.80000000000000))*(px)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-0.0225600000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.744000000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00)))));
330 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.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.00216000000000000))*(cj0)*(r00)*(r01)*(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.00108000000000000))*((cj0)*(cj0))*((r00)*(r00))))+(((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(0.0126000000000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-0.0181200000000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(0.0126000000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(-1.44000000000000))*(pp)*(r01)*(r02)*(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.360000000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((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(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.0126000000000000))*(py)*(sj0)*((r01)*(r01))))+(((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.0181200000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.360000000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.0126000000000000))*(cj0)*(px)*((r00)*(r00))))+(((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.00108000000000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.44000000000000))*(cj0)*(r00)*(r02)*((px)*(px))))+(((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(-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.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(4.80000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.216000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.108000000000000))*(pz)*((r02)*(r02))))+(((IkReal(0.00338400000000000))*((r02)*(r02))))+(((IkReal(0.180000000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(1.44000000000000))*(cj0)*(px)*(pz)*((r00)*(r00)))));
331 op[2]=((((IkReal(-16.0000000000000))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(1.20000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(1.20000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(8.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-1.39200000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((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(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(-1.50900000000000))*(pp)*((r02)*(r02))))+(((IkReal(0.600000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(8.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.00768000000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(-0.0255600000000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(0.0992000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.39200000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-2.00000000000000))*((pp)*(pp))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.39200000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(1.20000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-2.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(0.0992000000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-1.39200000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(1.39200000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(0.696000000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(1.20000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(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(0.204720000000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(-1.39200000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-32.0000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.0992000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-2.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(0.212400000000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(0.00133200000000000))*((r02)*(r02))))+(((IkReal(2.25300000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.198400000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((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(1.20000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-1.59040000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-1.59040000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.765000000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(1.20000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.212400000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(0.212400000000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(1.20000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-1.53000000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(1.48800000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.204720000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(0.198400000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(2.40000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(-0.765000000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(-1.39200000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-16.0000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((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(0.212400000000000))*(px)*(r00)*(r01)*(sj0)))+(((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(1.48800000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(-0.600000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-0.0127800000000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-32.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((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(-8.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(0.0992000000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(8.00000000000000))*(pp)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-1.59040000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((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(-8.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.20000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(0.696000000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-1.20000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-0.0127800000000000))*((r01)*(r01))*((sj0)*(sj0))))+(((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(-1.59040000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(2.40000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-1.39200000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-1.39200000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(8.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(1.20000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02))));
332 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(-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.00216000000000000))*(cj0)*(r00)*(r01)*(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.00108000000000000))*((cj0)*(cj0))*((r00)*(r00))))+(((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(0.0126000000000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(-0.0181200000000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(0.0126000000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(-1.44000000000000))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(0.108000000000000))*(pz)*((r01)*(r01))*((sj0)*(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.360000000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(4.80000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((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(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.0126000000000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.00921600000000000))*(cj0)*(r00)*(r02)))+(((IkReal(-1.44000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.0307200000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(-0.0181200000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.108000000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.360000000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.0126000000000000))*(cj0)*(px)*((r00)*(r00))))+(((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(-1.44000000000000))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.00108000000000000))*((r01)*(r01))*((sj0)*(sj0))))+(((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(-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.00921600000000000))*(r01)*(r02)*(sj0)))+(((IkReal(0.360000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(-0.360000000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.108000000000000))*(pz)*((r02)*(r02))))+(((IkReal(4.80000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.00338400000000000))*((r02)*(r02))))+(((IkReal(-1.44000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-1.44000000000000))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(0.180000000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00)))));
333 op[4]=((((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-0.313500000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-1.20000000000000))*(pp)*(py)*(r01)*(r02)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.00564000000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.00180000000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(2.40000000000000))*(px)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(0.644800000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((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.00720000000000000))*(px)*(r00)*(r02)))+(((IkReal(0.600000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(-0.372000000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-2.40000000000000))*(pz)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.00564000000000000))*(cj0)*(px)*((r02)*(r02))))+(((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.0496000000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-1.20000000000000))*(pp)*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((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(0.644800000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((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.180000000000000))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(0.0225600000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-4.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((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.00453600000000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(-1.20000000000000))*(pp)*(pz)*((r02)*(r02))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.744000000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((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.600000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(2.40000000000000))*((pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(0.744000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((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(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.00180000000000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-0.372000000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(0.600000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.0225600000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(4.80000000000000))*(px)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(4.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.00384000000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(-0.600000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-0.180000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.744000000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((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.00720000000000000))*(pz)*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-1.20000000000000))*(cj0)*(pp)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-2.40000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.0345000000000000))*(pp)*((r02)*(r02))))+(((IkReal(0.000108000000000000))*((r02)*(r02))))+(((IkReal(-0.180000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((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.00180000000000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(0.600000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.300000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.00180000000000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-2.40000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.0225600000000000))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(2.40000000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.744000000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(0.600000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-0.744000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(4.80000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(0.0992000000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.644800000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-8.00000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-0.696000000000000))*(py)*(pz)*(r01)*(r02)))+(((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.00226800000000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-2.40000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.180000000000000))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(-8.00000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-1.20000000000000))*(cj0)*(pp)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(0.0496000000000000))*((py)*(py))*((r02)*(r02))*((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(-4.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.382500000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(2.40000000000000))*(pz)*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.0225600000000000))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.300000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((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.0496000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.180000000000000))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(0.0992000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.300000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(0.0496000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((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.696000000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(-0.300000000000000))*(pp)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.600000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.0451200000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.644800000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(1.20000000000000))*(pp)*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(0.744000000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(1.20000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.744000000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(0.180000000000000))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(0.744000000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.180000000000000))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-0.00720000000000000))*(py)*(r01)*(r02)))+(((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.0225600000000000))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.600000000000000))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.180000000000000))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(0.0225600000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(0.600000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-2.40000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.00226800000000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.744000000000000))*((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)))));
334 polyroots4(op,zeror,numroots);
335 IkReal j1array[4], cj1array[4], sj1array[4], tempj1array[1];
336 int numsolutions = 0;
337 for(int ij1 = 0; ij1 < numroots; ++ij1)
338 {
339 IkReal htj1 = zeror[ij1];
340 tempj1array[0]=((IkReal(2.00000000000000))*(atan(htj1)));
341 for(int kj1 = 0; kj1 < 1; ++kj1)
342 {
343 j1array[numsolutions] = tempj1array[kj1];
344 if( j1array[numsolutions] > IKPI )
345 {
346  j1array[numsolutions]-=IK2PI;
347 }
348 else if( j1array[numsolutions] < -IKPI )
349 {
350  j1array[numsolutions]+=IK2PI;
351 }
352 sj1array[numsolutions] = IKsin(j1array[numsolutions]);
353 cj1array[numsolutions] = IKcos(j1array[numsolutions]);
354 numsolutions++;
355 }
356 }
357 bool j1valid[4]={true,true,true,true};
358 _nj1 = 4;
359 for(int ij1 = 0; ij1 < numsolutions; ++ij1)
360  {
361 if( !j1valid[ij1] )
362 {
363  continue;
364 }
365  j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
366 htj1 = IKtan(j1/2);
367 
368 _ij1[0] = ij1; _ij1[1] = -1;
369 for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
370 {
371 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
372 {
373  j1valid[iij1]=false; _ij1[1] = iij1; break;
374 }
375 }
376 {
377 IkReal j4array[2], cj4array[2], sj4array[2];
378 bool j4valid[2]={false};
379 _nj4 = 2;
380 sj4array[0]=((((IkReal(-1.00000000000000))*(r00)*(sj0)))+(((cj0)*(r01))));
381 if( sj4array[0] >= -1-IKFAST_SINCOS_THRESH && sj4array[0] <= 1+IKFAST_SINCOS_THRESH )
382 {
383  j4valid[0] = j4valid[1] = true;
384  j4array[0] = IKasin(sj4array[0]);
385  cj4array[0] = IKcos(j4array[0]);
386  sj4array[1] = sj4array[0];
387  j4array[1] = j4array[0] > 0 ? (IKPI-j4array[0]) : (-IKPI-j4array[0]);
388  cj4array[1] = -cj4array[0];
389 }
390 else if( isnan(sj4array[0]) )
391 {
392  // probably any value will work
393  j4valid[0] = true;
394  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
395 }
396 for(int ij4 = 0; ij4 < 2; ++ij4)
397 {
398 if( !j4valid[ij4] )
399 {
400  continue;
401 }
402 _ij4[0] = ij4; _ij4[1] = -1;
403 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
404 {
405 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
406 {
407  j4valid[iij4]=false; _ij4[1] = iij4; break;
408 }
409 }
410 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
411 
412 {
413 IkReal dummyeval[1];
414 IkReal gconst0;
415 gconst0=IKsign(cj4);
416 dummyeval[0]=cj4;
417 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
418 {
419 {
420 IkReal dummyeval[1];
421 IkReal gconst1;
422 gconst1=IKsign(cj4);
423 dummyeval[0]=cj4;
424 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
425 {
426 {
427 IkReal evalcond[9];
428 IkReal x31=(py)*(py);
429 IkReal x32=(px)*(px);
430 IkReal x33=(pz)*(pz);
431 IkReal x34=((r02)*(sj1));
432 IkReal x35=((cj1)*(pz));
433 IkReal x36=((IkReal(0.150000000000000))*(r00));
434 IkReal x37=((IkReal(0.600000000000000))*(r02));
435 IkReal x38=((cj0)*(px));
436 IkReal x39=((r01)*(sj0));
437 IkReal x40=((IkReal(1.00000000000000))*(cj1));
438 IkReal x41=((py)*(sj0));
439 IkReal x42=((py)*(r01));
440 IkReal x43=((IkReal(0.150000000000000))*(sj1));
441 IkReal x44=((IkReal(1.00000000000000))*(pz));
442 IkReal x45=((px)*(sj1));
443 IkReal x46=((IkReal(0.600000000000000))*(r00));
444 IkReal x47=((cj0)*(r00));
445 IkReal x48=((IkReal(0.0843750000000000))*(cj1));
446 IkReal x49=((IkReal(2.00000000000000))*(pz));
447 IkReal x50=((cj1)*(px));
448 IkReal x51=((IkReal(1.00000000000000))*(px));
449 IkReal x52=((IkReal(0.300000000000000))*(cj1));
450 IkReal x53=((IkReal(0.0956250000000000))*(sj1));
451 IkReal x54=((IkReal(1.00000000000000))*(sj1));
452 IkReal x55=((cj0)*(pz));
453 IkReal x56=((IkReal(0.150000000000000))*(cj1));
454 IkReal x57=((IkReal(2.00000000000000))*(sj1));
455 IkReal x58=((IkReal(2.00000000000000))*(cj1));
456 IkReal x59=((IkReal(2.00000000000000))*(r00));
457 IkReal x60=((IkReal(0.300000000000000))*(sj1));
458 IkReal x61=((r02)*(x41));
459 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
460 evalcond[1]=((((IkReal(-1.00000000000000))*(sj0)*(x51)))+(((cj0)*(py))));
461 evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r00)*(sj0)))+(((cj0)*(r01))));
462 evalcond[3]=((((IkReal(-1.00000000000000))*(x40)*(x47)))+(x34)+(((IkReal(-1.00000000000000))*(x39)*(x40))));
463 evalcond[4]=((((IkReal(-1.00000000000000))*(r02)*(x40)))+(((IkReal(-1.00000000000000))*(x39)*(x54)))+(((IkReal(-1.00000000000000))*(x47)*(x54))));
464 evalcond[5]=((((IkReal(0.0750000000000000))*(x39)))+(((r02)*(x52)))+(((IkReal(-1.00000000000000))*(r00)*(x51)))+(((IkReal(0.0750000000000000))*(x47)))+(((x39)*(x60)))+(((IkReal(-1.00000000000000))*(x42)))+(((IkReal(-1.00000000000000))*(r02)*(x44)))+(((x47)*(x60))));
465 evalcond[6]=((x61)+(((r02)*(x38)))+(((IkReal(-1.00000000000000))*(x39)*(x44)))+(((x47)*(x52)))+(((IkReal(-1.00000000000000))*(x44)*(x47)))+(((IkReal(-0.0750000000000000))*(r02)))+(((IkReal(-0.300000000000000))*(x34)))+(((x39)*(x52))));
466 evalcond[7]=((((r02)*(x33)*(x58)))+(((IkReal(-1.00000000000000))*(x42)*(x43)))+(((x39)*(x53)))+(((x34)*(x41)*(x49)))+(((x34)*(x38)*(x49)))+(((px)*(x35)*(x59)))+(((x41)*(x45)*(x59)))+(((IkReal(-1.00000000000000))*(pp)*(x47)*(x54)))+(((x56)*(x61)))+(((x38)*(x42)*(x57)))+(((IkReal(-1.00000000000000))*(x36)*(x45)))+(((IkReal(-1.00000000000000))*(cj0)*(x35)*(x36)))+(((IkReal(-1.00000000000000))*(pp)*(x39)*(x54)))+(((IkReal(-0.600000000000000))*(x42)))+(((x32)*(x47)*(x57)))+(((IkReal(-0.150000000000000))*(pz)*(x34)))+(((IkReal(-1.00000000000000))*(pz)*(x37)))+(((IkReal(-1.00000000000000))*(px)*(x46)))+(((r02)*(x38)*(x56)))+(((IkReal(-1.00000000000000))*(pp)*(r02)*(x40)))+(((r02)*(x48)))+(((x47)*(x53)))+(((x31)*(x39)*(x57)))+(((IkReal(0.0450000000000000))*(x39)))+(((IkReal(2.00000000000000))*(x35)*(x42)))+(((IkReal(-0.150000000000000))*(x35)*(x39)))+(((IkReal(0.0450000000000000))*(x47))));
467 evalcond[8]=((((pz)*(x39)*(x43)))+(((IkReal(-1.00000000000000))*(pp)*(x40)*(x47)))+(((IkReal(0.600000000000000))*(pz)*(x39)))+(((IkReal(0.0450000000000000))*(r02)))+(((pp)*(x34)))+(((x41)*(x50)*(x59)))+(((IkReal(-0.150000000000000))*(x34)*(x41)))+(((IkReal(-2.00000000000000))*(x33)*(x34)))+(((x31)*(x39)*(x58)))+(((IkReal(-1.00000000000000))*(x47)*(x48)))+(((sj1)*(x36)*(x55)))+(((IkReal(2.00000000000000))*(r02)*(x35)*(x38)))+(((x32)*(x47)*(x58)))+(((IkReal(-1.00000000000000))*(x39)*(x48)))+(((IkReal(-1.00000000000000))*(r00)*(x45)*(x49)))+(((IkReal(-1.00000000000000))*(sj1)*(x42)*(x49)))+(((IkReal(-1.00000000000000))*(pp)*(x39)*(x40)))+(((IkReal(-1.00000000000000))*(x37)*(x41)))+(((IkReal(-1.00000000000000))*(x42)*(x56)))+(((IkReal(-1.00000000000000))*(x36)*(x50)))+(((IkReal(0.0956250000000000))*(x34)))+(((IkReal(-0.150000000000000))*(r02)*(x35)))+(((IkReal(-0.150000000000000))*(x34)*(x38)))+(((x46)*(x55)))+(((x38)*(x42)*(x58)))+(((IkReal(-1.00000000000000))*(x37)*(x38)))+(((IkReal(2.00000000000000))*(x35)*(x61))));
468 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 )
469 {
470 {
471 IkReal j3array[2], cj3array[2], sj3array[2];
472 bool j3valid[2]={false};
473 _nj3 = 2;
474 IkReal x62=((IkReal(11.4095661394664))*(sj1));
475 IkReal x63=((cj0)*(px));
476 IkReal x64=((py)*(sj0));
477 if( (((IkReal(0.357499739036614))+(((IkReal(-19.0159435657774))*(pp)))+(((IkReal(2.85239153486660))*(x63)))+(((x62)*(x64)))+(((x62)*(x63)))+(((IkReal(2.85239153486660))*(x64)))+(((IkReal(11.4095661394664))*(cj1)*(pz)))+(((IkReal(-0.855717460459981))*(sj1))))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.357499739036614))+(((IkReal(-19.0159435657774))*(pp)))+(((IkReal(2.85239153486660))*(x63)))+(((x62)*(x64)))+(((x62)*(x63)))+(((IkReal(2.85239153486660))*(x64)))+(((IkReal(11.4095661394664))*(cj1)*(pz)))+(((IkReal(-0.855717460459981))*(sj1))))) > 1+IKFAST_SINCOS_THRESH )
478  continue;
479 IkReal x65=IKasin(((IkReal(0.357499739036614))+(((IkReal(-19.0159435657774))*(pp)))+(((IkReal(2.85239153486660))*(x63)))+(((x62)*(x64)))+(((x62)*(x63)))+(((IkReal(2.85239153486660))*(x64)))+(((IkReal(11.4095661394664))*(cj1)*(pz)))+(((IkReal(-0.855717460459981))*(sj1)))));
480 j3array[0]=((IkReal(-1.34057673951805))+(((IkReal(-1.00000000000000))*(x65))));
481 sj3array[0]=IKsin(j3array[0]);
482 cj3array[0]=IKcos(j3array[0]);
483 j3array[1]=((IkReal(1.80101591407174))+(x65));
484 sj3array[1]=IKsin(j3array[1]);
485 cj3array[1]=IKcos(j3array[1]);
486 if( j3array[0] > IKPI )
487 {
488  j3array[0]-=IK2PI;
489 }
490 else if( j3array[0] < -IKPI )
491 { j3array[0]+=IK2PI;
492 }
493 j3valid[0] = true;
494 if( j3array[1] > IKPI )
495 {
496  j3array[1]-=IK2PI;
497 }
498 else if( j3array[1] < -IKPI )
499 { j3array[1]+=IK2PI;
500 }
501 j3valid[1] = true;
502 for(int ij3 = 0; ij3 < 2; ++ij3)
503 {
504 if( !j3valid[ij3] )
505 {
506  continue;
507 }
508 _ij3[0] = ij3; _ij3[1] = -1;
509 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
510 {
511 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
512 {
513  j3valid[iij3]=false; _ij3[1] = iij3; break;
514 }
515 }
516 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
517 {
518 IkReal evalcond[1];
519 IkReal x66=((IkReal(2.00000000000000))*(sj0));
520 IkReal x67=((px)*(py));
521 IkReal x68=((r00)*(sj0));
522 IkReal x69=((cj0)*(r01));
523 IkReal x70=((py)*(r00));
524 IkReal x71=((IkReal(2.00000000000000))*(cj0));
525 IkReal x72=((px)*(r01));
526 IkReal x73=((IkReal(0.600000000000000))*(cj1));
527 IkReal x74=((py)*(r02));
528 IkReal x75=((IkReal(0.600000000000000))*(sj1));
529 IkReal x76=((IkReal(0.0450000000000000))*(sj1));
530 IkReal x77=((px)*(r02));
531 evalcond[0]=((IkReal(-0.114425000000000))+(((IkReal(-1.00000000000000))*(pz)*(x71)*(x74)))+(((IkReal(0.150000000000000))*(x70)))+(((IkReal(-1.00000000000000))*(sj0)*(x73)*(x77)))+(((IkReal(-1.00000000000000))*(x68)*(x76)))+(((IkReal(0.0956250000000000))*(x69)))+(((cj0)*(x73)*(x74)))+(((IkReal(-2.00000000000000))*(x69)*((py)*(py))))+(((IkReal(-1.00000000000000))*(pz)*(x69)*(x73)))+(((IkReal(-1.00000000000000))*(x72)*(x75)))+(((r00)*(x66)*((px)*(px))))+(((pz)*(x68)*(x73)))+(((IkReal(-0.150000000000000))*(x72)))+(((pz)*(x66)*(x77)))+(((pp)*(x69)))+(((IkReal(-0.0120000000000000))*(IKsin(j3))))+(((x70)*(x75)))+(((IkReal(-0.0512000000000000))*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(pp)*(x68)))+(((r01)*(x66)*(x67)))+(((IkReal(-0.0956250000000000))*(x68)))+(((IkReal(-1.00000000000000))*(r00)*(x67)*(x71)))+(((x69)*(x76))));
532 if( IKabs(evalcond[0]) > 0.000001 )
533 {
534 continue;
535 }
536 }
537 
538 {
539 IkReal dummyeval[1];
540 IkReal gconst8;
541 gconst8=IKsign(((IkReal(0.108025000000000))+(((IkReal(0.0512000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
542 dummyeval[0]=((IkReal(16.8789062500000))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3))+(((IkReal(8.00000000000000))*(cj3))));
543 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
544 {
545 {
546 IkReal dummyeval[1];
547 IkReal gconst9;
548 gconst9=IKsign(((IkReal(0.108025000000000))+(((IkReal(0.0512000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
549 dummyeval[0]=((IkReal(16.8789062500000))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3))+(((IkReal(8.00000000000000))*(cj3))));
550 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
551 {
552 continue;
553 
554 } else
555 {
556 {
557 IkReal j2array[1], cj2array[1], sj2array[1];
558 bool j2valid[1]={false};
559 _nj2 = 1;
560 IkReal x78=((r02)*(sj0));
561 IkReal x79=((IkReal(0.0800000000000000))*(sj1));
562 IkReal x80=((cj3)*(px));
563 IkReal x81=((cj0)*(r01));
564 IkReal x82=((IkReal(0.0240000000000000))*(cj1));
565 IkReal x83=((pz)*(sj3));
566 IkReal x84=((IkReal(0.00600000000000000))*(cj1));
567 IkReal x85=((IkReal(0.00562500000000000))*(cj1));
568 IkReal x86=((IkReal(0.0750000000000000))*(sj1));
569 IkReal x87=((r00)*(sj0));
570 IkReal x88=((cj3)*(pz));
571 IkReal x89=((IkReal(0.320000000000000))*(py));
572 IkReal x90=((IkReal(0.0800000000000000))*(cj1));
573 IkReal x91=((IkReal(0.0750000000000000))*(cj1));
574 IkReal x92=((py)*(sj0));
575 IkReal x93=((IkReal(0.320000000000000))*(sj1));
576 IkReal x94=((cj0)*(px));
577 IkReal x95=((py)*(r00));
578 IkReal x96=((IkReal(0.00600000000000000))*(sj1));
579 IkReal x97=((cj1)*(px)*(r01));
580 IkReal x98=((cj0)*(py)*(r02));
581 IkReal x99=((px)*(sj3)*(x79));
582 if( IKabs(((gconst9)*(((IkReal(-0.0960000000000000))+(((IkReal(-1.00000000000000))*(sj3)*(x79)*(x98)))+(((sj0)*(sj1)*(x89)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(sj3)*(x90)))+(((cj3)*(x79)*(x92)))+(((sj3)*(x81)*(x84)))+(((px)*(x78)*(x86)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((x93)*(x94)))+(((x79)*(x81)*(x83)))+(((IkReal(-1.00000000000000))*(x85)*(x87)))+(((IkReal(-1.00000000000000))*(x86)*(x98)))+(((sj3)*(x90)*(x95)))+(((IkReal(-1.00000000000000))*(x79)*(x83)*(x87)))+(((cj0)*(x79)*(x80)))+(((IkReal(-1.00000000000000))*(sj3)*(x84)*(x87)))+(((IkReal(0.320000000000000))*(cj1)*(pz)))+(((pz)*(x81)*(x86)))+(((x88)*(x90)))+(((IkReal(-1.00000000000000))*(pz)*(x86)*(x87)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x91)))+(((x78)*(x99)))+(((x81)*(x85)))+(((IkReal(-1.00000000000000))*(cj3)*(x96)))+(((x91)*(x95))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((cj0)*(r02)*(sj1)*(x89)))+(((x79)*(x87)*(x88)))+(((cj3)*(x79)*(x98)))+(((IkReal(-1.00000000000000))*(pz)*(x81)*(x93)))+(((IkReal(-1.00000000000000))*(px)*(x78)*(x93)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x89)))+(((IkReal(-1.00000000000000))*(x78)*(x79)*(x80)))+(((pz)*(x91)))+(((IkReal(-1.00000000000000))*(x79)*(x81)*(x88)))+(((x82)*(x87)))+(((IkReal(-1.00000000000000))*(cj3)*(x90)*(x95)))+(((sj3)*(x79)*(x92)))+(((IkReal(-1.00000000000000))*(sj3)*(x96)))+(((cj3)*(x84)*(x87)))+(((pz)*(x87)*(x93)))+(((r01)*(x80)*(x90)))+(((x83)*(x90)))+(((IkReal(-1.00000000000000))*(cj3)*(x81)*(x84)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x86)*(x94)))+(((sj3)*(x79)*(x94)))+(((IkReal(0.320000000000000))*(x97)))+(((x86)*(x92)))+(((IkReal(-1.00000000000000))*(x81)*(x82))))))) < IKFAST_ATAN2_MAGTHRESH )
583  continue;
584 j2array[0]=IKatan2(((gconst9)*(((IkReal(-0.0960000000000000))+(((IkReal(-1.00000000000000))*(sj3)*(x79)*(x98)))+(((sj0)*(sj1)*(x89)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(sj3)*(x90)))+(((cj3)*(x79)*(x92)))+(((sj3)*(x81)*(x84)))+(((px)*(x78)*(x86)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((x93)*(x94)))+(((x79)*(x81)*(x83)))+(((IkReal(-1.00000000000000))*(x85)*(x87)))+(((IkReal(-1.00000000000000))*(x86)*(x98)))+(((sj3)*(x90)*(x95)))+(((IkReal(-1.00000000000000))*(x79)*(x83)*(x87)))+(((cj0)*(x79)*(x80)))+(((IkReal(-1.00000000000000))*(sj3)*(x84)*(x87)))+(((IkReal(0.320000000000000))*(cj1)*(pz)))+(((pz)*(x81)*(x86)))+(((x88)*(x90)))+(((IkReal(-1.00000000000000))*(pz)*(x86)*(x87)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x91)))+(((x78)*(x99)))+(((x81)*(x85)))+(((IkReal(-1.00000000000000))*(cj3)*(x96)))+(((x91)*(x95)))))), ((gconst9)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((cj0)*(r02)*(sj1)*(x89)))+(((x79)*(x87)*(x88)))+(((cj3)*(x79)*(x98)))+(((IkReal(-1.00000000000000))*(pz)*(x81)*(x93)))+(((IkReal(-1.00000000000000))*(px)*(x78)*(x93)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x89)))+(((IkReal(-1.00000000000000))*(x78)*(x79)*(x80)))+(((pz)*(x91)))+(((IkReal(-1.00000000000000))*(x79)*(x81)*(x88)))+(((x82)*(x87)))+(((IkReal(-1.00000000000000))*(cj3)*(x90)*(x95)))+(((sj3)*(x79)*(x92)))+(((IkReal(-1.00000000000000))*(sj3)*(x96)))+(((cj3)*(x84)*(x87)))+(((pz)*(x87)*(x93)))+(((r01)*(x80)*(x90)))+(((x83)*(x90)))+(((IkReal(-1.00000000000000))*(cj3)*(x81)*(x84)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x86)*(x94)))+(((sj3)*(x79)*(x94)))+(((IkReal(0.320000000000000))*(x97)))+(((x86)*(x92)))+(((IkReal(-1.00000000000000))*(x81)*(x82)))))));
585 sj2array[0]=IKsin(j2array[0]);
586 cj2array[0]=IKcos(j2array[0]);
587 if( j2array[0] > IKPI )
588 {
589  j2array[0]-=IK2PI;
590 }
591 else if( j2array[0] < -IKPI )
592 { j2array[0]+=IK2PI;
593 }
594 j2valid[0] = true;
595 for(int ij2 = 0; ij2 < 1; ++ij2)
596 {
597 if( !j2valid[ij2] )
598 {
599  continue;
600 }
601 _ij2[0] = ij2; _ij2[1] = -1;
602 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
603 {
604 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
605 {
606  j2valid[iij2]=false; _ij2[1] = iij2; break;
607 }
608 }
609 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
610 {
611 IkReal evalcond[4];
612 IkReal x100=IKcos(j2);
613 IkReal x101=IKsin(j2);
614 IkReal x102=((IkReal(0.0800000000000000))*(sj3));
615 IkReal x103=((cj0)*(r01));
616 IkReal x104=((IkReal(1.00000000000000))*(px));
617 IkReal x105=((py)*(sj1));
618 IkReal x106=((cj0)*(r02));
619 IkReal x107=((IkReal(0.0750000000000000))*(cj1));
620 IkReal x108=((r02)*(sj0));
621 IkReal x109=((IkReal(0.0750000000000000))*(sj1));
622 IkReal x110=((cj1)*(pz));
623 IkReal x111=((r00)*(sj0));
624 IkReal x112=((IkReal(0.0800000000000000))*(cj3));
625 IkReal x113=((pz)*(sj1));
626 IkReal x114=((IkReal(1.00000000000000))*(sj0));
627 IkReal x115=((cj1)*(py));
628 IkReal x116=((IkReal(0.0750000000000000))*(x101));
629 IkReal x117=((IkReal(0.320000000000000))*(x100));
630 IkReal x118=((IkReal(0.320000000000000))*(x101));
631 IkReal x119=((IkReal(0.0750000000000000))*(x100));
632 IkReal x120=((sj1)*(x111));
633 IkReal x121=((x101)*(x102));
634 IkReal x122=((x100)*(x112));
635 IkReal x123=((x100)*(x102));
636 IkReal x124=((x101)*(x112));
637 IkReal x125=((x121)+(x116));
638 IkReal x126=((x122)+(x117));
639 IkReal x127=((x124)+(x123)+(x119)+(x118));
640 evalcond[0]=((((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x104)))+(((IkReal(-1.00000000000000))*(x114)*(x115)))+(x126)+(((IkReal(-1.00000000000000))*(x125)))+(x107)+(x113));
641 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x105)*(x114)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x104)))+(x127)+(x109)+(((IkReal(-1.00000000000000))*(x110))));
642 evalcond[2]=((((IkReal(-1.00000000000000))*(sj1)*(x104)*(x108)))+(((x111)*(x113)))+(((x107)*(x111)))+(((x105)*(x106)))+(((IkReal(-1.00000000000000))*(x126)))+(((IkReal(-1.00000000000000))*(r00)*(x115)))+(((cj1)*(px)*(r01)))+(x125)+(((IkReal(-1.00000000000000))*(x103)*(x113)))+(((IkReal(-1.00000000000000))*(x103)*(x107))));
643 evalcond[3]=((((IkReal(-1.00000000000000))*(x109)*(x111)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x104)))+(((x110)*(x111)))+(((IkReal(-1.00000000000000))*(cj1)*(x104)*(x108)))+(((IkReal(-1.00000000000000))*(x103)*(x110)))+(x127)+(((IkReal(0.300000000000000))*(x103)))+(((x106)*(x115)))+(((IkReal(-0.300000000000000))*(x111)))+(((r00)*(x105)))+(((x103)*(x109))));
644 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
645 {
646 continue;
647 }
648 }
649 
650 {
651 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
652 vinfos[0].jointtype = 1;
653 vinfos[0].foffset = j0;
654 vinfos[0].indices[0] = _ij0[0];
655 vinfos[0].indices[1] = _ij0[1];
656 vinfos[0].maxsolutions = _nj0;
657 vinfos[1].jointtype = 1;
658 vinfos[1].foffset = j1;
659 vinfos[1].indices[0] = _ij1[0];
660 vinfos[1].indices[1] = _ij1[1];
661 vinfos[1].maxsolutions = _nj1;
662 vinfos[2].jointtype = 1;
663 vinfos[2].foffset = j2;
664 vinfos[2].indices[0] = _ij2[0];
665 vinfos[2].indices[1] = _ij2[1];
666 vinfos[2].maxsolutions = _nj2;
667 vinfos[3].jointtype = 1;
668 vinfos[3].foffset = j3;
669 vinfos[3].indices[0] = _ij3[0];
670 vinfos[3].indices[1] = _ij3[1];
671 vinfos[3].maxsolutions = _nj3;
672 vinfos[4].jointtype = 1;
673 vinfos[4].foffset = j4;
674 vinfos[4].indices[0] = _ij4[0];
675 vinfos[4].indices[1] = _ij4[1];
676 vinfos[4].maxsolutions = _nj4;
677 std::vector<int> vfree(0);
678 solutions.AddSolution(vinfos,vfree);
679 }
680 }
681 }
682 
683 }
684 
685 }
686 
687 } else
688 {
689 {
690 IkReal j2array[1], cj2array[1], sj2array[1];
691 bool j2valid[1]={false};
692 _nj2 = 1;
693 IkReal x128=((IkReal(0.0800000000000000))*(cj1));
694 IkReal x129=((cj0)*(px));
695 IkReal x130=((py)*(sj0));
696 IkReal x131=((IkReal(0.320000000000000))*(sj1));
697 IkReal x132=((IkReal(0.00600000000000000))*(cj3));
698 IkReal x133=((pz)*(sj3));
699 IkReal x134=((IkReal(0.0750000000000000))*(cj1));
700 IkReal x135=((IkReal(0.0800000000000000))*(sj1));
701 IkReal x136=((IkReal(0.0750000000000000))*(sj1));
702 IkReal x137=((IkReal(0.00600000000000000))*(sj3));
703 IkReal x138=((cj3)*(pz));
704 IkReal x139=((IkReal(0.320000000000000))*(cj1));
705 if( IKabs(((gconst8)*(((IkReal(-0.0960000000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x132)))+(((IkReal(-1.00000000000000))*(x130)*(x134)))+(((IkReal(-1.00000000000000))*(sj3)*(x128)*(x130)))+(((cj3)*(x129)*(x135)))+(((cj3)*(x130)*(x135)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((x133)*(x135)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(sj3)*(x128)*(x129)))+(((x128)*(x138)))+(((IkReal(-1.00000000000000))*(x129)*(x134)))+(((x130)*(x131)))+(((cj1)*(x137)))+(((pz)*(x136)))+(((pz)*(x139)))+(((IkReal(0.00562500000000000))*(cj1)))+(((x129)*(x131))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((sj3)*(x129)*(x135)))+(((x129)*(x139)))+(((pz)*(x134)))+(((x130)*(x139)))+(((IkReal(-1.00000000000000))*(x135)*(x138)))+(((IkReal(-1.00000000000000))*(pz)*(x131)))+(((x130)*(x136)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((sj3)*(x130)*(x135)))+(((cj3)*(x128)*(x130)))+(((IkReal(-1.00000000000000))*(cj1)*(x132)))+(((x128)*(x133)))+(((x129)*(x136)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(sj1)*(x137)))+(((cj3)*(x128)*(x129))))))) < IKFAST_ATAN2_MAGTHRESH )
706  continue;
707 j2array[0]=IKatan2(((gconst8)*(((IkReal(-0.0960000000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x132)))+(((IkReal(-1.00000000000000))*(x130)*(x134)))+(((IkReal(-1.00000000000000))*(sj3)*(x128)*(x130)))+(((cj3)*(x129)*(x135)))+(((cj3)*(x130)*(x135)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((x133)*(x135)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(sj3)*(x128)*(x129)))+(((x128)*(x138)))+(((IkReal(-1.00000000000000))*(x129)*(x134)))+(((x130)*(x131)))+(((cj1)*(x137)))+(((pz)*(x136)))+(((pz)*(x139)))+(((IkReal(0.00562500000000000))*(cj1)))+(((x129)*(x131)))))), ((gconst8)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((sj3)*(x129)*(x135)))+(((x129)*(x139)))+(((pz)*(x134)))+(((x130)*(x139)))+(((IkReal(-1.00000000000000))*(x135)*(x138)))+(((IkReal(-1.00000000000000))*(pz)*(x131)))+(((x130)*(x136)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((sj3)*(x130)*(x135)))+(((cj3)*(x128)*(x130)))+(((IkReal(-1.00000000000000))*(cj1)*(x132)))+(((x128)*(x133)))+(((x129)*(x136)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(sj1)*(x137)))+(((cj3)*(x128)*(x129)))))));
708 sj2array[0]=IKsin(j2array[0]);
709 cj2array[0]=IKcos(j2array[0]);
710 if( j2array[0] > IKPI )
711 {
712  j2array[0]-=IK2PI;
713 }
714 else if( j2array[0] < -IKPI )
715 { j2array[0]+=IK2PI;
716 }
717 j2valid[0] = true;
718 for(int ij2 = 0; ij2 < 1; ++ij2)
719 {
720 if( !j2valid[ij2] )
721 {
722  continue;
723 }
724 _ij2[0] = ij2; _ij2[1] = -1;
725 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
726 {
727 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
728 {
729  j2valid[iij2]=false; _ij2[1] = iij2; break;
730 }
731 }
732 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
733 {
734 IkReal evalcond[4];
735 IkReal x140=IKcos(j2);
736 IkReal x141=IKsin(j2);
737 IkReal x142=((IkReal(0.0800000000000000))*(sj3));
738 IkReal x143=((cj0)*(r01));
739 IkReal x144=((IkReal(1.00000000000000))*(px));
740 IkReal x145=((py)*(sj1));
741 IkReal x146=((cj0)*(r02));
742 IkReal x147=((IkReal(0.0750000000000000))*(cj1));
743 IkReal x148=((r02)*(sj0));
744 IkReal x149=((IkReal(0.0750000000000000))*(sj1));
745 IkReal x150=((cj1)*(pz));
746 IkReal x151=((r00)*(sj0));
747 IkReal x152=((IkReal(0.0800000000000000))*(cj3));
748 IkReal x153=((pz)*(sj1));
749 IkReal x154=((IkReal(1.00000000000000))*(sj0));
750 IkReal x155=((cj1)*(py));
751 IkReal x156=((IkReal(0.0750000000000000))*(x141));
752 IkReal x157=((IkReal(0.320000000000000))*(x140));
753 IkReal x158=((IkReal(0.320000000000000))*(x141));
754 IkReal x159=((IkReal(0.0750000000000000))*(x140));
755 IkReal x160=((sj1)*(x151));
756 IkReal x161=((x141)*(x142));
757 IkReal x162=((x140)*(x152));
758 IkReal x163=((x140)*(x142));
759 IkReal x164=((x141)*(x152));
760 IkReal x165=((x156)+(x161));
761 IkReal x166=((x157)+(x162));
762 IkReal x167=((x159)+(x158)+(x163)+(x164));
763 evalcond[0]=((((IkReal(-1.00000000000000))*(x154)*(x155)))+(x153)+(((IkReal(-1.00000000000000))*(x165)))+(x166)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x144)))+(x147));
764 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x150)))+(x167)+(((IkReal(-1.00000000000000))*(x145)*(x154)))+(x149)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x144))));
765 evalcond[2]=((((x151)*(x153)))+(((IkReal(-1.00000000000000))*(x143)*(x147)))+(((IkReal(-1.00000000000000))*(x166)))+(((cj1)*(px)*(r01)))+(x165)+(((x147)*(x151)))+(((IkReal(-1.00000000000000))*(sj1)*(x144)*(x148)))+(((IkReal(-1.00000000000000))*(x143)*(x153)))+(((x145)*(x146)))+(((IkReal(-1.00000000000000))*(r00)*(x155))));
766 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(sj1)*(x144)))+(((r00)*(x145)))+(((x143)*(x149)))+(((IkReal(0.300000000000000))*(x143)))+(((x150)*(x151)))+(x167)+(((IkReal(-0.300000000000000))*(x151)))+(((IkReal(-1.00000000000000))*(x143)*(x150)))+(((IkReal(-1.00000000000000))*(cj1)*(x144)*(x148)))+(((IkReal(-1.00000000000000))*(x149)*(x151)))+(((x146)*(x155))));
767 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
768 {
769 continue;
770 }
771 }
772 
773 {
774 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
775 vinfos[0].jointtype = 1;
776 vinfos[0].foffset = j0;
777 vinfos[0].indices[0] = _ij0[0];
778 vinfos[0].indices[1] = _ij0[1];
779 vinfos[0].maxsolutions = _nj0;
780 vinfos[1].jointtype = 1;
781 vinfos[1].foffset = j1;
782 vinfos[1].indices[0] = _ij1[0];
783 vinfos[1].indices[1] = _ij1[1];
784 vinfos[1].maxsolutions = _nj1;
785 vinfos[2].jointtype = 1;
786 vinfos[2].foffset = j2;
787 vinfos[2].indices[0] = _ij2[0];
788 vinfos[2].indices[1] = _ij2[1];
789 vinfos[2].maxsolutions = _nj2;
790 vinfos[3].jointtype = 1;
791 vinfos[3].foffset = j3;
792 vinfos[3].indices[0] = _ij3[0];
793 vinfos[3].indices[1] = _ij3[1];
794 vinfos[3].maxsolutions = _nj3;
795 vinfos[4].jointtype = 1;
796 vinfos[4].foffset = j4;
797 vinfos[4].indices[0] = _ij4[0];
798 vinfos[4].indices[1] = _ij4[1];
799 vinfos[4].maxsolutions = _nj4;
800 std::vector<int> vfree(0);
801 solutions.AddSolution(vinfos,vfree);
802 }
803 }
804 }
805 
806 }
807 
808 }
809 }
810 }
811 
812 } else
813 {
814 IkReal x168=(py)*(py);
815 IkReal x169=(px)*(px);
816 IkReal x170=(pz)*(pz);
817 IkReal x171=((r02)*(sj1));
818 IkReal x172=((cj1)*(pz));
819 IkReal x173=((IkReal(0.150000000000000))*(r00));
820 IkReal x174=((IkReal(0.600000000000000))*(r02));
821 IkReal x175=((cj0)*(px));
822 IkReal x176=((r01)*(sj0));
823 IkReal x177=((IkReal(1.00000000000000))*(cj1));
824 IkReal x178=((py)*(sj0));
825 IkReal x179=((py)*(r01));
826 IkReal x180=((IkReal(0.150000000000000))*(sj1));
827 IkReal x181=((IkReal(1.00000000000000))*(pz));
828 IkReal x182=((px)*(sj1));
829 IkReal x183=((IkReal(0.600000000000000))*(r00));
830 IkReal x184=((cj0)*(r00));
831 IkReal x185=((IkReal(0.0843750000000000))*(cj1));
832 IkReal x186=((IkReal(2.00000000000000))*(pz));
833 IkReal x187=((cj1)*(px));
834 IkReal x188=((IkReal(1.00000000000000))*(px));
835 IkReal x189=((IkReal(0.300000000000000))*(cj1));
836 IkReal x190=((IkReal(0.0956250000000000))*(sj1));
837 IkReal x191=((IkReal(1.00000000000000))*(sj1));
838 IkReal x192=((cj0)*(pz));
839 IkReal x193=((IkReal(0.150000000000000))*(cj1));
840 IkReal x194=((IkReal(2.00000000000000))*(sj1));
841 IkReal x195=((IkReal(2.00000000000000))*(cj1));
842 IkReal x196=((IkReal(2.00000000000000))*(r00));
843 IkReal x197=((IkReal(0.300000000000000))*(sj1));
844 IkReal x198=((r02)*(x178));
845 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
846 evalcond[1]=((((IkReal(-1.00000000000000))*(sj0)*(x188)))+(((cj0)*(py))));
847 evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r00)*(sj0)))+(((cj0)*(r01))));
848 evalcond[3]=((x171)+(((IkReal(-1.00000000000000))*(x177)*(x184)))+(((IkReal(-1.00000000000000))*(x176)*(x177))));
849 evalcond[4]=((((IkReal(-1.00000000000000))*(x176)*(x191)))+(((IkReal(-1.00000000000000))*(r02)*(x177)))+(((IkReal(-1.00000000000000))*(x184)*(x191))));
850 evalcond[5]=((((x184)*(x197)))+(((IkReal(-1.00000000000000))*(r02)*(x181)))+(((IkReal(-1.00000000000000))*(r00)*(x188)))+(((IkReal(0.0750000000000000))*(x184)))+(((r02)*(x189)))+(((x176)*(x197)))+(((IkReal(-1.00000000000000))*(x179)))+(((IkReal(0.0750000000000000))*(x176))));
851 evalcond[6]=((((IkReal(-1.00000000000000))*(x181)*(x184)))+(((x184)*(x189)))+(((IkReal(-0.300000000000000))*(x171)))+(((IkReal(-1.00000000000000))*(x176)*(x181)))+(x198)+(((r02)*(x175)))+(((IkReal(-0.0750000000000000))*(r02)))+(((x176)*(x189))));
852 evalcond[7]=((((IkReal(2.00000000000000))*(x172)*(x179)))+(((IkReal(-1.00000000000000))*(pp)*(r02)*(x177)))+(((r02)*(x185)))+(((x178)*(x182)*(x196)))+(((x171)*(x178)*(x186)))+(((IkReal(-1.00000000000000))*(x173)*(x182)))+(((IkReal(-0.600000000000000))*(x179)))+(((IkReal(-0.150000000000000))*(x172)*(x176)))+(((IkReal(-1.00000000000000))*(pp)*(x184)*(x191)))+(((x168)*(x176)*(x194)))+(((x176)*(x190)))+(((IkReal(-1.00000000000000))*(pz)*(x174)))+(((x193)*(x198)))+(((x175)*(x179)*(x194)))+(((IkReal(-1.00000000000000))*(px)*(x183)))+(((x184)*(x190)))+(((r02)*(x170)*(x195)))+(((x171)*(x175)*(x186)))+(((px)*(x172)*(x196)))+(((IkReal(0.0450000000000000))*(x184)))+(((IkReal(-1.00000000000000))*(pp)*(x176)*(x191)))+(((IkReal(-1.00000000000000))*(cj0)*(x172)*(x173)))+(((IkReal(-1.00000000000000))*(x179)*(x180)))+(((IkReal(-0.150000000000000))*(pz)*(x171)))+(((x169)*(x184)*(x194)))+(((r02)*(x175)*(x193)))+(((IkReal(0.0450000000000000))*(x176))));
853 evalcond[8]=((((IkReal(0.0956250000000000))*(x171)))+(((IkReal(0.600000000000000))*(pz)*(x176)))+(((x168)*(x176)*(x195)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-0.150000000000000))*(x171)*(x178)))+(((IkReal(-1.00000000000000))*(r00)*(x182)*(x186)))+(((IkReal(-1.00000000000000))*(x174)*(x178)))+(((x169)*(x184)*(x195)))+(((IkReal(-0.150000000000000))*(r02)*(x172)))+(((IkReal(-1.00000000000000))*(pp)*(x177)*(x184)))+(((IkReal(2.00000000000000))*(r02)*(x172)*(x175)))+(((IkReal(-1.00000000000000))*(x174)*(x175)))+(((IkReal(-1.00000000000000))*(pp)*(x176)*(x177)))+(((x183)*(x192)))+(((IkReal(-1.00000000000000))*(x176)*(x185)))+(((pp)*(x171)))+(((IkReal(-2.00000000000000))*(x170)*(x171)))+(((IkReal(-1.00000000000000))*(x184)*(x185)))+(((IkReal(-1.00000000000000))*(x173)*(x187)))+(((sj1)*(x173)*(x192)))+(((IkReal(-0.150000000000000))*(x171)*(x175)))+(((x175)*(x179)*(x195)))+(((IkReal(-1.00000000000000))*(sj1)*(x179)*(x186)))+(((IkReal(2.00000000000000))*(x172)*(x198)))+(((x178)*(x187)*(x196)))+(((pz)*(x176)*(x180)))+(((IkReal(-1.00000000000000))*(x179)*(x193))));
854 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 )
855 {
856 {
857 IkReal j3array[2], cj3array[2], sj3array[2];
858 bool j3valid[2]={false};
859 _nj3 = 2;
860 IkReal x199=((IkReal(11.4095661394664))*(sj1));
861 IkReal x200=((cj0)*(px));
862 IkReal x201=((py)*(sj0));
863 if( (((IkReal(0.357499739036614))+(((IkReal(-19.0159435657774))*(pp)))+(((x199)*(x201)))+(((IkReal(2.85239153486660))*(x200)))+(((x199)*(x200)))+(((IkReal(2.85239153486660))*(x201)))+(((IkReal(11.4095661394664))*(cj1)*(pz)))+(((IkReal(-0.855717460459981))*(sj1))))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.357499739036614))+(((IkReal(-19.0159435657774))*(pp)))+(((x199)*(x201)))+(((IkReal(2.85239153486660))*(x200)))+(((x199)*(x200)))+(((IkReal(2.85239153486660))*(x201)))+(((IkReal(11.4095661394664))*(cj1)*(pz)))+(((IkReal(-0.855717460459981))*(sj1))))) > 1+IKFAST_SINCOS_THRESH )
864  continue;
865 IkReal x202=IKasin(((IkReal(0.357499739036614))+(((IkReal(-19.0159435657774))*(pp)))+(((x199)*(x201)))+(((IkReal(2.85239153486660))*(x200)))+(((x199)*(x200)))+(((IkReal(2.85239153486660))*(x201)))+(((IkReal(11.4095661394664))*(cj1)*(pz)))+(((IkReal(-0.855717460459981))*(sj1)))));
866 j3array[0]=((IkReal(-1.34057673951805))+(((IkReal(-1.00000000000000))*(x202))));
867 sj3array[0]=IKsin(j3array[0]);
868 cj3array[0]=IKcos(j3array[0]);
869 j3array[1]=((IkReal(1.80101591407174))+(x202));
870 sj3array[1]=IKsin(j3array[1]);
871 cj3array[1]=IKcos(j3array[1]);
872 if( j3array[0] > IKPI )
873 {
874  j3array[0]-=IK2PI;
875 }
876 else if( j3array[0] < -IKPI )
877 { j3array[0]+=IK2PI;
878 }
879 j3valid[0] = true;
880 if( j3array[1] > IKPI )
881 {
882  j3array[1]-=IK2PI;
883 }
884 else if( j3array[1] < -IKPI )
885 { j3array[1]+=IK2PI;
886 }
887 j3valid[1] = true;
888 for(int ij3 = 0; ij3 < 2; ++ij3)
889 {
890 if( !j3valid[ij3] )
891 {
892  continue;
893 }
894 _ij3[0] = ij3; _ij3[1] = -1;
895 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
896 {
897 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
898 {
899  j3valid[iij3]=false; _ij3[1] = iij3; break;
900 }
901 }
902 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
903 {
904 IkReal evalcond[1];
905 IkReal x203=((IkReal(2.00000000000000))*(sj0));
906 IkReal x204=((px)*(py));
907 IkReal x205=((r00)*(sj0));
908 IkReal x206=((cj0)*(r01));
909 IkReal x207=((py)*(r00));
910 IkReal x208=((IkReal(2.00000000000000))*(cj0));
911 IkReal x209=((px)*(r01));
912 IkReal x210=((IkReal(0.600000000000000))*(cj1));
913 IkReal x211=((py)*(r02));
914 IkReal x212=((IkReal(0.600000000000000))*(sj1));
915 IkReal x213=((IkReal(0.0450000000000000))*(sj1));
916 IkReal x214=((px)*(r02));
917 evalcond[0]=((IkReal(0.114425000000000))+(((IkReal(0.0956250000000000))*(x206)))+(((IkReal(0.150000000000000))*(x207)))+(((IkReal(-0.0956250000000000))*(x205)))+(((IkReal(-1.00000000000000))*(pz)*(x208)*(x211)))+(((IkReal(-1.00000000000000))*(x205)*(x213)))+(((pz)*(x203)*(x214)))+(((r00)*(x203)*((px)*(px))))+(((IkReal(-0.150000000000000))*(x209)))+(((IkReal(-1.00000000000000))*(x209)*(x212)))+(((pp)*(x206)))+(((IkReal(-1.00000000000000))*(r00)*(x204)*(x208)))+(((cj0)*(x210)*(x211)))+(((IkReal(-1.00000000000000))*(pz)*(x206)*(x210)))+(((IkReal(0.0512000000000000))*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(sj0)*(x210)*(x214)))+(((r01)*(x203)*(x204)))+(((IkReal(-1.00000000000000))*(pp)*(x205)))+(((x206)*(x213)))+(((IkReal(0.0120000000000000))*(IKsin(j3))))+(((pz)*(x205)*(x210)))+(((IkReal(-2.00000000000000))*(x206)*((py)*(py))))+(((x207)*(x212))));
918 if( IKabs(evalcond[0]) > 0.000001 )
919 {
920 continue;
921 }
922 }
923 
924 {
925 IkReal dummyeval[1];
926 IkReal gconst10;
927 gconst10=IKsign(((IkReal(0.108025000000000))+(((IkReal(0.0512000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
928 dummyeval[0]=((IkReal(16.8789062500000))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3))+(((IkReal(8.00000000000000))*(cj3))));
929 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
930 {
931 {
932 IkReal dummyeval[1];
933 IkReal gconst11;
934 gconst11=IKsign(((IkReal(-0.108025000000000))+(((IkReal(-0.0512000000000000))*(cj3)))+(((IkReal(-0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(-0.0120000000000000))*(sj3)))+(((IkReal(-0.00640000000000000))*((cj3)*(cj3))))));
935 dummyeval[0]=((IkReal(-16.8789062500000))+(((IkReal(-1.87500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-8.00000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
936 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
937 {
938 continue;
939 
940 } else
941 {
942 {
943 IkReal j2array[1], cj2array[1], sj2array[1];
944 bool j2valid[1]={false};
945 _nj2 = 1;
946 IkReal x215=((r02)*(sj0));
947 IkReal x216=((IkReal(0.0800000000000000))*(sj1));
948 IkReal x217=((cj3)*(px));
949 IkReal x218=((cj0)*(r01));
950 IkReal x219=((IkReal(0.0240000000000000))*(cj1));
951 IkReal x220=((pz)*(sj3));
952 IkReal x221=((IkReal(0.00600000000000000))*(cj1));
953 IkReal x222=((IkReal(0.0750000000000000))*(sj1));
954 IkReal x223=((py)*(sj0));
955 IkReal x224=((IkReal(0.00562500000000000))*(cj1));
956 IkReal x225=((r00)*(sj0));
957 IkReal x226=((cj3)*(pz));
958 IkReal x227=((IkReal(0.320000000000000))*(sj1));
959 IkReal x228=((IkReal(0.0800000000000000))*(cj1));
960 IkReal x229=((IkReal(0.0750000000000000))*(cj1));
961 IkReal x230=((py)*(r00));
962 IkReal x231=((IkReal(0.320000000000000))*(cj1));
963 IkReal x232=((cj0)*(px));
964 IkReal x233=((IkReal(0.00600000000000000))*(sj1));
965 IkReal x234=((cj1)*(px)*(r01));
966 IkReal x235=((cj0)*(py)*(r02));
967 IkReal x236=((px)*(sj3)*(x216));
968 if( IKabs(((gconst11)*(((IkReal(0.0960000000000000))+(((IkReal(-1.00000000000000))*(pz)*(x222)*(x225)))+(((IkReal(-1.00000000000000))*(pz)*(x231)))+(((pz)*(x218)*(x222)))+(((IkReal(-1.00000000000000))*(x223)*(x227)))+(((x215)*(x236)))+(((cj3)*(x233)))+(((x216)*(x218)*(x220)))+(((IkReal(-1.00000000000000))*(sj3)*(x221)*(x225)))+(((px)*(x215)*(x222)))+(((sj3)*(x228)*(x230)))+(((x229)*(x230)))+(((IkReal(-1.00000000000000))*(x216)*(x220)*(x225)))+(((IkReal(-1.00000000000000))*(cj3)*(x216)*(x223)))+(((IkReal(-1.00000000000000))*(x227)*(x232)))+(((sj3)*(x218)*(x221)))+(((IkReal(0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(sj3)*(x228)))+(((IkReal(-1.00000000000000))*(x226)*(x228)))+(((IkReal(-1.00000000000000))*(x222)*(x235)))+(((IkReal(-1.00000000000000))*(sj3)*(x216)*(x235)))+(((IkReal(-1.00000000000000))*(cj0)*(x216)*(x217)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x229)))+(((IkReal(-1.00000000000000))*(x224)*(x225)))+(((IkReal(0.0240000000000000))*(cj3)))+(((x218)*(x224))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((IkReal(0.0225000000000000))+(((IkReal(-1.00000000000000))*(pz)*(x218)*(x227)))+(((x216)*(x225)*(x226)))+(((IkReal(-1.00000000000000))*(x222)*(x232)))+(((IkReal(0.00562500000000000))*(sj1)))+(((cj3)*(x221)*(x225)))+(((IkReal(-1.00000000000000))*(x222)*(x223)))+(((IkReal(-1.00000000000000))*(cj3)*(x218)*(x221)))+(((x219)*(x225)))+(((r01)*(x217)*(x228)))+(((sj3)*(x233)))+(((IkReal(-1.00000000000000))*(x218)*(x219)))+(((IkReal(-1.00000000000000))*(x230)*(x231)))+(((IkReal(-1.00000000000000))*(sj3)*(x216)*(x232)))+(((IkReal(-1.00000000000000))*(x215)*(x216)*(x217)))+(((px)*(r01)*(x231)))+(((IkReal(-1.00000000000000))*(x220)*(x228)))+(((IkReal(-1.00000000000000))*(sj3)*(x216)*(x223)))+(((x227)*(x235)))+(((IkReal(-1.00000000000000))*(pz)*(x229)))+(((IkReal(-1.00000000000000))*(x216)*(x218)*(x226)))+(((IkReal(-1.00000000000000))*(cj3)*(x228)*(x230)))+(((cj3)*(x216)*(x235)))+(((IkReal(-1.00000000000000))*(px)*(x215)*(x227)))+(((IkReal(0.0240000000000000))*(sj3)))+(((pz)*(x225)*(x227))))))) < IKFAST_ATAN2_MAGTHRESH )
969  continue;
970 j2array[0]=IKatan2(((gconst11)*(((IkReal(0.0960000000000000))+(((IkReal(-1.00000000000000))*(pz)*(x222)*(x225)))+(((IkReal(-1.00000000000000))*(pz)*(x231)))+(((pz)*(x218)*(x222)))+(((IkReal(-1.00000000000000))*(x223)*(x227)))+(((x215)*(x236)))+(((cj3)*(x233)))+(((x216)*(x218)*(x220)))+(((IkReal(-1.00000000000000))*(sj3)*(x221)*(x225)))+(((px)*(x215)*(x222)))+(((sj3)*(x228)*(x230)))+(((x229)*(x230)))+(((IkReal(-1.00000000000000))*(x216)*(x220)*(x225)))+(((IkReal(-1.00000000000000))*(cj3)*(x216)*(x223)))+(((IkReal(-1.00000000000000))*(x227)*(x232)))+(((sj3)*(x218)*(x221)))+(((IkReal(0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(sj3)*(x228)))+(((IkReal(-1.00000000000000))*(x226)*(x228)))+(((IkReal(-1.00000000000000))*(x222)*(x235)))+(((IkReal(-1.00000000000000))*(sj3)*(x216)*(x235)))+(((IkReal(-1.00000000000000))*(cj0)*(x216)*(x217)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x229)))+(((IkReal(-1.00000000000000))*(x224)*(x225)))+(((IkReal(0.0240000000000000))*(cj3)))+(((x218)*(x224)))))), ((gconst11)*(((IkReal(0.0225000000000000))+(((IkReal(-1.00000000000000))*(pz)*(x218)*(x227)))+(((x216)*(x225)*(x226)))+(((IkReal(-1.00000000000000))*(x222)*(x232)))+(((IkReal(0.00562500000000000))*(sj1)))+(((cj3)*(x221)*(x225)))+(((IkReal(-1.00000000000000))*(x222)*(x223)))+(((IkReal(-1.00000000000000))*(cj3)*(x218)*(x221)))+(((x219)*(x225)))+(((r01)*(x217)*(x228)))+(((sj3)*(x233)))+(((IkReal(-1.00000000000000))*(x218)*(x219)))+(((IkReal(-1.00000000000000))*(x230)*(x231)))+(((IkReal(-1.00000000000000))*(sj3)*(x216)*(x232)))+(((IkReal(-1.00000000000000))*(x215)*(x216)*(x217)))+(((px)*(r01)*(x231)))+(((IkReal(-1.00000000000000))*(x220)*(x228)))+(((IkReal(-1.00000000000000))*(sj3)*(x216)*(x223)))+(((x227)*(x235)))+(((IkReal(-1.00000000000000))*(pz)*(x229)))+(((IkReal(-1.00000000000000))*(x216)*(x218)*(x226)))+(((IkReal(-1.00000000000000))*(cj3)*(x228)*(x230)))+(((cj3)*(x216)*(x235)))+(((IkReal(-1.00000000000000))*(px)*(x215)*(x227)))+(((IkReal(0.0240000000000000))*(sj3)))+(((pz)*(x225)*(x227)))))));
971 sj2array[0]=IKsin(j2array[0]);
972 cj2array[0]=IKcos(j2array[0]);
973 if( j2array[0] > IKPI )
974 {
975  j2array[0]-=IK2PI;
976 }
977 else if( j2array[0] < -IKPI )
978 { j2array[0]+=IK2PI;
979 }
980 j2valid[0] = true;
981 for(int ij2 = 0; ij2 < 1; ++ij2)
982 {
983 if( !j2valid[ij2] )
984 {
985  continue;
986 }
987 _ij2[0] = ij2; _ij2[1] = -1;
988 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
989 {
990 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
991 {
992  j2valid[iij2]=false; _ij2[1] = iij2; break;
993 }
994 }
995 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
996 {
997 IkReal evalcond[4];
998 IkReal x237=IKcos(j2);
999 IkReal x238=IKsin(j2);
1000 IkReal x239=((IkReal(0.0800000000000000))*(sj3));
1001 IkReal x240=((cj0)*(r01));
1002 IkReal x241=((IkReal(1.00000000000000))*(px));
1003 IkReal x242=((py)*(sj1));
1004 IkReal x243=((cj0)*(r02));
1005 IkReal x244=((IkReal(0.0750000000000000))*(cj1));
1006 IkReal x245=((r02)*(sj0));
1007 IkReal x246=((IkReal(0.0750000000000000))*(sj1));
1008 IkReal x247=((cj1)*(pz));
1009 IkReal x248=((r00)*(sj0));
1010 IkReal x249=((IkReal(0.0800000000000000))*(cj3));
1011 IkReal x250=((pz)*(sj1));
1012 IkReal x251=((IkReal(1.00000000000000))*(sj0));
1013 IkReal x252=((cj1)*(py));
1014 IkReal x253=((IkReal(0.320000000000000))*(x237));
1015 IkReal x254=((IkReal(0.0750000000000000))*(x238));
1016 IkReal x255=((IkReal(0.320000000000000))*(x238));
1017 IkReal x256=((IkReal(0.0750000000000000))*(x237));
1018 IkReal x257=((sj1)*(x248));
1019 IkReal x258=((x237)*(x249));
1020 IkReal x259=((x238)*(x239));
1021 IkReal x260=((x237)*(x239));
1022 IkReal x261=((x238)*(x249));
1023 IkReal x262=((x259)+(x254));
1024 IkReal x263=((x258)+(x253));
1025 IkReal x264=((x255)+(x256)+(x261)+(x260));
1026 evalcond[0]=((x250)+(((IkReal(-1.00000000000000))*(x251)*(x252)))+(x244)+(((IkReal(-1.00000000000000))*(x262)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x241)))+(x263));
1027 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x247)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x241)))+(((IkReal(-1.00000000000000))*(x242)*(x251)))+(x246)+(x264));
1028 evalcond[2]=((((x244)*(x248)))+(((IkReal(-1.00000000000000))*(x240)*(x244)))+(((IkReal(-1.00000000000000))*(x240)*(x250)))+(((IkReal(-1.00000000000000))*(r00)*(x252)))+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(sj1)*(x241)*(x245)))+(((x248)*(x250)))+(((x242)*(x243)))+(((IkReal(-1.00000000000000))*(x262)))+(x263));
1029 evalcond[3]=((((IkReal(-1.00000000000000))*(x240)*(x247)))+(((x247)*(x248)))+(((IkReal(-1.00000000000000))*(cj1)*(x241)*(x245)))+(((x243)*(x252)))+(((r00)*(x242)))+(((IkReal(-1.00000000000000))*(x264)))+(((IkReal(-0.300000000000000))*(x248)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x241)))+(((IkReal(0.300000000000000))*(x240)))+(((x240)*(x246)))+(((IkReal(-1.00000000000000))*(x246)*(x248))));
1030 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1031 {
1032 continue;
1033 }
1034 }
1035 
1036 {
1037 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1038 vinfos[0].jointtype = 1;
1039 vinfos[0].foffset = j0;
1040 vinfos[0].indices[0] = _ij0[0];
1041 vinfos[0].indices[1] = _ij0[1];
1042 vinfos[0].maxsolutions = _nj0;
1043 vinfos[1].jointtype = 1;
1044 vinfos[1].foffset = j1;
1045 vinfos[1].indices[0] = _ij1[0];
1046 vinfos[1].indices[1] = _ij1[1];
1047 vinfos[1].maxsolutions = _nj1;
1048 vinfos[2].jointtype = 1;
1049 vinfos[2].foffset = j2;
1050 vinfos[2].indices[0] = _ij2[0];
1051 vinfos[2].indices[1] = _ij2[1];
1052 vinfos[2].maxsolutions = _nj2;
1053 vinfos[3].jointtype = 1;
1054 vinfos[3].foffset = j3;
1055 vinfos[3].indices[0] = _ij3[0];
1056 vinfos[3].indices[1] = _ij3[1];
1057 vinfos[3].maxsolutions = _nj3;
1058 vinfos[4].jointtype = 1;
1059 vinfos[4].foffset = j4;
1060 vinfos[4].indices[0] = _ij4[0];
1061 vinfos[4].indices[1] = _ij4[1];
1062 vinfos[4].maxsolutions = _nj4;
1063 std::vector<int> vfree(0);
1064 solutions.AddSolution(vinfos,vfree);
1065 }
1066 }
1067 }
1068 
1069 }
1070 
1071 }
1072 
1073 } else
1074 {
1075 {
1076 IkReal j2array[1], cj2array[1], sj2array[1];
1077 bool j2valid[1]={false};
1078 _nj2 = 1;
1079 IkReal x265=((IkReal(0.0800000000000000))*(cj1));
1080 IkReal x266=((cj0)*(px));
1081 IkReal x267=((py)*(sj0));
1082 IkReal x268=((IkReal(0.320000000000000))*(sj1));
1083 IkReal x269=((IkReal(0.00600000000000000))*(cj3));
1084 IkReal x270=((pz)*(sj3));
1085 IkReal x271=((IkReal(0.0750000000000000))*(cj1));
1086 IkReal x272=((IkReal(0.0800000000000000))*(sj1));
1087 IkReal x273=((IkReal(0.0750000000000000))*(sj1));
1088 IkReal x274=((IkReal(0.00600000000000000))*(sj3));
1089 IkReal x275=((cj3)*(pz));
1090 IkReal x276=((IkReal(0.320000000000000))*(cj1));
1091 if( IKabs(((gconst10)*(((IkReal(-0.0960000000000000))+(((cj3)*(x266)*(x272)))+(((cj3)*(x267)*(x272)))+(((x266)*(x268)))+(((IkReal(-1.00000000000000))*(sj3)*(x265)*(x266)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x265)*(x267)))+(((x270)*(x272)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x267)*(x271)))+(((pz)*(x273)))+(((IkReal(-1.00000000000000))*(x266)*(x271)))+(((pz)*(x276)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(sj1)*(x269)))+(((x265)*(x275)))+(((x267)*(x268)))+(((cj1)*(x274))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((IkReal(-0.0225000000000000))+(((sj3)*(x266)*(x272)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((x267)*(x273)))+(((x267)*(x276)))+(((x266)*(x276)))+(((IkReal(-1.00000000000000))*(pz)*(x268)))+(((x266)*(x273)))+(((x265)*(x270)))+(((IkReal(-1.00000000000000))*(sj1)*(x274)))+(((cj3)*(x265)*(x267)))+(((sj3)*(x267)*(x272)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((cj3)*(x265)*(x266)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x272)*(x275)))+(((IkReal(-1.00000000000000))*(cj1)*(x269)))+(((pz)*(x271))))))) < IKFAST_ATAN2_MAGTHRESH )
1092  continue;
1093 j2array[0]=IKatan2(((gconst10)*(((IkReal(-0.0960000000000000))+(((cj3)*(x266)*(x272)))+(((cj3)*(x267)*(x272)))+(((x266)*(x268)))+(((IkReal(-1.00000000000000))*(sj3)*(x265)*(x266)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x265)*(x267)))+(((x270)*(x272)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x267)*(x271)))+(((pz)*(x273)))+(((IkReal(-1.00000000000000))*(x266)*(x271)))+(((pz)*(x276)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(sj1)*(x269)))+(((x265)*(x275)))+(((x267)*(x268)))+(((cj1)*(x274)))))), ((gconst10)*(((IkReal(-0.0225000000000000))+(((sj3)*(x266)*(x272)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((x267)*(x273)))+(((x267)*(x276)))+(((x266)*(x276)))+(((IkReal(-1.00000000000000))*(pz)*(x268)))+(((x266)*(x273)))+(((x265)*(x270)))+(((IkReal(-1.00000000000000))*(sj1)*(x274)))+(((cj3)*(x265)*(x267)))+(((sj3)*(x267)*(x272)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((cj3)*(x265)*(x266)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x272)*(x275)))+(((IkReal(-1.00000000000000))*(cj1)*(x269)))+(((pz)*(x271)))))));
1094 sj2array[0]=IKsin(j2array[0]);
1095 cj2array[0]=IKcos(j2array[0]);
1096 if( j2array[0] > IKPI )
1097 {
1098  j2array[0]-=IK2PI;
1099 }
1100 else if( j2array[0] < -IKPI )
1101 { j2array[0]+=IK2PI;
1102 }
1103 j2valid[0] = true;
1104 for(int ij2 = 0; ij2 < 1; ++ij2)
1105 {
1106 if( !j2valid[ij2] )
1107 {
1108  continue;
1109 }
1110 _ij2[0] = ij2; _ij2[1] = -1;
1111 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
1112 {
1113 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
1114 {
1115  j2valid[iij2]=false; _ij2[1] = iij2; break;
1116 }
1117 }
1118 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
1119 {
1120 IkReal evalcond[4];
1121 IkReal x277=IKcos(j2);
1122 IkReal x278=IKsin(j2);
1123 IkReal x279=((IkReal(0.0800000000000000))*(sj3));
1124 IkReal x280=((cj0)*(r01));
1125 IkReal x281=((IkReal(1.00000000000000))*(px));
1126 IkReal x282=((py)*(sj1));
1127 IkReal x283=((cj0)*(r02));
1128 IkReal x284=((IkReal(0.0750000000000000))*(cj1));
1129 IkReal x285=((r02)*(sj0));
1130 IkReal x286=((IkReal(0.0750000000000000))*(sj1));
1131 IkReal x287=((cj1)*(pz));
1132 IkReal x288=((r00)*(sj0));
1133 IkReal x289=((IkReal(0.0800000000000000))*(cj3));
1134 IkReal x290=((pz)*(sj1));
1135 IkReal x291=((IkReal(1.00000000000000))*(sj0));
1136 IkReal x292=((cj1)*(py));
1137 IkReal x293=((IkReal(0.320000000000000))*(x277));
1138 IkReal x294=((IkReal(0.0750000000000000))*(x278));
1139 IkReal x295=((IkReal(0.320000000000000))*(x278));
1140 IkReal x296=((IkReal(0.0750000000000000))*(x277));
1141 IkReal x297=((sj1)*(x288));
1142 IkReal x298=((x277)*(x289));
1143 IkReal x299=((x278)*(x279));
1144 IkReal x300=((x277)*(x279));
1145 IkReal x301=((x278)*(x289));
1146 IkReal x302=((x299)+(x294));
1147 IkReal x303=((x298)+(x293));
1148 IkReal x304=((x300)+(x301)+(x295)+(x296));
1149 evalcond[0]=((((IkReal(-1.00000000000000))*(x302)))+(x284)+(x303)+(((IkReal(-1.00000000000000))*(x291)*(x292)))+(x290)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x281))));
1150 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x282)*(x291)))+(x286)+(x304)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x281)))+(((IkReal(-1.00000000000000))*(x287))));
1151 evalcond[2]=((((IkReal(-1.00000000000000))*(x302)))+(((x288)*(x290)))+(x303)+(((x282)*(x283)))+(((IkReal(-1.00000000000000))*(sj1)*(x281)*(x285)))+(((IkReal(-1.00000000000000))*(x280)*(x284)))+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(x280)*(x290)))+(((x284)*(x288)))+(((IkReal(-1.00000000000000))*(r00)*(x292))));
1152 evalcond[3]=((((x280)*(x286)))+(((IkReal(-1.00000000000000))*(x280)*(x287)))+(((IkReal(-1.00000000000000))*(x286)*(x288)))+(((IkReal(-1.00000000000000))*(x304)))+(((IkReal(0.300000000000000))*(x280)))+(((x283)*(x292)))+(((r00)*(x282)))+(((IkReal(-1.00000000000000))*(cj1)*(x281)*(x285)))+(((x287)*(x288)))+(((IkReal(-0.300000000000000))*(x288)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x281))));
1153 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1154 {
1155 continue;
1156 }
1157 }
1158 
1159 {
1160 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1161 vinfos[0].jointtype = 1;
1162 vinfos[0].foffset = j0;
1163 vinfos[0].indices[0] = _ij0[0];
1164 vinfos[0].indices[1] = _ij0[1];
1165 vinfos[0].maxsolutions = _nj0;
1166 vinfos[1].jointtype = 1;
1167 vinfos[1].foffset = j1;
1168 vinfos[1].indices[0] = _ij1[0];
1169 vinfos[1].indices[1] = _ij1[1];
1170 vinfos[1].maxsolutions = _nj1;
1171 vinfos[2].jointtype = 1;
1172 vinfos[2].foffset = j2;
1173 vinfos[2].indices[0] = _ij2[0];
1174 vinfos[2].indices[1] = _ij2[1];
1175 vinfos[2].maxsolutions = _nj2;
1176 vinfos[3].jointtype = 1;
1177 vinfos[3].foffset = j3;
1178 vinfos[3].indices[0] = _ij3[0];
1179 vinfos[3].indices[1] = _ij3[1];
1180 vinfos[3].maxsolutions = _nj3;
1181 vinfos[4].jointtype = 1;
1182 vinfos[4].foffset = j4;
1183 vinfos[4].indices[0] = _ij4[0];
1184 vinfos[4].indices[1] = _ij4[1];
1185 vinfos[4].maxsolutions = _nj4;
1186 std::vector<int> vfree(0);
1187 solutions.AddSolution(vinfos,vfree);
1188 }
1189 }
1190 }
1191 
1192 }
1193 
1194 }
1195 }
1196 }
1197 
1198 } else
1199 {
1200 if( 1 )
1201 {
1202 continue;
1203 
1204 } else
1205 {
1206 }
1207 }
1208 }
1209 }
1210 
1211 } else
1212 {
1213 {
1214 IkReal j3array[1], cj3array[1], sj3array[1];
1215 bool j3valid[1]={false};
1216 _nj3 = 1;
1217 IkReal x305=((r01)*(sj0));
1218 IkReal x306=((cj0)*(r00));
1219 IkReal x307=((IkReal(12800.0000000000))*(pz));
1220 IkReal x308=((IkReal(900.000000000000))*(cj1));
1221 IkReal x309=((IkReal(3000.00000000000))*(pz));
1222 IkReal x310=((IkReal(3840.00000000000))*(sj1));
1223 IkReal x311=((IkReal(900.000000000000))*(sj1));
1224 IkReal x312=((px)*(r00));
1225 IkReal x313=((IkReal(3840.00000000000))*(cj1));
1226 IkReal x314=((py)*(r01));
1227 IkReal x315=((py)*(r02)*(sj0));
1228 IkReal x316=((cj0)*(px)*(r02));
1229 if( IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(r02)*(x307)))+(((x306)*(x310)))+(((IkReal(-1.00000000000000))*(r02)*(x311)))+(((x305)*(x310)))+(((IkReal(-1.00000000000000))*(x305)*(x309)))+(((IkReal(3000.00000000000))*(x315)))+(((IkReal(-12800.0000000000))*(x314)))+(((x305)*(x308)))+(((IkReal(3000.00000000000))*(x316)))+(((IkReal(-12800.0000000000))*(x312)))+(((IkReal(960.000000000000))*(x306)))+(((IkReal(-225.000000000000))*(r02)))+(((r02)*(x313)))+(((x306)*(x308)))+(((IkReal(960.000000000000))*(x305)))+(((IkReal(-1.00000000000000))*(x306)*(x309)))+(((IkReal(-240.000000000000))*(cj4))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(x306)*(x307)))+(((IkReal(-1.00000000000000))*(x305)*(x307)))+(((IkReal(-960.000000000000))*(r02)))+(((IkReal(3000.00000000000))*(x314)))+(((x305)*(x313)))+(((IkReal(-225.000000000000))*(x306)))+(((IkReal(-1.00000000000000))*(r02)*(x308)))+(((IkReal(3000.00000000000))*(x312)))+(((IkReal(-225.000000000000))*(x305)))+(((IkReal(12800.0000000000))*(x316)))+(((r02)*(x309)))+(((IkReal(12800.0000000000))*(x315)))+(((IkReal(-1.00000000000000))*(r02)*(x310)))+(((IkReal(-1024.00000000000))*(cj4)))+(((IkReal(-1.00000000000000))*(x305)*(x311)))+(((IkReal(-1.00000000000000))*(x306)*(x311)))+(((x306)*(x313))))))) < IKFAST_ATAN2_MAGTHRESH )
1230  continue;
1231 j3array[0]=IKatan2(((gconst1)*(((((IkReal(-1.00000000000000))*(r02)*(x307)))+(((x306)*(x310)))+(((IkReal(-1.00000000000000))*(r02)*(x311)))+(((x305)*(x310)))+(((IkReal(-1.00000000000000))*(x305)*(x309)))+(((IkReal(3000.00000000000))*(x315)))+(((IkReal(-12800.0000000000))*(x314)))+(((x305)*(x308)))+(((IkReal(3000.00000000000))*(x316)))+(((IkReal(-12800.0000000000))*(x312)))+(((IkReal(960.000000000000))*(x306)))+(((IkReal(-225.000000000000))*(r02)))+(((r02)*(x313)))+(((x306)*(x308)))+(((IkReal(960.000000000000))*(x305)))+(((IkReal(-1.00000000000000))*(x306)*(x309)))+(((IkReal(-240.000000000000))*(cj4)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(x306)*(x307)))+(((IkReal(-1.00000000000000))*(x305)*(x307)))+(((IkReal(-960.000000000000))*(r02)))+(((IkReal(3000.00000000000))*(x314)))+(((x305)*(x313)))+(((IkReal(-225.000000000000))*(x306)))+(((IkReal(-1.00000000000000))*(r02)*(x308)))+(((IkReal(3000.00000000000))*(x312)))+(((IkReal(-225.000000000000))*(x305)))+(((IkReal(12800.0000000000))*(x316)))+(((r02)*(x309)))+(((IkReal(12800.0000000000))*(x315)))+(((IkReal(-1.00000000000000))*(r02)*(x310)))+(((IkReal(-1024.00000000000))*(cj4)))+(((IkReal(-1.00000000000000))*(x305)*(x311)))+(((IkReal(-1.00000000000000))*(x306)*(x311)))+(((x306)*(x313)))))));
1232 sj3array[0]=IKsin(j3array[0]);
1233 cj3array[0]=IKcos(j3array[0]);
1234 if( j3array[0] > IKPI )
1235 {
1236  j3array[0]-=IK2PI;
1237 }
1238 else if( j3array[0] < -IKPI )
1239 { j3array[0]+=IK2PI;
1240 }
1241 j3valid[0] = true;
1242 for(int ij3 = 0; ij3 < 1; ++ij3)
1243 {
1244 if( !j3valid[ij3] )
1245 {
1246  continue;
1247 }
1248 _ij3[0] = ij3; _ij3[1] = -1;
1249 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
1250 {
1251 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1252 {
1253  j3valid[iij3]=false; _ij3[1] = iij3; break;
1254 }
1255 }
1256 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1257 {
1258 IkReal evalcond[4];
1259 IkReal x317=IKcos(j3);
1260 IkReal x318=IKsin(j3);
1261 IkReal x319=((r01)*(sj0));
1262 IkReal x320=((IkReal(0.300000000000000))*(r02));
1263 IkReal x321=((r00)*(sj0));
1264 IkReal x322=((py)*(sj0));
1265 IkReal x323=((IkReal(0.600000000000000))*(cj1));
1266 IkReal x324=((IkReal(0.150000000000000))*(px));
1267 IkReal x325=((IkReal(0.320000000000000))*(cj4));
1268 IkReal x326=((IkReal(1.00000000000000))*(pz));
1269 IkReal x327=((py)*(r00));
1270 IkReal x328=((IkReal(0.300000000000000))*(cj1));
1271 IkReal x329=((IkReal(1.00000000000000))*(pp));
1272 IkReal x330=((cj0)*(r00));
1273 IkReal x331=((IkReal(0.0450000000000000))*(sj1));
1274 IkReal x332=((IkReal(0.600000000000000))*(sj1));
1275 IkReal x333=((IkReal(0.0750000000000000))*(cj4));
1276 IkReal x334=((cj0)*(r01));
1277 IkReal x335=((IkReal(2.00000000000000))*(pz));
1278 IkReal x336=((cj0)*(px));
1279 IkReal x337=((IkReal(0.300000000000000))*(sj1));
1280 IkReal x338=((IkReal(2.00000000000000))*(px)*(py));
1281 IkReal x339=((IkReal(0.0120000000000000))*(x318));
1282 IkReal x340=((IkReal(0.0512000000000000))*(x317));
1283 IkReal x341=((cj0)*(py)*(r02));
1284 IkReal x342=((px)*(r02)*(sj0));
1285 evalcond[0]=((IkReal(0.0188000000000000))+(((IkReal(-1.00000000000000))*(x329)))+(((pz)*(x323)))+(((cj0)*(x324)))+(((IkReal(0.150000000000000))*(x322)))+(((IkReal(-1.00000000000000))*(x331)))+(x339)+(((x332)*(x336)))+(x340)+(((x322)*(x332))));
1286 evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(r02)*(x326)))+(((IkReal(0.0750000000000000))*(x319)))+(((cj1)*(x320)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((x330)*(x337)))+(((IkReal(0.0750000000000000))*(x330)))+(((IkReal(-1.00000000000000))*(x318)*(x325)))+(((x317)*(x333)))+(((x319)*(x337))));
1287 evalcond[2]=((((IkReal(-1.00000000000000))*(x318)*(x333)))+(((x319)*(x328)))+(((IkReal(-0.0800000000000000))*(cj4)))+(((IkReal(-1.00000000000000))*(sj1)*(x320)))+(((IkReal(-1.00000000000000))*(x326)*(x330)))+(((r02)*(x322)))+(((IkReal(-0.0750000000000000))*(r02)))+(((r02)*(x336)))+(((IkReal(-1.00000000000000))*(x319)*(x326)))+(((x328)*(x330)))+(((IkReal(-1.00000000000000))*(x317)*(x325))));
1288 evalcond[3]=((((IkReal(0.150000000000000))*(x327)))+(((x335)*(x342)))+(((IkReal(2.00000000000000))*(x321)*((px)*(px))))+(((IkReal(-2.00000000000000))*(x334)*((py)*(py))))+(((pp)*(x334)))+(((IkReal(-1.00000000000000))*(sj4)*(x340)))+(((IkReal(-0.0956250000000000))*(x321)))+(((IkReal(-1.00000000000000))*(x321)*(x329)))+(((IkReal(-1.00000000000000))*(x321)*(x331)))+(((pz)*(x321)*(x323)))+(((x323)*(x341)))+(((x331)*(x334)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x332)))+(((x327)*(x332)))+(((IkReal(0.0956250000000000))*(x334)))+(((IkReal(-0.114425000000000))*(sj4)))+(((IkReal(-1.00000000000000))*(pz)*(x323)*(x334)))+(((IkReal(-1.00000000000000))*(x335)*(x341)))+(((IkReal(-1.00000000000000))*(sj4)*(x339)))+(((IkReal(-1.00000000000000))*(x323)*(x342)))+(((IkReal(-2.00000000000000))*(x327)*(x336)))+(((IkReal(-1.00000000000000))*(r01)*(x324)))+(((x319)*(x338))));
1289 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1290 {
1291 continue;
1292 }
1293 }
1294 
1295 {
1296 IkReal dummyeval[1];
1297 IkReal gconst2;
1298 gconst2=IKsign(((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3))))));
1299 dummyeval[0]=((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3)))));
1300 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1301 {
1302 {
1303 IkReal dummyeval[1];
1304 IkReal gconst3;
1305 IkReal x343=((IkReal(0.0800000000000000))*(cj4));
1306 gconst3=IKsign(((((IkReal(0.320000000000000))*(cj3)*(cj4)))+(((IkReal(0.0750000000000000))*(cj4)*(sj3)))+(((x343)*((cj3)*(cj3))))+(((x343)*((sj3)*(sj3))))));
1307 IkReal x344=((IkReal(1.06666666666667))*(cj4));
1308 dummyeval[0]=((((IkReal(4.26666666666667))*(cj3)*(cj4)))+(((x344)*((sj3)*(sj3))))+(((x344)*((cj3)*(cj3))))+(((cj4)*(sj3))));
1309 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1310 {
1311 {
1312 IkReal evalcond[11];
1313 IkReal x345=((IkReal(0.0512000000000000))*(cj3));
1314 IkReal x346=((IkReal(0.0120000000000000))*(sj3));
1315 IkReal x347=(py)*(py);
1316 IkReal x348=(px)*(px);
1317 IkReal x349=(pz)*(pz);
1318 IkReal x350=((r01)*(sj0));
1319 IkReal x351=((py)*(r00));
1320 IkReal x352=((pz)*(sj1));
1321 IkReal x353=((py)*(r01));
1322 IkReal x354=((px)*(sj0));
1323 IkReal x355=((IkReal(0.600000000000000))*(r02));
1324 IkReal x356=((IkReal(0.150000000000000))*(cj1));
1325 IkReal x357=((cj0)*(sj1));
1326 IkReal x358=((IkReal(0.150000000000000))*(px));
1327 IkReal x359=((IkReal(2.00000000000000))*(cj1));
1328 IkReal x360=((cj0)*(r01));
1329 IkReal x361=((r02)*(sj1));
1330 IkReal x362=((px)*(r00));
1331 IkReal x363=((IkReal(0.300000000000000))*(r00));
1332 IkReal x364=((IkReal(1.00000000000000))*(pz));
1333 IkReal x365=((r00)*(sj1));
1334 IkReal x366=((cj0)*(r00));
1335 IkReal x367=((cj0)*(cj1));
1336 IkReal x368=((IkReal(1.00000000000000))*(sj1));
1337 IkReal x369=((IkReal(0.0956250000000000))*(r00));
1338 IkReal x370=((IkReal(0.600000000000000))*(pz));
1339 IkReal x371=((IkReal(0.600000000000000))*(sj1));
1340 IkReal x372=((IkReal(2.00000000000000))*(px));
1341 IkReal x373=((IkReal(2.00000000000000))*(sj1));
1342 IkReal x374=((IkReal(0.150000000000000))*(sj1));
1343 IkReal x375=((cj1)*(r02));
1344 IkReal x376=((cj0)*(px));
1345 IkReal x377=((IkReal(0.0843750000000000))*(cj1));
1346 IkReal x378=((py)*(sj0));
1347 IkReal x379=((pz)*(r02));
1348 IkReal x380=((IkReal(1.00000000000000))*(cj1));
1349 IkReal x381=((cj0)*(py));
1350 IkReal x382=((r00)*(sj0));
1351 IkReal x383=((r02)*(x378));
1352 IkReal x384=((pp)*(x380));
1353 IkReal x385=((IkReal(1.00000000000000))*(pp)*(r00));
1354 IkReal x386=((x346)+(x345));
1355 IkReal x387=((IkReal(2.00000000000000))*(r00)*(x348));
1356 IkReal x388=((cj0)*(x372)*(x379));
1357 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
1358 evalcond[1]=((x381)+(((IkReal(-1.00000000000000))*(x354))));
1359 evalcond[2]=((IkReal(-1.00000000000000))+(x360)+(((IkReal(-1.00000000000000))*(x382))));
1360 evalcond[3]=((((IkReal(-1.00000000000000))*(x366)*(x380)))+(x361)+(((IkReal(-1.00000000000000))*(x350)*(x380))));
1361 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x357)))+(((IkReal(-1.00000000000000))*(x375)))+(((IkReal(-1.00000000000000))*(x350)*(x368))));
1362 evalcond[5]=((IkReal(0.0188000000000000))+(x386)+(((IkReal(0.600000000000000))*(px)*(x357)))+(((IkReal(0.150000000000000))*(x378)))+(((IkReal(-1.00000000000000))*(pp)))+(((x371)*(x378)))+(((cj0)*(x358)))+(((cj1)*(x370)))+(((IkReal(-0.0450000000000000))*(sj1))));
1363 evalcond[6]=((((IkReal(0.300000000000000))*(x375)))+(((x357)*(x363)))+(((IkReal(0.0750000000000000))*(x350)))+(((IkReal(-1.00000000000000))*(r02)*(x364)))+(((IkReal(-1.00000000000000))*(x353)))+(((IkReal(0.300000000000000))*(sj1)*(x350)))+(((IkReal(0.0750000000000000))*(x366)))+(((IkReal(-1.00000000000000))*(x362))));
1364 evalcond[7]=((((IkReal(-1.00000000000000))*(x350)*(x364)))+(x383)+(((x363)*(x367)))+(((IkReal(0.300000000000000))*(cj1)*(x350)))+(((r02)*(x376)))+(((IkReal(-0.300000000000000))*(x361)))+(((IkReal(-1.00000000000000))*(x364)*(x366)))+(((IkReal(-0.0750000000000000))*(r02))));
1365 evalcond[8]=((IkReal(-0.114425000000000))+(((IkReal(-2.00000000000000))*(x379)*(x381)))+(((IkReal(-2.00000000000000))*(x347)*(x360)))+(((IkReal(-1.00000000000000))*(sj0)*(x369)))+(((x351)*(x371)))+(((IkReal(-1.00000000000000))*(x386)))+(((IkReal(-1.00000000000000))*(cj1)*(x354)*(x355)))+(((IkReal(0.150000000000000))*(x351)))+(((IkReal(-1.00000000000000))*(pp)*(x382)))+(((py)*(x350)*(x372)))+(((IkReal(-0.0450000000000000))*(sj0)*(x365)))+(((py)*(x355)*(x367)))+(((cj1)*(x370)*(x382)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x371)))+(((IkReal(-1.00000000000000))*(cj0)*(x351)*(x372)))+(((IkReal(-1.00000000000000))*(cj1)*(x360)*(x370)))+(((IkReal(0.0956250000000000))*(x360)))+(((IkReal(0.0450000000000000))*(r01)*(x357)))+(((IkReal(-1.00000000000000))*(r01)*(x358)))+(((IkReal(2.00000000000000))*(x354)*(x379)))+(((IkReal(2.00000000000000))*(x348)*(x382)))+(((pp)*(x360))));
1366 evalcond[9]=((((IkReal(-1.00000000000000))*(x357)*(x385)))+(((x353)*(x357)*(x372)))+(((cj0)*(r02)*(x352)*(x372)))+(((x357)*(x387)))+(((r02)*(x356)*(x376)))+(((IkReal(-0.150000000000000))*(r02)*(x352)))+(((IkReal(-1.00000000000000))*(x358)*(x365)))+(((IkReal(0.0450000000000000))*(x350)))+(((x347)*(x350)*(x373)))+(((IkReal(-0.600000000000000))*(x362)))+(((IkReal(-1.00000000000000))*(pp)*(x375)))+(((x356)*(x383)))+(((pz)*(x353)*(x359)))+(((IkReal(0.0843750000000000))*(x375)))+(((IkReal(-1.00000000000000))*(pz)*(x350)*(x356)))+(((r02)*(x349)*(x359)))+(((IkReal(0.0956250000000000))*(sj1)*(x350)))+(((IkReal(2.00000000000000))*(x352)*(x383)))+(((IkReal(-1.00000000000000))*(pz)*(x356)*(x366)))+(((IkReal(-0.600000000000000))*(x353)))+(((IkReal(-1.00000000000000))*(x353)*(x374)))+(((x357)*(x369)))+(((x351)*(x354)*(x373)))+(((IkReal(-1.00000000000000))*(pz)*(x355)))+(((IkReal(-1.00000000000000))*(pp)*(x350)*(x368)))+(((IkReal(0.0450000000000000))*(x366)))+(((pz)*(x359)*(x362))));
1367 evalcond[10]=((((IkReal(-1.00000000000000))*(x356)*(x379)))+(((IkReal(-1.00000000000000))*(x350)*(x384)))+(((x347)*(x350)*(x359)))+(((IkReal(-1.00000000000000))*(x366)*(x384)))+(((IkReal(0.150000000000000))*(x352)*(x366)))+(((x366)*(x370)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x355)*(x378)))+(((x353)*(x359)*(x376)))+(((IkReal(-1.00000000000000))*(x356)*(x362)))+(((IkReal(-2.00000000000000))*(x349)*(x361)))+(((x359)*(x378)*(x379)))+(((x359)*(x376)*(x379)))+(((x351)*(x354)*(x359)))+(((x348)*(x359)*(x366)))+(((IkReal(-1.00000000000000))*(x350)*(x377)))+(((IkReal(-2.00000000000000))*(x352)*(x362)))+(((IkReal(-1.00000000000000))*(x353)*(x356)))+(((pp)*(x361)))+(((IkReal(-1.00000000000000))*(r02)*(x357)*(x358)))+(((IkReal(0.0956250000000000))*(x361)))+(((IkReal(-1.00000000000000))*(x366)*(x377)))+(((IkReal(-0.150000000000000))*(x361)*(x378)))+(((x350)*(x370)))+(((IkReal(-2.00000000000000))*(x352)*(x353)))+(((IkReal(-1.00000000000000))*(x355)*(x376)))+(((IkReal(0.150000000000000))*(x350)*(x352))));
1368 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 )
1369 {
1370 {
1371 IkReal dummyeval[1];
1372 IkReal gconst4;
1373 gconst4=IKsign(((IkReal(0.108025000000000))+(((IkReal(0.0512000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
1374 dummyeval[0]=((IkReal(16.8789062500000))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3))+(((IkReal(8.00000000000000))*(cj3))));
1375 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1376 {
1377 {
1378 IkReal dummyeval[1];
1379 IkReal gconst5;
1380 gconst5=IKsign(((IkReal(0.108025000000000))+(((IkReal(0.0512000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
1381 dummyeval[0]=((IkReal(16.8789062500000))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3))+(((IkReal(8.00000000000000))*(cj3))));
1382 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1383 {
1384 continue;
1385 
1386 } else
1387 {
1388 {
1389 IkReal j2array[1], cj2array[1], sj2array[1];
1390 bool j2valid[1]={false};
1391 _nj2 = 1;
1392 IkReal x389=((r02)*(sj0));
1393 IkReal x390=((IkReal(0.0800000000000000))*(sj1));
1394 IkReal x391=((cj3)*(px));
1395 IkReal x392=((cj0)*(r01));
1396 IkReal x393=((IkReal(0.0240000000000000))*(cj1));
1397 IkReal x394=((pz)*(sj3));
1398 IkReal x395=((IkReal(0.00600000000000000))*(cj1));
1399 IkReal x396=((IkReal(0.320000000000000))*(px));
1400 IkReal x397=((cj0)*(sj1));
1401 IkReal x398=((cj1)*(r01));
1402 IkReal x399=((IkReal(0.0750000000000000))*(px));
1403 IkReal x400=((IkReal(0.00562500000000000))*(cj1));
1404 IkReal x401=((py)*(r02));
1405 IkReal x402=((IkReal(0.0750000000000000))*(pz));
1406 IkReal x403=((r00)*(sj0));
1407 IkReal x404=((cj3)*(pz));
1408 IkReal x405=((IkReal(0.320000000000000))*(py));
1409 IkReal x406=((sj0)*(sj1));
1410 IkReal x407=((IkReal(0.0800000000000000))*(cj1));
1411 IkReal x408=((cj1)*(r00));
1412 IkReal x409=((IkReal(0.0750000000000000))*(py));
1413 IkReal x410=((cj3)*(py));
1414 IkReal x411=((IkReal(0.00600000000000000))*(sj1));
1415 IkReal x412=((py)*(sj3));
1416 IkReal x413=((IkReal(0.320000000000000))*(pz)*(sj1));
1417 IkReal x414=((px)*(sj3)*(x390));
1418 if( IKabs(((gconst5)*(((IkReal(-0.0960000000000000))+(((x390)*(x392)*(x394)))+(((sj1)*(x392)*(x402)))+(((x392)*(x400)))+(((r00)*(x407)*(x412)))+(((x405)*(x406)))+(((IkReal(-1.00000000000000))*(x400)*(x403)))+(((cj0)*(x390)*(x391)))+(((IkReal(-0.0750000000000000))*(x397)*(x401)))+(((IkReal(-1.00000000000000))*(sj1)*(x402)*(x403)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((x404)*(x407)))+(((sj1)*(x389)*(x399)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((x389)*(x414)))+(((x408)*(x409)))+(((IkReal(-1.00000000000000))*(x398)*(x399)))+(((IkReal(-0.0800000000000000))*(px)*(sj3)*(x398)))+(((sj0)*(x390)*(x410)))+(((IkReal(-1.00000000000000))*(sj3)*(x395)*(x403)))+(((IkReal(0.320000000000000))*(cj1)*(pz)))+(((IkReal(-1.00000000000000))*(x390)*(x394)*(x403)))+(((x396)*(x397)))+(((IkReal(-1.00000000000000))*(cj3)*(x411)))+(((sj3)*(x392)*(x395)))+(((IkReal(-1.00000000000000))*(cj0)*(sj3)*(x390)*(x401))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x390)*(x392)*(x404)))+(((IkReal(-1.00000000000000))*(x392)*(x393)))+(((cj0)*(x414)))+(((x390)*(x403)*(x404)))+(((cj1)*(x402)))+(((x406)*(x409)))+(((IkReal(0.320000000000000))*(x397)*(x401)))+(((IkReal(-1.00000000000000))*(sj1)*(x389)*(x396)))+(((x396)*(x398)))+(((x397)*(x399)))+(((x394)*(x407)))+(((IkReal(-1.00000000000000))*(x392)*(x413)))+(((IkReal(-1.00000000000000))*(x389)*(x390)*(x391)))+(((IkReal(-1.00000000000000))*(sj3)*(x411)))+(((cj3)*(x395)*(x403)))+(((cj0)*(cj3)*(x390)*(x401)))+(((sj0)*(x390)*(x412)))+(((IkReal(-1.00000000000000))*(x405)*(x408)))+(((x393)*(x403)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(0.0800000000000000))*(x391)*(x398)))+(((x403)*(x413)))+(((IkReal(-1.00000000000000))*(r00)*(x407)*(x410)))+(((IkReal(-1.00000000000000))*(cj3)*(x392)*(x395))))))) < IKFAST_ATAN2_MAGTHRESH )
1419  continue;
1420 j2array[0]=IKatan2(((gconst5)*(((IkReal(-0.0960000000000000))+(((x390)*(x392)*(x394)))+(((sj1)*(x392)*(x402)))+(((x392)*(x400)))+(((r00)*(x407)*(x412)))+(((x405)*(x406)))+(((IkReal(-1.00000000000000))*(x400)*(x403)))+(((cj0)*(x390)*(x391)))+(((IkReal(-0.0750000000000000))*(x397)*(x401)))+(((IkReal(-1.00000000000000))*(sj1)*(x402)*(x403)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((x404)*(x407)))+(((sj1)*(x389)*(x399)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((x389)*(x414)))+(((x408)*(x409)))+(((IkReal(-1.00000000000000))*(x398)*(x399)))+(((IkReal(-0.0800000000000000))*(px)*(sj3)*(x398)))+(((sj0)*(x390)*(x410)))+(((IkReal(-1.00000000000000))*(sj3)*(x395)*(x403)))+(((IkReal(0.320000000000000))*(cj1)*(pz)))+(((IkReal(-1.00000000000000))*(x390)*(x394)*(x403)))+(((x396)*(x397)))+(((IkReal(-1.00000000000000))*(cj3)*(x411)))+(((sj3)*(x392)*(x395)))+(((IkReal(-1.00000000000000))*(cj0)*(sj3)*(x390)*(x401)))))), ((gconst5)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x390)*(x392)*(x404)))+(((IkReal(-1.00000000000000))*(x392)*(x393)))+(((cj0)*(x414)))+(((x390)*(x403)*(x404)))+(((cj1)*(x402)))+(((x406)*(x409)))+(((IkReal(0.320000000000000))*(x397)*(x401)))+(((IkReal(-1.00000000000000))*(sj1)*(x389)*(x396)))+(((x396)*(x398)))+(((x397)*(x399)))+(((x394)*(x407)))+(((IkReal(-1.00000000000000))*(x392)*(x413)))+(((IkReal(-1.00000000000000))*(x389)*(x390)*(x391)))+(((IkReal(-1.00000000000000))*(sj3)*(x411)))+(((cj3)*(x395)*(x403)))+(((cj0)*(cj3)*(x390)*(x401)))+(((sj0)*(x390)*(x412)))+(((IkReal(-1.00000000000000))*(x405)*(x408)))+(((x393)*(x403)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(0.0800000000000000))*(x391)*(x398)))+(((x403)*(x413)))+(((IkReal(-1.00000000000000))*(r00)*(x407)*(x410)))+(((IkReal(-1.00000000000000))*(cj3)*(x392)*(x395)))))));
1421 sj2array[0]=IKsin(j2array[0]);
1422 cj2array[0]=IKcos(j2array[0]);
1423 if( j2array[0] > IKPI )
1424 {
1425  j2array[0]-=IK2PI;
1426 }
1427 else if( j2array[0] < -IKPI )
1428 { j2array[0]+=IK2PI;
1429 }
1430 j2valid[0] = true;
1431 for(int ij2 = 0; ij2 < 1; ++ij2)
1432 {
1433 if( !j2valid[ij2] )
1434 {
1435  continue;
1436 }
1437 _ij2[0] = ij2; _ij2[1] = -1;
1438 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
1439 {
1440 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
1441 {
1442  j2valid[iij2]=false; _ij2[1] = iij2; break;
1443 }
1444 }
1445 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
1446 {
1447 IkReal evalcond[4];
1448 IkReal x415=IKcos(j2);
1449 IkReal x416=IKsin(j2);
1450 IkReal x417=((IkReal(0.0800000000000000))*(sj3));
1451 IkReal x418=((cj0)*(r01));
1452 IkReal x419=((IkReal(1.00000000000000))*(px));
1453 IkReal x420=((py)*(sj1));
1454 IkReal x421=((cj0)*(r02));
1455 IkReal x422=((IkReal(0.0750000000000000))*(cj1));
1456 IkReal x423=((r02)*(sj0));
1457 IkReal x424=((IkReal(0.0750000000000000))*(sj1));
1458 IkReal x425=((cj1)*(pz));
1459 IkReal x426=((r00)*(sj0));
1460 IkReal x427=((IkReal(0.0800000000000000))*(cj3));
1461 IkReal x428=((pz)*(sj1));
1462 IkReal x429=((IkReal(1.00000000000000))*(sj0));
1463 IkReal x430=((cj1)*(py));
1464 IkReal x431=((IkReal(0.0750000000000000))*(x416));
1465 IkReal x432=((IkReal(0.320000000000000))*(x415));
1466 IkReal x433=((IkReal(0.320000000000000))*(x416));
1467 IkReal x434=((IkReal(0.0750000000000000))*(x415));
1468 IkReal x435=((sj1)*(x426));
1469 IkReal x436=((x416)*(x417));
1470 IkReal x437=((x415)*(x427));
1471 IkReal x438=((x415)*(x417));
1472 IkReal x439=((x416)*(x427));
1473 IkReal x440=((x431)+(x436));
1474 IkReal x441=((x432)+(x437));
1475 IkReal x442=((x438)+(x439)+(x433)+(x434));
1476 evalcond[0]=((x428)+(x422)+(x441)+(((IkReal(-1.00000000000000))*(x429)*(x430)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x419)))+(((IkReal(-1.00000000000000))*(x440))));
1477 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x419)))+(((IkReal(-1.00000000000000))*(x420)*(x429)))+(x424)+(x442)+(((IkReal(-1.00000000000000))*(x425))));
1478 evalcond[2]=((((IkReal(-1.00000000000000))*(x418)*(x422)))+(x440)+(((IkReal(-1.00000000000000))*(x418)*(x428)))+(((x420)*(x421)))+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(x441)))+(((IkReal(-1.00000000000000))*(r00)*(x430)))+(((x422)*(x426)))+(((IkReal(-1.00000000000000))*(sj1)*(x419)*(x423)))+(((x426)*(x428))));
1479 evalcond[3]=((((IkReal(-0.300000000000000))*(x426)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x419)))+(((x418)*(x424)))+(((x421)*(x430)))+(((x425)*(x426)))+(x442)+(((IkReal(-1.00000000000000))*(cj1)*(x419)*(x423)))+(((r00)*(x420)))+(((IkReal(0.300000000000000))*(x418)))+(((IkReal(-1.00000000000000))*(x424)*(x426)))+(((IkReal(-1.00000000000000))*(x418)*(x425))));
1480 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1481 {
1482 continue;
1483 }
1484 }
1485 
1486 {
1487 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1488 vinfos[0].jointtype = 1;
1489 vinfos[0].foffset = j0;
1490 vinfos[0].indices[0] = _ij0[0];
1491 vinfos[0].indices[1] = _ij0[1];
1492 vinfos[0].maxsolutions = _nj0;
1493 vinfos[1].jointtype = 1;
1494 vinfos[1].foffset = j1;
1495 vinfos[1].indices[0] = _ij1[0];
1496 vinfos[1].indices[1] = _ij1[1];
1497 vinfos[1].maxsolutions = _nj1;
1498 vinfos[2].jointtype = 1;
1499 vinfos[2].foffset = j2;
1500 vinfos[2].indices[0] = _ij2[0];
1501 vinfos[2].indices[1] = _ij2[1];
1502 vinfos[2].maxsolutions = _nj2;
1503 vinfos[3].jointtype = 1;
1504 vinfos[3].foffset = j3;
1505 vinfos[3].indices[0] = _ij3[0];
1506 vinfos[3].indices[1] = _ij3[1];
1507 vinfos[3].maxsolutions = _nj3;
1508 vinfos[4].jointtype = 1;
1509 vinfos[4].foffset = j4;
1510 vinfos[4].indices[0] = _ij4[0];
1511 vinfos[4].indices[1] = _ij4[1];
1512 vinfos[4].maxsolutions = _nj4;
1513 std::vector<int> vfree(0);
1514 solutions.AddSolution(vinfos,vfree);
1515 }
1516 }
1517 }
1518 
1519 }
1520 
1521 }
1522 
1523 } else
1524 {
1525 {
1526 IkReal j2array[1], cj2array[1], sj2array[1];
1527 bool j2valid[1]={false};
1528 _nj2 = 1;
1529 IkReal x443=((IkReal(0.0800000000000000))*(cj1));
1530 IkReal x444=((cj0)*(px));
1531 IkReal x445=((py)*(sj0));
1532 IkReal x446=((IkReal(0.320000000000000))*(sj1));
1533 IkReal x447=((IkReal(0.00600000000000000))*(cj3));
1534 IkReal x448=((pz)*(sj3));
1535 IkReal x449=((IkReal(0.0750000000000000))*(cj1));
1536 IkReal x450=((IkReal(0.0800000000000000))*(sj1));
1537 IkReal x451=((IkReal(0.0750000000000000))*(sj1));
1538 IkReal x452=((IkReal(0.00600000000000000))*(sj3));
1539 IkReal x453=((cj3)*(pz));
1540 IkReal x454=((IkReal(0.320000000000000))*(cj1));
1541 if( IKabs(((gconst4)*(((IkReal(-0.0960000000000000))+(((x445)*(x446)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(sj3)*(x443)*(x445)))+(((x448)*(x450)))+(((x443)*(x453)))+(((pz)*(x451)))+(((IkReal(-1.00000000000000))*(sj3)*(x443)*(x444)))+(((cj3)*(x444)*(x450)))+(((cj1)*(x452)))+(((IkReal(-1.00000000000000))*(x445)*(x449)))+(((IkReal(-1.00000000000000))*(sj1)*(x447)))+(((IkReal(0.00562500000000000))*(cj1)))+(((x444)*(x446)))+(((pz)*(x454)))+(((cj3)*(x445)*(x450)))+(((IkReal(-1.00000000000000))*(x444)*(x449))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(pz)*(x446)))+(((pz)*(x449)))+(((x445)*(x454)))+(((IkReal(-1.00000000000000))*(cj1)*(x447)))+(((x444)*(x451)))+(((cj3)*(x443)*(x444)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(sj1)*(x452)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x443)*(x448)))+(((cj3)*(x443)*(x445)))+(((x445)*(x451)))+(((sj3)*(x445)*(x450)))+(((sj3)*(x444)*(x450)))+(((IkReal(-1.00000000000000))*(x450)*(x453)))+(((x444)*(x454))))))) < IKFAST_ATAN2_MAGTHRESH )
1542  continue;
1543 j2array[0]=IKatan2(((gconst4)*(((IkReal(-0.0960000000000000))+(((x445)*(x446)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(sj3)*(x443)*(x445)))+(((x448)*(x450)))+(((x443)*(x453)))+(((pz)*(x451)))+(((IkReal(-1.00000000000000))*(sj3)*(x443)*(x444)))+(((cj3)*(x444)*(x450)))+(((cj1)*(x452)))+(((IkReal(-1.00000000000000))*(x445)*(x449)))+(((IkReal(-1.00000000000000))*(sj1)*(x447)))+(((IkReal(0.00562500000000000))*(cj1)))+(((x444)*(x446)))+(((pz)*(x454)))+(((cj3)*(x445)*(x450)))+(((IkReal(-1.00000000000000))*(x444)*(x449)))))), ((gconst4)*(((IkReal(-0.0225000000000000))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(pz)*(x446)))+(((pz)*(x449)))+(((x445)*(x454)))+(((IkReal(-1.00000000000000))*(cj1)*(x447)))+(((x444)*(x451)))+(((cj3)*(x443)*(x444)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(sj1)*(x452)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((x443)*(x448)))+(((cj3)*(x443)*(x445)))+(((x445)*(x451)))+(((sj3)*(x445)*(x450)))+(((sj3)*(x444)*(x450)))+(((IkReal(-1.00000000000000))*(x450)*(x453)))+(((x444)*(x454)))))));
1544 sj2array[0]=IKsin(j2array[0]);
1545 cj2array[0]=IKcos(j2array[0]);
1546 if( j2array[0] > IKPI )
1547 {
1548  j2array[0]-=IK2PI;
1549 }
1550 else if( j2array[0] < -IKPI )
1551 { j2array[0]+=IK2PI;
1552 }
1553 j2valid[0] = true;
1554 for(int ij2 = 0; ij2 < 1; ++ij2)
1555 {
1556 if( !j2valid[ij2] )
1557 {
1558  continue;
1559 }
1560 _ij2[0] = ij2; _ij2[1] = -1;
1561 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
1562 {
1563 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
1564 {
1565  j2valid[iij2]=false; _ij2[1] = iij2; break;
1566 }
1567 }
1568 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
1569 {
1570 IkReal evalcond[4];
1571 IkReal x455=IKcos(j2);
1572 IkReal x456=IKsin(j2);
1573 IkReal x457=((IkReal(0.0800000000000000))*(sj3));
1574 IkReal x458=((cj0)*(r01));
1575 IkReal x459=((IkReal(1.00000000000000))*(px));
1576 IkReal x460=((py)*(sj1));
1577 IkReal x461=((cj0)*(r02));
1578 IkReal x462=((IkReal(0.0750000000000000))*(cj1));
1579 IkReal x463=((r02)*(sj0));
1580 IkReal x464=((IkReal(0.0750000000000000))*(sj1));
1581 IkReal x465=((cj1)*(pz));
1582 IkReal x466=((r00)*(sj0));
1583 IkReal x467=((IkReal(0.0800000000000000))*(cj3));
1584 IkReal x468=((pz)*(sj1));
1585 IkReal x469=((IkReal(1.00000000000000))*(sj0));
1586 IkReal x470=((cj1)*(py));
1587 IkReal x471=((IkReal(0.0750000000000000))*(x456));
1588 IkReal x472=((IkReal(0.320000000000000))*(x455));
1589 IkReal x473=((IkReal(0.320000000000000))*(x456));
1590 IkReal x474=((IkReal(0.0750000000000000))*(x455));
1591 IkReal x475=((sj1)*(x466));
1592 IkReal x476=((x456)*(x457));
1593 IkReal x477=((x455)*(x467));
1594 IkReal x478=((x455)*(x457));
1595 IkReal x479=((x456)*(x467));
1596 IkReal x480=((x476)+(x471));
1597 IkReal x481=((x477)+(x472));
1598 IkReal x482=((x474)+(x473)+(x478)+(x479));
1599 evalcond[0]=((((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x459)))+(x468)+(x462)+(x481)+(((IkReal(-1.00000000000000))*(x480)))+(((IkReal(-1.00000000000000))*(x469)*(x470))));
1600 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x465)))+(x464)+(((IkReal(-1.00000000000000))*(x460)*(x469)))+(x482)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x459))));
1601 evalcond[2]=((((IkReal(-1.00000000000000))*(x458)*(x462)))+(((x460)*(x461)))+(((x466)*(x468)))+(((IkReal(-1.00000000000000))*(sj1)*(x459)*(x463)))+(((cj1)*(px)*(r01)))+(((x462)*(x466)))+(x480)+(((IkReal(-1.00000000000000))*(r00)*(x470)))+(((IkReal(-1.00000000000000))*(x481)))+(((IkReal(-1.00000000000000))*(x458)*(x468))));
1602 evalcond[3]=((((r00)*(x460)))+(((x465)*(x466)))+(((IkReal(-1.00000000000000))*(cj1)*(x459)*(x463)))+(((x458)*(x464)))+(((x461)*(x470)))+(((IkReal(-1.00000000000000))*(x458)*(x465)))+(((IkReal(-0.300000000000000))*(x466)))+(x482)+(((IkReal(0.300000000000000))*(x458)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x459)))+(((IkReal(-1.00000000000000))*(x464)*(x466))));
1603 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1604 {
1605 continue;
1606 }
1607 }
1608 
1609 {
1610 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1611 vinfos[0].jointtype = 1;
1612 vinfos[0].foffset = j0;
1613 vinfos[0].indices[0] = _ij0[0];
1614 vinfos[0].indices[1] = _ij0[1];
1615 vinfos[0].maxsolutions = _nj0;
1616 vinfos[1].jointtype = 1;
1617 vinfos[1].foffset = j1;
1618 vinfos[1].indices[0] = _ij1[0];
1619 vinfos[1].indices[1] = _ij1[1];
1620 vinfos[1].maxsolutions = _nj1;
1621 vinfos[2].jointtype = 1;
1622 vinfos[2].foffset = j2;
1623 vinfos[2].indices[0] = _ij2[0];
1624 vinfos[2].indices[1] = _ij2[1];
1625 vinfos[2].maxsolutions = _nj2;
1626 vinfos[3].jointtype = 1;
1627 vinfos[3].foffset = j3;
1628 vinfos[3].indices[0] = _ij3[0];
1629 vinfos[3].indices[1] = _ij3[1];
1630 vinfos[3].maxsolutions = _nj3;
1631 vinfos[4].jointtype = 1;
1632 vinfos[4].foffset = j4;
1633 vinfos[4].indices[0] = _ij4[0];
1634 vinfos[4].indices[1] = _ij4[1];
1635 vinfos[4].maxsolutions = _nj4;
1636 std::vector<int> vfree(0);
1637 solutions.AddSolution(vinfos,vfree);
1638 }
1639 }
1640 }
1641 
1642 }
1643 
1644 }
1645 
1646 } else
1647 {
1648 IkReal x483=((IkReal(0.0512000000000000))*(cj3));
1649 IkReal x484=((IkReal(0.0120000000000000))*(sj3));
1650 IkReal x485=(py)*(py);
1651 IkReal x486=(px)*(px);
1652 IkReal x487=(pz)*(pz);
1653 IkReal x488=((r01)*(sj0));
1654 IkReal x489=((py)*(r00));
1655 IkReal x490=((pz)*(sj1));
1656 IkReal x491=((py)*(r01));
1657 IkReal x492=((px)*(sj0));
1658 IkReal x493=((IkReal(0.600000000000000))*(r02));
1659 IkReal x494=((IkReal(0.150000000000000))*(cj1));
1660 IkReal x495=((cj0)*(sj1));
1661 IkReal x496=((IkReal(0.150000000000000))*(px));
1662 IkReal x497=((IkReal(2.00000000000000))*(cj1));
1663 IkReal x498=((cj0)*(r01));
1664 IkReal x499=((r02)*(sj1));
1665 IkReal x500=((px)*(r00));
1666 IkReal x501=((IkReal(0.300000000000000))*(r00));
1667 IkReal x502=((IkReal(1.00000000000000))*(pz));
1668 IkReal x503=((r00)*(sj1));
1669 IkReal x504=((cj0)*(r00));
1670 IkReal x505=((cj0)*(cj1));
1671 IkReal x506=((IkReal(1.00000000000000))*(sj1));
1672 IkReal x507=((IkReal(0.0956250000000000))*(r00));
1673 IkReal x508=((IkReal(0.600000000000000))*(pz));
1674 IkReal x509=((IkReal(0.600000000000000))*(sj1));
1675 IkReal x510=((IkReal(2.00000000000000))*(px));
1676 IkReal x511=((IkReal(2.00000000000000))*(sj1));
1677 IkReal x512=((IkReal(0.150000000000000))*(sj1));
1678 IkReal x513=((cj1)*(r02));
1679 IkReal x514=((cj0)*(px));
1680 IkReal x515=((IkReal(0.0843750000000000))*(cj1));
1681 IkReal x516=((py)*(sj0));
1682 IkReal x517=((pz)*(r02));
1683 IkReal x518=((IkReal(1.00000000000000))*(cj1));
1684 IkReal x519=((cj0)*(py));
1685 IkReal x520=((r00)*(sj0));
1686 IkReal x521=((r02)*(x516));
1687 IkReal x522=((pp)*(x518));
1688 IkReal x523=((IkReal(1.00000000000000))*(pp)*(r00));
1689 IkReal x524=((x483)+(x484));
1690 IkReal x525=((IkReal(2.00000000000000))*(r00)*(x486));
1691 IkReal x526=((cj0)*(x510)*(x517));
1692 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
1693 evalcond[1]=((((IkReal(-1.00000000000000))*(x492)))+(x519));
1694 evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x520)))+(x498));
1695 evalcond[3]=((((IkReal(-1.00000000000000))*(x488)*(x518)))+(x499)+(((IkReal(-1.00000000000000))*(x504)*(x518))));
1696 evalcond[4]=((((IkReal(-1.00000000000000))*(x488)*(x506)))+(((IkReal(-1.00000000000000))*(r00)*(x495)))+(((IkReal(-1.00000000000000))*(x513))));
1697 evalcond[5]=((IkReal(0.0188000000000000))+(((cj0)*(x496)))+(((IkReal(0.150000000000000))*(x516)))+(((x509)*(x516)))+(((cj1)*(x508)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(0.600000000000000))*(px)*(x495)))+(((IkReal(-0.0450000000000000))*(sj1)))+(x524));
1698 evalcond[6]=((((IkReal(-1.00000000000000))*(x500)))+(((IkReal(0.0750000000000000))*(x504)))+(((IkReal(0.300000000000000))*(sj1)*(x488)))+(((x495)*(x501)))+(((IkReal(-1.00000000000000))*(r02)*(x502)))+(((IkReal(0.300000000000000))*(x513)))+(((IkReal(0.0750000000000000))*(x488)))+(((IkReal(-1.00000000000000))*(x491))));
1699 evalcond[7]=((((IkReal(-0.300000000000000))*(x499)))+(((IkReal(0.300000000000000))*(cj1)*(x488)))+(((IkReal(-1.00000000000000))*(x502)*(x504)))+(((r02)*(x514)))+(((IkReal(-1.00000000000000))*(x488)*(x502)))+(((IkReal(-0.0750000000000000))*(r02)))+(x521)+(((x501)*(x505))));
1700 evalcond[8]=((IkReal(0.114425000000000))+(((cj1)*(x508)*(x520)))+(((IkReal(0.0956250000000000))*(x498)))+(((pp)*(x498)))+(((IkReal(-1.00000000000000))*(cj0)*(x489)*(x510)))+(((IkReal(-1.00000000000000))*(cj1)*(x498)*(x508)))+(((IkReal(0.0450000000000000))*(r01)*(x495)))+(((IkReal(-2.00000000000000))*(x485)*(x498)))+(((IkReal(-1.00000000000000))*(pp)*(x520)))+(((py)*(x493)*(x505)))+(((IkReal(0.150000000000000))*(x489)))+(((IkReal(-1.00000000000000))*(cj1)*(x492)*(x493)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x509)))+(((IkReal(-1.00000000000000))*(r01)*(x496)))+(((py)*(x488)*(x510)))+(((IkReal(2.00000000000000))*(x486)*(x520)))+(((IkReal(-0.0450000000000000))*(sj0)*(x503)))+(((IkReal(2.00000000000000))*(x492)*(x517)))+(((IkReal(-2.00000000000000))*(x517)*(x519)))+(((IkReal(-1.00000000000000))*(sj0)*(x507)))+(x524)+(((x489)*(x509))));
1701 evalcond[9]=((((IkReal(0.0843750000000000))*(x513)))+(((IkReal(-1.00000000000000))*(x491)*(x512)))+(((pz)*(x491)*(x497)))+(((IkReal(0.0450000000000000))*(x504)))+(((IkReal(0.0450000000000000))*(x488)))+(((x491)*(x495)*(x510)))+(((x495)*(x525)))+(((IkReal(-0.150000000000000))*(r02)*(x490)))+(((IkReal(-1.00000000000000))*(pp)*(x513)))+(((r02)*(x494)*(x514)))+(((r02)*(x487)*(x497)))+(((x495)*(x507)))+(((IkReal(-1.00000000000000))*(x495)*(x523)))+(((IkReal(0.0956250000000000))*(sj1)*(x488)))+(((IkReal(-1.00000000000000))*(pz)*(x488)*(x494)))+(((IkReal(2.00000000000000))*(x490)*(x521)))+(((x489)*(x492)*(x511)))+(((pz)*(x497)*(x500)))+(((x485)*(x488)*(x511)))+(((cj0)*(r02)*(x490)*(x510)))+(((IkReal(-1.00000000000000))*(pz)*(x494)*(x504)))+(((IkReal(-0.600000000000000))*(x491)))+(((IkReal(-1.00000000000000))*(pp)*(x488)*(x506)))+(((IkReal(-0.600000000000000))*(x500)))+(((x494)*(x521)))+(((IkReal(-1.00000000000000))*(x496)*(x503)))+(((IkReal(-1.00000000000000))*(pz)*(x493))));
1702 evalcond[10]=((((IkReal(-2.00000000000000))*(x490)*(x491)))+(((IkReal(-1.00000000000000))*(x488)*(x522)))+(((IkReal(-1.00000000000000))*(r02)*(x495)*(x496)))+(((IkReal(-2.00000000000000))*(x490)*(x500)))+(((IkReal(-1.00000000000000))*(x494)*(x517)))+(((IkReal(-2.00000000000000))*(x487)*(x499)))+(((x497)*(x516)*(x517)))+(((IkReal(0.150000000000000))*(x488)*(x490)))+(((x489)*(x492)*(x497)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x494)*(x500)))+(((IkReal(-1.00000000000000))*(x488)*(x515)))+(((x486)*(x497)*(x504)))+(((IkReal(-1.00000000000000))*(x504)*(x515)))+(((x504)*(x508)))+(((pp)*(x499)))+(((x488)*(x508)))+(((x485)*(x488)*(x497)))+(((IkReal(-1.00000000000000))*(x493)*(x514)))+(((IkReal(-0.150000000000000))*(x499)*(x516)))+(((IkReal(0.150000000000000))*(x490)*(x504)))+(((IkReal(-1.00000000000000))*(x491)*(x494)))+(((IkReal(-1.00000000000000))*(x504)*(x522)))+(((IkReal(-1.00000000000000))*(x493)*(x516)))+(((IkReal(0.0956250000000000))*(x499)))+(((x491)*(x497)*(x514)))+(((x497)*(x514)*(x517))));
1703 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 )
1704 {
1705 {
1706 IkReal dummyeval[1];
1707 IkReal gconst6;
1708 gconst6=IKsign(((IkReal(0.108025000000000))+(((IkReal(0.0512000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
1709 dummyeval[0]=((IkReal(16.8789062500000))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3))+(((IkReal(8.00000000000000))*(cj3))));
1710 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1711 {
1712 {
1713 IkReal dummyeval[1];
1714 IkReal gconst7;
1715 gconst7=IKsign(((IkReal(-0.108025000000000))+(((IkReal(-0.0512000000000000))*(cj3)))+(((IkReal(-0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(-0.0120000000000000))*(sj3)))+(((IkReal(-0.00640000000000000))*((cj3)*(cj3))))));
1716 dummyeval[0]=((IkReal(-16.8789062500000))+(((IkReal(-1.87500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-8.00000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
1717 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1718 {
1719 continue;
1720 
1721 } else
1722 {
1723 {
1724 IkReal j2array[1], cj2array[1], sj2array[1];
1725 bool j2valid[1]={false};
1726 _nj2 = 1;
1727 IkReal x527=((r02)*(sj0));
1728 IkReal x528=((IkReal(0.0800000000000000))*(sj1));
1729 IkReal x529=((cj3)*(px));
1730 IkReal x530=((cj0)*(r01));
1731 IkReal x531=((IkReal(0.0240000000000000))*(cj1));
1732 IkReal x532=((pz)*(sj3));
1733 IkReal x533=((IkReal(0.00600000000000000))*(cj1));
1734 IkReal x534=((IkReal(0.0750000000000000))*(sj1));
1735 IkReal x535=((py)*(sj0));
1736 IkReal x536=((IkReal(0.00562500000000000))*(cj1));
1737 IkReal x537=((r00)*(sj0));
1738 IkReal x538=((cj3)*(pz));
1739 IkReal x539=((IkReal(0.320000000000000))*(sj1));
1740 IkReal x540=((IkReal(0.0800000000000000))*(cj1));
1741 IkReal x541=((IkReal(0.0750000000000000))*(cj1));
1742 IkReal x542=((py)*(r00));
1743 IkReal x543=((IkReal(0.320000000000000))*(cj1));
1744 IkReal x544=((cj0)*(px));
1745 IkReal x545=((IkReal(0.00600000000000000))*(sj1));
1746 IkReal x546=((cj1)*(px)*(r01));
1747 IkReal x547=((cj0)*(py)*(r02));
1748 IkReal x548=((px)*(sj3)*(x528));
1749 if( IKabs(((gconst7)*(((IkReal(0.0960000000000000))+(((IkReal(-1.00000000000000))*(x535)*(x539)))+(((sj3)*(x540)*(x542)))+(((IkReal(-1.00000000000000))*(cj3)*(x528)*(x535)))+(((IkReal(-1.00000000000000))*(sj3)*(x528)*(x547)))+(((px)*(x527)*(x534)))+(((IkReal(-1.00000000000000))*(x534)*(x547)))+(((IkReal(-1.00000000000000))*(x536)*(x537)))+(((x528)*(x530)*(x532)))+(((IkReal(-1.00000000000000))*(pz)*(x534)*(x537)))+(((IkReal(-1.00000000000000))*(pz)*(x543)))+(((sj3)*(x530)*(x533)))+(((IkReal(0.0240000000000000))*(sj1)))+(((x530)*(x536)))+(((IkReal(-1.00000000000000))*(cj0)*(x528)*(x529)))+(((IkReal(-1.00000000000000))*(x539)*(x544)))+(((x541)*(x542)))+(((pz)*(x530)*(x534)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x541)))+(((x527)*(x548)))+(((cj3)*(x545)))+(((IkReal(-1.00000000000000))*(sj3)*(x533)*(x537)))+(((IkReal(0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x528)*(x532)*(x537)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(sj3)*(x540)))+(((IkReal(-1.00000000000000))*(x538)*(x540))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((IkReal(0.0225000000000000))+(((cj3)*(x533)*(x537)))+(((IkReal(-1.00000000000000))*(pz)*(x530)*(x539)))+(((IkReal(-1.00000000000000))*(sj3)*(x528)*(x535)))+(((IkReal(0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(pz)*(x541)))+(((IkReal(-1.00000000000000))*(x527)*(x528)*(x529)))+(((x539)*(x547)))+(((IkReal(-1.00000000000000))*(cj3)*(x540)*(x542)))+(((IkReal(-1.00000000000000))*(x542)*(x543)))+(((IkReal(-1.00000000000000))*(x530)*(x531)))+(((IkReal(-1.00000000000000))*(x534)*(x535)))+(((cj3)*(x528)*(x547)))+(((x528)*(x537)*(x538)))+(((IkReal(-1.00000000000000))*(x528)*(x530)*(x538)))+(((pz)*(x537)*(x539)))+(((r01)*(x529)*(x540)))+(((IkReal(-1.00000000000000))*(cj3)*(x530)*(x533)))+(((IkReal(-1.00000000000000))*(x534)*(x544)))+(((IkReal(-1.00000000000000))*(px)*(x527)*(x539)))+(((IkReal(-1.00000000000000))*(x532)*(x540)))+(((px)*(r01)*(x543)))+(((x531)*(x537)))+(((IkReal(-1.00000000000000))*(sj3)*(x528)*(x544)))+(((sj3)*(x545)))+(((IkReal(0.0240000000000000))*(sj3))))))) < IKFAST_ATAN2_MAGTHRESH )
1750  continue;
1751 j2array[0]=IKatan2(((gconst7)*(((IkReal(0.0960000000000000))+(((IkReal(-1.00000000000000))*(x535)*(x539)))+(((sj3)*(x540)*(x542)))+(((IkReal(-1.00000000000000))*(cj3)*(x528)*(x535)))+(((IkReal(-1.00000000000000))*(sj3)*(x528)*(x547)))+(((px)*(x527)*(x534)))+(((IkReal(-1.00000000000000))*(x534)*(x547)))+(((IkReal(-1.00000000000000))*(x536)*(x537)))+(((x528)*(x530)*(x532)))+(((IkReal(-1.00000000000000))*(pz)*(x534)*(x537)))+(((IkReal(-1.00000000000000))*(pz)*(x543)))+(((sj3)*(x530)*(x533)))+(((IkReal(0.0240000000000000))*(sj1)))+(((x530)*(x536)))+(((IkReal(-1.00000000000000))*(cj0)*(x528)*(x529)))+(((IkReal(-1.00000000000000))*(x539)*(x544)))+(((x541)*(x542)))+(((pz)*(x530)*(x534)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x541)))+(((x527)*(x548)))+(((cj3)*(x545)))+(((IkReal(-1.00000000000000))*(sj3)*(x533)*(x537)))+(((IkReal(0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x528)*(x532)*(x537)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(sj3)*(x540)))+(((IkReal(-1.00000000000000))*(x538)*(x540)))))), ((gconst7)*(((IkReal(0.0225000000000000))+(((cj3)*(x533)*(x537)))+(((IkReal(-1.00000000000000))*(pz)*(x530)*(x539)))+(((IkReal(-1.00000000000000))*(sj3)*(x528)*(x535)))+(((IkReal(0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(pz)*(x541)))+(((IkReal(-1.00000000000000))*(x527)*(x528)*(x529)))+(((x539)*(x547)))+(((IkReal(-1.00000000000000))*(cj3)*(x540)*(x542)))+(((IkReal(-1.00000000000000))*(x542)*(x543)))+(((IkReal(-1.00000000000000))*(x530)*(x531)))+(((IkReal(-1.00000000000000))*(x534)*(x535)))+(((cj3)*(x528)*(x547)))+(((x528)*(x537)*(x538)))+(((IkReal(-1.00000000000000))*(x528)*(x530)*(x538)))+(((pz)*(x537)*(x539)))+(((r01)*(x529)*(x540)))+(((IkReal(-1.00000000000000))*(cj3)*(x530)*(x533)))+(((IkReal(-1.00000000000000))*(x534)*(x544)))+(((IkReal(-1.00000000000000))*(px)*(x527)*(x539)))+(((IkReal(-1.00000000000000))*(x532)*(x540)))+(((px)*(r01)*(x543)))+(((x531)*(x537)))+(((IkReal(-1.00000000000000))*(sj3)*(x528)*(x544)))+(((sj3)*(x545)))+(((IkReal(0.0240000000000000))*(sj3)))))));
1752 sj2array[0]=IKsin(j2array[0]);
1753 cj2array[0]=IKcos(j2array[0]);
1754 if( j2array[0] > IKPI )
1755 {
1756  j2array[0]-=IK2PI;
1757 }
1758 else if( j2array[0] < -IKPI )
1759 { j2array[0]+=IK2PI;
1760 }
1761 j2valid[0] = true;
1762 for(int ij2 = 0; ij2 < 1; ++ij2)
1763 {
1764 if( !j2valid[ij2] )
1765 {
1766  continue;
1767 }
1768 _ij2[0] = ij2; _ij2[1] = -1;
1769 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
1770 {
1771 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
1772 {
1773  j2valid[iij2]=false; _ij2[1] = iij2; break;
1774 }
1775 }
1776 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
1777 {
1778 IkReal evalcond[4];
1779 IkReal x549=IKcos(j2);
1780 IkReal x550=IKsin(j2);
1781 IkReal x551=((IkReal(0.0800000000000000))*(sj3));
1782 IkReal x552=((cj0)*(r01));
1783 IkReal x553=((IkReal(1.00000000000000))*(px));
1784 IkReal x554=((py)*(sj1));
1785 IkReal x555=((cj0)*(r02));
1786 IkReal x556=((IkReal(0.0750000000000000))*(cj1));
1787 IkReal x557=((r02)*(sj0));
1788 IkReal x558=((IkReal(0.0750000000000000))*(sj1));
1789 IkReal x559=((cj1)*(pz));
1790 IkReal x560=((r00)*(sj0));
1791 IkReal x561=((IkReal(0.0800000000000000))*(cj3));
1792 IkReal x562=((pz)*(sj1));
1793 IkReal x563=((IkReal(1.00000000000000))*(sj0));
1794 IkReal x564=((cj1)*(py));
1795 IkReal x565=((IkReal(0.320000000000000))*(x549));
1796 IkReal x566=((IkReal(0.0750000000000000))*(x550));
1797 IkReal x567=((IkReal(0.320000000000000))*(x550));
1798 IkReal x568=((IkReal(0.0750000000000000))*(x549));
1799 IkReal x569=((sj1)*(x560));
1800 IkReal x570=((x549)*(x561));
1801 IkReal x571=((x550)*(x551));
1802 IkReal x572=((x549)*(x551));
1803 IkReal x573=((x550)*(x561));
1804 IkReal x574=((x571)+(x566));
1805 IkReal x575=((x570)+(x565));
1806 IkReal x576=((x573)+(x572)+(x568)+(x567));
1807 evalcond[0]=((x575)+(((IkReal(-1.00000000000000))*(x574)))+(x562)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x553)))+(((IkReal(-1.00000000000000))*(x563)*(x564)))+(x556));
1808 evalcond[1]=((IkReal(0.300000000000000))+(x576)+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x553)))+(((IkReal(-1.00000000000000))*(x559)))+(((IkReal(-1.00000000000000))*(x554)*(x563)))+(x558));
1809 evalcond[2]=((x575)+(((IkReal(-1.00000000000000))*(x574)))+(((IkReal(-1.00000000000000))*(sj1)*(x553)*(x557)))+(((x554)*(x555)))+(((x556)*(x560)))+(((IkReal(-1.00000000000000))*(x552)*(x562)))+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(x552)*(x556)))+(((x560)*(x562)))+(((IkReal(-1.00000000000000))*(r00)*(x564))));
1810 evalcond[3]=((((IkReal(-1.00000000000000))*(x552)*(x559)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x553)))+(((x552)*(x558)))+(((x559)*(x560)))+(((IkReal(-1.00000000000000))*(x576)))+(((IkReal(-1.00000000000000))*(x558)*(x560)))+(((IkReal(0.300000000000000))*(x552)))+(((r00)*(x554)))+(((x555)*(x564)))+(((IkReal(-1.00000000000000))*(cj1)*(x553)*(x557)))+(((IkReal(-0.300000000000000))*(x560))));
1811 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1812 {
1813 continue;
1814 }
1815 }
1816 
1817 {
1818 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1819 vinfos[0].jointtype = 1;
1820 vinfos[0].foffset = j0;
1821 vinfos[0].indices[0] = _ij0[0];
1822 vinfos[0].indices[1] = _ij0[1];
1823 vinfos[0].maxsolutions = _nj0;
1824 vinfos[1].jointtype = 1;
1825 vinfos[1].foffset = j1;
1826 vinfos[1].indices[0] = _ij1[0];
1827 vinfos[1].indices[1] = _ij1[1];
1828 vinfos[1].maxsolutions = _nj1;
1829 vinfos[2].jointtype = 1;
1830 vinfos[2].foffset = j2;
1831 vinfos[2].indices[0] = _ij2[0];
1832 vinfos[2].indices[1] = _ij2[1];
1833 vinfos[2].maxsolutions = _nj2;
1834 vinfos[3].jointtype = 1;
1835 vinfos[3].foffset = j3;
1836 vinfos[3].indices[0] = _ij3[0];
1837 vinfos[3].indices[1] = _ij3[1];
1838 vinfos[3].maxsolutions = _nj3;
1839 vinfos[4].jointtype = 1;
1840 vinfos[4].foffset = j4;
1841 vinfos[4].indices[0] = _ij4[0];
1842 vinfos[4].indices[1] = _ij4[1];
1843 vinfos[4].maxsolutions = _nj4;
1844 std::vector<int> vfree(0);
1845 solutions.AddSolution(vinfos,vfree);
1846 }
1847 }
1848 }
1849 
1850 }
1851 
1852 }
1853 
1854 } else
1855 {
1856 {
1857 IkReal j2array[1], cj2array[1], sj2array[1];
1858 bool j2valid[1]={false};
1859 _nj2 = 1;
1860 IkReal x577=((IkReal(0.0800000000000000))*(cj1));
1861 IkReal x578=((cj0)*(px));
1862 IkReal x579=((py)*(sj0));
1863 IkReal x580=((IkReal(0.320000000000000))*(sj1));
1864 IkReal x581=((IkReal(0.00600000000000000))*(cj3));
1865 IkReal x582=((pz)*(sj3));
1866 IkReal x583=((IkReal(0.0750000000000000))*(cj1));
1867 IkReal x584=((IkReal(0.0800000000000000))*(sj1));
1868 IkReal x585=((IkReal(0.0750000000000000))*(sj1));
1869 IkReal x586=((IkReal(0.00600000000000000))*(sj3));
1870 IkReal x587=((cj3)*(pz));
1871 IkReal x588=((IkReal(0.320000000000000))*(cj1));
1872 if( IKabs(((gconst6)*(((IkReal(-0.0960000000000000))+(((cj1)*(x586)))+(((pz)*(x588)))+(((IkReal(-1.00000000000000))*(sj3)*(x577)*(x579)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x577)*(x578)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((x577)*(x587)))+(((x579)*(x580)))+(((pz)*(x585)))+(((cj3)*(x579)*(x584)))+(((x578)*(x580)))+(((IkReal(0.00562500000000000))*(cj1)))+(((cj3)*(x578)*(x584)))+(((x582)*(x584)))+(((IkReal(-1.00000000000000))*(x579)*(x583)))+(((IkReal(-1.00000000000000))*(sj1)*(x581)))+(((IkReal(-1.00000000000000))*(x578)*(x583))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(-0.0225000000000000))+(((sj3)*(x578)*(x584)))+(((IkReal(-1.00000000000000))*(sj1)*(x586)))+(((x579)*(x588)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((x578)*(x585)))+(((cj3)*(x577)*(x579)))+(((pz)*(x583)))+(((x578)*(x588)))+(((IkReal(-1.00000000000000))*(cj1)*(x581)))+(((IkReal(-1.00000000000000))*(x584)*(x587)))+(((sj3)*(x579)*(x584)))+(((x577)*(x582)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(pz)*(x580)))+(((x579)*(x585)))+(((cj3)*(x577)*(x578))))))) < IKFAST_ATAN2_MAGTHRESH )
1873  continue;
1874 j2array[0]=IKatan2(((gconst6)*(((IkReal(-0.0960000000000000))+(((cj1)*(x586)))+(((pz)*(x588)))+(((IkReal(-1.00000000000000))*(sj3)*(x577)*(x579)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(sj3)*(x577)*(x578)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((x577)*(x587)))+(((x579)*(x580)))+(((pz)*(x585)))+(((cj3)*(x579)*(x584)))+(((x578)*(x580)))+(((IkReal(0.00562500000000000))*(cj1)))+(((cj3)*(x578)*(x584)))+(((x582)*(x584)))+(((IkReal(-1.00000000000000))*(x579)*(x583)))+(((IkReal(-1.00000000000000))*(sj1)*(x581)))+(((IkReal(-1.00000000000000))*(x578)*(x583)))))), ((gconst6)*(((IkReal(-0.0225000000000000))+(((sj3)*(x578)*(x584)))+(((IkReal(-1.00000000000000))*(sj1)*(x586)))+(((x579)*(x588)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((x578)*(x585)))+(((cj3)*(x577)*(x579)))+(((pz)*(x583)))+(((x578)*(x588)))+(((IkReal(-1.00000000000000))*(cj1)*(x581)))+(((IkReal(-1.00000000000000))*(x584)*(x587)))+(((sj3)*(x579)*(x584)))+(((x577)*(x582)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(pz)*(x580)))+(((x579)*(x585)))+(((cj3)*(x577)*(x578)))))));
1875 sj2array[0]=IKsin(j2array[0]);
1876 cj2array[0]=IKcos(j2array[0]);
1877 if( j2array[0] > IKPI )
1878 {
1879  j2array[0]-=IK2PI;
1880 }
1881 else if( j2array[0] < -IKPI )
1882 { j2array[0]+=IK2PI;
1883 }
1884 j2valid[0] = true;
1885 for(int ij2 = 0; ij2 < 1; ++ij2)
1886 {
1887 if( !j2valid[ij2] )
1888 {
1889  continue;
1890 }
1891 _ij2[0] = ij2; _ij2[1] = -1;
1892 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
1893 {
1894 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
1895 {
1896  j2valid[iij2]=false; _ij2[1] = iij2; break;
1897 }
1898 }
1899 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
1900 {
1901 IkReal evalcond[4];
1902 IkReal x589=IKcos(j2);
1903 IkReal x590=IKsin(j2);
1904 IkReal x591=((IkReal(0.0800000000000000))*(sj3));
1905 IkReal x592=((cj0)*(r01));
1906 IkReal x593=((IkReal(1.00000000000000))*(px));
1907 IkReal x594=((py)*(sj1));
1908 IkReal x595=((cj0)*(r02));
1909 IkReal x596=((IkReal(0.0750000000000000))*(cj1));
1910 IkReal x597=((r02)*(sj0));
1911 IkReal x598=((IkReal(0.0750000000000000))*(sj1));
1912 IkReal x599=((cj1)*(pz));
1913 IkReal x600=((r00)*(sj0));
1914 IkReal x601=((IkReal(0.0800000000000000))*(cj3));
1915 IkReal x602=((pz)*(sj1));
1916 IkReal x603=((IkReal(1.00000000000000))*(sj0));
1917 IkReal x604=((cj1)*(py));
1918 IkReal x605=((IkReal(0.320000000000000))*(x589));
1919 IkReal x606=((IkReal(0.0750000000000000))*(x590));
1920 IkReal x607=((IkReal(0.320000000000000))*(x590));
1921 IkReal x608=((IkReal(0.0750000000000000))*(x589));
1922 IkReal x609=((sj1)*(x600));
1923 IkReal x610=((x589)*(x601));
1924 IkReal x611=((x590)*(x591));
1925 IkReal x612=((x589)*(x591));
1926 IkReal x613=((x590)*(x601));
1927 IkReal x614=((x611)+(x606));
1928 IkReal x615=((x610)+(x605));
1929 IkReal x616=((x612)+(x613)+(x607)+(x608));
1930 evalcond[0]=((((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x593)))+(((IkReal(-1.00000000000000))*(x603)*(x604)))+(((IkReal(-1.00000000000000))*(x614)))+(x615)+(x596)+(x602));
1931 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x593)))+(((IkReal(-1.00000000000000))*(x594)*(x603)))+(x616)+(x598)+(((IkReal(-1.00000000000000))*(x599))));
1932 evalcond[2]=((((x594)*(x595)))+(((IkReal(-1.00000000000000))*(x592)*(x596)))+(((x600)*(x602)))+(((IkReal(-1.00000000000000))*(sj1)*(x593)*(x597)))+(((x596)*(x600)))+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(x614)))+(x615)+(((IkReal(-1.00000000000000))*(x592)*(x602)))+(((IkReal(-1.00000000000000))*(r00)*(x604))));
1933 evalcond[3]=((((IkReal(-1.00000000000000))*(x592)*(x599)))+(((x595)*(x604)))+(((x599)*(x600)))+(((x592)*(x598)))+(((IkReal(0.300000000000000))*(x592)))+(((IkReal(-0.300000000000000))*(x600)))+(((r00)*(x594)))+(((IkReal(-1.00000000000000))*(x616)))+(((IkReal(-1.00000000000000))*(cj1)*(x593)*(x597)))+(((IkReal(-1.00000000000000))*(x598)*(x600)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x593))));
1934 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1935 {
1936 continue;
1937 }
1938 }
1939 
1940 {
1941 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
1942 vinfos[0].jointtype = 1;
1943 vinfos[0].foffset = j0;
1944 vinfos[0].indices[0] = _ij0[0];
1945 vinfos[0].indices[1] = _ij0[1];
1946 vinfos[0].maxsolutions = _nj0;
1947 vinfos[1].jointtype = 1;
1948 vinfos[1].foffset = j1;
1949 vinfos[1].indices[0] = _ij1[0];
1950 vinfos[1].indices[1] = _ij1[1];
1951 vinfos[1].maxsolutions = _nj1;
1952 vinfos[2].jointtype = 1;
1953 vinfos[2].foffset = j2;
1954 vinfos[2].indices[0] = _ij2[0];
1955 vinfos[2].indices[1] = _ij2[1];
1956 vinfos[2].maxsolutions = _nj2;
1957 vinfos[3].jointtype = 1;
1958 vinfos[3].foffset = j3;
1959 vinfos[3].indices[0] = _ij3[0];
1960 vinfos[3].indices[1] = _ij3[1];
1961 vinfos[3].maxsolutions = _nj3;
1962 vinfos[4].jointtype = 1;
1963 vinfos[4].foffset = j4;
1964 vinfos[4].indices[0] = _ij4[0];
1965 vinfos[4].indices[1] = _ij4[1];
1966 vinfos[4].maxsolutions = _nj4;
1967 std::vector<int> vfree(0);
1968 solutions.AddSolution(vinfos,vfree);
1969 }
1970 }
1971 }
1972 
1973 }
1974 
1975 }
1976 
1977 } else
1978 {
1979 if( 1 )
1980 {
1981 continue;
1982 
1983 } else
1984 {
1985 }
1986 }
1987 }
1988 }
1989 
1990 } else
1991 {
1992 {
1993 IkReal j2array[1], cj2array[1], sj2array[1];
1994 bool j2valid[1]={false};
1995 _nj2 = 1;
1996 IkReal x617=((IkReal(0.0800000000000000))*(cj3));
1997 IkReal x618=((IkReal(0.0750000000000000))*(cj1));
1998 IkReal x619=((r01)*(sj0));
1999 IkReal x620=((r02)*(sj1));
2000 IkReal x621=((IkReal(0.0800000000000000))*(sj3));
2001 IkReal x622=((cj3)*(cj4));
2002 IkReal x623=((cj4)*(sj3));
2003 IkReal x624=((pz)*(sj1));
2004 IkReal x625=((cj0)*(cj1)*(r00));
2005 IkReal x626=((cj0)*(cj1)*(px));
2006 IkReal x627=((cj1)*(py)*(sj0));
2007 if( IKabs(((gconst3)*(((((IkReal(-0.320000000000000))*(cj1)*(x619)))+(((IkReal(-0.320000000000000))*(x625)))+(((IkReal(-1.00000000000000))*(cj1)*(x617)*(x619)))+(((x617)*(x620)))+(((x623)*(x624)))+(((x618)*(x623)))+(((IkReal(-1.00000000000000))*(x623)*(x626)))+(((IkReal(-1.00000000000000))*(x617)*(x625)))+(((IkReal(-1.00000000000000))*(x623)*(x627)))+(((IkReal(0.320000000000000))*(x620))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(x618)*(x619)))+(((IkReal(-1.00000000000000))*(cj1)*(x619)*(x621)))+(((x622)*(x626)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x618)))+(((IkReal(-1.00000000000000))*(x621)*(x625)))+(((IkReal(-1.00000000000000))*(x622)*(x624)))+(((IkReal(-1.00000000000000))*(x618)*(x622)))+(((x622)*(x627)))+(((IkReal(0.0750000000000000))*(x620)))+(((x620)*(x621))))))) < IKFAST_ATAN2_MAGTHRESH )
2008  continue;
2009 j2array[0]=IKatan2(((gconst3)*(((((IkReal(-0.320000000000000))*(cj1)*(x619)))+(((IkReal(-0.320000000000000))*(x625)))+(((IkReal(-1.00000000000000))*(cj1)*(x617)*(x619)))+(((x617)*(x620)))+(((x623)*(x624)))+(((x618)*(x623)))+(((IkReal(-1.00000000000000))*(x623)*(x626)))+(((IkReal(-1.00000000000000))*(x617)*(x625)))+(((IkReal(-1.00000000000000))*(x623)*(x627)))+(((IkReal(0.320000000000000))*(x620)))))), ((gconst3)*(((((IkReal(-1.00000000000000))*(x618)*(x619)))+(((IkReal(-1.00000000000000))*(cj1)*(x619)*(x621)))+(((x622)*(x626)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x618)))+(((IkReal(-1.00000000000000))*(x621)*(x625)))+(((IkReal(-1.00000000000000))*(x622)*(x624)))+(((IkReal(-1.00000000000000))*(x618)*(x622)))+(((x622)*(x627)))+(((IkReal(0.0750000000000000))*(x620)))+(((x620)*(x621)))))));
2010 sj2array[0]=IKsin(j2array[0]);
2011 cj2array[0]=IKcos(j2array[0]);
2012 if( j2array[0] > IKPI )
2013 {
2014  j2array[0]-=IK2PI;
2015 }
2016 else if( j2array[0] < -IKPI )
2017 { j2array[0]+=IK2PI;
2018 }
2019 j2valid[0] = true;
2020 for(int ij2 = 0; ij2 < 1; ++ij2)
2021 {
2022 if( !j2valid[ij2] )
2023 {
2024  continue;
2025 }
2026 _ij2[0] = ij2; _ij2[1] = -1;
2027 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2028 {
2029 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2030 {
2031  j2valid[iij2]=false; _ij2[1] = iij2; break;
2032 }
2033 }
2034 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2035 {
2036 IkReal evalcond[8];
2037 IkReal x628=IKcos(j2);
2038 IkReal x629=IKsin(j2);
2039 IkReal x630=(py)*(py);
2040 IkReal x631=(px)*(px);
2041 IkReal x632=(pz)*(pz);
2042 IkReal x633=((sj0)*(sj1));
2043 IkReal x634=((IkReal(1.00000000000000))*(r01));
2044 IkReal x635=((r00)*(sj1));
2045 IkReal x636=((IkReal(0.0480000000000000))*(sj3));
2046 IkReal x637=((IkReal(0.0750000000000000))*(r00));
2047 IkReal x638=((IkReal(0.0800000000000000))*(sj3));
2048 IkReal x639=((cj1)*(r01));
2049 IkReal x640=((IkReal(0.150000000000000))*(py));
2050 IkReal x641=((pz)*(r02));
2051 IkReal x642=((px)*(r02));
2052 IkReal x643=((IkReal(0.0750000000000000))*(sj4));
2053 IkReal x644=((cj1)*(sj0));
2054 IkReal x645=((pz)*(r00));
2055 IkReal x646=((IkReal(0.0750000000000000))*(cj0));
2056 IkReal x647=((IkReal(2.00000000000000))*(cj0));
2057 IkReal x648=((cj1)*(r02));
2058 IkReal x649=((IkReal(0.150000000000000))*(sj1));
2059 IkReal x650=((px)*(r00));
2060 IkReal x651=((IkReal(2.00000000000000))*(py));
2061 IkReal x652=((IkReal(0.0903750000000000))*(sj3));
2062 IkReal x653=((r02)*(sj1));
2063 IkReal x654=((IkReal(0.150000000000000))*(cj1));
2064 IkReal x655=((cj0)*(py));
2065 IkReal x656=((IkReal(0.0480000000000000))*(cj3));
2066 IkReal x657=((r01)*(sj1));
2067 IkReal x658=((IkReal(0.0800000000000000))*(cj3));
2068 IkReal x659=((IkReal(0.150000000000000))*(pz));
2069 IkReal x660=((IkReal(1.00000000000000))*(cj1));
2070 IkReal x661=((cj0)*(r00));
2071 IkReal x662=((IkReal(0.600000000000000))*(py));
2072 IkReal x663=((IkReal(1.00000000000000))*(py));
2073 IkReal x664=((px)*(sj1));
2074 IkReal x665=((IkReal(1.00000000000000))*(cj0));
2075 IkReal x666=((cj0)*(pz));
2076 IkReal x667=((IkReal(0.103175000000000))*(cj3));
2077 IkReal x668=((IkReal(0.600000000000000))*(cj0));
2078 IkReal x669=((IkReal(1.00000000000000))*(sj3));
2079 IkReal x670=((px)*(py));
2080 IkReal x671=((r01)*(sj0));
2081 IkReal x672=((IkReal(2.00000000000000))*(px));
2082 IkReal x673=((cj4)*(x629));
2083 IkReal x674=((cj4)*(x628));
2084 IkReal x675=((sj4)*(x629));
2085 IkReal x676=((IkReal(2.00000000000000))*(x632));
2086 IkReal x677=((sj4)*(x628));
2087 IkReal x678=((IkReal(2.00000000000000))*(x630));
2088 IkReal x679=((px)*(x641)*(x647));
2089 evalcond[0]=((x653)+(((IkReal(-1.00000000000000))*(x669)*(x674)))+(((IkReal(-1.00000000000000))*(x660)*(x661)))+(((IkReal(-1.00000000000000))*(x634)*(x644)))+(((IkReal(-1.00000000000000))*(cj3)*(x673))));
2090 evalcond[1]=((((IkReal(-1.00000000000000))*(x669)*(x673)))+(((IkReal(-1.00000000000000))*(x633)*(x634)))+(((cj3)*(x674)))+(((IkReal(-1.00000000000000))*(x635)*(x665)))+(((IkReal(-1.00000000000000))*(x648))));
2091 evalcond[2]=((((IkReal(-1.00000000000000))*(x629)*(x638)))+(((IkReal(-1.00000000000000))*(x644)*(x663)))+(((pz)*(sj1)))+(((x628)*(x658)))+(((IkReal(-0.0750000000000000))*(x629)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x660)))+(((IkReal(0.0750000000000000))*(cj1)))+(((IkReal(0.320000000000000))*(x628))));
2092 evalcond[3]=((IkReal(0.300000000000000))+(((IkReal(0.320000000000000))*(x629)))+(((IkReal(-1.00000000000000))*(x633)*(x663)))+(((x628)*(x638)))+(((IkReal(0.0750000000000000))*(x628)))+(((IkReal(-1.00000000000000))*(pz)*(x660)))+(((IkReal(-1.00000000000000))*(x664)*(x665)))+(((x629)*(x658)))+(((IkReal(0.0750000000000000))*(sj1))));
2093 evalcond[4]=((((x638)*(x675)))+(((IkReal(-0.320000000000000))*(x677)))+(((x629)*(x643)))+(((IkReal(-1.00000000000000))*(x658)*(x677)))+(((x633)*(x645)))+(((IkReal(-1.00000000000000))*(sj1)*(x634)*(x666)))+(((px)*(x639)))+(((x637)*(x644)))+(((IkReal(-1.00000000000000))*(x633)*(x642)))+(((x653)*(x655)))+(((IkReal(-1.00000000000000))*(x639)*(x646)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x660))));
2094 evalcond[5]=((((x628)*(x643)))+(((x646)*(x657)))+(((py)*(x635)))+(((x638)*(x677)))+(((IkReal(-0.300000000000000))*(r00)*(sj0)))+(((x658)*(x675)))+(((x644)*(x645)))+(((IkReal(-1.00000000000000))*(x634)*(x664)))+(((IkReal(-1.00000000000000))*(x633)*(x637)))+(((x648)*(x655)))+(((IkReal(0.320000000000000))*(x675)))+(((IkReal(-1.00000000000000))*(cj1)*(x634)*(x666)))+(((IkReal(0.300000000000000))*(cj0)*(r01)))+(((IkReal(-1.00000000000000))*(x642)*(x644))));
2095 evalcond[6]=((((cj1)*(x645)*(x672)))+(((x647)*(x657)*(x670)))+(((x633)*(x650)*(x651)))+(((IkReal(-1.00000000000000))*(sj0)*(x639)*(x659)))+(((IkReal(0.0512000000000000))*(x674)))+(((cj0)*(x642)*(x654)))+(((IkReal(0.0956250000000000))*(cj0)*(x635)))+(((x633)*(x641)*(x651)))+(((IkReal(-0.600000000000000))*(x641)))+(((IkReal(-1.00000000000000))*(r01)*(x662)))+(((IkReal(-1.00000000000000))*(pp)*(x635)*(x665)))+(((x652)*(x673)))+(((IkReal(0.0450000000000000))*(x671)))+(((IkReal(-1.00000000000000))*(x640)*(x657)))+(((IkReal(-1.00000000000000))*(x641)*(x649)))+(((x636)*(x674)))+(((IkReal(-1.00000000000000))*(pp)*(x633)*(x634)))+(((IkReal(0.0450000000000000))*(x661)))+(((r01)*(x633)*(x678)))+(((IkReal(-1.00000000000000))*(cj0)*(x645)*(x654)))+(((x631)*(x635)*(x647)))+(((IkReal(-0.600000000000000))*(x650)))+(((x641)*(x647)*(x664)))+(((IkReal(-1.00000000000000))*(x656)*(x673)))+(((r02)*(x640)*(x644)))+(((IkReal(0.0956250000000000))*(r01)*(x633)))+(((IkReal(-0.0120000000000000))*(x673)))+(((IkReal(-1.00000000000000))*(pp)*(x648)))+(((x648)*(x676)))+(((IkReal(0.0843750000000000))*(x648)))+(((pz)*(x639)*(x651)))+(((x667)*(x674)))+(((IkReal(-0.150000000000000))*(px)*(x635))));
2096 evalcond[7]=((((IkReal(-1.00000000000000))*(x636)*(x673)))+(((IkReal(-1.00000000000000))*(cj0)*(x642)*(x649)))+(((IkReal(-1.00000000000000))*(x641)*(x654)))+(((IkReal(-1.00000000000000))*(x642)*(x668)))+(((IkReal(-1.00000000000000))*(r02)*(x633)*(x640)))+(((cj0)*(x635)*(x659)))+(((IkReal(0.0450000000000000))*(r02)))+(((cj1)*(x679)))+(((IkReal(-1.00000000000000))*(pp)*(x660)*(x661)))+(((IkReal(-1.00000000000000))*(x667)*(x673)))+(((IkReal(-1.00000000000000))*(pp)*(x634)*(x644)))+(((sj0)*(x639)*(x678)))+(((IkReal(-0.0843750000000000))*(sj0)*(x639)))+(((x639)*(x647)*(x670)))+(((IkReal(-1.00000000000000))*(x653)*(x676)))+(((cj1)*(r00)*(x631)*(x647)))+(((IkReal(0.0956250000000000))*(x653)))+(((x652)*(x674)))+(((x644)*(x650)*(x651)))+(((r01)*(x633)*(x659)))+(((IkReal(-1.00000000000000))*(pz)*(x635)*(x672)))+(((IkReal(-0.0512000000000000))*(x673)))+(((IkReal(-1.00000000000000))*(x639)*(x640)))+(((IkReal(-1.00000000000000))*(x650)*(x654)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x662)))+(((x641)*(x644)*(x651)))+(((pp)*(x653)))+(((IkReal(-1.00000000000000))*(x656)*(x674)))+(((IkReal(-0.0843750000000000))*(cj1)*(x661)))+(((x645)*(x668)))+(((IkReal(-0.0120000000000000))*(x674)))+(((IkReal(0.600000000000000))*(pz)*(x671)))+(((IkReal(-1.00000000000000))*(pz)*(x651)*(x657))));
2097 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 )
2098 {
2099 continue;
2100 }
2101 }
2102 
2103 {
2104 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
2105 vinfos[0].jointtype = 1;
2106 vinfos[0].foffset = j0;
2107 vinfos[0].indices[0] = _ij0[0];
2108 vinfos[0].indices[1] = _ij0[1];
2109 vinfos[0].maxsolutions = _nj0;
2110 vinfos[1].jointtype = 1;
2111 vinfos[1].foffset = j1;
2112 vinfos[1].indices[0] = _ij1[0];
2113 vinfos[1].indices[1] = _ij1[1];
2114 vinfos[1].maxsolutions = _nj1;
2115 vinfos[2].jointtype = 1;
2116 vinfos[2].foffset = j2;
2117 vinfos[2].indices[0] = _ij2[0];
2118 vinfos[2].indices[1] = _ij2[1];
2119 vinfos[2].maxsolutions = _nj2;
2120 vinfos[3].jointtype = 1;
2121 vinfos[3].foffset = j3;
2122 vinfos[3].indices[0] = _ij3[0];
2123 vinfos[3].indices[1] = _ij3[1];
2124 vinfos[3].maxsolutions = _nj3;
2125 vinfos[4].jointtype = 1;
2126 vinfos[4].foffset = j4;
2127 vinfos[4].indices[0] = _ij4[0];
2128 vinfos[4].indices[1] = _ij4[1];
2129 vinfos[4].maxsolutions = _nj4;
2130 std::vector<int> vfree(0);
2131 solutions.AddSolution(vinfos,vfree);
2132 }
2133 }
2134 }
2135 
2136 }
2137 
2138 }
2139 
2140 } else
2141 {
2142 {
2143 IkReal j2array[1], cj2array[1], sj2array[1];
2144 bool j2valid[1]={false};
2145 _nj2 = 1;
2146 IkReal x680=((IkReal(1.00000000000000))*(cj1));
2147 IkReal x681=((cj0)*(r00));
2148 IkReal x682=((cj3)*(r02));
2149 IkReal x683=((sj1)*(sj3));
2150 IkReal x684=((r01)*(sj0));
2151 IkReal x685=((cj3)*(x684));
2152 if( IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(x683)*(x684)))+(((IkReal(-1.00000000000000))*(cj3)*(x680)*(x681)))+(((IkReal(-1.00000000000000))*(x680)*(x685)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x680)))+(((IkReal(-1.00000000000000))*(x681)*(x683)))+(((sj1)*(x682))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(sj3)*(x680)*(x684)))+(((cj3)*(sj1)*(x681)))+(((cj1)*(x682)))+(((IkReal(-1.00000000000000))*(sj3)*(x680)*(x681)))+(((r02)*(x683)))+(((sj1)*(x685))))))) < IKFAST_ATAN2_MAGTHRESH )
2153  continue;
2154 j2array[0]=IKatan2(((gconst2)*(((((IkReal(-1.00000000000000))*(x683)*(x684)))+(((IkReal(-1.00000000000000))*(cj3)*(x680)*(x681)))+(((IkReal(-1.00000000000000))*(x680)*(x685)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x680)))+(((IkReal(-1.00000000000000))*(x681)*(x683)))+(((sj1)*(x682)))))), ((gconst2)*(((((IkReal(-1.00000000000000))*(sj3)*(x680)*(x684)))+(((cj3)*(sj1)*(x681)))+(((cj1)*(x682)))+(((IkReal(-1.00000000000000))*(sj3)*(x680)*(x681)))+(((r02)*(x683)))+(((sj1)*(x685)))))));
2155 sj2array[0]=IKsin(j2array[0]);
2156 cj2array[0]=IKcos(j2array[0]);
2157 if( j2array[0] > IKPI )
2158 {
2159  j2array[0]-=IK2PI;
2160 }
2161 else if( j2array[0] < -IKPI )
2162 { j2array[0]+=IK2PI;
2163 }
2164 j2valid[0] = true;
2165 for(int ij2 = 0; ij2 < 1; ++ij2)
2166 {
2167 if( !j2valid[ij2] )
2168 {
2169  continue;
2170 }
2171 _ij2[0] = ij2; _ij2[1] = -1;
2172 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2173 {
2174 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2175 {
2176  j2valid[iij2]=false; _ij2[1] = iij2; break;
2177 }
2178 }
2179 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2180 {
2181 IkReal evalcond[8];
2182 IkReal x686=IKcos(j2);
2183 IkReal x687=IKsin(j2);
2184 IkReal x688=(py)*(py);
2185 IkReal x689=(px)*(px);
2186 IkReal x690=(pz)*(pz);
2187 IkReal x691=((sj0)*(sj1));
2188 IkReal x692=((IkReal(1.00000000000000))*(r01));
2189 IkReal x693=((r00)*(sj1));
2190 IkReal x694=((IkReal(0.0480000000000000))*(sj3));
2191 IkReal x695=((IkReal(0.0750000000000000))*(r00));
2192 IkReal x696=((IkReal(0.0800000000000000))*(sj3));
2193 IkReal x697=((cj1)*(r01));
2194 IkReal x698=((IkReal(0.150000000000000))*(py));
2195 IkReal x699=((pz)*(r02));
2196 IkReal x700=((px)*(r02));
2197 IkReal x701=((IkReal(0.0750000000000000))*(sj4));
2198 IkReal x702=((cj1)*(sj0));
2199 IkReal x703=((pz)*(r00));
2200 IkReal x704=((IkReal(0.0750000000000000))*(cj0));
2201 IkReal x705=((IkReal(2.00000000000000))*(cj0));
2202 IkReal x706=((cj1)*(r02));
2203 IkReal x707=((IkReal(0.150000000000000))*(sj1));
2204 IkReal x708=((px)*(r00));
2205 IkReal x709=((IkReal(2.00000000000000))*(py));
2206 IkReal x710=((IkReal(0.0903750000000000))*(sj3));
2207 IkReal x711=((r02)*(sj1));
2208 IkReal x712=((IkReal(0.150000000000000))*(cj1));
2209 IkReal x713=((cj0)*(py));
2210 IkReal x714=((IkReal(0.0480000000000000))*(cj3));
2211 IkReal x715=((r01)*(sj1));
2212 IkReal x716=((IkReal(0.0800000000000000))*(cj3));
2213 IkReal x717=((IkReal(0.150000000000000))*(pz));
2214 IkReal x718=((IkReal(1.00000000000000))*(cj1));
2215 IkReal x719=((cj0)*(r00));
2216 IkReal x720=((IkReal(0.600000000000000))*(py));
2217 IkReal x721=((IkReal(1.00000000000000))*(py));
2218 IkReal x722=((px)*(sj1));
2219 IkReal x723=((IkReal(1.00000000000000))*(cj0));
2220 IkReal x724=((cj0)*(pz));
2221 IkReal x725=((IkReal(0.103175000000000))*(cj3));
2222 IkReal x726=((IkReal(0.600000000000000))*(cj0));
2223 IkReal x727=((IkReal(1.00000000000000))*(sj3));
2224 IkReal x728=((px)*(py));
2225 IkReal x729=((r01)*(sj0));
2226 IkReal x730=((IkReal(2.00000000000000))*(px));
2227 IkReal x731=((cj4)*(x687));
2228 IkReal x732=((cj4)*(x686));
2229 IkReal x733=((sj4)*(x687));
2230 IkReal x734=((IkReal(2.00000000000000))*(x690));
2231 IkReal x735=((sj4)*(x686));
2232 IkReal x736=((IkReal(2.00000000000000))*(x688));
2233 IkReal x737=((px)*(x699)*(x705));
2234 evalcond[0]=((((IkReal(-1.00000000000000))*(x718)*(x719)))+(((IkReal(-1.00000000000000))*(x727)*(x732)))+(((IkReal(-1.00000000000000))*(cj3)*(x731)))+(x711)+(((IkReal(-1.00000000000000))*(x692)*(x702))));
2235 evalcond[1]=((((cj3)*(x732)))+(((IkReal(-1.00000000000000))*(x693)*(x723)))+(((IkReal(-1.00000000000000))*(x691)*(x692)))+(((IkReal(-1.00000000000000))*(x727)*(x731)))+(((IkReal(-1.00000000000000))*(x706))));
2236 evalcond[2]=((((IkReal(-0.0750000000000000))*(x687)))+(((IkReal(-1.00000000000000))*(x702)*(x721)))+(((x686)*(x716)))+(((pz)*(sj1)))+(((IkReal(0.320000000000000))*(x686)))+(((IkReal(-1.00000000000000))*(x687)*(x696)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x718)))+(((IkReal(0.0750000000000000))*(cj1))));
2237 evalcond[3]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x691)*(x721)))+(((IkReal(0.320000000000000))*(x687)))+(((IkReal(-1.00000000000000))*(x722)*(x723)))+(((IkReal(0.0750000000000000))*(x686)))+(((x686)*(x696)))+(((IkReal(-1.00000000000000))*(pz)*(x718)))+(((x687)*(x716)))+(((IkReal(0.0750000000000000))*(sj1))));
2238 evalcond[4]=((((px)*(x697)))+(((IkReal(-1.00000000000000))*(x691)*(x700)))+(((x695)*(x702)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x718)))+(((IkReal(-0.320000000000000))*(x735)))+(((IkReal(-1.00000000000000))*(sj1)*(x692)*(x724)))+(((x687)*(x701)))+(((IkReal(-1.00000000000000))*(x697)*(x704)))+(((x711)*(x713)))+(((x696)*(x733)))+(((IkReal(-1.00000000000000))*(x716)*(x735)))+(((x691)*(x703))));
2239 evalcond[5]=((((IkReal(0.320000000000000))*(x733)))+(((x702)*(x703)))+(((x706)*(x713)))+(((IkReal(-1.00000000000000))*(x692)*(x722)))+(((x696)*(x735)))+(((IkReal(-1.00000000000000))*(cj1)*(x692)*(x724)))+(((IkReal(-0.300000000000000))*(r00)*(sj0)))+(((x704)*(x715)))+(((py)*(x693)))+(((IkReal(-1.00000000000000))*(x700)*(x702)))+(((x716)*(x733)))+(((IkReal(0.300000000000000))*(cj0)*(r01)))+(((IkReal(-1.00000000000000))*(x691)*(x695)))+(((x686)*(x701))));
2240 evalcond[6]=((((IkReal(-1.00000000000000))*(pp)*(x693)*(x723)))+(((IkReal(0.0956250000000000))*(cj0)*(x693)))+(((x706)*(x734)))+(((IkReal(-0.600000000000000))*(x699)))+(((IkReal(0.0512000000000000))*(x732)))+(((IkReal(-0.150000000000000))*(px)*(x693)))+(((x691)*(x699)*(x709)))+(((pz)*(x697)*(x709)))+(((r02)*(x698)*(x702)))+(((r01)*(x691)*(x736)))+(((IkReal(-1.00000000000000))*(x698)*(x715)))+(((IkReal(-1.00000000000000))*(sj0)*(x697)*(x717)))+(((x694)*(x732)))+(((x705)*(x715)*(x728)))+(((cj0)*(x700)*(x712)))+(((x689)*(x693)*(x705)))+(((IkReal(0.0450000000000000))*(x729)))+(((IkReal(-0.0120000000000000))*(x731)))+(((IkReal(0.0956250000000000))*(r01)*(x691)))+(((IkReal(-1.00000000000000))*(x714)*(x731)))+(((IkReal(-0.600000000000000))*(x708)))+(((x725)*(x732)))+(((IkReal(-1.00000000000000))*(pp)*(x691)*(x692)))+(((x691)*(x708)*(x709)))+(((IkReal(0.0843750000000000))*(x706)))+(((IkReal(-1.00000000000000))*(cj0)*(x703)*(x712)))+(((x699)*(x705)*(x722)))+(((x710)*(x731)))+(((cj1)*(x703)*(x730)))+(((IkReal(0.0450000000000000))*(x719)))+(((IkReal(-1.00000000000000))*(r01)*(x720)))+(((IkReal(-1.00000000000000))*(pp)*(x706)))+(((IkReal(-1.00000000000000))*(x699)*(x707))));
2241 evalcond[7]=((((IkReal(0.0956250000000000))*(x711)))+(((x702)*(x708)*(x709)))+(((IkReal(-0.0512000000000000))*(x731)))+(((IkReal(-1.00000000000000))*(x699)*(x712)))+(((IkReal(-1.00000000000000))*(pp)*(x692)*(x702)))+(((IkReal(0.0450000000000000))*(r02)))+(((x699)*(x702)*(x709)))+(((cj1)*(x737)))+(((IkReal(-1.00000000000000))*(cj0)*(x700)*(x707)))+(((IkReal(-1.00000000000000))*(pp)*(x718)*(x719)))+(((cj0)*(x693)*(x717)))+(((IkReal(-0.0843750000000000))*(sj0)*(x697)))+(((IkReal(-1.00000000000000))*(pz)*(x709)*(x715)))+(((pp)*(x711)))+(((r01)*(x691)*(x717)))+(((IkReal(-1.00000000000000))*(pz)*(x693)*(x730)))+(((x697)*(x705)*(x728)))+(((IkReal(-1.00000000000000))*(x714)*(x732)))+(((sj0)*(x697)*(x736)))+(((IkReal(-0.0120000000000000))*(x732)))+(((IkReal(-1.00000000000000))*(x697)*(x698)))+(((IkReal(-1.00000000000000))*(x711)*(x734)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x720)))+(((cj1)*(r00)*(x689)*(x705)))+(((IkReal(-1.00000000000000))*(x725)*(x731)))+(((IkReal(0.600000000000000))*(pz)*(x729)))+(((IkReal(-1.00000000000000))*(x694)*(x731)))+(((IkReal(-0.0843750000000000))*(cj1)*(x719)))+(((IkReal(-1.00000000000000))*(x708)*(x712)))+(((x710)*(x732)))+(((IkReal(-1.00000000000000))*(r02)*(x691)*(x698)))+(((IkReal(-1.00000000000000))*(x700)*(x726)))+(((x703)*(x726))));
2242 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 )
2243 {
2244 continue;
2245 }
2246 }
2247 
2248 {
2249 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
2250 vinfos[0].jointtype = 1;
2251 vinfos[0].foffset = j0;
2252 vinfos[0].indices[0] = _ij0[0];
2253 vinfos[0].indices[1] = _ij0[1];
2254 vinfos[0].maxsolutions = _nj0;
2255 vinfos[1].jointtype = 1;
2256 vinfos[1].foffset = j1;
2257 vinfos[1].indices[0] = _ij1[0];
2258 vinfos[1].indices[1] = _ij1[1];
2259 vinfos[1].maxsolutions = _nj1;
2260 vinfos[2].jointtype = 1;
2261 vinfos[2].foffset = j2;
2262 vinfos[2].indices[0] = _ij2[0];
2263 vinfos[2].indices[1] = _ij2[1];
2264 vinfos[2].maxsolutions = _nj2;
2265 vinfos[3].jointtype = 1;
2266 vinfos[3].foffset = j3;
2267 vinfos[3].indices[0] = _ij3[0];
2268 vinfos[3].indices[1] = _ij3[1];
2269 vinfos[3].maxsolutions = _nj3;
2270 vinfos[4].jointtype = 1;
2271 vinfos[4].foffset = j4;
2272 vinfos[4].indices[0] = _ij4[0];
2273 vinfos[4].indices[1] = _ij4[1];
2274 vinfos[4].maxsolutions = _nj4;
2275 std::vector<int> vfree(0);
2276 solutions.AddSolution(vinfos,vfree);
2277 }
2278 }
2279 }
2280 
2281 }
2282 
2283 }
2284 }
2285 }
2286 
2287 }
2288 
2289 }
2290 
2291 } else
2292 {
2293 {
2294 IkReal j3array[1], cj3array[1], sj3array[1];
2295 bool j3valid[1]={false};
2296 _nj3 = 1;
2297 IkReal x738=((r01)*(sj0));
2298 IkReal x739=((cj0)*(sj1));
2299 IkReal x740=((cj4)*(px));
2300 IkReal x741=((cj0)*(r00));
2301 IkReal x742=((cj4)*(pp));
2302 IkReal x743=((cj1)*(r02));
2303 IkReal x744=((pz)*(r02));
2304 IkReal x745=((cj4)*(sj1));
2305 IkReal x746=((px)*(r00));
2306 IkReal x747=((py)*(r01));
2307 IkReal x748=((cj4)*(py)*(sj0));
2308 IkReal x749=((cj1)*(cj4)*(pz));
2309 if( IKabs(((gconst0)*(((((IkReal(3840.00000000000))*(r00)*(x739)))+(((IkReal(3840.00000000000))*(sj1)*(x738)))+(((IkReal(-12800.0000000000))*(x747)))+(((IkReal(-12800.0000000000))*(x744)))+(((IkReal(-2812.50000000000))*(x748)))+(((IkReal(-352.500000000000))*(cj4)))+(((IkReal(960.000000000000))*(x738)))+(((IkReal(843.750000000000))*(x745)))+(((IkReal(3840.00000000000))*(x743)))+(((IkReal(-12800.0000000000))*(x746)))+(((IkReal(-11250.0000000000))*(x749)))+(((IkReal(-2812.50000000000))*(cj0)*(x740)))+(((IkReal(-11250.0000000000))*(x739)*(x740)))+(((IkReal(960.000000000000))*(x741)))+(((IkReal(18750.0000000000))*(x742)))+(((IkReal(-11250.0000000000))*(py)*(sj0)*(x745))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-12000.0000000000))*(cj0)*(x740)))+(((IkReal(-900.000000000000))*(r00)*(x739)))+(((IkReal(-12000.0000000000))*(x748)))+(((IkReal(-48000.0000000000))*(x739)*(x740)))+(((IkReal(-48000.0000000000))*(py)*(sj0)*(x745)))+(((IkReal(3000.00000000000))*(x747)))+(((IkReal(-48000.0000000000))*(x749)))+(((IkReal(-900.000000000000))*(sj1)*(x738)))+(((IkReal(-225.000000000000))*(x741)))+(((IkReal(3000.00000000000))*(x746)))+(((IkReal(3000.00000000000))*(x744)))+(((IkReal(-900.000000000000))*(x743)))+(((IkReal(80000.0000000000))*(x742)))+(((IkReal(-225.000000000000))*(x738)))+(((IkReal(3600.00000000000))*(x745)))+(((IkReal(-1504.00000000000))*(cj4))))))) < IKFAST_ATAN2_MAGTHRESH )
2310  continue;
2311 j3array[0]=IKatan2(((gconst0)*(((((IkReal(3840.00000000000))*(r00)*(x739)))+(((IkReal(3840.00000000000))*(sj1)*(x738)))+(((IkReal(-12800.0000000000))*(x747)))+(((IkReal(-12800.0000000000))*(x744)))+(((IkReal(-2812.50000000000))*(x748)))+(((IkReal(-352.500000000000))*(cj4)))+(((IkReal(960.000000000000))*(x738)))+(((IkReal(843.750000000000))*(x745)))+(((IkReal(3840.00000000000))*(x743)))+(((IkReal(-12800.0000000000))*(x746)))+(((IkReal(-11250.0000000000))*(x749)))+(((IkReal(-2812.50000000000))*(cj0)*(x740)))+(((IkReal(-11250.0000000000))*(x739)*(x740)))+(((IkReal(960.000000000000))*(x741)))+(((IkReal(18750.0000000000))*(x742)))+(((IkReal(-11250.0000000000))*(py)*(sj0)*(x745)))))), ((gconst0)*(((((IkReal(-12000.0000000000))*(cj0)*(x740)))+(((IkReal(-900.000000000000))*(r00)*(x739)))+(((IkReal(-12000.0000000000))*(x748)))+(((IkReal(-48000.0000000000))*(x739)*(x740)))+(((IkReal(-48000.0000000000))*(py)*(sj0)*(x745)))+(((IkReal(3000.00000000000))*(x747)))+(((IkReal(-48000.0000000000))*(x749)))+(((IkReal(-900.000000000000))*(sj1)*(x738)))+(((IkReal(-225.000000000000))*(x741)))+(((IkReal(3000.00000000000))*(x746)))+(((IkReal(3000.00000000000))*(x744)))+(((IkReal(-900.000000000000))*(x743)))+(((IkReal(80000.0000000000))*(x742)))+(((IkReal(-225.000000000000))*(x738)))+(((IkReal(3600.00000000000))*(x745)))+(((IkReal(-1504.00000000000))*(cj4)))))));
2312 sj3array[0]=IKsin(j3array[0]);
2313 cj3array[0]=IKcos(j3array[0]);
2314 if( j3array[0] > IKPI )
2315 {
2316  j3array[0]-=IK2PI;
2317 }
2318 else if( j3array[0] < -IKPI )
2319 { j3array[0]+=IK2PI;
2320 }
2321 j3valid[0] = true;
2322 for(int ij3 = 0; ij3 < 1; ++ij3)
2323 {
2324 if( !j3valid[ij3] )
2325 {
2326  continue;
2327 }
2328 _ij3[0] = ij3; _ij3[1] = -1;
2329 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2330 {
2331 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2332 {
2333  j3valid[iij3]=false; _ij3[1] = iij3; break;
2334 }
2335 }
2336 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2337 {
2338 IkReal evalcond[4];
2339 IkReal x750=IKcos(j3);
2340 IkReal x751=IKsin(j3);
2341 IkReal x752=((r01)*(sj0));
2342 IkReal x753=((IkReal(0.300000000000000))*(r02));
2343 IkReal x754=((r00)*(sj0));
2344 IkReal x755=((py)*(sj0));
2345 IkReal x756=((IkReal(0.600000000000000))*(cj1));
2346 IkReal x757=((IkReal(0.150000000000000))*(px));
2347 IkReal x758=((IkReal(0.320000000000000))*(cj4));
2348 IkReal x759=((IkReal(1.00000000000000))*(pz));
2349 IkReal x760=((py)*(r00));
2350 IkReal x761=((IkReal(0.300000000000000))*(cj1));
2351 IkReal x762=((IkReal(1.00000000000000))*(pp));
2352 IkReal x763=((cj0)*(r00));
2353 IkReal x764=((IkReal(0.0450000000000000))*(sj1));
2354 IkReal x765=((IkReal(0.600000000000000))*(sj1));
2355 IkReal x766=((IkReal(0.0750000000000000))*(cj4));
2356 IkReal x767=((cj0)*(r01));
2357 IkReal x768=((IkReal(2.00000000000000))*(pz));
2358 IkReal x769=((cj0)*(px));
2359 IkReal x770=((IkReal(0.300000000000000))*(sj1));
2360 IkReal x771=((IkReal(2.00000000000000))*(px)*(py));
2361 IkReal x772=((IkReal(0.0120000000000000))*(x751));
2362 IkReal x773=((IkReal(0.0512000000000000))*(x750));
2363 IkReal x774=((cj0)*(py)*(r02));
2364 IkReal x775=((px)*(r02)*(sj0));
2365 evalcond[0]=((IkReal(0.0188000000000000))+(((cj0)*(x757)))+(((pz)*(x756)))+(((IkReal(0.150000000000000))*(x755)))+(((x755)*(x765)))+(((IkReal(-1.00000000000000))*(x762)))+(x773)+(x772)+(((IkReal(-1.00000000000000))*(x764)))+(((x765)*(x769))));
2366 evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((x752)*(x770)))+(((IkReal(0.0750000000000000))*(x763)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((cj1)*(x753)))+(((x750)*(x766)))+(((IkReal(0.0750000000000000))*(x752)))+(((IkReal(-1.00000000000000))*(r02)*(x759)))+(((x763)*(x770)))+(((IkReal(-1.00000000000000))*(x751)*(x758))));
2367 evalcond[2]=((((IkReal(-0.0800000000000000))*(cj4)))+(((r02)*(x769)))+(((x752)*(x761)))+(((IkReal(-1.00000000000000))*(x759)*(x763)))+(((r02)*(x755)))+(((IkReal(-1.00000000000000))*(sj1)*(x753)))+(((IkReal(-1.00000000000000))*(x752)*(x759)))+(((x761)*(x763)))+(((IkReal(-0.0750000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x751)*(x766)))+(((IkReal(-1.00000000000000))*(x750)*(x758))));
2368 evalcond[3]=((((pp)*(x767)))+(((x752)*(x771)))+(((IkReal(-1.00000000000000))*(x754)*(x764)))+(((IkReal(-1.00000000000000))*(r01)*(x757)))+(((pz)*(x754)*(x756)))+(((IkReal(0.150000000000000))*(x760)))+(((IkReal(-1.00000000000000))*(x754)*(x762)))+(((IkReal(-2.00000000000000))*(x760)*(x769)))+(((IkReal(-1.00000000000000))*(pz)*(x756)*(x767)))+(((IkReal(-1.00000000000000))*(x768)*(x774)))+(((x756)*(x774)))+(((IkReal(-1.00000000000000))*(sj4)*(x772)))+(((IkReal(0.0956250000000000))*(x767)))+(((IkReal(-2.00000000000000))*(x767)*((py)*(py))))+(((IkReal(-0.114425000000000))*(sj4)))+(((IkReal(-1.00000000000000))*(sj4)*(x773)))+(((x768)*(x775)))+(((IkReal(2.00000000000000))*(x754)*((px)*(px))))+(((IkReal(-0.0956250000000000))*(x754)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x765)))+(((x760)*(x765)))+(((IkReal(-1.00000000000000))*(x756)*(x775)))+(((x764)*(x767))));
2369 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2370 {
2371 continue;
2372 }
2373 }
2374 
2375 {
2376 IkReal dummyeval[1];
2377 IkReal gconst2;
2378 gconst2=IKsign(((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3))))));
2379 dummyeval[0]=((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3)))));
2380 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2381 {
2382 {
2383 IkReal dummyeval[1];
2384 IkReal gconst3;
2385 IkReal x776=((IkReal(0.0800000000000000))*(cj4));
2386 gconst3=IKsign(((((IkReal(0.320000000000000))*(cj3)*(cj4)))+(((IkReal(0.0750000000000000))*(cj4)*(sj3)))+(((x776)*((sj3)*(sj3))))+(((x776)*((cj3)*(cj3))))));
2387 IkReal x777=((IkReal(1.06666666666667))*(cj4));
2388 dummyeval[0]=((((IkReal(4.26666666666667))*(cj3)*(cj4)))+(((x777)*((cj3)*(cj3))))+(((x777)*((sj3)*(sj3))))+(((cj4)*(sj3))));
2389 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2390 {
2391 {
2392 IkReal evalcond[11];
2393 IkReal x778=((IkReal(0.0512000000000000))*(cj3));
2394 IkReal x779=((IkReal(0.0120000000000000))*(sj3));
2395 IkReal x780=(py)*(py);
2396 IkReal x781=(px)*(px);
2397 IkReal x782=(pz)*(pz);
2398 IkReal x783=((r01)*(sj0));
2399 IkReal x784=((py)*(r00));
2400 IkReal x785=((pz)*(sj1));
2401 IkReal x786=((py)*(r01));
2402 IkReal x787=((px)*(sj0));
2403 IkReal x788=((IkReal(0.600000000000000))*(r02));
2404 IkReal x789=((IkReal(0.150000000000000))*(cj1));
2405 IkReal x790=((cj0)*(sj1));
2406 IkReal x791=((IkReal(0.150000000000000))*(px));
2407 IkReal x792=((IkReal(2.00000000000000))*(cj1));
2408 IkReal x793=((cj0)*(r01));
2409 IkReal x794=((r02)*(sj1));
2410 IkReal x795=((px)*(r00));
2411 IkReal x796=((IkReal(0.300000000000000))*(r00));
2412 IkReal x797=((IkReal(1.00000000000000))*(pz));
2413 IkReal x798=((r00)*(sj1));
2414 IkReal x799=((cj0)*(r00));
2415 IkReal x800=((cj0)*(cj1));
2416 IkReal x801=((IkReal(1.00000000000000))*(sj1));
2417 IkReal x802=((IkReal(0.0956250000000000))*(r00));
2418 IkReal x803=((IkReal(0.600000000000000))*(pz));
2419 IkReal x804=((IkReal(0.600000000000000))*(sj1));
2420 IkReal x805=((IkReal(2.00000000000000))*(px));
2421 IkReal x806=((IkReal(2.00000000000000))*(sj1));
2422 IkReal x807=((IkReal(0.150000000000000))*(sj1));
2423 IkReal x808=((cj1)*(r02));
2424 IkReal x809=((cj0)*(px));
2425 IkReal x810=((IkReal(0.0843750000000000))*(cj1));
2426 IkReal x811=((py)*(sj0));
2427 IkReal x812=((pz)*(r02));
2428 IkReal x813=((IkReal(1.00000000000000))*(cj1));
2429 IkReal x814=((cj0)*(py));
2430 IkReal x815=((r00)*(sj0));
2431 IkReal x816=((r02)*(x811));
2432 IkReal x817=((pp)*(x813));
2433 IkReal x818=((IkReal(1.00000000000000))*(pp)*(r00));
2434 IkReal x819=((x779)+(x778));
2435 IkReal x820=((IkReal(2.00000000000000))*(r00)*(x781));
2436 IkReal x821=((cj0)*(x805)*(x812));
2437 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
2438 evalcond[1]=((((IkReal(-1.00000000000000))*(x787)))+(x814));
2439 evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x815)))+(x793));
2440 evalcond[3]=((((IkReal(-1.00000000000000))*(x799)*(x813)))+(x794)+(((IkReal(-1.00000000000000))*(x783)*(x813))));
2441 evalcond[4]=((((IkReal(-1.00000000000000))*(x808)))+(((IkReal(-1.00000000000000))*(r00)*(x790)))+(((IkReal(-1.00000000000000))*(x783)*(x801))));
2442 evalcond[5]=((IkReal(0.0188000000000000))+(((cj0)*(x791)))+(((x804)*(x811)))+(((IkReal(-1.00000000000000))*(pp)))+(((cj1)*(x803)))+(((IkReal(0.150000000000000))*(x811)))+(((IkReal(0.600000000000000))*(px)*(x790)))+(x819)+(((IkReal(-0.0450000000000000))*(sj1))));
2443 evalcond[6]=((((IkReal(-1.00000000000000))*(r02)*(x797)))+(((IkReal(0.0750000000000000))*(x783)))+(((IkReal(-1.00000000000000))*(x786)))+(((IkReal(0.300000000000000))*(x808)))+(((IkReal(0.300000000000000))*(sj1)*(x783)))+(((IkReal(0.0750000000000000))*(x799)))+(((x790)*(x796)))+(((IkReal(-1.00000000000000))*(x795))));
2444 evalcond[7]=((((r02)*(x809)))+(((IkReal(0.300000000000000))*(cj1)*(x783)))+(((IkReal(-1.00000000000000))*(x797)*(x799)))+(((IkReal(-1.00000000000000))*(x783)*(x797)))+(((IkReal(-0.300000000000000))*(x794)))+(((IkReal(-0.0750000000000000))*(r02)))+(x816)+(((x796)*(x800))));
2445 evalcond[8]=((IkReal(-0.114425000000000))+(((pp)*(x793)))+(((IkReal(2.00000000000000))*(x787)*(x812)))+(((IkReal(0.0450000000000000))*(r01)*(x790)))+(((IkReal(-0.0450000000000000))*(sj0)*(x798)))+(((IkReal(-1.00000000000000))*(cj1)*(x787)*(x788)))+(((cj1)*(x803)*(x815)))+(((IkReal(2.00000000000000))*(x781)*(x815)))+(((IkReal(-1.00000000000000))*(r01)*(x791)))+(((IkReal(-2.00000000000000))*(x780)*(x793)))+(((IkReal(0.0956250000000000))*(x793)))+(((IkReal(-2.00000000000000))*(x812)*(x814)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x804)))+(((IkReal(-1.00000000000000))*(x819)))+(((IkReal(0.150000000000000))*(x784)))+(((x784)*(x804)))+(((IkReal(-1.00000000000000))*(pp)*(x815)))+(((IkReal(-1.00000000000000))*(sj0)*(x802)))+(((py)*(x783)*(x805)))+(((py)*(x788)*(x800)))+(((IkReal(-1.00000000000000))*(cj0)*(x784)*(x805)))+(((IkReal(-1.00000000000000))*(cj1)*(x793)*(x803))));
2446 evalcond[9]=((((IkReal(0.0843750000000000))*(x808)))+(((r02)*(x789)*(x809)))+(((IkReal(-1.00000000000000))*(pz)*(x789)*(x799)))+(((IkReal(-1.00000000000000))*(pp)*(x783)*(x801)))+(((IkReal(-0.600000000000000))*(x786)))+(((x790)*(x802)))+(((x780)*(x783)*(x806)))+(((r02)*(x782)*(x792)))+(((IkReal(-1.00000000000000))*(pp)*(x808)))+(((pz)*(x786)*(x792)))+(((IkReal(-1.00000000000000))*(pz)*(x788)))+(((x789)*(x816)))+(((cj0)*(r02)*(x785)*(x805)))+(((IkReal(2.00000000000000))*(x785)*(x816)))+(((x784)*(x787)*(x806)))+(((IkReal(-1.00000000000000))*(pz)*(x783)*(x789)))+(((IkReal(-0.150000000000000))*(r02)*(x785)))+(((IkReal(0.0450000000000000))*(x799)))+(((pz)*(x792)*(x795)))+(((IkReal(-1.00000000000000))*(x791)*(x798)))+(((x790)*(x820)))+(((IkReal(-1.00000000000000))*(x786)*(x807)))+(((IkReal(-1.00000000000000))*(x790)*(x818)))+(((IkReal(-0.600000000000000))*(x795)))+(((x786)*(x790)*(x805)))+(((IkReal(0.0956250000000000))*(sj1)*(x783)))+(((IkReal(0.0450000000000000))*(x783))));
2447 evalcond[10]=((((x783)*(x803)))+(((IkReal(-1.00000000000000))*(x783)*(x810)))+(((x784)*(x787)*(x792)))+(((x799)*(x803)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x788)*(x811)))+(((IkReal(-1.00000000000000))*(r02)*(x790)*(x791)))+(((IkReal(-1.00000000000000))*(x799)*(x810)))+(((IkReal(-2.00000000000000))*(x785)*(x786)))+(((pp)*(x794)))+(((IkReal(0.0956250000000000))*(x794)))+(((IkReal(-1.00000000000000))*(x789)*(x795)))+(((IkReal(-1.00000000000000))*(x783)*(x817)))+(((IkReal(0.150000000000000))*(x785)*(x799)))+(((IkReal(-1.00000000000000))*(x799)*(x817)))+(((x781)*(x792)*(x799)))+(((IkReal(-1.00000000000000))*(x786)*(x789)))+(((IkReal(-2.00000000000000))*(x782)*(x794)))+(((IkReal(-0.150000000000000))*(x794)*(x811)))+(((IkReal(-2.00000000000000))*(x785)*(x795)))+(((x786)*(x792)*(x809)))+(((x792)*(x811)*(x812)))+(((IkReal(-1.00000000000000))*(x789)*(x812)))+(((x780)*(x783)*(x792)))+(((x792)*(x809)*(x812)))+(((IkReal(0.150000000000000))*(x783)*(x785)))+(((IkReal(-1.00000000000000))*(x788)*(x809))));
2448 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 )
2449 {
2450 {
2451 IkReal dummyeval[1];
2452 IkReal gconst4;
2453 gconst4=IKsign(((IkReal(0.108025000000000))+(((IkReal(0.0512000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
2454 dummyeval[0]=((IkReal(16.8789062500000))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3))+(((IkReal(8.00000000000000))*(cj3))));
2455 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2456 {
2457 {
2458 IkReal dummyeval[1];
2459 IkReal gconst5;
2460 gconst5=IKsign(((IkReal(0.108025000000000))+(((IkReal(0.0512000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
2461 dummyeval[0]=((IkReal(16.8789062500000))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3))+(((IkReal(8.00000000000000))*(cj3))));
2462 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2463 {
2464 continue;
2465 
2466 } else
2467 {
2468 {
2469 IkReal j2array[1], cj2array[1], sj2array[1];
2470 bool j2valid[1]={false};
2471 _nj2 = 1;
2472 IkReal x822=((r02)*(sj0));
2473 IkReal x823=((IkReal(0.0800000000000000))*(sj1));
2474 IkReal x824=((cj3)*(px));
2475 IkReal x825=((cj0)*(r01));
2476 IkReal x826=((IkReal(0.0240000000000000))*(cj1));
2477 IkReal x827=((pz)*(sj3));
2478 IkReal x828=((IkReal(0.00600000000000000))*(cj1));
2479 IkReal x829=((IkReal(0.320000000000000))*(px));
2480 IkReal x830=((cj0)*(sj1));
2481 IkReal x831=((cj1)*(r01));
2482 IkReal x832=((IkReal(0.0750000000000000))*(px));
2483 IkReal x833=((IkReal(0.00562500000000000))*(cj1));
2484 IkReal x834=((py)*(r02));
2485 IkReal x835=((IkReal(0.0750000000000000))*(pz));
2486 IkReal x836=((r00)*(sj0));
2487 IkReal x837=((cj3)*(pz));
2488 IkReal x838=((IkReal(0.320000000000000))*(py));
2489 IkReal x839=((sj0)*(sj1));
2490 IkReal x840=((IkReal(0.0800000000000000))*(cj1));
2491 IkReal x841=((cj1)*(r00));
2492 IkReal x842=((IkReal(0.0750000000000000))*(py));
2493 IkReal x843=((cj3)*(py));
2494 IkReal x844=((IkReal(0.00600000000000000))*(sj1));
2495 IkReal x845=((py)*(sj3));
2496 IkReal x846=((IkReal(0.320000000000000))*(pz)*(sj1));
2497 IkReal x847=((px)*(sj3)*(x823));
2498 if( IKabs(((gconst5)*(((IkReal(-0.0960000000000000))+(((IkReal(-0.0800000000000000))*(px)*(sj3)*(x831)))+(((sj0)*(x823)*(x843)))+(((IkReal(-1.00000000000000))*(sj1)*(x835)*(x836)))+(((x838)*(x839)))+(((x837)*(x840)))+(((IkReal(-1.00000000000000))*(cj0)*(sj3)*(x823)*(x834)))+(((IkReal(-1.00000000000000))*(x831)*(x832)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((sj1)*(x825)*(x835)))+(((sj3)*(x825)*(x828)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((sj1)*(x822)*(x832)))+(((x841)*(x842)))+(((x825)*(x833)))+(((IkReal(-1.00000000000000))*(x833)*(x836)))+(((IkReal(-1.00000000000000))*(cj3)*(x844)))+(((IkReal(-1.00000000000000))*(sj3)*(x828)*(x836)))+(((IkReal(-1.00000000000000))*(x823)*(x827)*(x836)))+(((IkReal(0.320000000000000))*(cj1)*(pz)))+(((x829)*(x830)))+(((x823)*(x825)*(x827)))+(((r00)*(x840)*(x845)))+(((x822)*(x847)))+(((IkReal(-0.0750000000000000))*(x830)*(x834)))+(((cj0)*(x823)*(x824))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(-0.0225000000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x825)*(x828)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x822)*(x823)*(x824)))+(((x830)*(x832)))+(((IkReal(-1.00000000000000))*(x825)*(x826)))+(((IkReal(-1.00000000000000))*(sj3)*(x844)))+(((x827)*(x840)))+(((IkReal(-1.00000000000000))*(sj1)*(x822)*(x829)))+(((x829)*(x831)))+(((x826)*(x836)))+(((cj1)*(x835)))+(((IkReal(-1.00000000000000))*(x838)*(x841)))+(((x839)*(x842)))+(((IkReal(-1.00000000000000))*(x823)*(x825)*(x837)))+(((x823)*(x836)*(x837)))+(((sj0)*(x823)*(x845)))+(((x836)*(x846)))+(((cj0)*(cj3)*(x823)*(x834)))+(((IkReal(-1.00000000000000))*(x825)*(x846)))+(((IkReal(-1.00000000000000))*(r00)*(x840)*(x843)))+(((IkReal(0.0800000000000000))*(x824)*(x831)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((cj3)*(x828)*(x836)))+(((cj0)*(x847)))+(((IkReal(0.320000000000000))*(x830)*(x834))))))) < IKFAST_ATAN2_MAGTHRESH )
2499  continue;
2500 j2array[0]=IKatan2(((gconst5)*(((IkReal(-0.0960000000000000))+(((IkReal(-0.0800000000000000))*(px)*(sj3)*(x831)))+(((sj0)*(x823)*(x843)))+(((IkReal(-1.00000000000000))*(sj1)*(x835)*(x836)))+(((x838)*(x839)))+(((x837)*(x840)))+(((IkReal(-1.00000000000000))*(cj0)*(sj3)*(x823)*(x834)))+(((IkReal(-1.00000000000000))*(x831)*(x832)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((sj1)*(x825)*(x835)))+(((sj3)*(x825)*(x828)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((sj1)*(x822)*(x832)))+(((x841)*(x842)))+(((x825)*(x833)))+(((IkReal(-1.00000000000000))*(x833)*(x836)))+(((IkReal(-1.00000000000000))*(cj3)*(x844)))+(((IkReal(-1.00000000000000))*(sj3)*(x828)*(x836)))+(((IkReal(-1.00000000000000))*(x823)*(x827)*(x836)))+(((IkReal(0.320000000000000))*(cj1)*(pz)))+(((x829)*(x830)))+(((x823)*(x825)*(x827)))+(((r00)*(x840)*(x845)))+(((x822)*(x847)))+(((IkReal(-0.0750000000000000))*(x830)*(x834)))+(((cj0)*(x823)*(x824)))))), ((gconst5)*(((IkReal(-0.0225000000000000))+(((IkReal(-1.00000000000000))*(cj3)*(x825)*(x828)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x822)*(x823)*(x824)))+(((x830)*(x832)))+(((IkReal(-1.00000000000000))*(x825)*(x826)))+(((IkReal(-1.00000000000000))*(sj3)*(x844)))+(((x827)*(x840)))+(((IkReal(-1.00000000000000))*(sj1)*(x822)*(x829)))+(((x829)*(x831)))+(((x826)*(x836)))+(((cj1)*(x835)))+(((IkReal(-1.00000000000000))*(x838)*(x841)))+(((x839)*(x842)))+(((IkReal(-1.00000000000000))*(x823)*(x825)*(x837)))+(((x823)*(x836)*(x837)))+(((sj0)*(x823)*(x845)))+(((x836)*(x846)))+(((cj0)*(cj3)*(x823)*(x834)))+(((IkReal(-1.00000000000000))*(x825)*(x846)))+(((IkReal(-1.00000000000000))*(r00)*(x840)*(x843)))+(((IkReal(0.0800000000000000))*(x824)*(x831)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((cj3)*(x828)*(x836)))+(((cj0)*(x847)))+(((IkReal(0.320000000000000))*(x830)*(x834)))))));
2501 sj2array[0]=IKsin(j2array[0]);
2502 cj2array[0]=IKcos(j2array[0]);
2503 if( j2array[0] > IKPI )
2504 {
2505  j2array[0]-=IK2PI;
2506 }
2507 else if( j2array[0] < -IKPI )
2508 { j2array[0]+=IK2PI;
2509 }
2510 j2valid[0] = true;
2511 for(int ij2 = 0; ij2 < 1; ++ij2)
2512 {
2513 if( !j2valid[ij2] )
2514 {
2515  continue;
2516 }
2517 _ij2[0] = ij2; _ij2[1] = -1;
2518 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2519 {
2520 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2521 {
2522  j2valid[iij2]=false; _ij2[1] = iij2; break;
2523 }
2524 }
2525 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2526 {
2527 IkReal evalcond[4];
2528 IkReal x848=IKcos(j2);
2529 IkReal x849=IKsin(j2);
2530 IkReal x850=((IkReal(0.0800000000000000))*(sj3));
2531 IkReal x851=((cj0)*(r01));
2532 IkReal x852=((IkReal(1.00000000000000))*(px));
2533 IkReal x853=((py)*(sj1));
2534 IkReal x854=((cj0)*(r02));
2535 IkReal x855=((IkReal(0.0750000000000000))*(cj1));
2536 IkReal x856=((r02)*(sj0));
2537 IkReal x857=((IkReal(0.0750000000000000))*(sj1));
2538 IkReal x858=((cj1)*(pz));
2539 IkReal x859=((r00)*(sj0));
2540 IkReal x860=((IkReal(0.0800000000000000))*(cj3));
2541 IkReal x861=((pz)*(sj1));
2542 IkReal x862=((IkReal(1.00000000000000))*(sj0));
2543 IkReal x863=((cj1)*(py));
2544 IkReal x864=((IkReal(0.0750000000000000))*(x849));
2545 IkReal x865=((IkReal(0.320000000000000))*(x848));
2546 IkReal x866=((IkReal(0.320000000000000))*(x849));
2547 IkReal x867=((IkReal(0.0750000000000000))*(x848));
2548 IkReal x868=((sj1)*(x859));
2549 IkReal x869=((x849)*(x850));
2550 IkReal x870=((x848)*(x860));
2551 IkReal x871=((x848)*(x850));
2552 IkReal x872=((x849)*(x860));
2553 IkReal x873=((x869)+(x864));
2554 IkReal x874=((x865)+(x870));
2555 IkReal x875=((x867)+(x866)+(x871)+(x872));
2556 evalcond[0]=((((IkReal(-1.00000000000000))*(x862)*(x863)))+(x861)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x852)))+(x855)+(x874)+(((IkReal(-1.00000000000000))*(x873))));
2557 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x858)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x852)))+(((IkReal(-1.00000000000000))*(x853)*(x862)))+(x857)+(x875));
2558 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x863)))+(((x859)*(x861)))+(((x855)*(x859)))+(((IkReal(-1.00000000000000))*(x874)))+(((x853)*(x854)))+(((IkReal(-1.00000000000000))*(sj1)*(x852)*(x856)))+(((IkReal(-1.00000000000000))*(x851)*(x861)))+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(x851)*(x855)))+(x873));
2559 evalcond[3]=((((IkReal(-1.00000000000000))*(x851)*(x858)))+(((IkReal(-1.00000000000000))*(cj1)*(x852)*(x856)))+(((r00)*(x853)))+(((x851)*(x857)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x852)))+(((IkReal(-1.00000000000000))*(x857)*(x859)))+(x875)+(((IkReal(0.300000000000000))*(x851)))+(((x854)*(x863)))+(((x858)*(x859)))+(((IkReal(-0.300000000000000))*(x859))));
2560 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2561 {
2562 continue;
2563 }
2564 }
2565 
2566 {
2567 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
2568 vinfos[0].jointtype = 1;
2569 vinfos[0].foffset = j0;
2570 vinfos[0].indices[0] = _ij0[0];
2571 vinfos[0].indices[1] = _ij0[1];
2572 vinfos[0].maxsolutions = _nj0;
2573 vinfos[1].jointtype = 1;
2574 vinfos[1].foffset = j1;
2575 vinfos[1].indices[0] = _ij1[0];
2576 vinfos[1].indices[1] = _ij1[1];
2577 vinfos[1].maxsolutions = _nj1;
2578 vinfos[2].jointtype = 1;
2579 vinfos[2].foffset = j2;
2580 vinfos[2].indices[0] = _ij2[0];
2581 vinfos[2].indices[1] = _ij2[1];
2582 vinfos[2].maxsolutions = _nj2;
2583 vinfos[3].jointtype = 1;
2584 vinfos[3].foffset = j3;
2585 vinfos[3].indices[0] = _ij3[0];
2586 vinfos[3].indices[1] = _ij3[1];
2587 vinfos[3].maxsolutions = _nj3;
2588 vinfos[4].jointtype = 1;
2589 vinfos[4].foffset = j4;
2590 vinfos[4].indices[0] = _ij4[0];
2591 vinfos[4].indices[1] = _ij4[1];
2592 vinfos[4].maxsolutions = _nj4;
2593 std::vector<int> vfree(0);
2594 solutions.AddSolution(vinfos,vfree);
2595 }
2596 }
2597 }
2598 
2599 }
2600 
2601 }
2602 
2603 } else
2604 {
2605 {
2606 IkReal j2array[1], cj2array[1], sj2array[1];
2607 bool j2valid[1]={false};
2608 _nj2 = 1;
2609 IkReal x876=((IkReal(0.0800000000000000))*(cj1));
2610 IkReal x877=((cj0)*(px));
2611 IkReal x878=((py)*(sj0));
2612 IkReal x879=((IkReal(0.320000000000000))*(sj1));
2613 IkReal x880=((IkReal(0.00600000000000000))*(cj3));
2614 IkReal x881=((pz)*(sj3));
2615 IkReal x882=((IkReal(0.0750000000000000))*(cj1));
2616 IkReal x883=((IkReal(0.0800000000000000))*(sj1));
2617 IkReal x884=((IkReal(0.0750000000000000))*(sj1));
2618 IkReal x885=((IkReal(0.00600000000000000))*(sj3));
2619 IkReal x886=((cj3)*(pz));
2620 IkReal x887=((IkReal(0.320000000000000))*(cj1));
2621 if( IKabs(((gconst4)*(((IkReal(-0.0960000000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x880)))+(((IkReal(-1.00000000000000))*(sj3)*(x876)*(x878)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((x877)*(x879)))+(((x881)*(x883)))+(((pz)*(x884)))+(((pz)*(x887)))+(((cj1)*(x885)))+(((IkReal(0.00562500000000000))*(cj1)))+(((x878)*(x879)))+(((cj3)*(x878)*(x883)))+(((x876)*(x886)))+(((IkReal(-1.00000000000000))*(sj3)*(x876)*(x877)))+(((IkReal(-1.00000000000000))*(x877)*(x882)))+(((cj3)*(x877)*(x883)))+(((IkReal(-1.00000000000000))*(x878)*(x882))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(-0.0225000000000000))+(((x876)*(x881)))+(((cj3)*(x876)*(x877)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x883)*(x886)))+(((sj3)*(x878)*(x883)))+(((x877)*(x887)))+(((sj3)*(x877)*(x883)))+(((x878)*(x884)))+(((cj3)*(x876)*(x878)))+(((IkReal(-1.00000000000000))*(cj1)*(x880)))+(((x877)*(x884)))+(((x878)*(x887)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((pz)*(x882)))+(((IkReal(-1.00000000000000))*(sj1)*(x885)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(pz)*(x879))))))) < IKFAST_ATAN2_MAGTHRESH )
2622  continue;
2623 j2array[0]=IKatan2(((gconst4)*(((IkReal(-0.0960000000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x880)))+(((IkReal(-1.00000000000000))*(sj3)*(x876)*(x878)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((x877)*(x879)))+(((x881)*(x883)))+(((pz)*(x884)))+(((pz)*(x887)))+(((cj1)*(x885)))+(((IkReal(0.00562500000000000))*(cj1)))+(((x878)*(x879)))+(((cj3)*(x878)*(x883)))+(((x876)*(x886)))+(((IkReal(-1.00000000000000))*(sj3)*(x876)*(x877)))+(((IkReal(-1.00000000000000))*(x877)*(x882)))+(((cj3)*(x877)*(x883)))+(((IkReal(-1.00000000000000))*(x878)*(x882)))))), ((gconst4)*(((IkReal(-0.0225000000000000))+(((x876)*(x881)))+(((cj3)*(x876)*(x877)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x883)*(x886)))+(((sj3)*(x878)*(x883)))+(((x877)*(x887)))+(((sj3)*(x877)*(x883)))+(((x878)*(x884)))+(((cj3)*(x876)*(x878)))+(((IkReal(-1.00000000000000))*(cj1)*(x880)))+(((x877)*(x884)))+(((x878)*(x887)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((pz)*(x882)))+(((IkReal(-1.00000000000000))*(sj1)*(x885)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(pz)*(x879)))))));
2624 sj2array[0]=IKsin(j2array[0]);
2625 cj2array[0]=IKcos(j2array[0]);
2626 if( j2array[0] > IKPI )
2627 {
2628  j2array[0]-=IK2PI;
2629 }
2630 else if( j2array[0] < -IKPI )
2631 { j2array[0]+=IK2PI;
2632 }
2633 j2valid[0] = true;
2634 for(int ij2 = 0; ij2 < 1; ++ij2)
2635 {
2636 if( !j2valid[ij2] )
2637 {
2638  continue;
2639 }
2640 _ij2[0] = ij2; _ij2[1] = -1;
2641 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2642 {
2643 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2644 {
2645  j2valid[iij2]=false; _ij2[1] = iij2; break;
2646 }
2647 }
2648 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2649 {
2650 IkReal evalcond[4];
2651 IkReal x888=IKcos(j2);
2652 IkReal x889=IKsin(j2);
2653 IkReal x890=((IkReal(0.0800000000000000))*(sj3));
2654 IkReal x891=((cj0)*(r01));
2655 IkReal x892=((IkReal(1.00000000000000))*(px));
2656 IkReal x893=((py)*(sj1));
2657 IkReal x894=((cj0)*(r02));
2658 IkReal x895=((IkReal(0.0750000000000000))*(cj1));
2659 IkReal x896=((r02)*(sj0));
2660 IkReal x897=((IkReal(0.0750000000000000))*(sj1));
2661 IkReal x898=((cj1)*(pz));
2662 IkReal x899=((r00)*(sj0));
2663 IkReal x900=((IkReal(0.0800000000000000))*(cj3));
2664 IkReal x901=((pz)*(sj1));
2665 IkReal x902=((IkReal(1.00000000000000))*(sj0));
2666 IkReal x903=((cj1)*(py));
2667 IkReal x904=((IkReal(0.0750000000000000))*(x889));
2668 IkReal x905=((IkReal(0.320000000000000))*(x888));
2669 IkReal x906=((IkReal(0.320000000000000))*(x889));
2670 IkReal x907=((IkReal(0.0750000000000000))*(x888));
2671 IkReal x908=((sj1)*(x899));
2672 IkReal x909=((x889)*(x890));
2673 IkReal x910=((x888)*(x900));
2674 IkReal x911=((x888)*(x890));
2675 IkReal x912=((x889)*(x900));
2676 IkReal x913=((x904)+(x909));
2677 IkReal x914=((x905)+(x910));
2678 IkReal x915=((x906)+(x907)+(x912)+(x911));
2679 evalcond[0]=((((IkReal(-1.00000000000000))*(x902)*(x903)))+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x892)))+(((IkReal(-1.00000000000000))*(x913)))+(x901)+(x914)+(x895));
2680 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x892)))+(((IkReal(-1.00000000000000))*(x893)*(x902)))+(((IkReal(-1.00000000000000))*(x898)))+(x915)+(x897));
2681 evalcond[2]=((((x895)*(x899)))+(((IkReal(-1.00000000000000))*(x891)*(x895)))+(((IkReal(-1.00000000000000))*(x891)*(x901)))+(((cj1)*(px)*(r01)))+(((x893)*(x894)))+(((IkReal(-1.00000000000000))*(sj1)*(x892)*(x896)))+(((IkReal(-1.00000000000000))*(x914)))+(x913)+(((IkReal(-1.00000000000000))*(r00)*(x903)))+(((x899)*(x901))));
2682 evalcond[3]=((((IkReal(-0.300000000000000))*(x899)))+(((IkReal(-1.00000000000000))*(x897)*(x899)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x892)))+(((IkReal(-1.00000000000000))*(x891)*(x898)))+(((x891)*(x897)))+(x915)+(((x894)*(x903)))+(((IkReal(-1.00000000000000))*(cj1)*(x892)*(x896)))+(((r00)*(x893)))+(((IkReal(0.300000000000000))*(x891)))+(((x898)*(x899))));
2683 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2684 {
2685 continue;
2686 }
2687 }
2688 
2689 {
2690 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
2691 vinfos[0].jointtype = 1;
2692 vinfos[0].foffset = j0;
2693 vinfos[0].indices[0] = _ij0[0];
2694 vinfos[0].indices[1] = _ij0[1];
2695 vinfos[0].maxsolutions = _nj0;
2696 vinfos[1].jointtype = 1;
2697 vinfos[1].foffset = j1;
2698 vinfos[1].indices[0] = _ij1[0];
2699 vinfos[1].indices[1] = _ij1[1];
2700 vinfos[1].maxsolutions = _nj1;
2701 vinfos[2].jointtype = 1;
2702 vinfos[2].foffset = j2;
2703 vinfos[2].indices[0] = _ij2[0];
2704 vinfos[2].indices[1] = _ij2[1];
2705 vinfos[2].maxsolutions = _nj2;
2706 vinfos[3].jointtype = 1;
2707 vinfos[3].foffset = j3;
2708 vinfos[3].indices[0] = _ij3[0];
2709 vinfos[3].indices[1] = _ij3[1];
2710 vinfos[3].maxsolutions = _nj3;
2711 vinfos[4].jointtype = 1;
2712 vinfos[4].foffset = j4;
2713 vinfos[4].indices[0] = _ij4[0];
2714 vinfos[4].indices[1] = _ij4[1];
2715 vinfos[4].maxsolutions = _nj4;
2716 std::vector<int> vfree(0);
2717 solutions.AddSolution(vinfos,vfree);
2718 }
2719 }
2720 }
2721 
2722 }
2723 
2724 }
2725 
2726 } else
2727 {
2728 IkReal x916=((IkReal(0.0512000000000000))*(cj3));
2729 IkReal x917=((IkReal(0.0120000000000000))*(sj3));
2730 IkReal x918=(py)*(py);
2731 IkReal x919=(px)*(px);
2732 IkReal x920=(pz)*(pz);
2733 IkReal x921=((r01)*(sj0));
2734 IkReal x922=((py)*(r00));
2735 IkReal x923=((pz)*(sj1));
2736 IkReal x924=((py)*(r01));
2737 IkReal x925=((px)*(sj0));
2738 IkReal x926=((IkReal(0.600000000000000))*(r02));
2739 IkReal x927=((IkReal(0.150000000000000))*(cj1));
2740 IkReal x928=((cj0)*(sj1));
2741 IkReal x929=((IkReal(0.150000000000000))*(px));
2742 IkReal x930=((IkReal(2.00000000000000))*(cj1));
2743 IkReal x931=((cj0)*(r01));
2744 IkReal x932=((r02)*(sj1));
2745 IkReal x933=((px)*(r00));
2746 IkReal x934=((IkReal(0.300000000000000))*(r00));
2747 IkReal x935=((IkReal(1.00000000000000))*(pz));
2748 IkReal x936=((r00)*(sj1));
2749 IkReal x937=((cj0)*(r00));
2750 IkReal x938=((cj0)*(cj1));
2751 IkReal x939=((IkReal(1.00000000000000))*(sj1));
2752 IkReal x940=((IkReal(0.0956250000000000))*(r00));
2753 IkReal x941=((IkReal(0.600000000000000))*(pz));
2754 IkReal x942=((IkReal(0.600000000000000))*(sj1));
2755 IkReal x943=((IkReal(2.00000000000000))*(px));
2756 IkReal x944=((IkReal(2.00000000000000))*(sj1));
2757 IkReal x945=((IkReal(0.150000000000000))*(sj1));
2758 IkReal x946=((cj1)*(r02));
2759 IkReal x947=((cj0)*(px));
2760 IkReal x948=((IkReal(0.0843750000000000))*(cj1));
2761 IkReal x949=((py)*(sj0));
2762 IkReal x950=((pz)*(r02));
2763 IkReal x951=((IkReal(1.00000000000000))*(cj1));
2764 IkReal x952=((cj0)*(py));
2765 IkReal x953=((r00)*(sj0));
2766 IkReal x954=((r02)*(x949));
2767 IkReal x955=((pp)*(x951));
2768 IkReal x956=((IkReal(1.00000000000000))*(pp)*(r00));
2769 IkReal x957=((x917)+(x916));
2770 IkReal x958=((IkReal(2.00000000000000))*(r00)*(x919));
2771 IkReal x959=((cj0)*(x943)*(x950));
2772 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
2773 evalcond[1]=((((IkReal(-1.00000000000000))*(x925)))+(x952));
2774 evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x953)))+(x931));
2775 evalcond[3]=((((IkReal(-1.00000000000000))*(x937)*(x951)))+(x932)+(((IkReal(-1.00000000000000))*(x921)*(x951))));
2776 evalcond[4]=((((IkReal(-1.00000000000000))*(r00)*(x928)))+(((IkReal(-1.00000000000000))*(x921)*(x939)))+(((IkReal(-1.00000000000000))*(x946))));
2777 evalcond[5]=((IkReal(0.0188000000000000))+(((IkReal(0.150000000000000))*(x949)))+(((x942)*(x949)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(0.600000000000000))*(px)*(x928)))+(x957)+(((cj1)*(x941)))+(((cj0)*(x929)))+(((IkReal(-0.0450000000000000))*(sj1))));
2778 evalcond[6]=((((IkReal(0.300000000000000))*(sj1)*(x921)))+(((IkReal(-1.00000000000000))*(r02)*(x935)))+(((IkReal(-1.00000000000000))*(x933)))+(((IkReal(0.300000000000000))*(x946)))+(((IkReal(-1.00000000000000))*(x924)))+(((IkReal(0.0750000000000000))*(x937)))+(((IkReal(0.0750000000000000))*(x921)))+(((x928)*(x934))));
2779 evalcond[7]=((((IkReal(0.300000000000000))*(cj1)*(x921)))+(((IkReal(-1.00000000000000))*(x935)*(x937)))+(((r02)*(x947)))+(x954)+(((IkReal(-0.300000000000000))*(x932)))+(((IkReal(-0.0750000000000000))*(r02)))+(((x934)*(x938)))+(((IkReal(-1.00000000000000))*(x921)*(x935))));
2780 evalcond[8]=((IkReal(0.114425000000000))+(((x922)*(x942)))+(((py)*(x921)*(x943)))+(((IkReal(-1.00000000000000))*(cj1)*(x931)*(x941)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x942)))+(((IkReal(-1.00000000000000))*(sj0)*(x940)))+(((IkReal(2.00000000000000))*(x925)*(x950)))+(((IkReal(0.150000000000000))*(x922)))+(((IkReal(-2.00000000000000))*(x918)*(x931)))+(((IkReal(-1.00000000000000))*(cj0)*(x922)*(x943)))+(((cj1)*(x941)*(x953)))+(((IkReal(2.00000000000000))*(x919)*(x953)))+(x957)+(((IkReal(-1.00000000000000))*(pp)*(x953)))+(((pp)*(x931)))+(((IkReal(0.0956250000000000))*(x931)))+(((IkReal(-0.0450000000000000))*(sj0)*(x936)))+(((IkReal(-2.00000000000000))*(x950)*(x952)))+(((py)*(x926)*(x938)))+(((IkReal(0.0450000000000000))*(r01)*(x928)))+(((IkReal(-1.00000000000000))*(r01)*(x929)))+(((IkReal(-1.00000000000000))*(cj1)*(x925)*(x926))));
2781 evalcond[9]=((((x924)*(x928)*(x943)))+(((x928)*(x958)))+(((r02)*(x927)*(x947)))+(((IkReal(-0.600000000000000))*(x933)))+(((r02)*(x920)*(x930)))+(((IkReal(-1.00000000000000))*(x928)*(x956)))+(((IkReal(-1.00000000000000))*(pp)*(x921)*(x939)))+(((IkReal(2.00000000000000))*(x923)*(x954)))+(((x928)*(x940)))+(((x918)*(x921)*(x944)))+(((cj0)*(r02)*(x923)*(x943)))+(((x922)*(x925)*(x944)))+(((IkReal(-1.00000000000000))*(pz)*(x926)))+(((x927)*(x954)))+(((IkReal(-1.00000000000000))*(pz)*(x921)*(x927)))+(((IkReal(-1.00000000000000))*(x929)*(x936)))+(((pz)*(x924)*(x930)))+(((pz)*(x930)*(x933)))+(((IkReal(-1.00000000000000))*(pp)*(x946)))+(((IkReal(0.0956250000000000))*(sj1)*(x921)))+(((IkReal(-1.00000000000000))*(x924)*(x945)))+(((IkReal(0.0843750000000000))*(x946)))+(((IkReal(-0.150000000000000))*(r02)*(x923)))+(((IkReal(0.0450000000000000))*(x921)))+(((IkReal(-1.00000000000000))*(pz)*(x927)*(x937)))+(((IkReal(0.0450000000000000))*(x937)))+(((IkReal(-0.600000000000000))*(x924))));
2782 evalcond[10]=((((IkReal(-1.00000000000000))*(x926)*(x947)))+(((IkReal(-1.00000000000000))*(x927)*(x933)))+(((x930)*(x949)*(x950)))+(((IkReal(-2.00000000000000))*(x923)*(x924)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(x921)*(x955)))+(((pp)*(x932)))+(((IkReal(-1.00000000000000))*(x921)*(x948)))+(((IkReal(-2.00000000000000))*(x920)*(x932)))+(((IkReal(0.0956250000000000))*(x932)))+(((x921)*(x941)))+(((IkReal(0.150000000000000))*(x921)*(x923)))+(((IkReal(-1.00000000000000))*(x926)*(x949)))+(((x924)*(x930)*(x947)))+(((IkReal(0.150000000000000))*(x923)*(x937)))+(((IkReal(-1.00000000000000))*(x937)*(x948)))+(((x919)*(x930)*(x937)))+(((x930)*(x947)*(x950)))+(((IkReal(-1.00000000000000))*(x937)*(x955)))+(((x937)*(x941)))+(((x922)*(x925)*(x930)))+(((x918)*(x921)*(x930)))+(((IkReal(-0.150000000000000))*(x932)*(x949)))+(((IkReal(-1.00000000000000))*(x924)*(x927)))+(((IkReal(-1.00000000000000))*(x927)*(x950)))+(((IkReal(-1.00000000000000))*(r02)*(x928)*(x929)))+(((IkReal(-2.00000000000000))*(x923)*(x933))));
2783 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 )
2784 {
2785 {
2786 IkReal dummyeval[1];
2787 IkReal gconst6;
2788 gconst6=IKsign(((IkReal(0.108025000000000))+(((IkReal(0.0512000000000000))*(cj3)))+(((IkReal(0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(0.00640000000000000))*((cj3)*(cj3))))+(((IkReal(0.0120000000000000))*(sj3)))));
2789 dummyeval[0]=((IkReal(16.8789062500000))+((cj3)*(cj3))+(((IkReal(1.87500000000000))*(sj3)))+((sj3)*(sj3))+(((IkReal(8.00000000000000))*(cj3))));
2790 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2791 {
2792 {
2793 IkReal dummyeval[1];
2794 IkReal gconst7;
2795 gconst7=IKsign(((IkReal(-0.108025000000000))+(((IkReal(-0.0512000000000000))*(cj3)))+(((IkReal(-0.00640000000000000))*((sj3)*(sj3))))+(((IkReal(-0.0120000000000000))*(sj3)))+(((IkReal(-0.00640000000000000))*((cj3)*(cj3))))));
2796 dummyeval[0]=((IkReal(-16.8789062500000))+(((IkReal(-1.87500000000000))*(sj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-8.00000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
2797 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2798 {
2799 continue;
2800 
2801 } else
2802 {
2803 {
2804 IkReal j2array[1], cj2array[1], sj2array[1];
2805 bool j2valid[1]={false};
2806 _nj2 = 1;
2807 IkReal x960=((r02)*(sj0));
2808 IkReal x961=((IkReal(0.0800000000000000))*(sj1));
2809 IkReal x962=((cj3)*(px));
2810 IkReal x963=((cj0)*(r01));
2811 IkReal x964=((IkReal(0.0240000000000000))*(cj1));
2812 IkReal x965=((pz)*(sj3));
2813 IkReal x966=((IkReal(0.00600000000000000))*(cj1));
2814 IkReal x967=((IkReal(0.0750000000000000))*(sj1));
2815 IkReal x968=((py)*(sj0));
2816 IkReal x969=((IkReal(0.00562500000000000))*(cj1));
2817 IkReal x970=((r00)*(sj0));
2818 IkReal x971=((cj3)*(pz));
2819 IkReal x972=((IkReal(0.320000000000000))*(sj1));
2820 IkReal x973=((IkReal(0.0800000000000000))*(cj1));
2821 IkReal x974=((IkReal(0.0750000000000000))*(cj1));
2822 IkReal x975=((py)*(r00));
2823 IkReal x976=((IkReal(0.320000000000000))*(cj1));
2824 IkReal x977=((cj0)*(px));
2825 IkReal x978=((IkReal(0.00600000000000000))*(sj1));
2826 IkReal x979=((cj1)*(px)*(r01));
2827 IkReal x980=((cj0)*(py)*(r02));
2828 IkReal x981=((px)*(sj3)*(x961));
2829 if( IKabs(((gconst7)*(((IkReal(0.0960000000000000))+(((IkReal(-1.00000000000000))*(x971)*(x973)))+(((IkReal(-1.00000000000000))*(sj3)*(x961)*(x980)))+(((IkReal(-1.00000000000000))*(x961)*(x965)*(x970)))+(((cj3)*(x978)))+(((px)*(x960)*(x967)))+(((sj3)*(x973)*(x975)))+(((x960)*(x981)))+(((IkReal(-1.00000000000000))*(x969)*(x970)))+(((sj3)*(x963)*(x966)))+(((IkReal(-1.00000000000000))*(cj3)*(x961)*(x968)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x974)))+(((IkReal(-1.00000000000000))*(pz)*(x967)*(x970)))+(((IkReal(-1.00000000000000))*(x972)*(x977)))+(((IkReal(-1.00000000000000))*(cj0)*(x961)*(x962)))+(((IkReal(-1.00000000000000))*(sj3)*(x966)*(x970)))+(((IkReal(-1.00000000000000))*(pz)*(x976)))+(((IkReal(-1.00000000000000))*(x968)*(x972)))+(((IkReal(0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x967)*(x980)))+(((pz)*(x963)*(x967)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(sj3)*(x973)))+(((x961)*(x963)*(x965)))+(((x974)*(x975)))+(((IkReal(0.0240000000000000))*(cj3)))+(((x963)*(x969))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((IkReal(0.0225000000000000))+(((x961)*(x970)*(x971)))+(((IkReal(-1.00000000000000))*(pz)*(x963)*(x972)))+(((cj3)*(x966)*(x970)))+(((IkReal(0.00562500000000000))*(sj1)))+(((pz)*(x970)*(x972)))+(((IkReal(-1.00000000000000))*(px)*(x960)*(x972)))+(((IkReal(-1.00000000000000))*(x967)*(x977)))+(((IkReal(-1.00000000000000))*(sj3)*(x961)*(x977)))+(((IkReal(-1.00000000000000))*(pz)*(x974)))+(((x972)*(x980)))+(((IkReal(-1.00000000000000))*(x967)*(x968)))+(((IkReal(-1.00000000000000))*(x960)*(x961)*(x962)))+(((sj3)*(x978)))+(((IkReal(-1.00000000000000))*(x975)*(x976)))+(((IkReal(-1.00000000000000))*(sj3)*(x961)*(x968)))+(((x964)*(x970)))+(((IkReal(-1.00000000000000))*(x963)*(x964)))+(((cj3)*(x961)*(x980)))+(((IkReal(-1.00000000000000))*(x965)*(x973)))+(((IkReal(-1.00000000000000))*(x961)*(x963)*(x971)))+(((px)*(r01)*(x976)))+(((IkReal(-1.00000000000000))*(cj3)*(x963)*(x966)))+(((IkReal(0.0240000000000000))*(sj3)))+(((r01)*(x962)*(x973)))+(((IkReal(-1.00000000000000))*(cj3)*(x973)*(x975))))))) < IKFAST_ATAN2_MAGTHRESH )
2830  continue;
2831 j2array[0]=IKatan2(((gconst7)*(((IkReal(0.0960000000000000))+(((IkReal(-1.00000000000000))*(x971)*(x973)))+(((IkReal(-1.00000000000000))*(sj3)*(x961)*(x980)))+(((IkReal(-1.00000000000000))*(x961)*(x965)*(x970)))+(((cj3)*(x978)))+(((px)*(x960)*(x967)))+(((sj3)*(x973)*(x975)))+(((x960)*(x981)))+(((IkReal(-1.00000000000000))*(x969)*(x970)))+(((sj3)*(x963)*(x966)))+(((IkReal(-1.00000000000000))*(cj3)*(x961)*(x968)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(x974)))+(((IkReal(-1.00000000000000))*(pz)*(x967)*(x970)))+(((IkReal(-1.00000000000000))*(x972)*(x977)))+(((IkReal(-1.00000000000000))*(cj0)*(x961)*(x962)))+(((IkReal(-1.00000000000000))*(sj3)*(x966)*(x970)))+(((IkReal(-1.00000000000000))*(pz)*(x976)))+(((IkReal(-1.00000000000000))*(x968)*(x972)))+(((IkReal(0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(x967)*(x980)))+(((pz)*(x963)*(x967)))+(((IkReal(-1.00000000000000))*(px)*(r01)*(sj3)*(x973)))+(((x961)*(x963)*(x965)))+(((x974)*(x975)))+(((IkReal(0.0240000000000000))*(cj3)))+(((x963)*(x969)))))), ((gconst7)*(((IkReal(0.0225000000000000))+(((x961)*(x970)*(x971)))+(((IkReal(-1.00000000000000))*(pz)*(x963)*(x972)))+(((cj3)*(x966)*(x970)))+(((IkReal(0.00562500000000000))*(sj1)))+(((pz)*(x970)*(x972)))+(((IkReal(-1.00000000000000))*(px)*(x960)*(x972)))+(((IkReal(-1.00000000000000))*(x967)*(x977)))+(((IkReal(-1.00000000000000))*(sj3)*(x961)*(x977)))+(((IkReal(-1.00000000000000))*(pz)*(x974)))+(((x972)*(x980)))+(((IkReal(-1.00000000000000))*(x967)*(x968)))+(((IkReal(-1.00000000000000))*(x960)*(x961)*(x962)))+(((sj3)*(x978)))+(((IkReal(-1.00000000000000))*(x975)*(x976)))+(((IkReal(-1.00000000000000))*(sj3)*(x961)*(x968)))+(((x964)*(x970)))+(((IkReal(-1.00000000000000))*(x963)*(x964)))+(((cj3)*(x961)*(x980)))+(((IkReal(-1.00000000000000))*(x965)*(x973)))+(((IkReal(-1.00000000000000))*(x961)*(x963)*(x971)))+(((px)*(r01)*(x976)))+(((IkReal(-1.00000000000000))*(cj3)*(x963)*(x966)))+(((IkReal(0.0240000000000000))*(sj3)))+(((r01)*(x962)*(x973)))+(((IkReal(-1.00000000000000))*(cj3)*(x973)*(x975)))))));
2832 sj2array[0]=IKsin(j2array[0]);
2833 cj2array[0]=IKcos(j2array[0]);
2834 if( j2array[0] > IKPI )
2835 {
2836  j2array[0]-=IK2PI;
2837 }
2838 else if( j2array[0] < -IKPI )
2839 { j2array[0]+=IK2PI;
2840 }
2841 j2valid[0] = true;
2842 for(int ij2 = 0; ij2 < 1; ++ij2)
2843 {
2844 if( !j2valid[ij2] )
2845 {
2846  continue;
2847 }
2848 _ij2[0] = ij2; _ij2[1] = -1;
2849 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2850 {
2851 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2852 {
2853  j2valid[iij2]=false; _ij2[1] = iij2; break;
2854 }
2855 }
2856 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2857 {
2858 IkReal evalcond[4];
2859 IkReal x982=IKcos(j2);
2860 IkReal x983=IKsin(j2);
2861 IkReal x984=((IkReal(0.0800000000000000))*(sj3));
2862 IkReal x985=((cj0)*(r01));
2863 IkReal x986=((IkReal(1.00000000000000))*(px));
2864 IkReal x987=((py)*(sj1));
2865 IkReal x988=((cj0)*(r02));
2866 IkReal x989=((IkReal(0.0750000000000000))*(cj1));
2867 IkReal x990=((r02)*(sj0));
2868 IkReal x991=((IkReal(0.0750000000000000))*(sj1));
2869 IkReal x992=((cj1)*(pz));
2870 IkReal x993=((r00)*(sj0));
2871 IkReal x994=((IkReal(0.0800000000000000))*(cj3));
2872 IkReal x995=((pz)*(sj1));
2873 IkReal x996=((IkReal(1.00000000000000))*(sj0));
2874 IkReal x997=((cj1)*(py));
2875 IkReal x998=((IkReal(0.320000000000000))*(x982));
2876 IkReal x999=((IkReal(0.0750000000000000))*(x983));
2877 IkReal x1000=((IkReal(0.320000000000000))*(x983));
2878 IkReal x1001=((IkReal(0.0750000000000000))*(x982));
2879 IkReal x1002=((sj1)*(x993));
2880 IkReal x1003=((x982)*(x994));
2881 IkReal x1004=((x983)*(x984));
2882 IkReal x1005=((x982)*(x984));
2883 IkReal x1006=((x983)*(x994));
2884 IkReal x1007=((x1004)+(x999));
2885 IkReal x1008=((x1003)+(x998));
2886 IkReal x1009=((x1006)+(x1005)+(x1001)+(x1000));
2887 evalcond[0]=((((IkReal(-1.00000000000000))*(x996)*(x997)))+(x989)+(((IkReal(-1.00000000000000))*(x1007)))+(x1008)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x986)))+(x995));
2888 evalcond[1]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x992)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x986)))+(x1009)+(((IkReal(-1.00000000000000))*(x987)*(x996)))+(x991));
2889 evalcond[2]=((((x993)*(x995)))+(((IkReal(-1.00000000000000))*(sj1)*(x986)*(x990)))+(((x987)*(x988)))+(((IkReal(-1.00000000000000))*(x985)*(x989)))+(((IkReal(-1.00000000000000))*(x1007)))+(((cj1)*(px)*(r01)))+(x1008)+(((x989)*(x993)))+(((IkReal(-1.00000000000000))*(x985)*(x995)))+(((IkReal(-1.00000000000000))*(r00)*(x997))));
2890 evalcond[3]=((((IkReal(0.300000000000000))*(x985)))+(((IkReal(-1.00000000000000))*(x1009)))+(((x985)*(x991)))+(((IkReal(-0.300000000000000))*(x993)))+(((x988)*(x997)))+(((IkReal(-1.00000000000000))*(x991)*(x993)))+(((x992)*(x993)))+(((IkReal(-1.00000000000000))*(x985)*(x992)))+(((IkReal(-1.00000000000000))*(cj1)*(x986)*(x990)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x986)))+(((r00)*(x987))));
2891 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2892 {
2893 continue;
2894 }
2895 }
2896 
2897 {
2898 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
2899 vinfos[0].jointtype = 1;
2900 vinfos[0].foffset = j0;
2901 vinfos[0].indices[0] = _ij0[0];
2902 vinfos[0].indices[1] = _ij0[1];
2903 vinfos[0].maxsolutions = _nj0;
2904 vinfos[1].jointtype = 1;
2905 vinfos[1].foffset = j1;
2906 vinfos[1].indices[0] = _ij1[0];
2907 vinfos[1].indices[1] = _ij1[1];
2908 vinfos[1].maxsolutions = _nj1;
2909 vinfos[2].jointtype = 1;
2910 vinfos[2].foffset = j2;
2911 vinfos[2].indices[0] = _ij2[0];
2912 vinfos[2].indices[1] = _ij2[1];
2913 vinfos[2].maxsolutions = _nj2;
2914 vinfos[3].jointtype = 1;
2915 vinfos[3].foffset = j3;
2916 vinfos[3].indices[0] = _ij3[0];
2917 vinfos[3].indices[1] = _ij3[1];
2918 vinfos[3].maxsolutions = _nj3;
2919 vinfos[4].jointtype = 1;
2920 vinfos[4].foffset = j4;
2921 vinfos[4].indices[0] = _ij4[0];
2922 vinfos[4].indices[1] = _ij4[1];
2923 vinfos[4].maxsolutions = _nj4;
2924 std::vector<int> vfree(0);
2925 solutions.AddSolution(vinfos,vfree);
2926 }
2927 }
2928 }
2929 
2930 }
2931 
2932 }
2933 
2934 } else
2935 {
2936 {
2937 IkReal j2array[1], cj2array[1], sj2array[1];
2938 bool j2valid[1]={false};
2939 _nj2 = 1;
2940 IkReal x1010=((IkReal(0.0800000000000000))*(cj1));
2941 IkReal x1011=((cj0)*(px));
2942 IkReal x1012=((py)*(sj0));
2943 IkReal x1013=((IkReal(0.320000000000000))*(sj1));
2944 IkReal x1014=((IkReal(0.00600000000000000))*(cj3));
2945 IkReal x1015=((pz)*(sj3));
2946 IkReal x1016=((IkReal(0.0750000000000000))*(cj1));
2947 IkReal x1017=((IkReal(0.0800000000000000))*(sj1));
2948 IkReal x1018=((IkReal(0.0750000000000000))*(sj1));
2949 IkReal x1019=((IkReal(0.00600000000000000))*(sj3));
2950 IkReal x1020=((cj3)*(pz));
2951 IkReal x1021=((IkReal(0.320000000000000))*(cj1));
2952 if( IKabs(((gconst6)*(((IkReal(-0.0960000000000000))+(((IkReal(-1.00000000000000))*(sj3)*(x1010)*(x1012)))+(((x1010)*(x1020)))+(((IkReal(-1.00000000000000))*(x1012)*(x1016)))+(((x1012)*(x1013)))+(((IkReal(-1.00000000000000))*(x1011)*(x1016)))+(((pz)*(x1018)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(sj3)*(x1010)*(x1011)))+(((cj3)*(x1012)*(x1017)))+(((cj3)*(x1011)*(x1017)))+(((pz)*(x1021)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(sj1)*(x1014)))+(((x1011)*(x1013)))+(((cj1)*(x1019)))+(((x1015)*(x1017))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(-0.0225000000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x1019)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((x1011)*(x1018)))+(((x1012)*(x1021)))+(((sj3)*(x1012)*(x1017)))+(((IkReal(-1.00000000000000))*(cj1)*(x1014)))+(((sj3)*(x1011)*(x1017)))+(((IkReal(-1.00000000000000))*(x1017)*(x1020)))+(((x1011)*(x1021)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(pz)*(x1013)))+(((x1012)*(x1018)))+(((x1010)*(x1015)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((cj3)*(x1010)*(x1012)))+(((cj3)*(x1010)*(x1011)))+(((pz)*(x1016))))))) < IKFAST_ATAN2_MAGTHRESH )
2953  continue;
2954 j2array[0]=IKatan2(((gconst6)*(((IkReal(-0.0960000000000000))+(((IkReal(-1.00000000000000))*(sj3)*(x1010)*(x1012)))+(((x1010)*(x1020)))+(((IkReal(-1.00000000000000))*(x1012)*(x1016)))+(((x1012)*(x1013)))+(((IkReal(-1.00000000000000))*(x1011)*(x1016)))+(((pz)*(x1018)))+(((IkReal(-0.0240000000000000))*(cj3)))+(((IkReal(-0.0240000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(sj3)*(x1010)*(x1011)))+(((cj3)*(x1012)*(x1017)))+(((cj3)*(x1011)*(x1017)))+(((pz)*(x1021)))+(((IkReal(0.00562500000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(sj1)*(x1014)))+(((x1011)*(x1013)))+(((cj1)*(x1019)))+(((x1015)*(x1017)))))), ((gconst6)*(((IkReal(-0.0225000000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x1019)))+(((IkReal(-0.0240000000000000))*(sj3)))+(((x1011)*(x1018)))+(((x1012)*(x1021)))+(((sj3)*(x1012)*(x1017)))+(((IkReal(-1.00000000000000))*(cj1)*(x1014)))+(((sj3)*(x1011)*(x1017)))+(((IkReal(-1.00000000000000))*(x1017)*(x1020)))+(((x1011)*(x1021)))+(((IkReal(-0.0240000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(pz)*(x1013)))+(((x1012)*(x1018)))+(((x1010)*(x1015)))+(((IkReal(-0.00562500000000000))*(sj1)))+(((cj3)*(x1010)*(x1012)))+(((cj3)*(x1010)*(x1011)))+(((pz)*(x1016)))))));
2955 sj2array[0]=IKsin(j2array[0]);
2956 cj2array[0]=IKcos(j2array[0]);
2957 if( j2array[0] > IKPI )
2958 {
2959  j2array[0]-=IK2PI;
2960 }
2961 else if( j2array[0] < -IKPI )
2962 { j2array[0]+=IK2PI;
2963 }
2964 j2valid[0] = true;
2965 for(int ij2 = 0; ij2 < 1; ++ij2)
2966 {
2967 if( !j2valid[ij2] )
2968 {
2969  continue;
2970 }
2971 _ij2[0] = ij2; _ij2[1] = -1;
2972 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
2973 {
2974 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
2975 {
2976  j2valid[iij2]=false; _ij2[1] = iij2; break;
2977 }
2978 }
2979 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
2980 {
2981 IkReal evalcond[4];
2982 IkReal x1022=IKcos(j2);
2983 IkReal x1023=IKsin(j2);
2984 IkReal x1024=((IkReal(0.0800000000000000))*(sj3));
2985 IkReal x1025=((cj0)*(r01));
2986 IkReal x1026=((IkReal(1.00000000000000))*(px));
2987 IkReal x1027=((py)*(sj1));
2988 IkReal x1028=((cj0)*(r02));
2989 IkReal x1029=((IkReal(0.0750000000000000))*(cj1));
2990 IkReal x1030=((r02)*(sj0));
2991 IkReal x1031=((IkReal(0.0750000000000000))*(sj1));
2992 IkReal x1032=((cj1)*(pz));
2993 IkReal x1033=((r00)*(sj0));
2994 IkReal x1034=((IkReal(0.0800000000000000))*(cj3));
2995 IkReal x1035=((pz)*(sj1));
2996 IkReal x1036=((IkReal(1.00000000000000))*(sj0));
2997 IkReal x1037=((cj1)*(py));
2998 IkReal x1038=((IkReal(0.320000000000000))*(x1022));
2999 IkReal x1039=((IkReal(0.0750000000000000))*(x1023));
3000 IkReal x1040=((IkReal(0.320000000000000))*(x1023));
3001 IkReal x1041=((IkReal(0.0750000000000000))*(x1022));
3002 IkReal x1042=((sj1)*(x1033));
3003 IkReal x1043=((x1022)*(x1034));
3004 IkReal x1044=((x1023)*(x1024));
3005 IkReal x1045=((x1022)*(x1024));
3006 IkReal x1046=((x1023)*(x1034));
3007 IkReal x1047=((x1044)+(x1039));
3008 IkReal x1048=((x1043)+(x1038));
3009 IkReal x1049=((x1041)+(x1040)+(x1046)+(x1045));
3010 evalcond[0]=((((IkReal(-1.00000000000000))*(x1036)*(x1037)))+(x1048)+(((IkReal(-1.00000000000000))*(cj0)*(cj1)*(x1026)))+(x1035)+(((IkReal(-1.00000000000000))*(x1047)))+(x1029));
3011 evalcond[1]=((IkReal(0.300000000000000))+(x1049)+(x1031)+(((IkReal(-1.00000000000000))*(x1032)))+(((IkReal(-1.00000000000000))*(cj0)*(sj1)*(x1026)))+(((IkReal(-1.00000000000000))*(x1027)*(x1036))));
3012 evalcond[2]=((x1048)+(((x1033)*(x1035)))+(((x1029)*(x1033)))+(((IkReal(-1.00000000000000))*(x1047)))+(((IkReal(-1.00000000000000))*(sj1)*(x1026)*(x1030)))+(((x1027)*(x1028)))+(((cj1)*(px)*(r01)))+(((IkReal(-1.00000000000000))*(x1025)*(x1029)))+(((IkReal(-1.00000000000000))*(r00)*(x1037)))+(((IkReal(-1.00000000000000))*(x1025)*(x1035))));
3013 evalcond[3]=((((IkReal(-0.300000000000000))*(x1033)))+(((x1032)*(x1033)))+(((IkReal(-1.00000000000000))*(cj1)*(x1026)*(x1030)))+(((IkReal(-1.00000000000000))*(x1025)*(x1032)))+(((IkReal(0.300000000000000))*(x1025)))+(((x1025)*(x1031)))+(((r00)*(x1027)))+(((x1028)*(x1037)))+(((IkReal(-1.00000000000000))*(x1049)))+(((IkReal(-1.00000000000000))*(r01)*(sj1)*(x1026)))+(((IkReal(-1.00000000000000))*(x1031)*(x1033))));
3014 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
3015 {
3016 continue;
3017 }
3018 }
3019 
3020 {
3021 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
3022 vinfos[0].jointtype = 1;
3023 vinfos[0].foffset = j0;
3024 vinfos[0].indices[0] = _ij0[0];
3025 vinfos[0].indices[1] = _ij0[1];
3026 vinfos[0].maxsolutions = _nj0;
3027 vinfos[1].jointtype = 1;
3028 vinfos[1].foffset = j1;
3029 vinfos[1].indices[0] = _ij1[0];
3030 vinfos[1].indices[1] = _ij1[1];
3031 vinfos[1].maxsolutions = _nj1;
3032 vinfos[2].jointtype = 1;
3033 vinfos[2].foffset = j2;
3034 vinfos[2].indices[0] = _ij2[0];
3035 vinfos[2].indices[1] = _ij2[1];
3036 vinfos[2].maxsolutions = _nj2;
3037 vinfos[3].jointtype = 1;
3038 vinfos[3].foffset = j3;
3039 vinfos[3].indices[0] = _ij3[0];
3040 vinfos[3].indices[1] = _ij3[1];
3041 vinfos[3].maxsolutions = _nj3;
3042 vinfos[4].jointtype = 1;
3043 vinfos[4].foffset = j4;
3044 vinfos[4].indices[0] = _ij4[0];
3045 vinfos[4].indices[1] = _ij4[1];
3046 vinfos[4].maxsolutions = _nj4;
3047 std::vector<int> vfree(0);
3048 solutions.AddSolution(vinfos,vfree);
3049 }
3050 }
3051 }
3052 
3053 }
3054 
3055 }
3056 
3057 } else
3058 {
3059 if( 1 )
3060 {
3061 continue;
3062 
3063 } else
3064 {
3065 }
3066 }
3067 }
3068 }
3069 
3070 } else
3071 {
3072 {
3073 IkReal j2array[1], cj2array[1], sj2array[1];
3074 bool j2valid[1]={false};
3075 _nj2 = 1;
3076 IkReal x1050=((IkReal(0.0800000000000000))*(cj3));
3077 IkReal x1051=((IkReal(0.0750000000000000))*(cj1));
3078 IkReal x1052=((r01)*(sj0));
3079 IkReal x1053=((r02)*(sj1));
3080 IkReal x1054=((IkReal(0.0800000000000000))*(sj3));
3081 IkReal x1055=((cj3)*(cj4));
3082 IkReal x1056=((cj4)*(sj3));
3083 IkReal x1057=((pz)*(sj1));
3084 IkReal x1058=((cj0)*(cj1)*(r00));
3085 IkReal x1059=((cj0)*(cj1)*(px));
3086 IkReal x1060=((cj1)*(py)*(sj0));
3087 if( IKabs(((gconst3)*(((((IkReal(-0.320000000000000))*(x1058)))+(((IkReal(-1.00000000000000))*(cj1)*(x1050)*(x1052)))+(((IkReal(-0.320000000000000))*(cj1)*(x1052)))+(((IkReal(-1.00000000000000))*(x1056)*(x1059)))+(((x1056)*(x1057)))+(((x1050)*(x1053)))+(((IkReal(-1.00000000000000))*(x1050)*(x1058)))+(((IkReal(-1.00000000000000))*(x1056)*(x1060)))+(((IkReal(0.320000000000000))*(x1053)))+(((x1051)*(x1056))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(x1051)*(x1055)))+(((x1055)*(x1060)))+(((x1055)*(x1059)))+(((IkReal(-1.00000000000000))*(x1054)*(x1058)))+(((IkReal(-1.00000000000000))*(x1051)*(x1052)))+(((IkReal(0.0750000000000000))*(x1053)))+(((IkReal(-1.00000000000000))*(cj1)*(x1052)*(x1054)))+(((x1053)*(x1054)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x1051)))+(((IkReal(-1.00000000000000))*(x1055)*(x1057))))))) < IKFAST_ATAN2_MAGTHRESH )
3088  continue;
3089 j2array[0]=IKatan2(((gconst3)*(((((IkReal(-0.320000000000000))*(x1058)))+(((IkReal(-1.00000000000000))*(cj1)*(x1050)*(x1052)))+(((IkReal(-0.320000000000000))*(cj1)*(x1052)))+(((IkReal(-1.00000000000000))*(x1056)*(x1059)))+(((x1056)*(x1057)))+(((x1050)*(x1053)))+(((IkReal(-1.00000000000000))*(x1050)*(x1058)))+(((IkReal(-1.00000000000000))*(x1056)*(x1060)))+(((IkReal(0.320000000000000))*(x1053)))+(((x1051)*(x1056)))))), ((gconst3)*(((((IkReal(-1.00000000000000))*(x1051)*(x1055)))+(((x1055)*(x1060)))+(((x1055)*(x1059)))+(((IkReal(-1.00000000000000))*(x1054)*(x1058)))+(((IkReal(-1.00000000000000))*(x1051)*(x1052)))+(((IkReal(0.0750000000000000))*(x1053)))+(((IkReal(-1.00000000000000))*(cj1)*(x1052)*(x1054)))+(((x1053)*(x1054)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x1051)))+(((IkReal(-1.00000000000000))*(x1055)*(x1057)))))));
3090 sj2array[0]=IKsin(j2array[0]);
3091 cj2array[0]=IKcos(j2array[0]);
3092 if( j2array[0] > IKPI )
3093 {
3094  j2array[0]-=IK2PI;
3095 }
3096 else if( j2array[0] < -IKPI )
3097 { j2array[0]+=IK2PI;
3098 }
3099 j2valid[0] = true;
3100 for(int ij2 = 0; ij2 < 1; ++ij2)
3101 {
3102 if( !j2valid[ij2] )
3103 {
3104  continue;
3105 }
3106 _ij2[0] = ij2; _ij2[1] = -1;
3107 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
3108 {
3109 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
3110 {
3111  j2valid[iij2]=false; _ij2[1] = iij2; break;
3112 }
3113 }
3114 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
3115 {
3116 IkReal evalcond[8];
3117 IkReal x1061=IKcos(j2);
3118 IkReal x1062=IKsin(j2);
3119 IkReal x1063=(py)*(py);
3120 IkReal x1064=(px)*(px);
3121 IkReal x1065=(pz)*(pz);
3122 IkReal x1066=((sj0)*(sj1));
3123 IkReal x1067=((IkReal(1.00000000000000))*(r01));
3124 IkReal x1068=((r00)*(sj1));
3125 IkReal x1069=((IkReal(0.0480000000000000))*(sj3));
3126 IkReal x1070=((IkReal(0.0750000000000000))*(r00));
3127 IkReal x1071=((IkReal(0.0800000000000000))*(sj3));
3128 IkReal x1072=((cj1)*(r01));
3129 IkReal x1073=((IkReal(0.150000000000000))*(py));
3130 IkReal x1074=((pz)*(r02));
3131 IkReal x1075=((px)*(r02));
3132 IkReal x1076=((IkReal(0.0750000000000000))*(sj4));
3133 IkReal x1077=((cj1)*(sj0));
3134 IkReal x1078=((pz)*(r00));
3135 IkReal x1079=((IkReal(0.0750000000000000))*(cj0));
3136 IkReal x1080=((IkReal(2.00000000000000))*(cj0));
3137 IkReal x1081=((cj1)*(r02));
3138 IkReal x1082=((IkReal(0.150000000000000))*(sj1));
3139 IkReal x1083=((px)*(r00));
3140 IkReal x1084=((IkReal(2.00000000000000))*(py));
3141 IkReal x1085=((IkReal(0.0903750000000000))*(sj3));
3142 IkReal x1086=((r02)*(sj1));
3143 IkReal x1087=((IkReal(0.150000000000000))*(cj1));
3144 IkReal x1088=((cj0)*(py));
3145 IkReal x1089=((IkReal(0.0480000000000000))*(cj3));
3146 IkReal x1090=((r01)*(sj1));
3147 IkReal x1091=((IkReal(0.0800000000000000))*(cj3));
3148 IkReal x1092=((IkReal(0.150000000000000))*(pz));
3149 IkReal x1093=((IkReal(1.00000000000000))*(cj1));
3150 IkReal x1094=((cj0)*(r00));
3151 IkReal x1095=((IkReal(0.600000000000000))*(py));
3152 IkReal x1096=((IkReal(1.00000000000000))*(py));
3153 IkReal x1097=((px)*(sj1));
3154 IkReal x1098=((IkReal(1.00000000000000))*(cj0));
3155 IkReal x1099=((cj0)*(pz));
3156 IkReal x1100=((IkReal(0.103175000000000))*(cj3));
3157 IkReal x1101=((IkReal(0.600000000000000))*(cj0));
3158 IkReal x1102=((IkReal(1.00000000000000))*(sj3));
3159 IkReal x1103=((px)*(py));
3160 IkReal x1104=((r01)*(sj0));
3161 IkReal x1105=((IkReal(2.00000000000000))*(px));
3162 IkReal x1106=((cj4)*(x1062));
3163 IkReal x1107=((cj4)*(x1061));
3164 IkReal x1108=((sj4)*(x1062));
3165 IkReal x1109=((IkReal(2.00000000000000))*(x1065));
3166 IkReal x1110=((sj4)*(x1061));
3167 IkReal x1111=((IkReal(2.00000000000000))*(x1063));
3168 IkReal x1112=((px)*(x1074)*(x1080));
3169 evalcond[0]=((((IkReal(-1.00000000000000))*(x1067)*(x1077)))+(x1086)+(((IkReal(-1.00000000000000))*(x1093)*(x1094)))+(((IkReal(-1.00000000000000))*(cj3)*(x1106)))+(((IkReal(-1.00000000000000))*(x1102)*(x1107))));
3170 evalcond[1]=((((IkReal(-1.00000000000000))*(x1102)*(x1106)))+(((IkReal(-1.00000000000000))*(x1081)))+(((cj3)*(x1107)))+(((IkReal(-1.00000000000000))*(x1068)*(x1098)))+(((IkReal(-1.00000000000000))*(x1066)*(x1067))));
3171 evalcond[2]=((((IkReal(-1.00000000000000))*(cj0)*(px)*(x1093)))+(((IkReal(0.320000000000000))*(x1061)))+(((IkReal(-1.00000000000000))*(x1062)*(x1071)))+(((pz)*(sj1)))+(((IkReal(-0.0750000000000000))*(x1062)))+(((x1061)*(x1091)))+(((IkReal(-1.00000000000000))*(x1077)*(x1096)))+(((IkReal(0.0750000000000000))*(cj1))));
3172 evalcond[3]=((IkReal(0.300000000000000))+(((IkReal(-1.00000000000000))*(x1097)*(x1098)))+(((x1062)*(x1091)))+(((IkReal(0.320000000000000))*(x1062)))+(((IkReal(0.0750000000000000))*(x1061)))+(((x1061)*(x1071)))+(((IkReal(-1.00000000000000))*(x1066)*(x1096)))+(((IkReal(0.0750000000000000))*(sj1)))+(((IkReal(-1.00000000000000))*(pz)*(x1093))));
3173 evalcond[4]=((((IkReal(-1.00000000000000))*(py)*(r00)*(x1093)))+(((IkReal(-1.00000000000000))*(x1066)*(x1075)))+(((IkReal(-1.00000000000000))*(sj1)*(x1067)*(x1099)))+(((IkReal(-0.320000000000000))*(x1110)))+(((x1066)*(x1078)))+(((px)*(x1072)))+(((x1086)*(x1088)))+(((x1071)*(x1108)))+(((x1062)*(x1076)))+(((IkReal(-1.00000000000000))*(x1091)*(x1110)))+(((IkReal(-1.00000000000000))*(x1072)*(x1079)))+(((x1070)*(x1077))));
3174 evalcond[5]=((((x1061)*(x1076)))+(((IkReal(0.320000000000000))*(x1108)))+(((py)*(x1068)))+(((IkReal(-1.00000000000000))*(x1067)*(x1097)))+(((IkReal(-0.300000000000000))*(r00)*(sj0)))+(((x1077)*(x1078)))+(((x1081)*(x1088)))+(((IkReal(-1.00000000000000))*(x1066)*(x1070)))+(((x1071)*(x1110)))+(((IkReal(0.300000000000000))*(cj0)*(r01)))+(((IkReal(-1.00000000000000))*(x1075)*(x1077)))+(((IkReal(-1.00000000000000))*(cj1)*(x1067)*(x1099)))+(((x1091)*(x1108)))+(((x1079)*(x1090))));
3175 evalcond[6]=((((x1069)*(x1107)))+(((cj0)*(x1075)*(x1087)))+(((IkReal(-1.00000000000000))*(pp)*(x1081)))+(((IkReal(-0.150000000000000))*(px)*(x1068)))+(((x1085)*(x1106)))+(((IkReal(-0.600000000000000))*(x1083)))+(((IkReal(0.0450000000000000))*(x1104)))+(((IkReal(0.0956250000000000))*(r01)*(x1066)))+(((x1081)*(x1109)))+(((IkReal(-1.00000000000000))*(x1074)*(x1082)))+(((cj1)*(x1078)*(x1105)))+(((x1066)*(x1074)*(x1084)))+(((IkReal(-1.00000000000000))*(sj0)*(x1072)*(x1092)))+(((IkReal(-1.00000000000000))*(r01)*(x1095)))+(((r01)*(x1066)*(x1111)))+(((IkReal(-0.600000000000000))*(x1074)))+(((x1066)*(x1083)*(x1084)))+(((x1100)*(x1107)))+(((IkReal(0.0843750000000000))*(x1081)))+(((IkReal(-1.00000000000000))*(x1089)*(x1106)))+(((x1064)*(x1068)*(x1080)))+(((r02)*(x1073)*(x1077)))+(((IkReal(-1.00000000000000))*(x1073)*(x1090)))+(((IkReal(0.0956250000000000))*(cj0)*(x1068)))+(((IkReal(0.0512000000000000))*(x1107)))+(((IkReal(-1.00000000000000))*(cj0)*(x1078)*(x1087)))+(((IkReal(-0.0120000000000000))*(x1106)))+(((IkReal(-1.00000000000000))*(pp)*(x1066)*(x1067)))+(((IkReal(-1.00000000000000))*(pp)*(x1068)*(x1098)))+(((x1074)*(x1080)*(x1097)))+(((IkReal(0.0450000000000000))*(x1094)))+(((pz)*(x1072)*(x1084)))+(((x1080)*(x1090)*(x1103))));
3176 evalcond[7]=((((IkReal(-0.0512000000000000))*(x1106)))+(((IkReal(-1.00000000000000))*(x1089)*(x1107)))+(((IkReal(0.0450000000000000))*(r02)))+(((x1078)*(x1101)))+(((IkReal(-1.00000000000000))*(pz)*(x1068)*(x1105)))+(((IkReal(0.600000000000000))*(pz)*(x1104)))+(((cj0)*(x1068)*(x1092)))+(((IkReal(-1.00000000000000))*(r02)*(x1066)*(x1073)))+(((x1072)*(x1080)*(x1103)))+(((IkReal(-0.0843750000000000))*(sj0)*(x1072)))+(((IkReal(-1.00000000000000))*(pp)*(x1067)*(x1077)))+(((x1085)*(x1107)))+(((IkReal(-1.00000000000000))*(pp)*(x1093)*(x1094)))+(((IkReal(-1.00000000000000))*(x1075)*(x1101)))+(((cj1)*(r00)*(x1064)*(x1080)))+(((IkReal(-1.00000000000000))*(x1074)*(x1087)))+(((pp)*(x1086)))+(((IkReal(-0.0120000000000000))*(x1107)))+(((IkReal(-1.00000000000000))*(x1083)*(x1087)))+(((IkReal(-1.00000000000000))*(cj0)*(x1075)*(x1082)))+(((x1074)*(x1077)*(x1084)))+(((IkReal(-0.0843750000000000))*(cj1)*(x1094)))+(((IkReal(-1.00000000000000))*(x1069)*(x1106)))+(((IkReal(-1.00000000000000))*(pz)*(x1084)*(x1090)))+(((sj0)*(x1072)*(x1111)))+(((IkReal(-1.00000000000000))*(x1072)*(x1073)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1095)))+(((IkReal(0.0956250000000000))*(x1086)))+(((IkReal(-1.00000000000000))*(x1086)*(x1109)))+(((cj1)*(x1112)))+(((IkReal(-1.00000000000000))*(x1100)*(x1106)))+(((x1077)*(x1083)*(x1084)))+(((r01)*(x1066)*(x1092))));
3177 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 )
3178 {
3179 continue;
3180 }
3181 }
3182 
3183 {
3184 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
3185 vinfos[0].jointtype = 1;
3186 vinfos[0].foffset = j0;
3187 vinfos[0].indices[0] = _ij0[0];
3188 vinfos[0].indices[1] = _ij0[1];
3189 vinfos[0].maxsolutions = _nj0;
3190 vinfos[1].jointtype = 1;
3191 vinfos[1].foffset = j1;
3192 vinfos[1].indices[0] = _ij1[0];
3193 vinfos[1].indices[1] = _ij1[1];
3194 vinfos[1].maxsolutions = _nj1;
3195 vinfos[2].jointtype = 1;
3196 vinfos[2].foffset = j2;
3197 vinfos[2].indices[0] = _ij2[0];
3198 vinfos[2].indices[1] = _ij2[1];
3199 vinfos[2].maxsolutions = _nj2;
3200 vinfos[3].jointtype = 1;
3201 vinfos[3].foffset = j3;
3202 vinfos[3].indices[0] = _ij3[0];
3203 vinfos[3].indices[1] = _ij3[1];
3204 vinfos[3].maxsolutions = _nj3;
3205 vinfos[4].jointtype = 1;
3206 vinfos[4].foffset = j4;
3207 vinfos[4].indices[0] = _ij4[0];
3208 vinfos[4].indices[1] = _ij4[1];
3209 vinfos[4].maxsolutions = _nj4;
3210 std::vector<int> vfree(0);
3211 solutions.AddSolution(vinfos,vfree);
3212 }
3213 }
3214 }
3215 
3216 }
3217 
3218 }
3219 
3220 } else
3221 {
3222 {
3223 IkReal j2array[1], cj2array[1], sj2array[1];
3224 bool j2valid[1]={false};
3225 _nj2 = 1;
3226 IkReal x1113=((IkReal(1.00000000000000))*(cj1));
3227 IkReal x1114=((cj0)*(r00));
3228 IkReal x1115=((cj3)*(r02));
3229 IkReal x1116=((sj1)*(sj3));
3230 IkReal x1117=((r01)*(sj0));
3231 IkReal x1118=((cj3)*(x1117));
3232 if( IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(cj3)*(x1113)*(x1114)))+(((IkReal(-1.00000000000000))*(x1113)*(x1118)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x1113)))+(((sj1)*(x1115)))+(((IkReal(-1.00000000000000))*(x1116)*(x1117)))+(((IkReal(-1.00000000000000))*(x1114)*(x1116))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((r02)*(x1116)))+(((sj1)*(x1118)))+(((IkReal(-1.00000000000000))*(sj3)*(x1113)*(x1114)))+(((IkReal(-1.00000000000000))*(sj3)*(x1113)*(x1117)))+(((cj1)*(x1115)))+(((cj3)*(sj1)*(x1114))))))) < IKFAST_ATAN2_MAGTHRESH )
3233  continue;
3234 j2array[0]=IKatan2(((gconst2)*(((((IkReal(-1.00000000000000))*(cj3)*(x1113)*(x1114)))+(((IkReal(-1.00000000000000))*(x1113)*(x1118)))+(((IkReal(-1.00000000000000))*(r02)*(sj3)*(x1113)))+(((sj1)*(x1115)))+(((IkReal(-1.00000000000000))*(x1116)*(x1117)))+(((IkReal(-1.00000000000000))*(x1114)*(x1116)))))), ((gconst2)*(((((r02)*(x1116)))+(((sj1)*(x1118)))+(((IkReal(-1.00000000000000))*(sj3)*(x1113)*(x1114)))+(((IkReal(-1.00000000000000))*(sj3)*(x1113)*(x1117)))+(((cj1)*(x1115)))+(((cj3)*(sj1)*(x1114)))))));
3235 sj2array[0]=IKsin(j2array[0]);
3236 cj2array[0]=IKcos(j2array[0]);
3237 if( j2array[0] > IKPI )
3238 {
3239  j2array[0]-=IK2PI;
3240 }
3241 else if( j2array[0] < -IKPI )
3242 { j2array[0]+=IK2PI;
3243 }
3244 j2valid[0] = true;
3245 for(int ij2 = 0; ij2 < 1; ++ij2)
3246 {
3247 if( !j2valid[ij2] )
3248 {
3249  continue;
3250 }
3251 _ij2[0] = ij2; _ij2[1] = -1;
3252 for(int iij2 = ij2+1; iij2 < 1; ++iij2)
3253 {
3254 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
3255 {
3256  j2valid[iij2]=false; _ij2[1] = iij2; break;
3257 }
3258 }
3259 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
3260 {
3261 IkReal evalcond[8];
3262 IkReal x1119=IKcos(j2);
3263 IkReal x1120=IKsin(j2);
3264 IkReal x1121=(py)*(py);
3265 IkReal x1122=(px)*(px);
3266 IkReal x1123=(pz)*(pz);
3267 IkReal x1124=((sj0)*(sj1));
3268 IkReal x1125=((IkReal(1.00000000000000))*(r01));
3269 IkReal x1126=((r00)*(sj1));
3270 IkReal x1127=((IkReal(0.0480000000000000))*(sj3));
3271 IkReal x1128=((IkReal(0.0750000000000000))*(r00));
3272 IkReal x1129=((IkReal(0.0800000000000000))*(sj3));
3273 IkReal x1130=((cj1)*(r01));
3274 IkReal x1131=((IkReal(0.150000000000000))*(py));
3275 IkReal x1132=((pz)*(r02));
3276 IkReal x1133=((px)*(r02));
3277 IkReal x1134=((IkReal(0.0750000000000000))*(sj4));
3278 IkReal x1135=((cj1)*(sj0));
3279 IkReal x1136=((pz)*(r00));
3280 IkReal x1137=((IkReal(0.0750000000000000))*(cj0));
3281 IkReal x1138=((IkReal(2.00000000000000))*(cj0));
3282 IkReal x1139=((cj1)*(r02));
3283 IkReal x1140=((IkReal(0.150000000000000))*(sj1));
3284 IkReal x1141=((px)*(r00));
3285 IkReal x1142=((IkReal(2.00000000000000))*(py));
3286 IkReal x1143=((IkReal(0.0903750000000000))*(sj3));
3287 IkReal x1144=((r02)*(sj1));
3288 IkReal x1145=((IkReal(0.150000000000000))*(cj1));
3289 IkReal x1146=((cj0)*(py));
3290 IkReal x1147=((IkReal(0.0480000000000000))*(cj3));
3291 IkReal x1148=((r01)*(sj1));
3292 IkReal x1149=((IkReal(0.0800000000000000))*(cj3));
3293 IkReal x1150=((IkReal(0.150000000000000))*(pz));
3294 IkReal x1151=((IkReal(1.00000000000000))*(cj1));
3295 IkReal x1152=((cj0)*(r00));
3296 IkReal x1153=((IkReal(0.600000000000000))*(py));
3297 IkReal x1154=((IkReal(1.00000000000000))*(py));
3298 IkReal x1155=((px)*(sj1));
3299 IkReal x1156=((IkReal(1.00000000000000))*(cj0));
3300 IkReal x1157=((cj0)*(pz));
3301 IkReal x1158=((IkReal(0.103175000000000))*(cj3));
3302 IkReal x1159=((IkReal(0.600000000000000))*(cj0));
3303 IkReal x1160=((IkReal(1.00000000000000))*(sj3));
3304 IkReal x1161=((px)*(py));
3305 IkReal x1162=((r01)*(sj0));
3306 IkReal x1163=((IkReal(2.00000000000000))*(px));
3307 IkReal x1164=((cj4)*(x1120));
3308 IkReal x1165=((cj4)*(x1119));
3309 IkReal x1166=((sj4)*(x1120));
3310 IkReal x1167=((IkReal(2.00000000000000))*(x1123));
3311 IkReal x1168=((sj4)*(x1119));
3312 IkReal x1169=((IkReal(2.00000000000000))*(x1121));
3313 IkReal x1170=((px)*(x1132)*(x1138));
3314 evalcond[0]=((x1144)+(((IkReal(-1.00000000000000))*(x1160)*(x1165)))+(((IkReal(-1.00000000000000))*(x1151)*(x1152)))+(((IkReal(-1.00000000000000))*(x1125)*(x1135)))+(((IkReal(-1.00000000000000))*(cj3)*(x1164))));
3315 evalcond[1]=((((IkReal(-1.00000000000000))*(x1160)*(x1164)))+(((IkReal(-1.00000000000000))*(x1139)))+(((cj3)*(x1165)))+(((IkReal(-1.00000000000000))*(x1126)*(x1156)))+(((IkReal(-1.00000000000000))*(x1124)*(x1125))));
3316 evalcond[2]=((((IkReal(-1.00000000000000))*(cj0)*(px)*(x1151)))+(((IkReal(-0.0750000000000000))*(x1120)))+(((IkReal(0.320000000000000))*(x1119)))+(((pz)*(sj1)))+(((IkReal(-1.00000000000000))*(x1135)*(x1154)))+(((x1119)*(x1149)))+(((IkReal(0.0750000000000000))*(cj1)))+(((IkReal(-1.00000000000000))*(x1120)*(x1129))));
3317 evalcond[3]=((IkReal(0.300000000000000))+(((x1120)*(x1149)))+(((IkReal(-1.00000000000000))*(x1124)*(x1154)))+(((IkReal(-1.00000000000000))*(pz)*(x1151)))+(((IkReal(0.0750000000000000))*(x1119)))+(((IkReal(0.320000000000000))*(x1120)))+(((IkReal(-1.00000000000000))*(x1155)*(x1156)))+(((x1119)*(x1129)))+(((IkReal(0.0750000000000000))*(sj1))));
3318 evalcond[4]=((((IkReal(-0.320000000000000))*(x1168)))+(((x1124)*(x1136)))+(((px)*(x1130)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x1151)))+(((IkReal(-1.00000000000000))*(x1130)*(x1137)))+(((x1129)*(x1166)))+(((IkReal(-1.00000000000000))*(x1124)*(x1133)))+(((IkReal(-1.00000000000000))*(sj1)*(x1125)*(x1157)))+(((x1128)*(x1135)))+(((x1144)*(x1146)))+(((IkReal(-1.00000000000000))*(x1149)*(x1168)))+(((x1120)*(x1134))));
3319 evalcond[5]=((((IkReal(0.320000000000000))*(x1166)))+(((x1137)*(x1148)))+(((x1149)*(x1166)))+(((x1135)*(x1136)))+(((x1119)*(x1134)))+(((py)*(x1126)))+(((IkReal(-1.00000000000000))*(x1133)*(x1135)))+(((IkReal(-0.300000000000000))*(r00)*(sj0)))+(((IkReal(-1.00000000000000))*(x1124)*(x1128)))+(((IkReal(-1.00000000000000))*(cj1)*(x1125)*(x1157)))+(((x1129)*(x1168)))+(((IkReal(-1.00000000000000))*(x1125)*(x1155)))+(((IkReal(0.300000000000000))*(cj0)*(r01)))+(((x1139)*(x1146))));
3320 evalcond[6]=((((x1124)*(x1132)*(x1142)))+(((IkReal(-1.00000000000000))*(x1131)*(x1148)))+(((IkReal(0.0512000000000000))*(x1165)))+(((cj0)*(x1133)*(x1145)))+(((pz)*(x1130)*(x1142)))+(((x1139)*(x1167)))+(((r02)*(x1131)*(x1135)))+(((x1143)*(x1164)))+(((IkReal(-1.00000000000000))*(pp)*(x1139)))+(((IkReal(0.0450000000000000))*(x1162)))+(((IkReal(-1.00000000000000))*(x1132)*(x1140)))+(((IkReal(-1.00000000000000))*(sj0)*(x1130)*(x1150)))+(((IkReal(0.0450000000000000))*(x1152)))+(((IkReal(-0.600000000000000))*(x1132)))+(((x1127)*(x1165)))+(((x1158)*(x1165)))+(((x1138)*(x1148)*(x1161)))+(((r01)*(x1124)*(x1169)))+(((cj1)*(x1136)*(x1163)))+(((IkReal(-0.0120000000000000))*(x1164)))+(((IkReal(0.0956250000000000))*(cj0)*(x1126)))+(((IkReal(-1.00000000000000))*(pp)*(x1124)*(x1125)))+(((IkReal(-0.600000000000000))*(x1141)))+(((x1122)*(x1126)*(x1138)))+(((IkReal(0.0956250000000000))*(r01)*(x1124)))+(((x1132)*(x1138)*(x1155)))+(((IkReal(-1.00000000000000))*(r01)*(x1153)))+(((IkReal(-1.00000000000000))*(x1147)*(x1164)))+(((IkReal(-1.00000000000000))*(cj0)*(x1136)*(x1145)))+(((IkReal(-0.150000000000000))*(px)*(x1126)))+(((IkReal(0.0843750000000000))*(x1139)))+(((x1124)*(x1141)*(x1142)))+(((IkReal(-1.00000000000000))*(pp)*(x1126)*(x1156))));
3321 evalcond[7]=((((IkReal(-1.00000000000000))*(x1130)*(x1131)))+(((IkReal(-1.00000000000000))*(pz)*(x1126)*(x1163)))+(((r01)*(x1124)*(x1150)))+(((IkReal(-0.0843750000000000))*(cj1)*(x1152)))+(((IkReal(-1.00000000000000))*(r02)*(sj0)*(x1153)))+(((IkReal(-0.0843750000000000))*(sj0)*(x1130)))+(((x1130)*(x1138)*(x1161)))+(((cj0)*(x1126)*(x1150)))+(((IkReal(0.0450000000000000))*(r02)))+(((IkReal(-1.00000000000000))*(pz)*(x1142)*(x1148)))+(((IkReal(-1.00000000000000))*(x1141)*(x1145)))+(((IkReal(0.0956250000000000))*(x1144)))+(((IkReal(-1.00000000000000))*(x1158)*(x1164)))+(((IkReal(-1.00000000000000))*(pp)*(x1125)*(x1135)))+(((IkReal(-1.00000000000000))*(x1133)*(x1159)))+(((cj1)*(x1170)))+(((pp)*(x1144)))+(((cj1)*(r00)*(x1122)*(x1138)))+(((IkReal(-1.00000000000000))*(x1132)*(x1145)))+(((IkReal(-0.0512000000000000))*(x1164)))+(((x1143)*(x1165)))+(((x1136)*(x1159)))+(((x1135)*(x1141)*(x1142)))+(((IkReal(-1.00000000000000))*(x1144)*(x1167)))+(((IkReal(-1.00000000000000))*(x1147)*(x1165)))+(((IkReal(0.600000000000000))*(pz)*(x1162)))+(((IkReal(-1.00000000000000))*(pp)*(x1151)*(x1152)))+(((IkReal(-1.00000000000000))*(x1127)*(x1164)))+(((IkReal(-0.0120000000000000))*(x1165)))+(((IkReal(-1.00000000000000))*(cj0)*(x1133)*(x1140)))+(((IkReal(-1.00000000000000))*(r02)*(x1124)*(x1131)))+(((x1132)*(x1135)*(x1142)))+(((sj0)*(x1130)*(x1169))));
3322 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 )
3323 {
3324 continue;
3325 }
3326 }
3327 
3328 {
3329 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
3330 vinfos[0].jointtype = 1;
3331 vinfos[0].foffset = j0;
3332 vinfos[0].indices[0] = _ij0[0];
3333 vinfos[0].indices[1] = _ij0[1];
3334 vinfos[0].maxsolutions = _nj0;
3335 vinfos[1].jointtype = 1;
3336 vinfos[1].foffset = j1;
3337 vinfos[1].indices[0] = _ij1[0];
3338 vinfos[1].indices[1] = _ij1[1];
3339 vinfos[1].maxsolutions = _nj1;
3340 vinfos[2].jointtype = 1;
3341 vinfos[2].foffset = j2;
3342 vinfos[2].indices[0] = _ij2[0];
3343 vinfos[2].indices[1] = _ij2[1];
3344 vinfos[2].maxsolutions = _nj2;
3345 vinfos[3].jointtype = 1;
3346 vinfos[3].foffset = j3;
3347 vinfos[3].indices[0] = _ij3[0];
3348 vinfos[3].indices[1] = _ij3[1];
3349 vinfos[3].maxsolutions = _nj3;
3350 vinfos[4].jointtype = 1;
3351 vinfos[4].foffset = j4;
3352 vinfos[4].indices[0] = _ij4[0];
3353 vinfos[4].indices[1] = _ij4[1];
3354 vinfos[4].maxsolutions = _nj4;
3355 std::vector<int> vfree(0);
3356 solutions.AddSolution(vinfos,vfree);
3357 }
3358 }
3359 }
3360 
3361 }
3362 
3363 }
3364 }
3365 }
3366 
3367 }
3368 
3369 }
3370 }
3371 }
3372  }
3373 }
3374 }
3375 }
3376 return solutions.GetNumSolutions()>0;
3377 }
3378 static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
3379 {
3380  using std::complex;
3381  IKFAST_ASSERT(rawcoeffs[0] != 0);
3382  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
3383  const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
3384  complex<IkReal> coeffs[4];
3385  const int maxsteps = 110;
3386  for(int i = 0; i < 4; ++i) {
3387  coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
3388  }
3389  complex<IkReal> roots[4];
3390  IkReal err[4];
3391  roots[0] = complex<IkReal>(1,0);
3392  roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
3393  err[0] = 1.0;
3394  err[1] = 1.0;
3395  for(int i = 2; i < 4; ++i) {
3396  roots[i] = roots[i-1]*roots[1];
3397  err[i] = 1.0;
3398  }
3399  for(int step = 0; step < maxsteps; ++step) {
3400  bool changed = false;
3401  for(int i = 0; i < 4; ++i) {
3402  if ( err[i] >= tol ) {
3403  changed = true;
3404  // evaluate
3405  complex<IkReal> x = roots[i] + coeffs[0];
3406  for(int j = 1; j < 4; ++j) {
3407  x = roots[i] * x + coeffs[j];
3408  }
3409  for(int j = 0; j < 4; ++j) {
3410  if( i != j ) {
3411  if( roots[i] != roots[j] ) {
3412  x /= (roots[i] - roots[j]);
3413  }
3414  }
3415  }
3416  roots[i] -= x;
3417  err[i] = abs(x);
3418  }
3419  }
3420  if( !changed ) {
3421  break;
3422  }
3423  }
3424 
3425  numroots = 0;
3426  bool visited[4] = {false};
3427  for(int i = 0; i < 4; ++i) {
3428  if( !visited[i] ) {
3429  // might be a multiple root, in which case it will have more error than the other roots
3430  // find any neighboring roots, and take the average
3431  complex<IkReal> newroot=roots[i];
3432  int n = 1;
3433  for(int j = i+1; j < 4; ++j) {
3434  if( abs(roots[i]-roots[j]) < 8*tolsqrt ) {
3435  newroot += roots[j];
3436  n += 1;
3437  visited[j] = true;
3438  }
3439  }
3440  if( n > 1 ) {
3441  newroot /= n;
3442  }
3443  // 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
3444  if( IKabs(imag(newroot)) < tolsqrt ) {
3445  rawroots[numroots++] = real(newroot);
3446  }
3447  }
3448  }
3449 }
3450 };
3451 
3452 
3455 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
3456 IKSolver solver;
3457 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
3458 }
3459 
3460 IKFAST_API const char* GetKinematicsHash() { return "e1ede99669e4a55117e29b2cffeb063c"; }
3461 
3462 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
3463 
3464 #ifdef IKFAST_NAMESPACE
3465 } // end namespace
3466 #endif
3467 
3468 #ifndef IKFAST_NO_MAIN
3469 #include <stdio.h>
3470 #include <stdlib.h>
3471 #ifdef IKFAST_NAMESPACE
3472 using namespace IKFAST_NAMESPACE;
3473 #endif
3474 int main(int argc, char** argv)
3475 {
3476  if( argc != 12+GetNumFreeParameters()+1 ) {
3477  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
3478  "Returns the ik solutions given the transformation of the end effector specified by\n"
3479  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
3480  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
3481  return 1;
3482  }
3483 
3484  IkSolutionList<IkReal> solutions;
3485  std::vector<IkReal> vfree(GetNumFreeParameters());
3486  IkReal eerot[9],eetrans[3];
3487  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
3488  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
3489  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
3490  for(std::size_t i = 0; i < vfree.size(); ++i)
3491  vfree[i] = atof(argv[13+i]);
3492  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
3493 
3494  if( !bSuccess ) {
3495  fprintf(stderr,"Failed to get ik solution\n");
3496  return -1;
3497  }
3498 
3499  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
3500  std::vector<IkReal> solvalues(GetNumJoints());
3501  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
3502  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
3503  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
3504  std::vector<IkReal> vsolfree(sol.GetFree().size());
3505  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
3506  for( std::size_t j = 0; j < solvalues.size(); ++j)
3507  printf("%.15f, ", solvalues[j]);
3508  printf("\n");
3509  }
3510  return 0;
3511 }
3512 
3513 #endif
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: lrmate200ic5f_kinematics/include/ikfast.h:239
IK2PI
#define IK2PI
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:62
main
int main(int argc, char **argv)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:3474
IKcos
float IKcos(float f)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:161
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
IKsqrt
float IKsqrt(float f)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:165
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: lrmate200ic5f_kinematics/include/ikfast.h:249
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:210
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:251
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: lrmate200ic5f_kinematics/include/ikfast.h:65
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:3460
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:3455
IKsin
float IKsin(float f)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:159
IKsign
float IKsign(float f)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:188
IKasin
float IKasin(float f)
Definition: fanuc_lrmate200ic5h_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...
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:109
GetIkType
IKFAST_API int GetIkType()
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:256
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_lrmate200ic5h_manipulator_ikfast_solver.cpp:250
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_lrmate200ic5h_manipulator_ikfast_solver.cpp:163
IKatan2
float IKatan2(float fy, float fx)
Definition: fanuc_lrmate200ic5h_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...
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:104
IKlog
float IKlog(float f)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:94
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
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:52
IKabs
float IKabs(float f)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:88
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:99
IKPI
#define IKPI
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:63
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:263
IKSolver::polyroots4
static void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int &numroots)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:3378
step
unsigned int step
acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
IKfmod
float IKfmod(float x, float y)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:128
abs
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:254
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)
IKsqr
float IKsqr(float f)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:91
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)
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:3462
IKFAST_STRINGIZE
#define IKFAST_STRINGIZE(s)
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:35
IKacos
float IKacos(float f)
Definition: fanuc_lrmate200ic5h_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)
x
double x
IKPI_2
#define IKPI_2
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:64
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)
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: fanuc_lrmate200ic5h_manipulator_ikfast_solver.cpp:252
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
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_lrmate200ic5h_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