nextage_right_arm_ikfast_solver.cpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
21 #include "nextage_ik_plugin/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,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47,x48,x49,x50,x51,x52,x53,x54,x55,x56,x57,x58,x59,x60,x61,x62,x63,x64,x65,x66,x67,x68,x69,x70,x71,x72,x73,x74,x75,x76;
212 x0=IKcos(j[0]);
213 x1=IKcos(j[1]);
214 x2=IKsin(j[2]);
215 x3=IKcos(j[2]);
216 x4=IKsin(j[1]);
217 x5=IKsin(j[3]);
218 x6=IKcos(j[3]);
219 x7=IKsin(j[0]);
220 x8=IKcos(j[4]);
221 x9=IKsin(j[4]);
222 x10=IKcos(j[5]);
223 x11=IKsin(j[5]);
224 x12=((IkReal(0.258820826967441))*(x9));
225 x13=((IkReal(0.0900000000000000))*(x4));
226 x14=((IkReal(0.258820826967441))*(x11));
227 x15=((IkReal(1.00000000000000))*(x8));
228 x16=((IkReal(1.00000000000000))*(x5));
229 x17=((IkReal(0.965925348838040))*(x11));
230 x18=((IkReal(0.0470000000000000))*(x3));
231 x19=((IkReal(0.965925348838040))*(x9));
232 x20=((IkReal(1.00000000000000))*(x9));
233 x21=((IkReal(0.00776462480902323))*(x2));
234 x22=((IkReal(0.965925348838040))*(x8));
235 x23=((IkReal(1.00000000000000))*(x2));
236 x24=((IkReal(0.258820826967441))*(x8));
237 x25=((IkReal(1.00000000000000))*(x7));
238 x26=((IkReal(1.00000000000000))*(x3));
239 x27=((IkReal(0.0900000000000000))*(x6));
240 x28=((IkReal(0.965925348838040))*(x10));
241 x29=((IkReal(0.258820826967441))*(x10));
242 x30=((IkReal(1.00000000000000))*(x6));
243 x31=((x0)*(x5));
244 x32=((x1)*(x3));
245 x33=((x6)*(x9));
246 x34=((x4)*(x7));
247 x35=((x0)*(x4));
248 x36=((IkReal(-0.0470000000000000))*(x6));
249 x37=((x3)*(x4));
250 x38=((x2)*(x4));
251 x39=((x1)*(x7));
252 x40=((x1)*(x2));
253 x41=((x5)*(x7));
254 x42=((x16)*(x7));
255 x43=((x25)*(x6));
256 x44=((IkReal(0.0470000000000000))*(x40));
257 x45=((x23)*(x35));
258 x46=((x23)*(x34));
259 x47=((((IkReal(-1.00000000000000))*(x1)*(x26)))+(x38));
260 x48=((((x1)*(x18)))+(((IkReal(-0.0470000000000000))*(x38))));
261 x49=((((IkReal(-0.0900000000000000))*(x32)))+(((x13)*(x2))));
262 x50=((((x1)*(x23)))+(((x26)*(x4))));
263 x51=((IkReal(-1.00000000000000))*(x50));
264 x52=((((x18)*(x4)))+(x44));
265 x53=((((IkReal(0.0900000000000000))*(x40)))+(((x13)*(x3))));
266 x54=((((IkReal(-1.00000000000000))*(x45)))+(((x0)*(x32))));
267 x55=((((IkReal(-1.00000000000000))*(x46)))+(((x32)*(x7))));
268 x56=((x52)*(x6));
269 x57=((x5)*(x51));
270 x58=((x51)*(x6));
271 x59=((((IkReal(-1.00000000000000))*(x0)*(x1)*(x26)))+(x45));
272 x60=((((IkReal(-1.00000000000000))*(x25)*(x32)))+(x46));
273 x61=((((IkReal(-1.00000000000000))*(x0)*(x1)*(x23)))+(((IkReal(-1.00000000000000))*(x26)*(x35))));
274 x62=((((IkReal(-1.00000000000000))*(x25)*(x37)))+(((IkReal(-1.00000000000000))*(x23)*(x39))));
275 x63=((((IkReal(0.0470000000000000))*(x2)*(x39)))+(((x18)*(x34))));
276 x64=((((IkReal(-0.0900000000000000))*(x2)*(x39)))+(((IkReal(-1.00000000000000))*(x13)*(x3)*(x7))));
277 x65=((x55)*(x6));
278 x66=((x54)*(x6));
279 x67=((x10)*(x5)*(x50));
280 x68=((x5)*(x59));
281 x69=((x31)+(x65));
282 x70=((((x0)*(x6)))+(((x5)*(x60))));
283 x71=((((IkReal(-1.00000000000000))*(x0)*(x30)))+(((IkReal(-1.00000000000000))*(x16)*(x60))));
284 x72=((((IkReal(-1.00000000000000))*(x15)*(x47)))+(((IkReal(-1.00000000000000))*(x20)*(x50)*(x6))));
285 x73=((x11)*(x72));
286 x74=((((x20)*(((((IkReal(-1.00000000000000))*(x42)))+(((x30)*(x54)))))))+(((IkReal(-1.00000000000000))*(x15)*(x61))));
287 x75=((((x20)*(((((x0)*(x16)))+(((x30)*(x55)))))))+(((IkReal(-1.00000000000000))*(x15)*(x62))));
288 x76=((x10)*(x75));
289 eerot[0]=((((x8)*(((((IkReal(-1.00000000000000))*(x42)))+(x66)))))+(((x61)*(x9))));
290 eerot[1]=((((x11)*(x74)))+(((x10)*(((((IkReal(-1.00000000000000))*(x43)))+(x68))))));
291 eerot[2]=((((x10)*(x74)))+(((x11)*(((x43)+(((IkReal(-1.00000000000000))*(x16)*(x59))))))));
292 IkReal x77=((x0)*(x40));
293 eetrans[0]=((((IkReal(-0.235000000000000))*(x77)))+(((IkReal(-0.235000000000000))*(x3)*(x35)))+(((IkReal(-0.250000000000000))*(x35)))+(((x8)*(((((x36)*(x54)))+(((IkReal(0.0470000000000000))*(x41)))))))+(((IkReal(0.0950000000000000))*(x7)))+(((x9)*(((((IkReal(0.0900000000000000))*(x41)))+(((IkReal(-1.00000000000000))*(x27)*(x54)))))))+(((IkReal(0.0300000000000000))*(x2)*(x35)))+(((x8)*(((((IkReal(-1.00000000000000))*(x0)*(x13)*(x3)))+(((IkReal(-0.0900000000000000))*(x77)))))))+(((IkReal(-0.0300000000000000))*(x0)*(x32)))+(((x9)*(((((x18)*(x35)))+(((x0)*(x44))))))));
294 eerot[3]=((((IkReal(-1.00000000000000))*(x12)*(x47)))+(((x19)*(x62)))+(((x22)*(x69)))+(((IkReal(-1.00000000000000))*(x24)*(x58))));
295 eerot[4]=((((x17)*(x75)))+(((x28)*(x70)))+(((IkReal(-1.00000000000000))*(x29)*(x5)*(x50)))+(((IkReal(-1.00000000000000))*(x14)*(x72))));
296 eerot[5]=((((x17)*(x71)))+(((x28)*(x75)))+(((IkReal(-1.00000000000000))*(x14)*(x57)))+(((IkReal(-1.00000000000000))*(x29)*(x72))));
297 IkReal x78=((IkReal(1.00000000000000))*(x24));
298 IkReal x79=((IkReal(1.00000000000000))*(x12));
299 eetrans[1]=((IkReal(-0.145000000000000))+(((IkReal(-0.0608228943373486))*(x38)))+(((IkReal(-0.226992456976939))*(x3)*(x34)))+(((IkReal(0.0647052067418602))*(x1)))+(((IkReal(-0.226992456976939))*(x2)*(x39)))+(((IkReal(-1.00000000000000))*(x53)*(x6)*(x79)))+(((x22)*(((((x36)*(x55)))+(((IkReal(-0.0470000000000000))*(x31)))))))+(((IkReal(-1.00000000000000))*(x1)*(x21)))+(((IkReal(-0.0289777604651412))*(x32)*(x7)))+(((x19)*(x63)))+(((IkReal(-0.241481337209510))*(x34)))+(((x19)*(((((IkReal(-1.00000000000000))*(x27)*(x55)))+(((IkReal(-0.0900000000000000))*(x31)))))))+(((IkReal(-1.00000000000000))*(x49)*(x78)))+(((IkReal(-1.00000000000000))*(x48)*(x79)))+(((IkReal(0.0608228943373486))*(x32)))+(((IkReal(0.0289777604651412))*(x2)*(x34)))+(((IkReal(-0.0917629081396138))*(x0)))+(((IkReal(-1.00000000000000))*(x56)*(x78)))+(((IkReal(-0.00776462480902323))*(x37)))+(((x22)*(x64))));
300 eerot[6]=((((x12)*(x62)))+(((x19)*(x47)))+(((x24)*(x69)))+(((x22)*(x58))));
301 eerot[7]=((((x14)*(x75)))+(((x17)*(x72)))+(((x28)*(x5)*(x50)))+(((x29)*(x70))));
302 eerot[8]=((((x14)*(x71)))+(((x17)*(x57)))+(((x28)*(x72)))+(((x29)*(x75))));
303 eetrans[2]=((IkReal(0.370300000000000))+(((x12)*(x63)))+(((IkReal(-0.226992456976939))*(x32)))+(((IkReal(0.0289777604651412))*(x37)))+(((x19)*(x48)))+(((x24)*(((((IkReal(-0.0470000000000000))*(x65)))+(((IkReal(-0.0470000000000000))*(x31)))))))+(((IkReal(-0.00776462480902323))*(x32)*(x7)))+(((IkReal(0.0289777604651412))*(x40)))+(((IkReal(0.226992456976939))*(x38)))+(((x19)*(x53)*(x6)))+(((x22)*(x49)))+(((x21)*(x34)))+(((x24)*(x64)))+(((IkReal(-0.0245879785619069))*(x0)))+(((IkReal(-0.241481337209510))*(x1)))+(((IkReal(-0.0608228943373486))*(x2)*(x39)))+(((x12)*(((((IkReal(-0.0900000000000000))*(x65)))+(((IkReal(-0.0900000000000000))*(x31)))))))+(((IkReal(-0.0608228943373486))*(x3)*(x34)))+(((x22)*(x56)))+(((IkReal(-0.0647052067418602))*(x34))));
304 }
305 
306 IKFAST_API int GetNumFreeParameters() { return 0; }
307 IKFAST_API int* GetFreeParameters() { return NULL; }
308 IKFAST_API int GetNumJoints() { return 6; }
309 
310 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
311 
312 IKFAST_API int GetIkType() { return 0x67000001; }
313 
314 class IKSolver {
315 public:
316 IkReal j9,cj9,sj9,htj9,j10,cj10,sj10,htj10,j11,cj11,sj11,htj11,j12,cj12,sj12,htj12,j13,cj13,sj13,htj13,j14,cj14,sj14,htj14,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_r10,r10,rxp1_0,new_r11,r11,rxp1_1,new_r12,r12,rxp1_2,new_r20,r20,rxp2_0,new_r21,r21,rxp2_1,new_r22,r22,rxp2_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
317 unsigned char _ij9[2], _nj9,_ij10[2], _nj10,_ij11[2], _nj11,_ij12[2], _nj12,_ij13[2], _nj13,_ij14[2], _nj14;
318 
319 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
320 j9=numeric_limits<IkReal>::quiet_NaN(); _ij9[0] = -1; _ij9[1] = -1; _nj9 = -1; j10=numeric_limits<IkReal>::quiet_NaN(); _ij10[0] = -1; _ij10[1] = -1; _nj10 = -1; j11=numeric_limits<IkReal>::quiet_NaN(); _ij11[0] = -1; _ij11[1] = -1; _nj11 = -1; j12=numeric_limits<IkReal>::quiet_NaN(); _ij12[0] = -1; _ij12[1] = -1; _nj12 = -1; j13=numeric_limits<IkReal>::quiet_NaN(); _ij13[0] = -1; _ij13[1] = -1; _nj13 = -1; j14=numeric_limits<IkReal>::quiet_NaN(); _ij14[0] = -1; _ij14[1] = -1; _nj14 = -1;
321 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
322  solutions.Clear();
323 r00 = eerot[0*3+0];
324 r01 = eerot[0*3+1];
325 r02 = eerot[0*3+2];
326 r10 = eerot[1*3+0];
327 r11 = eerot[1*3+1];
328 r12 = eerot[1*3+2];
329 r20 = eerot[2*3+0];
330 r21 = eerot[2*3+1];
331 r22 = eerot[2*3+2];
332 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
333 
334 new_r00=((IkReal(-1.00000000000000))*(r02));
335 new_r01=r01;
336 new_r02=r00;
337 new_px=((px)+(((IkReal(0.0470000000000000))*(r00))));
338 new_r10=((((IkReal(-0.965925348838040))*(r12)))+(((IkReal(-0.258820826967441))*(r22))));
339 new_r11=((((IkReal(0.258820826967441))*(r21)))+(((IkReal(0.965925348838040))*(r11))));
340 new_r12=((((IkReal(0.258820826967441))*(r20)))+(((IkReal(0.965925348838040))*(r10))));
341 new_py=((IkReal(0.0442178233554725))+(((IkReal(0.965925348838040))*(py)))+(((IkReal(0.0453984913953879))*(r10)))+(((IkReal(0.258820826967441))*(pz)))+(((IkReal(0.0121645788674697))*(r20))));
342 new_r20=((((IkReal(0.258820826967441))*(r12)))+(((IkReal(-0.965925348838040))*(r22))));
343 new_r21=((((IkReal(0.965925348838040))*(r21)))+(((IkReal(-0.258820826967441))*(r11))));
344 new_r22=((((IkReal(0.965925348838040))*(r20)))+(((IkReal(-0.258820826967441))*(r10))));
345 new_pz=((IkReal(-0.395211176585005))+(((IkReal(0.965925348838040))*(pz)))+(((IkReal(-0.0121645788674697))*(r10)))+(((IkReal(0.0453984913953879))*(r20)))+(((IkReal(-0.258820826967441))*(py))));
346 r00 = new_r00; r01 = new_r01; r02 = new_r02; r10 = new_r10; r11 = new_r11; r12 = new_r12; r20 = new_r20; r21 = new_r21; r22 = new_r22; px = new_px; py = new_py; pz = new_pz;
347 pp=(((px)*(px))+((py)*(py))+((pz)*(pz)));
348 npx=((((px)*(r00)))+(((py)*(r10)))+(((pz)*(r20))));
349 npy=((((px)*(r01)))+(((py)*(r11)))+(((pz)*(r21))));
350 npz=((((px)*(r02)))+(((py)*(r12)))+(((pz)*(r22))));
351 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
352 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
353 rxp0_2=((((IkReal(-1.00000000000000))*(px)*(r10)))+(((py)*(r00))));
354 rxp1_0=((((IkReal(-1.00000000000000))*(py)*(r21)))+(((pz)*(r11))));
355 rxp1_1=((((px)*(r21)))+(((IkReal(-1.00000000000000))*(pz)*(r01))));
356 rxp1_2=((((IkReal(-1.00000000000000))*(px)*(r11)))+(((py)*(r01))));
357 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
358 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
359 rxp2_2=((((IkReal(-1.00000000000000))*(px)*(r12)))+(((py)*(r02))));
360 IkReal op[72], zeror[48];
361 int numroots;
362 IkReal x80=((IkReal(0.650000000000000))*(npx));
363 IkReal x81=((IkReal(0.0600000000000000))*(npz));
364 IkReal x82=((IkReal(1.00000000000000))*(pp));
365 IkReal x83=((IkReal(0.0600000000000000))*(npy));
366 IkReal x84=((IkReal(0.0950000000000000))*(r22));
367 IkReal x85=((IkReal(0.120000000000000))*(npx));
368 IkReal x86=((IkReal(0.0950000000000000))*(r21));
369 IkReal x87=((IkReal(1.00000000000000))*(rxp0_2));
370 IkReal x88=((IkReal(0.190000000000000))*(r20));
371 IkReal x89=((IkReal(2.00000000000000))*(rxp1_2));
372 IkReal x90=((IkReal(0.940000000000000))*(npz));
373 IkReal x91=((IkReal(2.00000000000000))*(rxp2_2));
374 IkReal x92=((IkReal(0.180000000000000))*(r21));
375 IkReal x93=((IkReal(0.360000000000000))*(r20));
376 IkReal x94=((IkReal(0.380000000000000))*(r21));
377 IkReal x95=((IkReal(0.290000000000000))*(npx));
378 IkReal x96=((IkReal(-0.00570000000000000))+(((IkReal(-1.00000000000000))*(x83))));
379 IkReal x97=((IkReal(0.580000000000000))*(npy));
380 IkReal x98=((IkReal(0.00570000000000000))+(((IkReal(-1.00000000000000))*(x83))));
381 IkReal x99=((rxp0_2)+(((IkReal(-1.00000000000000))*(x84))));
382 IkReal x100=((IkReal(-1.30000000000000))*(npy));
383 IkReal x101=((IkReal(-0.0950000000000000))*(r21));
384 IkReal x102=((IkReal(-0.00570000000000000))+(x83));
385 IkReal x103=((rxp0_2)+(x84));
386 IkReal x104=((x84)+(((IkReal(-1.00000000000000))*(x87))));
387 IkReal x105=((IkReal(0.00570000000000000))+(x83));
388 IkReal x106=((IkReal(-2.00000000000000))*(rxp1_2));
389 IkReal x107=((IkReal(-0.0350000000000000))+(x80));
390 IkReal x108=((IkReal(0.0496000000000000))+(x81));
391 IkReal x109=((x82)+(x81));
392 IkReal x110=((((IkReal(-1.00000000000000))*(x84)))+(((IkReal(-1.00000000000000))*(x87))));
393 IkReal x111=((x85)+(x90));
394 IkReal x112=((x91)+(x92));
395 op[0]=((((IkReal(-1.00000000000000))*(x109)))+(x107));
396 op[1]=x102;
397 op[2]=x100;
398 op[3]=x85;
399 op[4]=((IkReal(-0.0350000000000000))+(((IkReal(-1.00000000000000))*(x109)))+(((IkReal(-1.00000000000000))*(x80))));
400 op[5]=x96;
401 op[6]=x110;
402 op[7]=x86;
403 op[8]=x89;
404 op[9]=x88;
405 op[10]=x99;
406 op[11]=x101;
407 op[12]=x105;
408 op[13]=((x107)+(x81)+(((IkReal(-1.00000000000000))*(x82))));
409 op[14]=x85;
410 op[15]=x100;
411 op[16]=x98;
412 op[17]=((IkReal(-0.0350000000000000))+(x81)+(((IkReal(-1.00000000000000))*(x80)))+(((IkReal(-1.00000000000000))*(x82))));
413 op[18]=x86;
414 op[19]=x104;
415 op[20]=x88;
416 op[21]=x89;
417 op[22]=x101;
418 op[23]=x103;
419 op[24]=((IkReal(-0.0108000000000000))+(x111));
420 op[25]=IkReal(0);
421 op[26]=((IkReal(-0.240000000000000))*(npy));
422 op[27]=IkReal(0);
423 op[28]=((IkReal(-0.0108000000000000))+(x90)+(((IkReal(-1.00000000000000))*(x85))));
424 op[29]=IkReal(0);
425 op[30]=((x88)+(x92)+(((IkReal(-1.00000000000000))*(x91))));
426 op[31]=IkReal(0);
427 op[32]=((x93)+(((IkReal(-1.00000000000000))*(x94))));
428 op[33]=IkReal(0);
429 op[34]=((((IkReal(-1.00000000000000))*(x112)))+(((IkReal(-1.00000000000000))*(x88))));
430 op[35]=IkReal(0);
431 op[36]=IkReal(0);
432 op[37]=((IkReal(0.0108000000000000))+(x90)+(((IkReal(-1.00000000000000))*(x85))));
433 op[38]=IkReal(0);
434 op[39]=((IkReal(0.240000000000000))*(npy));
435 op[40]=IkReal(0);
436 op[41]=((IkReal(0.0108000000000000))+(x111));
437 op[42]=IkReal(0);
438 op[43]=((x92)+(((IkReal(-1.00000000000000))*(x91)))+(((IkReal(-1.00000000000000))*(x88))));
439 op[44]=IkReal(0);
440 op[45]=((x93)+(x94));
441 op[46]=IkReal(0);
442 op[47]=((((IkReal(-1.00000000000000))*(x112)))+(x88));
443 op[48]=((x108)+(((IkReal(-1.00000000000000))*(x95)))+(((IkReal(-1.00000000000000))*(x82))));
444 op[49]=x102;
445 op[50]=x97;
446 op[51]=x85;
447 op[52]=((x108)+(x95)+(((IkReal(-1.00000000000000))*(x82))));
448 op[53]=x96;
449 op[54]=x103;
450 op[55]=x86;
451 op[56]=x106;
452 op[57]=x88;
453 op[58]=x104;
454 op[59]=x101;
455 op[60]=x105;
456 op[61]=((IkReal(0.0496000000000000))+(((IkReal(-1.00000000000000))*(x109)))+(((IkReal(-1.00000000000000))*(x95))));
457 op[62]=x85;
458 op[63]=x97;
459 op[64]=x98;
460 op[65]=((IkReal(0.0496000000000000))+(((IkReal(-1.00000000000000))*(x109)))+(x95));
461 op[66]=x86;
462 op[67]=x99;
463 op[68]=x88;
464 op[69]=x106;
465 op[70]=x101;
466 op[71]=x110;
467 solvedialyticpoly8qep(op,zeror,numroots);
468 IkReal j13array[16], cj13array[16], sj13array[16], j14array[16], cj14array[16], sj14array[16], j12array[16], cj12array[16], sj12array[16];
469 int numsolutions = 0;
470 for(int ij13 = 0; ij13 < numroots; ij13 += 3)
471 {
472 IkReal htj13 = zeror[ij13+0], htj14 = zeror[ij13+1], htj12 = zeror[ij13+2];
473 j13array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj13)));
474 j14array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj14)));
475 j12array[numsolutions]=((IkReal(2.00000000000000))*(atan(htj12)));
476 IkReal x113=(htj13)*(htj13);
477 IkReal x114=(htj14)*(htj14);
478 IkReal x115=(htj12)*(htj12);
479 cj13array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x113))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x113)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x113))))));
480 cj14array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x114))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x114)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x114))))));
481 cj12array[numsolutions]=((((IKabs(((IkReal(1.00000000000000))+(x115))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+(x115)))):(IkReal)1.0e30))*(((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x115))))));
482 sj13array[numsolutions]=((IkReal(2.00000000000000))*(htj13)*(((IKabs(((IkReal(1.00000000000000))+((htj13)*(htj13)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj13)*(htj13))))):(IkReal)1.0e30)));
483 sj14array[numsolutions]=((IkReal(2.00000000000000))*(htj14)*(((IKabs(((IkReal(1.00000000000000))+((htj14)*(htj14)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj14)*(htj14))))):(IkReal)1.0e30)));
484 sj12array[numsolutions]=((IkReal(2.00000000000000))*(htj12)*(((IKabs(((IkReal(1.00000000000000))+((htj12)*(htj12)))) != 0)?((IkReal)1/(((IkReal(1.00000000000000))+((htj12)*(htj12))))):(IkReal)1.0e30)));
485 if( j13array[numsolutions] > IKPI )
486 {
487  j13array[numsolutions]-=IK2PI;
488 }
489 else if( j13array[numsolutions] < -IKPI )
490 {
491  j13array[numsolutions]+=IK2PI;
492 }
493 if( j14array[numsolutions] > IKPI )
494 {
495  j14array[numsolutions]-=IK2PI;
496 }
497 else if( j14array[numsolutions] < -IKPI )
498 {
499  j14array[numsolutions]+=IK2PI;
500 }
501 if( j12array[numsolutions] > IKPI )
502 {
503  j12array[numsolutions]-=IK2PI;
504 }
505 else if( j12array[numsolutions] < -IKPI )
506 {
507  j12array[numsolutions]+=IK2PI;
508 }
509 numsolutions++;
510 }
511 bool j13valid[16]={true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true};
512 _nj13 = 16;
513 _nj14 = 1;
514 _nj12 = 1;
515 for(int ij13 = 0; ij13 < numsolutions; ++ij13)
516  {
517 if( !j13valid[ij13] )
518 {
519  continue;
520 }
521 _ij13[0] = ij13; _ij13[1] = -1;
522 _ij14[0] = 0; _ij14[1] = -1;
523 _ij12[0] = 0; _ij12[1] = -1;
524 for(int iij13 = ij13+1; iij13 < numsolutions; ++iij13)
525 {
526 if( !j13valid[iij13] ) { continue; }
527 if( IKabs(cj13array[ij13]-cj13array[iij13]) < IKFAST_SOLUTION_THRESH && IKabs(sj13array[ij13]-sj13array[iij13]) < IKFAST_SOLUTION_THRESH && IKabs(cj14array[ij13]-cj14array[iij13]) < IKFAST_SOLUTION_THRESH && IKabs(sj14array[ij13]-sj14array[iij13]) < IKFAST_SOLUTION_THRESH && IKabs(cj12array[ij13]-cj12array[iij13]) < IKFAST_SOLUTION_THRESH && IKabs(sj12array[ij13]-sj12array[iij13]) < IKFAST_SOLUTION_THRESH && 1 )
528 {
529  j13valid[iij13]=false; _ij13[1] = iij13; _ij14[1] = 0; _ij12[1] = 0; break;
530 }
531 }
532  j13 = j13array[ij13]; cj13 = cj13array[ij13]; sj13 = sj13array[ij13];
533 
534  j14 = j14array[ij13]; cj14 = cj14array[ij13]; sj14 = sj14array[ij13];
535 
536  j12 = j12array[ij13]; cj12 = cj12array[ij13]; sj12 = sj12array[ij13];
537 
538 {
539 IkReal dummyeval[1];
540 IkReal gconst0;
541 IkReal x116=(cj14)*(cj14);
542 IkReal x117=(sj14)*(sj14);
543 IkReal x118=((IkReal(1.00000000000000))*(r01));
544 IkReal x119=((sj13)*(sj14));
545 IkReal x120=((cj14)*(sj13));
546 IkReal x121=((r00)*(r11));
547 IkReal x122=((cj13)*(x116));
548 IkReal x123=((cj13)*(x117));
549 gconst0=IKsign(((((IkReal(-1.00000000000000))*(r12)*(x118)*(x120)))+(((x121)*(x122)))+(((x121)*(x123)))+(((r02)*(r10)*(x119)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x119)))+(((IkReal(-1.00000000000000))*(r10)*(x118)*(x123)))+(((IkReal(-1.00000000000000))*(r10)*(x118)*(x122)))+(((r02)*(r11)*(x120)))));
550 IkReal x124=(cj14)*(cj14);
551 IkReal x125=(sj14)*(sj14);
552 IkReal x126=((IkReal(1.00000000000000))*(r01));
553 IkReal x127=((sj13)*(sj14));
554 IkReal x128=((cj14)*(sj13));
555 IkReal x129=((r00)*(r11));
556 IkReal x130=((cj13)*(x124));
557 IkReal x131=((cj13)*(x125));
558 dummyeval[0]=((((IkReal(-1.00000000000000))*(r12)*(x126)*(x128)))+(((x129)*(x130)))+(((x129)*(x131)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x127)))+(((r02)*(r11)*(x128)))+(((r02)*(r10)*(x127)))+(((IkReal(-1.00000000000000))*(r10)*(x126)*(x130)))+(((IkReal(-1.00000000000000))*(r10)*(x126)*(x131))));
559 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
560 {
561 {
562 IkReal dummyeval[1];
563 IkReal gconst1;
564 IkReal x132=(sj14)*(sj14);
565 IkReal x133=(cj14)*(cj14);
566 IkReal x134=((cj13)*(r12));
567 IkReal x135=((IkReal(1.00000000000000))*(r10));
568 IkReal x136=((cj13)*(r02));
569 IkReal x137=((r01)*(sj13));
570 IkReal x138=((r00)*(r11)*(sj13));
571 gconst1=IKsign(((((r00)*(sj14)*(x134)))+(((IkReal(-1.00000000000000))*(x132)*(x135)*(x137)))+(((x133)*(x138)))+(((x132)*(x138)))+(((cj14)*(r01)*(x134)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x136)))+(((IkReal(-1.00000000000000))*(sj14)*(x135)*(x136)))+(((IkReal(-1.00000000000000))*(x133)*(x135)*(x137)))));
572 IkReal x139=(sj14)*(sj14);
573 IkReal x140=(cj14)*(cj14);
574 IkReal x141=((cj13)*(r12));
575 IkReal x142=((IkReal(1.00000000000000))*(r10));
576 IkReal x143=((cj13)*(r02));
577 IkReal x144=((r01)*(sj13));
578 IkReal x145=((r00)*(r11)*(sj13));
579 dummyeval[0]=((((x140)*(x145)))+(((cj14)*(r01)*(x141)))+(((IkReal(-1.00000000000000))*(x140)*(x142)*(x144)))+(((IkReal(-1.00000000000000))*(sj14)*(x142)*(x143)))+(((r00)*(sj14)*(x141)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x143)))+(((IkReal(-1.00000000000000))*(x139)*(x142)*(x144)))+(((x139)*(x145))));
580 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
581 {
582 {
583 IkReal dummyeval[1];
584 dummyeval[0]=sj12;
585 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
586 {
587 {
588 IkReal evalcond[3];
589 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
590 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
591 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
592 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 )
593 {
594 {
595 IkReal j11array[1], cj11array[1], sj11array[1];
596 bool j11valid[1]={false};
597 _nj11 = 1;
598 IkReal x146=((IkReal(4.00000000000000))*(cj13));
599 IkReal x147=((npy)*(sj14));
600 IkReal x148=((cj14)*(npx));
601 IkReal x149=((IkReal(4.00000000000000))*(sj13));
602 if( IKabs(((IkReal(0.120000000000000))+(((npz)*(x146)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x148)*(x149)))+(((x147)*(x149))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((npz)*(x149)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x146)*(x147)))+(((x146)*(x148))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((npz)*(x146)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x148)*(x149)))+(((x147)*(x149)))))+IKsqr(((IkReal(-0.940000000000000))+(((npz)*(x149)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x146)*(x147)))+(((x146)*(x148)))))-1) <= IKFAST_SINCOS_THRESH )
603  continue;
604 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((npz)*(x146)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x148)*(x149)))+(((x147)*(x149)))), ((IkReal(-0.940000000000000))+(((npz)*(x149)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x146)*(x147)))+(((x146)*(x148)))));
605 sj11array[0]=IKsin(j11array[0]);
606 cj11array[0]=IKcos(j11array[0]);
607 if( j11array[0] > IKPI )
608 {
609  j11array[0]-=IK2PI;
610 }
611 else if( j11array[0] < -IKPI )
612 { j11array[0]+=IK2PI;
613 }
614 j11valid[0] = true;
615 for(int ij11 = 0; ij11 < 1; ++ij11)
616 {
617 if( !j11valid[ij11] )
618 {
619  continue;
620 }
621 _ij11[0] = ij11; _ij11[1] = -1;
622 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
623 {
624 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
625 {
626  j11valid[iij11]=false; _ij11[1] = iij11; break;
627 }
628 }
629 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
630 {
631 IkReal evalcond[2];
632 IkReal x150=((IkReal(1.00000000000000))*(sj13));
633 IkReal x151=((cj14)*(npx));
634 IkReal x152=((npy)*(sj14));
635 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x152)))+(((IkReal(-1.00000000000000))*(cj13)*(x151)))+(((IkReal(-1.00000000000000))*(npz)*(x150))));
636 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(IKsin(j11))))+(((IkReal(-1.00000000000000))*(x150)*(x151)))+(((sj13)*(x152)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
637 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
638 {
639 continue;
640 }
641 }
642 
643 {
644 IkReal dummyeval[1];
645 IkReal gconst66;
646 IkReal x153=(sj14)*(sj14);
647 IkReal x154=(cj14)*(cj14);
648 IkReal x155=((IkReal(2.00000000000000))*(cj14)*(sj14));
649 gconst66=IKsign(((((x153)*((r00)*(r00))))+(((x154)*((r01)*(r01))))+(((x154)*((r11)*(r11))))+(((r10)*(r11)*(x155)))+(((x153)*((r10)*(r10))))+(((r00)*(r01)*(x155)))));
650 IkReal x156=(sj14)*(sj14);
651 IkReal x157=(cj14)*(cj14);
652 IkReal x158=((IkReal(2.00000000000000))*(cj14)*(sj14));
653 dummyeval[0]=((((x156)*((r00)*(r00))))+(((x157)*((r01)*(r01))))+(((x156)*((r10)*(r10))))+(((r10)*(r11)*(x158)))+(((x157)*((r11)*(r11))))+(((r00)*(r01)*(x158))));
654 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
655 {
656 {
657 IkReal dummyeval[1];
658 IkReal gconst68;
659 gconst68=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
660 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
661 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
662 {
663 {
664 IkReal dummyeval[1];
665 IkReal gconst67;
666 IkReal x159=(cj14)*(cj14);
667 IkReal x160=(sj14)*(sj14);
668 IkReal x161=((IkReal(1.00000000000000))*(r01));
669 IkReal x162=((sj13)*(sj14));
670 IkReal x163=((cj14)*(sj13));
671 IkReal x164=((r00)*(r11));
672 IkReal x165=((cj13)*(x159));
673 IkReal x166=((cj13)*(x160));
674 gconst67=IKsign(((((IkReal(-1.00000000000000))*(r12)*(x161)*(x163)))+(((IkReal(-1.00000000000000))*(r10)*(x161)*(x165)))+(((IkReal(-1.00000000000000))*(r10)*(x161)*(x166)))+(((x164)*(x166)))+(((x164)*(x165)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x162)))+(((r02)*(r11)*(x163)))+(((r02)*(r10)*(x162)))));
675 IkReal x167=(cj14)*(cj14);
676 IkReal x168=(sj14)*(sj14);
677 IkReal x169=((IkReal(1.00000000000000))*(r01));
678 IkReal x170=((sj13)*(sj14));
679 IkReal x171=((cj14)*(sj13));
680 IkReal x172=((r00)*(r11));
681 IkReal x173=((cj13)*(x167));
682 IkReal x174=((cj13)*(x168));
683 dummyeval[0]=((((x172)*(x174)))+(((x172)*(x173)))+(((IkReal(-1.00000000000000))*(r10)*(x169)*(x174)))+(((IkReal(-1.00000000000000))*(r10)*(x169)*(x173)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x170)))+(((r02)*(r10)*(x170)))+(((r02)*(r11)*(x171)))+(((IkReal(-1.00000000000000))*(r12)*(x169)*(x171))));
684 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
685 {
686 continue;
687 
688 } else
689 {
690 {
691 IkReal j9array[1], cj9array[1], sj9array[1];
692 bool j9valid[1]={false};
693 _nj9 = 1;
694 IkReal x175=((cj13)*(cj14));
695 IkReal x176=((IkReal(1.00000000000000))*(cj13)*(sj14));
696 if( IKabs(((gconst67)*(((((IkReal(-1.00000000000000))*(r11)*(x176)))+(((r12)*(sj13)))+(((r10)*(x175))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst67)*(((((r02)*(sj13)))+(((r00)*(x175)))+(((IkReal(-1.00000000000000))*(r01)*(x176))))))) < IKFAST_ATAN2_MAGTHRESH )
697  continue;
698 j9array[0]=IKatan2(((gconst67)*(((((IkReal(-1.00000000000000))*(r11)*(x176)))+(((r12)*(sj13)))+(((r10)*(x175)))))), ((gconst67)*(((((r02)*(sj13)))+(((r00)*(x175)))+(((IkReal(-1.00000000000000))*(r01)*(x176)))))));
699 sj9array[0]=IKsin(j9array[0]);
700 cj9array[0]=IKcos(j9array[0]);
701 if( j9array[0] > IKPI )
702 {
703  j9array[0]-=IK2PI;
704 }
705 else if( j9array[0] < -IKPI )
706 { j9array[0]+=IK2PI;
707 }
708 j9valid[0] = true;
709 for(int ij9 = 0; ij9 < 1; ++ij9)
710 {
711 if( !j9valid[ij9] )
712 {
713  continue;
714 }
715 _ij9[0] = ij9; _ij9[1] = -1;
716 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
717 {
718 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
719 {
720  j9valid[iij9]=false; _ij9[1] = iij9; break;
721 }
722 }
723 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
724 {
725 IkReal evalcond[4];
726 IkReal x177=IKsin(j9);
727 IkReal x178=IKcos(j9);
728 IkReal x179=((r10)*(sj14));
729 IkReal x180=((cj14)*(r11));
730 IkReal x181=((cj14)*(r10));
731 IkReal x182=((cj14)*(r00));
732 IkReal x183=((r11)*(sj14));
733 IkReal x184=((r00)*(sj14));
734 IkReal x185=((IkReal(1.00000000000000))*(x177));
735 IkReal x186=((cj13)*(x177));
736 IkReal x187=((sj13)*(x178));
737 IkReal x188=((r01)*(x177));
738 IkReal x189=((IkReal(1.00000000000000))*(x178));
739 evalcond[0]=((IkReal(1.00000000000000))+(((x177)*(x184)))+(((cj14)*(x188)))+(((IkReal(-1.00000000000000))*(x180)*(x189)))+(((IkReal(-1.00000000000000))*(x179)*(x189))));
740 evalcond[1]=((((IkReal(-1.00000000000000))*(x180)*(x185)))+(((IkReal(-1.00000000000000))*(x184)*(x189)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x189)))+(((IkReal(-1.00000000000000))*(x179)*(x185))));
741 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x183)*(x189)))+(((IkReal(-1.00000000000000))*(cj13)*(x182)*(x185)))+(((r12)*(x187)))+(((r01)*(sj14)*(x186)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x185)))+(((cj13)*(x178)*(x181))));
742 evalcond[3]=((((IkReal(-1.00000000000000))*(sj13)*(x182)*(x185)))+(((IkReal(-1.00000000000000))*(x183)*(x187)))+(((r02)*(x186)))+(((sj13)*(sj14)*(x188)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x189)))+(((x181)*(x187))));
743 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
744 {
745 continue;
746 }
747 }
748 
749 {
750 IkReal dummyeval[1];
751 IkReal gconst69;
752 gconst69=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
753 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
754 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
755 {
756 {
757 IkReal dummyeval[1];
758 IkReal gconst70;
759 gconst70=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
760 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
761 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
762 {
763 continue;
764 
765 } else
766 {
767 {
768 IkReal j10array[1], cj10array[1], sj10array[1];
769 bool j10valid[1]={false};
770 _nj10 = 1;
771 IkReal x190=((cj13)*(sj14));
772 IkReal x191=((IkReal(1.00000000000000))*(cj11));
773 IkReal x192=((r11)*(sj9));
774 IkReal x193=((IkReal(1.00000000000000))*(sj11));
775 IkReal x194=((cj13)*(cj14));
776 IkReal x195=((cj11)*(sj13));
777 IkReal x196=((r12)*(sj9));
778 IkReal x197=((cj9)*(r01));
779 IkReal x198=((sj11)*(sj13));
780 IkReal x199=((r10)*(sj9));
781 IkReal x200=((cj9)*(r00));
782 IkReal x201=((cj9)*(r02));
783 if( IKabs(((gconst70)*(((((IkReal(-1.00000000000000))*(r20)*(x193)*(x194)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x193)))+(((r21)*(sj11)*(x190)))+(((x195)*(x201)))+(((x195)*(x196)))+(((IkReal(-1.00000000000000))*(x190)*(x191)*(x197)))+(((IkReal(-1.00000000000000))*(x190)*(x191)*(x192)))+(((cj11)*(x194)*(x199)))+(((cj11)*(x194)*(x200))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst70)*(((((IkReal(-1.00000000000000))*(x190)*(x193)*(x197)))+(((x198)*(x201)))+(((sj11)*(x194)*(x200)))+(((cj11)*(r20)*(x194)))+(((x196)*(x198)))+(((IkReal(-1.00000000000000))*(r21)*(x190)*(x191)))+(((IkReal(-1.00000000000000))*(x190)*(x192)*(x193)))+(((sj11)*(x194)*(x199)))+(((r22)*(x195))))))) < IKFAST_ATAN2_MAGTHRESH )
784  continue;
785 j10array[0]=IKatan2(((gconst70)*(((((IkReal(-1.00000000000000))*(r20)*(x193)*(x194)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x193)))+(((r21)*(sj11)*(x190)))+(((x195)*(x201)))+(((x195)*(x196)))+(((IkReal(-1.00000000000000))*(x190)*(x191)*(x197)))+(((IkReal(-1.00000000000000))*(x190)*(x191)*(x192)))+(((cj11)*(x194)*(x199)))+(((cj11)*(x194)*(x200)))))), ((gconst70)*(((((IkReal(-1.00000000000000))*(x190)*(x193)*(x197)))+(((x198)*(x201)))+(((sj11)*(x194)*(x200)))+(((cj11)*(r20)*(x194)))+(((x196)*(x198)))+(((IkReal(-1.00000000000000))*(r21)*(x190)*(x191)))+(((IkReal(-1.00000000000000))*(x190)*(x192)*(x193)))+(((sj11)*(x194)*(x199)))+(((r22)*(x195)))))));
786 sj10array[0]=IKsin(j10array[0]);
787 cj10array[0]=IKcos(j10array[0]);
788 if( j10array[0] > IKPI )
789 {
790  j10array[0]-=IK2PI;
791 }
792 else if( j10array[0] < -IKPI )
793 { j10array[0]+=IK2PI;
794 }
795 j10valid[0] = true;
796 for(int ij10 = 0; ij10 < 1; ++ij10)
797 {
798 if( !j10valid[ij10] )
799 {
800  continue;
801 }
802 _ij10[0] = ij10; _ij10[1] = -1;
803 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
804 {
805 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
806 {
807  j10valid[iij10]=false; _ij10[1] = iij10; break;
808 }
809 }
810 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
811 {
812 IkReal evalcond[4];
813 IkReal x202=IKsin(j10);
814 IkReal x203=IKcos(j10);
815 IkReal x204=((IkReal(1.00000000000000))*(sj13));
816 IkReal x205=((r11)*(sj9));
817 IkReal x206=((cj9)*(r01));
818 IkReal x207=((r21)*(sj14));
819 IkReal x208=((cj9)*(r02));
820 IkReal x209=((sj13)*(sj9));
821 IkReal x210=((cj14)*(r10));
822 IkReal x211=((IkReal(1.00000000000000))*(cj13));
823 IkReal x212=((cj14)*(r20));
824 IkReal x213=((cj11)*(x202));
825 IkReal x214=((sj11)*(x203));
826 IkReal x215=((sj14)*(x211));
827 IkReal x216=((sj11)*(x202));
828 IkReal x217=((cj11)*(x203));
829 IkReal x218=((cj14)*(cj9)*(r00));
830 IkReal x219=((x213)+(x214));
831 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x204)))+(((cj13)*(x207)))+(x216)+(((IkReal(-1.00000000000000))*(x217)))+(((IkReal(-1.00000000000000))*(x211)*(x212))));
832 evalcond[1]=((((IkReal(-1.00000000000000))*(x204)*(x212)))+(((sj13)*(x207)))+(x219)+(((cj13)*(r22))));
833 evalcond[2]=((((cj13)*(x218)))+(((IkReal(-1.00000000000000))*(x206)*(x215)))+(((sj13)*(x208)))+(((IkReal(-1.00000000000000))*(x205)*(x215)))+(x219)+(((cj13)*(sj9)*(x210)))+(((r12)*(x209))));
834 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x211)))+(((x209)*(x210)))+(x217)+(((IkReal(-1.00000000000000))*(sj14)*(x204)*(x206)))+(((IkReal(-1.00000000000000))*(sj14)*(x204)*(x205)))+(((IkReal(-1.00000000000000))*(x216)))+(((IkReal(-1.00000000000000))*(x208)*(x211)))+(((sj13)*(x218))));
835 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
836 {
837 continue;
838 }
839 }
840 
841 {
842 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
843 vinfos[0].jointtype = 1;
844 vinfos[0].foffset = j9;
845 vinfos[0].indices[0] = _ij9[0];
846 vinfos[0].indices[1] = _ij9[1];
847 vinfos[0].maxsolutions = _nj9;
848 vinfos[1].jointtype = 1;
849 vinfos[1].foffset = j10;
850 vinfos[1].indices[0] = _ij10[0];
851 vinfos[1].indices[1] = _ij10[1];
852 vinfos[1].maxsolutions = _nj10;
853 vinfos[2].jointtype = 1;
854 vinfos[2].foffset = j11;
855 vinfos[2].indices[0] = _ij11[0];
856 vinfos[2].indices[1] = _ij11[1];
857 vinfos[2].maxsolutions = _nj11;
858 vinfos[3].jointtype = 1;
859 vinfos[3].foffset = j12;
860 vinfos[3].indices[0] = _ij12[0];
861 vinfos[3].indices[1] = _ij12[1];
862 vinfos[3].maxsolutions = _nj12;
863 vinfos[4].jointtype = 1;
864 vinfos[4].foffset = j13;
865 vinfos[4].indices[0] = _ij13[0];
866 vinfos[4].indices[1] = _ij13[1];
867 vinfos[4].maxsolutions = _nj13;
868 vinfos[5].jointtype = 1;
869 vinfos[5].foffset = j14;
870 vinfos[5].indices[0] = _ij14[0];
871 vinfos[5].indices[1] = _ij14[1];
872 vinfos[5].maxsolutions = _nj14;
873 std::vector<int> vfree(0);
874 solutions.AddSolution(vinfos,vfree);
875 }
876 }
877 }
878 
879 }
880 
881 }
882 
883 } else
884 {
885 {
886 IkReal j10array[1], cj10array[1], sj10array[1];
887 bool j10valid[1]={false};
888 _nj10 = 1;
889 IkReal x220=((cj13)*(sj11));
890 IkReal x221=((r21)*(sj14));
891 IkReal x222=((cj11)*(cj13));
892 IkReal x223=((r22)*(sj13));
893 IkReal x224=((sj11)*(sj13));
894 IkReal x225=((cj11)*(sj13));
895 IkReal x226=((IkReal(1.00000000000000))*(cj14)*(r20));
896 if( IKabs(((gconst69)*(((((IkReal(-1.00000000000000))*(x225)*(x226)))+(((x220)*(x221)))+(((IkReal(-1.00000000000000))*(sj11)*(x223)))+(((IkReal(-1.00000000000000))*(x220)*(x226)))+(((x221)*(x225)))+(((r22)*(x222))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst69)*(((((cj11)*(x223)))+(((IkReal(-1.00000000000000))*(x224)*(x226)))+(((cj14)*(r20)*(x222)))+(((x221)*(x224)))+(((r22)*(x220)))+(((IkReal(-1.00000000000000))*(x221)*(x222))))))) < IKFAST_ATAN2_MAGTHRESH )
897  continue;
898 j10array[0]=IKatan2(((gconst69)*(((((IkReal(-1.00000000000000))*(x225)*(x226)))+(((x220)*(x221)))+(((IkReal(-1.00000000000000))*(sj11)*(x223)))+(((IkReal(-1.00000000000000))*(x220)*(x226)))+(((x221)*(x225)))+(((r22)*(x222)))))), ((gconst69)*(((((cj11)*(x223)))+(((IkReal(-1.00000000000000))*(x224)*(x226)))+(((cj14)*(r20)*(x222)))+(((x221)*(x224)))+(((r22)*(x220)))+(((IkReal(-1.00000000000000))*(x221)*(x222)))))));
899 sj10array[0]=IKsin(j10array[0]);
900 cj10array[0]=IKcos(j10array[0]);
901 if( j10array[0] > IKPI )
902 {
903  j10array[0]-=IK2PI;
904 }
905 else if( j10array[0] < -IKPI )
906 { j10array[0]+=IK2PI;
907 }
908 j10valid[0] = true;
909 for(int ij10 = 0; ij10 < 1; ++ij10)
910 {
911 if( !j10valid[ij10] )
912 {
913  continue;
914 }
915 _ij10[0] = ij10; _ij10[1] = -1;
916 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
917 {
918 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
919 {
920  j10valid[iij10]=false; _ij10[1] = iij10; break;
921 }
922 }
923 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
924 {
925 IkReal evalcond[4];
926 IkReal x227=IKsin(j10);
927 IkReal x228=IKcos(j10);
928 IkReal x229=((IkReal(1.00000000000000))*(sj13));
929 IkReal x230=((r11)*(sj9));
930 IkReal x231=((cj9)*(r01));
931 IkReal x232=((r21)*(sj14));
932 IkReal x233=((cj9)*(r02));
933 IkReal x234=((sj13)*(sj9));
934 IkReal x235=((cj14)*(r10));
935 IkReal x236=((IkReal(1.00000000000000))*(cj13));
936 IkReal x237=((cj14)*(r20));
937 IkReal x238=((cj11)*(x227));
938 IkReal x239=((sj11)*(x228));
939 IkReal x240=((sj14)*(x236));
940 IkReal x241=((sj11)*(x227));
941 IkReal x242=((cj11)*(x228));
942 IkReal x243=((cj14)*(cj9)*(r00));
943 IkReal x244=((x238)+(x239));
944 evalcond[0]=((((IkReal(-1.00000000000000))*(x242)))+(x241)+(((cj13)*(x232)))+(((IkReal(-1.00000000000000))*(x236)*(x237)))+(((IkReal(-1.00000000000000))*(r22)*(x229))));
945 evalcond[1]=((((sj13)*(x232)))+(x244)+(((IkReal(-1.00000000000000))*(x229)*(x237)))+(((cj13)*(r22))));
946 evalcond[2]=((((IkReal(-1.00000000000000))*(x231)*(x240)))+(((sj13)*(x233)))+(((IkReal(-1.00000000000000))*(x230)*(x240)))+(x244)+(((cj13)*(x243)))+(((cj13)*(sj9)*(x235)))+(((r12)*(x234))));
947 evalcond[3]=((((x234)*(x235)))+(((IkReal(-1.00000000000000))*(x233)*(x236)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x236)))+(((IkReal(-1.00000000000000))*(x241)))+(((IkReal(-1.00000000000000))*(sj14)*(x229)*(x231)))+(((IkReal(-1.00000000000000))*(sj14)*(x229)*(x230)))+(x242)+(((sj13)*(x243))));
948 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
949 {
950 continue;
951 }
952 }
953 
954 {
955 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
956 vinfos[0].jointtype = 1;
957 vinfos[0].foffset = j9;
958 vinfos[0].indices[0] = _ij9[0];
959 vinfos[0].indices[1] = _ij9[1];
960 vinfos[0].maxsolutions = _nj9;
961 vinfos[1].jointtype = 1;
962 vinfos[1].foffset = j10;
963 vinfos[1].indices[0] = _ij10[0];
964 vinfos[1].indices[1] = _ij10[1];
965 vinfos[1].maxsolutions = _nj10;
966 vinfos[2].jointtype = 1;
967 vinfos[2].foffset = j11;
968 vinfos[2].indices[0] = _ij11[0];
969 vinfos[2].indices[1] = _ij11[1];
970 vinfos[2].maxsolutions = _nj11;
971 vinfos[3].jointtype = 1;
972 vinfos[3].foffset = j12;
973 vinfos[3].indices[0] = _ij12[0];
974 vinfos[3].indices[1] = _ij12[1];
975 vinfos[3].maxsolutions = _nj12;
976 vinfos[4].jointtype = 1;
977 vinfos[4].foffset = j13;
978 vinfos[4].indices[0] = _ij13[0];
979 vinfos[4].indices[1] = _ij13[1];
980 vinfos[4].maxsolutions = _nj13;
981 vinfos[5].jointtype = 1;
982 vinfos[5].foffset = j14;
983 vinfos[5].indices[0] = _ij14[0];
984 vinfos[5].indices[1] = _ij14[1];
985 vinfos[5].maxsolutions = _nj14;
986 std::vector<int> vfree(0);
987 solutions.AddSolution(vinfos,vfree);
988 }
989 }
990 }
991 
992 }
993 
994 }
995 }
996 }
997 
998 }
999 
1000 }
1001 
1002 } else
1003 {
1004 {
1005 IkReal j10array[1], cj10array[1], sj10array[1];
1006 bool j10valid[1]={false};
1007 _nj10 = 1;
1008 IkReal x245=((cj13)*(sj11));
1009 IkReal x246=((r21)*(sj14));
1010 IkReal x247=((cj11)*(cj13));
1011 IkReal x248=((cj11)*(sj13));
1012 IkReal x249=((sj11)*(sj13));
1013 IkReal x250=((IkReal(1.00000000000000))*(cj14)*(r20));
1014 if( IKabs(((gconst68)*(((((r22)*(x247)))+(((IkReal(-1.00000000000000))*(x248)*(x250)))+(((IkReal(-1.00000000000000))*(x245)*(x250)))+(((x245)*(x246)))+(((x246)*(x248)))+(((IkReal(-1.00000000000000))*(r22)*(x249))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst68)*(((((r22)*(x248)))+(((r22)*(x245)))+(((IkReal(-1.00000000000000))*(x249)*(x250)))+(((IkReal(-1.00000000000000))*(x246)*(x247)))+(((x246)*(x249)))+(((cj14)*(r20)*(x247))))))) < IKFAST_ATAN2_MAGTHRESH )
1015  continue;
1016 j10array[0]=IKatan2(((gconst68)*(((((r22)*(x247)))+(((IkReal(-1.00000000000000))*(x248)*(x250)))+(((IkReal(-1.00000000000000))*(x245)*(x250)))+(((x245)*(x246)))+(((x246)*(x248)))+(((IkReal(-1.00000000000000))*(r22)*(x249)))))), ((gconst68)*(((((r22)*(x248)))+(((r22)*(x245)))+(((IkReal(-1.00000000000000))*(x249)*(x250)))+(((IkReal(-1.00000000000000))*(x246)*(x247)))+(((x246)*(x249)))+(((cj14)*(r20)*(x247)))))));
1017 sj10array[0]=IKsin(j10array[0]);
1018 cj10array[0]=IKcos(j10array[0]);
1019 if( j10array[0] > IKPI )
1020 {
1021  j10array[0]-=IK2PI;
1022 }
1023 else if( j10array[0] < -IKPI )
1024 { j10array[0]+=IK2PI;
1025 }
1026 j10valid[0] = true;
1027 for(int ij10 = 0; ij10 < 1; ++ij10)
1028 {
1029 if( !j10valid[ij10] )
1030 {
1031  continue;
1032 }
1033 _ij10[0] = ij10; _ij10[1] = -1;
1034 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
1035 {
1036 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
1037 {
1038  j10valid[iij10]=false; _ij10[1] = iij10; break;
1039 }
1040 }
1041 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
1042 {
1043 IkReal evalcond[2];
1044 IkReal x251=IKsin(j10);
1045 IkReal x252=IKcos(j10);
1046 IkReal x253=((IkReal(1.00000000000000))*(sj13));
1047 IkReal x254=((cj14)*(r20));
1048 IkReal x255=((r21)*(sj14));
1049 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x253)))+(((cj13)*(x255)))+(((sj11)*(x251)))+(((IkReal(-1.00000000000000))*(cj13)*(x254)))+(((IkReal(-1.00000000000000))*(cj11)*(x252))));
1050 evalcond[1]=((((sj13)*(x255)))+(((cj11)*(x251)))+(((sj11)*(x252)))+(((IkReal(-1.00000000000000))*(x253)*(x254)))+(((cj13)*(r22))));
1051 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
1052 {
1053 continue;
1054 }
1055 }
1056 
1057 {
1058 IkReal dummyeval[1];
1059 IkReal gconst71;
1060 IkReal x256=(sj14)*(sj14);
1061 IkReal x257=(cj14)*(cj14);
1062 IkReal x258=((IkReal(2.00000000000000))*(cj14)*(sj14));
1063 gconst71=IKsign(((((r10)*(r11)*(x258)))+(((x256)*((r10)*(r10))))+(((x257)*((r11)*(r11))))+(((r00)*(r01)*(x258)))+(((x257)*((r01)*(r01))))+(((x256)*((r00)*(r00))))));
1064 IkReal x259=(sj14)*(sj14);
1065 IkReal x260=(cj14)*(cj14);
1066 IkReal x261=((IkReal(2.00000000000000))*(cj14)*(sj14));
1067 dummyeval[0]=((((r00)*(r01)*(x261)))+(((x260)*((r01)*(r01))))+(((x260)*((r11)*(r11))))+(((x259)*((r10)*(r10))))+(((x259)*((r00)*(r00))))+(((r10)*(r11)*(x261))));
1068 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1069 {
1070 {
1071 IkReal dummyeval[1];
1072 IkReal gconst72;
1073 IkReal x262=(cj14)*(cj14);
1074 IkReal x263=(sj14)*(sj14);
1075 IkReal x264=((IkReal(1.00000000000000))*(r01));
1076 IkReal x265=((sj13)*(sj14));
1077 IkReal x266=((cj14)*(sj13));
1078 IkReal x267=((r00)*(r11));
1079 IkReal x268=((cj13)*(x262));
1080 IkReal x269=((cj13)*(x263));
1081 gconst72=IKsign(((((x267)*(x268)))+(((x267)*(x269)))+(((r02)*(r10)*(x265)))+(((r02)*(r11)*(x266)))+(((IkReal(-1.00000000000000))*(r10)*(x264)*(x269)))+(((IkReal(-1.00000000000000))*(r10)*(x264)*(x268)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x265)))+(((IkReal(-1.00000000000000))*(r12)*(x264)*(x266)))));
1082 IkReal x270=(cj14)*(cj14);
1083 IkReal x271=(sj14)*(sj14);
1084 IkReal x272=((IkReal(1.00000000000000))*(r01));
1085 IkReal x273=((sj13)*(sj14));
1086 IkReal x274=((cj14)*(sj13));
1087 IkReal x275=((r00)*(r11));
1088 IkReal x276=((cj13)*(x270));
1089 IkReal x277=((cj13)*(x271));
1090 dummyeval[0]=((((x275)*(x277)))+(((x275)*(x276)))+(((IkReal(-1.00000000000000))*(r12)*(x272)*(x274)))+(((r02)*(r11)*(x274)))+(((r02)*(r10)*(x273)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x273)))+(((IkReal(-1.00000000000000))*(r10)*(x272)*(x277)))+(((IkReal(-1.00000000000000))*(r10)*(x272)*(x276))));
1091 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1092 {
1093 continue;
1094 
1095 } else
1096 {
1097 {
1098 IkReal j9array[1], cj9array[1], sj9array[1];
1099 bool j9valid[1]={false};
1100 _nj9 = 1;
1101 IkReal x278=((cj13)*(cj14));
1102 IkReal x279=((IkReal(1.00000000000000))*(cj13)*(sj14));
1103 if( IKabs(((gconst72)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x279)))+(((r10)*(x278))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst72)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x279)))+(((r00)*(x278))))))) < IKFAST_ATAN2_MAGTHRESH )
1104  continue;
1105 j9array[0]=IKatan2(((gconst72)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x279)))+(((r10)*(x278)))))), ((gconst72)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x279)))+(((r00)*(x278)))))));
1106 sj9array[0]=IKsin(j9array[0]);
1107 cj9array[0]=IKcos(j9array[0]);
1108 if( j9array[0] > IKPI )
1109 {
1110  j9array[0]-=IK2PI;
1111 }
1112 else if( j9array[0] < -IKPI )
1113 { j9array[0]+=IK2PI;
1114 }
1115 j9valid[0] = true;
1116 for(int ij9 = 0; ij9 < 1; ++ij9)
1117 {
1118 if( !j9valid[ij9] )
1119 {
1120  continue;
1121 }
1122 _ij9[0] = ij9; _ij9[1] = -1;
1123 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
1124 {
1125 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
1126 {
1127  j9valid[iij9]=false; _ij9[1] = iij9; break;
1128 }
1129 }
1130 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
1131 {
1132 IkReal evalcond[6];
1133 IkReal x280=IKsin(j9);
1134 IkReal x281=IKcos(j9);
1135 IkReal x282=((IkReal(1.00000000000000))*(cj14));
1136 IkReal x283=((IkReal(1.00000000000000))*(sj14));
1137 IkReal x284=((cj13)*(cj14));
1138 IkReal x285=((cj14)*(r10));
1139 IkReal x286=((r01)*(sj14));
1140 IkReal x287=((IkReal(1.00000000000000))*(r12));
1141 IkReal x288=((sj13)*(x281));
1142 IkReal x289=((r02)*(x280));
1143 IkReal x290=((r11)*(x280));
1144 IkReal x291=((r10)*(x281));
1145 IkReal x292=((r01)*(x281));
1146 IkReal x293=((sj13)*(x280));
1147 IkReal x294=((r11)*(x281));
1148 IkReal x295=((cj13)*(x280));
1149 IkReal x296=((r10)*(x280));
1150 IkReal x297=((r00)*(x281));
1151 IkReal x298=((cj13)*(x281));
1152 evalcond[0]=((IkReal(1.00000000000000))+(((r00)*(sj14)*(x280)))+(((IkReal(-1.00000000000000))*(x283)*(x291)))+(((cj14)*(r01)*(x280)))+(((IkReal(-1.00000000000000))*(x282)*(x294))));
1153 evalcond[1]=((((IkReal(-1.00000000000000))*(x283)*(x297)))+(((IkReal(-1.00000000000000))*(x283)*(x296)))+(((IkReal(-1.00000000000000))*(x282)*(x290)))+(((IkReal(-1.00000000000000))*(x282)*(x292))));
1154 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x283)*(x294)))+(((IkReal(-1.00000000000000))*(sj13)*(x289)))+(((x284)*(x291)))+(((r12)*(x288)))+(((x286)*(x295)))+(((IkReal(-1.00000000000000))*(r00)*(x282)*(x295))));
1155 evalcond[3]=((((x285)*(x288)))+(((cj13)*(x289)))+(((IkReal(-1.00000000000000))*(r11)*(x283)*(x288)))+(((x286)*(x293)))+(((IkReal(-1.00000000000000))*(x287)*(x298)))+(((IkReal(-1.00000000000000))*(r00)*(x282)*(x293))));
1156 evalcond[4]=((((r12)*(x293)))+(((IkReal(-1.00000000000000))*(cj13)*(x283)*(x290)))+(((IkReal(-1.00000000000000))*(cj13)*(x283)*(x292)))+(((r02)*(x288)))+(((cj10)*(sj11)))+(((x284)*(x296)))+(((x284)*(x297)))+(((cj11)*(sj10))));
1157 evalcond[5]=((((cj14)*(r00)*(x288)))+(((IkReal(-1.00000000000000))*(r01)*(x283)*(x288)))+(((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(r02)*(x298)))+(((x285)*(x293)))+(((IkReal(-1.00000000000000))*(x287)*(x295)))+(((cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(sj13)*(x283)*(x290))));
1158 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 )
1159 {
1160 continue;
1161 }
1162 }
1163 
1164 {
1165 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1166 vinfos[0].jointtype = 1;
1167 vinfos[0].foffset = j9;
1168 vinfos[0].indices[0] = _ij9[0];
1169 vinfos[0].indices[1] = _ij9[1];
1170 vinfos[0].maxsolutions = _nj9;
1171 vinfos[1].jointtype = 1;
1172 vinfos[1].foffset = j10;
1173 vinfos[1].indices[0] = _ij10[0];
1174 vinfos[1].indices[1] = _ij10[1];
1175 vinfos[1].maxsolutions = _nj10;
1176 vinfos[2].jointtype = 1;
1177 vinfos[2].foffset = j11;
1178 vinfos[2].indices[0] = _ij11[0];
1179 vinfos[2].indices[1] = _ij11[1];
1180 vinfos[2].maxsolutions = _nj11;
1181 vinfos[3].jointtype = 1;
1182 vinfos[3].foffset = j12;
1183 vinfos[3].indices[0] = _ij12[0];
1184 vinfos[3].indices[1] = _ij12[1];
1185 vinfos[3].maxsolutions = _nj12;
1186 vinfos[4].jointtype = 1;
1187 vinfos[4].foffset = j13;
1188 vinfos[4].indices[0] = _ij13[0];
1189 vinfos[4].indices[1] = _ij13[1];
1190 vinfos[4].maxsolutions = _nj13;
1191 vinfos[5].jointtype = 1;
1192 vinfos[5].foffset = j14;
1193 vinfos[5].indices[0] = _ij14[0];
1194 vinfos[5].indices[1] = _ij14[1];
1195 vinfos[5].maxsolutions = _nj14;
1196 std::vector<int> vfree(0);
1197 solutions.AddSolution(vinfos,vfree);
1198 }
1199 }
1200 }
1201 
1202 }
1203 
1204 }
1205 
1206 } else
1207 {
1208 {
1209 IkReal j9array[1], cj9array[1], sj9array[1];
1210 bool j9valid[1]={false};
1211 _nj9 = 1;
1212 if( IKabs(((gconst71)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst71)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
1213  continue;
1214 j9array[0]=IKatan2(((gconst71)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst71)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
1215 sj9array[0]=IKsin(j9array[0]);
1216 cj9array[0]=IKcos(j9array[0]);
1217 if( j9array[0] > IKPI )
1218 {
1219  j9array[0]-=IK2PI;
1220 }
1221 else if( j9array[0] < -IKPI )
1222 { j9array[0]+=IK2PI;
1223 }
1224 j9valid[0] = true;
1225 for(int ij9 = 0; ij9 < 1; ++ij9)
1226 {
1227 if( !j9valid[ij9] )
1228 {
1229  continue;
1230 }
1231 _ij9[0] = ij9; _ij9[1] = -1;
1232 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
1233 {
1234 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
1235 {
1236  j9valid[iij9]=false; _ij9[1] = iij9; break;
1237 }
1238 }
1239 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
1240 {
1241 IkReal evalcond[6];
1242 IkReal x299=IKsin(j9);
1243 IkReal x300=IKcos(j9);
1244 IkReal x301=((IkReal(1.00000000000000))*(cj14));
1245 IkReal x302=((IkReal(1.00000000000000))*(sj14));
1246 IkReal x303=((cj13)*(cj14));
1247 IkReal x304=((cj14)*(r10));
1248 IkReal x305=((r01)*(sj14));
1249 IkReal x306=((IkReal(1.00000000000000))*(r12));
1250 IkReal x307=((sj13)*(x300));
1251 IkReal x308=((r02)*(x299));
1252 IkReal x309=((r11)*(x299));
1253 IkReal x310=((r10)*(x300));
1254 IkReal x311=((r01)*(x300));
1255 IkReal x312=((sj13)*(x299));
1256 IkReal x313=((r11)*(x300));
1257 IkReal x314=((cj13)*(x299));
1258 IkReal x315=((r10)*(x299));
1259 IkReal x316=((r00)*(x300));
1260 IkReal x317=((cj13)*(x300));
1261 evalcond[0]=((IkReal(1.00000000000000))+(((cj14)*(r01)*(x299)))+(((IkReal(-1.00000000000000))*(x302)*(x310)))+(((r00)*(sj14)*(x299)))+(((IkReal(-1.00000000000000))*(x301)*(x313))));
1262 evalcond[1]=((((IkReal(-1.00000000000000))*(x301)*(x309)))+(((IkReal(-1.00000000000000))*(x302)*(x316)))+(((IkReal(-1.00000000000000))*(x302)*(x315)))+(((IkReal(-1.00000000000000))*(x301)*(x311))));
1263 evalcond[2]=((((x305)*(x314)))+(((IkReal(-1.00000000000000))*(r00)*(x301)*(x314)))+(((IkReal(-1.00000000000000))*(cj13)*(x302)*(x313)))+(((IkReal(-1.00000000000000))*(sj13)*(x308)))+(((x303)*(x310)))+(((r12)*(x307))));
1264 evalcond[3]=((((x305)*(x312)))+(((IkReal(-1.00000000000000))*(r00)*(x301)*(x312)))+(((IkReal(-1.00000000000000))*(r11)*(x302)*(x307)))+(((IkReal(-1.00000000000000))*(x306)*(x317)))+(((x304)*(x307)))+(((cj13)*(x308))));
1265 evalcond[4]=((((IkReal(-1.00000000000000))*(cj13)*(x302)*(x309)))+(((r02)*(x307)))+(((cj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x302)*(x311)))+(((x303)*(x315)))+(((x303)*(x316)))+(((r12)*(x312)))+(((cj11)*(sj10))));
1266 evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(x317)))+(((x304)*(x312)))+(((IkReal(-1.00000000000000))*(r01)*(x302)*(x307)))+(((IkReal(-1.00000000000000))*(sj13)*(x302)*(x309)))+(((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(x306)*(x314)))+(((cj10)*(cj11)))+(((cj14)*(r00)*(x307))));
1267 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 )
1268 {
1269 continue;
1270 }
1271 }
1272 
1273 {
1274 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1275 vinfos[0].jointtype = 1;
1276 vinfos[0].foffset = j9;
1277 vinfos[0].indices[0] = _ij9[0];
1278 vinfos[0].indices[1] = _ij9[1];
1279 vinfos[0].maxsolutions = _nj9;
1280 vinfos[1].jointtype = 1;
1281 vinfos[1].foffset = j10;
1282 vinfos[1].indices[0] = _ij10[0];
1283 vinfos[1].indices[1] = _ij10[1];
1284 vinfos[1].maxsolutions = _nj10;
1285 vinfos[2].jointtype = 1;
1286 vinfos[2].foffset = j11;
1287 vinfos[2].indices[0] = _ij11[0];
1288 vinfos[2].indices[1] = _ij11[1];
1289 vinfos[2].maxsolutions = _nj11;
1290 vinfos[3].jointtype = 1;
1291 vinfos[3].foffset = j12;
1292 vinfos[3].indices[0] = _ij12[0];
1293 vinfos[3].indices[1] = _ij12[1];
1294 vinfos[3].maxsolutions = _nj12;
1295 vinfos[4].jointtype = 1;
1296 vinfos[4].foffset = j13;
1297 vinfos[4].indices[0] = _ij13[0];
1298 vinfos[4].indices[1] = _ij13[1];
1299 vinfos[4].maxsolutions = _nj13;
1300 vinfos[5].jointtype = 1;
1301 vinfos[5].foffset = j14;
1302 vinfos[5].indices[0] = _ij14[0];
1303 vinfos[5].indices[1] = _ij14[1];
1304 vinfos[5].maxsolutions = _nj14;
1305 std::vector<int> vfree(0);
1306 solutions.AddSolution(vinfos,vfree);
1307 }
1308 }
1309 }
1310 
1311 }
1312 
1313 }
1314 }
1315 }
1316 
1317 }
1318 
1319 }
1320 
1321 } else
1322 {
1323 {
1324 IkReal j9array[1], cj9array[1], sj9array[1];
1325 bool j9valid[1]={false};
1326 _nj9 = 1;
1327 if( IKabs(((gconst66)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst66)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
1328  continue;
1329 j9array[0]=IKatan2(((gconst66)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst66)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
1330 sj9array[0]=IKsin(j9array[0]);
1331 cj9array[0]=IKcos(j9array[0]);
1332 if( j9array[0] > IKPI )
1333 {
1334  j9array[0]-=IK2PI;
1335 }
1336 else if( j9array[0] < -IKPI )
1337 { j9array[0]+=IK2PI;
1338 }
1339 j9valid[0] = true;
1340 for(int ij9 = 0; ij9 < 1; ++ij9)
1341 {
1342 if( !j9valid[ij9] )
1343 {
1344  continue;
1345 }
1346 _ij9[0] = ij9; _ij9[1] = -1;
1347 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
1348 {
1349 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
1350 {
1351  j9valid[iij9]=false; _ij9[1] = iij9; break;
1352 }
1353 }
1354 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
1355 {
1356 IkReal evalcond[4];
1357 IkReal x318=IKsin(j9);
1358 IkReal x319=IKcos(j9);
1359 IkReal x320=((r10)*(sj14));
1360 IkReal x321=((cj14)*(r11));
1361 IkReal x322=((cj14)*(r10));
1362 IkReal x323=((cj14)*(r00));
1363 IkReal x324=((r11)*(sj14));
1364 IkReal x325=((r00)*(sj14));
1365 IkReal x326=((IkReal(1.00000000000000))*(x318));
1366 IkReal x327=((cj13)*(x318));
1367 IkReal x328=((sj13)*(x319));
1368 IkReal x329=((r01)*(x318));
1369 IkReal x330=((IkReal(1.00000000000000))*(x319));
1370 evalcond[0]=((IkReal(1.00000000000000))+(((x318)*(x325)))+(((IkReal(-1.00000000000000))*(x321)*(x330)))+(((IkReal(-1.00000000000000))*(x320)*(x330)))+(((cj14)*(x329))));
1371 evalcond[1]=((((IkReal(-1.00000000000000))*(cj14)*(r01)*(x330)))+(((IkReal(-1.00000000000000))*(x321)*(x326)))+(((IkReal(-1.00000000000000))*(x320)*(x326)))+(((IkReal(-1.00000000000000))*(x325)*(x330))));
1372 evalcond[2]=((((r12)*(x328)))+(((IkReal(-1.00000000000000))*(cj13)*(x324)*(x330)))+(((cj13)*(x319)*(x322)))+(((r01)*(sj14)*(x327)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x326)))+(((IkReal(-1.00000000000000))*(cj13)*(x323)*(x326))));
1373 evalcond[3]=((((r02)*(x327)))+(((x322)*(x328)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x330)))+(((IkReal(-1.00000000000000))*(x324)*(x328)))+(((sj13)*(sj14)*(x329)))+(((IkReal(-1.00000000000000))*(sj13)*(x323)*(x326))));
1374 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1375 {
1376 continue;
1377 }
1378 }
1379 
1380 {
1381 IkReal dummyeval[1];
1382 IkReal gconst69;
1383 gconst69=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
1384 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
1385 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1386 {
1387 {
1388 IkReal dummyeval[1];
1389 IkReal gconst70;
1390 gconst70=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
1391 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
1392 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1393 {
1394 continue;
1395 
1396 } else
1397 {
1398 {
1399 IkReal j10array[1], cj10array[1], sj10array[1];
1400 bool j10valid[1]={false};
1401 _nj10 = 1;
1402 IkReal x331=((cj13)*(sj14));
1403 IkReal x332=((IkReal(1.00000000000000))*(cj11));
1404 IkReal x333=((r11)*(sj9));
1405 IkReal x334=((IkReal(1.00000000000000))*(sj11));
1406 IkReal x335=((cj13)*(cj14));
1407 IkReal x336=((cj11)*(sj13));
1408 IkReal x337=((r12)*(sj9));
1409 IkReal x338=((cj9)*(r01));
1410 IkReal x339=((sj11)*(sj13));
1411 IkReal x340=((r10)*(sj9));
1412 IkReal x341=((cj9)*(r00));
1413 IkReal x342=((cj9)*(r02));
1414 if( IKabs(((gconst70)*(((((x336)*(x342)))+(((x336)*(x337)))+(((IkReal(-1.00000000000000))*(x331)*(x332)*(x333)))+(((IkReal(-1.00000000000000))*(x331)*(x332)*(x338)))+(((r21)*(sj11)*(x331)))+(((IkReal(-1.00000000000000))*(r20)*(x334)*(x335)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x334)))+(((cj11)*(x335)*(x340)))+(((cj11)*(x335)*(x341))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst70)*(((((r22)*(x336)))+(((x337)*(x339)))+(((sj11)*(x335)*(x340)))+(((sj11)*(x335)*(x341)))+(((IkReal(-1.00000000000000))*(x331)*(x333)*(x334)))+(((cj11)*(r20)*(x335)))+(((IkReal(-1.00000000000000))*(r21)*(x331)*(x332)))+(((x339)*(x342)))+(((IkReal(-1.00000000000000))*(x331)*(x334)*(x338))))))) < IKFAST_ATAN2_MAGTHRESH )
1415  continue;
1416 j10array[0]=IKatan2(((gconst70)*(((((x336)*(x342)))+(((x336)*(x337)))+(((IkReal(-1.00000000000000))*(x331)*(x332)*(x333)))+(((IkReal(-1.00000000000000))*(x331)*(x332)*(x338)))+(((r21)*(sj11)*(x331)))+(((IkReal(-1.00000000000000))*(r20)*(x334)*(x335)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x334)))+(((cj11)*(x335)*(x340)))+(((cj11)*(x335)*(x341)))))), ((gconst70)*(((((r22)*(x336)))+(((x337)*(x339)))+(((sj11)*(x335)*(x340)))+(((sj11)*(x335)*(x341)))+(((IkReal(-1.00000000000000))*(x331)*(x333)*(x334)))+(((cj11)*(r20)*(x335)))+(((IkReal(-1.00000000000000))*(r21)*(x331)*(x332)))+(((x339)*(x342)))+(((IkReal(-1.00000000000000))*(x331)*(x334)*(x338)))))));
1417 sj10array[0]=IKsin(j10array[0]);
1418 cj10array[0]=IKcos(j10array[0]);
1419 if( j10array[0] > IKPI )
1420 {
1421  j10array[0]-=IK2PI;
1422 }
1423 else if( j10array[0] < -IKPI )
1424 { j10array[0]+=IK2PI;
1425 }
1426 j10valid[0] = true;
1427 for(int ij10 = 0; ij10 < 1; ++ij10)
1428 {
1429 if( !j10valid[ij10] )
1430 {
1431  continue;
1432 }
1433 _ij10[0] = ij10; _ij10[1] = -1;
1434 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
1435 {
1436 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
1437 {
1438  j10valid[iij10]=false; _ij10[1] = iij10; break;
1439 }
1440 }
1441 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
1442 {
1443 IkReal evalcond[4];
1444 IkReal x343=IKsin(j10);
1445 IkReal x344=IKcos(j10);
1446 IkReal x345=((IkReal(1.00000000000000))*(sj13));
1447 IkReal x346=((r11)*(sj9));
1448 IkReal x347=((cj9)*(r01));
1449 IkReal x348=((r21)*(sj14));
1450 IkReal x349=((cj9)*(r02));
1451 IkReal x350=((sj13)*(sj9));
1452 IkReal x351=((cj14)*(r10));
1453 IkReal x352=((IkReal(1.00000000000000))*(cj13));
1454 IkReal x353=((cj14)*(r20));
1455 IkReal x354=((cj11)*(x343));
1456 IkReal x355=((sj11)*(x344));
1457 IkReal x356=((sj14)*(x352));
1458 IkReal x357=((sj11)*(x343));
1459 IkReal x358=((cj11)*(x344));
1460 IkReal x359=((cj14)*(cj9)*(r00));
1461 IkReal x360=((x355)+(x354));
1462 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x345)))+(((cj13)*(x348)))+(((IkReal(-1.00000000000000))*(x358)))+(x357)+(((IkReal(-1.00000000000000))*(x352)*(x353))));
1463 evalcond[1]=((((sj13)*(x348)))+(((IkReal(-1.00000000000000))*(x345)*(x353)))+(x360)+(((cj13)*(r22))));
1464 evalcond[2]=((((cj13)*(sj9)*(x351)))+(((sj13)*(x349)))+(((r12)*(x350)))+(((cj13)*(x359)))+(((IkReal(-1.00000000000000))*(x346)*(x356)))+(x360)+(((IkReal(-1.00000000000000))*(x347)*(x356))));
1465 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x345)*(x347)))+(((IkReal(-1.00000000000000))*(sj14)*(x345)*(x346)))+(((IkReal(-1.00000000000000))*(x349)*(x352)))+(((IkReal(-1.00000000000000))*(x357)))+(x358)+(((sj13)*(x359)))+(((x350)*(x351)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x352))));
1466 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1467 {
1468 continue;
1469 }
1470 }
1471 
1472 {
1473 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1474 vinfos[0].jointtype = 1;
1475 vinfos[0].foffset = j9;
1476 vinfos[0].indices[0] = _ij9[0];
1477 vinfos[0].indices[1] = _ij9[1];
1478 vinfos[0].maxsolutions = _nj9;
1479 vinfos[1].jointtype = 1;
1480 vinfos[1].foffset = j10;
1481 vinfos[1].indices[0] = _ij10[0];
1482 vinfos[1].indices[1] = _ij10[1];
1483 vinfos[1].maxsolutions = _nj10;
1484 vinfos[2].jointtype = 1;
1485 vinfos[2].foffset = j11;
1486 vinfos[2].indices[0] = _ij11[0];
1487 vinfos[2].indices[1] = _ij11[1];
1488 vinfos[2].maxsolutions = _nj11;
1489 vinfos[3].jointtype = 1;
1490 vinfos[3].foffset = j12;
1491 vinfos[3].indices[0] = _ij12[0];
1492 vinfos[3].indices[1] = _ij12[1];
1493 vinfos[3].maxsolutions = _nj12;
1494 vinfos[4].jointtype = 1;
1495 vinfos[4].foffset = j13;
1496 vinfos[4].indices[0] = _ij13[0];
1497 vinfos[4].indices[1] = _ij13[1];
1498 vinfos[4].maxsolutions = _nj13;
1499 vinfos[5].jointtype = 1;
1500 vinfos[5].foffset = j14;
1501 vinfos[5].indices[0] = _ij14[0];
1502 vinfos[5].indices[1] = _ij14[1];
1503 vinfos[5].maxsolutions = _nj14;
1504 std::vector<int> vfree(0);
1505 solutions.AddSolution(vinfos,vfree);
1506 }
1507 }
1508 }
1509 
1510 }
1511 
1512 }
1513 
1514 } else
1515 {
1516 {
1517 IkReal j10array[1], cj10array[1], sj10array[1];
1518 bool j10valid[1]={false};
1519 _nj10 = 1;
1520 IkReal x361=((cj13)*(sj11));
1521 IkReal x362=((r21)*(sj14));
1522 IkReal x363=((cj11)*(cj13));
1523 IkReal x364=((r22)*(sj13));
1524 IkReal x365=((sj11)*(sj13));
1525 IkReal x366=((cj11)*(sj13));
1526 IkReal x367=((IkReal(1.00000000000000))*(cj14)*(r20));
1527 if( IKabs(((gconst69)*(((((x361)*(x362)))+(((IkReal(-1.00000000000000))*(x361)*(x367)))+(((IkReal(-1.00000000000000))*(sj11)*(x364)))+(((IkReal(-1.00000000000000))*(x366)*(x367)))+(((r22)*(x363)))+(((x362)*(x366))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst69)*(((((cj14)*(r20)*(x363)))+(((cj11)*(x364)))+(((IkReal(-1.00000000000000))*(x362)*(x363)))+(((r22)*(x361)))+(((IkReal(-1.00000000000000))*(x365)*(x367)))+(((x362)*(x365))))))) < IKFAST_ATAN2_MAGTHRESH )
1528  continue;
1529 j10array[0]=IKatan2(((gconst69)*(((((x361)*(x362)))+(((IkReal(-1.00000000000000))*(x361)*(x367)))+(((IkReal(-1.00000000000000))*(sj11)*(x364)))+(((IkReal(-1.00000000000000))*(x366)*(x367)))+(((r22)*(x363)))+(((x362)*(x366)))))), ((gconst69)*(((((cj14)*(r20)*(x363)))+(((cj11)*(x364)))+(((IkReal(-1.00000000000000))*(x362)*(x363)))+(((r22)*(x361)))+(((IkReal(-1.00000000000000))*(x365)*(x367)))+(((x362)*(x365)))))));
1530 sj10array[0]=IKsin(j10array[0]);
1531 cj10array[0]=IKcos(j10array[0]);
1532 if( j10array[0] > IKPI )
1533 {
1534  j10array[0]-=IK2PI;
1535 }
1536 else if( j10array[0] < -IKPI )
1537 { j10array[0]+=IK2PI;
1538 }
1539 j10valid[0] = true;
1540 for(int ij10 = 0; ij10 < 1; ++ij10)
1541 {
1542 if( !j10valid[ij10] )
1543 {
1544  continue;
1545 }
1546 _ij10[0] = ij10; _ij10[1] = -1;
1547 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
1548 {
1549 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
1550 {
1551  j10valid[iij10]=false; _ij10[1] = iij10; break;
1552 }
1553 }
1554 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
1555 {
1556 IkReal evalcond[4];
1557 IkReal x368=IKsin(j10);
1558 IkReal x369=IKcos(j10);
1559 IkReal x370=((IkReal(1.00000000000000))*(sj13));
1560 IkReal x371=((r11)*(sj9));
1561 IkReal x372=((cj9)*(r01));
1562 IkReal x373=((r21)*(sj14));
1563 IkReal x374=((cj9)*(r02));
1564 IkReal x375=((sj13)*(sj9));
1565 IkReal x376=((cj14)*(r10));
1566 IkReal x377=((IkReal(1.00000000000000))*(cj13));
1567 IkReal x378=((cj14)*(r20));
1568 IkReal x379=((cj11)*(x368));
1569 IkReal x380=((sj11)*(x369));
1570 IkReal x381=((sj14)*(x377));
1571 IkReal x382=((sj11)*(x368));
1572 IkReal x383=((cj11)*(x369));
1573 IkReal x384=((cj14)*(cj9)*(r00));
1574 IkReal x385=((x380)+(x379));
1575 evalcond[0]=((((IkReal(-1.00000000000000))*(x377)*(x378)))+(x382)+(((cj13)*(x373)))+(((IkReal(-1.00000000000000))*(x383)))+(((IkReal(-1.00000000000000))*(r22)*(x370))));
1576 evalcond[1]=((((IkReal(-1.00000000000000))*(x370)*(x378)))+(((sj13)*(x373)))+(x385)+(((cj13)*(r22))));
1577 evalcond[2]=((((cj13)*(x384)))+(((sj13)*(x374)))+(x385)+(((r12)*(x375)))+(((IkReal(-1.00000000000000))*(x371)*(x381)))+(((cj13)*(sj9)*(x376)))+(((IkReal(-1.00000000000000))*(x372)*(x381))));
1578 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x370)*(x371)))+(((IkReal(-1.00000000000000))*(sj14)*(x370)*(x372)))+(((x375)*(x376)))+(x383)+(((IkReal(-1.00000000000000))*(x374)*(x377)))+(((IkReal(-1.00000000000000))*(x382)))+(((sj13)*(x384)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x377))));
1579 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1580 {
1581 continue;
1582 }
1583 }
1584 
1585 {
1586 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1587 vinfos[0].jointtype = 1;
1588 vinfos[0].foffset = j9;
1589 vinfos[0].indices[0] = _ij9[0];
1590 vinfos[0].indices[1] = _ij9[1];
1591 vinfos[0].maxsolutions = _nj9;
1592 vinfos[1].jointtype = 1;
1593 vinfos[1].foffset = j10;
1594 vinfos[1].indices[0] = _ij10[0];
1595 vinfos[1].indices[1] = _ij10[1];
1596 vinfos[1].maxsolutions = _nj10;
1597 vinfos[2].jointtype = 1;
1598 vinfos[2].foffset = j11;
1599 vinfos[2].indices[0] = _ij11[0];
1600 vinfos[2].indices[1] = _ij11[1];
1601 vinfos[2].maxsolutions = _nj11;
1602 vinfos[3].jointtype = 1;
1603 vinfos[3].foffset = j12;
1604 vinfos[3].indices[0] = _ij12[0];
1605 vinfos[3].indices[1] = _ij12[1];
1606 vinfos[3].maxsolutions = _nj12;
1607 vinfos[4].jointtype = 1;
1608 vinfos[4].foffset = j13;
1609 vinfos[4].indices[0] = _ij13[0];
1610 vinfos[4].indices[1] = _ij13[1];
1611 vinfos[4].maxsolutions = _nj13;
1612 vinfos[5].jointtype = 1;
1613 vinfos[5].foffset = j14;
1614 vinfos[5].indices[0] = _ij14[0];
1615 vinfos[5].indices[1] = _ij14[1];
1616 vinfos[5].maxsolutions = _nj14;
1617 std::vector<int> vfree(0);
1618 solutions.AddSolution(vinfos,vfree);
1619 }
1620 }
1621 }
1622 
1623 }
1624 
1625 }
1626 }
1627 }
1628 
1629 }
1630 
1631 }
1632 }
1633 }
1634 
1635 } else
1636 {
1637 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
1638 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
1639 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
1640 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 )
1641 {
1642 {
1643 IkReal j11array[1], cj11array[1], sj11array[1];
1644 bool j11valid[1]={false};
1645 _nj11 = 1;
1646 IkReal x386=((IkReal(4.00000000000000))*(cj13));
1647 IkReal x387=((npy)*(sj14));
1648 IkReal x388=((cj14)*(npx));
1649 IkReal x389=((IkReal(4.00000000000000))*(sj13));
1650 if( IKabs(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x386)))+(((x388)*(x389)))+(((IkReal(-1.00000000000000))*(x387)*(x389)))+(((IkReal(-0.360000000000000))*(sj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((x386)*(x388)))+(((IkReal(-0.360000000000000))*(cj13)))+(((npz)*(x389)))+(((IkReal(-1.00000000000000))*(x386)*(x387))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x386)))+(((x388)*(x389)))+(((IkReal(-1.00000000000000))*(x387)*(x389)))+(((IkReal(-0.360000000000000))*(sj13)))))+IKsqr(((IkReal(-0.940000000000000))+(((x386)*(x388)))+(((IkReal(-0.360000000000000))*(cj13)))+(((npz)*(x389)))+(((IkReal(-1.00000000000000))*(x386)*(x387)))))-1) <= IKFAST_SINCOS_THRESH )
1651  continue;
1652 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x386)))+(((x388)*(x389)))+(((IkReal(-1.00000000000000))*(x387)*(x389)))+(((IkReal(-0.360000000000000))*(sj13)))), ((IkReal(-0.940000000000000))+(((x386)*(x388)))+(((IkReal(-0.360000000000000))*(cj13)))+(((npz)*(x389)))+(((IkReal(-1.00000000000000))*(x386)*(x387)))));
1653 sj11array[0]=IKsin(j11array[0]);
1654 cj11array[0]=IKcos(j11array[0]);
1655 if( j11array[0] > IKPI )
1656 {
1657  j11array[0]-=IK2PI;
1658 }
1659 else if( j11array[0] < -IKPI )
1660 { j11array[0]+=IK2PI;
1661 }
1662 j11valid[0] = true;
1663 for(int ij11 = 0; ij11 < 1; ++ij11)
1664 {
1665 if( !j11valid[ij11] )
1666 {
1667  continue;
1668 }
1669 _ij11[0] = ij11; _ij11[1] = -1;
1670 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
1671 {
1672 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
1673 {
1674  j11valid[iij11]=false; _ij11[1] = iij11; break;
1675 }
1676 }
1677 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
1678 {
1679 IkReal evalcond[2];
1680 IkReal x390=((IkReal(1.00000000000000))*(sj13));
1681 IkReal x391=((cj14)*(npx));
1682 IkReal x392=((npy)*(sj14));
1683 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(cj13)*(x391)))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x392)))+(((IkReal(-1.00000000000000))*(npz)*(x390))));
1684 evalcond[1]=((IkReal(-0.0300000000000000))+(((sj13)*(x392)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x390)*(x391)))+(((IkReal(0.250000000000000))*(IKsin(j11)))));
1685 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
1686 {
1687 continue;
1688 }
1689 }
1690 
1691 {
1692 IkReal dummyeval[1];
1693 IkReal gconst75;
1694 IkReal x393=(sj14)*(sj14);
1695 IkReal x394=(cj14)*(cj14);
1696 IkReal x395=((IkReal(1.00000000000000))*(x393));
1697 IkReal x396=((IkReal(2.00000000000000))*(cj14)*(sj14));
1698 IkReal x397=((IkReal(1.00000000000000))*(x394));
1699 gconst75=IKsign(((((IkReal(-1.00000000000000))*(r00)*(r01)*(x396)))+(((IkReal(-1.00000000000000))*(x397)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x396)))+(((IkReal(-1.00000000000000))*(x397)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x395)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x395)*((r00)*(r00))))));
1700 IkReal x398=(sj14)*(sj14);
1701 IkReal x399=(cj14)*(cj14);
1702 IkReal x400=((IkReal(1.00000000000000))*(x398));
1703 IkReal x401=((IkReal(2.00000000000000))*(cj14)*(sj14));
1704 IkReal x402=((IkReal(1.00000000000000))*(x399));
1705 dummyeval[0]=((((IkReal(-1.00000000000000))*(x400)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x400)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x402)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x401)))+(((IkReal(-1.00000000000000))*(x402)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x401))));
1706 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1707 {
1708 {
1709 IkReal dummyeval[1];
1710 IkReal gconst77;
1711 gconst77=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
1712 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
1713 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1714 {
1715 {
1716 IkReal dummyeval[1];
1717 IkReal gconst76;
1718 IkReal x403=(sj14)*(sj14);
1719 IkReal x404=(cj14)*(cj14);
1720 IkReal x405=((cj14)*(sj13));
1721 IkReal x406=((IkReal(1.00000000000000))*(r11));
1722 IkReal x407=((cj13)*(r00));
1723 IkReal x408=((sj13)*(sj14));
1724 IkReal x409=((cj13)*(r01)*(r10));
1725 gconst76=IKsign(((((r01)*(r12)*(x405)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x408)))+(((IkReal(-1.00000000000000))*(x404)*(x406)*(x407)))+(((x404)*(x409)))+(((IkReal(-1.00000000000000))*(x403)*(x406)*(x407)))+(((IkReal(-1.00000000000000))*(r02)*(x405)*(x406)))+(((x403)*(x409)))+(((r00)*(r12)*(x408)))));
1726 IkReal x410=(sj14)*(sj14);
1727 IkReal x411=(cj14)*(cj14);
1728 IkReal x412=((cj14)*(sj13));
1729 IkReal x413=((IkReal(1.00000000000000))*(r11));
1730 IkReal x414=((cj13)*(r00));
1731 IkReal x415=((sj13)*(sj14));
1732 IkReal x416=((cj13)*(r01)*(r10));
1733 dummyeval[0]=((((IkReal(-1.00000000000000))*(r02)*(r10)*(x415)))+(((IkReal(-1.00000000000000))*(x411)*(x413)*(x414)))+(((r01)*(r12)*(x412)))+(((x411)*(x416)))+(((x410)*(x416)))+(((IkReal(-1.00000000000000))*(r02)*(x412)*(x413)))+(((IkReal(-1.00000000000000))*(x410)*(x413)*(x414)))+(((r00)*(r12)*(x415))));
1734 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1735 {
1736 continue;
1737 
1738 } else
1739 {
1740 {
1741 IkReal j9array[1], cj9array[1], sj9array[1];
1742 bool j9valid[1]={false};
1743 _nj9 = 1;
1744 IkReal x417=((cj13)*(cj14));
1745 IkReal x418=((IkReal(1.00000000000000))*(cj13)*(sj14));
1746 if( IKabs(((gconst76)*(((((r12)*(sj13)))+(((r10)*(x417)))+(((IkReal(-1.00000000000000))*(r11)*(x418))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst76)*(((((r02)*(sj13)))+(((r00)*(x417)))+(((IkReal(-1.00000000000000))*(r01)*(x418))))))) < IKFAST_ATAN2_MAGTHRESH )
1747  continue;
1748 j9array[0]=IKatan2(((gconst76)*(((((r12)*(sj13)))+(((r10)*(x417)))+(((IkReal(-1.00000000000000))*(r11)*(x418)))))), ((gconst76)*(((((r02)*(sj13)))+(((r00)*(x417)))+(((IkReal(-1.00000000000000))*(r01)*(x418)))))));
1749 sj9array[0]=IKsin(j9array[0]);
1750 cj9array[0]=IKcos(j9array[0]);
1751 if( j9array[0] > IKPI )
1752 {
1753  j9array[0]-=IK2PI;
1754 }
1755 else if( j9array[0] < -IKPI )
1756 { j9array[0]+=IK2PI;
1757 }
1758 j9valid[0] = true;
1759 for(int ij9 = 0; ij9 < 1; ++ij9)
1760 {
1761 if( !j9valid[ij9] )
1762 {
1763  continue;
1764 }
1765 _ij9[0] = ij9; _ij9[1] = -1;
1766 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
1767 {
1768 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
1769 {
1770  j9valid[iij9]=false; _ij9[1] = iij9; break;
1771 }
1772 }
1773 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
1774 {
1775 IkReal evalcond[4];
1776 IkReal x419=IKsin(j9);
1777 IkReal x420=IKcos(j9);
1778 IkReal x421=((r10)*(sj14));
1779 IkReal x422=((cj14)*(r11));
1780 IkReal x423=((cj14)*(r10));
1781 IkReal x424=((cj14)*(r00));
1782 IkReal x425=((r11)*(sj14));
1783 IkReal x426=((r00)*(sj14));
1784 IkReal x427=((IkReal(1.00000000000000))*(x419));
1785 IkReal x428=((cj13)*(x419));
1786 IkReal x429=((sj13)*(x420));
1787 IkReal x430=((r01)*(x419));
1788 IkReal x431=((IkReal(1.00000000000000))*(x420));
1789 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x421)*(x431)))+(((x419)*(x426)))+(((IkReal(-1.00000000000000))*(x422)*(x431)))+(((cj14)*(x430))));
1790 evalcond[1]=((((IkReal(-1.00000000000000))*(x422)*(x427)))+(((IkReal(-1.00000000000000))*(x421)*(x427)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x431)))+(((IkReal(-1.00000000000000))*(x426)*(x431))));
1791 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x424)*(x427)))+(((cj13)*(x420)*(x423)))+(((IkReal(-1.00000000000000))*(cj13)*(x425)*(x431)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x427)))+(((r12)*(x429)))+(((r01)*(sj14)*(x428))));
1792 evalcond[3]=((((IkReal(-1.00000000000000))*(x425)*(x429)))+(((IkReal(-1.00000000000000))*(sj13)*(x424)*(x427)))+(((r02)*(x428)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x431)))+(((sj13)*(sj14)*(x430)))+(((x423)*(x429))));
1793 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1794 {
1795 continue;
1796 }
1797 }
1798 
1799 {
1800 IkReal dummyeval[1];
1801 IkReal gconst78;
1802 gconst78=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
1803 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
1804 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1805 {
1806 {
1807 IkReal dummyeval[1];
1808 IkReal gconst79;
1809 gconst79=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
1810 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
1811 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1812 {
1813 continue;
1814 
1815 } else
1816 {
1817 {
1818 IkReal j10array[1], cj10array[1], sj10array[1];
1819 bool j10valid[1]={false};
1820 _nj10 = 1;
1821 IkReal x432=((cj13)*(sj14));
1822 IkReal x433=((IkReal(1.00000000000000))*(cj11));
1823 IkReal x434=((r11)*(sj9));
1824 IkReal x435=((IkReal(1.00000000000000))*(sj11));
1825 IkReal x436=((cj13)*(cj14));
1826 IkReal x437=((cj11)*(sj13));
1827 IkReal x438=((r12)*(sj9));
1828 IkReal x439=((cj9)*(r01));
1829 IkReal x440=((sj11)*(sj13));
1830 IkReal x441=((cj9)*(r02));
1831 IkReal x442=((r10)*(sj9));
1832 IkReal x443=((cj9)*(r00));
1833 if( IKabs(((gconst79)*(((((IkReal(-1.00000000000000))*(x432)*(x433)*(x434)))+(((IkReal(-1.00000000000000))*(x432)*(x433)*(x439)))+(((IkReal(-1.00000000000000))*(r20)*(x435)*(x436)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x435)))+(((x437)*(x438)))+(((x437)*(x441)))+(((r21)*(sj11)*(x432)))+(((cj11)*(x436)*(x443)))+(((cj11)*(x436)*(x442))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst79)*(((((IkReal(-1.00000000000000))*(x432)*(x434)*(x435)))+(((cj11)*(r20)*(x436)))+(((x440)*(x441)))+(((r22)*(x437)))+(((x438)*(x440)))+(((IkReal(-1.00000000000000))*(r21)*(x432)*(x433)))+(((IkReal(-1.00000000000000))*(x432)*(x435)*(x439)))+(((sj11)*(x436)*(x442)))+(((sj11)*(x436)*(x443))))))) < IKFAST_ATAN2_MAGTHRESH )
1834  continue;
1835 j10array[0]=IKatan2(((gconst79)*(((((IkReal(-1.00000000000000))*(x432)*(x433)*(x434)))+(((IkReal(-1.00000000000000))*(x432)*(x433)*(x439)))+(((IkReal(-1.00000000000000))*(r20)*(x435)*(x436)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x435)))+(((x437)*(x438)))+(((x437)*(x441)))+(((r21)*(sj11)*(x432)))+(((cj11)*(x436)*(x443)))+(((cj11)*(x436)*(x442)))))), ((gconst79)*(((((IkReal(-1.00000000000000))*(x432)*(x434)*(x435)))+(((cj11)*(r20)*(x436)))+(((x440)*(x441)))+(((r22)*(x437)))+(((x438)*(x440)))+(((IkReal(-1.00000000000000))*(r21)*(x432)*(x433)))+(((IkReal(-1.00000000000000))*(x432)*(x435)*(x439)))+(((sj11)*(x436)*(x442)))+(((sj11)*(x436)*(x443)))))));
1836 sj10array[0]=IKsin(j10array[0]);
1837 cj10array[0]=IKcos(j10array[0]);
1838 if( j10array[0] > IKPI )
1839 {
1840  j10array[0]-=IK2PI;
1841 }
1842 else if( j10array[0] < -IKPI )
1843 { j10array[0]+=IK2PI;
1844 }
1845 j10valid[0] = true;
1846 for(int ij10 = 0; ij10 < 1; ++ij10)
1847 {
1848 if( !j10valid[ij10] )
1849 {
1850  continue;
1851 }
1852 _ij10[0] = ij10; _ij10[1] = -1;
1853 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
1854 {
1855 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
1856 {
1857  j10valid[iij10]=false; _ij10[1] = iij10; break;
1858 }
1859 }
1860 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
1861 {
1862 IkReal evalcond[4];
1863 IkReal x444=IKsin(j10);
1864 IkReal x445=IKcos(j10);
1865 IkReal x446=((IkReal(1.00000000000000))*(sj13));
1866 IkReal x447=((r11)*(sj9));
1867 IkReal x448=((cj9)*(r01));
1868 IkReal x449=((IkReal(1.00000000000000))*(cj11));
1869 IkReal x450=((r21)*(sj14));
1870 IkReal x451=((cj9)*(r02));
1871 IkReal x452=((sj13)*(sj9));
1872 IkReal x453=((cj14)*(r10));
1873 IkReal x454=((IkReal(1.00000000000000))*(cj13));
1874 IkReal x455=((cj14)*(r20));
1875 IkReal x456=((sj11)*(x444));
1876 IkReal x457=((sj14)*(x454));
1877 IkReal x458=((sj11)*(x445));
1878 IkReal x459=((cj14)*(cj9)*(r00));
1879 IkReal x460=((x445)*(x449));
1880 evalcond[0]=((((cj13)*(x450)))+(((IkReal(-1.00000000000000))*(x460)))+(((IkReal(-1.00000000000000))*(x454)*(x455)))+(x456)+(((IkReal(-1.00000000000000))*(r22)*(x446))));
1881 evalcond[1]=((((IkReal(-1.00000000000000))*(x446)*(x455)))+(((IkReal(-1.00000000000000))*(x444)*(x449)))+(((IkReal(-1.00000000000000))*(x458)))+(((sj13)*(x450)))+(((cj13)*(r22))));
1882 evalcond[2]=((((cj11)*(x444)))+(((cj13)*(x459)))+(((IkReal(-1.00000000000000))*(x447)*(x457)))+(x458)+(((r12)*(x452)))+(((sj13)*(x451)))+(((cj13)*(sj9)*(x453)))+(((IkReal(-1.00000000000000))*(x448)*(x457))));
1883 evalcond[3]=((((x452)*(x453)))+(((IkReal(-1.00000000000000))*(sj14)*(x446)*(x448)))+(((IkReal(-1.00000000000000))*(sj14)*(x446)*(x447)))+(((IkReal(-1.00000000000000))*(x460)))+(x456)+(((sj13)*(x459)))+(((IkReal(-1.00000000000000))*(x451)*(x454)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x454))));
1884 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1885 {
1886 continue;
1887 }
1888 }
1889 
1890 {
1891 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1892 vinfos[0].jointtype = 1;
1893 vinfos[0].foffset = j9;
1894 vinfos[0].indices[0] = _ij9[0];
1895 vinfos[0].indices[1] = _ij9[1];
1896 vinfos[0].maxsolutions = _nj9;
1897 vinfos[1].jointtype = 1;
1898 vinfos[1].foffset = j10;
1899 vinfos[1].indices[0] = _ij10[0];
1900 vinfos[1].indices[1] = _ij10[1];
1901 vinfos[1].maxsolutions = _nj10;
1902 vinfos[2].jointtype = 1;
1903 vinfos[2].foffset = j11;
1904 vinfos[2].indices[0] = _ij11[0];
1905 vinfos[2].indices[1] = _ij11[1];
1906 vinfos[2].maxsolutions = _nj11;
1907 vinfos[3].jointtype = 1;
1908 vinfos[3].foffset = j12;
1909 vinfos[3].indices[0] = _ij12[0];
1910 vinfos[3].indices[1] = _ij12[1];
1911 vinfos[3].maxsolutions = _nj12;
1912 vinfos[4].jointtype = 1;
1913 vinfos[4].foffset = j13;
1914 vinfos[4].indices[0] = _ij13[0];
1915 vinfos[4].indices[1] = _ij13[1];
1916 vinfos[4].maxsolutions = _nj13;
1917 vinfos[5].jointtype = 1;
1918 vinfos[5].foffset = j14;
1919 vinfos[5].indices[0] = _ij14[0];
1920 vinfos[5].indices[1] = _ij14[1];
1921 vinfos[5].maxsolutions = _nj14;
1922 std::vector<int> vfree(0);
1923 solutions.AddSolution(vinfos,vfree);
1924 }
1925 }
1926 }
1927 
1928 }
1929 
1930 }
1931 
1932 } else
1933 {
1934 {
1935 IkReal j10array[1], cj10array[1], sj10array[1];
1936 bool j10valid[1]={false};
1937 _nj10 = 1;
1938 IkReal x461=((r22)*(sj11));
1939 IkReal x462=((IkReal(1.00000000000000))*(cj13));
1940 IkReal x463=((cj14)*(r20));
1941 IkReal x464=((r21)*(sj14));
1942 IkReal x465=((cj11)*(cj13));
1943 IkReal x466=((IkReal(1.00000000000000))*(sj13));
1944 if( IKabs(((gconst78)*(((((r22)*(x465)))+(((IkReal(-1.00000000000000))*(sj11)*(x462)*(x464)))+(((cj11)*(sj13)*(x464)))+(((cj13)*(sj11)*(x463)))+(((IkReal(-1.00000000000000))*(cj11)*(x463)*(x466)))+(((sj13)*(x461))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst78)*(((((IkReal(-1.00000000000000))*(cj11)*(r22)*(x466)))+(((cj13)*(x461)))+(((sj11)*(sj13)*(x464)))+(((IkReal(-1.00000000000000))*(cj11)*(x462)*(x463)))+(((x464)*(x465)))+(((IkReal(-1.00000000000000))*(sj11)*(x463)*(x466))))))) < IKFAST_ATAN2_MAGTHRESH )
1945  continue;
1946 j10array[0]=IKatan2(((gconst78)*(((((r22)*(x465)))+(((IkReal(-1.00000000000000))*(sj11)*(x462)*(x464)))+(((cj11)*(sj13)*(x464)))+(((cj13)*(sj11)*(x463)))+(((IkReal(-1.00000000000000))*(cj11)*(x463)*(x466)))+(((sj13)*(x461)))))), ((gconst78)*(((((IkReal(-1.00000000000000))*(cj11)*(r22)*(x466)))+(((cj13)*(x461)))+(((sj11)*(sj13)*(x464)))+(((IkReal(-1.00000000000000))*(cj11)*(x462)*(x463)))+(((x464)*(x465)))+(((IkReal(-1.00000000000000))*(sj11)*(x463)*(x466)))))));
1947 sj10array[0]=IKsin(j10array[0]);
1948 cj10array[0]=IKcos(j10array[0]);
1949 if( j10array[0] > IKPI )
1950 {
1951  j10array[0]-=IK2PI;
1952 }
1953 else if( j10array[0] < -IKPI )
1954 { j10array[0]+=IK2PI;
1955 }
1956 j10valid[0] = true;
1957 for(int ij10 = 0; ij10 < 1; ++ij10)
1958 {
1959 if( !j10valid[ij10] )
1960 {
1961  continue;
1962 }
1963 _ij10[0] = ij10; _ij10[1] = -1;
1964 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
1965 {
1966 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
1967 {
1968  j10valid[iij10]=false; _ij10[1] = iij10; break;
1969 }
1970 }
1971 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
1972 {
1973 IkReal evalcond[4];
1974 IkReal x467=IKsin(j10);
1975 IkReal x468=IKcos(j10);
1976 IkReal x469=((IkReal(1.00000000000000))*(sj13));
1977 IkReal x470=((r11)*(sj9));
1978 IkReal x471=((cj9)*(r01));
1979 IkReal x472=((IkReal(1.00000000000000))*(cj11));
1980 IkReal x473=((r21)*(sj14));
1981 IkReal x474=((cj9)*(r02));
1982 IkReal x475=((sj13)*(sj9));
1983 IkReal x476=((cj14)*(r10));
1984 IkReal x477=((IkReal(1.00000000000000))*(cj13));
1985 IkReal x478=((cj14)*(r20));
1986 IkReal x479=((sj11)*(x467));
1987 IkReal x480=((sj14)*(x477));
1988 IkReal x481=((sj11)*(x468));
1989 IkReal x482=((cj14)*(cj9)*(r00));
1990 IkReal x483=((x468)*(x472));
1991 evalcond[0]=((((IkReal(-1.00000000000000))*(x477)*(x478)))+(((IkReal(-1.00000000000000))*(x483)))+(((cj13)*(x473)))+(x479)+(((IkReal(-1.00000000000000))*(r22)*(x469))));
1992 evalcond[1]=((((IkReal(-1.00000000000000))*(x467)*(x472)))+(((IkReal(-1.00000000000000))*(x469)*(x478)))+(((IkReal(-1.00000000000000))*(x481)))+(((sj13)*(x473)))+(((cj13)*(r22))));
1993 evalcond[2]=((((cj11)*(x467)))+(((cj13)*(sj9)*(x476)))+(((IkReal(-1.00000000000000))*(x470)*(x480)))+(((sj13)*(x474)))+(x481)+(((r12)*(x475)))+(((IkReal(-1.00000000000000))*(x471)*(x480)))+(((cj13)*(x482))));
1994 evalcond[3]=((((IkReal(-1.00000000000000))*(x483)))+(((sj13)*(x482)))+(((IkReal(-1.00000000000000))*(sj14)*(x469)*(x471)))+(((IkReal(-1.00000000000000))*(sj14)*(x469)*(x470)))+(x479)+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x477)))+(((x475)*(x476)))+(((IkReal(-1.00000000000000))*(x474)*(x477))));
1995 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1996 {
1997 continue;
1998 }
1999 }
2000 
2001 {
2002 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2003 vinfos[0].jointtype = 1;
2004 vinfos[0].foffset = j9;
2005 vinfos[0].indices[0] = _ij9[0];
2006 vinfos[0].indices[1] = _ij9[1];
2007 vinfos[0].maxsolutions = _nj9;
2008 vinfos[1].jointtype = 1;
2009 vinfos[1].foffset = j10;
2010 vinfos[1].indices[0] = _ij10[0];
2011 vinfos[1].indices[1] = _ij10[1];
2012 vinfos[1].maxsolutions = _nj10;
2013 vinfos[2].jointtype = 1;
2014 vinfos[2].foffset = j11;
2015 vinfos[2].indices[0] = _ij11[0];
2016 vinfos[2].indices[1] = _ij11[1];
2017 vinfos[2].maxsolutions = _nj11;
2018 vinfos[3].jointtype = 1;
2019 vinfos[3].foffset = j12;
2020 vinfos[3].indices[0] = _ij12[0];
2021 vinfos[3].indices[1] = _ij12[1];
2022 vinfos[3].maxsolutions = _nj12;
2023 vinfos[4].jointtype = 1;
2024 vinfos[4].foffset = j13;
2025 vinfos[4].indices[0] = _ij13[0];
2026 vinfos[4].indices[1] = _ij13[1];
2027 vinfos[4].maxsolutions = _nj13;
2028 vinfos[5].jointtype = 1;
2029 vinfos[5].foffset = j14;
2030 vinfos[5].indices[0] = _ij14[0];
2031 vinfos[5].indices[1] = _ij14[1];
2032 vinfos[5].maxsolutions = _nj14;
2033 std::vector<int> vfree(0);
2034 solutions.AddSolution(vinfos,vfree);
2035 }
2036 }
2037 }
2038 
2039 }
2040 
2041 }
2042 }
2043 }
2044 
2045 }
2046 
2047 }
2048 
2049 } else
2050 {
2051 {
2052 IkReal j10array[1], cj10array[1], sj10array[1];
2053 bool j10valid[1]={false};
2054 _nj10 = 1;
2055 IkReal x484=((cj13)*(sj11));
2056 IkReal x485=((r21)*(sj14));
2057 IkReal x486=((cj11)*(cj13));
2058 IkReal x487=((IkReal(1.00000000000000))*(sj13));
2059 IkReal x488=((sj11)*(sj13));
2060 IkReal x489=((cj14)*(r20));
2061 if( IKabs(((gconst77)*(((((IkReal(-1.00000000000000))*(cj11)*(x487)*(x489)))+(((x484)*(x489)))+(((IkReal(-1.00000000000000))*(x484)*(x485)))+(((r22)*(x488)))+(((r22)*(x486)))+(((cj11)*(sj13)*(x485))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst77)*(((((IkReal(-1.00000000000000))*(x486)*(x489)))+(((x485)*(x486)))+(((x485)*(x488)))+(((IkReal(-1.00000000000000))*(cj11)*(r22)*(x487)))+(((r22)*(x484)))+(((IkReal(-1.00000000000000))*(sj11)*(x487)*(x489))))))) < IKFAST_ATAN2_MAGTHRESH )
2062  continue;
2063 j10array[0]=IKatan2(((gconst77)*(((((IkReal(-1.00000000000000))*(cj11)*(x487)*(x489)))+(((x484)*(x489)))+(((IkReal(-1.00000000000000))*(x484)*(x485)))+(((r22)*(x488)))+(((r22)*(x486)))+(((cj11)*(sj13)*(x485)))))), ((gconst77)*(((((IkReal(-1.00000000000000))*(x486)*(x489)))+(((x485)*(x486)))+(((x485)*(x488)))+(((IkReal(-1.00000000000000))*(cj11)*(r22)*(x487)))+(((r22)*(x484)))+(((IkReal(-1.00000000000000))*(sj11)*(x487)*(x489)))))));
2064 sj10array[0]=IKsin(j10array[0]);
2065 cj10array[0]=IKcos(j10array[0]);
2066 if( j10array[0] > IKPI )
2067 {
2068  j10array[0]-=IK2PI;
2069 }
2070 else if( j10array[0] < -IKPI )
2071 { j10array[0]+=IK2PI;
2072 }
2073 j10valid[0] = true;
2074 for(int ij10 = 0; ij10 < 1; ++ij10)
2075 {
2076 if( !j10valid[ij10] )
2077 {
2078  continue;
2079 }
2080 _ij10[0] = ij10; _ij10[1] = -1;
2081 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
2082 {
2083 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
2084 {
2085  j10valid[iij10]=false; _ij10[1] = iij10; break;
2086 }
2087 }
2088 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
2089 {
2090 IkReal evalcond[2];
2091 IkReal x490=IKsin(j10);
2092 IkReal x491=IKcos(j10);
2093 IkReal x492=((IkReal(1.00000000000000))*(sj13));
2094 IkReal x493=((cj14)*(r20));
2095 IkReal x494=((r21)*(sj14));
2096 IkReal x495=((IkReal(1.00000000000000))*(x491));
2097 evalcond[0]=((((cj13)*(x494)))+(((IkReal(-1.00000000000000))*(cj11)*(x495)))+(((IkReal(-1.00000000000000))*(cj13)*(x493)))+(((IkReal(-1.00000000000000))*(r22)*(x492)))+(((sj11)*(x490))));
2098 evalcond[1]=((((IkReal(-1.00000000000000))*(sj11)*(x495)))+(((IkReal(-1.00000000000000))*(cj11)*(x490)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x492)*(x493)))+(((sj13)*(x494))));
2099 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
2100 {
2101 continue;
2102 }
2103 }
2104 
2105 {
2106 IkReal dummyeval[1];
2107 IkReal gconst80;
2108 IkReal x496=(sj14)*(sj14);
2109 IkReal x497=(cj14)*(cj14);
2110 IkReal x498=((IkReal(1.00000000000000))*(x496));
2111 IkReal x499=((IkReal(2.00000000000000))*(cj14)*(sj14));
2112 IkReal x500=((IkReal(1.00000000000000))*(x497));
2113 gconst80=IKsign(((((IkReal(-1.00000000000000))*(x498)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x499)))+(((IkReal(-1.00000000000000))*(x498)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x500)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x499)))+(((IkReal(-1.00000000000000))*(x500)*((r01)*(r01))))));
2114 IkReal x501=(sj14)*(sj14);
2115 IkReal x502=(cj14)*(cj14);
2116 IkReal x503=((IkReal(1.00000000000000))*(x501));
2117 IkReal x504=((IkReal(2.00000000000000))*(cj14)*(sj14));
2118 IkReal x505=((IkReal(1.00000000000000))*(x502));
2119 dummyeval[0]=((((IkReal(-1.00000000000000))*(x505)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x505)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x504)))+(((IkReal(-1.00000000000000))*(x503)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x504)))+(((IkReal(-1.00000000000000))*(x503)*((r10)*(r10)))));
2120 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2121 {
2122 {
2123 IkReal dummyeval[1];
2124 IkReal gconst81;
2125 IkReal x506=(sj14)*(sj14);
2126 IkReal x507=(cj14)*(cj14);
2127 IkReal x508=((cj14)*(sj13));
2128 IkReal x509=((IkReal(1.00000000000000))*(r11));
2129 IkReal x510=((cj13)*(r00));
2130 IkReal x511=((sj13)*(sj14));
2131 IkReal x512=((cj13)*(r01)*(r10));
2132 gconst81=IKsign(((((x507)*(x512)))+(((IkReal(-1.00000000000000))*(x506)*(x509)*(x510)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x511)))+(((r00)*(r12)*(x511)))+(((IkReal(-1.00000000000000))*(r02)*(x508)*(x509)))+(((IkReal(-1.00000000000000))*(x507)*(x509)*(x510)))+(((r01)*(r12)*(x508)))+(((x506)*(x512)))));
2133 IkReal x513=(sj14)*(sj14);
2134 IkReal x514=(cj14)*(cj14);
2135 IkReal x515=((cj14)*(sj13));
2136 IkReal x516=((IkReal(1.00000000000000))*(r11));
2137 IkReal x517=((cj13)*(r00));
2138 IkReal x518=((sj13)*(sj14));
2139 IkReal x519=((cj13)*(r01)*(r10));
2140 dummyeval[0]=((((r01)*(r12)*(x515)))+(((IkReal(-1.00000000000000))*(x514)*(x516)*(x517)))+(((x514)*(x519)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x518)))+(((x513)*(x519)))+(((r00)*(r12)*(x518)))+(((IkReal(-1.00000000000000))*(r02)*(x515)*(x516)))+(((IkReal(-1.00000000000000))*(x513)*(x516)*(x517))));
2141 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2142 {
2143 continue;
2144 
2145 } else
2146 {
2147 {
2148 IkReal j9array[1], cj9array[1], sj9array[1];
2149 bool j9valid[1]={false};
2150 _nj9 = 1;
2151 IkReal x520=((cj13)*(cj14));
2152 IkReal x521=((IkReal(1.00000000000000))*(cj13)*(sj14));
2153 if( IKabs(((gconst81)*(((((r12)*(sj13)))+(((r10)*(x520)))+(((IkReal(-1.00000000000000))*(r11)*(x521))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst81)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x521)))+(((r00)*(x520))))))) < IKFAST_ATAN2_MAGTHRESH )
2154  continue;
2155 j9array[0]=IKatan2(((gconst81)*(((((r12)*(sj13)))+(((r10)*(x520)))+(((IkReal(-1.00000000000000))*(r11)*(x521)))))), ((gconst81)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x521)))+(((r00)*(x520)))))));
2156 sj9array[0]=IKsin(j9array[0]);
2157 cj9array[0]=IKcos(j9array[0]);
2158 if( j9array[0] > IKPI )
2159 {
2160  j9array[0]-=IK2PI;
2161 }
2162 else if( j9array[0] < -IKPI )
2163 { j9array[0]+=IK2PI;
2164 }
2165 j9valid[0] = true;
2166 for(int ij9 = 0; ij9 < 1; ++ij9)
2167 {
2168 if( !j9valid[ij9] )
2169 {
2170  continue;
2171 }
2172 _ij9[0] = ij9; _ij9[1] = -1;
2173 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
2174 {
2175 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
2176 {
2177  j9valid[iij9]=false; _ij9[1] = iij9; break;
2178 }
2179 }
2180 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
2181 {
2182 IkReal evalcond[6];
2183 IkReal x522=IKsin(j9);
2184 IkReal x523=IKcos(j9);
2185 IkReal x524=((IkReal(1.00000000000000))*(cj14));
2186 IkReal x525=((IkReal(1.00000000000000))*(sj14));
2187 IkReal x526=((cj13)*(cj14));
2188 IkReal x527=((cj14)*(r10));
2189 IkReal x528=((r01)*(sj14));
2190 IkReal x529=((IkReal(1.00000000000000))*(r12));
2191 IkReal x530=((sj13)*(x523));
2192 IkReal x531=((r02)*(x522));
2193 IkReal x532=((r11)*(x522));
2194 IkReal x533=((r10)*(x523));
2195 IkReal x534=((r01)*(x523));
2196 IkReal x535=((sj13)*(x522));
2197 IkReal x536=((r11)*(x523));
2198 IkReal x537=((cj13)*(x522));
2199 IkReal x538=((r10)*(x522));
2200 IkReal x539=((r00)*(x523));
2201 IkReal x540=((cj13)*(x523));
2202 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x524)*(x536)))+(((IkReal(-1.00000000000000))*(x525)*(x533)))+(((cj14)*(r01)*(x522)))+(((r00)*(sj14)*(x522))));
2203 evalcond[1]=((((IkReal(-1.00000000000000))*(x524)*(x532)))+(((IkReal(-1.00000000000000))*(x524)*(x534)))+(((IkReal(-1.00000000000000))*(x525)*(x539)))+(((IkReal(-1.00000000000000))*(x525)*(x538))));
2204 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x525)*(x536)))+(((x526)*(x533)))+(((r12)*(x530)))+(((IkReal(-1.00000000000000))*(sj13)*(x531)))+(((x528)*(x537)))+(((IkReal(-1.00000000000000))*(r00)*(x524)*(x537))));
2205 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x525)*(x530)))+(((cj13)*(x531)))+(((IkReal(-1.00000000000000))*(x529)*(x540)))+(((x528)*(x535)))+(((x527)*(x530)))+(((IkReal(-1.00000000000000))*(r00)*(x524)*(x535))));
2206 evalcond[4]=((((r02)*(x530)))+(((IkReal(-1.00000000000000))*(cj13)*(x525)*(x534)))+(((IkReal(-1.00000000000000))*(cj13)*(x525)*(x532)))+(((x526)*(x538)))+(((x526)*(x539)))+(((r12)*(x535)))+(((cj10)*(sj11)))+(((cj11)*(sj10))));
2207 evalcond[5]=((((IkReal(-1.00000000000000))*(x529)*(x537)))+(((cj14)*(r00)*(x530)))+(((sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(r01)*(x525)*(x530)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11)))+(((x527)*(x535)))+(((IkReal(-1.00000000000000))*(sj13)*(x525)*(x532)))+(((IkReal(-1.00000000000000))*(r02)*(x540))));
2208 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 )
2209 {
2210 continue;
2211 }
2212 }
2213 
2214 {
2215 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2216 vinfos[0].jointtype = 1;
2217 vinfos[0].foffset = j9;
2218 vinfos[0].indices[0] = _ij9[0];
2219 vinfos[0].indices[1] = _ij9[1];
2220 vinfos[0].maxsolutions = _nj9;
2221 vinfos[1].jointtype = 1;
2222 vinfos[1].foffset = j10;
2223 vinfos[1].indices[0] = _ij10[0];
2224 vinfos[1].indices[1] = _ij10[1];
2225 vinfos[1].maxsolutions = _nj10;
2226 vinfos[2].jointtype = 1;
2227 vinfos[2].foffset = j11;
2228 vinfos[2].indices[0] = _ij11[0];
2229 vinfos[2].indices[1] = _ij11[1];
2230 vinfos[2].maxsolutions = _nj11;
2231 vinfos[3].jointtype = 1;
2232 vinfos[3].foffset = j12;
2233 vinfos[3].indices[0] = _ij12[0];
2234 vinfos[3].indices[1] = _ij12[1];
2235 vinfos[3].maxsolutions = _nj12;
2236 vinfos[4].jointtype = 1;
2237 vinfos[4].foffset = j13;
2238 vinfos[4].indices[0] = _ij13[0];
2239 vinfos[4].indices[1] = _ij13[1];
2240 vinfos[4].maxsolutions = _nj13;
2241 vinfos[5].jointtype = 1;
2242 vinfos[5].foffset = j14;
2243 vinfos[5].indices[0] = _ij14[0];
2244 vinfos[5].indices[1] = _ij14[1];
2245 vinfos[5].maxsolutions = _nj14;
2246 std::vector<int> vfree(0);
2247 solutions.AddSolution(vinfos,vfree);
2248 }
2249 }
2250 }
2251 
2252 }
2253 
2254 }
2255 
2256 } else
2257 {
2258 {
2259 IkReal j9array[1], cj9array[1], sj9array[1];
2260 bool j9valid[1]={false};
2261 _nj9 = 1;
2262 if( IKabs(((gconst80)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst80)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
2263  continue;
2264 j9array[0]=IKatan2(((gconst80)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst80)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
2265 sj9array[0]=IKsin(j9array[0]);
2266 cj9array[0]=IKcos(j9array[0]);
2267 if( j9array[0] > IKPI )
2268 {
2269  j9array[0]-=IK2PI;
2270 }
2271 else if( j9array[0] < -IKPI )
2272 { j9array[0]+=IK2PI;
2273 }
2274 j9valid[0] = true;
2275 for(int ij9 = 0; ij9 < 1; ++ij9)
2276 {
2277 if( !j9valid[ij9] )
2278 {
2279  continue;
2280 }
2281 _ij9[0] = ij9; _ij9[1] = -1;
2282 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
2283 {
2284 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
2285 {
2286  j9valid[iij9]=false; _ij9[1] = iij9; break;
2287 }
2288 }
2289 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
2290 {
2291 IkReal evalcond[6];
2292 IkReal x541=IKsin(j9);
2293 IkReal x542=IKcos(j9);
2294 IkReal x543=((IkReal(1.00000000000000))*(cj14));
2295 IkReal x544=((IkReal(1.00000000000000))*(sj14));
2296 IkReal x545=((cj13)*(cj14));
2297 IkReal x546=((cj14)*(r10));
2298 IkReal x547=((r01)*(sj14));
2299 IkReal x548=((IkReal(1.00000000000000))*(r12));
2300 IkReal x549=((sj13)*(x542));
2301 IkReal x550=((r02)*(x541));
2302 IkReal x551=((r11)*(x541));
2303 IkReal x552=((r10)*(x542));
2304 IkReal x553=((r01)*(x542));
2305 IkReal x554=((sj13)*(x541));
2306 IkReal x555=((r11)*(x542));
2307 IkReal x556=((cj13)*(x541));
2308 IkReal x557=((r10)*(x541));
2309 IkReal x558=((r00)*(x542));
2310 IkReal x559=((cj13)*(x542));
2311 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x544)*(x552)))+(((cj14)*(r01)*(x541)))+(((r00)*(sj14)*(x541)))+(((IkReal(-1.00000000000000))*(x543)*(x555))));
2312 evalcond[1]=((((IkReal(-1.00000000000000))*(x544)*(x558)))+(((IkReal(-1.00000000000000))*(x544)*(x557)))+(((IkReal(-1.00000000000000))*(x543)*(x553)))+(((IkReal(-1.00000000000000))*(x543)*(x551))));
2313 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x543)*(x556)))+(((IkReal(-1.00000000000000))*(sj13)*(x550)))+(((r12)*(x549)))+(((IkReal(-1.00000000000000))*(cj13)*(x544)*(x555)))+(((x547)*(x556)))+(((x545)*(x552))));
2314 evalcond[3]=((((IkReal(-1.00000000000000))*(r00)*(x543)*(x554)))+(((cj13)*(x550)))+(((IkReal(-1.00000000000000))*(r11)*(x544)*(x549)))+(((IkReal(-1.00000000000000))*(x548)*(x559)))+(((x547)*(x554)))+(((x546)*(x549))));
2315 evalcond[4]=((((r02)*(x549)))+(((r12)*(x554)))+(((cj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x544)*(x553)))+(((IkReal(-1.00000000000000))*(cj13)*(x544)*(x551)))+(((x545)*(x558)))+(((x545)*(x557)))+(((cj11)*(sj10))));
2316 evalcond[5]=((((IkReal(-1.00000000000000))*(sj13)*(x544)*(x551)))+(((IkReal(-1.00000000000000))*(r02)*(x559)))+(((cj14)*(r00)*(x549)))+(((IkReal(-1.00000000000000))*(r01)*(x544)*(x549)))+(((IkReal(-1.00000000000000))*(x548)*(x556)))+(((sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11)))+(((x546)*(x554))));
2317 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 )
2318 {
2319 continue;
2320 }
2321 }
2322 
2323 {
2324 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2325 vinfos[0].jointtype = 1;
2326 vinfos[0].foffset = j9;
2327 vinfos[0].indices[0] = _ij9[0];
2328 vinfos[0].indices[1] = _ij9[1];
2329 vinfos[0].maxsolutions = _nj9;
2330 vinfos[1].jointtype = 1;
2331 vinfos[1].foffset = j10;
2332 vinfos[1].indices[0] = _ij10[0];
2333 vinfos[1].indices[1] = _ij10[1];
2334 vinfos[1].maxsolutions = _nj10;
2335 vinfos[2].jointtype = 1;
2336 vinfos[2].foffset = j11;
2337 vinfos[2].indices[0] = _ij11[0];
2338 vinfos[2].indices[1] = _ij11[1];
2339 vinfos[2].maxsolutions = _nj11;
2340 vinfos[3].jointtype = 1;
2341 vinfos[3].foffset = j12;
2342 vinfos[3].indices[0] = _ij12[0];
2343 vinfos[3].indices[1] = _ij12[1];
2344 vinfos[3].maxsolutions = _nj12;
2345 vinfos[4].jointtype = 1;
2346 vinfos[4].foffset = j13;
2347 vinfos[4].indices[0] = _ij13[0];
2348 vinfos[4].indices[1] = _ij13[1];
2349 vinfos[4].maxsolutions = _nj13;
2350 vinfos[5].jointtype = 1;
2351 vinfos[5].foffset = j14;
2352 vinfos[5].indices[0] = _ij14[0];
2353 vinfos[5].indices[1] = _ij14[1];
2354 vinfos[5].maxsolutions = _nj14;
2355 std::vector<int> vfree(0);
2356 solutions.AddSolution(vinfos,vfree);
2357 }
2358 }
2359 }
2360 
2361 }
2362 
2363 }
2364 }
2365 }
2366 
2367 }
2368 
2369 }
2370 
2371 } else
2372 {
2373 {
2374 IkReal j9array[1], cj9array[1], sj9array[1];
2375 bool j9valid[1]={false};
2376 _nj9 = 1;
2377 if( IKabs(((gconst75)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst75)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
2378  continue;
2379 j9array[0]=IKatan2(((gconst75)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst75)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
2380 sj9array[0]=IKsin(j9array[0]);
2381 cj9array[0]=IKcos(j9array[0]);
2382 if( j9array[0] > IKPI )
2383 {
2384  j9array[0]-=IK2PI;
2385 }
2386 else if( j9array[0] < -IKPI )
2387 { j9array[0]+=IK2PI;
2388 }
2389 j9valid[0] = true;
2390 for(int ij9 = 0; ij9 < 1; ++ij9)
2391 {
2392 if( !j9valid[ij9] )
2393 {
2394  continue;
2395 }
2396 _ij9[0] = ij9; _ij9[1] = -1;
2397 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
2398 {
2399 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
2400 {
2401  j9valid[iij9]=false; _ij9[1] = iij9; break;
2402 }
2403 }
2404 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
2405 {
2406 IkReal evalcond[4];
2407 IkReal x560=IKsin(j9);
2408 IkReal x561=IKcos(j9);
2409 IkReal x562=((r10)*(sj14));
2410 IkReal x563=((cj14)*(r11));
2411 IkReal x564=((cj14)*(r10));
2412 IkReal x565=((cj14)*(r00));
2413 IkReal x566=((r11)*(sj14));
2414 IkReal x567=((r00)*(sj14));
2415 IkReal x568=((IkReal(1.00000000000000))*(x560));
2416 IkReal x569=((cj13)*(x560));
2417 IkReal x570=((sj13)*(x561));
2418 IkReal x571=((r01)*(x560));
2419 IkReal x572=((IkReal(1.00000000000000))*(x561));
2420 evalcond[0]=((IkReal(-1.00000000000000))+(((cj14)*(x571)))+(((x560)*(x567)))+(((IkReal(-1.00000000000000))*(x563)*(x572)))+(((IkReal(-1.00000000000000))*(x562)*(x572))));
2421 evalcond[1]=((((IkReal(-1.00000000000000))*(x562)*(x568)))+(((IkReal(-1.00000000000000))*(x567)*(x572)))+(((IkReal(-1.00000000000000))*(x563)*(x568)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x572))));
2422 evalcond[2]=((((r12)*(x570)))+(((IkReal(-1.00000000000000))*(cj13)*(x565)*(x568)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x568)))+(((IkReal(-1.00000000000000))*(cj13)*(x566)*(x572)))+(((r01)*(sj14)*(x569)))+(((cj13)*(x561)*(x564))));
2423 evalcond[3]=((((sj13)*(sj14)*(x571)))+(((IkReal(-1.00000000000000))*(x566)*(x570)))+(((IkReal(-1.00000000000000))*(sj13)*(x565)*(x568)))+(((x564)*(x570)))+(((r02)*(x569)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x572))));
2424 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2425 {
2426 continue;
2427 }
2428 }
2429 
2430 {
2431 IkReal dummyeval[1];
2432 IkReal gconst78;
2433 gconst78=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
2434 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
2435 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2436 {
2437 {
2438 IkReal dummyeval[1];
2439 IkReal gconst79;
2440 gconst79=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
2441 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
2442 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2443 {
2444 continue;
2445 
2446 } else
2447 {
2448 {
2449 IkReal j10array[1], cj10array[1], sj10array[1];
2450 bool j10valid[1]={false};
2451 _nj10 = 1;
2452 IkReal x573=((cj13)*(sj14));
2453 IkReal x574=((IkReal(1.00000000000000))*(cj11));
2454 IkReal x575=((r11)*(sj9));
2455 IkReal x576=((IkReal(1.00000000000000))*(sj11));
2456 IkReal x577=((cj13)*(cj14));
2457 IkReal x578=((cj11)*(sj13));
2458 IkReal x579=((r12)*(sj9));
2459 IkReal x580=((cj9)*(r01));
2460 IkReal x581=((sj11)*(sj13));
2461 IkReal x582=((cj9)*(r02));
2462 IkReal x583=((r10)*(sj9));
2463 IkReal x584=((cj9)*(r00));
2464 if( IKabs(((gconst79)*(((((x578)*(x582)))+(((x578)*(x579)))+(((cj11)*(x577)*(x584)))+(((cj11)*(x577)*(x583)))+(((IkReal(-1.00000000000000))*(x573)*(x574)*(x580)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x576)))+(((IkReal(-1.00000000000000))*(r20)*(x576)*(x577)))+(((IkReal(-1.00000000000000))*(x573)*(x574)*(x575)))+(((r21)*(sj11)*(x573))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst79)*(((((IkReal(-1.00000000000000))*(x573)*(x576)*(x580)))+(((x581)*(x582)))+(((x579)*(x581)))+(((sj11)*(x577)*(x584)))+(((sj11)*(x577)*(x583)))+(((cj11)*(r20)*(x577)))+(((IkReal(-1.00000000000000))*(x573)*(x575)*(x576)))+(((IkReal(-1.00000000000000))*(r21)*(x573)*(x574)))+(((r22)*(x578))))))) < IKFAST_ATAN2_MAGTHRESH )
2465  continue;
2466 j10array[0]=IKatan2(((gconst79)*(((((x578)*(x582)))+(((x578)*(x579)))+(((cj11)*(x577)*(x584)))+(((cj11)*(x577)*(x583)))+(((IkReal(-1.00000000000000))*(x573)*(x574)*(x580)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x576)))+(((IkReal(-1.00000000000000))*(r20)*(x576)*(x577)))+(((IkReal(-1.00000000000000))*(x573)*(x574)*(x575)))+(((r21)*(sj11)*(x573)))))), ((gconst79)*(((((IkReal(-1.00000000000000))*(x573)*(x576)*(x580)))+(((x581)*(x582)))+(((x579)*(x581)))+(((sj11)*(x577)*(x584)))+(((sj11)*(x577)*(x583)))+(((cj11)*(r20)*(x577)))+(((IkReal(-1.00000000000000))*(x573)*(x575)*(x576)))+(((IkReal(-1.00000000000000))*(r21)*(x573)*(x574)))+(((r22)*(x578)))))));
2467 sj10array[0]=IKsin(j10array[0]);
2468 cj10array[0]=IKcos(j10array[0]);
2469 if( j10array[0] > IKPI )
2470 {
2471  j10array[0]-=IK2PI;
2472 }
2473 else if( j10array[0] < -IKPI )
2474 { j10array[0]+=IK2PI;
2475 }
2476 j10valid[0] = true;
2477 for(int ij10 = 0; ij10 < 1; ++ij10)
2478 {
2479 if( !j10valid[ij10] )
2480 {
2481  continue;
2482 }
2483 _ij10[0] = ij10; _ij10[1] = -1;
2484 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
2485 {
2486 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
2487 {
2488  j10valid[iij10]=false; _ij10[1] = iij10; break;
2489 }
2490 }
2491 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
2492 {
2493 IkReal evalcond[4];
2494 IkReal x585=IKsin(j10);
2495 IkReal x586=IKcos(j10);
2496 IkReal x587=((IkReal(1.00000000000000))*(sj13));
2497 IkReal x588=((r11)*(sj9));
2498 IkReal x589=((cj9)*(r01));
2499 IkReal x590=((IkReal(1.00000000000000))*(cj11));
2500 IkReal x591=((r21)*(sj14));
2501 IkReal x592=((cj9)*(r02));
2502 IkReal x593=((sj13)*(sj9));
2503 IkReal x594=((cj14)*(r10));
2504 IkReal x595=((IkReal(1.00000000000000))*(cj13));
2505 IkReal x596=((cj14)*(r20));
2506 IkReal x597=((sj11)*(x585));
2507 IkReal x598=((sj14)*(x595));
2508 IkReal x599=((sj11)*(x586));
2509 IkReal x600=((cj14)*(cj9)*(r00));
2510 IkReal x601=((x586)*(x590));
2511 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x587)))+(((cj13)*(x591)))+(x597)+(((IkReal(-1.00000000000000))*(x595)*(x596)))+(((IkReal(-1.00000000000000))*(x601))));
2512 evalcond[1]=((((sj13)*(x591)))+(((IkReal(-1.00000000000000))*(x587)*(x596)))+(((IkReal(-1.00000000000000))*(x585)*(x590)))+(((IkReal(-1.00000000000000))*(x599)))+(((cj13)*(r22))));
2513 evalcond[2]=((((IkReal(-1.00000000000000))*(x588)*(x598)))+(((sj13)*(x592)))+(((IkReal(-1.00000000000000))*(x589)*(x598)))+(((cj13)*(sj9)*(x594)))+(((cj11)*(x585)))+(x599)+(((r12)*(x593)))+(((cj13)*(x600))));
2514 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x587)*(x589)))+(((IkReal(-1.00000000000000))*(sj14)*(x587)*(x588)))+(((sj13)*(x600)))+(((IkReal(-1.00000000000000))*(x592)*(x595)))+(x597)+(((IkReal(-1.00000000000000))*(x601)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x595)))+(((x593)*(x594))));
2515 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2516 {
2517 continue;
2518 }
2519 }
2520 
2521 {
2522 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2523 vinfos[0].jointtype = 1;
2524 vinfos[0].foffset = j9;
2525 vinfos[0].indices[0] = _ij9[0];
2526 vinfos[0].indices[1] = _ij9[1];
2527 vinfos[0].maxsolutions = _nj9;
2528 vinfos[1].jointtype = 1;
2529 vinfos[1].foffset = j10;
2530 vinfos[1].indices[0] = _ij10[0];
2531 vinfos[1].indices[1] = _ij10[1];
2532 vinfos[1].maxsolutions = _nj10;
2533 vinfos[2].jointtype = 1;
2534 vinfos[2].foffset = j11;
2535 vinfos[2].indices[0] = _ij11[0];
2536 vinfos[2].indices[1] = _ij11[1];
2537 vinfos[2].maxsolutions = _nj11;
2538 vinfos[3].jointtype = 1;
2539 vinfos[3].foffset = j12;
2540 vinfos[3].indices[0] = _ij12[0];
2541 vinfos[3].indices[1] = _ij12[1];
2542 vinfos[3].maxsolutions = _nj12;
2543 vinfos[4].jointtype = 1;
2544 vinfos[4].foffset = j13;
2545 vinfos[4].indices[0] = _ij13[0];
2546 vinfos[4].indices[1] = _ij13[1];
2547 vinfos[4].maxsolutions = _nj13;
2548 vinfos[5].jointtype = 1;
2549 vinfos[5].foffset = j14;
2550 vinfos[5].indices[0] = _ij14[0];
2551 vinfos[5].indices[1] = _ij14[1];
2552 vinfos[5].maxsolutions = _nj14;
2553 std::vector<int> vfree(0);
2554 solutions.AddSolution(vinfos,vfree);
2555 }
2556 }
2557 }
2558 
2559 }
2560 
2561 }
2562 
2563 } else
2564 {
2565 {
2566 IkReal j10array[1], cj10array[1], sj10array[1];
2567 bool j10valid[1]={false};
2568 _nj10 = 1;
2569 IkReal x602=((r22)*(sj11));
2570 IkReal x603=((IkReal(1.00000000000000))*(cj13));
2571 IkReal x604=((cj14)*(r20));
2572 IkReal x605=((r21)*(sj14));
2573 IkReal x606=((cj11)*(cj13));
2574 IkReal x607=((IkReal(1.00000000000000))*(sj13));
2575 if( IKabs(((gconst78)*(((((IkReal(-1.00000000000000))*(sj11)*(x603)*(x605)))+(((sj13)*(x602)))+(((cj13)*(sj11)*(x604)))+(((cj11)*(sj13)*(x605)))+(((IkReal(-1.00000000000000))*(cj11)*(x604)*(x607)))+(((r22)*(x606))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst78)*(((((x605)*(x606)))+(((IkReal(-1.00000000000000))*(cj11)*(r22)*(x607)))+(((sj11)*(sj13)*(x605)))+(((IkReal(-1.00000000000000))*(cj11)*(x603)*(x604)))+(((cj13)*(x602)))+(((IkReal(-1.00000000000000))*(sj11)*(x604)*(x607))))))) < IKFAST_ATAN2_MAGTHRESH )
2576  continue;
2577 j10array[0]=IKatan2(((gconst78)*(((((IkReal(-1.00000000000000))*(sj11)*(x603)*(x605)))+(((sj13)*(x602)))+(((cj13)*(sj11)*(x604)))+(((cj11)*(sj13)*(x605)))+(((IkReal(-1.00000000000000))*(cj11)*(x604)*(x607)))+(((r22)*(x606)))))), ((gconst78)*(((((x605)*(x606)))+(((IkReal(-1.00000000000000))*(cj11)*(r22)*(x607)))+(((sj11)*(sj13)*(x605)))+(((IkReal(-1.00000000000000))*(cj11)*(x603)*(x604)))+(((cj13)*(x602)))+(((IkReal(-1.00000000000000))*(sj11)*(x604)*(x607)))))));
2578 sj10array[0]=IKsin(j10array[0]);
2579 cj10array[0]=IKcos(j10array[0]);
2580 if( j10array[0] > IKPI )
2581 {
2582  j10array[0]-=IK2PI;
2583 }
2584 else if( j10array[0] < -IKPI )
2585 { j10array[0]+=IK2PI;
2586 }
2587 j10valid[0] = true;
2588 for(int ij10 = 0; ij10 < 1; ++ij10)
2589 {
2590 if( !j10valid[ij10] )
2591 {
2592  continue;
2593 }
2594 _ij10[0] = ij10; _ij10[1] = -1;
2595 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
2596 {
2597 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
2598 {
2599  j10valid[iij10]=false; _ij10[1] = iij10; break;
2600 }
2601 }
2602 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
2603 {
2604 IkReal evalcond[4];
2605 IkReal x608=IKsin(j10);
2606 IkReal x609=IKcos(j10);
2607 IkReal x610=((IkReal(1.00000000000000))*(sj13));
2608 IkReal x611=((r11)*(sj9));
2609 IkReal x612=((cj9)*(r01));
2610 IkReal x613=((IkReal(1.00000000000000))*(cj11));
2611 IkReal x614=((r21)*(sj14));
2612 IkReal x615=((cj9)*(r02));
2613 IkReal x616=((sj13)*(sj9));
2614 IkReal x617=((cj14)*(r10));
2615 IkReal x618=((IkReal(1.00000000000000))*(cj13));
2616 IkReal x619=((cj14)*(r20));
2617 IkReal x620=((sj11)*(x608));
2618 IkReal x621=((sj14)*(x618));
2619 IkReal x622=((sj11)*(x609));
2620 IkReal x623=((cj14)*(cj9)*(r00));
2621 IkReal x624=((x609)*(x613));
2622 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x610)))+(((IkReal(-1.00000000000000))*(x618)*(x619)))+(((IkReal(-1.00000000000000))*(x624)))+(x620)+(((cj13)*(x614))));
2623 evalcond[1]=((((IkReal(-1.00000000000000))*(x610)*(x619)))+(((sj13)*(x614)))+(((IkReal(-1.00000000000000))*(x622)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x608)*(x613))));
2624 evalcond[2]=((((cj13)*(x623)))+(((sj13)*(x615)))+(((cj11)*(x608)))+(((IkReal(-1.00000000000000))*(x611)*(x621)))+(((cj13)*(sj9)*(x617)))+(((r12)*(x616)))+(((IkReal(-1.00000000000000))*(x612)*(x621)))+(x622));
2625 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x610)*(x612)))+(((IkReal(-1.00000000000000))*(sj14)*(x610)*(x611)))+(((IkReal(-1.00000000000000))*(x624)))+(((x616)*(x617)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x618)))+(x620)+(((IkReal(-1.00000000000000))*(x615)*(x618)))+(((sj13)*(x623))));
2626 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2627 {
2628 continue;
2629 }
2630 }
2631 
2632 {
2633 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2634 vinfos[0].jointtype = 1;
2635 vinfos[0].foffset = j9;
2636 vinfos[0].indices[0] = _ij9[0];
2637 vinfos[0].indices[1] = _ij9[1];
2638 vinfos[0].maxsolutions = _nj9;
2639 vinfos[1].jointtype = 1;
2640 vinfos[1].foffset = j10;
2641 vinfos[1].indices[0] = _ij10[0];
2642 vinfos[1].indices[1] = _ij10[1];
2643 vinfos[1].maxsolutions = _nj10;
2644 vinfos[2].jointtype = 1;
2645 vinfos[2].foffset = j11;
2646 vinfos[2].indices[0] = _ij11[0];
2647 vinfos[2].indices[1] = _ij11[1];
2648 vinfos[2].maxsolutions = _nj11;
2649 vinfos[3].jointtype = 1;
2650 vinfos[3].foffset = j12;
2651 vinfos[3].indices[0] = _ij12[0];
2652 vinfos[3].indices[1] = _ij12[1];
2653 vinfos[3].maxsolutions = _nj12;
2654 vinfos[4].jointtype = 1;
2655 vinfos[4].foffset = j13;
2656 vinfos[4].indices[0] = _ij13[0];
2657 vinfos[4].indices[1] = _ij13[1];
2658 vinfos[4].maxsolutions = _nj13;
2659 vinfos[5].jointtype = 1;
2660 vinfos[5].foffset = j14;
2661 vinfos[5].indices[0] = _ij14[0];
2662 vinfos[5].indices[1] = _ij14[1];
2663 vinfos[5].maxsolutions = _nj14;
2664 std::vector<int> vfree(0);
2665 solutions.AddSolution(vinfos,vfree);
2666 }
2667 }
2668 }
2669 
2670 }
2671 
2672 }
2673 }
2674 }
2675 
2676 }
2677 
2678 }
2679 }
2680 }
2681 
2682 } else
2683 {
2684 if( 1 )
2685 {
2686 continue;
2687 
2688 } else
2689 {
2690 }
2691 }
2692 }
2693 }
2694 
2695 } else
2696 {
2697 {
2698 IkReal j11array[1], cj11array[1], sj11array[1];
2699 bool j11valid[1]={false};
2700 _nj11 = 1;
2701 IkReal x625=((IkReal(4.00000000000000))*(sj14));
2702 IkReal x626=((IkReal(4.00000000000000))*(cj14));
2703 if( IKabs(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npy)*(x626)))+(((IkReal(-1.00000000000000))*(npx)*(x625))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x626)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x625))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npy)*(x626)))+(((IkReal(-1.00000000000000))*(npx)*(x625)))))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x626)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x625)))))-1) <= IKFAST_SINCOS_THRESH )
2704  continue;
2705 j11array[0]=IKatan2(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npy)*(x626)))+(((IkReal(-1.00000000000000))*(npx)*(x625)))))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x626)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x625)))));
2706 sj11array[0]=IKsin(j11array[0]);
2707 cj11array[0]=IKcos(j11array[0]);
2708 if( j11array[0] > IKPI )
2709 {
2710  j11array[0]-=IK2PI;
2711 }
2712 else if( j11array[0] < -IKPI )
2713 { j11array[0]+=IK2PI;
2714 }
2715 j11valid[0] = true;
2716 for(int ij11 = 0; ij11 < 1; ++ij11)
2717 {
2718 if( !j11valid[ij11] )
2719 {
2720  continue;
2721 }
2722 _ij11[0] = ij11; _ij11[1] = -1;
2723 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
2724 {
2725 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
2726 {
2727  j11valid[iij11]=false; _ij11[1] = iij11; break;
2728 }
2729 }
2730 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
2731 {
2732 IkReal evalcond[3];
2733 IkReal x627=IKsin(j11);
2734 IkReal x628=((IkReal(1.00000000000000))*(sj13));
2735 IkReal x629=((cj14)*(npx));
2736 IkReal x630=((npy)*(sj14));
2737 IkReal x631=((IkReal(0.250000000000000))*(x627));
2738 evalcond[0]=((((IkReal(0.0950000000000000))*(cj12)))+(((cj14)*(npy)))+(((IkReal(-0.0300000000000000))*(sj12)))+(((npx)*(sj14)))+(((sj12)*(x631))));
2739 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(cj13)*(x629)))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x630)))+(((IkReal(-1.00000000000000))*(npz)*(x628))));
2740 evalcond[2]=((((IkReal(0.0950000000000000))*(sj12)))+(((IkReal(-1.00000000000000))*(x628)*(x629)))+(((cj13)*(npz)))+(((IkReal(0.0300000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(cj12)*(x631)))+(((IkReal(0.0900000000000000))*(sj13)))+(((sj13)*(x630))));
2741 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
2742 {
2743 continue;
2744 }
2745 }
2746 
2747 {
2748 IkReal dummyeval[1];
2749 IkReal gconst4;
2750 gconst4=IKsign(((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11))))));
2751 dummyeval[0]=((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11)))));
2752 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2753 {
2754 {
2755 IkReal dummyeval[1];
2756 IkReal gconst5;
2757 gconst5=IKsign(((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11))))));
2758 dummyeval[0]=((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11)))));
2759 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2760 {
2761 {
2762 IkReal dummyeval[1];
2763 IkReal gconst2;
2764 IkReal x632=(cj14)*(cj14);
2765 IkReal x633=(sj14)*(sj14);
2766 IkReal x634=((IkReal(1.00000000000000))*(r01));
2767 IkReal x635=((sj13)*(sj14));
2768 IkReal x636=((cj14)*(sj13));
2769 IkReal x637=((r00)*(r11));
2770 IkReal x638=((cj13)*(x632));
2771 IkReal x639=((cj13)*(x633));
2772 gconst2=IKsign(((((r02)*(r11)*(x636)))+(((r02)*(r10)*(x635)))+(((IkReal(-1.00000000000000))*(r12)*(x634)*(x636)))+(((IkReal(-1.00000000000000))*(r10)*(x634)*(x639)))+(((IkReal(-1.00000000000000))*(r10)*(x634)*(x638)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x635)))+(((x637)*(x639)))+(((x637)*(x638)))));
2773 IkReal x640=(cj14)*(cj14);
2774 IkReal x641=(sj14)*(sj14);
2775 IkReal x642=((IkReal(1.00000000000000))*(r01));
2776 IkReal x643=((sj13)*(sj14));
2777 IkReal x644=((cj14)*(sj13));
2778 IkReal x645=((r00)*(r11));
2779 IkReal x646=((cj13)*(x640));
2780 IkReal x647=((cj13)*(x641));
2781 dummyeval[0]=((((r02)*(r10)*(x643)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x643)))+(((IkReal(-1.00000000000000))*(r12)*(x642)*(x644)))+(((x645)*(x647)))+(((x645)*(x646)))+(((IkReal(-1.00000000000000))*(r10)*(x642)*(x647)))+(((IkReal(-1.00000000000000))*(r10)*(x642)*(x646)))+(((r02)*(r11)*(x644))));
2782 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2783 {
2784 {
2785 IkReal evalcond[5];
2786 IkReal x648=((IkReal(1.00000000000000))*(sj13));
2787 IkReal x649=((npy)*(sj14));
2788 IkReal x650=((IkReal(1.00000000000000))*(cj14)*(npx));
2789 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
2790 evalcond[1]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(sj11))));
2791 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x648)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x650)))+(((cj13)*(x649))));
2792 evalcond[3]=((((r21)*(sj13)*(sj14)))+(((IkReal(-1.00000000000000))*(cj14)*(r20)*(x648)))+(((cj13)*(r22))));
2793 evalcond[4]=((IkReal(0.0950000000000000))+(((IkReal(-1.00000000000000))*(cj14)*(npx)*(x648)))+(((sj13)*(x649)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
2794 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 )
2795 {
2796 {
2797 IkReal dummyeval[1];
2798 IkReal gconst20;
2799 gconst20=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
2800 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
2801 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2802 {
2803 {
2804 IkReal dummyeval[1];
2805 IkReal gconst18;
2806 IkReal x651=(sj14)*(sj14);
2807 IkReal x652=(cj14)*(cj14);
2808 IkReal x653=((r01)*(r10));
2809 IkReal x654=((cj13)*(cj14));
2810 IkReal x655=((IkReal(1.00000000000000))*(r12));
2811 IkReal x656=((cj13)*(sj14));
2812 IkReal x657=((sj13)*(x651));
2813 IkReal x658=((IkReal(1.00000000000000))*(r00)*(r11));
2814 IkReal x659=((sj13)*(x652));
2815 gconst18=IKsign(((((IkReal(-1.00000000000000))*(r00)*(x655)*(x656)))+(((IkReal(-1.00000000000000))*(x657)*(x658)))+(((x653)*(x657)))+(((x653)*(x659)))+(((r02)*(r10)*(x656)))+(((IkReal(-1.00000000000000))*(r01)*(x654)*(x655)))+(((IkReal(-1.00000000000000))*(x658)*(x659)))+(((r02)*(r11)*(x654)))));
2816 IkReal x660=(sj14)*(sj14);
2817 IkReal x661=(cj14)*(cj14);
2818 IkReal x662=((r01)*(r10));
2819 IkReal x663=((cj13)*(cj14));
2820 IkReal x664=((IkReal(1.00000000000000))*(r12));
2821 IkReal x665=((cj13)*(sj14));
2822 IkReal x666=((sj13)*(x660));
2823 IkReal x667=((IkReal(1.00000000000000))*(r00)*(r11));
2824 IkReal x668=((sj13)*(x661));
2825 dummyeval[0]=((((x662)*(x668)))+(((x662)*(x666)))+(((IkReal(-1.00000000000000))*(r01)*(x663)*(x664)))+(((IkReal(-1.00000000000000))*(r00)*(x664)*(x665)))+(((r02)*(r11)*(x663)))+(((IkReal(-1.00000000000000))*(x666)*(x667)))+(((IkReal(-1.00000000000000))*(x667)*(x668)))+(((r02)*(r10)*(x665))));
2826 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2827 {
2828 {
2829 IkReal dummyeval[1];
2830 IkReal gconst19;
2831 IkReal x669=(sj13)*(sj13);
2832 IkReal x670=(cj13)*(cj13);
2833 IkReal x671=((cj14)*(r00));
2834 IkReal x672=((r11)*(sj14));
2835 IkReal x673=((r12)*(x670));
2836 IkReal x674=((IkReal(1.00000000000000))*(r01)*(sj14));
2837 IkReal x675=((r02)*(x669));
2838 IkReal x676=((IkReal(1.00000000000000))*(cj14)*(r10));
2839 IkReal x677=((r12)*(x669));
2840 IkReal x678=((r02)*(x670));
2841 gconst19=IKsign(((((x672)*(x675)))+(((x672)*(x678)))+(((IkReal(-1.00000000000000))*(x675)*(x676)))+(((x671)*(x673)))+(((x671)*(x677)))+(((IkReal(-1.00000000000000))*(x676)*(x678)))+(((IkReal(-1.00000000000000))*(x674)*(x677)))+(((IkReal(-1.00000000000000))*(x673)*(x674)))));
2842 IkReal x679=(sj13)*(sj13);
2843 IkReal x680=(cj13)*(cj13);
2844 IkReal x681=((cj14)*(r00));
2845 IkReal x682=((r11)*(sj14));
2846 IkReal x683=((r12)*(x680));
2847 IkReal x684=((IkReal(1.00000000000000))*(r01)*(sj14));
2848 IkReal x685=((r02)*(x679));
2849 IkReal x686=((IkReal(1.00000000000000))*(cj14)*(r10));
2850 IkReal x687=((r12)*(x679));
2851 IkReal x688=((r02)*(x680));
2852 dummyeval[0]=((((IkReal(-1.00000000000000))*(x685)*(x686)))+(((x682)*(x688)))+(((x682)*(x685)))+(((IkReal(-1.00000000000000))*(x686)*(x688)))+(((IkReal(-1.00000000000000))*(x684)*(x687)))+(((x681)*(x683)))+(((x681)*(x687)))+(((IkReal(-1.00000000000000))*(x683)*(x684))));
2853 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2854 {
2855 continue;
2856 
2857 } else
2858 {
2859 {
2860 IkReal j9array[1], cj9array[1], sj9array[1];
2861 bool j9valid[1]={false};
2862 _nj9 = 1;
2863 IkReal x689=((cj13)*(cj14));
2864 IkReal x690=((IkReal(1.00000000000000))*(cj13)*(sj14));
2865 if( IKabs(((gconst19)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x690)))+(((r10)*(x689))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst19)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x690)))+(((r00)*(x689))))))) < IKFAST_ATAN2_MAGTHRESH )
2866  continue;
2867 j9array[0]=IKatan2(((gconst19)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x690)))+(((r10)*(x689)))))), ((gconst19)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x690)))+(((r00)*(x689)))))));
2868 sj9array[0]=IKsin(j9array[0]);
2869 cj9array[0]=IKcos(j9array[0]);
2870 if( j9array[0] > IKPI )
2871 {
2872  j9array[0]-=IK2PI;
2873 }
2874 else if( j9array[0] < -IKPI )
2875 { j9array[0]+=IK2PI;
2876 }
2877 j9valid[0] = true;
2878 for(int ij9 = 0; ij9 < 1; ++ij9)
2879 {
2880 if( !j9valid[ij9] )
2881 {
2882  continue;
2883 }
2884 _ij9[0] = ij9; _ij9[1] = -1;
2885 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
2886 {
2887 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
2888 {
2889  j9valid[iij9]=false; _ij9[1] = iij9; break;
2890 }
2891 }
2892 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
2893 {
2894 IkReal evalcond[4];
2895 IkReal x691=IKsin(j9);
2896 IkReal x692=IKcos(j9);
2897 IkReal x693=((IkReal(1.00000000000000))*(sj14));
2898 IkReal x694=((cj13)*(cj14));
2899 IkReal x695=((IkReal(1.00000000000000))*(r00));
2900 IkReal x696=((r01)*(sj14));
2901 IkReal x697=((IkReal(1.00000000000000))*(cj13));
2902 IkReal x698=((sj13)*(x692));
2903 IkReal x699=((r02)*(x691));
2904 IkReal x700=((r10)*(x692));
2905 IkReal x701=((r11)*(x692));
2906 IkReal x702=((sj13)*(x691));
2907 IkReal x703=((cj14)*(x702));
2908 evalcond[0]=((((IkReal(-1.00000000000000))*(x693)*(x700)))+(((r00)*(sj14)*(x691)))+(((cj14)*(r01)*(x691)))+(((IkReal(-1.00000000000000))*(cj14)*(x701))));
2909 evalcond[1]=((((IkReal(-1.00000000000000))*(x691)*(x694)*(x695)))+(((x694)*(x700)))+(((IkReal(-1.00000000000000))*(sj13)*(x699)))+(((r12)*(x698)))+(((cj13)*(x691)*(x696)))+(((IkReal(-1.00000000000000))*(cj13)*(x693)*(x701))));
2910 evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r11)*(x693)*(x698)))+(((IkReal(-1.00000000000000))*(x695)*(x703)))+(((cj13)*(x699)))+(((cj14)*(r10)*(x698)))+(((IkReal(-1.00000000000000))*(r12)*(x692)*(x697)))+(((x696)*(x702))));
2911 evalcond[3]=((((cj14)*(r00)*(x698)))+(((IkReal(-1.00000000000000))*(r02)*(x692)*(x697)))+(((IkReal(-1.00000000000000))*(r01)*(x693)*(x698)))+(((IkReal(-1.00000000000000))*(r12)*(x691)*(x697)))+(((IkReal(-1.00000000000000))*(r11)*(x693)*(x702)))+(((r10)*(x703))));
2912 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2913 {
2914 continue;
2915 }
2916 }
2917 
2918 {
2919 IkReal dummyeval[1];
2920 IkReal gconst21;
2921 gconst21=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
2922 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
2923 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2924 {
2925 {
2926 IkReal dummyeval[1];
2927 IkReal gconst22;
2928 gconst22=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
2929 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
2930 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2931 {
2932 continue;
2933 
2934 } else
2935 {
2936 {
2937 IkReal j10array[1], cj10array[1], sj10array[1];
2938 bool j10valid[1]={false};
2939 _nj10 = 1;
2940 IkReal x704=((IkReal(1.00000000000000))*(cj11));
2941 IkReal x705=((r20)*(sj14));
2942 IkReal x706=((cj14)*(r21));
2943 IkReal x707=((cj14)*(cj9)*(r01));
2944 IkReal x708=((r10)*(sj14)*(sj9));
2945 IkReal x709=((cj9)*(r00)*(sj14));
2946 IkReal x710=((cj14)*(r11)*(sj9));
2947 if( IKabs(((gconst22)*(((((sj11)*(x708)))+(((sj11)*(x709)))+(((sj11)*(x707)))+(((sj11)*(x710)))+(((cj11)*(x706)))+(((cj11)*(x705))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst22)*(((((sj11)*(x705)))+(((sj11)*(x706)))+(((IkReal(-1.00000000000000))*(x704)*(x710)))+(((IkReal(-1.00000000000000))*(x704)*(x707)))+(((IkReal(-1.00000000000000))*(x704)*(x709)))+(((IkReal(-1.00000000000000))*(x704)*(x708))))))) < IKFAST_ATAN2_MAGTHRESH )
2948  continue;
2949 j10array[0]=IKatan2(((gconst22)*(((((sj11)*(x708)))+(((sj11)*(x709)))+(((sj11)*(x707)))+(((sj11)*(x710)))+(((cj11)*(x706)))+(((cj11)*(x705)))))), ((gconst22)*(((((sj11)*(x705)))+(((sj11)*(x706)))+(((IkReal(-1.00000000000000))*(x704)*(x710)))+(((IkReal(-1.00000000000000))*(x704)*(x707)))+(((IkReal(-1.00000000000000))*(x704)*(x709)))+(((IkReal(-1.00000000000000))*(x704)*(x708)))))));
2950 sj10array[0]=IKsin(j10array[0]);
2951 cj10array[0]=IKcos(j10array[0]);
2952 if( j10array[0] > IKPI )
2953 {
2954  j10array[0]-=IK2PI;
2955 }
2956 else if( j10array[0] < -IKPI )
2957 { j10array[0]+=IK2PI;
2958 }
2959 j10valid[0] = true;
2960 for(int ij10 = 0; ij10 < 1; ++ij10)
2961 {
2962 if( !j10valid[ij10] )
2963 {
2964  continue;
2965 }
2966 _ij10[0] = ij10; _ij10[1] = -1;
2967 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
2968 {
2969 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
2970 {
2971  j10valid[iij10]=false; _ij10[1] = iij10; break;
2972 }
2973 }
2974 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
2975 {
2976 IkReal evalcond[4];
2977 IkReal x711=IKsin(j10);
2978 IkReal x712=IKcos(j10);
2979 IkReal x713=((cj13)*(sj14));
2980 IkReal x714=((cj13)*(cj14));
2981 IkReal x715=((r10)*(sj9));
2982 IkReal x716=((IkReal(1.00000000000000))*(cj9));
2983 IkReal x717=((sj11)*(x711));
2984 IkReal x718=((IkReal(1.00000000000000))*(x712));
2985 IkReal x719=((cj11)*(x711));
2986 IkReal x720=((IkReal(1.00000000000000))*(r11)*(sj9));
2987 IkReal x721=((cj11)*(x718));
2988 evalcond[0]=((((IkReal(-1.00000000000000))*(sj11)*(x718)))+(((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x719))));
2989 evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x714)))+(((IkReal(-1.00000000000000))*(x721)))+(((r21)*(x713)))+(x717));
2990 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(x720)))+(((IkReal(-1.00000000000000))*(x721)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x716)))+(((IkReal(-1.00000000000000))*(sj14)*(x715)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x716)))+(x717));
2991 evalcond[3]=((((IkReal(-1.00000000000000))*(x713)*(x720)))+(((x714)*(x715)))+(((sj11)*(x712)))+(((IkReal(-1.00000000000000))*(r01)*(x713)*(x716)))+(((cj9)*(r00)*(x714)))+(((cj9)*(r02)*(sj13)))+(x719)+(((r12)*(sj13)*(sj9))));
2992 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2993 {
2994 continue;
2995 }
2996 }
2997 
2998 {
2999 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3000 vinfos[0].jointtype = 1;
3001 vinfos[0].foffset = j9;
3002 vinfos[0].indices[0] = _ij9[0];
3003 vinfos[0].indices[1] = _ij9[1];
3004 vinfos[0].maxsolutions = _nj9;
3005 vinfos[1].jointtype = 1;
3006 vinfos[1].foffset = j10;
3007 vinfos[1].indices[0] = _ij10[0];
3008 vinfos[1].indices[1] = _ij10[1];
3009 vinfos[1].maxsolutions = _nj10;
3010 vinfos[2].jointtype = 1;
3011 vinfos[2].foffset = j11;
3012 vinfos[2].indices[0] = _ij11[0];
3013 vinfos[2].indices[1] = _ij11[1];
3014 vinfos[2].maxsolutions = _nj11;
3015 vinfos[3].jointtype = 1;
3016 vinfos[3].foffset = j12;
3017 vinfos[3].indices[0] = _ij12[0];
3018 vinfos[3].indices[1] = _ij12[1];
3019 vinfos[3].maxsolutions = _nj12;
3020 vinfos[4].jointtype = 1;
3021 vinfos[4].foffset = j13;
3022 vinfos[4].indices[0] = _ij13[0];
3023 vinfos[4].indices[1] = _ij13[1];
3024 vinfos[4].maxsolutions = _nj13;
3025 vinfos[5].jointtype = 1;
3026 vinfos[5].foffset = j14;
3027 vinfos[5].indices[0] = _ij14[0];
3028 vinfos[5].indices[1] = _ij14[1];
3029 vinfos[5].maxsolutions = _nj14;
3030 std::vector<int> vfree(0);
3031 solutions.AddSolution(vinfos,vfree);
3032 }
3033 }
3034 }
3035 
3036 }
3037 
3038 }
3039 
3040 } else
3041 {
3042 {
3043 IkReal j10array[1], cj10array[1], sj10array[1];
3044 bool j10valid[1]={false};
3045 _nj10 = 1;
3046 IkReal x722=((cj11)*(r20));
3047 IkReal x723=((IkReal(1.00000000000000))*(cj13));
3048 IkReal x724=((r21)*(sj14));
3049 IkReal x725=((r22)*(sj13));
3050 IkReal x726=((cj14)*(sj11));
3051 if( IKabs(((gconst21)*(((((sj14)*(x722)))+(((cj11)*(cj14)*(r21)))+(((cj13)*(r20)*(x726)))+(((IkReal(-1.00000000000000))*(sj11)*(x723)*(x724)))+(((sj11)*(x725))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst21)*(((((IkReal(-1.00000000000000))*(cj11)*(x725)))+(((cj11)*(cj13)*(x724)))+(((r20)*(sj11)*(sj14)))+(((r21)*(x726)))+(((IkReal(-1.00000000000000))*(cj14)*(x722)*(x723))))))) < IKFAST_ATAN2_MAGTHRESH )
3052  continue;
3053 j10array[0]=IKatan2(((gconst21)*(((((sj14)*(x722)))+(((cj11)*(cj14)*(r21)))+(((cj13)*(r20)*(x726)))+(((IkReal(-1.00000000000000))*(sj11)*(x723)*(x724)))+(((sj11)*(x725)))))), ((gconst21)*(((((IkReal(-1.00000000000000))*(cj11)*(x725)))+(((cj11)*(cj13)*(x724)))+(((r20)*(sj11)*(sj14)))+(((r21)*(x726)))+(((IkReal(-1.00000000000000))*(cj14)*(x722)*(x723)))))));
3054 sj10array[0]=IKsin(j10array[0]);
3055 cj10array[0]=IKcos(j10array[0]);
3056 if( j10array[0] > IKPI )
3057 {
3058  j10array[0]-=IK2PI;
3059 }
3060 else if( j10array[0] < -IKPI )
3061 { j10array[0]+=IK2PI;
3062 }
3063 j10valid[0] = true;
3064 for(int ij10 = 0; ij10 < 1; ++ij10)
3065 {
3066 if( !j10valid[ij10] )
3067 {
3068  continue;
3069 }
3070 _ij10[0] = ij10; _ij10[1] = -1;
3071 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
3072 {
3073 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
3074 {
3075  j10valid[iij10]=false; _ij10[1] = iij10; break;
3076 }
3077 }
3078 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
3079 {
3080 IkReal evalcond[4];
3081 IkReal x727=IKsin(j10);
3082 IkReal x728=IKcos(j10);
3083 IkReal x729=((cj13)*(sj14));
3084 IkReal x730=((cj13)*(cj14));
3085 IkReal x731=((r10)*(sj9));
3086 IkReal x732=((IkReal(1.00000000000000))*(cj9));
3087 IkReal x733=((sj11)*(x727));
3088 IkReal x734=((IkReal(1.00000000000000))*(x728));
3089 IkReal x735=((cj11)*(x727));
3090 IkReal x736=((IkReal(1.00000000000000))*(r11)*(sj9));
3091 IkReal x737=((cj11)*(x734));
3092 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x734)))+(((IkReal(-1.00000000000000))*(x735))));
3093 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x730)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x737)))+(x733)+(((r21)*(x729))));
3094 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(x736)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x732)))+(((IkReal(-1.00000000000000))*(sj14)*(x731)))+(((IkReal(-1.00000000000000))*(x737)))+(x733)+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x732))));
3095 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x729)*(x732)))+(((cj9)*(r00)*(x730)))+(((cj9)*(r02)*(sj13)))+(x735)+(((r12)*(sj13)*(sj9)))+(((sj11)*(x728)))+(((IkReal(-1.00000000000000))*(x729)*(x736)))+(((x730)*(x731))));
3096 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
3097 {
3098 continue;
3099 }
3100 }
3101 
3102 {
3103 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3104 vinfos[0].jointtype = 1;
3105 vinfos[0].foffset = j9;
3106 vinfos[0].indices[0] = _ij9[0];
3107 vinfos[0].indices[1] = _ij9[1];
3108 vinfos[0].maxsolutions = _nj9;
3109 vinfos[1].jointtype = 1;
3110 vinfos[1].foffset = j10;
3111 vinfos[1].indices[0] = _ij10[0];
3112 vinfos[1].indices[1] = _ij10[1];
3113 vinfos[1].maxsolutions = _nj10;
3114 vinfos[2].jointtype = 1;
3115 vinfos[2].foffset = j11;
3116 vinfos[2].indices[0] = _ij11[0];
3117 vinfos[2].indices[1] = _ij11[1];
3118 vinfos[2].maxsolutions = _nj11;
3119 vinfos[3].jointtype = 1;
3120 vinfos[3].foffset = j12;
3121 vinfos[3].indices[0] = _ij12[0];
3122 vinfos[3].indices[1] = _ij12[1];
3123 vinfos[3].maxsolutions = _nj12;
3124 vinfos[4].jointtype = 1;
3125 vinfos[4].foffset = j13;
3126 vinfos[4].indices[0] = _ij13[0];
3127 vinfos[4].indices[1] = _ij13[1];
3128 vinfos[4].maxsolutions = _nj13;
3129 vinfos[5].jointtype = 1;
3130 vinfos[5].foffset = j14;
3131 vinfos[5].indices[0] = _ij14[0];
3132 vinfos[5].indices[1] = _ij14[1];
3133 vinfos[5].maxsolutions = _nj14;
3134 std::vector<int> vfree(0);
3135 solutions.AddSolution(vinfos,vfree);
3136 }
3137 }
3138 }
3139 
3140 }
3141 
3142 }
3143 }
3144 }
3145 
3146 }
3147 
3148 }
3149 
3150 } else
3151 {
3152 {
3153 IkReal j9array[1], cj9array[1], sj9array[1];
3154 bool j9valid[1]={false};
3155 _nj9 = 1;
3156 IkReal x738=((IkReal(1.00000000000000))*(cj14));
3157 IkReal x739=((IkReal(1.00000000000000))*(sj14));
3158 if( IKabs(((gconst18)*(((((IkReal(-1.00000000000000))*(r10)*(x739)))+(((IkReal(-1.00000000000000))*(r11)*(x738))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst18)*(((((IkReal(-1.00000000000000))*(r01)*(x738)))+(((IkReal(-1.00000000000000))*(r00)*(x739))))))) < IKFAST_ATAN2_MAGTHRESH )
3159  continue;
3160 j9array[0]=IKatan2(((gconst18)*(((((IkReal(-1.00000000000000))*(r10)*(x739)))+(((IkReal(-1.00000000000000))*(r11)*(x738)))))), ((gconst18)*(((((IkReal(-1.00000000000000))*(r01)*(x738)))+(((IkReal(-1.00000000000000))*(r00)*(x739)))))));
3161 sj9array[0]=IKsin(j9array[0]);
3162 cj9array[0]=IKcos(j9array[0]);
3163 if( j9array[0] > IKPI )
3164 {
3165  j9array[0]-=IK2PI;
3166 }
3167 else if( j9array[0] < -IKPI )
3168 { j9array[0]+=IK2PI;
3169 }
3170 j9valid[0] = true;
3171 for(int ij9 = 0; ij9 < 1; ++ij9)
3172 {
3173 if( !j9valid[ij9] )
3174 {
3175  continue;
3176 }
3177 _ij9[0] = ij9; _ij9[1] = -1;
3178 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
3179 {
3180 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
3181 {
3182  j9valid[iij9]=false; _ij9[1] = iij9; break;
3183 }
3184 }
3185 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
3186 {
3187 IkReal evalcond[4];
3188 IkReal x740=IKsin(j9);
3189 IkReal x741=IKcos(j9);
3190 IkReal x742=((IkReal(1.00000000000000))*(sj14));
3191 IkReal x743=((cj13)*(cj14));
3192 IkReal x744=((IkReal(1.00000000000000))*(r00));
3193 IkReal x745=((r01)*(sj14));
3194 IkReal x746=((IkReal(1.00000000000000))*(cj13));
3195 IkReal x747=((sj13)*(x741));
3196 IkReal x748=((r02)*(x740));
3197 IkReal x749=((r10)*(x741));
3198 IkReal x750=((r11)*(x741));
3199 IkReal x751=((sj13)*(x740));
3200 IkReal x752=((cj14)*(x751));
3201 evalcond[0]=((((cj14)*(r01)*(x740)))+(((r00)*(sj14)*(x740)))+(((IkReal(-1.00000000000000))*(x742)*(x749)))+(((IkReal(-1.00000000000000))*(cj14)*(x750))));
3202 evalcond[1]=((((IkReal(-1.00000000000000))*(x740)*(x743)*(x744)))+(((cj13)*(x740)*(x745)))+(((IkReal(-1.00000000000000))*(sj13)*(x748)))+(((x743)*(x749)))+(((r12)*(x747)))+(((IkReal(-1.00000000000000))*(cj13)*(x742)*(x750))));
3203 evalcond[2]=((IkReal(1.00000000000000))+(((x745)*(x751)))+(((IkReal(-1.00000000000000))*(r11)*(x742)*(x747)))+(((cj13)*(x748)))+(((cj14)*(r10)*(x747)))+(((IkReal(-1.00000000000000))*(r12)*(x741)*(x746)))+(((IkReal(-1.00000000000000))*(x744)*(x752))));
3204 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x742)*(x747)))+(((IkReal(-1.00000000000000))*(r12)*(x740)*(x746)))+(((r10)*(x752)))+(((IkReal(-1.00000000000000))*(r11)*(x742)*(x751)))+(((IkReal(-1.00000000000000))*(r02)*(x741)*(x746)))+(((cj14)*(r00)*(x747))));
3205 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
3206 {
3207 continue;
3208 }
3209 }
3210 
3211 {
3212 IkReal dummyeval[1];
3213 IkReal gconst21;
3214 gconst21=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
3215 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
3216 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
3217 {
3218 {
3219 IkReal dummyeval[1];
3220 IkReal gconst22;
3221 gconst22=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
3222 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
3223 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
3224 {
3225 continue;
3226 
3227 } else
3228 {
3229 {
3230 IkReal j10array[1], cj10array[1], sj10array[1];
3231 bool j10valid[1]={false};
3232 _nj10 = 1;
3233 IkReal x753=((IkReal(1.00000000000000))*(cj11));
3234 IkReal x754=((r20)*(sj14));
3235 IkReal x755=((cj14)*(r21));
3236 IkReal x756=((cj14)*(cj9)*(r01));
3237 IkReal x757=((r10)*(sj14)*(sj9));
3238 IkReal x758=((cj9)*(r00)*(sj14));
3239 IkReal x759=((cj14)*(r11)*(sj9));
3240 if( IKabs(((gconst22)*(((((cj11)*(x755)))+(((cj11)*(x754)))+(((sj11)*(x756)))+(((sj11)*(x757)))+(((sj11)*(x758)))+(((sj11)*(x759))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst22)*(((((IkReal(-1.00000000000000))*(x753)*(x757)))+(((IkReal(-1.00000000000000))*(x753)*(x756)))+(((IkReal(-1.00000000000000))*(x753)*(x758)))+(((IkReal(-1.00000000000000))*(x753)*(x759)))+(((sj11)*(x754)))+(((sj11)*(x755))))))) < IKFAST_ATAN2_MAGTHRESH )
3241  continue;
3242 j10array[0]=IKatan2(((gconst22)*(((((cj11)*(x755)))+(((cj11)*(x754)))+(((sj11)*(x756)))+(((sj11)*(x757)))+(((sj11)*(x758)))+(((sj11)*(x759)))))), ((gconst22)*(((((IkReal(-1.00000000000000))*(x753)*(x757)))+(((IkReal(-1.00000000000000))*(x753)*(x756)))+(((IkReal(-1.00000000000000))*(x753)*(x758)))+(((IkReal(-1.00000000000000))*(x753)*(x759)))+(((sj11)*(x754)))+(((sj11)*(x755)))))));
3243 sj10array[0]=IKsin(j10array[0]);
3244 cj10array[0]=IKcos(j10array[0]);
3245 if( j10array[0] > IKPI )
3246 {
3247  j10array[0]-=IK2PI;
3248 }
3249 else if( j10array[0] < -IKPI )
3250 { j10array[0]+=IK2PI;
3251 }
3252 j10valid[0] = true;
3253 for(int ij10 = 0; ij10 < 1; ++ij10)
3254 {
3255 if( !j10valid[ij10] )
3256 {
3257  continue;
3258 }
3259 _ij10[0] = ij10; _ij10[1] = -1;
3260 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
3261 {
3262 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
3263 {
3264  j10valid[iij10]=false; _ij10[1] = iij10; break;
3265 }
3266 }
3267 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
3268 {
3269 IkReal evalcond[4];
3270 IkReal x760=IKsin(j10);
3271 IkReal x761=IKcos(j10);
3272 IkReal x762=((cj13)*(sj14));
3273 IkReal x763=((cj13)*(cj14));
3274 IkReal x764=((r10)*(sj9));
3275 IkReal x765=((IkReal(1.00000000000000))*(cj9));
3276 IkReal x766=((sj11)*(x760));
3277 IkReal x767=((IkReal(1.00000000000000))*(x761));
3278 IkReal x768=((cj11)*(x760));
3279 IkReal x769=((IkReal(1.00000000000000))*(r11)*(sj9));
3280 IkReal x770=((cj11)*(x767));
3281 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x768)))+(((IkReal(-1.00000000000000))*(sj11)*(x767))));
3282 evalcond[1]=((((r21)*(x762)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x763)))+(x766)+(((IkReal(-1.00000000000000))*(x770))));
3283 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(sj14)*(x765)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x765)))+(((IkReal(-1.00000000000000))*(cj14)*(x769)))+(x766)+(((IkReal(-1.00000000000000))*(x770)))+(((IkReal(-1.00000000000000))*(sj14)*(x764))));
3284 evalcond[3]=((((sj11)*(x761)))+(((cj9)*(r00)*(x763)))+(((IkReal(-1.00000000000000))*(x762)*(x769)))+(((cj9)*(r02)*(sj13)))+(((x763)*(x764)))+(((IkReal(-1.00000000000000))*(r01)*(x762)*(x765)))+(x768)+(((r12)*(sj13)*(sj9))));
3285 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
3286 {
3287 continue;
3288 }
3289 }
3290 
3291 {
3292 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3293 vinfos[0].jointtype = 1;
3294 vinfos[0].foffset = j9;
3295 vinfos[0].indices[0] = _ij9[0];
3296 vinfos[0].indices[1] = _ij9[1];
3297 vinfos[0].maxsolutions = _nj9;
3298 vinfos[1].jointtype = 1;
3299 vinfos[1].foffset = j10;
3300 vinfos[1].indices[0] = _ij10[0];
3301 vinfos[1].indices[1] = _ij10[1];
3302 vinfos[1].maxsolutions = _nj10;
3303 vinfos[2].jointtype = 1;
3304 vinfos[2].foffset = j11;
3305 vinfos[2].indices[0] = _ij11[0];
3306 vinfos[2].indices[1] = _ij11[1];
3307 vinfos[2].maxsolutions = _nj11;
3308 vinfos[3].jointtype = 1;
3309 vinfos[3].foffset = j12;
3310 vinfos[3].indices[0] = _ij12[0];
3311 vinfos[3].indices[1] = _ij12[1];
3312 vinfos[3].maxsolutions = _nj12;
3313 vinfos[4].jointtype = 1;
3314 vinfos[4].foffset = j13;
3315 vinfos[4].indices[0] = _ij13[0];
3316 vinfos[4].indices[1] = _ij13[1];
3317 vinfos[4].maxsolutions = _nj13;
3318 vinfos[5].jointtype = 1;
3319 vinfos[5].foffset = j14;
3320 vinfos[5].indices[0] = _ij14[0];
3321 vinfos[5].indices[1] = _ij14[1];
3322 vinfos[5].maxsolutions = _nj14;
3323 std::vector<int> vfree(0);
3324 solutions.AddSolution(vinfos,vfree);
3325 }
3326 }
3327 }
3328 
3329 }
3330 
3331 }
3332 
3333 } else
3334 {
3335 {
3336 IkReal j10array[1], cj10array[1], sj10array[1];
3337 bool j10valid[1]={false};
3338 _nj10 = 1;
3339 IkReal x771=((cj11)*(r20));
3340 IkReal x772=((IkReal(1.00000000000000))*(cj13));
3341 IkReal x773=((r21)*(sj14));
3342 IkReal x774=((r22)*(sj13));
3343 IkReal x775=((cj14)*(sj11));
3344 if( IKabs(((gconst21)*(((((cj11)*(cj14)*(r21)))+(((cj13)*(r20)*(x775)))+(((sj11)*(x774)))+(((IkReal(-1.00000000000000))*(sj11)*(x772)*(x773)))+(((sj14)*(x771))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst21)*(((((IkReal(-1.00000000000000))*(cj11)*(x774)))+(((cj11)*(cj13)*(x773)))+(((r21)*(x775)))+(((r20)*(sj11)*(sj14)))+(((IkReal(-1.00000000000000))*(cj14)*(x771)*(x772))))))) < IKFAST_ATAN2_MAGTHRESH )
3345  continue;
3346 j10array[0]=IKatan2(((gconst21)*(((((cj11)*(cj14)*(r21)))+(((cj13)*(r20)*(x775)))+(((sj11)*(x774)))+(((IkReal(-1.00000000000000))*(sj11)*(x772)*(x773)))+(((sj14)*(x771)))))), ((gconst21)*(((((IkReal(-1.00000000000000))*(cj11)*(x774)))+(((cj11)*(cj13)*(x773)))+(((r21)*(x775)))+(((r20)*(sj11)*(sj14)))+(((IkReal(-1.00000000000000))*(cj14)*(x771)*(x772)))))));
3347 sj10array[0]=IKsin(j10array[0]);
3348 cj10array[0]=IKcos(j10array[0]);
3349 if( j10array[0] > IKPI )
3350 {
3351  j10array[0]-=IK2PI;
3352 }
3353 else if( j10array[0] < -IKPI )
3354 { j10array[0]+=IK2PI;
3355 }
3356 j10valid[0] = true;
3357 for(int ij10 = 0; ij10 < 1; ++ij10)
3358 {
3359 if( !j10valid[ij10] )
3360 {
3361  continue;
3362 }
3363 _ij10[0] = ij10; _ij10[1] = -1;
3364 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
3365 {
3366 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
3367 {
3368  j10valid[iij10]=false; _ij10[1] = iij10; break;
3369 }
3370 }
3371 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
3372 {
3373 IkReal evalcond[4];
3374 IkReal x776=IKsin(j10);
3375 IkReal x777=IKcos(j10);
3376 IkReal x778=((cj13)*(sj14));
3377 IkReal x779=((cj13)*(cj14));
3378 IkReal x780=((r10)*(sj9));
3379 IkReal x781=((IkReal(1.00000000000000))*(cj9));
3380 IkReal x782=((sj11)*(x776));
3381 IkReal x783=((IkReal(1.00000000000000))*(x777));
3382 IkReal x784=((cj11)*(x776));
3383 IkReal x785=((IkReal(1.00000000000000))*(r11)*(sj9));
3384 IkReal x786=((cj11)*(x783));
3385 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x784)))+(((IkReal(-1.00000000000000))*(sj11)*(x783))));
3386 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x779)))+(((IkReal(-1.00000000000000))*(x786)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x778)))+(x782));
3387 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(sj14)*(x781)))+(((IkReal(-1.00000000000000))*(x786)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x781)))+(((IkReal(-1.00000000000000))*(cj14)*(x785)))+(x782)+(((IkReal(-1.00000000000000))*(sj14)*(x780))));
3388 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x778)*(x781)))+(((cj9)*(r02)*(sj13)))+(((x779)*(x780)))+(((cj9)*(r00)*(x779)))+(((sj11)*(x777)))+(x784)+(((IkReal(-1.00000000000000))*(x778)*(x785)))+(((r12)*(sj13)*(sj9))));
3389 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
3390 {
3391 continue;
3392 }
3393 }
3394 
3395 {
3396 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3397 vinfos[0].jointtype = 1;
3398 vinfos[0].foffset = j9;
3399 vinfos[0].indices[0] = _ij9[0];
3400 vinfos[0].indices[1] = _ij9[1];
3401 vinfos[0].maxsolutions = _nj9;
3402 vinfos[1].jointtype = 1;
3403 vinfos[1].foffset = j10;
3404 vinfos[1].indices[0] = _ij10[0];
3405 vinfos[1].indices[1] = _ij10[1];
3406 vinfos[1].maxsolutions = _nj10;
3407 vinfos[2].jointtype = 1;
3408 vinfos[2].foffset = j11;
3409 vinfos[2].indices[0] = _ij11[0];
3410 vinfos[2].indices[1] = _ij11[1];
3411 vinfos[2].maxsolutions = _nj11;
3412 vinfos[3].jointtype = 1;
3413 vinfos[3].foffset = j12;
3414 vinfos[3].indices[0] = _ij12[0];
3415 vinfos[3].indices[1] = _ij12[1];
3416 vinfos[3].maxsolutions = _nj12;
3417 vinfos[4].jointtype = 1;
3418 vinfos[4].foffset = j13;
3419 vinfos[4].indices[0] = _ij13[0];
3420 vinfos[4].indices[1] = _ij13[1];
3421 vinfos[4].maxsolutions = _nj13;
3422 vinfos[5].jointtype = 1;
3423 vinfos[5].foffset = j14;
3424 vinfos[5].indices[0] = _ij14[0];
3425 vinfos[5].indices[1] = _ij14[1];
3426 vinfos[5].maxsolutions = _nj14;
3427 std::vector<int> vfree(0);
3428 solutions.AddSolution(vinfos,vfree);
3429 }
3430 }
3431 }
3432 
3433 }
3434 
3435 }
3436 }
3437 }
3438 
3439 }
3440 
3441 }
3442 
3443 } else
3444 {
3445 {
3446 IkReal j10array[1], cj10array[1], sj10array[1];
3447 bool j10valid[1]={false};
3448 _nj10 = 1;
3449 IkReal x787=((cj11)*(r20));
3450 IkReal x788=((IkReal(1.00000000000000))*(cj13));
3451 IkReal x789=((r21)*(sj14));
3452 IkReal x790=((r22)*(sj13));
3453 IkReal x791=((r20)*(sj11));
3454 IkReal x792=((cj14)*(r21));
3455 if( IKabs(((gconst20)*(((((cj11)*(x792)))+(((IkReal(-1.00000000000000))*(sj11)*(x788)*(x789)))+(((cj13)*(cj14)*(x791)))+(((sj11)*(x790)))+(((sj14)*(x787))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst20)*(((((IkReal(-1.00000000000000))*(cj11)*(x790)))+(((cj11)*(cj13)*(x789)))+(((IkReal(-1.00000000000000))*(cj14)*(x787)*(x788)))+(((sj11)*(x792)))+(((sj14)*(x791))))))) < IKFAST_ATAN2_MAGTHRESH )
3456  continue;
3457 j10array[0]=IKatan2(((gconst20)*(((((cj11)*(x792)))+(((IkReal(-1.00000000000000))*(sj11)*(x788)*(x789)))+(((cj13)*(cj14)*(x791)))+(((sj11)*(x790)))+(((sj14)*(x787)))))), ((gconst20)*(((((IkReal(-1.00000000000000))*(cj11)*(x790)))+(((cj11)*(cj13)*(x789)))+(((IkReal(-1.00000000000000))*(cj14)*(x787)*(x788)))+(((sj11)*(x792)))+(((sj14)*(x791)))))));
3458 sj10array[0]=IKsin(j10array[0]);
3459 cj10array[0]=IKcos(j10array[0]);
3460 if( j10array[0] > IKPI )
3461 {
3462  j10array[0]-=IK2PI;
3463 }
3464 else if( j10array[0] < -IKPI )
3465 { j10array[0]+=IK2PI;
3466 }
3467 j10valid[0] = true;
3468 for(int ij10 = 0; ij10 < 1; ++ij10)
3469 {
3470 if( !j10valid[ij10] )
3471 {
3472  continue;
3473 }
3474 _ij10[0] = ij10; _ij10[1] = -1;
3475 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
3476 {
3477 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
3478 {
3479  j10valid[iij10]=false; _ij10[1] = iij10; break;
3480 }
3481 }
3482 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
3483 {
3484 IkReal evalcond[2];
3485 IkReal x793=IKsin(j10);
3486 IkReal x794=IKcos(j10);
3487 IkReal x795=((IkReal(1.00000000000000))*(x794));
3488 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(cj11)*(x793)))+(((IkReal(-1.00000000000000))*(sj11)*(x795))));
3489 evalcond[1]=((((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)))+(((IkReal(-1.00000000000000))*(cj11)*(x795)))+(((cj13)*(r21)*(sj14)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((sj11)*(x793))));
3490 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
3491 {
3492 continue;
3493 }
3494 }
3495 
3496 {
3497 IkReal dummyeval[1];
3498 IkReal gconst24;
3499 IkReal x796=(sj14)*(sj14);
3500 IkReal x797=(cj14)*(cj14);
3501 IkReal x798=((r01)*(r10));
3502 IkReal x799=((cj13)*(cj14));
3503 IkReal x800=((IkReal(1.00000000000000))*(r12));
3504 IkReal x801=((cj13)*(sj14));
3505 IkReal x802=((sj13)*(x796));
3506 IkReal x803=((IkReal(1.00000000000000))*(r00)*(r11));
3507 IkReal x804=((sj13)*(x797));
3508 gconst24=IKsign(((((r02)*(r10)*(x801)))+(((r02)*(r11)*(x799)))+(((IkReal(-1.00000000000000))*(r00)*(x800)*(x801)))+(((x798)*(x802)))+(((x798)*(x804)))+(((IkReal(-1.00000000000000))*(r01)*(x799)*(x800)))+(((IkReal(-1.00000000000000))*(x803)*(x804)))+(((IkReal(-1.00000000000000))*(x802)*(x803)))));
3509 IkReal x805=(sj14)*(sj14);
3510 IkReal x806=(cj14)*(cj14);
3511 IkReal x807=((r01)*(r10));
3512 IkReal x808=((cj13)*(cj14));
3513 IkReal x809=((IkReal(1.00000000000000))*(r12));
3514 IkReal x810=((cj13)*(sj14));
3515 IkReal x811=((sj13)*(x805));
3516 IkReal x812=((IkReal(1.00000000000000))*(r00)*(r11));
3517 IkReal x813=((sj13)*(x806));
3518 dummyeval[0]=((((IkReal(-1.00000000000000))*(x812)*(x813)))+(((r02)*(r11)*(x808)))+(((IkReal(-1.00000000000000))*(r00)*(x809)*(x810)))+(((IkReal(-1.00000000000000))*(x811)*(x812)))+(((x807)*(x813)))+(((x807)*(x811)))+(((r02)*(r10)*(x810)))+(((IkReal(-1.00000000000000))*(r01)*(x808)*(x809))));
3519 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
3520 {
3521 {
3522 IkReal dummyeval[1];
3523 IkReal gconst23;
3524 IkReal x814=(sj14)*(sj14);
3525 IkReal x815=(cj14)*(cj14);
3526 IkReal x816=((IkReal(1.00000000000000))*(x814));
3527 IkReal x817=((IkReal(2.00000000000000))*(cj14)*(sj14));
3528 IkReal x818=((IkReal(1.00000000000000))*(x815));
3529 gconst23=IKsign(((((IkReal(-1.00000000000000))*(x818)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x817)))+(((IkReal(-1.00000000000000))*(x816)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x816)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x818)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x817)))));
3530 IkReal x819=(sj14)*(sj14);
3531 IkReal x820=(cj14)*(cj14);
3532 IkReal x821=((IkReal(1.00000000000000))*(x819));
3533 IkReal x822=((IkReal(2.00000000000000))*(cj14)*(sj14));
3534 IkReal x823=((IkReal(1.00000000000000))*(x820));
3535 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(r01)*(x822)))+(((IkReal(-1.00000000000000))*(x821)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x822)))+(((IkReal(-1.00000000000000))*(x821)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x823)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x823)*((r01)*(r01)))));
3536 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
3537 {
3538 continue;
3539 
3540 } else
3541 {
3542 {
3543 IkReal j9array[1], cj9array[1], sj9array[1];
3544 bool j9valid[1]={false};
3545 _nj9 = 1;
3546 IkReal x824=((cj14)*(r11));
3547 IkReal x825=((cj10)*(cj11));
3548 IkReal x826=((cj14)*(r01));
3549 IkReal x827=((sj14)*(x825));
3550 IkReal x828=((IkReal(1.00000000000000))*(sj10)*(sj11));
3551 if( IKabs(((gconst23)*(((((x824)*(x825)))+(((r10)*(x827)))+(((IkReal(-1.00000000000000))*(x824)*(x828)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(x828))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst23)*(((((x825)*(x826)))+(((r00)*(x827)))+(((IkReal(-1.00000000000000))*(x826)*(x828)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x828))))))) < IKFAST_ATAN2_MAGTHRESH )
3552  continue;
3553 j9array[0]=IKatan2(((gconst23)*(((((x824)*(x825)))+(((r10)*(x827)))+(((IkReal(-1.00000000000000))*(x824)*(x828)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(x828)))))), ((gconst23)*(((((x825)*(x826)))+(((r00)*(x827)))+(((IkReal(-1.00000000000000))*(x826)*(x828)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x828)))))));
3554 sj9array[0]=IKsin(j9array[0]);
3555 cj9array[0]=IKcos(j9array[0]);
3556 if( j9array[0] > IKPI )
3557 {
3558  j9array[0]-=IK2PI;
3559 }
3560 else if( j9array[0] < -IKPI )
3561 { j9array[0]+=IK2PI;
3562 }
3563 j9valid[0] = true;
3564 for(int ij9 = 0; ij9 < 1; ++ij9)
3565 {
3566 if( !j9valid[ij9] )
3567 {
3568  continue;
3569 }
3570 _ij9[0] = ij9; _ij9[1] = -1;
3571 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
3572 {
3573 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
3574 {
3575  j9valid[iij9]=false; _ij9[1] = iij9; break;
3576 }
3577 }
3578 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
3579 {
3580 IkReal evalcond[6];
3581 IkReal x829=IKsin(j9);
3582 IkReal x830=IKcos(j9);
3583 IkReal x831=((IkReal(1.00000000000000))*(cj14));
3584 IkReal x832=((IkReal(1.00000000000000))*(sj14));
3585 IkReal x833=((cj13)*(cj14));
3586 IkReal x834=((cj14)*(r10));
3587 IkReal x835=((r01)*(sj14));
3588 IkReal x836=((IkReal(1.00000000000000))*(r12));
3589 IkReal x837=((sj13)*(x830));
3590 IkReal x838=((r02)*(x829));
3591 IkReal x839=((r11)*(x829));
3592 IkReal x840=((r10)*(x830));
3593 IkReal x841=((r01)*(x830));
3594 IkReal x842=((sj13)*(x829));
3595 IkReal x843=((r11)*(x830));
3596 IkReal x844=((cj13)*(x829));
3597 IkReal x845=((r10)*(x829));
3598 IkReal x846=((r00)*(x830));
3599 IkReal x847=((cj13)*(x830));
3600 evalcond[0]=((((IkReal(-1.00000000000000))*(x831)*(x843)))+(((r00)*(sj14)*(x829)))+(((cj14)*(r01)*(x829)))+(((IkReal(-1.00000000000000))*(x832)*(x840))));
3601 evalcond[1]=((((IkReal(-1.00000000000000))*(x831)*(x841)))+(((sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(x832)*(x846)))+(((IkReal(-1.00000000000000))*(x832)*(x845)))+(((IkReal(-1.00000000000000))*(x831)*(x839))));
3602 evalcond[2]=((((IkReal(-1.00000000000000))*(sj13)*(x838)))+(((x833)*(x840)))+(((x835)*(x844)))+(((IkReal(-1.00000000000000))*(r00)*(x831)*(x844)))+(((IkReal(-1.00000000000000))*(cj13)*(x832)*(x843)))+(((r12)*(x837))));
3603 evalcond[3]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x836)*(x847)))+(((x835)*(x842)))+(((IkReal(-1.00000000000000))*(r00)*(x831)*(x842)))+(((IkReal(-1.00000000000000))*(r11)*(x832)*(x837)))+(((x834)*(x837)))+(((cj13)*(x838))));
3604 evalcond[4]=((((x833)*(x845)))+(((x833)*(x846)))+(((r02)*(x837)))+(((cj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x832)*(x839)))+(((IkReal(-1.00000000000000))*(cj13)*(x832)*(x841)))+(((r12)*(x842)))+(((cj11)*(sj10))));
3605 evalcond[5]=((((IkReal(-1.00000000000000))*(sj13)*(x832)*(x839)))+(((IkReal(-1.00000000000000))*(x836)*(x844)))+(((IkReal(-1.00000000000000))*(r02)*(x847)))+(((cj14)*(r00)*(x837)))+(((x834)*(x842)))+(((IkReal(-1.00000000000000))*(r01)*(x832)*(x837))));
3606 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 )
3607 {
3608 continue;
3609 }
3610 }
3611 
3612 {
3613 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3614 vinfos[0].jointtype = 1;
3615 vinfos[0].foffset = j9;
3616 vinfos[0].indices[0] = _ij9[0];
3617 vinfos[0].indices[1] = _ij9[1];
3618 vinfos[0].maxsolutions = _nj9;
3619 vinfos[1].jointtype = 1;
3620 vinfos[1].foffset = j10;
3621 vinfos[1].indices[0] = _ij10[0];
3622 vinfos[1].indices[1] = _ij10[1];
3623 vinfos[1].maxsolutions = _nj10;
3624 vinfos[2].jointtype = 1;
3625 vinfos[2].foffset = j11;
3626 vinfos[2].indices[0] = _ij11[0];
3627 vinfos[2].indices[1] = _ij11[1];
3628 vinfos[2].maxsolutions = _nj11;
3629 vinfos[3].jointtype = 1;
3630 vinfos[3].foffset = j12;
3631 vinfos[3].indices[0] = _ij12[0];
3632 vinfos[3].indices[1] = _ij12[1];
3633 vinfos[3].maxsolutions = _nj12;
3634 vinfos[4].jointtype = 1;
3635 vinfos[4].foffset = j13;
3636 vinfos[4].indices[0] = _ij13[0];
3637 vinfos[4].indices[1] = _ij13[1];
3638 vinfos[4].maxsolutions = _nj13;
3639 vinfos[5].jointtype = 1;
3640 vinfos[5].foffset = j14;
3641 vinfos[5].indices[0] = _ij14[0];
3642 vinfos[5].indices[1] = _ij14[1];
3643 vinfos[5].maxsolutions = _nj14;
3644 std::vector<int> vfree(0);
3645 solutions.AddSolution(vinfos,vfree);
3646 }
3647 }
3648 }
3649 
3650 }
3651 
3652 }
3653 
3654 } else
3655 {
3656 {
3657 IkReal j9array[1], cj9array[1], sj9array[1];
3658 bool j9valid[1]={false};
3659 _nj9 = 1;
3660 IkReal x848=((IkReal(1.00000000000000))*(sj14));
3661 IkReal x849=((IkReal(1.00000000000000))*(cj14));
3662 if( IKabs(((gconst24)*(((((IkReal(-1.00000000000000))*(r10)*(x848)))+(((IkReal(-1.00000000000000))*(r11)*(x849))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst24)*(((((IkReal(-1.00000000000000))*(r01)*(x849)))+(((IkReal(-1.00000000000000))*(r00)*(x848))))))) < IKFAST_ATAN2_MAGTHRESH )
3663  continue;
3664 j9array[0]=IKatan2(((gconst24)*(((((IkReal(-1.00000000000000))*(r10)*(x848)))+(((IkReal(-1.00000000000000))*(r11)*(x849)))))), ((gconst24)*(((((IkReal(-1.00000000000000))*(r01)*(x849)))+(((IkReal(-1.00000000000000))*(r00)*(x848)))))));
3665 sj9array[0]=IKsin(j9array[0]);
3666 cj9array[0]=IKcos(j9array[0]);
3667 if( j9array[0] > IKPI )
3668 {
3669  j9array[0]-=IK2PI;
3670 }
3671 else if( j9array[0] < -IKPI )
3672 { j9array[0]+=IK2PI;
3673 }
3674 j9valid[0] = true;
3675 for(int ij9 = 0; ij9 < 1; ++ij9)
3676 {
3677 if( !j9valid[ij9] )
3678 {
3679  continue;
3680 }
3681 _ij9[0] = ij9; _ij9[1] = -1;
3682 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
3683 {
3684 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
3685 {
3686  j9valid[iij9]=false; _ij9[1] = iij9; break;
3687 }
3688 }
3689 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
3690 {
3691 IkReal evalcond[6];
3692 IkReal x850=IKsin(j9);
3693 IkReal x851=IKcos(j9);
3694 IkReal x852=((IkReal(1.00000000000000))*(cj14));
3695 IkReal x853=((IkReal(1.00000000000000))*(sj14));
3696 IkReal x854=((cj13)*(cj14));
3697 IkReal x855=((cj14)*(r10));
3698 IkReal x856=((r01)*(sj14));
3699 IkReal x857=((IkReal(1.00000000000000))*(r12));
3700 IkReal x858=((sj13)*(x851));
3701 IkReal x859=((r02)*(x850));
3702 IkReal x860=((r11)*(x850));
3703 IkReal x861=((r10)*(x851));
3704 IkReal x862=((r01)*(x851));
3705 IkReal x863=((sj13)*(x850));
3706 IkReal x864=((r11)*(x851));
3707 IkReal x865=((cj13)*(x850));
3708 IkReal x866=((r10)*(x850));
3709 IkReal x867=((r00)*(x851));
3710 IkReal x868=((cj13)*(x851));
3711 evalcond[0]=((((r00)*(sj14)*(x850)))+(((cj14)*(r01)*(x850)))+(((IkReal(-1.00000000000000))*(x852)*(x864)))+(((IkReal(-1.00000000000000))*(x853)*(x861))));
3712 evalcond[1]=((((IkReal(-1.00000000000000))*(x852)*(x860)))+(((IkReal(-1.00000000000000))*(x852)*(x862)))+(((IkReal(-1.00000000000000))*(x853)*(x867)))+(((IkReal(-1.00000000000000))*(x853)*(x866)))+(((sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11))));
3713 evalcond[2]=((((r12)*(x858)))+(((x856)*(x865)))+(((IkReal(-1.00000000000000))*(r00)*(x852)*(x865)))+(((IkReal(-1.00000000000000))*(sj13)*(x859)))+(((IkReal(-1.00000000000000))*(cj13)*(x853)*(x864)))+(((x854)*(x861))));
3714 evalcond[3]=((IkReal(1.00000000000000))+(((x855)*(x858)))+(((cj13)*(x859)))+(((x856)*(x863)))+(((IkReal(-1.00000000000000))*(r00)*(x852)*(x863)))+(((IkReal(-1.00000000000000))*(r11)*(x853)*(x858)))+(((IkReal(-1.00000000000000))*(x857)*(x868))));
3715 evalcond[4]=((((r12)*(x863)))+(((cj10)*(sj11)))+(((r02)*(x858)))+(((IkReal(-1.00000000000000))*(cj13)*(x853)*(x860)))+(((IkReal(-1.00000000000000))*(cj13)*(x853)*(x862)))+(((x854)*(x866)))+(((x854)*(x867)))+(((cj11)*(sj10))));
3716 evalcond[5]=((((cj14)*(r00)*(x858)))+(((IkReal(-1.00000000000000))*(sj13)*(x853)*(x860)))+(((x855)*(x863)))+(((IkReal(-1.00000000000000))*(r02)*(x868)))+(((IkReal(-1.00000000000000))*(r01)*(x853)*(x858)))+(((IkReal(-1.00000000000000))*(x857)*(x865))));
3717 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 )
3718 {
3719 continue;
3720 }
3721 }
3722 
3723 {
3724 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3725 vinfos[0].jointtype = 1;
3726 vinfos[0].foffset = j9;
3727 vinfos[0].indices[0] = _ij9[0];
3728 vinfos[0].indices[1] = _ij9[1];
3729 vinfos[0].maxsolutions = _nj9;
3730 vinfos[1].jointtype = 1;
3731 vinfos[1].foffset = j10;
3732 vinfos[1].indices[0] = _ij10[0];
3733 vinfos[1].indices[1] = _ij10[1];
3734 vinfos[1].maxsolutions = _nj10;
3735 vinfos[2].jointtype = 1;
3736 vinfos[2].foffset = j11;
3737 vinfos[2].indices[0] = _ij11[0];
3738 vinfos[2].indices[1] = _ij11[1];
3739 vinfos[2].maxsolutions = _nj11;
3740 vinfos[3].jointtype = 1;
3741 vinfos[3].foffset = j12;
3742 vinfos[3].indices[0] = _ij12[0];
3743 vinfos[3].indices[1] = _ij12[1];
3744 vinfos[3].maxsolutions = _nj12;
3745 vinfos[4].jointtype = 1;
3746 vinfos[4].foffset = j13;
3747 vinfos[4].indices[0] = _ij13[0];
3748 vinfos[4].indices[1] = _ij13[1];
3749 vinfos[4].maxsolutions = _nj13;
3750 vinfos[5].jointtype = 1;
3751 vinfos[5].foffset = j14;
3752 vinfos[5].indices[0] = _ij14[0];
3753 vinfos[5].indices[1] = _ij14[1];
3754 vinfos[5].maxsolutions = _nj14;
3755 std::vector<int> vfree(0);
3756 solutions.AddSolution(vinfos,vfree);
3757 }
3758 }
3759 }
3760 
3761 }
3762 
3763 }
3764 }
3765 }
3766 
3767 }
3768 
3769 }
3770 
3771 } else
3772 {
3773 IkReal x869=((IkReal(1.00000000000000))*(sj13));
3774 IkReal x870=((npy)*(sj14));
3775 IkReal x871=((IkReal(1.00000000000000))*(cj14)*(npx));
3776 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
3777 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((cj14)*(npy)))+(((npx)*(sj14))));
3778 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(x871)))+(((IkReal(0.250000000000000))*(cj11)))+(((cj13)*(x870)))+(((IkReal(-1.00000000000000))*(npz)*(x869))));
3779 evalcond[3]=((((r21)*(sj13)*(sj14)))+(((IkReal(-1.00000000000000))*(cj14)*(r20)*(x869)))+(((cj13)*(r22))));
3780 evalcond[4]=((IkReal(-0.0950000000000000))+(((sj13)*(x870)))+(((IkReal(-1.00000000000000))*(cj14)*(npx)*(x869)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
3781 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 )
3782 {
3783 {
3784 IkReal dummyeval[1];
3785 IkReal gconst27;
3786 gconst27=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
3787 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
3788 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
3789 {
3790 {
3791 IkReal dummyeval[1];
3792 IkReal gconst25;
3793 IkReal x872=(sj14)*(sj14);
3794 IkReal x873=(cj14)*(cj14);
3795 IkReal x874=((cj13)*(r12));
3796 IkReal x875=((IkReal(1.00000000000000))*(r10));
3797 IkReal x876=((cj13)*(r02));
3798 IkReal x877=((r01)*(sj13));
3799 IkReal x878=((r00)*(r11)*(sj13));
3800 gconst25=IKsign(((((x872)*(x878)))+(((IkReal(-1.00000000000000))*(x872)*(x875)*(x877)))+(((IkReal(-1.00000000000000))*(x873)*(x875)*(x877)))+(((IkReal(-1.00000000000000))*(sj14)*(x875)*(x876)))+(((x873)*(x878)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x876)))+(((r00)*(sj14)*(x874)))+(((cj14)*(r01)*(x874)))));
3801 IkReal x879=(sj14)*(sj14);
3802 IkReal x880=(cj14)*(cj14);
3803 IkReal x881=((cj13)*(r12));
3804 IkReal x882=((IkReal(1.00000000000000))*(r10));
3805 IkReal x883=((cj13)*(r02));
3806 IkReal x884=((r01)*(sj13));
3807 IkReal x885=((r00)*(r11)*(sj13));
3808 dummyeval[0]=((((IkReal(-1.00000000000000))*(x880)*(x882)*(x884)))+(((x879)*(x885)))+(((IkReal(-1.00000000000000))*(sj14)*(x882)*(x883)))+(((x880)*(x885)))+(((cj14)*(r01)*(x881)))+(((r00)*(sj14)*(x881)))+(((IkReal(-1.00000000000000))*(x879)*(x882)*(x884)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x883))));
3809 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
3810 {
3811 {
3812 IkReal dummyeval[1];
3813 IkReal gconst26;
3814 IkReal x886=(cj13)*(cj13);
3815 IkReal x887=(sj13)*(sj13);
3816 IkReal x888=((r01)*(sj14));
3817 IkReal x889=((r12)*(x887));
3818 IkReal x890=((IkReal(1.00000000000000))*(cj14)*(r00));
3819 IkReal x891=((cj14)*(r02)*(r10));
3820 IkReal x892=((r12)*(x886));
3821 IkReal x893=((IkReal(1.00000000000000))*(r02)*(r11)*(sj14));
3822 gconst26=IKsign(((((x886)*(x891)))+(((IkReal(-1.00000000000000))*(x889)*(x890)))+(((x888)*(x889)))+(((x888)*(x892)))+(((IkReal(-1.00000000000000))*(x890)*(x892)))+(((x887)*(x891)))+(((IkReal(-1.00000000000000))*(x886)*(x893)))+(((IkReal(-1.00000000000000))*(x887)*(x893)))));
3823 IkReal x894=(cj13)*(cj13);
3824 IkReal x895=(sj13)*(sj13);
3825 IkReal x896=((r01)*(sj14));
3826 IkReal x897=((r12)*(x895));
3827 IkReal x898=((IkReal(1.00000000000000))*(cj14)*(r00));
3828 IkReal x899=((cj14)*(r02)*(r10));
3829 IkReal x900=((r12)*(x894));
3830 IkReal x901=x893;
3831 dummyeval[0]=((((x894)*(x899)))+(((IkReal(-1.00000000000000))*(x895)*(x901)))+(((x896)*(x897)))+(((IkReal(-1.00000000000000))*(x894)*(x901)))+(((x895)*(x899)))+(((IkReal(-1.00000000000000))*(x898)*(x900)))+(((x896)*(x900)))+(((IkReal(-1.00000000000000))*(x897)*(x898))));
3832 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
3833 {
3834 continue;
3835 
3836 } else
3837 {
3838 {
3839 IkReal j9array[1], cj9array[1], sj9array[1];
3840 bool j9valid[1]={false};
3841 _nj9 = 1;
3842 IkReal x902=((cj13)*(cj14));
3843 IkReal x903=((IkReal(1.00000000000000))*(cj13)*(sj14));
3844 if( IKabs(((gconst26)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x903)))+(((r10)*(x902))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst26)*(((((r00)*(x902)))+(((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x903))))))) < IKFAST_ATAN2_MAGTHRESH )
3845  continue;
3846 j9array[0]=IKatan2(((gconst26)*(((((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x903)))+(((r10)*(x902)))))), ((gconst26)*(((((r00)*(x902)))+(((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x903)))))));
3847 sj9array[0]=IKsin(j9array[0]);
3848 cj9array[0]=IKcos(j9array[0]);
3849 if( j9array[0] > IKPI )
3850 {
3851  j9array[0]-=IK2PI;
3852 }
3853 else if( j9array[0] < -IKPI )
3854 { j9array[0]+=IK2PI;
3855 }
3856 j9valid[0] = true;
3857 for(int ij9 = 0; ij9 < 1; ++ij9)
3858 {
3859 if( !j9valid[ij9] )
3860 {
3861  continue;
3862 }
3863 _ij9[0] = ij9; _ij9[1] = -1;
3864 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
3865 {
3866 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
3867 {
3868  j9valid[iij9]=false; _ij9[1] = iij9; break;
3869 }
3870 }
3871 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
3872 {
3873 IkReal evalcond[4];
3874 IkReal x904=IKsin(j9);
3875 IkReal x905=IKcos(j9);
3876 IkReal x906=((IkReal(1.00000000000000))*(sj14));
3877 IkReal x907=((cj13)*(cj14));
3878 IkReal x908=((IkReal(1.00000000000000))*(r00));
3879 IkReal x909=((r01)*(sj14));
3880 IkReal x910=((IkReal(1.00000000000000))*(cj13));
3881 IkReal x911=((sj13)*(x905));
3882 IkReal x912=((r02)*(x904));
3883 IkReal x913=((r10)*(x905));
3884 IkReal x914=((r11)*(x905));
3885 IkReal x915=((sj13)*(x904));
3886 IkReal x916=((cj14)*(x915));
3887 evalcond[0]=((((IkReal(-1.00000000000000))*(x906)*(x913)))+(((cj14)*(r01)*(x904)))+(((r00)*(sj14)*(x904)))+(((IkReal(-1.00000000000000))*(cj14)*(x914))));
3888 evalcond[1]=((((IkReal(-1.00000000000000))*(sj13)*(x912)))+(((cj13)*(x904)*(x909)))+(((IkReal(-1.00000000000000))*(x904)*(x907)*(x908)))+(((r12)*(x911)))+(((x907)*(x913)))+(((IkReal(-1.00000000000000))*(cj13)*(x906)*(x914))));
3889 evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r11)*(x906)*(x911)))+(((x909)*(x915)))+(((IkReal(-1.00000000000000))*(x908)*(x916)))+(((IkReal(-1.00000000000000))*(r12)*(x905)*(x910)))+(((cj14)*(r10)*(x911)))+(((cj13)*(x912))));
3890 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x906)*(x915)))+(((cj14)*(r00)*(x911)))+(((IkReal(-1.00000000000000))*(r01)*(x906)*(x911)))+(((r10)*(x916)))+(((IkReal(-1.00000000000000))*(r12)*(x904)*(x910)))+(((IkReal(-1.00000000000000))*(r02)*(x905)*(x910))));
3891 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
3892 {
3893 continue;
3894 }
3895 }
3896 
3897 {
3898 IkReal dummyeval[1];
3899 IkReal gconst28;
3900 gconst28=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
3901 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
3902 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
3903 {
3904 {
3905 IkReal dummyeval[1];
3906 IkReal gconst29;
3907 gconst29=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
3908 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
3909 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
3910 {
3911 continue;
3912 
3913 } else
3914 {
3915 {
3916 IkReal j10array[1], cj10array[1], sj10array[1];
3917 bool j10valid[1]={false};
3918 _nj10 = 1;
3919 IkReal x917=((IkReal(1.00000000000000))*(sj11));
3920 IkReal x918=((cj14)*(r21));
3921 IkReal x919=((IkReal(1.00000000000000))*(cj11));
3922 IkReal x920=((r20)*(sj14));
3923 IkReal x921=((cj9)*(r00)*(sj14));
3924 IkReal x922=((cj14)*(r11)*(sj9));
3925 IkReal x923=((cj14)*(cj9)*(r01));
3926 IkReal x924=((r10)*(sj14)*(sj9));
3927 if( IKabs(((gconst29)*(((((IkReal(-1.00000000000000))*(x918)*(x919)))+(((IkReal(-1.00000000000000))*(x919)*(x920)))+(((IkReal(-1.00000000000000))*(x917)*(x924)))+(((IkReal(-1.00000000000000))*(x917)*(x921)))+(((IkReal(-1.00000000000000))*(x917)*(x923)))+(((IkReal(-1.00000000000000))*(x917)*(x922))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst29)*(((((cj11)*(x924)))+(((cj11)*(x922)))+(((cj11)*(x923)))+(((cj11)*(x921)))+(((IkReal(-1.00000000000000))*(x917)*(x920)))+(((IkReal(-1.00000000000000))*(x917)*(x918))))))) < IKFAST_ATAN2_MAGTHRESH )
3928  continue;
3929 j10array[0]=IKatan2(((gconst29)*(((((IkReal(-1.00000000000000))*(x918)*(x919)))+(((IkReal(-1.00000000000000))*(x919)*(x920)))+(((IkReal(-1.00000000000000))*(x917)*(x924)))+(((IkReal(-1.00000000000000))*(x917)*(x921)))+(((IkReal(-1.00000000000000))*(x917)*(x923)))+(((IkReal(-1.00000000000000))*(x917)*(x922)))))), ((gconst29)*(((((cj11)*(x924)))+(((cj11)*(x922)))+(((cj11)*(x923)))+(((cj11)*(x921)))+(((IkReal(-1.00000000000000))*(x917)*(x920)))+(((IkReal(-1.00000000000000))*(x917)*(x918)))))));
3930 sj10array[0]=IKsin(j10array[0]);
3931 cj10array[0]=IKcos(j10array[0]);
3932 if( j10array[0] > IKPI )
3933 {
3934  j10array[0]-=IK2PI;
3935 }
3936 else if( j10array[0] < -IKPI )
3937 { j10array[0]+=IK2PI;
3938 }
3939 j10valid[0] = true;
3940 for(int ij10 = 0; ij10 < 1; ++ij10)
3941 {
3942 if( !j10valid[ij10] )
3943 {
3944  continue;
3945 }
3946 _ij10[0] = ij10; _ij10[1] = -1;
3947 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
3948 {
3949 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
3950 {
3951  j10valid[iij10]=false; _ij10[1] = iij10; break;
3952 }
3953 }
3954 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
3955 {
3956 IkReal evalcond[4];
3957 IkReal x925=IKsin(j10);
3958 IkReal x926=IKcos(j10);
3959 IkReal x927=((cj14)*(sj9));
3960 IkReal x928=((IkReal(1.00000000000000))*(r11));
3961 IkReal x929=((cj13)*(sj14));
3962 IkReal x930=((IkReal(1.00000000000000))*(cj9));
3963 IkReal x931=((cj13)*(cj14));
3964 IkReal x932=((cj11)*(x925));
3965 IkReal x933=((sj11)*(x926));
3966 IkReal x934=((cj11)*(x926));
3967 IkReal x935=((sj11)*(x925));
3968 IkReal x936=((x933)+(x932));
3969 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(x936));
3970 evalcond[1]=((((r21)*(x929)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x931)))+(((IkReal(-1.00000000000000))*(x934)))+(x935));
3971 evalcond[2]=((((IkReal(-1.00000000000000))*(x927)*(x928)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x935)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x930)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x930)))+(x934));
3972 evalcond[3]=((((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x929)*(x930)))+(((cj9)*(r00)*(x931)))+(((IkReal(-1.00000000000000))*(sj9)*(x928)*(x929)))+(((cj13)*(r10)*(x927)))+(x936)+(((r12)*(sj13)*(sj9))));
3973 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
3974 {
3975 continue;
3976 }
3977 }
3978 
3979 {
3980 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
3981 vinfos[0].jointtype = 1;
3982 vinfos[0].foffset = j9;
3983 vinfos[0].indices[0] = _ij9[0];
3984 vinfos[0].indices[1] = _ij9[1];
3985 vinfos[0].maxsolutions = _nj9;
3986 vinfos[1].jointtype = 1;
3987 vinfos[1].foffset = j10;
3988 vinfos[1].indices[0] = _ij10[0];
3989 vinfos[1].indices[1] = _ij10[1];
3990 vinfos[1].maxsolutions = _nj10;
3991 vinfos[2].jointtype = 1;
3992 vinfos[2].foffset = j11;
3993 vinfos[2].indices[0] = _ij11[0];
3994 vinfos[2].indices[1] = _ij11[1];
3995 vinfos[2].maxsolutions = _nj11;
3996 vinfos[3].jointtype = 1;
3997 vinfos[3].foffset = j12;
3998 vinfos[3].indices[0] = _ij12[0];
3999 vinfos[3].indices[1] = _ij12[1];
4000 vinfos[3].maxsolutions = _nj12;
4001 vinfos[4].jointtype = 1;
4002 vinfos[4].foffset = j13;
4003 vinfos[4].indices[0] = _ij13[0];
4004 vinfos[4].indices[1] = _ij13[1];
4005 vinfos[4].maxsolutions = _nj13;
4006 vinfos[5].jointtype = 1;
4007 vinfos[5].foffset = j14;
4008 vinfos[5].indices[0] = _ij14[0];
4009 vinfos[5].indices[1] = _ij14[1];
4010 vinfos[5].maxsolutions = _nj14;
4011 std::vector<int> vfree(0);
4012 solutions.AddSolution(vinfos,vfree);
4013 }
4014 }
4015 }
4016 
4017 }
4018 
4019 }
4020 
4021 } else
4022 {
4023 {
4024 IkReal j10array[1], cj10array[1], sj10array[1];
4025 bool j10valid[1]={false};
4026 _nj10 = 1;
4027 IkReal x937=((cj13)*(sj11));
4028 IkReal x938=((r21)*(sj14));
4029 IkReal x939=((cj14)*(r20));
4030 IkReal x940=((cj11)*(cj13));
4031 IkReal x941=((r22)*(sj13));
4032 IkReal x942=((r20)*(sj14));
4033 IkReal x943=((cj14)*(r21));
4034 if( IKabs(((gconst28)*(((((IkReal(-1.00000000000000))*(sj11)*(x941)))+(((x937)*(x938)))+(((cj11)*(x942)))+(((cj11)*(x943)))+(((IkReal(-1.00000000000000))*(x937)*(x939))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst28)*(((((cj11)*(x941)))+(((x939)*(x940)))+(((IkReal(-1.00000000000000))*(x938)*(x940)))+(((sj11)*(x943)))+(((sj11)*(x942))))))) < IKFAST_ATAN2_MAGTHRESH )
4035  continue;
4036 j10array[0]=IKatan2(((gconst28)*(((((IkReal(-1.00000000000000))*(sj11)*(x941)))+(((x937)*(x938)))+(((cj11)*(x942)))+(((cj11)*(x943)))+(((IkReal(-1.00000000000000))*(x937)*(x939)))))), ((gconst28)*(((((cj11)*(x941)))+(((x939)*(x940)))+(((IkReal(-1.00000000000000))*(x938)*(x940)))+(((sj11)*(x943)))+(((sj11)*(x942)))))));
4037 sj10array[0]=IKsin(j10array[0]);
4038 cj10array[0]=IKcos(j10array[0]);
4039 if( j10array[0] > IKPI )
4040 {
4041  j10array[0]-=IK2PI;
4042 }
4043 else if( j10array[0] < -IKPI )
4044 { j10array[0]+=IK2PI;
4045 }
4046 j10valid[0] = true;
4047 for(int ij10 = 0; ij10 < 1; ++ij10)
4048 {
4049 if( !j10valid[ij10] )
4050 {
4051  continue;
4052 }
4053 _ij10[0] = ij10; _ij10[1] = -1;
4054 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
4055 {
4056 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
4057 {
4058  j10valid[iij10]=false; _ij10[1] = iij10; break;
4059 }
4060 }
4061 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
4062 {
4063 IkReal evalcond[4];
4064 IkReal x944=IKsin(j10);
4065 IkReal x945=IKcos(j10);
4066 IkReal x946=((cj14)*(sj9));
4067 IkReal x947=((IkReal(1.00000000000000))*(r11));
4068 IkReal x948=((cj13)*(sj14));
4069 IkReal x949=((IkReal(1.00000000000000))*(cj9));
4070 IkReal x950=((cj13)*(cj14));
4071 IkReal x951=((cj11)*(x944));
4072 IkReal x952=((sj11)*(x945));
4073 IkReal x953=((cj11)*(x945));
4074 IkReal x954=((sj11)*(x944));
4075 IkReal x955=((x952)+(x951));
4076 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(x955));
4077 evalcond[1]=((((r21)*(x948)))+(((IkReal(-1.00000000000000))*(x953)))+(((IkReal(-1.00000000000000))*(r20)*(x950)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(x954));
4078 evalcond[2]=((((IkReal(-1.00000000000000))*(x946)*(x947)))+(((IkReal(-1.00000000000000))*(x954)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x949)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x949)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(x953));
4079 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x948)*(x949)))+(((cj13)*(r10)*(x946)))+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(sj9)*(x947)*(x948)))+(((cj9)*(r00)*(x950)))+(x955)+(((r12)*(sj13)*(sj9))));
4080 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
4081 {
4082 continue;
4083 }
4084 }
4085 
4086 {
4087 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4088 vinfos[0].jointtype = 1;
4089 vinfos[0].foffset = j9;
4090 vinfos[0].indices[0] = _ij9[0];
4091 vinfos[0].indices[1] = _ij9[1];
4092 vinfos[0].maxsolutions = _nj9;
4093 vinfos[1].jointtype = 1;
4094 vinfos[1].foffset = j10;
4095 vinfos[1].indices[0] = _ij10[0];
4096 vinfos[1].indices[1] = _ij10[1];
4097 vinfos[1].maxsolutions = _nj10;
4098 vinfos[2].jointtype = 1;
4099 vinfos[2].foffset = j11;
4100 vinfos[2].indices[0] = _ij11[0];
4101 vinfos[2].indices[1] = _ij11[1];
4102 vinfos[2].maxsolutions = _nj11;
4103 vinfos[3].jointtype = 1;
4104 vinfos[3].foffset = j12;
4105 vinfos[3].indices[0] = _ij12[0];
4106 vinfos[3].indices[1] = _ij12[1];
4107 vinfos[3].maxsolutions = _nj12;
4108 vinfos[4].jointtype = 1;
4109 vinfos[4].foffset = j13;
4110 vinfos[4].indices[0] = _ij13[0];
4111 vinfos[4].indices[1] = _ij13[1];
4112 vinfos[4].maxsolutions = _nj13;
4113 vinfos[5].jointtype = 1;
4114 vinfos[5].foffset = j14;
4115 vinfos[5].indices[0] = _ij14[0];
4116 vinfos[5].indices[1] = _ij14[1];
4117 vinfos[5].maxsolutions = _nj14;
4118 std::vector<int> vfree(0);
4119 solutions.AddSolution(vinfos,vfree);
4120 }
4121 }
4122 }
4123 
4124 }
4125 
4126 }
4127 }
4128 }
4129 
4130 }
4131 
4132 }
4133 
4134 } else
4135 {
4136 {
4137 IkReal j9array[1], cj9array[1], sj9array[1];
4138 bool j9valid[1]={false};
4139 _nj9 = 1;
4140 IkReal x956=((IkReal(1.00000000000000))*(sj14));
4141 IkReal x957=((IkReal(1.00000000000000))*(cj14));
4142 if( IKabs(((gconst25)*(((((IkReal(-1.00000000000000))*(r10)*(x956)))+(((IkReal(-1.00000000000000))*(r11)*(x957))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst25)*(((((IkReal(-1.00000000000000))*(r01)*(x957)))+(((IkReal(-1.00000000000000))*(r00)*(x956))))))) < IKFAST_ATAN2_MAGTHRESH )
4143  continue;
4144 j9array[0]=IKatan2(((gconst25)*(((((IkReal(-1.00000000000000))*(r10)*(x956)))+(((IkReal(-1.00000000000000))*(r11)*(x957)))))), ((gconst25)*(((((IkReal(-1.00000000000000))*(r01)*(x957)))+(((IkReal(-1.00000000000000))*(r00)*(x956)))))));
4145 sj9array[0]=IKsin(j9array[0]);
4146 cj9array[0]=IKcos(j9array[0]);
4147 if( j9array[0] > IKPI )
4148 {
4149  j9array[0]-=IK2PI;
4150 }
4151 else if( j9array[0] < -IKPI )
4152 { j9array[0]+=IK2PI;
4153 }
4154 j9valid[0] = true;
4155 for(int ij9 = 0; ij9 < 1; ++ij9)
4156 {
4157 if( !j9valid[ij9] )
4158 {
4159  continue;
4160 }
4161 _ij9[0] = ij9; _ij9[1] = -1;
4162 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
4163 {
4164 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
4165 {
4166  j9valid[iij9]=false; _ij9[1] = iij9; break;
4167 }
4168 }
4169 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
4170 {
4171 IkReal evalcond[4];
4172 IkReal x958=IKsin(j9);
4173 IkReal x959=IKcos(j9);
4174 IkReal x960=((IkReal(1.00000000000000))*(sj14));
4175 IkReal x961=((cj13)*(cj14));
4176 IkReal x962=((IkReal(1.00000000000000))*(r00));
4177 IkReal x963=((r01)*(sj14));
4178 IkReal x964=((IkReal(1.00000000000000))*(cj13));
4179 IkReal x965=((sj13)*(x959));
4180 IkReal x966=((r02)*(x958));
4181 IkReal x967=((r10)*(x959));
4182 IkReal x968=((r11)*(x959));
4183 IkReal x969=((sj13)*(x958));
4184 IkReal x970=((cj14)*(x969));
4185 evalcond[0]=((((cj14)*(r01)*(x958)))+(((IkReal(-1.00000000000000))*(cj14)*(x968)))+(((IkReal(-1.00000000000000))*(x960)*(x967)))+(((r00)*(sj14)*(x958))));
4186 evalcond[1]=((((cj13)*(x958)*(x963)))+(((x961)*(x967)))+(((IkReal(-1.00000000000000))*(x958)*(x961)*(x962)))+(((r12)*(x965)))+(((IkReal(-1.00000000000000))*(cj13)*(x960)*(x968)))+(((IkReal(-1.00000000000000))*(sj13)*(x966))));
4187 evalcond[2]=((IkReal(-1.00000000000000))+(((cj14)*(r10)*(x965)))+(((IkReal(-1.00000000000000))*(x962)*(x970)))+(((IkReal(-1.00000000000000))*(r11)*(x960)*(x965)))+(((x963)*(x969)))+(((cj13)*(x966)))+(((IkReal(-1.00000000000000))*(r12)*(x959)*(x964))));
4188 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x960)*(x969)))+(((IkReal(-1.00000000000000))*(r01)*(x960)*(x965)))+(((IkReal(-1.00000000000000))*(r12)*(x958)*(x964)))+(((IkReal(-1.00000000000000))*(r02)*(x959)*(x964)))+(((r10)*(x970)))+(((cj14)*(r00)*(x965))));
4189 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
4190 {
4191 continue;
4192 }
4193 }
4194 
4195 {
4196 IkReal dummyeval[1];
4197 IkReal gconst28;
4198 gconst28=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
4199 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
4200 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
4201 {
4202 {
4203 IkReal dummyeval[1];
4204 IkReal gconst29;
4205 gconst29=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
4206 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
4207 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
4208 {
4209 continue;
4210 
4211 } else
4212 {
4213 {
4214 IkReal j10array[1], cj10array[1], sj10array[1];
4215 bool j10valid[1]={false};
4216 _nj10 = 1;
4217 IkReal x971=((IkReal(1.00000000000000))*(sj11));
4218 IkReal x972=((cj14)*(r21));
4219 IkReal x973=((IkReal(1.00000000000000))*(cj11));
4220 IkReal x974=((r20)*(sj14));
4221 IkReal x975=((cj9)*(r00)*(sj14));
4222 IkReal x976=((cj14)*(r11)*(sj9));
4223 IkReal x977=((cj14)*(cj9)*(r01));
4224 IkReal x978=((r10)*(sj14)*(sj9));
4225 if( IKabs(((gconst29)*(((((IkReal(-1.00000000000000))*(x971)*(x976)))+(((IkReal(-1.00000000000000))*(x971)*(x977)))+(((IkReal(-1.00000000000000))*(x971)*(x978)))+(((IkReal(-1.00000000000000))*(x971)*(x975)))+(((IkReal(-1.00000000000000))*(x972)*(x973)))+(((IkReal(-1.00000000000000))*(x973)*(x974))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst29)*(((((cj11)*(x975)))+(((cj11)*(x977)))+(((cj11)*(x976)))+(((cj11)*(x978)))+(((IkReal(-1.00000000000000))*(x971)*(x972)))+(((IkReal(-1.00000000000000))*(x971)*(x974))))))) < IKFAST_ATAN2_MAGTHRESH )
4226  continue;
4227 j10array[0]=IKatan2(((gconst29)*(((((IkReal(-1.00000000000000))*(x971)*(x976)))+(((IkReal(-1.00000000000000))*(x971)*(x977)))+(((IkReal(-1.00000000000000))*(x971)*(x978)))+(((IkReal(-1.00000000000000))*(x971)*(x975)))+(((IkReal(-1.00000000000000))*(x972)*(x973)))+(((IkReal(-1.00000000000000))*(x973)*(x974)))))), ((gconst29)*(((((cj11)*(x975)))+(((cj11)*(x977)))+(((cj11)*(x976)))+(((cj11)*(x978)))+(((IkReal(-1.00000000000000))*(x971)*(x972)))+(((IkReal(-1.00000000000000))*(x971)*(x974)))))));
4228 sj10array[0]=IKsin(j10array[0]);
4229 cj10array[0]=IKcos(j10array[0]);
4230 if( j10array[0] > IKPI )
4231 {
4232  j10array[0]-=IK2PI;
4233 }
4234 else if( j10array[0] < -IKPI )
4235 { j10array[0]+=IK2PI;
4236 }
4237 j10valid[0] = true;
4238 for(int ij10 = 0; ij10 < 1; ++ij10)
4239 {
4240 if( !j10valid[ij10] )
4241 {
4242  continue;
4243 }
4244 _ij10[0] = ij10; _ij10[1] = -1;
4245 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
4246 {
4247 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
4248 {
4249  j10valid[iij10]=false; _ij10[1] = iij10; break;
4250 }
4251 }
4252 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
4253 {
4254 IkReal evalcond[4];
4255 IkReal x979=IKsin(j10);
4256 IkReal x980=IKcos(j10);
4257 IkReal x981=((cj14)*(sj9));
4258 IkReal x982=((IkReal(1.00000000000000))*(r11));
4259 IkReal x983=((cj13)*(sj14));
4260 IkReal x984=((IkReal(1.00000000000000))*(cj9));
4261 IkReal x985=((cj13)*(cj14));
4262 IkReal x986=((cj11)*(x979));
4263 IkReal x987=((sj11)*(x980));
4264 IkReal x988=((cj11)*(x980));
4265 IkReal x989=((sj11)*(x979));
4266 IkReal x990=((x986)+(x987));
4267 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(x990));
4268 evalcond[1]=((((IkReal(-1.00000000000000))*(x988)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x983)))+(((IkReal(-1.00000000000000))*(r20)*(x985)))+(x989));
4269 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(sj14)*(x984)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x984)))+(((IkReal(-1.00000000000000))*(x989)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x981)*(x982)))+(x988));
4270 evalcond[3]=((((IkReal(-1.00000000000000))*(sj9)*(x982)*(x983)))+(((cj9)*(r02)*(sj13)))+(((cj13)*(r10)*(x981)))+(((cj9)*(r00)*(x985)))+(x990)+(((IkReal(-1.00000000000000))*(r01)*(x983)*(x984)))+(((r12)*(sj13)*(sj9))));
4271 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
4272 {
4273 continue;
4274 }
4275 }
4276 
4277 {
4278 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4279 vinfos[0].jointtype = 1;
4280 vinfos[0].foffset = j9;
4281 vinfos[0].indices[0] = _ij9[0];
4282 vinfos[0].indices[1] = _ij9[1];
4283 vinfos[0].maxsolutions = _nj9;
4284 vinfos[1].jointtype = 1;
4285 vinfos[1].foffset = j10;
4286 vinfos[1].indices[0] = _ij10[0];
4287 vinfos[1].indices[1] = _ij10[1];
4288 vinfos[1].maxsolutions = _nj10;
4289 vinfos[2].jointtype = 1;
4290 vinfos[2].foffset = j11;
4291 vinfos[2].indices[0] = _ij11[0];
4292 vinfos[2].indices[1] = _ij11[1];
4293 vinfos[2].maxsolutions = _nj11;
4294 vinfos[3].jointtype = 1;
4295 vinfos[3].foffset = j12;
4296 vinfos[3].indices[0] = _ij12[0];
4297 vinfos[3].indices[1] = _ij12[1];
4298 vinfos[3].maxsolutions = _nj12;
4299 vinfos[4].jointtype = 1;
4300 vinfos[4].foffset = j13;
4301 vinfos[4].indices[0] = _ij13[0];
4302 vinfos[4].indices[1] = _ij13[1];
4303 vinfos[4].maxsolutions = _nj13;
4304 vinfos[5].jointtype = 1;
4305 vinfos[5].foffset = j14;
4306 vinfos[5].indices[0] = _ij14[0];
4307 vinfos[5].indices[1] = _ij14[1];
4308 vinfos[5].maxsolutions = _nj14;
4309 std::vector<int> vfree(0);
4310 solutions.AddSolution(vinfos,vfree);
4311 }
4312 }
4313 }
4314 
4315 }
4316 
4317 }
4318 
4319 } else
4320 {
4321 {
4322 IkReal j10array[1], cj10array[1], sj10array[1];
4323 bool j10valid[1]={false};
4324 _nj10 = 1;
4325 IkReal x991=((cj13)*(sj11));
4326 IkReal x992=((r21)*(sj14));
4327 IkReal x993=((cj14)*(r20));
4328 IkReal x994=((cj11)*(cj13));
4329 IkReal x995=((r22)*(sj13));
4330 IkReal x996=((r20)*(sj14));
4331 IkReal x997=((cj14)*(r21));
4332 if( IKabs(((gconst28)*(((((x991)*(x992)))+(((IkReal(-1.00000000000000))*(x991)*(x993)))+(((cj11)*(x997)))+(((cj11)*(x996)))+(((IkReal(-1.00000000000000))*(sj11)*(x995))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst28)*(((((x993)*(x994)))+(((IkReal(-1.00000000000000))*(x992)*(x994)))+(((sj11)*(x996)))+(((sj11)*(x997)))+(((cj11)*(x995))))))) < IKFAST_ATAN2_MAGTHRESH )
4333  continue;
4334 j10array[0]=IKatan2(((gconst28)*(((((x991)*(x992)))+(((IkReal(-1.00000000000000))*(x991)*(x993)))+(((cj11)*(x997)))+(((cj11)*(x996)))+(((IkReal(-1.00000000000000))*(sj11)*(x995)))))), ((gconst28)*(((((x993)*(x994)))+(((IkReal(-1.00000000000000))*(x992)*(x994)))+(((sj11)*(x996)))+(((sj11)*(x997)))+(((cj11)*(x995)))))));
4335 sj10array[0]=IKsin(j10array[0]);
4336 cj10array[0]=IKcos(j10array[0]);
4337 if( j10array[0] > IKPI )
4338 {
4339  j10array[0]-=IK2PI;
4340 }
4341 else if( j10array[0] < -IKPI )
4342 { j10array[0]+=IK2PI;
4343 }
4344 j10valid[0] = true;
4345 for(int ij10 = 0; ij10 < 1; ++ij10)
4346 {
4347 if( !j10valid[ij10] )
4348 {
4349  continue;
4350 }
4351 _ij10[0] = ij10; _ij10[1] = -1;
4352 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
4353 {
4354 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
4355 {
4356  j10valid[iij10]=false; _ij10[1] = iij10; break;
4357 }
4358 }
4359 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
4360 {
4361 IkReal evalcond[4];
4362 IkReal x998=IKsin(j10);
4363 IkReal x999=IKcos(j10);
4364 IkReal x1000=((cj14)*(sj9));
4365 IkReal x1001=((IkReal(1.00000000000000))*(r11));
4366 IkReal x1002=((cj13)*(sj14));
4367 IkReal x1003=((IkReal(1.00000000000000))*(cj9));
4368 IkReal x1004=((cj13)*(cj14));
4369 IkReal x1005=((cj11)*(x998));
4370 IkReal x1006=((sj11)*(x999));
4371 IkReal x1007=((cj11)*(x999));
4372 IkReal x1008=((sj11)*(x998));
4373 IkReal x1009=((x1006)+(x1005));
4374 evalcond[0]=((x1009)+(((cj14)*(r21)))+(((r20)*(sj14))));
4375 evalcond[1]=((((IkReal(-1.00000000000000))*(x1007)))+(x1008)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x1004)))+(((r21)*(x1002))));
4376 evalcond[2]=((((IkReal(-1.00000000000000))*(x1008)))+(x1007)+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1003)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x1003)))+(((IkReal(-1.00000000000000))*(x1000)*(x1001))));
4377 evalcond[3]=((x1009)+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(sj9)*(x1001)*(x1002)))+(((cj13)*(r10)*(x1000)))+(((IkReal(-1.00000000000000))*(r01)*(x1002)*(x1003)))+(((cj9)*(r00)*(x1004)))+(((r12)*(sj13)*(sj9))));
4378 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
4379 {
4380 continue;
4381 }
4382 }
4383 
4384 {
4385 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4386 vinfos[0].jointtype = 1;
4387 vinfos[0].foffset = j9;
4388 vinfos[0].indices[0] = _ij9[0];
4389 vinfos[0].indices[1] = _ij9[1];
4390 vinfos[0].maxsolutions = _nj9;
4391 vinfos[1].jointtype = 1;
4392 vinfos[1].foffset = j10;
4393 vinfos[1].indices[0] = _ij10[0];
4394 vinfos[1].indices[1] = _ij10[1];
4395 vinfos[1].maxsolutions = _nj10;
4396 vinfos[2].jointtype = 1;
4397 vinfos[2].foffset = j11;
4398 vinfos[2].indices[0] = _ij11[0];
4399 vinfos[2].indices[1] = _ij11[1];
4400 vinfos[2].maxsolutions = _nj11;
4401 vinfos[3].jointtype = 1;
4402 vinfos[3].foffset = j12;
4403 vinfos[3].indices[0] = _ij12[0];
4404 vinfos[3].indices[1] = _ij12[1];
4405 vinfos[3].maxsolutions = _nj12;
4406 vinfos[4].jointtype = 1;
4407 vinfos[4].foffset = j13;
4408 vinfos[4].indices[0] = _ij13[0];
4409 vinfos[4].indices[1] = _ij13[1];
4410 vinfos[4].maxsolutions = _nj13;
4411 vinfos[5].jointtype = 1;
4412 vinfos[5].foffset = j14;
4413 vinfos[5].indices[0] = _ij14[0];
4414 vinfos[5].indices[1] = _ij14[1];
4415 vinfos[5].maxsolutions = _nj14;
4416 std::vector<int> vfree(0);
4417 solutions.AddSolution(vinfos,vfree);
4418 }
4419 }
4420 }
4421 
4422 }
4423 
4424 }
4425 }
4426 }
4427 
4428 }
4429 
4430 }
4431 
4432 } else
4433 {
4434 {
4435 IkReal j10array[1], cj10array[1], sj10array[1];
4436 bool j10valid[1]={false};
4437 _nj10 = 1;
4438 IkReal x1010=((cj13)*(sj11));
4439 IkReal x1011=((r21)*(sj14));
4440 IkReal x1012=((cj14)*(r20));
4441 IkReal x1013=((cj11)*(cj13));
4442 IkReal x1014=((r22)*(sj13));
4443 IkReal x1015=((r20)*(sj14));
4444 IkReal x1016=((cj14)*(r21));
4445 if( IKabs(((gconst27)*(((((IkReal(-1.00000000000000))*(x1010)*(x1012)))+(((IkReal(-1.00000000000000))*(sj11)*(x1014)))+(((cj11)*(x1015)))+(((cj11)*(x1016)))+(((x1010)*(x1011))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst27)*(((((IkReal(-1.00000000000000))*(x1011)*(x1013)))+(((x1012)*(x1013)))+(((sj11)*(x1016)))+(((sj11)*(x1015)))+(((cj11)*(x1014))))))) < IKFAST_ATAN2_MAGTHRESH )
4446  continue;
4447 j10array[0]=IKatan2(((gconst27)*(((((IkReal(-1.00000000000000))*(x1010)*(x1012)))+(((IkReal(-1.00000000000000))*(sj11)*(x1014)))+(((cj11)*(x1015)))+(((cj11)*(x1016)))+(((x1010)*(x1011)))))), ((gconst27)*(((((IkReal(-1.00000000000000))*(x1011)*(x1013)))+(((x1012)*(x1013)))+(((sj11)*(x1016)))+(((sj11)*(x1015)))+(((cj11)*(x1014)))))));
4448 sj10array[0]=IKsin(j10array[0]);
4449 cj10array[0]=IKcos(j10array[0]);
4450 if( j10array[0] > IKPI )
4451 {
4452  j10array[0]-=IK2PI;
4453 }
4454 else if( j10array[0] < -IKPI )
4455 { j10array[0]+=IK2PI;
4456 }
4457 j10valid[0] = true;
4458 for(int ij10 = 0; ij10 < 1; ++ij10)
4459 {
4460 if( !j10valid[ij10] )
4461 {
4462  continue;
4463 }
4464 _ij10[0] = ij10; _ij10[1] = -1;
4465 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
4466 {
4467 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
4468 {
4469  j10valid[iij10]=false; _ij10[1] = iij10; break;
4470 }
4471 }
4472 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
4473 {
4474 IkReal evalcond[2];
4475 IkReal x1017=IKsin(j10);
4476 IkReal x1018=IKcos(j10);
4477 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((sj11)*(x1018)))+(((cj11)*(x1017))));
4478 evalcond[1]=((((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)))+(((cj13)*(r21)*(sj14)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((sj11)*(x1017)))+(((IkReal(-1.00000000000000))*(cj11)*(x1018))));
4479 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
4480 {
4481 continue;
4482 }
4483 }
4484 
4485 {
4486 IkReal dummyeval[1];
4487 IkReal gconst31;
4488 IkReal x1019=(sj14)*(sj14);
4489 IkReal x1020=(cj14)*(cj14);
4490 IkReal x1021=((cj13)*(r12));
4491 IkReal x1022=((IkReal(1.00000000000000))*(r10));
4492 IkReal x1023=((cj13)*(r02));
4493 IkReal x1024=((r01)*(sj13));
4494 IkReal x1025=((r00)*(r11)*(sj13));
4495 gconst31=IKsign(((((IkReal(-1.00000000000000))*(sj14)*(x1022)*(x1023)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1023)))+(((x1019)*(x1025)))+(((r00)*(sj14)*(x1021)))+(((IkReal(-1.00000000000000))*(x1019)*(x1022)*(x1024)))+(((x1020)*(x1025)))+(((cj14)*(r01)*(x1021)))+(((IkReal(-1.00000000000000))*(x1020)*(x1022)*(x1024)))));
4496 IkReal x1026=(sj14)*(sj14);
4497 IkReal x1027=(cj14)*(cj14);
4498 IkReal x1028=((cj13)*(r12));
4499 IkReal x1029=((IkReal(1.00000000000000))*(r10));
4500 IkReal x1030=((cj13)*(r02));
4501 IkReal x1031=((r01)*(sj13));
4502 IkReal x1032=((r00)*(r11)*(sj13));
4503 dummyeval[0]=((((x1026)*(x1032)))+(((IkReal(-1.00000000000000))*(x1026)*(x1029)*(x1031)))+(((IkReal(-1.00000000000000))*(sj14)*(x1029)*(x1030)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1030)))+(((IkReal(-1.00000000000000))*(x1027)*(x1029)*(x1031)))+(((r00)*(sj14)*(x1028)))+(((x1027)*(x1032)))+(((cj14)*(r01)*(x1028))));
4504 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
4505 {
4506 {
4507 IkReal dummyeval[1];
4508 IkReal gconst30;
4509 IkReal x1033=(sj14)*(sj14);
4510 IkReal x1034=(cj14)*(cj14);
4511 IkReal x1035=((IkReal(1.00000000000000))*(x1033));
4512 IkReal x1036=((IkReal(2.00000000000000))*(cj14)*(sj14));
4513 IkReal x1037=((IkReal(1.00000000000000))*(x1034));
4514 gconst30=IKsign(((((IkReal(-1.00000000000000))*(x1037)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1037)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1036)))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1036)))+(((IkReal(-1.00000000000000))*(x1035)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1035)*((r00)*(r00))))));
4515 IkReal x1038=(sj14)*(sj14);
4516 IkReal x1039=(cj14)*(cj14);
4517 IkReal x1040=((IkReal(1.00000000000000))*(x1038));
4518 IkReal x1041=((IkReal(2.00000000000000))*(cj14)*(sj14));
4519 IkReal x1042=((IkReal(1.00000000000000))*(x1039));
4520 dummyeval[0]=((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1041)))+(((IkReal(-1.00000000000000))*(x1042)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1040)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(x1040)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1041)))+(((IkReal(-1.00000000000000))*(x1042)*((r11)*(r11)))));
4521 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
4522 {
4523 continue;
4524 
4525 } else
4526 {
4527 {
4528 IkReal j9array[1], cj9array[1], sj9array[1];
4529 bool j9valid[1]={false};
4530 _nj9 = 1;
4531 IkReal x1043=((sj10)*(sj11));
4532 IkReal x1044=((r00)*(sj14));
4533 IkReal x1045=((cj14)*(r11));
4534 IkReal x1046=((r10)*(sj14));
4535 IkReal x1047=((cj14)*(r01));
4536 IkReal x1048=((IkReal(1.00000000000000))*(cj10)*(cj11));
4537 if( IKabs(((gconst30)*(((((IkReal(-1.00000000000000))*(x1046)*(x1048)))+(((x1043)*(x1045)))+(((x1043)*(x1046)))+(((IkReal(-1.00000000000000))*(x1045)*(x1048))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst30)*(((((x1043)*(x1044)))+(((x1043)*(x1047)))+(((IkReal(-1.00000000000000))*(x1044)*(x1048)))+(((IkReal(-1.00000000000000))*(x1047)*(x1048))))))) < IKFAST_ATAN2_MAGTHRESH )
4538  continue;
4539 j9array[0]=IKatan2(((gconst30)*(((((IkReal(-1.00000000000000))*(x1046)*(x1048)))+(((x1043)*(x1045)))+(((x1043)*(x1046)))+(((IkReal(-1.00000000000000))*(x1045)*(x1048)))))), ((gconst30)*(((((x1043)*(x1044)))+(((x1043)*(x1047)))+(((IkReal(-1.00000000000000))*(x1044)*(x1048)))+(((IkReal(-1.00000000000000))*(x1047)*(x1048)))))));
4540 sj9array[0]=IKsin(j9array[0]);
4541 cj9array[0]=IKcos(j9array[0]);
4542 if( j9array[0] > IKPI )
4543 {
4544  j9array[0]-=IK2PI;
4545 }
4546 else if( j9array[0] < -IKPI )
4547 { j9array[0]+=IK2PI;
4548 }
4549 j9valid[0] = true;
4550 for(int ij9 = 0; ij9 < 1; ++ij9)
4551 {
4552 if( !j9valid[ij9] )
4553 {
4554  continue;
4555 }
4556 _ij9[0] = ij9; _ij9[1] = -1;
4557 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
4558 {
4559 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
4560 {
4561  j9valid[iij9]=false; _ij9[1] = iij9; break;
4562 }
4563 }
4564 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
4565 {
4566 IkReal evalcond[6];
4567 IkReal x1049=IKsin(j9);
4568 IkReal x1050=IKcos(j9);
4569 IkReal x1051=((IkReal(1.00000000000000))*(cj14));
4570 IkReal x1052=((IkReal(1.00000000000000))*(sj14));
4571 IkReal x1053=((cj13)*(cj14));
4572 IkReal x1054=((cj14)*(r10));
4573 IkReal x1055=((r01)*(sj14));
4574 IkReal x1056=((IkReal(1.00000000000000))*(r12));
4575 IkReal x1057=((sj13)*(x1050));
4576 IkReal x1058=((r02)*(x1049));
4577 IkReal x1059=((r11)*(x1049));
4578 IkReal x1060=((r10)*(x1050));
4579 IkReal x1061=((r01)*(x1050));
4580 IkReal x1062=((sj13)*(x1049));
4581 IkReal x1063=((r11)*(x1050));
4582 IkReal x1064=((cj13)*(x1049));
4583 IkReal x1065=((r10)*(x1049));
4584 IkReal x1066=((r00)*(x1050));
4585 IkReal x1067=((cj13)*(x1050));
4586 evalcond[0]=((((cj14)*(r01)*(x1049)))+(((IkReal(-1.00000000000000))*(x1052)*(x1060)))+(((IkReal(-1.00000000000000))*(x1051)*(x1063)))+(((r00)*(sj14)*(x1049))));
4587 evalcond[1]=((((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(x1052)*(x1066)))+(((IkReal(-1.00000000000000))*(x1052)*(x1065)))+(((IkReal(-1.00000000000000))*(x1051)*(x1061)))+(((cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(x1051)*(x1059))));
4588 evalcond[2]=((((r12)*(x1057)))+(((x1055)*(x1064)))+(((IkReal(-1.00000000000000))*(cj13)*(x1052)*(x1063)))+(((IkReal(-1.00000000000000))*(r00)*(x1051)*(x1064)))+(((x1053)*(x1060)))+(((IkReal(-1.00000000000000))*(sj13)*(x1058))));
4589 evalcond[3]=((IkReal(-1.00000000000000))+(((x1054)*(x1057)))+(((x1055)*(x1062)))+(((IkReal(-1.00000000000000))*(r11)*(x1052)*(x1057)))+(((IkReal(-1.00000000000000))*(r00)*(x1051)*(x1062)))+(((IkReal(-1.00000000000000))*(x1056)*(x1067)))+(((cj13)*(x1058))));
4590 evalcond[4]=((((IkReal(-1.00000000000000))*(cj13)*(x1052)*(x1059)))+(((r12)*(x1062)))+(((r02)*(x1057)))+(((IkReal(-1.00000000000000))*(cj13)*(x1052)*(x1061)))+(((cj10)*(sj11)))+(((x1053)*(x1065)))+(((x1053)*(x1066)))+(((cj11)*(sj10))));
4591 evalcond[5]=((((IkReal(-1.00000000000000))*(sj13)*(x1052)*(x1059)))+(((x1054)*(x1062)))+(((IkReal(-1.00000000000000))*(r02)*(x1067)))+(((cj14)*(r00)*(x1057)))+(((IkReal(-1.00000000000000))*(x1056)*(x1064)))+(((IkReal(-1.00000000000000))*(r01)*(x1052)*(x1057))));
4592 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 )
4593 {
4594 continue;
4595 }
4596 }
4597 
4598 {
4599 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4600 vinfos[0].jointtype = 1;
4601 vinfos[0].foffset = j9;
4602 vinfos[0].indices[0] = _ij9[0];
4603 vinfos[0].indices[1] = _ij9[1];
4604 vinfos[0].maxsolutions = _nj9;
4605 vinfos[1].jointtype = 1;
4606 vinfos[1].foffset = j10;
4607 vinfos[1].indices[0] = _ij10[0];
4608 vinfos[1].indices[1] = _ij10[1];
4609 vinfos[1].maxsolutions = _nj10;
4610 vinfos[2].jointtype = 1;
4611 vinfos[2].foffset = j11;
4612 vinfos[2].indices[0] = _ij11[0];
4613 vinfos[2].indices[1] = _ij11[1];
4614 vinfos[2].maxsolutions = _nj11;
4615 vinfos[3].jointtype = 1;
4616 vinfos[3].foffset = j12;
4617 vinfos[3].indices[0] = _ij12[0];
4618 vinfos[3].indices[1] = _ij12[1];
4619 vinfos[3].maxsolutions = _nj12;
4620 vinfos[4].jointtype = 1;
4621 vinfos[4].foffset = j13;
4622 vinfos[4].indices[0] = _ij13[0];
4623 vinfos[4].indices[1] = _ij13[1];
4624 vinfos[4].maxsolutions = _nj13;
4625 vinfos[5].jointtype = 1;
4626 vinfos[5].foffset = j14;
4627 vinfos[5].indices[0] = _ij14[0];
4628 vinfos[5].indices[1] = _ij14[1];
4629 vinfos[5].maxsolutions = _nj14;
4630 std::vector<int> vfree(0);
4631 solutions.AddSolution(vinfos,vfree);
4632 }
4633 }
4634 }
4635 
4636 }
4637 
4638 }
4639 
4640 } else
4641 {
4642 {
4643 IkReal j9array[1], cj9array[1], sj9array[1];
4644 bool j9valid[1]={false};
4645 _nj9 = 1;
4646 IkReal x1068=((IkReal(1.00000000000000))*(sj14));
4647 IkReal x1069=((IkReal(1.00000000000000))*(cj14));
4648 if( IKabs(((gconst31)*(((((IkReal(-1.00000000000000))*(r11)*(x1069)))+(((IkReal(-1.00000000000000))*(r10)*(x1068))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst31)*(((((IkReal(-1.00000000000000))*(r01)*(x1069)))+(((IkReal(-1.00000000000000))*(r00)*(x1068))))))) < IKFAST_ATAN2_MAGTHRESH )
4649  continue;
4650 j9array[0]=IKatan2(((gconst31)*(((((IkReal(-1.00000000000000))*(r11)*(x1069)))+(((IkReal(-1.00000000000000))*(r10)*(x1068)))))), ((gconst31)*(((((IkReal(-1.00000000000000))*(r01)*(x1069)))+(((IkReal(-1.00000000000000))*(r00)*(x1068)))))));
4651 sj9array[0]=IKsin(j9array[0]);
4652 cj9array[0]=IKcos(j9array[0]);
4653 if( j9array[0] > IKPI )
4654 {
4655  j9array[0]-=IK2PI;
4656 }
4657 else if( j9array[0] < -IKPI )
4658 { j9array[0]+=IK2PI;
4659 }
4660 j9valid[0] = true;
4661 for(int ij9 = 0; ij9 < 1; ++ij9)
4662 {
4663 if( !j9valid[ij9] )
4664 {
4665  continue;
4666 }
4667 _ij9[0] = ij9; _ij9[1] = -1;
4668 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
4669 {
4670 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
4671 {
4672  j9valid[iij9]=false; _ij9[1] = iij9; break;
4673 }
4674 }
4675 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
4676 {
4677 IkReal evalcond[6];
4678 IkReal x1070=IKsin(j9);
4679 IkReal x1071=IKcos(j9);
4680 IkReal x1072=((IkReal(1.00000000000000))*(cj14));
4681 IkReal x1073=((IkReal(1.00000000000000))*(sj14));
4682 IkReal x1074=((cj13)*(cj14));
4683 IkReal x1075=((cj14)*(r10));
4684 IkReal x1076=((r01)*(sj14));
4685 IkReal x1077=((IkReal(1.00000000000000))*(r12));
4686 IkReal x1078=((sj13)*(x1071));
4687 IkReal x1079=((r02)*(x1070));
4688 IkReal x1080=((r11)*(x1070));
4689 IkReal x1081=((r10)*(x1071));
4690 IkReal x1082=((r01)*(x1071));
4691 IkReal x1083=((sj13)*(x1070));
4692 IkReal x1084=((r11)*(x1071));
4693 IkReal x1085=((cj13)*(x1070));
4694 IkReal x1086=((r10)*(x1070));
4695 IkReal x1087=((r00)*(x1071));
4696 IkReal x1088=((cj13)*(x1071));
4697 evalcond[0]=((((cj14)*(r01)*(x1070)))+(((IkReal(-1.00000000000000))*(x1073)*(x1081)))+(((r00)*(sj14)*(x1070)))+(((IkReal(-1.00000000000000))*(x1072)*(x1084))));
4698 evalcond[1]=((((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(x1073)*(x1086)))+(((IkReal(-1.00000000000000))*(x1073)*(x1087)))+(((cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(x1072)*(x1082)))+(((IkReal(-1.00000000000000))*(x1072)*(x1080))));
4699 evalcond[2]=((((IkReal(-1.00000000000000))*(sj13)*(x1079)))+(((x1076)*(x1085)))+(((r12)*(x1078)))+(((IkReal(-1.00000000000000))*(r00)*(x1072)*(x1085)))+(((IkReal(-1.00000000000000))*(cj13)*(x1073)*(x1084)))+(((x1074)*(x1081))));
4700 evalcond[3]=((IkReal(-1.00000000000000))+(((x1076)*(x1083)))+(((IkReal(-1.00000000000000))*(r00)*(x1072)*(x1083)))+(((IkReal(-1.00000000000000))*(r11)*(x1073)*(x1078)))+(((x1075)*(x1078)))+(((IkReal(-1.00000000000000))*(x1077)*(x1088)))+(((cj13)*(x1079))));
4701 evalcond[4]=((((IkReal(-1.00000000000000))*(cj13)*(x1073)*(x1082)))+(((IkReal(-1.00000000000000))*(cj13)*(x1073)*(x1080)))+(((r12)*(x1083)))+(((cj10)*(sj11)))+(((x1074)*(x1087)))+(((x1074)*(x1086)))+(((cj11)*(sj10)))+(((r02)*(x1078))));
4702 evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(x1088)))+(((x1075)*(x1083)))+(((IkReal(-1.00000000000000))*(r01)*(x1073)*(x1078)))+(((IkReal(-1.00000000000000))*(sj13)*(x1073)*(x1080)))+(((cj14)*(r00)*(x1078)))+(((IkReal(-1.00000000000000))*(x1077)*(x1085))));
4703 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 )
4704 {
4705 continue;
4706 }
4707 }
4708 
4709 {
4710 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4711 vinfos[0].jointtype = 1;
4712 vinfos[0].foffset = j9;
4713 vinfos[0].indices[0] = _ij9[0];
4714 vinfos[0].indices[1] = _ij9[1];
4715 vinfos[0].maxsolutions = _nj9;
4716 vinfos[1].jointtype = 1;
4717 vinfos[1].foffset = j10;
4718 vinfos[1].indices[0] = _ij10[0];
4719 vinfos[1].indices[1] = _ij10[1];
4720 vinfos[1].maxsolutions = _nj10;
4721 vinfos[2].jointtype = 1;
4722 vinfos[2].foffset = j11;
4723 vinfos[2].indices[0] = _ij11[0];
4724 vinfos[2].indices[1] = _ij11[1];
4725 vinfos[2].maxsolutions = _nj11;
4726 vinfos[3].jointtype = 1;
4727 vinfos[3].foffset = j12;
4728 vinfos[3].indices[0] = _ij12[0];
4729 vinfos[3].indices[1] = _ij12[1];
4730 vinfos[3].maxsolutions = _nj12;
4731 vinfos[4].jointtype = 1;
4732 vinfos[4].foffset = j13;
4733 vinfos[4].indices[0] = _ij13[0];
4734 vinfos[4].indices[1] = _ij13[1];
4735 vinfos[4].maxsolutions = _nj13;
4736 vinfos[5].jointtype = 1;
4737 vinfos[5].foffset = j14;
4738 vinfos[5].indices[0] = _ij14[0];
4739 vinfos[5].indices[1] = _ij14[1];
4740 vinfos[5].maxsolutions = _nj14;
4741 std::vector<int> vfree(0);
4742 solutions.AddSolution(vinfos,vfree);
4743 }
4744 }
4745 }
4746 
4747 }
4748 
4749 }
4750 }
4751 }
4752 
4753 }
4754 
4755 }
4756 
4757 } else
4758 {
4759 IkReal x1089=((IkReal(1.00000000000000))*(sj13));
4760 IkReal x1090=((cj14)*(npx));
4761 IkReal x1091=((npy)*(sj14));
4762 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
4763 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
4764 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
4765 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((cj13)*(x1091)))+(((IkReal(-1.00000000000000))*(cj13)*(x1090)))+(((IkReal(-1.00000000000000))*(npz)*(x1089))));
4766 evalcond[4]=((IkReal(0.0300000000000000))+(((sj13)*(x1091)))+(((IkReal(-0.250000000000000))*(sj11)))+(((IkReal(-1.00000000000000))*(x1089)*(x1090)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
4767 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 )
4768 {
4769 {
4770 IkReal dummyeval[1];
4771 IkReal gconst32;
4772 IkReal x1092=(sj14)*(sj14);
4773 IkReal x1093=(cj14)*(cj14);
4774 IkReal x1094=((IkReal(2.00000000000000))*(cj14)*(sj14));
4775 gconst32=IKsign(((((x1092)*((r10)*(r10))))+(((r00)*(r01)*(x1094)))+(((x1093)*((r01)*(r01))))+(((r10)*(r11)*(x1094)))+(((x1092)*((r00)*(r00))))+(((x1093)*((r11)*(r11))))));
4776 IkReal x1095=(sj14)*(sj14);
4777 IkReal x1096=(cj14)*(cj14);
4778 IkReal x1097=((IkReal(2.00000000000000))*(cj14)*(sj14));
4779 dummyeval[0]=((((x1096)*((r01)*(r01))))+(((x1095)*((r10)*(r10))))+(((x1095)*((r00)*(r00))))+(((x1096)*((r11)*(r11))))+(((r00)*(r01)*(x1097)))+(((r10)*(r11)*(x1097))));
4780 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
4781 {
4782 {
4783 IkReal dummyeval[1];
4784 IkReal gconst34;
4785 gconst34=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
4786 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
4787 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
4788 {
4789 {
4790 IkReal dummyeval[1];
4791 IkReal gconst33;
4792 IkReal x1098=(cj14)*(cj14);
4793 IkReal x1099=(sj14)*(sj14);
4794 IkReal x1100=((IkReal(1.00000000000000))*(r01));
4795 IkReal x1101=((sj13)*(sj14));
4796 IkReal x1102=((cj14)*(sj13));
4797 IkReal x1103=((r00)*(r11));
4798 IkReal x1104=((cj13)*(x1098));
4799 IkReal x1105=((cj13)*(x1099));
4800 gconst33=IKsign(((((IkReal(-1.00000000000000))*(r10)*(x1100)*(x1104)))+(((IkReal(-1.00000000000000))*(r10)*(x1100)*(x1105)))+(((IkReal(-1.00000000000000))*(r12)*(x1100)*(x1102)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x1101)))+(((r02)*(r11)*(x1102)))+(((r02)*(r10)*(x1101)))+(((x1103)*(x1105)))+(((x1103)*(x1104)))));
4801 IkReal x1106=(cj14)*(cj14);
4802 IkReal x1107=(sj14)*(sj14);
4803 IkReal x1108=((IkReal(1.00000000000000))*(r01));
4804 IkReal x1109=((sj13)*(sj14));
4805 IkReal x1110=((cj14)*(sj13));
4806 IkReal x1111=((r00)*(r11));
4807 IkReal x1112=((cj13)*(x1106));
4808 IkReal x1113=((cj13)*(x1107));
4809 dummyeval[0]=((((x1111)*(x1112)))+(((x1111)*(x1113)))+(((IkReal(-1.00000000000000))*(r12)*(x1108)*(x1110)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x1109)))+(((r02)*(r11)*(x1110)))+(((r02)*(r10)*(x1109)))+(((IkReal(-1.00000000000000))*(r10)*(x1108)*(x1113)))+(((IkReal(-1.00000000000000))*(r10)*(x1108)*(x1112))));
4810 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
4811 {
4812 continue;
4813 
4814 } else
4815 {
4816 {
4817 IkReal j9array[1], cj9array[1], sj9array[1];
4818 bool j9valid[1]={false};
4819 _nj9 = 1;
4820 IkReal x1114=((cj13)*(cj14));
4821 IkReal x1115=((IkReal(1.00000000000000))*(cj13)*(sj14));
4822 if( IKabs(((gconst33)*(((((IkReal(-1.00000000000000))*(r11)*(x1115)))+(((r12)*(sj13)))+(((r10)*(x1114))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst33)*(((((IkReal(-1.00000000000000))*(r01)*(x1115)))+(((r02)*(sj13)))+(((r00)*(x1114))))))) < IKFAST_ATAN2_MAGTHRESH )
4823  continue;
4824 j9array[0]=IKatan2(((gconst33)*(((((IkReal(-1.00000000000000))*(r11)*(x1115)))+(((r12)*(sj13)))+(((r10)*(x1114)))))), ((gconst33)*(((((IkReal(-1.00000000000000))*(r01)*(x1115)))+(((r02)*(sj13)))+(((r00)*(x1114)))))));
4825 sj9array[0]=IKsin(j9array[0]);
4826 cj9array[0]=IKcos(j9array[0]);
4827 if( j9array[0] > IKPI )
4828 {
4829  j9array[0]-=IK2PI;
4830 }
4831 else if( j9array[0] < -IKPI )
4832 { j9array[0]+=IK2PI;
4833 }
4834 j9valid[0] = true;
4835 for(int ij9 = 0; ij9 < 1; ++ij9)
4836 {
4837 if( !j9valid[ij9] )
4838 {
4839  continue;
4840 }
4841 _ij9[0] = ij9; _ij9[1] = -1;
4842 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
4843 {
4844 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
4845 {
4846  j9valid[iij9]=false; _ij9[1] = iij9; break;
4847 }
4848 }
4849 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
4850 {
4851 IkReal evalcond[4];
4852 IkReal x1116=IKsin(j9);
4853 IkReal x1117=IKcos(j9);
4854 IkReal x1118=((r10)*(sj14));
4855 IkReal x1119=((cj14)*(r11));
4856 IkReal x1120=((cj14)*(r10));
4857 IkReal x1121=((cj14)*(r00));
4858 IkReal x1122=((r11)*(sj14));
4859 IkReal x1123=((r00)*(sj14));
4860 IkReal x1124=((IkReal(1.00000000000000))*(x1116));
4861 IkReal x1125=((cj13)*(x1116));
4862 IkReal x1126=((sj13)*(x1117));
4863 IkReal x1127=((r01)*(x1116));
4864 IkReal x1128=((IkReal(1.00000000000000))*(x1117));
4865 evalcond[0]=((IkReal(1.00000000000000))+(((cj14)*(x1127)))+(((IkReal(-1.00000000000000))*(x1119)*(x1128)))+(((x1116)*(x1123)))+(((IkReal(-1.00000000000000))*(x1118)*(x1128))));
4866 evalcond[1]=((((IkReal(-1.00000000000000))*(x1123)*(x1128)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1128)))+(((IkReal(-1.00000000000000))*(x1119)*(x1124)))+(((IkReal(-1.00000000000000))*(x1118)*(x1124))));
4867 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x1122)*(x1128)))+(((cj13)*(x1117)*(x1120)))+(((r01)*(sj14)*(x1125)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x1124)))+(((r12)*(x1126)))+(((IkReal(-1.00000000000000))*(cj13)*(x1121)*(x1124))));
4868 evalcond[3]=((((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1128)))+(((x1120)*(x1126)))+(((sj13)*(sj14)*(x1127)))+(((IkReal(-1.00000000000000))*(x1122)*(x1126)))+(((IkReal(-1.00000000000000))*(sj13)*(x1121)*(x1124)))+(((r02)*(x1125))));
4869 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
4870 {
4871 continue;
4872 }
4873 }
4874 
4875 {
4876 IkReal dummyeval[1];
4877 IkReal gconst35;
4878 gconst35=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
4879 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
4880 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
4881 {
4882 {
4883 IkReal dummyeval[1];
4884 IkReal gconst36;
4885 gconst36=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
4886 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
4887 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
4888 {
4889 continue;
4890 
4891 } else
4892 {
4893 {
4894 IkReal j10array[1], cj10array[1], sj10array[1];
4895 bool j10valid[1]={false};
4896 _nj10 = 1;
4897 IkReal x1129=((cj13)*(sj14));
4898 IkReal x1130=((IkReal(1.00000000000000))*(cj11));
4899 IkReal x1131=((r11)*(sj9));
4900 IkReal x1132=((cj13)*(cj14));
4901 IkReal x1133=((IkReal(1.00000000000000))*(sj11));
4902 IkReal x1134=((cj11)*(sj13));
4903 IkReal x1135=((r10)*(sj9));
4904 IkReal x1136=((sj11)*(sj13));
4905 IkReal x1137=((cj9)*(r02));
4906 IkReal x1138=((r12)*(sj9));
4907 IkReal x1139=((cj9)*(r00));
4908 IkReal x1140=((cj9)*(r01));
4909 if( IKabs(((gconst36)*(((((r21)*(sj11)*(x1129)))+(((IkReal(-1.00000000000000))*(x1129)*(x1130)*(x1140)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1133)))+(((x1134)*(x1137)))+(((x1134)*(x1138)))+(((cj11)*(x1132)*(x1139)))+(((cj11)*(x1132)*(x1135)))+(((IkReal(-1.00000000000000))*(x1129)*(x1130)*(x1131)))+(((IkReal(-1.00000000000000))*(r20)*(x1132)*(x1133))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst36)*(((((cj11)*(r20)*(x1132)))+(((sj11)*(x1132)*(x1139)))+(((sj11)*(x1132)*(x1135)))+(((IkReal(-1.00000000000000))*(x1129)*(x1131)*(x1133)))+(((x1136)*(x1137)))+(((x1136)*(x1138)))+(((IkReal(-1.00000000000000))*(r21)*(x1129)*(x1130)))+(((IkReal(-1.00000000000000))*(x1129)*(x1133)*(x1140)))+(((r22)*(x1134))))))) < IKFAST_ATAN2_MAGTHRESH )
4910  continue;
4911 j10array[0]=IKatan2(((gconst36)*(((((r21)*(sj11)*(x1129)))+(((IkReal(-1.00000000000000))*(x1129)*(x1130)*(x1140)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1133)))+(((x1134)*(x1137)))+(((x1134)*(x1138)))+(((cj11)*(x1132)*(x1139)))+(((cj11)*(x1132)*(x1135)))+(((IkReal(-1.00000000000000))*(x1129)*(x1130)*(x1131)))+(((IkReal(-1.00000000000000))*(r20)*(x1132)*(x1133)))))), ((gconst36)*(((((cj11)*(r20)*(x1132)))+(((sj11)*(x1132)*(x1139)))+(((sj11)*(x1132)*(x1135)))+(((IkReal(-1.00000000000000))*(x1129)*(x1131)*(x1133)))+(((x1136)*(x1137)))+(((x1136)*(x1138)))+(((IkReal(-1.00000000000000))*(r21)*(x1129)*(x1130)))+(((IkReal(-1.00000000000000))*(x1129)*(x1133)*(x1140)))+(((r22)*(x1134)))))));
4912 sj10array[0]=IKsin(j10array[0]);
4913 cj10array[0]=IKcos(j10array[0]);
4914 if( j10array[0] > IKPI )
4915 {
4916  j10array[0]-=IK2PI;
4917 }
4918 else if( j10array[0] < -IKPI )
4919 { j10array[0]+=IK2PI;
4920 }
4921 j10valid[0] = true;
4922 for(int ij10 = 0; ij10 < 1; ++ij10)
4923 {
4924 if( !j10valid[ij10] )
4925 {
4926  continue;
4927 }
4928 _ij10[0] = ij10; _ij10[1] = -1;
4929 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
4930 {
4931 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
4932 {
4933  j10valid[iij10]=false; _ij10[1] = iij10; break;
4934 }
4935 }
4936 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
4937 {
4938 IkReal evalcond[4];
4939 IkReal x1141=IKsin(j10);
4940 IkReal x1142=IKcos(j10);
4941 IkReal x1143=((IkReal(1.00000000000000))*(sj13));
4942 IkReal x1144=((r11)*(sj9));
4943 IkReal x1145=((cj9)*(r01));
4944 IkReal x1146=((r21)*(sj14));
4945 IkReal x1147=((cj9)*(r02));
4946 IkReal x1148=((sj13)*(sj9));
4947 IkReal x1149=((cj14)*(r10));
4948 IkReal x1150=((IkReal(1.00000000000000))*(cj13));
4949 IkReal x1151=((cj14)*(r20));
4950 IkReal x1152=((cj11)*(x1141));
4951 IkReal x1153=((sj11)*(x1142));
4952 IkReal x1154=((sj14)*(x1150));
4953 IkReal x1155=((sj11)*(x1141));
4954 IkReal x1156=((cj11)*(x1142));
4955 IkReal x1157=((cj14)*(cj9)*(r00));
4956 IkReal x1158=((x1153)+(x1152));
4957 evalcond[0]=((x1155)+(((cj13)*(x1146)))+(((IkReal(-1.00000000000000))*(x1156)))+(((IkReal(-1.00000000000000))*(r22)*(x1143)))+(((IkReal(-1.00000000000000))*(x1150)*(x1151))));
4958 evalcond[1]=((x1158)+(((IkReal(-1.00000000000000))*(x1143)*(x1151)))+(((sj13)*(x1146)))+(((cj13)*(r22))));
4959 evalcond[2]=((((IkReal(-1.00000000000000))*(x1144)*(x1154)))+(x1158)+(((cj13)*(sj9)*(x1149)))+(((cj13)*(x1157)))+(((sj13)*(x1147)))+(((IkReal(-1.00000000000000))*(x1145)*(x1154)))+(((r12)*(x1148))));
4960 evalcond[3]=((x1156)+(((IkReal(-1.00000000000000))*(x1147)*(x1150)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1150)))+(((IkReal(-1.00000000000000))*(x1155)))+(((sj13)*(x1157)))+(((IkReal(-1.00000000000000))*(sj14)*(x1143)*(x1144)))+(((IkReal(-1.00000000000000))*(sj14)*(x1143)*(x1145)))+(((x1148)*(x1149))));
4961 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
4962 {
4963 continue;
4964 }
4965 }
4966 
4967 {
4968 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
4969 vinfos[0].jointtype = 1;
4970 vinfos[0].foffset = j9;
4971 vinfos[0].indices[0] = _ij9[0];
4972 vinfos[0].indices[1] = _ij9[1];
4973 vinfos[0].maxsolutions = _nj9;
4974 vinfos[1].jointtype = 1;
4975 vinfos[1].foffset = j10;
4976 vinfos[1].indices[0] = _ij10[0];
4977 vinfos[1].indices[1] = _ij10[1];
4978 vinfos[1].maxsolutions = _nj10;
4979 vinfos[2].jointtype = 1;
4980 vinfos[2].foffset = j11;
4981 vinfos[2].indices[0] = _ij11[0];
4982 vinfos[2].indices[1] = _ij11[1];
4983 vinfos[2].maxsolutions = _nj11;
4984 vinfos[3].jointtype = 1;
4985 vinfos[3].foffset = j12;
4986 vinfos[3].indices[0] = _ij12[0];
4987 vinfos[3].indices[1] = _ij12[1];
4988 vinfos[3].maxsolutions = _nj12;
4989 vinfos[4].jointtype = 1;
4990 vinfos[4].foffset = j13;
4991 vinfos[4].indices[0] = _ij13[0];
4992 vinfos[4].indices[1] = _ij13[1];
4993 vinfos[4].maxsolutions = _nj13;
4994 vinfos[5].jointtype = 1;
4995 vinfos[5].foffset = j14;
4996 vinfos[5].indices[0] = _ij14[0];
4997 vinfos[5].indices[1] = _ij14[1];
4998 vinfos[5].maxsolutions = _nj14;
4999 std::vector<int> vfree(0);
5000 solutions.AddSolution(vinfos,vfree);
5001 }
5002 }
5003 }
5004 
5005 }
5006 
5007 }
5008 
5009 } else
5010 {
5011 {
5012 IkReal j10array[1], cj10array[1], sj10array[1];
5013 bool j10valid[1]={false};
5014 _nj10 = 1;
5015 IkReal x1159=((cj13)*(sj11));
5016 IkReal x1160=((r21)*(sj14));
5017 IkReal x1161=((cj11)*(cj13));
5018 IkReal x1162=((cj11)*(sj13));
5019 IkReal x1163=((sj11)*(sj13));
5020 IkReal x1164=((IkReal(1.00000000000000))*(cj14)*(r20));
5021 if( IKabs(((gconst35)*(((((x1159)*(x1160)))+(((IkReal(-1.00000000000000))*(x1159)*(x1164)))+(((IkReal(-1.00000000000000))*(x1162)*(x1164)))+(((IkReal(-1.00000000000000))*(r22)*(x1163)))+(((r22)*(x1161)))+(((x1160)*(x1162))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst35)*(((((cj14)*(r20)*(x1161)))+(((r22)*(x1159)))+(((IkReal(-1.00000000000000))*(x1163)*(x1164)))+(((r22)*(x1162)))+(((IkReal(-1.00000000000000))*(x1160)*(x1161)))+(((x1160)*(x1163))))))) < IKFAST_ATAN2_MAGTHRESH )
5022  continue;
5023 j10array[0]=IKatan2(((gconst35)*(((((x1159)*(x1160)))+(((IkReal(-1.00000000000000))*(x1159)*(x1164)))+(((IkReal(-1.00000000000000))*(x1162)*(x1164)))+(((IkReal(-1.00000000000000))*(r22)*(x1163)))+(((r22)*(x1161)))+(((x1160)*(x1162)))))), ((gconst35)*(((((cj14)*(r20)*(x1161)))+(((r22)*(x1159)))+(((IkReal(-1.00000000000000))*(x1163)*(x1164)))+(((r22)*(x1162)))+(((IkReal(-1.00000000000000))*(x1160)*(x1161)))+(((x1160)*(x1163)))))));
5024 sj10array[0]=IKsin(j10array[0]);
5025 cj10array[0]=IKcos(j10array[0]);
5026 if( j10array[0] > IKPI )
5027 {
5028  j10array[0]-=IK2PI;
5029 }
5030 else if( j10array[0] < -IKPI )
5031 { j10array[0]+=IK2PI;
5032 }
5033 j10valid[0] = true;
5034 for(int ij10 = 0; ij10 < 1; ++ij10)
5035 {
5036 if( !j10valid[ij10] )
5037 {
5038  continue;
5039 }
5040 _ij10[0] = ij10; _ij10[1] = -1;
5041 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
5042 {
5043 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
5044 {
5045  j10valid[iij10]=false; _ij10[1] = iij10; break;
5046 }
5047 }
5048 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
5049 {
5050 IkReal evalcond[4];
5051 IkReal x1165=IKsin(j10);
5052 IkReal x1166=IKcos(j10);
5053 IkReal x1167=((IkReal(1.00000000000000))*(sj13));
5054 IkReal x1168=((r11)*(sj9));
5055 IkReal x1169=((cj9)*(r01));
5056 IkReal x1170=((r21)*(sj14));
5057 IkReal x1171=((cj9)*(r02));
5058 IkReal x1172=((sj13)*(sj9));
5059 IkReal x1173=((cj14)*(r10));
5060 IkReal x1174=((IkReal(1.00000000000000))*(cj13));
5061 IkReal x1175=((cj14)*(r20));
5062 IkReal x1176=((cj11)*(x1165));
5063 IkReal x1177=((sj11)*(x1166));
5064 IkReal x1178=((sj14)*(x1174));
5065 IkReal x1179=((sj11)*(x1165));
5066 IkReal x1180=((cj11)*(x1166));
5067 IkReal x1181=((cj14)*(cj9)*(r00));
5068 IkReal x1182=((x1177)+(x1176));
5069 evalcond[0]=((((cj13)*(x1170)))+(x1179)+(((IkReal(-1.00000000000000))*(r22)*(x1167)))+(((IkReal(-1.00000000000000))*(x1180)))+(((IkReal(-1.00000000000000))*(x1174)*(x1175))));
5070 evalcond[1]=((x1182)+(((sj13)*(x1170)))+(((IkReal(-1.00000000000000))*(x1167)*(x1175)))+(((cj13)*(r22))));
5071 evalcond[2]=((x1182)+(((cj13)*(sj9)*(x1173)))+(((sj13)*(x1171)))+(((IkReal(-1.00000000000000))*(x1168)*(x1178)))+(((r12)*(x1172)))+(((IkReal(-1.00000000000000))*(x1169)*(x1178)))+(((cj13)*(x1181))));
5072 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1174)))+(x1180)+(((IkReal(-1.00000000000000))*(x1171)*(x1174)))+(((IkReal(-1.00000000000000))*(sj14)*(x1167)*(x1168)))+(((IkReal(-1.00000000000000))*(sj14)*(x1167)*(x1169)))+(((x1172)*(x1173)))+(((sj13)*(x1181)))+(((IkReal(-1.00000000000000))*(x1179))));
5073 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
5074 {
5075 continue;
5076 }
5077 }
5078 
5079 {
5080 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5081 vinfos[0].jointtype = 1;
5082 vinfos[0].foffset = j9;
5083 vinfos[0].indices[0] = _ij9[0];
5084 vinfos[0].indices[1] = _ij9[1];
5085 vinfos[0].maxsolutions = _nj9;
5086 vinfos[1].jointtype = 1;
5087 vinfos[1].foffset = j10;
5088 vinfos[1].indices[0] = _ij10[0];
5089 vinfos[1].indices[1] = _ij10[1];
5090 vinfos[1].maxsolutions = _nj10;
5091 vinfos[2].jointtype = 1;
5092 vinfos[2].foffset = j11;
5093 vinfos[2].indices[0] = _ij11[0];
5094 vinfos[2].indices[1] = _ij11[1];
5095 vinfos[2].maxsolutions = _nj11;
5096 vinfos[3].jointtype = 1;
5097 vinfos[3].foffset = j12;
5098 vinfos[3].indices[0] = _ij12[0];
5099 vinfos[3].indices[1] = _ij12[1];
5100 vinfos[3].maxsolutions = _nj12;
5101 vinfos[4].jointtype = 1;
5102 vinfos[4].foffset = j13;
5103 vinfos[4].indices[0] = _ij13[0];
5104 vinfos[4].indices[1] = _ij13[1];
5105 vinfos[4].maxsolutions = _nj13;
5106 vinfos[5].jointtype = 1;
5107 vinfos[5].foffset = j14;
5108 vinfos[5].indices[0] = _ij14[0];
5109 vinfos[5].indices[1] = _ij14[1];
5110 vinfos[5].maxsolutions = _nj14;
5111 std::vector<int> vfree(0);
5112 solutions.AddSolution(vinfos,vfree);
5113 }
5114 }
5115 }
5116 
5117 }
5118 
5119 }
5120 }
5121 }
5122 
5123 }
5124 
5125 }
5126 
5127 } else
5128 {
5129 {
5130 IkReal j10array[1], cj10array[1], sj10array[1];
5131 bool j10valid[1]={false};
5132 _nj10 = 1;
5133 IkReal x1183=((cj13)*(sj11));
5134 IkReal x1184=((r21)*(sj14));
5135 IkReal x1185=((cj11)*(cj13));
5136 IkReal x1186=((cj11)*(sj13));
5137 IkReal x1187=((sj11)*(sj13));
5138 IkReal x1188=((IkReal(1.00000000000000))*(cj14)*(r20));
5139 if( IKabs(((gconst34)*(((((x1184)*(x1186)))+(((x1183)*(x1184)))+(((r22)*(x1185)))+(((IkReal(-1.00000000000000))*(x1186)*(x1188)))+(((IkReal(-1.00000000000000))*(x1183)*(x1188)))+(((IkReal(-1.00000000000000))*(r22)*(x1187))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst34)*(((((cj14)*(r20)*(x1185)))+(((x1184)*(x1187)))+(((r22)*(x1186)))+(((r22)*(x1183)))+(((IkReal(-1.00000000000000))*(x1187)*(x1188)))+(((IkReal(-1.00000000000000))*(x1184)*(x1185))))))) < IKFAST_ATAN2_MAGTHRESH )
5140  continue;
5141 j10array[0]=IKatan2(((gconst34)*(((((x1184)*(x1186)))+(((x1183)*(x1184)))+(((r22)*(x1185)))+(((IkReal(-1.00000000000000))*(x1186)*(x1188)))+(((IkReal(-1.00000000000000))*(x1183)*(x1188)))+(((IkReal(-1.00000000000000))*(r22)*(x1187)))))), ((gconst34)*(((((cj14)*(r20)*(x1185)))+(((x1184)*(x1187)))+(((r22)*(x1186)))+(((r22)*(x1183)))+(((IkReal(-1.00000000000000))*(x1187)*(x1188)))+(((IkReal(-1.00000000000000))*(x1184)*(x1185)))))));
5142 sj10array[0]=IKsin(j10array[0]);
5143 cj10array[0]=IKcos(j10array[0]);
5144 if( j10array[0] > IKPI )
5145 {
5146  j10array[0]-=IK2PI;
5147 }
5148 else if( j10array[0] < -IKPI )
5149 { j10array[0]+=IK2PI;
5150 }
5151 j10valid[0] = true;
5152 for(int ij10 = 0; ij10 < 1; ++ij10)
5153 {
5154 if( !j10valid[ij10] )
5155 {
5156  continue;
5157 }
5158 _ij10[0] = ij10; _ij10[1] = -1;
5159 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
5160 {
5161 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
5162 {
5163  j10valid[iij10]=false; _ij10[1] = iij10; break;
5164 }
5165 }
5166 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
5167 {
5168 IkReal evalcond[2];
5169 IkReal x1189=IKsin(j10);
5170 IkReal x1190=IKcos(j10);
5171 IkReal x1191=((IkReal(1.00000000000000))*(sj13));
5172 IkReal x1192=((cj14)*(r20));
5173 IkReal x1193=((r21)*(sj14));
5174 evalcond[0]=((((IkReal(-1.00000000000000))*(cj11)*(x1190)))+(((IkReal(-1.00000000000000))*(r22)*(x1191)))+(((sj11)*(x1189)))+(((IkReal(-1.00000000000000))*(cj13)*(x1192)))+(((cj13)*(x1193))));
5175 evalcond[1]=((((IkReal(-1.00000000000000))*(x1191)*(x1192)))+(((sj11)*(x1190)))+(((cj11)*(x1189)))+(((sj13)*(x1193)))+(((cj13)*(r22))));
5176 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
5177 {
5178 continue;
5179 }
5180 }
5181 
5182 {
5183 IkReal dummyeval[1];
5184 IkReal gconst37;
5185 IkReal x1194=(sj14)*(sj14);
5186 IkReal x1195=(cj14)*(cj14);
5187 IkReal x1196=((IkReal(2.00000000000000))*(cj14)*(sj14));
5188 gconst37=IKsign(((((x1194)*((r10)*(r10))))+(((x1195)*((r01)*(r01))))+(((r10)*(r11)*(x1196)))+(((x1195)*((r11)*(r11))))+(((x1194)*((r00)*(r00))))+(((r00)*(r01)*(x1196)))));
5189 IkReal x1197=(sj14)*(sj14);
5190 IkReal x1198=(cj14)*(cj14);
5191 IkReal x1199=((IkReal(2.00000000000000))*(cj14)*(sj14));
5192 dummyeval[0]=((((x1197)*((r00)*(r00))))+(((r10)*(r11)*(x1199)))+(((x1198)*((r01)*(r01))))+(((x1197)*((r10)*(r10))))+(((r00)*(r01)*(x1199)))+(((x1198)*((r11)*(r11)))));
5193 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
5194 {
5195 {
5196 IkReal dummyeval[1];
5197 IkReal gconst38;
5198 IkReal x1200=(cj14)*(cj14);
5199 IkReal x1201=(sj14)*(sj14);
5200 IkReal x1202=((IkReal(1.00000000000000))*(r01));
5201 IkReal x1203=((sj13)*(sj14));
5202 IkReal x1204=((cj14)*(sj13));
5203 IkReal x1205=((r00)*(r11));
5204 IkReal x1206=((cj13)*(x1200));
5205 IkReal x1207=((cj13)*(x1201));
5206 gconst38=IKsign(((((IkReal(-1.00000000000000))*(r12)*(x1202)*(x1204)))+(((x1205)*(x1206)))+(((x1205)*(x1207)))+(((IkReal(-1.00000000000000))*(r10)*(x1202)*(x1207)))+(((IkReal(-1.00000000000000))*(r10)*(x1202)*(x1206)))+(((r02)*(r11)*(x1204)))+(((r02)*(r10)*(x1203)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x1203)))));
5207 IkReal x1208=(cj14)*(cj14);
5208 IkReal x1209=(sj14)*(sj14);
5209 IkReal x1210=((IkReal(1.00000000000000))*(r01));
5210 IkReal x1211=((sj13)*(sj14));
5211 IkReal x1212=((cj14)*(sj13));
5212 IkReal x1213=((r00)*(r11));
5213 IkReal x1214=((cj13)*(x1208));
5214 IkReal x1215=((cj13)*(x1209));
5215 dummyeval[0]=((((x1213)*(x1215)))+(((x1213)*(x1214)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x1211)))+(((IkReal(-1.00000000000000))*(r10)*(x1210)*(x1215)))+(((IkReal(-1.00000000000000))*(r10)*(x1210)*(x1214)))+(((IkReal(-1.00000000000000))*(r12)*(x1210)*(x1212)))+(((r02)*(r10)*(x1211)))+(((r02)*(r11)*(x1212))));
5216 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
5217 {
5218 continue;
5219 
5220 } else
5221 {
5222 {
5223 IkReal j9array[1], cj9array[1], sj9array[1];
5224 bool j9valid[1]={false};
5225 _nj9 = 1;
5226 IkReal x1216=((cj13)*(cj14));
5227 IkReal x1217=((IkReal(1.00000000000000))*(cj13)*(sj14));
5228 if( IKabs(((gconst38)*(((((r12)*(sj13)))+(((r10)*(x1216)))+(((IkReal(-1.00000000000000))*(r11)*(x1217))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst38)*(((((r02)*(sj13)))+(((r00)*(x1216)))+(((IkReal(-1.00000000000000))*(r01)*(x1217))))))) < IKFAST_ATAN2_MAGTHRESH )
5229  continue;
5230 j9array[0]=IKatan2(((gconst38)*(((((r12)*(sj13)))+(((r10)*(x1216)))+(((IkReal(-1.00000000000000))*(r11)*(x1217)))))), ((gconst38)*(((((r02)*(sj13)))+(((r00)*(x1216)))+(((IkReal(-1.00000000000000))*(r01)*(x1217)))))));
5231 sj9array[0]=IKsin(j9array[0]);
5232 cj9array[0]=IKcos(j9array[0]);
5233 if( j9array[0] > IKPI )
5234 {
5235  j9array[0]-=IK2PI;
5236 }
5237 else if( j9array[0] < -IKPI )
5238 { j9array[0]+=IK2PI;
5239 }
5240 j9valid[0] = true;
5241 for(int ij9 = 0; ij9 < 1; ++ij9)
5242 {
5243 if( !j9valid[ij9] )
5244 {
5245  continue;
5246 }
5247 _ij9[0] = ij9; _ij9[1] = -1;
5248 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
5249 {
5250 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
5251 {
5252  j9valid[iij9]=false; _ij9[1] = iij9; break;
5253 }
5254 }
5255 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
5256 {
5257 IkReal evalcond[6];
5258 IkReal x1218=IKsin(j9);
5259 IkReal x1219=IKcos(j9);
5260 IkReal x1220=((IkReal(1.00000000000000))*(cj14));
5261 IkReal x1221=((IkReal(1.00000000000000))*(sj14));
5262 IkReal x1222=((cj13)*(cj14));
5263 IkReal x1223=((cj14)*(r10));
5264 IkReal x1224=((r01)*(sj14));
5265 IkReal x1225=((IkReal(1.00000000000000))*(r12));
5266 IkReal x1226=((sj13)*(x1219));
5267 IkReal x1227=((r02)*(x1218));
5268 IkReal x1228=((r11)*(x1218));
5269 IkReal x1229=((r10)*(x1219));
5270 IkReal x1230=((r01)*(x1219));
5271 IkReal x1231=((sj13)*(x1218));
5272 IkReal x1232=((r11)*(x1219));
5273 IkReal x1233=((cj13)*(x1218));
5274 IkReal x1234=((r10)*(x1218));
5275 IkReal x1235=((r00)*(x1219));
5276 IkReal x1236=((cj13)*(x1219));
5277 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1220)*(x1232)))+(((IkReal(-1.00000000000000))*(x1221)*(x1229)))+(((cj14)*(r01)*(x1218)))+(((r00)*(sj14)*(x1218))));
5278 evalcond[1]=((((IkReal(-1.00000000000000))*(x1220)*(x1230)))+(((IkReal(-1.00000000000000))*(x1220)*(x1228)))+(((IkReal(-1.00000000000000))*(x1221)*(x1234)))+(((IkReal(-1.00000000000000))*(x1221)*(x1235))));
5279 evalcond[2]=((((x1224)*(x1233)))+(((x1222)*(x1229)))+(((IkReal(-1.00000000000000))*(r00)*(x1220)*(x1233)))+(((IkReal(-1.00000000000000))*(sj13)*(x1227)))+(((r12)*(x1226)))+(((IkReal(-1.00000000000000))*(cj13)*(x1221)*(x1232))));
5280 evalcond[3]=((((IkReal(-1.00000000000000))*(r11)*(x1221)*(x1226)))+(((IkReal(-1.00000000000000))*(x1225)*(x1236)))+(((x1223)*(x1226)))+(((x1224)*(x1231)))+(((cj13)*(x1227)))+(((IkReal(-1.00000000000000))*(r00)*(x1220)*(x1231))));
5281 evalcond[4]=((((cj10)*(sj11)))+(((r02)*(x1226)))+(((x1222)*(x1235)))+(((x1222)*(x1234)))+(((IkReal(-1.00000000000000))*(cj13)*(x1221)*(x1228)))+(((cj11)*(sj10)))+(((IkReal(-1.00000000000000))*(cj13)*(x1221)*(x1230)))+(((r12)*(x1231))));
5282 evalcond[5]=((((IkReal(-1.00000000000000))*(x1225)*(x1233)))+(((IkReal(-1.00000000000000))*(r01)*(x1221)*(x1226)))+(((x1223)*(x1231)))+(((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(sj13)*(x1221)*(x1228)))+(((IkReal(-1.00000000000000))*(r02)*(x1236)))+(((cj14)*(r00)*(x1226)))+(((cj10)*(cj11))));
5283 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 )
5284 {
5285 continue;
5286 }
5287 }
5288 
5289 {
5290 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5291 vinfos[0].jointtype = 1;
5292 vinfos[0].foffset = j9;
5293 vinfos[0].indices[0] = _ij9[0];
5294 vinfos[0].indices[1] = _ij9[1];
5295 vinfos[0].maxsolutions = _nj9;
5296 vinfos[1].jointtype = 1;
5297 vinfos[1].foffset = j10;
5298 vinfos[1].indices[0] = _ij10[0];
5299 vinfos[1].indices[1] = _ij10[1];
5300 vinfos[1].maxsolutions = _nj10;
5301 vinfos[2].jointtype = 1;
5302 vinfos[2].foffset = j11;
5303 vinfos[2].indices[0] = _ij11[0];
5304 vinfos[2].indices[1] = _ij11[1];
5305 vinfos[2].maxsolutions = _nj11;
5306 vinfos[3].jointtype = 1;
5307 vinfos[3].foffset = j12;
5308 vinfos[3].indices[0] = _ij12[0];
5309 vinfos[3].indices[1] = _ij12[1];
5310 vinfos[3].maxsolutions = _nj12;
5311 vinfos[4].jointtype = 1;
5312 vinfos[4].foffset = j13;
5313 vinfos[4].indices[0] = _ij13[0];
5314 vinfos[4].indices[1] = _ij13[1];
5315 vinfos[4].maxsolutions = _nj13;
5316 vinfos[5].jointtype = 1;
5317 vinfos[5].foffset = j14;
5318 vinfos[5].indices[0] = _ij14[0];
5319 vinfos[5].indices[1] = _ij14[1];
5320 vinfos[5].maxsolutions = _nj14;
5321 std::vector<int> vfree(0);
5322 solutions.AddSolution(vinfos,vfree);
5323 }
5324 }
5325 }
5326 
5327 }
5328 
5329 }
5330 
5331 } else
5332 {
5333 {
5334 IkReal j9array[1], cj9array[1], sj9array[1];
5335 bool j9valid[1]={false};
5336 _nj9 = 1;
5337 if( IKabs(((gconst37)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst37)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
5338  continue;
5339 j9array[0]=IKatan2(((gconst37)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst37)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
5340 sj9array[0]=IKsin(j9array[0]);
5341 cj9array[0]=IKcos(j9array[0]);
5342 if( j9array[0] > IKPI )
5343 {
5344  j9array[0]-=IK2PI;
5345 }
5346 else if( j9array[0] < -IKPI )
5347 { j9array[0]+=IK2PI;
5348 }
5349 j9valid[0] = true;
5350 for(int ij9 = 0; ij9 < 1; ++ij9)
5351 {
5352 if( !j9valid[ij9] )
5353 {
5354  continue;
5355 }
5356 _ij9[0] = ij9; _ij9[1] = -1;
5357 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
5358 {
5359 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
5360 {
5361  j9valid[iij9]=false; _ij9[1] = iij9; break;
5362 }
5363 }
5364 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
5365 {
5366 IkReal evalcond[6];
5367 IkReal x1237=IKsin(j9);
5368 IkReal x1238=IKcos(j9);
5369 IkReal x1239=((IkReal(1.00000000000000))*(cj14));
5370 IkReal x1240=((IkReal(1.00000000000000))*(sj14));
5371 IkReal x1241=((cj13)*(cj14));
5372 IkReal x1242=((cj14)*(r10));
5373 IkReal x1243=((r01)*(sj14));
5374 IkReal x1244=((IkReal(1.00000000000000))*(r12));
5375 IkReal x1245=((sj13)*(x1238));
5376 IkReal x1246=((r02)*(x1237));
5377 IkReal x1247=((r11)*(x1237));
5378 IkReal x1248=((r10)*(x1238));
5379 IkReal x1249=((r01)*(x1238));
5380 IkReal x1250=((sj13)*(x1237));
5381 IkReal x1251=((r11)*(x1238));
5382 IkReal x1252=((cj13)*(x1237));
5383 IkReal x1253=((r10)*(x1237));
5384 IkReal x1254=((r00)*(x1238));
5385 IkReal x1255=((cj13)*(x1238));
5386 evalcond[0]=((IkReal(1.00000000000000))+(((cj14)*(r01)*(x1237)))+(((r00)*(sj14)*(x1237)))+(((IkReal(-1.00000000000000))*(x1239)*(x1251)))+(((IkReal(-1.00000000000000))*(x1240)*(x1248))));
5387 evalcond[1]=((((IkReal(-1.00000000000000))*(x1239)*(x1247)))+(((IkReal(-1.00000000000000))*(x1239)*(x1249)))+(((IkReal(-1.00000000000000))*(x1240)*(x1253)))+(((IkReal(-1.00000000000000))*(x1240)*(x1254))));
5388 evalcond[2]=((((x1243)*(x1252)))+(((IkReal(-1.00000000000000))*(r00)*(x1239)*(x1252)))+(((IkReal(-1.00000000000000))*(cj13)*(x1240)*(x1251)))+(((IkReal(-1.00000000000000))*(sj13)*(x1246)))+(((x1241)*(x1248)))+(((r12)*(x1245))));
5389 evalcond[3]=((((x1243)*(x1250)))+(((IkReal(-1.00000000000000))*(r00)*(x1239)*(x1250)))+(((IkReal(-1.00000000000000))*(x1244)*(x1255)))+(((x1242)*(x1245)))+(((IkReal(-1.00000000000000))*(r11)*(x1240)*(x1245)))+(((cj13)*(x1246))));
5390 evalcond[4]=((((IkReal(-1.00000000000000))*(cj13)*(x1240)*(x1249)))+(((IkReal(-1.00000000000000))*(cj13)*(x1240)*(x1247)))+(((x1241)*(x1254)))+(((x1241)*(x1253)))+(((r12)*(x1250)))+(((cj10)*(sj11)))+(((r02)*(x1245)))+(((cj11)*(sj10))));
5391 evalcond[5]=((((IkReal(-1.00000000000000))*(r01)*(x1240)*(x1245)))+(((IkReal(-1.00000000000000))*(sj13)*(x1240)*(x1247)))+(((IkReal(-1.00000000000000))*(r02)*(x1255)))+(((IkReal(-1.00000000000000))*(sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(x1244)*(x1252)))+(((cj14)*(r00)*(x1245)))+(((cj10)*(cj11)))+(((x1242)*(x1250))));
5392 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 )
5393 {
5394 continue;
5395 }
5396 }
5397 
5398 {
5399 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5400 vinfos[0].jointtype = 1;
5401 vinfos[0].foffset = j9;
5402 vinfos[0].indices[0] = _ij9[0];
5403 vinfos[0].indices[1] = _ij9[1];
5404 vinfos[0].maxsolutions = _nj9;
5405 vinfos[1].jointtype = 1;
5406 vinfos[1].foffset = j10;
5407 vinfos[1].indices[0] = _ij10[0];
5408 vinfos[1].indices[1] = _ij10[1];
5409 vinfos[1].maxsolutions = _nj10;
5410 vinfos[2].jointtype = 1;
5411 vinfos[2].foffset = j11;
5412 vinfos[2].indices[0] = _ij11[0];
5413 vinfos[2].indices[1] = _ij11[1];
5414 vinfos[2].maxsolutions = _nj11;
5415 vinfos[3].jointtype = 1;
5416 vinfos[3].foffset = j12;
5417 vinfos[3].indices[0] = _ij12[0];
5418 vinfos[3].indices[1] = _ij12[1];
5419 vinfos[3].maxsolutions = _nj12;
5420 vinfos[4].jointtype = 1;
5421 vinfos[4].foffset = j13;
5422 vinfos[4].indices[0] = _ij13[0];
5423 vinfos[4].indices[1] = _ij13[1];
5424 vinfos[4].maxsolutions = _nj13;
5425 vinfos[5].jointtype = 1;
5426 vinfos[5].foffset = j14;
5427 vinfos[5].indices[0] = _ij14[0];
5428 vinfos[5].indices[1] = _ij14[1];
5429 vinfos[5].maxsolutions = _nj14;
5430 std::vector<int> vfree(0);
5431 solutions.AddSolution(vinfos,vfree);
5432 }
5433 }
5434 }
5435 
5436 }
5437 
5438 }
5439 }
5440 }
5441 
5442 }
5443 
5444 }
5445 
5446 } else
5447 {
5448 {
5449 IkReal j9array[1], cj9array[1], sj9array[1];
5450 bool j9valid[1]={false};
5451 _nj9 = 1;
5452 if( IKabs(((gconst32)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst32)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
5453  continue;
5454 j9array[0]=IKatan2(((gconst32)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst32)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
5455 sj9array[0]=IKsin(j9array[0]);
5456 cj9array[0]=IKcos(j9array[0]);
5457 if( j9array[0] > IKPI )
5458 {
5459  j9array[0]-=IK2PI;
5460 }
5461 else if( j9array[0] < -IKPI )
5462 { j9array[0]+=IK2PI;
5463 }
5464 j9valid[0] = true;
5465 for(int ij9 = 0; ij9 < 1; ++ij9)
5466 {
5467 if( !j9valid[ij9] )
5468 {
5469  continue;
5470 }
5471 _ij9[0] = ij9; _ij9[1] = -1;
5472 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
5473 {
5474 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
5475 {
5476  j9valid[iij9]=false; _ij9[1] = iij9; break;
5477 }
5478 }
5479 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
5480 {
5481 IkReal evalcond[4];
5482 IkReal x1256=IKsin(j9);
5483 IkReal x1257=IKcos(j9);
5484 IkReal x1258=((r10)*(sj14));
5485 IkReal x1259=((cj14)*(r11));
5486 IkReal x1260=((cj14)*(r10));
5487 IkReal x1261=((cj14)*(r00));
5488 IkReal x1262=((r11)*(sj14));
5489 IkReal x1263=((r00)*(sj14));
5490 IkReal x1264=((IkReal(1.00000000000000))*(x1256));
5491 IkReal x1265=((cj13)*(x1256));
5492 IkReal x1266=((sj13)*(x1257));
5493 IkReal x1267=((r01)*(x1256));
5494 IkReal x1268=((IkReal(1.00000000000000))*(x1257));
5495 evalcond[0]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x1258)*(x1268)))+(((cj14)*(x1267)))+(((x1256)*(x1263)))+(((IkReal(-1.00000000000000))*(x1259)*(x1268))));
5496 evalcond[1]=((((IkReal(-1.00000000000000))*(x1258)*(x1264)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1268)))+(((IkReal(-1.00000000000000))*(x1263)*(x1268)))+(((IkReal(-1.00000000000000))*(x1259)*(x1264))));
5497 evalcond[2]=((((r01)*(sj14)*(x1265)))+(((cj13)*(x1257)*(x1260)))+(((IkReal(-1.00000000000000))*(cj13)*(x1262)*(x1268)))+(((r12)*(x1266)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x1264)))+(((IkReal(-1.00000000000000))*(cj13)*(x1261)*(x1264))));
5498 evalcond[3]=((((r02)*(x1265)))+(((IkReal(-1.00000000000000))*(x1262)*(x1266)))+(((x1260)*(x1266)))+(((sj13)*(sj14)*(x1267)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1268)))+(((IkReal(-1.00000000000000))*(sj13)*(x1261)*(x1264))));
5499 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
5500 {
5501 continue;
5502 }
5503 }
5504 
5505 {
5506 IkReal dummyeval[1];
5507 IkReal gconst35;
5508 gconst35=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
5509 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
5510 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
5511 {
5512 {
5513 IkReal dummyeval[1];
5514 IkReal gconst36;
5515 gconst36=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
5516 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
5517 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
5518 {
5519 continue;
5520 
5521 } else
5522 {
5523 {
5524 IkReal j10array[1], cj10array[1], sj10array[1];
5525 bool j10valid[1]={false};
5526 _nj10 = 1;
5527 IkReal x1269=((cj13)*(sj14));
5528 IkReal x1270=((IkReal(1.00000000000000))*(cj11));
5529 IkReal x1271=((r11)*(sj9));
5530 IkReal x1272=((cj13)*(cj14));
5531 IkReal x1273=((IkReal(1.00000000000000))*(sj11));
5532 IkReal x1274=((cj11)*(sj13));
5533 IkReal x1275=((r10)*(sj9));
5534 IkReal x1276=((sj11)*(sj13));
5535 IkReal x1277=((cj9)*(r02));
5536 IkReal x1278=((r12)*(sj9));
5537 IkReal x1279=((cj9)*(r00));
5538 IkReal x1280=((cj9)*(r01));
5539 if( IKabs(((gconst36)*(((((cj11)*(x1272)*(x1279)))+(((cj11)*(x1272)*(x1275)))+(((x1274)*(x1278)))+(((x1274)*(x1277)))+(((IkReal(-1.00000000000000))*(x1269)*(x1270)*(x1280)))+(((IkReal(-1.00000000000000))*(r20)*(x1272)*(x1273)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1273)))+(((r21)*(sj11)*(x1269)))+(((IkReal(-1.00000000000000))*(x1269)*(x1270)*(x1271))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst36)*(((((IkReal(-1.00000000000000))*(x1269)*(x1273)*(x1280)))+(((x1276)*(x1277)))+(((x1276)*(x1278)))+(((IkReal(-1.00000000000000))*(r21)*(x1269)*(x1270)))+(((sj11)*(x1272)*(x1275)))+(((sj11)*(x1272)*(x1279)))+(((r22)*(x1274)))+(((cj11)*(r20)*(x1272)))+(((IkReal(-1.00000000000000))*(x1269)*(x1271)*(x1273))))))) < IKFAST_ATAN2_MAGTHRESH )
5540  continue;
5541 j10array[0]=IKatan2(((gconst36)*(((((cj11)*(x1272)*(x1279)))+(((cj11)*(x1272)*(x1275)))+(((x1274)*(x1278)))+(((x1274)*(x1277)))+(((IkReal(-1.00000000000000))*(x1269)*(x1270)*(x1280)))+(((IkReal(-1.00000000000000))*(r20)*(x1272)*(x1273)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1273)))+(((r21)*(sj11)*(x1269)))+(((IkReal(-1.00000000000000))*(x1269)*(x1270)*(x1271)))))), ((gconst36)*(((((IkReal(-1.00000000000000))*(x1269)*(x1273)*(x1280)))+(((x1276)*(x1277)))+(((x1276)*(x1278)))+(((IkReal(-1.00000000000000))*(r21)*(x1269)*(x1270)))+(((sj11)*(x1272)*(x1275)))+(((sj11)*(x1272)*(x1279)))+(((r22)*(x1274)))+(((cj11)*(r20)*(x1272)))+(((IkReal(-1.00000000000000))*(x1269)*(x1271)*(x1273)))))));
5542 sj10array[0]=IKsin(j10array[0]);
5543 cj10array[0]=IKcos(j10array[0]);
5544 if( j10array[0] > IKPI )
5545 {
5546  j10array[0]-=IK2PI;
5547 }
5548 else if( j10array[0] < -IKPI )
5549 { j10array[0]+=IK2PI;
5550 }
5551 j10valid[0] = true;
5552 for(int ij10 = 0; ij10 < 1; ++ij10)
5553 {
5554 if( !j10valid[ij10] )
5555 {
5556  continue;
5557 }
5558 _ij10[0] = ij10; _ij10[1] = -1;
5559 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
5560 {
5561 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
5562 {
5563  j10valid[iij10]=false; _ij10[1] = iij10; break;
5564 }
5565 }
5566 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
5567 {
5568 IkReal evalcond[4];
5569 IkReal x1281=IKsin(j10);
5570 IkReal x1282=IKcos(j10);
5571 IkReal x1283=((IkReal(1.00000000000000))*(sj13));
5572 IkReal x1284=((r11)*(sj9));
5573 IkReal x1285=((cj9)*(r01));
5574 IkReal x1286=((r21)*(sj14));
5575 IkReal x1287=((cj9)*(r02));
5576 IkReal x1288=((sj13)*(sj9));
5577 IkReal x1289=((cj14)*(r10));
5578 IkReal x1290=((IkReal(1.00000000000000))*(cj13));
5579 IkReal x1291=((cj14)*(r20));
5580 IkReal x1292=((cj11)*(x1281));
5581 IkReal x1293=((sj11)*(x1282));
5582 IkReal x1294=((sj14)*(x1290));
5583 IkReal x1295=((sj11)*(x1281));
5584 IkReal x1296=((cj11)*(x1282));
5585 IkReal x1297=((cj14)*(cj9)*(r00));
5586 IkReal x1298=((x1292)+(x1293));
5587 evalcond[0]=((x1295)+(((IkReal(-1.00000000000000))*(r22)*(x1283)))+(((IkReal(-1.00000000000000))*(x1290)*(x1291)))+(((IkReal(-1.00000000000000))*(x1296)))+(((cj13)*(x1286))));
5588 evalcond[1]=((x1298)+(((IkReal(-1.00000000000000))*(x1283)*(x1291)))+(((sj13)*(x1286)))+(((cj13)*(r22))));
5589 evalcond[2]=((((IkReal(-1.00000000000000))*(x1284)*(x1294)))+(x1298)+(((cj13)*(x1297)))+(((sj13)*(x1287)))+(((IkReal(-1.00000000000000))*(x1285)*(x1294)))+(((r12)*(x1288)))+(((cj13)*(sj9)*(x1289))));
5590 evalcond[3]=((((x1288)*(x1289)))+(x1296)+(((sj13)*(x1297)))+(((IkReal(-1.00000000000000))*(x1287)*(x1290)))+(((IkReal(-1.00000000000000))*(sj14)*(x1283)*(x1284)))+(((IkReal(-1.00000000000000))*(sj14)*(x1283)*(x1285)))+(((IkReal(-1.00000000000000))*(x1295)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1290))));
5591 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
5592 {
5593 continue;
5594 }
5595 }
5596 
5597 {
5598 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5599 vinfos[0].jointtype = 1;
5600 vinfos[0].foffset = j9;
5601 vinfos[0].indices[0] = _ij9[0];
5602 vinfos[0].indices[1] = _ij9[1];
5603 vinfos[0].maxsolutions = _nj9;
5604 vinfos[1].jointtype = 1;
5605 vinfos[1].foffset = j10;
5606 vinfos[1].indices[0] = _ij10[0];
5607 vinfos[1].indices[1] = _ij10[1];
5608 vinfos[1].maxsolutions = _nj10;
5609 vinfos[2].jointtype = 1;
5610 vinfos[2].foffset = j11;
5611 vinfos[2].indices[0] = _ij11[0];
5612 vinfos[2].indices[1] = _ij11[1];
5613 vinfos[2].maxsolutions = _nj11;
5614 vinfos[3].jointtype = 1;
5615 vinfos[3].foffset = j12;
5616 vinfos[3].indices[0] = _ij12[0];
5617 vinfos[3].indices[1] = _ij12[1];
5618 vinfos[3].maxsolutions = _nj12;
5619 vinfos[4].jointtype = 1;
5620 vinfos[4].foffset = j13;
5621 vinfos[4].indices[0] = _ij13[0];
5622 vinfos[4].indices[1] = _ij13[1];
5623 vinfos[4].maxsolutions = _nj13;
5624 vinfos[5].jointtype = 1;
5625 vinfos[5].foffset = j14;
5626 vinfos[5].indices[0] = _ij14[0];
5627 vinfos[5].indices[1] = _ij14[1];
5628 vinfos[5].maxsolutions = _nj14;
5629 std::vector<int> vfree(0);
5630 solutions.AddSolution(vinfos,vfree);
5631 }
5632 }
5633 }
5634 
5635 }
5636 
5637 }
5638 
5639 } else
5640 {
5641 {
5642 IkReal j10array[1], cj10array[1], sj10array[1];
5643 bool j10valid[1]={false};
5644 _nj10 = 1;
5645 IkReal x1299=((cj13)*(sj11));
5646 IkReal x1300=((r21)*(sj14));
5647 IkReal x1301=((cj11)*(cj13));
5648 IkReal x1302=((cj11)*(sj13));
5649 IkReal x1303=((sj11)*(sj13));
5650 IkReal x1304=((IkReal(1.00000000000000))*(cj14)*(r20));
5651 if( IKabs(((gconst35)*(((((x1299)*(x1300)))+(((IkReal(-1.00000000000000))*(x1299)*(x1304)))+(((x1300)*(x1302)))+(((IkReal(-1.00000000000000))*(r22)*(x1303)))+(((r22)*(x1301)))+(((IkReal(-1.00000000000000))*(x1302)*(x1304))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst35)*(((((x1300)*(x1303)))+(((IkReal(-1.00000000000000))*(x1303)*(x1304)))+(((r22)*(x1302)))+(((IkReal(-1.00000000000000))*(x1300)*(x1301)))+(((r22)*(x1299)))+(((cj14)*(r20)*(x1301))))))) < IKFAST_ATAN2_MAGTHRESH )
5652  continue;
5653 j10array[0]=IKatan2(((gconst35)*(((((x1299)*(x1300)))+(((IkReal(-1.00000000000000))*(x1299)*(x1304)))+(((x1300)*(x1302)))+(((IkReal(-1.00000000000000))*(r22)*(x1303)))+(((r22)*(x1301)))+(((IkReal(-1.00000000000000))*(x1302)*(x1304)))))), ((gconst35)*(((((x1300)*(x1303)))+(((IkReal(-1.00000000000000))*(x1303)*(x1304)))+(((r22)*(x1302)))+(((IkReal(-1.00000000000000))*(x1300)*(x1301)))+(((r22)*(x1299)))+(((cj14)*(r20)*(x1301)))))));
5654 sj10array[0]=IKsin(j10array[0]);
5655 cj10array[0]=IKcos(j10array[0]);
5656 if( j10array[0] > IKPI )
5657 {
5658  j10array[0]-=IK2PI;
5659 }
5660 else if( j10array[0] < -IKPI )
5661 { j10array[0]+=IK2PI;
5662 }
5663 j10valid[0] = true;
5664 for(int ij10 = 0; ij10 < 1; ++ij10)
5665 {
5666 if( !j10valid[ij10] )
5667 {
5668  continue;
5669 }
5670 _ij10[0] = ij10; _ij10[1] = -1;
5671 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
5672 {
5673 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
5674 {
5675  j10valid[iij10]=false; _ij10[1] = iij10; break;
5676 }
5677 }
5678 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
5679 {
5680 IkReal evalcond[4];
5681 IkReal x1305=IKsin(j10);
5682 IkReal x1306=IKcos(j10);
5683 IkReal x1307=((IkReal(1.00000000000000))*(sj13));
5684 IkReal x1308=((r11)*(sj9));
5685 IkReal x1309=((cj9)*(r01));
5686 IkReal x1310=((r21)*(sj14));
5687 IkReal x1311=((cj9)*(r02));
5688 IkReal x1312=((sj13)*(sj9));
5689 IkReal x1313=((cj14)*(r10));
5690 IkReal x1314=((IkReal(1.00000000000000))*(cj13));
5691 IkReal x1315=((cj14)*(r20));
5692 IkReal x1316=((cj11)*(x1305));
5693 IkReal x1317=((sj11)*(x1306));
5694 IkReal x1318=((sj14)*(x1314));
5695 IkReal x1319=((sj11)*(x1305));
5696 IkReal x1320=((cj11)*(x1306));
5697 IkReal x1321=((cj14)*(cj9)*(r00));
5698 IkReal x1322=((x1317)+(x1316));
5699 evalcond[0]=((x1319)+(((cj13)*(x1310)))+(((IkReal(-1.00000000000000))*(r22)*(x1307)))+(((IkReal(-1.00000000000000))*(x1314)*(x1315)))+(((IkReal(-1.00000000000000))*(x1320))));
5700 evalcond[1]=((x1322)+(((sj13)*(x1310)))+(((IkReal(-1.00000000000000))*(x1307)*(x1315)))+(((cj13)*(r22))));
5701 evalcond[2]=((x1322)+(((sj13)*(x1311)))+(((cj13)*(x1321)))+(((IkReal(-1.00000000000000))*(x1308)*(x1318)))+(((r12)*(x1312)))+(((cj13)*(sj9)*(x1313)))+(((IkReal(-1.00000000000000))*(x1309)*(x1318))));
5702 evalcond[3]=((x1320)+(((IkReal(-1.00000000000000))*(x1311)*(x1314)))+(((sj13)*(x1321)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1314)))+(((IkReal(-1.00000000000000))*(x1319)))+(((IkReal(-1.00000000000000))*(sj14)*(x1307)*(x1308)))+(((IkReal(-1.00000000000000))*(sj14)*(x1307)*(x1309)))+(((x1312)*(x1313))));
5703 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
5704 {
5705 continue;
5706 }
5707 }
5708 
5709 {
5710 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5711 vinfos[0].jointtype = 1;
5712 vinfos[0].foffset = j9;
5713 vinfos[0].indices[0] = _ij9[0];
5714 vinfos[0].indices[1] = _ij9[1];
5715 vinfos[0].maxsolutions = _nj9;
5716 vinfos[1].jointtype = 1;
5717 vinfos[1].foffset = j10;
5718 vinfos[1].indices[0] = _ij10[0];
5719 vinfos[1].indices[1] = _ij10[1];
5720 vinfos[1].maxsolutions = _nj10;
5721 vinfos[2].jointtype = 1;
5722 vinfos[2].foffset = j11;
5723 vinfos[2].indices[0] = _ij11[0];
5724 vinfos[2].indices[1] = _ij11[1];
5725 vinfos[2].maxsolutions = _nj11;
5726 vinfos[3].jointtype = 1;
5727 vinfos[3].foffset = j12;
5728 vinfos[3].indices[0] = _ij12[0];
5729 vinfos[3].indices[1] = _ij12[1];
5730 vinfos[3].maxsolutions = _nj12;
5731 vinfos[4].jointtype = 1;
5732 vinfos[4].foffset = j13;
5733 vinfos[4].indices[0] = _ij13[0];
5734 vinfos[4].indices[1] = _ij13[1];
5735 vinfos[4].maxsolutions = _nj13;
5736 vinfos[5].jointtype = 1;
5737 vinfos[5].foffset = j14;
5738 vinfos[5].indices[0] = _ij14[0];
5739 vinfos[5].indices[1] = _ij14[1];
5740 vinfos[5].maxsolutions = _nj14;
5741 std::vector<int> vfree(0);
5742 solutions.AddSolution(vinfos,vfree);
5743 }
5744 }
5745 }
5746 
5747 }
5748 
5749 }
5750 }
5751 }
5752 
5753 }
5754 
5755 }
5756 
5757 } else
5758 {
5759 IkReal x1323=((IkReal(1.00000000000000))*(sj13));
5760 IkReal x1324=((cj14)*(npx));
5761 IkReal x1325=((npy)*(sj14));
5762 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
5763 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
5764 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
5765 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x1325)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(npz)*(x1323)))+(((IkReal(-1.00000000000000))*(cj13)*(x1324))));
5766 evalcond[4]=((IkReal(-0.0300000000000000))+(((IkReal(-1.00000000000000))*(x1323)*(x1324)))+(((sj13)*(x1325)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(0.250000000000000))*(sj11))));
5767 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 )
5768 {
5769 {
5770 IkReal dummyeval[1];
5771 IkReal gconst39;
5772 IkReal x1326=(sj14)*(sj14);
5773 IkReal x1327=(cj14)*(cj14);
5774 IkReal x1328=((IkReal(1.00000000000000))*(x1326));
5775 IkReal x1329=((IkReal(2.00000000000000))*(cj14)*(sj14));
5776 IkReal x1330=((IkReal(1.00000000000000))*(x1327));
5777 gconst39=IKsign(((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1329)))+(((IkReal(-1.00000000000000))*(x1328)*((r00)*(r00))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1329)))+(((IkReal(-1.00000000000000))*(x1328)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1330)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1330)*((r01)*(r01))))));
5778 IkReal x1331=(sj14)*(sj14);
5779 IkReal x1332=(cj14)*(cj14);
5780 IkReal x1333=((IkReal(1.00000000000000))*(x1331));
5781 IkReal x1334=((IkReal(2.00000000000000))*(cj14)*(sj14));
5782 IkReal x1335=((IkReal(1.00000000000000))*(x1332));
5783 dummyeval[0]=((((IkReal(-1.00000000000000))*(x1333)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(r10)*(r11)*(x1334)))+(((IkReal(-1.00000000000000))*(x1335)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1335)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1334)))+(((IkReal(-1.00000000000000))*(x1333)*((r00)*(r00)))));
5784 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
5785 {
5786 {
5787 IkReal dummyeval[1];
5788 IkReal gconst41;
5789 gconst41=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
5790 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
5791 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
5792 {
5793 {
5794 IkReal dummyeval[1];
5795 IkReal gconst40;
5796 IkReal x1336=(sj14)*(sj14);
5797 IkReal x1337=(cj14)*(cj14);
5798 IkReal x1338=((cj14)*(sj13));
5799 IkReal x1339=((IkReal(1.00000000000000))*(r11));
5800 IkReal x1340=((cj13)*(r00));
5801 IkReal x1341=((sj13)*(sj14));
5802 IkReal x1342=((cj13)*(r01)*(r10));
5803 gconst40=IKsign(((((IkReal(-1.00000000000000))*(r02)*(r10)*(x1341)))+(((r01)*(r12)*(x1338)))+(((IkReal(-1.00000000000000))*(x1336)*(x1339)*(x1340)))+(((x1337)*(x1342)))+(((x1336)*(x1342)))+(((IkReal(-1.00000000000000))*(r02)*(x1338)*(x1339)))+(((IkReal(-1.00000000000000))*(x1337)*(x1339)*(x1340)))+(((r00)*(r12)*(x1341)))));
5804 IkReal x1343=(sj14)*(sj14);
5805 IkReal x1344=(cj14)*(cj14);
5806 IkReal x1345=((cj14)*(sj13));
5807 IkReal x1346=((IkReal(1.00000000000000))*(r11));
5808 IkReal x1347=((cj13)*(r00));
5809 IkReal x1348=((sj13)*(sj14));
5810 IkReal x1349=((cj13)*(r01)*(r10));
5811 dummyeval[0]=((((IkReal(-1.00000000000000))*(r02)*(x1345)*(x1346)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x1348)))+(((IkReal(-1.00000000000000))*(x1344)*(x1346)*(x1347)))+(((x1344)*(x1349)))+(((r00)*(r12)*(x1348)))+(((r01)*(r12)*(x1345)))+(((IkReal(-1.00000000000000))*(x1343)*(x1346)*(x1347)))+(((x1343)*(x1349))));
5812 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
5813 {
5814 continue;
5815 
5816 } else
5817 {
5818 {
5819 IkReal j9array[1], cj9array[1], sj9array[1];
5820 bool j9valid[1]={false};
5821 _nj9 = 1;
5822 IkReal x1350=((cj13)*(cj14));
5823 IkReal x1351=((IkReal(1.00000000000000))*(cj13)*(sj14));
5824 if( IKabs(((gconst40)*(((((r10)*(x1350)))+(((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x1351))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst40)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x1351)))+(((r00)*(x1350))))))) < IKFAST_ATAN2_MAGTHRESH )
5825  continue;
5826 j9array[0]=IKatan2(((gconst40)*(((((r10)*(x1350)))+(((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x1351)))))), ((gconst40)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x1351)))+(((r00)*(x1350)))))));
5827 sj9array[0]=IKsin(j9array[0]);
5828 cj9array[0]=IKcos(j9array[0]);
5829 if( j9array[0] > IKPI )
5830 {
5831  j9array[0]-=IK2PI;
5832 }
5833 else if( j9array[0] < -IKPI )
5834 { j9array[0]+=IK2PI;
5835 }
5836 j9valid[0] = true;
5837 for(int ij9 = 0; ij9 < 1; ++ij9)
5838 {
5839 if( !j9valid[ij9] )
5840 {
5841  continue;
5842 }
5843 _ij9[0] = ij9; _ij9[1] = -1;
5844 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
5845 {
5846 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
5847 {
5848  j9valid[iij9]=false; _ij9[1] = iij9; break;
5849 }
5850 }
5851 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
5852 {
5853 IkReal evalcond[4];
5854 IkReal x1352=IKsin(j9);
5855 IkReal x1353=IKcos(j9);
5856 IkReal x1354=((r10)*(sj14));
5857 IkReal x1355=((cj14)*(r11));
5858 IkReal x1356=((cj14)*(r10));
5859 IkReal x1357=((cj14)*(r00));
5860 IkReal x1358=((r11)*(sj14));
5861 IkReal x1359=((r00)*(sj14));
5862 IkReal x1360=((IkReal(1.00000000000000))*(x1352));
5863 IkReal x1361=((cj13)*(x1352));
5864 IkReal x1362=((sj13)*(x1353));
5865 IkReal x1363=((r01)*(x1352));
5866 IkReal x1364=((IkReal(1.00000000000000))*(x1353));
5867 evalcond[0]=((IkReal(-1.00000000000000))+(((cj14)*(x1363)))+(((IkReal(-1.00000000000000))*(x1354)*(x1364)))+(((IkReal(-1.00000000000000))*(x1355)*(x1364)))+(((x1352)*(x1359))));
5868 evalcond[1]=((((IkReal(-1.00000000000000))*(x1354)*(x1360)))+(((IkReal(-1.00000000000000))*(x1359)*(x1364)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1364)))+(((IkReal(-1.00000000000000))*(x1355)*(x1360))));
5869 evalcond[2]=((((IkReal(-1.00000000000000))*(cj13)*(x1357)*(x1360)))+(((IkReal(-1.00000000000000))*(cj13)*(x1358)*(x1364)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x1360)))+(((cj13)*(x1353)*(x1356)))+(((r01)*(sj14)*(x1361)))+(((r12)*(x1362))));
5870 evalcond[3]=((((x1356)*(x1362)))+(((IkReal(-1.00000000000000))*(sj13)*(x1357)*(x1360)))+(((r02)*(x1361)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1364)))+(((IkReal(-1.00000000000000))*(x1358)*(x1362)))+(((sj13)*(sj14)*(x1363))));
5871 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
5872 {
5873 continue;
5874 }
5875 }
5876 
5877 {
5878 IkReal dummyeval[1];
5879 IkReal gconst42;
5880 gconst42=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
5881 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
5882 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
5883 {
5884 {
5885 IkReal dummyeval[1];
5886 IkReal gconst43;
5887 gconst43=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
5888 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
5889 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
5890 {
5891 continue;
5892 
5893 } else
5894 {
5895 {
5896 IkReal j10array[1], cj10array[1], sj10array[1];
5897 bool j10valid[1]={false};
5898 _nj10 = 1;
5899 IkReal x1365=((cj13)*(sj14));
5900 IkReal x1366=((IkReal(1.00000000000000))*(cj11));
5901 IkReal x1367=((r11)*(sj9));
5902 IkReal x1368=((IkReal(1.00000000000000))*(sj11));
5903 IkReal x1369=((cj13)*(cj14));
5904 IkReal x1370=((cj11)*(sj13));
5905 IkReal x1371=((r12)*(sj9));
5906 IkReal x1372=((cj9)*(r01));
5907 IkReal x1373=((sj11)*(sj13));
5908 IkReal x1374=((cj9)*(r02));
5909 IkReal x1375=((r10)*(sj9));
5910 IkReal x1376=((cj9)*(r00));
5911 if( IKabs(((gconst43)*(((((r21)*(sj11)*(x1365)))+(((IkReal(-1.00000000000000))*(r20)*(x1368)*(x1369)))+(((cj11)*(x1369)*(x1376)))+(((cj11)*(x1369)*(x1375)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)*(x1372)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)*(x1367)))+(((x1370)*(x1371)))+(((x1370)*(x1374)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1368))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst43)*(((((IkReal(-1.00000000000000))*(x1365)*(x1368)*(x1372)))+(((r22)*(x1370)))+(((x1371)*(x1373)))+(((cj11)*(r20)*(x1369)))+(((IkReal(-1.00000000000000))*(x1365)*(x1367)*(x1368)))+(((sj11)*(x1369)*(x1376)))+(((sj11)*(x1369)*(x1375)))+(((IkReal(-1.00000000000000))*(r21)*(x1365)*(x1366)))+(((x1373)*(x1374))))))) < IKFAST_ATAN2_MAGTHRESH )
5912  continue;
5913 j10array[0]=IKatan2(((gconst43)*(((((r21)*(sj11)*(x1365)))+(((IkReal(-1.00000000000000))*(r20)*(x1368)*(x1369)))+(((cj11)*(x1369)*(x1376)))+(((cj11)*(x1369)*(x1375)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)*(x1372)))+(((IkReal(-1.00000000000000))*(x1365)*(x1366)*(x1367)))+(((x1370)*(x1371)))+(((x1370)*(x1374)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1368)))))), ((gconst43)*(((((IkReal(-1.00000000000000))*(x1365)*(x1368)*(x1372)))+(((r22)*(x1370)))+(((x1371)*(x1373)))+(((cj11)*(r20)*(x1369)))+(((IkReal(-1.00000000000000))*(x1365)*(x1367)*(x1368)))+(((sj11)*(x1369)*(x1376)))+(((sj11)*(x1369)*(x1375)))+(((IkReal(-1.00000000000000))*(r21)*(x1365)*(x1366)))+(((x1373)*(x1374)))))));
5914 sj10array[0]=IKsin(j10array[0]);
5915 cj10array[0]=IKcos(j10array[0]);
5916 if( j10array[0] > IKPI )
5917 {
5918  j10array[0]-=IK2PI;
5919 }
5920 else if( j10array[0] < -IKPI )
5921 { j10array[0]+=IK2PI;
5922 }
5923 j10valid[0] = true;
5924 for(int ij10 = 0; ij10 < 1; ++ij10)
5925 {
5926 if( !j10valid[ij10] )
5927 {
5928  continue;
5929 }
5930 _ij10[0] = ij10; _ij10[1] = -1;
5931 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
5932 {
5933 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
5934 {
5935  j10valid[iij10]=false; _ij10[1] = iij10; break;
5936 }
5937 }
5938 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
5939 {
5940 IkReal evalcond[4];
5941 IkReal x1377=IKsin(j10);
5942 IkReal x1378=IKcos(j10);
5943 IkReal x1379=((IkReal(1.00000000000000))*(sj13));
5944 IkReal x1380=((r11)*(sj9));
5945 IkReal x1381=((cj9)*(r01));
5946 IkReal x1382=((IkReal(1.00000000000000))*(cj11));
5947 IkReal x1383=((r21)*(sj14));
5948 IkReal x1384=((cj9)*(r02));
5949 IkReal x1385=((sj13)*(sj9));
5950 IkReal x1386=((cj14)*(r10));
5951 IkReal x1387=((IkReal(1.00000000000000))*(cj13));
5952 IkReal x1388=((cj14)*(r20));
5953 IkReal x1389=((sj11)*(x1377));
5954 IkReal x1390=((sj14)*(x1387));
5955 IkReal x1391=((sj11)*(x1378));
5956 IkReal x1392=((cj14)*(cj9)*(r00));
5957 IkReal x1393=((x1378)*(x1382));
5958 evalcond[0]=((x1389)+(((IkReal(-1.00000000000000))*(r22)*(x1379)))+(((cj13)*(x1383)))+(((IkReal(-1.00000000000000))*(x1387)*(x1388)))+(((IkReal(-1.00000000000000))*(x1393))));
5959 evalcond[1]=((((IkReal(-1.00000000000000))*(x1377)*(x1382)))+(((sj13)*(x1383)))+(((IkReal(-1.00000000000000))*(x1391)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x1379)*(x1388))));
5960 evalcond[2]=((x1391)+(((cj13)*(x1392)))+(((sj13)*(x1384)))+(((IkReal(-1.00000000000000))*(x1381)*(x1390)))+(((IkReal(-1.00000000000000))*(x1380)*(x1390)))+(((cj11)*(x1377)))+(((r12)*(x1385)))+(((cj13)*(sj9)*(x1386))));
5961 evalcond[3]=((x1389)+(((IkReal(-1.00000000000000))*(x1384)*(x1387)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1387)))+(((IkReal(-1.00000000000000))*(sj14)*(x1379)*(x1380)))+(((IkReal(-1.00000000000000))*(sj14)*(x1379)*(x1381)))+(((sj13)*(x1392)))+(((x1385)*(x1386)))+(((IkReal(-1.00000000000000))*(x1393))));
5962 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
5963 {
5964 continue;
5965 }
5966 }
5967 
5968 {
5969 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
5970 vinfos[0].jointtype = 1;
5971 vinfos[0].foffset = j9;
5972 vinfos[0].indices[0] = _ij9[0];
5973 vinfos[0].indices[1] = _ij9[1];
5974 vinfos[0].maxsolutions = _nj9;
5975 vinfos[1].jointtype = 1;
5976 vinfos[1].foffset = j10;
5977 vinfos[1].indices[0] = _ij10[0];
5978 vinfos[1].indices[1] = _ij10[1];
5979 vinfos[1].maxsolutions = _nj10;
5980 vinfos[2].jointtype = 1;
5981 vinfos[2].foffset = j11;
5982 vinfos[2].indices[0] = _ij11[0];
5983 vinfos[2].indices[1] = _ij11[1];
5984 vinfos[2].maxsolutions = _nj11;
5985 vinfos[3].jointtype = 1;
5986 vinfos[3].foffset = j12;
5987 vinfos[3].indices[0] = _ij12[0];
5988 vinfos[3].indices[1] = _ij12[1];
5989 vinfos[3].maxsolutions = _nj12;
5990 vinfos[4].jointtype = 1;
5991 vinfos[4].foffset = j13;
5992 vinfos[4].indices[0] = _ij13[0];
5993 vinfos[4].indices[1] = _ij13[1];
5994 vinfos[4].maxsolutions = _nj13;
5995 vinfos[5].jointtype = 1;
5996 vinfos[5].foffset = j14;
5997 vinfos[5].indices[0] = _ij14[0];
5998 vinfos[5].indices[1] = _ij14[1];
5999 vinfos[5].maxsolutions = _nj14;
6000 std::vector<int> vfree(0);
6001 solutions.AddSolution(vinfos,vfree);
6002 }
6003 }
6004 }
6005 
6006 }
6007 
6008 }
6009 
6010 } else
6011 {
6012 {
6013 IkReal j10array[1], cj10array[1], sj10array[1];
6014 bool j10valid[1]={false};
6015 _nj10 = 1;
6016 IkReal x1394=((cj11)*(r22));
6017 IkReal x1395=((IkReal(1.00000000000000))*(sj13));
6018 IkReal x1396=((cj13)*(sj11));
6019 IkReal x1397=((r21)*(sj14));
6020 IkReal x1398=((cj11)*(cj13));
6021 IkReal x1399=((sj11)*(sj13));
6022 IkReal x1400=((cj14)*(r20));
6023 if( IKabs(((gconst42)*(((((cj11)*(sj13)*(x1397)))+(((x1396)*(x1400)))+(((cj13)*(x1394)))+(((r22)*(x1399)))+(((IkReal(-1.00000000000000))*(x1396)*(x1397)))+(((IkReal(-1.00000000000000))*(cj11)*(x1395)*(x1400))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst42)*(((((IkReal(-1.00000000000000))*(x1398)*(x1400)))+(((IkReal(-1.00000000000000))*(x1394)*(x1395)))+(((IkReal(-1.00000000000000))*(sj11)*(x1395)*(x1400)))+(((x1397)*(x1399)))+(((x1397)*(x1398)))+(((r22)*(x1396))))))) < IKFAST_ATAN2_MAGTHRESH )
6024  continue;
6025 j10array[0]=IKatan2(((gconst42)*(((((cj11)*(sj13)*(x1397)))+(((x1396)*(x1400)))+(((cj13)*(x1394)))+(((r22)*(x1399)))+(((IkReal(-1.00000000000000))*(x1396)*(x1397)))+(((IkReal(-1.00000000000000))*(cj11)*(x1395)*(x1400)))))), ((gconst42)*(((((IkReal(-1.00000000000000))*(x1398)*(x1400)))+(((IkReal(-1.00000000000000))*(x1394)*(x1395)))+(((IkReal(-1.00000000000000))*(sj11)*(x1395)*(x1400)))+(((x1397)*(x1399)))+(((x1397)*(x1398)))+(((r22)*(x1396)))))));
6026 sj10array[0]=IKsin(j10array[0]);
6027 cj10array[0]=IKcos(j10array[0]);
6028 if( j10array[0] > IKPI )
6029 {
6030  j10array[0]-=IK2PI;
6031 }
6032 else if( j10array[0] < -IKPI )
6033 { j10array[0]+=IK2PI;
6034 }
6035 j10valid[0] = true;
6036 for(int ij10 = 0; ij10 < 1; ++ij10)
6037 {
6038 if( !j10valid[ij10] )
6039 {
6040  continue;
6041 }
6042 _ij10[0] = ij10; _ij10[1] = -1;
6043 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
6044 {
6045 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
6046 {
6047  j10valid[iij10]=false; _ij10[1] = iij10; break;
6048 }
6049 }
6050 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
6051 {
6052 IkReal evalcond[4];
6053 IkReal x1401=IKsin(j10);
6054 IkReal x1402=IKcos(j10);
6055 IkReal x1403=((IkReal(1.00000000000000))*(sj13));
6056 IkReal x1404=((r11)*(sj9));
6057 IkReal x1405=((cj9)*(r01));
6058 IkReal x1406=((IkReal(1.00000000000000))*(cj11));
6059 IkReal x1407=((r21)*(sj14));
6060 IkReal x1408=((cj9)*(r02));
6061 IkReal x1409=((sj13)*(sj9));
6062 IkReal x1410=((cj14)*(r10));
6063 IkReal x1411=((IkReal(1.00000000000000))*(cj13));
6064 IkReal x1412=((cj14)*(r20));
6065 IkReal x1413=((sj11)*(x1401));
6066 IkReal x1414=((sj14)*(x1411));
6067 IkReal x1415=((sj11)*(x1402));
6068 IkReal x1416=((cj14)*(cj9)*(r00));
6069 IkReal x1417=((x1402)*(x1406));
6070 evalcond[0]=((((IkReal(-1.00000000000000))*(x1417)))+(((IkReal(-1.00000000000000))*(r22)*(x1403)))+(((cj13)*(x1407)))+(((IkReal(-1.00000000000000))*(x1411)*(x1412)))+(x1413));
6071 evalcond[1]=((((IkReal(-1.00000000000000))*(x1403)*(x1412)))+(((IkReal(-1.00000000000000))*(x1401)*(x1406)))+(((cj13)*(r22)))+(((sj13)*(x1407)))+(((IkReal(-1.00000000000000))*(x1415))));
6072 evalcond[2]=((((cj13)*(sj9)*(x1410)))+(((cj13)*(x1416)))+(((r12)*(x1409)))+(((IkReal(-1.00000000000000))*(x1404)*(x1414)))+(x1415)+(((IkReal(-1.00000000000000))*(x1405)*(x1414)))+(((cj11)*(x1401)))+(((sj13)*(x1408))));
6073 evalcond[3]=((((IkReal(-1.00000000000000))*(x1417)))+(((IkReal(-1.00000000000000))*(sj14)*(x1403)*(x1405)))+(((IkReal(-1.00000000000000))*(sj14)*(x1403)*(x1404)))+(((IkReal(-1.00000000000000))*(x1408)*(x1411)))+(((sj13)*(x1416)))+(x1413)+(((x1409)*(x1410)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1411))));
6074 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
6075 {
6076 continue;
6077 }
6078 }
6079 
6080 {
6081 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6082 vinfos[0].jointtype = 1;
6083 vinfos[0].foffset = j9;
6084 vinfos[0].indices[0] = _ij9[0];
6085 vinfos[0].indices[1] = _ij9[1];
6086 vinfos[0].maxsolutions = _nj9;
6087 vinfos[1].jointtype = 1;
6088 vinfos[1].foffset = j10;
6089 vinfos[1].indices[0] = _ij10[0];
6090 vinfos[1].indices[1] = _ij10[1];
6091 vinfos[1].maxsolutions = _nj10;
6092 vinfos[2].jointtype = 1;
6093 vinfos[2].foffset = j11;
6094 vinfos[2].indices[0] = _ij11[0];
6095 vinfos[2].indices[1] = _ij11[1];
6096 vinfos[2].maxsolutions = _nj11;
6097 vinfos[3].jointtype = 1;
6098 vinfos[3].foffset = j12;
6099 vinfos[3].indices[0] = _ij12[0];
6100 vinfos[3].indices[1] = _ij12[1];
6101 vinfos[3].maxsolutions = _nj12;
6102 vinfos[4].jointtype = 1;
6103 vinfos[4].foffset = j13;
6104 vinfos[4].indices[0] = _ij13[0];
6105 vinfos[4].indices[1] = _ij13[1];
6106 vinfos[4].maxsolutions = _nj13;
6107 vinfos[5].jointtype = 1;
6108 vinfos[5].foffset = j14;
6109 vinfos[5].indices[0] = _ij14[0];
6110 vinfos[5].indices[1] = _ij14[1];
6111 vinfos[5].maxsolutions = _nj14;
6112 std::vector<int> vfree(0);
6113 solutions.AddSolution(vinfos,vfree);
6114 }
6115 }
6116 }
6117 
6118 }
6119 
6120 }
6121 }
6122 }
6123 
6124 }
6125 
6126 }
6127 
6128 } else
6129 {
6130 {
6131 IkReal j10array[1], cj10array[1], sj10array[1];
6132 bool j10valid[1]={false};
6133 _nj10 = 1;
6134 IkReal x1418=((r22)*(sj13));
6135 IkReal x1419=((IkReal(1.00000000000000))*(cj11));
6136 IkReal x1420=((cj13)*(sj11));
6137 IkReal x1421=((cj14)*(r20));
6138 IkReal x1422=((r21)*(sj14));
6139 IkReal x1423=((cj11)*(cj13));
6140 IkReal x1424=((sj11)*(sj13));
6141 if( IKabs(((gconst41)*(((((IkReal(-1.00000000000000))*(sj13)*(x1419)*(x1421)))+(((x1420)*(x1421)))+(((IkReal(-1.00000000000000))*(x1420)*(x1422)))+(((r22)*(x1423)))+(((sj11)*(x1418)))+(((cj11)*(sj13)*(x1422))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst41)*(((((x1422)*(x1423)))+(((x1422)*(x1424)))+(((r22)*(x1420)))+(((IkReal(-1.00000000000000))*(cj13)*(x1419)*(x1421)))+(((IkReal(-1.00000000000000))*(x1418)*(x1419)))+(((IkReal(-1.00000000000000))*(x1421)*(x1424))))))) < IKFAST_ATAN2_MAGTHRESH )
6142  continue;
6143 j10array[0]=IKatan2(((gconst41)*(((((IkReal(-1.00000000000000))*(sj13)*(x1419)*(x1421)))+(((x1420)*(x1421)))+(((IkReal(-1.00000000000000))*(x1420)*(x1422)))+(((r22)*(x1423)))+(((sj11)*(x1418)))+(((cj11)*(sj13)*(x1422)))))), ((gconst41)*(((((x1422)*(x1423)))+(((x1422)*(x1424)))+(((r22)*(x1420)))+(((IkReal(-1.00000000000000))*(cj13)*(x1419)*(x1421)))+(((IkReal(-1.00000000000000))*(x1418)*(x1419)))+(((IkReal(-1.00000000000000))*(x1421)*(x1424)))))));
6144 sj10array[0]=IKsin(j10array[0]);
6145 cj10array[0]=IKcos(j10array[0]);
6146 if( j10array[0] > IKPI )
6147 {
6148  j10array[0]-=IK2PI;
6149 }
6150 else if( j10array[0] < -IKPI )
6151 { j10array[0]+=IK2PI;
6152 }
6153 j10valid[0] = true;
6154 for(int ij10 = 0; ij10 < 1; ++ij10)
6155 {
6156 if( !j10valid[ij10] )
6157 {
6158  continue;
6159 }
6160 _ij10[0] = ij10; _ij10[1] = -1;
6161 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
6162 {
6163 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
6164 {
6165  j10valid[iij10]=false; _ij10[1] = iij10; break;
6166 }
6167 }
6168 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
6169 {
6170 IkReal evalcond[2];
6171 IkReal x1425=IKsin(j10);
6172 IkReal x1426=IKcos(j10);
6173 IkReal x1427=((IkReal(1.00000000000000))*(sj13));
6174 IkReal x1428=((cj14)*(r20));
6175 IkReal x1429=((r21)*(sj14));
6176 IkReal x1430=((IkReal(1.00000000000000))*(x1426));
6177 evalcond[0]=((((IkReal(-1.00000000000000))*(cj11)*(x1430)))+(((IkReal(-1.00000000000000))*(r22)*(x1427)))+(((IkReal(-1.00000000000000))*(cj13)*(x1428)))+(((cj13)*(x1429)))+(((sj11)*(x1425))));
6178 evalcond[1]=((((IkReal(-1.00000000000000))*(x1427)*(x1428)))+(((IkReal(-1.00000000000000))*(sj11)*(x1430)))+(((sj13)*(x1429)))+(((IkReal(-1.00000000000000))*(cj11)*(x1425)))+(((cj13)*(r22))));
6179 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
6180 {
6181 continue;
6182 }
6183 }
6184 
6185 {
6186 IkReal dummyeval[1];
6187 IkReal gconst44;
6188 IkReal x1431=(sj14)*(sj14);
6189 IkReal x1432=(cj14)*(cj14);
6190 IkReal x1433=((IkReal(1.00000000000000))*(x1431));
6191 IkReal x1434=((IkReal(2.00000000000000))*(cj14)*(sj14));
6192 IkReal x1435=((IkReal(1.00000000000000))*(x1432));
6193 gconst44=IKsign(((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1434)))+(((IkReal(-1.00000000000000))*(x1433)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1435)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(x1435)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1434)))+(((IkReal(-1.00000000000000))*(x1433)*((r00)*(r00))))));
6194 IkReal x1436=(sj14)*(sj14);
6195 IkReal x1437=(cj14)*(cj14);
6196 IkReal x1438=((IkReal(1.00000000000000))*(x1436));
6197 IkReal x1439=((IkReal(2.00000000000000))*(cj14)*(sj14));
6198 IkReal x1440=((IkReal(1.00000000000000))*(x1437));
6199 dummyeval[0]=((((IkReal(-1.00000000000000))*(r10)*(r11)*(x1439)))+(((IkReal(-1.00000000000000))*(x1438)*((r10)*(r10))))+(((IkReal(-1.00000000000000))*(x1440)*((r01)*(r01))))+(((IkReal(-1.00000000000000))*(x1440)*((r11)*(r11))))+(((IkReal(-1.00000000000000))*(r00)*(r01)*(x1439)))+(((IkReal(-1.00000000000000))*(x1438)*((r00)*(r00)))));
6200 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
6201 {
6202 {
6203 IkReal dummyeval[1];
6204 IkReal gconst45;
6205 IkReal x1441=(sj14)*(sj14);
6206 IkReal x1442=(cj14)*(cj14);
6207 IkReal x1443=((cj14)*(sj13));
6208 IkReal x1444=((IkReal(1.00000000000000))*(r11));
6209 IkReal x1445=((cj13)*(r00));
6210 IkReal x1446=((sj13)*(sj14));
6211 IkReal x1447=((cj13)*(r01)*(r10));
6212 gconst45=IKsign(((((x1442)*(x1447)))+(((x1441)*(x1447)))+(((r01)*(r12)*(x1443)))+(((IkReal(-1.00000000000000))*(x1442)*(x1444)*(x1445)))+(((IkReal(-1.00000000000000))*(r02)*(x1443)*(x1444)))+(((r00)*(r12)*(x1446)))+(((IkReal(-1.00000000000000))*(x1441)*(x1444)*(x1445)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x1446)))));
6213 IkReal x1448=(sj14)*(sj14);
6214 IkReal x1449=(cj14)*(cj14);
6215 IkReal x1450=((cj14)*(sj13));
6216 IkReal x1451=((IkReal(1.00000000000000))*(r11));
6217 IkReal x1452=((cj13)*(r00));
6218 IkReal x1453=((sj13)*(sj14));
6219 IkReal x1454=((cj13)*(r01)*(r10));
6220 dummyeval[0]=((((r00)*(r12)*(x1453)))+(((IkReal(-1.00000000000000))*(x1449)*(x1451)*(x1452)))+(((IkReal(-1.00000000000000))*(r02)*(r10)*(x1453)))+(((x1448)*(x1454)))+(((IkReal(-1.00000000000000))*(x1448)*(x1451)*(x1452)))+(((r01)*(r12)*(x1450)))+(((IkReal(-1.00000000000000))*(r02)*(x1450)*(x1451)))+(((x1449)*(x1454))));
6221 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
6222 {
6223 continue;
6224 
6225 } else
6226 {
6227 {
6228 IkReal j9array[1], cj9array[1], sj9array[1];
6229 bool j9valid[1]={false};
6230 _nj9 = 1;
6231 IkReal x1455=((cj13)*(cj14));
6232 IkReal x1456=((IkReal(1.00000000000000))*(cj13)*(sj14));
6233 if( IKabs(((gconst45)*(((((r10)*(x1455)))+(((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x1456))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst45)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x1456)))+(((r00)*(x1455))))))) < IKFAST_ATAN2_MAGTHRESH )
6234  continue;
6235 j9array[0]=IKatan2(((gconst45)*(((((r10)*(x1455)))+(((r12)*(sj13)))+(((IkReal(-1.00000000000000))*(r11)*(x1456)))))), ((gconst45)*(((((r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x1456)))+(((r00)*(x1455)))))));
6236 sj9array[0]=IKsin(j9array[0]);
6237 cj9array[0]=IKcos(j9array[0]);
6238 if( j9array[0] > IKPI )
6239 {
6240  j9array[0]-=IK2PI;
6241 }
6242 else if( j9array[0] < -IKPI )
6243 { j9array[0]+=IK2PI;
6244 }
6245 j9valid[0] = true;
6246 for(int ij9 = 0; ij9 < 1; ++ij9)
6247 {
6248 if( !j9valid[ij9] )
6249 {
6250  continue;
6251 }
6252 _ij9[0] = ij9; _ij9[1] = -1;
6253 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
6254 {
6255 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
6256 {
6257  j9valid[iij9]=false; _ij9[1] = iij9; break;
6258 }
6259 }
6260 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
6261 {
6262 IkReal evalcond[6];
6263 IkReal x1457=IKsin(j9);
6264 IkReal x1458=IKcos(j9);
6265 IkReal x1459=((IkReal(1.00000000000000))*(cj14));
6266 IkReal x1460=((IkReal(1.00000000000000))*(sj14));
6267 IkReal x1461=((cj13)*(cj14));
6268 IkReal x1462=((cj14)*(r10));
6269 IkReal x1463=((r01)*(sj14));
6270 IkReal x1464=((IkReal(1.00000000000000))*(r12));
6271 IkReal x1465=((sj13)*(x1458));
6272 IkReal x1466=((r02)*(x1457));
6273 IkReal x1467=((r11)*(x1457));
6274 IkReal x1468=((r10)*(x1458));
6275 IkReal x1469=((r01)*(x1458));
6276 IkReal x1470=((sj13)*(x1457));
6277 IkReal x1471=((r11)*(x1458));
6278 IkReal x1472=((cj13)*(x1457));
6279 IkReal x1473=((r10)*(x1457));
6280 IkReal x1474=((r00)*(x1458));
6281 IkReal x1475=((cj13)*(x1458));
6282 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1460)*(x1468)))+(((r00)*(sj14)*(x1457)))+(((cj14)*(r01)*(x1457)))+(((IkReal(-1.00000000000000))*(x1459)*(x1471))));
6283 evalcond[1]=((((IkReal(-1.00000000000000))*(x1459)*(x1467)))+(((IkReal(-1.00000000000000))*(x1459)*(x1469)))+(((IkReal(-1.00000000000000))*(x1460)*(x1474)))+(((IkReal(-1.00000000000000))*(x1460)*(x1473))));
6284 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(x1459)*(x1472)))+(((x1461)*(x1468)))+(((IkReal(-1.00000000000000))*(cj13)*(x1460)*(x1471)))+(((IkReal(-1.00000000000000))*(sj13)*(x1466)))+(((x1463)*(x1472)))+(((r12)*(x1465))));
6285 evalcond[3]=((((IkReal(-1.00000000000000))*(r00)*(x1459)*(x1470)))+(((cj13)*(x1466)))+(((IkReal(-1.00000000000000))*(r11)*(x1460)*(x1465)))+(((x1463)*(x1470)))+(((x1462)*(x1465)))+(((IkReal(-1.00000000000000))*(x1464)*(x1475))));
6286 evalcond[4]=((((r12)*(x1470)))+(((cj10)*(sj11)))+(((r02)*(x1465)))+(((IkReal(-1.00000000000000))*(cj13)*(x1460)*(x1469)))+(((IkReal(-1.00000000000000))*(cj13)*(x1460)*(x1467)))+(((cj11)*(sj10)))+(((x1461)*(x1474)))+(((x1461)*(x1473))));
6287 evalcond[5]=((((IkReal(-1.00000000000000))*(r01)*(x1460)*(x1465)))+(((IkReal(-1.00000000000000))*(sj13)*(x1460)*(x1467)))+(((x1462)*(x1470)))+(((IkReal(-1.00000000000000))*(r02)*(x1475)))+(((sj10)*(sj11)))+(((cj14)*(r00)*(x1465)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(x1464)*(x1472))));
6288 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 )
6289 {
6290 continue;
6291 }
6292 }
6293 
6294 {
6295 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6296 vinfos[0].jointtype = 1;
6297 vinfos[0].foffset = j9;
6298 vinfos[0].indices[0] = _ij9[0];
6299 vinfos[0].indices[1] = _ij9[1];
6300 vinfos[0].maxsolutions = _nj9;
6301 vinfos[1].jointtype = 1;
6302 vinfos[1].foffset = j10;
6303 vinfos[1].indices[0] = _ij10[0];
6304 vinfos[1].indices[1] = _ij10[1];
6305 vinfos[1].maxsolutions = _nj10;
6306 vinfos[2].jointtype = 1;
6307 vinfos[2].foffset = j11;
6308 vinfos[2].indices[0] = _ij11[0];
6309 vinfos[2].indices[1] = _ij11[1];
6310 vinfos[2].maxsolutions = _nj11;
6311 vinfos[3].jointtype = 1;
6312 vinfos[3].foffset = j12;
6313 vinfos[3].indices[0] = _ij12[0];
6314 vinfos[3].indices[1] = _ij12[1];
6315 vinfos[3].maxsolutions = _nj12;
6316 vinfos[4].jointtype = 1;
6317 vinfos[4].foffset = j13;
6318 vinfos[4].indices[0] = _ij13[0];
6319 vinfos[4].indices[1] = _ij13[1];
6320 vinfos[4].maxsolutions = _nj13;
6321 vinfos[5].jointtype = 1;
6322 vinfos[5].foffset = j14;
6323 vinfos[5].indices[0] = _ij14[0];
6324 vinfos[5].indices[1] = _ij14[1];
6325 vinfos[5].maxsolutions = _nj14;
6326 std::vector<int> vfree(0);
6327 solutions.AddSolution(vinfos,vfree);
6328 }
6329 }
6330 }
6331 
6332 }
6333 
6334 }
6335 
6336 } else
6337 {
6338 {
6339 IkReal j9array[1], cj9array[1], sj9array[1];
6340 bool j9valid[1]={false};
6341 _nj9 = 1;
6342 if( IKabs(((gconst44)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst44)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
6343  continue;
6344 j9array[0]=IKatan2(((gconst44)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst44)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
6345 sj9array[0]=IKsin(j9array[0]);
6346 cj9array[0]=IKcos(j9array[0]);
6347 if( j9array[0] > IKPI )
6348 {
6349  j9array[0]-=IK2PI;
6350 }
6351 else if( j9array[0] < -IKPI )
6352 { j9array[0]+=IK2PI;
6353 }
6354 j9valid[0] = true;
6355 for(int ij9 = 0; ij9 < 1; ++ij9)
6356 {
6357 if( !j9valid[ij9] )
6358 {
6359  continue;
6360 }
6361 _ij9[0] = ij9; _ij9[1] = -1;
6362 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
6363 {
6364 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
6365 {
6366  j9valid[iij9]=false; _ij9[1] = iij9; break;
6367 }
6368 }
6369 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
6370 {
6371 IkReal evalcond[6];
6372 IkReal x1476=IKsin(j9);
6373 IkReal x1477=IKcos(j9);
6374 IkReal x1478=((IkReal(1.00000000000000))*(cj14));
6375 IkReal x1479=((IkReal(1.00000000000000))*(sj14));
6376 IkReal x1480=((cj13)*(cj14));
6377 IkReal x1481=((cj14)*(r10));
6378 IkReal x1482=((r01)*(sj14));
6379 IkReal x1483=((IkReal(1.00000000000000))*(r12));
6380 IkReal x1484=((sj13)*(x1477));
6381 IkReal x1485=((r02)*(x1476));
6382 IkReal x1486=((r11)*(x1476));
6383 IkReal x1487=((r10)*(x1477));
6384 IkReal x1488=((r01)*(x1477));
6385 IkReal x1489=((sj13)*(x1476));
6386 IkReal x1490=((r11)*(x1477));
6387 IkReal x1491=((cj13)*(x1476));
6388 IkReal x1492=((r10)*(x1476));
6389 IkReal x1493=((r00)*(x1477));
6390 IkReal x1494=((cj13)*(x1477));
6391 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1479)*(x1487)))+(((r00)*(sj14)*(x1476)))+(((cj14)*(r01)*(x1476)))+(((IkReal(-1.00000000000000))*(x1478)*(x1490))));
6392 evalcond[1]=((((IkReal(-1.00000000000000))*(x1479)*(x1493)))+(((IkReal(-1.00000000000000))*(x1479)*(x1492)))+(((IkReal(-1.00000000000000))*(x1478)*(x1486)))+(((IkReal(-1.00000000000000))*(x1478)*(x1488))));
6393 evalcond[2]=((((x1482)*(x1491)))+(((IkReal(-1.00000000000000))*(sj13)*(x1485)))+(((x1480)*(x1487)))+(((IkReal(-1.00000000000000))*(r00)*(x1478)*(x1491)))+(((r12)*(x1484)))+(((IkReal(-1.00000000000000))*(cj13)*(x1479)*(x1490))));
6394 evalcond[3]=((((IkReal(-1.00000000000000))*(r00)*(x1478)*(x1489)))+(((x1482)*(x1489)))+(((cj13)*(x1485)))+(((x1481)*(x1484)))+(((IkReal(-1.00000000000000))*(x1483)*(x1494)))+(((IkReal(-1.00000000000000))*(r11)*(x1479)*(x1484))));
6395 evalcond[4]=((((r12)*(x1489)))+(((x1480)*(x1492)))+(((x1480)*(x1493)))+(((cj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x1479)*(x1488)))+(((IkReal(-1.00000000000000))*(cj13)*(x1479)*(x1486)))+(((r02)*(x1484)))+(((cj11)*(sj10))));
6396 evalcond[5]=((((IkReal(-1.00000000000000))*(r02)*(x1494)))+(((cj14)*(r00)*(x1484)))+(((x1481)*(x1489)))+(((IkReal(-1.00000000000000))*(sj13)*(x1479)*(x1486)))+(((sj10)*(sj11)))+(((IkReal(-1.00000000000000))*(x1483)*(x1491)))+(((IkReal(-1.00000000000000))*(cj10)*(cj11)))+(((IkReal(-1.00000000000000))*(r01)*(x1479)*(x1484))));
6397 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 )
6398 {
6399 continue;
6400 }
6401 }
6402 
6403 {
6404 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6405 vinfos[0].jointtype = 1;
6406 vinfos[0].foffset = j9;
6407 vinfos[0].indices[0] = _ij9[0];
6408 vinfos[0].indices[1] = _ij9[1];
6409 vinfos[0].maxsolutions = _nj9;
6410 vinfos[1].jointtype = 1;
6411 vinfos[1].foffset = j10;
6412 vinfos[1].indices[0] = _ij10[0];
6413 vinfos[1].indices[1] = _ij10[1];
6414 vinfos[1].maxsolutions = _nj10;
6415 vinfos[2].jointtype = 1;
6416 vinfos[2].foffset = j11;
6417 vinfos[2].indices[0] = _ij11[0];
6418 vinfos[2].indices[1] = _ij11[1];
6419 vinfos[2].maxsolutions = _nj11;
6420 vinfos[3].jointtype = 1;
6421 vinfos[3].foffset = j12;
6422 vinfos[3].indices[0] = _ij12[0];
6423 vinfos[3].indices[1] = _ij12[1];
6424 vinfos[3].maxsolutions = _nj12;
6425 vinfos[4].jointtype = 1;
6426 vinfos[4].foffset = j13;
6427 vinfos[4].indices[0] = _ij13[0];
6428 vinfos[4].indices[1] = _ij13[1];
6429 vinfos[4].maxsolutions = _nj13;
6430 vinfos[5].jointtype = 1;
6431 vinfos[5].foffset = j14;
6432 vinfos[5].indices[0] = _ij14[0];
6433 vinfos[5].indices[1] = _ij14[1];
6434 vinfos[5].maxsolutions = _nj14;
6435 std::vector<int> vfree(0);
6436 solutions.AddSolution(vinfos,vfree);
6437 }
6438 }
6439 }
6440 
6441 }
6442 
6443 }
6444 }
6445 }
6446 
6447 }
6448 
6449 }
6450 
6451 } else
6452 {
6453 {
6454 IkReal j9array[1], cj9array[1], sj9array[1];
6455 bool j9valid[1]={false};
6456 _nj9 = 1;
6457 if( IKabs(((gconst39)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst39)*(((((cj14)*(r11)))+(((r10)*(sj14))))))) < IKFAST_ATAN2_MAGTHRESH )
6458  continue;
6459 j9array[0]=IKatan2(((gconst39)*(((((IkReal(-1.00000000000000))*(cj14)*(r01)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)))))), ((gconst39)*(((((cj14)*(r11)))+(((r10)*(sj14)))))));
6460 sj9array[0]=IKsin(j9array[0]);
6461 cj9array[0]=IKcos(j9array[0]);
6462 if( j9array[0] > IKPI )
6463 {
6464  j9array[0]-=IK2PI;
6465 }
6466 else if( j9array[0] < -IKPI )
6467 { j9array[0]+=IK2PI;
6468 }
6469 j9valid[0] = true;
6470 for(int ij9 = 0; ij9 < 1; ++ij9)
6471 {
6472 if( !j9valid[ij9] )
6473 {
6474  continue;
6475 }
6476 _ij9[0] = ij9; _ij9[1] = -1;
6477 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
6478 {
6479 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
6480 {
6481  j9valid[iij9]=false; _ij9[1] = iij9; break;
6482 }
6483 }
6484 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
6485 {
6486 IkReal evalcond[4];
6487 IkReal x1495=IKsin(j9);
6488 IkReal x1496=IKcos(j9);
6489 IkReal x1497=((r10)*(sj14));
6490 IkReal x1498=((cj14)*(r11));
6491 IkReal x1499=((cj14)*(r10));
6492 IkReal x1500=((cj14)*(r00));
6493 IkReal x1501=((r11)*(sj14));
6494 IkReal x1502=((r00)*(sj14));
6495 IkReal x1503=((IkReal(1.00000000000000))*(x1495));
6496 IkReal x1504=((cj13)*(x1495));
6497 IkReal x1505=((sj13)*(x1496));
6498 IkReal x1506=((r01)*(x1495));
6499 IkReal x1507=((IkReal(1.00000000000000))*(x1496));
6500 evalcond[0]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1498)*(x1507)))+(((x1495)*(x1502)))+(((IkReal(-1.00000000000000))*(x1497)*(x1507)))+(((cj14)*(x1506))));
6501 evalcond[1]=((((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1507)))+(((IkReal(-1.00000000000000))*(x1498)*(x1503)))+(((IkReal(-1.00000000000000))*(x1497)*(x1503)))+(((IkReal(-1.00000000000000))*(x1502)*(x1507))));
6502 evalcond[2]=((((cj13)*(x1496)*(x1499)))+(((r12)*(x1505)))+(((IkReal(-1.00000000000000))*(cj13)*(x1501)*(x1507)))+(((r01)*(sj14)*(x1504)))+(((IkReal(-1.00000000000000))*(cj13)*(x1500)*(x1503)))+(((IkReal(-1.00000000000000))*(r02)*(sj13)*(x1503))));
6503 evalcond[3]=((((x1499)*(x1505)))+(((sj13)*(sj14)*(x1506)))+(((r02)*(x1504)))+(((IkReal(-1.00000000000000))*(sj13)*(x1500)*(x1503)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1507)))+(((IkReal(-1.00000000000000))*(x1501)*(x1505))));
6504 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
6505 {
6506 continue;
6507 }
6508 }
6509 
6510 {
6511 IkReal dummyeval[1];
6512 IkReal gconst42;
6513 gconst42=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
6514 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
6515 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
6516 {
6517 {
6518 IkReal dummyeval[1];
6519 IkReal gconst43;
6520 gconst43=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
6521 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
6522 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
6523 {
6524 continue;
6525 
6526 } else
6527 {
6528 {
6529 IkReal j10array[1], cj10array[1], sj10array[1];
6530 bool j10valid[1]={false};
6531 _nj10 = 1;
6532 IkReal x1508=((cj13)*(sj14));
6533 IkReal x1509=((IkReal(1.00000000000000))*(cj11));
6534 IkReal x1510=((r11)*(sj9));
6535 IkReal x1511=((IkReal(1.00000000000000))*(sj11));
6536 IkReal x1512=((cj13)*(cj14));
6537 IkReal x1513=((cj11)*(sj13));
6538 IkReal x1514=((r12)*(sj9));
6539 IkReal x1515=((cj9)*(r01));
6540 IkReal x1516=((sj11)*(sj13));
6541 IkReal x1517=((cj9)*(r02));
6542 IkReal x1518=((r10)*(sj9));
6543 IkReal x1519=((cj9)*(r00));
6544 if( IKabs(((gconst43)*(((((x1513)*(x1514)))+(((x1513)*(x1517)))+(((r21)*(sj11)*(x1508)))+(((IkReal(-1.00000000000000))*(x1508)*(x1509)*(x1510)))+(((IkReal(-1.00000000000000))*(x1508)*(x1509)*(x1515)))+(((IkReal(-1.00000000000000))*(r20)*(x1511)*(x1512)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1511)))+(((cj11)*(x1512)*(x1518)))+(((cj11)*(x1512)*(x1519))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst43)*(((((x1514)*(x1516)))+(((x1516)*(x1517)))+(((cj11)*(r20)*(x1512)))+(((IkReal(-1.00000000000000))*(r21)*(x1508)*(x1509)))+(((r22)*(x1513)))+(((sj11)*(x1512)*(x1519)))+(((sj11)*(x1512)*(x1518)))+(((IkReal(-1.00000000000000))*(x1508)*(x1510)*(x1511)))+(((IkReal(-1.00000000000000))*(x1508)*(x1511)*(x1515))))))) < IKFAST_ATAN2_MAGTHRESH )
6545  continue;
6546 j10array[0]=IKatan2(((gconst43)*(((((x1513)*(x1514)))+(((x1513)*(x1517)))+(((r21)*(sj11)*(x1508)))+(((IkReal(-1.00000000000000))*(x1508)*(x1509)*(x1510)))+(((IkReal(-1.00000000000000))*(x1508)*(x1509)*(x1515)))+(((IkReal(-1.00000000000000))*(r20)*(x1511)*(x1512)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1511)))+(((cj11)*(x1512)*(x1518)))+(((cj11)*(x1512)*(x1519)))))), ((gconst43)*(((((x1514)*(x1516)))+(((x1516)*(x1517)))+(((cj11)*(r20)*(x1512)))+(((IkReal(-1.00000000000000))*(r21)*(x1508)*(x1509)))+(((r22)*(x1513)))+(((sj11)*(x1512)*(x1519)))+(((sj11)*(x1512)*(x1518)))+(((IkReal(-1.00000000000000))*(x1508)*(x1510)*(x1511)))+(((IkReal(-1.00000000000000))*(x1508)*(x1511)*(x1515)))))));
6547 sj10array[0]=IKsin(j10array[0]);
6548 cj10array[0]=IKcos(j10array[0]);
6549 if( j10array[0] > IKPI )
6550 {
6551  j10array[0]-=IK2PI;
6552 }
6553 else if( j10array[0] < -IKPI )
6554 { j10array[0]+=IK2PI;
6555 }
6556 j10valid[0] = true;
6557 for(int ij10 = 0; ij10 < 1; ++ij10)
6558 {
6559 if( !j10valid[ij10] )
6560 {
6561  continue;
6562 }
6563 _ij10[0] = ij10; _ij10[1] = -1;
6564 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
6565 {
6566 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
6567 {
6568  j10valid[iij10]=false; _ij10[1] = iij10; break;
6569 }
6570 }
6571 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
6572 {
6573 IkReal evalcond[4];
6574 IkReal x1520=IKsin(j10);
6575 IkReal x1521=IKcos(j10);
6576 IkReal x1522=((IkReal(1.00000000000000))*(sj13));
6577 IkReal x1523=((r11)*(sj9));
6578 IkReal x1524=((cj9)*(r01));
6579 IkReal x1525=((IkReal(1.00000000000000))*(cj11));
6580 IkReal x1526=((r21)*(sj14));
6581 IkReal x1527=((cj9)*(r02));
6582 IkReal x1528=((sj13)*(sj9));
6583 IkReal x1529=((cj14)*(r10));
6584 IkReal x1530=((IkReal(1.00000000000000))*(cj13));
6585 IkReal x1531=((cj14)*(r20));
6586 IkReal x1532=((sj11)*(x1520));
6587 IkReal x1533=((sj14)*(x1530));
6588 IkReal x1534=((sj11)*(x1521));
6589 IkReal x1535=((cj14)*(cj9)*(r00));
6590 IkReal x1536=((x1521)*(x1525));
6591 evalcond[0]=((((IkReal(-1.00000000000000))*(x1530)*(x1531)))+(((IkReal(-1.00000000000000))*(x1536)))+(((IkReal(-1.00000000000000))*(r22)*(x1522)))+(x1532)+(((cj13)*(x1526))));
6592 evalcond[1]=((((IkReal(-1.00000000000000))*(x1534)))+(((sj13)*(x1526)))+(((IkReal(-1.00000000000000))*(x1522)*(x1531)))+(((IkReal(-1.00000000000000))*(x1520)*(x1525)))+(((cj13)*(r22))));
6593 evalcond[2]=((((IkReal(-1.00000000000000))*(x1524)*(x1533)))+(((cj11)*(x1520)))+(((r12)*(x1528)))+(((cj13)*(sj9)*(x1529)))+(x1534)+(((cj13)*(x1535)))+(((sj13)*(x1527)))+(((IkReal(-1.00000000000000))*(x1523)*(x1533))));
6594 evalcond[3]=((((x1528)*(x1529)))+(((IkReal(-1.00000000000000))*(x1536)))+(x1532)+(((IkReal(-1.00000000000000))*(sj14)*(x1522)*(x1523)))+(((IkReal(-1.00000000000000))*(sj14)*(x1522)*(x1524)))+(((sj13)*(x1535)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1530)))+(((IkReal(-1.00000000000000))*(x1527)*(x1530))));
6595 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
6596 {
6597 continue;
6598 }
6599 }
6600 
6601 {
6602 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6603 vinfos[0].jointtype = 1;
6604 vinfos[0].foffset = j9;
6605 vinfos[0].indices[0] = _ij9[0];
6606 vinfos[0].indices[1] = _ij9[1];
6607 vinfos[0].maxsolutions = _nj9;
6608 vinfos[1].jointtype = 1;
6609 vinfos[1].foffset = j10;
6610 vinfos[1].indices[0] = _ij10[0];
6611 vinfos[1].indices[1] = _ij10[1];
6612 vinfos[1].maxsolutions = _nj10;
6613 vinfos[2].jointtype = 1;
6614 vinfos[2].foffset = j11;
6615 vinfos[2].indices[0] = _ij11[0];
6616 vinfos[2].indices[1] = _ij11[1];
6617 vinfos[2].maxsolutions = _nj11;
6618 vinfos[3].jointtype = 1;
6619 vinfos[3].foffset = j12;
6620 vinfos[3].indices[0] = _ij12[0];
6621 vinfos[3].indices[1] = _ij12[1];
6622 vinfos[3].maxsolutions = _nj12;
6623 vinfos[4].jointtype = 1;
6624 vinfos[4].foffset = j13;
6625 vinfos[4].indices[0] = _ij13[0];
6626 vinfos[4].indices[1] = _ij13[1];
6627 vinfos[4].maxsolutions = _nj13;
6628 vinfos[5].jointtype = 1;
6629 vinfos[5].foffset = j14;
6630 vinfos[5].indices[0] = _ij14[0];
6631 vinfos[5].indices[1] = _ij14[1];
6632 vinfos[5].maxsolutions = _nj14;
6633 std::vector<int> vfree(0);
6634 solutions.AddSolution(vinfos,vfree);
6635 }
6636 }
6637 }
6638 
6639 }
6640 
6641 }
6642 
6643 } else
6644 {
6645 {
6646 IkReal j10array[1], cj10array[1], sj10array[1];
6647 bool j10valid[1]={false};
6648 _nj10 = 1;
6649 IkReal x1537=((cj11)*(r22));
6650 IkReal x1538=((IkReal(1.00000000000000))*(sj13));
6651 IkReal x1539=((cj13)*(sj11));
6652 IkReal x1540=((r21)*(sj14));
6653 IkReal x1541=((cj11)*(cj13));
6654 IkReal x1542=((sj11)*(sj13));
6655 IkReal x1543=((cj14)*(r20));
6656 if( IKabs(((gconst42)*(((((IkReal(-1.00000000000000))*(x1539)*(x1540)))+(((cj11)*(sj13)*(x1540)))+(((x1539)*(x1543)))+(((IkReal(-1.00000000000000))*(cj11)*(x1538)*(x1543)))+(((cj13)*(x1537)))+(((r22)*(x1542))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst42)*(((((IkReal(-1.00000000000000))*(x1541)*(x1543)))+(((IkReal(-1.00000000000000))*(x1537)*(x1538)))+(((x1540)*(x1541)))+(((x1540)*(x1542)))+(((r22)*(x1539)))+(((IkReal(-1.00000000000000))*(sj11)*(x1538)*(x1543))))))) < IKFAST_ATAN2_MAGTHRESH )
6657  continue;
6658 j10array[0]=IKatan2(((gconst42)*(((((IkReal(-1.00000000000000))*(x1539)*(x1540)))+(((cj11)*(sj13)*(x1540)))+(((x1539)*(x1543)))+(((IkReal(-1.00000000000000))*(cj11)*(x1538)*(x1543)))+(((cj13)*(x1537)))+(((r22)*(x1542)))))), ((gconst42)*(((((IkReal(-1.00000000000000))*(x1541)*(x1543)))+(((IkReal(-1.00000000000000))*(x1537)*(x1538)))+(((x1540)*(x1541)))+(((x1540)*(x1542)))+(((r22)*(x1539)))+(((IkReal(-1.00000000000000))*(sj11)*(x1538)*(x1543)))))));
6659 sj10array[0]=IKsin(j10array[0]);
6660 cj10array[0]=IKcos(j10array[0]);
6661 if( j10array[0] > IKPI )
6662 {
6663  j10array[0]-=IK2PI;
6664 }
6665 else if( j10array[0] < -IKPI )
6666 { j10array[0]+=IK2PI;
6667 }
6668 j10valid[0] = true;
6669 for(int ij10 = 0; ij10 < 1; ++ij10)
6670 {
6671 if( !j10valid[ij10] )
6672 {
6673  continue;
6674 }
6675 _ij10[0] = ij10; _ij10[1] = -1;
6676 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
6677 {
6678 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
6679 {
6680  j10valid[iij10]=false; _ij10[1] = iij10; break;
6681 }
6682 }
6683 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
6684 {
6685 IkReal evalcond[4];
6686 IkReal x1544=IKsin(j10);
6687 IkReal x1545=IKcos(j10);
6688 IkReal x1546=((IkReal(1.00000000000000))*(sj13));
6689 IkReal x1547=((r11)*(sj9));
6690 IkReal x1548=((cj9)*(r01));
6691 IkReal x1549=((IkReal(1.00000000000000))*(cj11));
6692 IkReal x1550=((r21)*(sj14));
6693 IkReal x1551=((cj9)*(r02));
6694 IkReal x1552=((sj13)*(sj9));
6695 IkReal x1553=((cj14)*(r10));
6696 IkReal x1554=((IkReal(1.00000000000000))*(cj13));
6697 IkReal x1555=((cj14)*(r20));
6698 IkReal x1556=((sj11)*(x1544));
6699 IkReal x1557=((sj14)*(x1554));
6700 IkReal x1558=((sj11)*(x1545));
6701 IkReal x1559=((cj14)*(cj9)*(r00));
6702 IkReal x1560=((x1545)*(x1549));
6703 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1546)))+(x1556)+(((cj13)*(x1550)))+(((IkReal(-1.00000000000000))*(x1560)))+(((IkReal(-1.00000000000000))*(x1554)*(x1555))));
6704 evalcond[1]=((((IkReal(-1.00000000000000))*(x1558)))+(((IkReal(-1.00000000000000))*(x1546)*(x1555)))+(((sj13)*(x1550)))+(((IkReal(-1.00000000000000))*(x1544)*(x1549)))+(((cj13)*(r22))));
6705 evalcond[2]=((((sj13)*(x1551)))+(((cj11)*(x1544)))+(((IkReal(-1.00000000000000))*(x1548)*(x1557)))+(((r12)*(x1552)))+(x1558)+(((cj13)*(x1559)))+(((IkReal(-1.00000000000000))*(x1547)*(x1557)))+(((cj13)*(sj9)*(x1553))));
6706 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x1546)*(x1548)))+(((IkReal(-1.00000000000000))*(sj14)*(x1546)*(x1547)))+(((x1552)*(x1553)))+(((sj13)*(x1559)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1554)))+(x1556)+(((IkReal(-1.00000000000000))*(x1551)*(x1554)))+(((IkReal(-1.00000000000000))*(x1560))));
6707 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
6708 {
6709 continue;
6710 }
6711 }
6712 
6713 {
6714 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6715 vinfos[0].jointtype = 1;
6716 vinfos[0].foffset = j9;
6717 vinfos[0].indices[0] = _ij9[0];
6718 vinfos[0].indices[1] = _ij9[1];
6719 vinfos[0].maxsolutions = _nj9;
6720 vinfos[1].jointtype = 1;
6721 vinfos[1].foffset = j10;
6722 vinfos[1].indices[0] = _ij10[0];
6723 vinfos[1].indices[1] = _ij10[1];
6724 vinfos[1].maxsolutions = _nj10;
6725 vinfos[2].jointtype = 1;
6726 vinfos[2].foffset = j11;
6727 vinfos[2].indices[0] = _ij11[0];
6728 vinfos[2].indices[1] = _ij11[1];
6729 vinfos[2].maxsolutions = _nj11;
6730 vinfos[3].jointtype = 1;
6731 vinfos[3].foffset = j12;
6732 vinfos[3].indices[0] = _ij12[0];
6733 vinfos[3].indices[1] = _ij12[1];
6734 vinfos[3].maxsolutions = _nj12;
6735 vinfos[4].jointtype = 1;
6736 vinfos[4].foffset = j13;
6737 vinfos[4].indices[0] = _ij13[0];
6738 vinfos[4].indices[1] = _ij13[1];
6739 vinfos[4].maxsolutions = _nj13;
6740 vinfos[5].jointtype = 1;
6741 vinfos[5].foffset = j14;
6742 vinfos[5].indices[0] = _ij14[0];
6743 vinfos[5].indices[1] = _ij14[1];
6744 vinfos[5].maxsolutions = _nj14;
6745 std::vector<int> vfree(0);
6746 solutions.AddSolution(vinfos,vfree);
6747 }
6748 }
6749 }
6750 
6751 }
6752 
6753 }
6754 }
6755 }
6756 
6757 }
6758 
6759 }
6760 
6761 } else
6762 {
6763 if( 1 )
6764 {
6765 continue;
6766 
6767 } else
6768 {
6769 }
6770 }
6771 }
6772 }
6773 }
6774 }
6775 
6776 } else
6777 {
6778 {
6779 IkReal j9array[1], cj9array[1], sj9array[1];
6780 bool j9valid[1]={false};
6781 _nj9 = 1;
6782 IkReal x1561=((cj12)*(sj13));
6783 IkReal x1562=((cj12)*(cj13));
6784 IkReal x1563=((IkReal(1.00000000000000))*(sj14));
6785 if( IKabs(((gconst2)*(((((r12)*(x1561)))+(((IkReal(-1.00000000000000))*(r11)*(x1562)*(x1563)))+(((cj14)*(r10)*(x1562))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((IkReal(-1.00000000000000))*(r01)*(x1562)*(x1563)))+(((cj14)*(r00)*(x1562)))+(((r02)*(x1561))))))) < IKFAST_ATAN2_MAGTHRESH )
6786  continue;
6787 j9array[0]=IKatan2(((gconst2)*(((((r12)*(x1561)))+(((IkReal(-1.00000000000000))*(r11)*(x1562)*(x1563)))+(((cj14)*(r10)*(x1562)))))), ((gconst2)*(((((IkReal(-1.00000000000000))*(r01)*(x1562)*(x1563)))+(((cj14)*(r00)*(x1562)))+(((r02)*(x1561)))))));
6788 sj9array[0]=IKsin(j9array[0]);
6789 cj9array[0]=IKcos(j9array[0]);
6790 if( j9array[0] > IKPI )
6791 {
6792  j9array[0]-=IK2PI;
6793 }
6794 else if( j9array[0] < -IKPI )
6795 { j9array[0]+=IK2PI;
6796 }
6797 j9valid[0] = true;
6798 for(int ij9 = 0; ij9 < 1; ++ij9)
6799 {
6800 if( !j9valid[ij9] )
6801 {
6802  continue;
6803 }
6804 _ij9[0] = ij9; _ij9[1] = -1;
6805 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
6806 {
6807 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
6808 {
6809  j9valid[iij9]=false; _ij9[1] = iij9; break;
6810 }
6811 }
6812 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
6813 {
6814 IkReal evalcond[3];
6815 IkReal x1564=IKsin(j9);
6816 IkReal x1565=IKcos(j9);
6817 IkReal x1566=((IkReal(1.00000000000000))*(sj14));
6818 IkReal x1567=((sj13)*(x1565));
6819 IkReal x1568=((cj13)*(x1564));
6820 IkReal x1569=((IkReal(1.00000000000000))*(cj14)*(r00));
6821 IkReal x1570=((r01)*(x1564));
6822 IkReal x1571=((r10)*(x1565));
6823 IkReal x1572=((sj13)*(x1564));
6824 IkReal x1573=((cj13)*(x1565));
6825 evalcond[0]=((((IkReal(-1.00000000000000))*(x1566)*(x1571)))+(cj12)+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1565)))+(((r00)*(sj14)*(x1564)))+(((cj14)*(x1570))));
6826 evalcond[1]=((((r01)*(sj14)*(x1568)))+(((cj13)*(cj14)*(x1571)))+(((r12)*(x1567)))+(((IkReal(-1.00000000000000))*(x1568)*(x1569)))+(((IkReal(-1.00000000000000))*(r11)*(x1566)*(x1573)))+(((IkReal(-1.00000000000000))*(r02)*(x1572))));
6827 evalcond[2]=((sj12)+(((IkReal(-1.00000000000000))*(x1569)*(x1572)))+(((cj14)*(r10)*(x1567)))+(((sj13)*(sj14)*(x1570)))+(((IkReal(-1.00000000000000))*(r12)*(x1573)))+(((IkReal(-1.00000000000000))*(r11)*(x1566)*(x1567)))+(((r02)*(x1568))));
6828 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
6829 {
6830 continue;
6831 }
6832 }
6833 
6834 {
6835 IkReal dummyeval[1];
6836 IkReal gconst6;
6837 gconst6=IKsign(((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11))))));
6838 dummyeval[0]=((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11)))));
6839 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
6840 {
6841 {
6842 IkReal dummyeval[1];
6843 IkReal gconst7;
6844 gconst7=IKsign(((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11))))));
6845 dummyeval[0]=((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11)))));
6846 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
6847 {
6848 {
6849 IkReal evalcond[9];
6850 IkReal x1574=((r00)*(sj9));
6851 IkReal x1575=((IkReal(1.00000000000000))*(sj13));
6852 IkReal x1576=((cj13)*(sj14));
6853 IkReal x1577=((cj9)*(sj14));
6854 IkReal x1578=((cj13)*(r02));
6855 IkReal x1579=((sj13)*(sj14));
6856 IkReal x1580=((r01)*(sj9));
6857 IkReal x1581=((cj9)*(sj13));
6858 IkReal x1582=((IkReal(1.00000000000000))*(cj9));
6859 IkReal x1583=((cj14)*(r10));
6860 IkReal x1584=((cj14)*(npx));
6861 IkReal x1585=((IkReal(1.00000000000000))*(cj13));
6862 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
6863 evalcond[1]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(sj11))));
6864 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x1575)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(x1584)*(x1585)))+(((npy)*(x1576))));
6865 evalcond[3]=((((sj14)*(x1574)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1582)))+(((IkReal(-1.00000000000000))*(r10)*(x1577)))+(((cj14)*(x1580))));
6866 evalcond[4]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x1575)))+(((r21)*(x1579)))+(((cj13)*(r22))));
6867 evalcond[5]=((IkReal(0.0950000000000000))+(((npy)*(x1579)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x1575)*(x1584))));
6868 evalcond[6]=((((cj13)*(cj9)*(x1583)))+(((IkReal(-1.00000000000000))*(cj14)*(x1574)*(x1585)))+(((x1576)*(x1580)))+(((r12)*(x1581)))+(((IkReal(-1.00000000000000))*(r11)*(x1576)*(x1582)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x1575))));
6869 evalcond[7]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1582)))+(((x1579)*(x1580)))+(((IkReal(-1.00000000000000))*(r11)*(x1575)*(x1577)))+(((IkReal(-1.00000000000000))*(cj14)*(x1574)*(x1575)))+(((sj9)*(x1578)))+(((x1581)*(x1583))));
6870 evalcond[8]=((((IkReal(-1.00000000000000))*(x1578)*(x1582)))+(((IkReal(-1.00000000000000))*(r01)*(x1575)*(x1577)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x1575)))+(((sj13)*(sj9)*(x1583)))+(((cj14)*(r00)*(x1581)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1585))));
6871 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 )
6872 {
6873 {
6874 IkReal dummyeval[1];
6875 IkReal gconst8;
6876 gconst8=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
6877 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
6878 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
6879 {
6880 {
6881 IkReal dummyeval[1];
6882 IkReal gconst9;
6883 gconst9=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
6884 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
6885 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
6886 {
6887 continue;
6888 
6889 } else
6890 {
6891 {
6892 IkReal j10array[1], cj10array[1], sj10array[1];
6893 bool j10valid[1]={false};
6894 _nj10 = 1;
6895 IkReal x1586=((IkReal(1.00000000000000))*(cj11));
6896 IkReal x1587=((r20)*(sj14));
6897 IkReal x1588=((cj14)*(r21));
6898 IkReal x1589=((cj14)*(cj9)*(r01));
6899 IkReal x1590=((r10)*(sj14)*(sj9));
6900 IkReal x1591=((cj9)*(r00)*(sj14));
6901 IkReal x1592=((cj14)*(r11)*(sj9));
6902 if( IKabs(((gconst9)*(((((sj11)*(x1592)))+(((sj11)*(x1590)))+(((sj11)*(x1591)))+(((sj11)*(x1589)))+(((cj11)*(x1588)))+(((cj11)*(x1587))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((((sj11)*(x1588)))+(((sj11)*(x1587)))+(((IkReal(-1.00000000000000))*(x1586)*(x1591)))+(((IkReal(-1.00000000000000))*(x1586)*(x1590)))+(((IkReal(-1.00000000000000))*(x1586)*(x1592)))+(((IkReal(-1.00000000000000))*(x1586)*(x1589))))))) < IKFAST_ATAN2_MAGTHRESH )
6903  continue;
6904 j10array[0]=IKatan2(((gconst9)*(((((sj11)*(x1592)))+(((sj11)*(x1590)))+(((sj11)*(x1591)))+(((sj11)*(x1589)))+(((cj11)*(x1588)))+(((cj11)*(x1587)))))), ((gconst9)*(((((sj11)*(x1588)))+(((sj11)*(x1587)))+(((IkReal(-1.00000000000000))*(x1586)*(x1591)))+(((IkReal(-1.00000000000000))*(x1586)*(x1590)))+(((IkReal(-1.00000000000000))*(x1586)*(x1592)))+(((IkReal(-1.00000000000000))*(x1586)*(x1589)))))));
6905 sj10array[0]=IKsin(j10array[0]);
6906 cj10array[0]=IKcos(j10array[0]);
6907 if( j10array[0] > IKPI )
6908 {
6909  j10array[0]-=IK2PI;
6910 }
6911 else if( j10array[0] < -IKPI )
6912 { j10array[0]+=IK2PI;
6913 }
6914 j10valid[0] = true;
6915 for(int ij10 = 0; ij10 < 1; ++ij10)
6916 {
6917 if( !j10valid[ij10] )
6918 {
6919  continue;
6920 }
6921 _ij10[0] = ij10; _ij10[1] = -1;
6922 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
6923 {
6924 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
6925 {
6926  j10valid[iij10]=false; _ij10[1] = iij10; break;
6927 }
6928 }
6929 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
6930 {
6931 IkReal evalcond[4];
6932 IkReal x1593=IKsin(j10);
6933 IkReal x1594=IKcos(j10);
6934 IkReal x1595=((cj13)*(sj14));
6935 IkReal x1596=((cj13)*(cj14));
6936 IkReal x1597=((r10)*(sj9));
6937 IkReal x1598=((IkReal(1.00000000000000))*(cj9));
6938 IkReal x1599=((sj11)*(x1593));
6939 IkReal x1600=((IkReal(1.00000000000000))*(x1594));
6940 IkReal x1601=((cj11)*(x1593));
6941 IkReal x1602=((IkReal(1.00000000000000))*(r11)*(sj9));
6942 IkReal x1603=((cj11)*(x1600));
6943 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x1601)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x1600))));
6944 evalcond[1]=((((IkReal(-1.00000000000000))*(x1603)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(x1599)+(((r21)*(x1595)))+(((IkReal(-1.00000000000000))*(r20)*(x1596))));
6945 evalcond[2]=((((IkReal(-1.00000000000000))*(x1603)))+(((IkReal(-1.00000000000000))*(cj14)*(x1602)))+(x1599)+(((IkReal(-1.00000000000000))*(sj14)*(x1597)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1598)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x1598))));
6946 evalcond[3]=((((x1596)*(x1597)))+(((cj9)*(r00)*(x1596)))+(((sj11)*(x1594)))+(x1601)+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(x1595)*(x1602)))+(((IkReal(-1.00000000000000))*(r01)*(x1595)*(x1598)))+(((r12)*(sj13)*(sj9))));
6947 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
6948 {
6949 continue;
6950 }
6951 }
6952 
6953 {
6954 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
6955 vinfos[0].jointtype = 1;
6956 vinfos[0].foffset = j9;
6957 vinfos[0].indices[0] = _ij9[0];
6958 vinfos[0].indices[1] = _ij9[1];
6959 vinfos[0].maxsolutions = _nj9;
6960 vinfos[1].jointtype = 1;
6961 vinfos[1].foffset = j10;
6962 vinfos[1].indices[0] = _ij10[0];
6963 vinfos[1].indices[1] = _ij10[1];
6964 vinfos[1].maxsolutions = _nj10;
6965 vinfos[2].jointtype = 1;
6966 vinfos[2].foffset = j11;
6967 vinfos[2].indices[0] = _ij11[0];
6968 vinfos[2].indices[1] = _ij11[1];
6969 vinfos[2].maxsolutions = _nj11;
6970 vinfos[3].jointtype = 1;
6971 vinfos[3].foffset = j12;
6972 vinfos[3].indices[0] = _ij12[0];
6973 vinfos[3].indices[1] = _ij12[1];
6974 vinfos[3].maxsolutions = _nj12;
6975 vinfos[4].jointtype = 1;
6976 vinfos[4].foffset = j13;
6977 vinfos[4].indices[0] = _ij13[0];
6978 vinfos[4].indices[1] = _ij13[1];
6979 vinfos[4].maxsolutions = _nj13;
6980 vinfos[5].jointtype = 1;
6981 vinfos[5].foffset = j14;
6982 vinfos[5].indices[0] = _ij14[0];
6983 vinfos[5].indices[1] = _ij14[1];
6984 vinfos[5].maxsolutions = _nj14;
6985 std::vector<int> vfree(0);
6986 solutions.AddSolution(vinfos,vfree);
6987 }
6988 }
6989 }
6990 
6991 }
6992 
6993 }
6994 
6995 } else
6996 {
6997 {
6998 IkReal j10array[1], cj10array[1], sj10array[1];
6999 bool j10valid[1]={false};
7000 _nj10 = 1;
7001 IkReal x1604=((cj11)*(r20));
7002 IkReal x1605=((IkReal(1.00000000000000))*(cj13));
7003 IkReal x1606=((r21)*(sj14));
7004 IkReal x1607=((r22)*(sj13));
7005 IkReal x1608=((cj14)*(sj11));
7006 if( IKabs(((gconst8)*(((((IkReal(-1.00000000000000))*(sj11)*(x1605)*(x1606)))+(((cj11)*(cj14)*(r21)))+(((sj11)*(x1607)))+(((sj14)*(x1604)))+(((cj13)*(r20)*(x1608))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst8)*(((((r21)*(x1608)))+(((IkReal(-1.00000000000000))*(cj14)*(x1604)*(x1605)))+(((IkReal(-1.00000000000000))*(cj11)*(x1607)))+(((r20)*(sj11)*(sj14)))+(((cj11)*(cj13)*(x1606))))))) < IKFAST_ATAN2_MAGTHRESH )
7007  continue;
7008 j10array[0]=IKatan2(((gconst8)*(((((IkReal(-1.00000000000000))*(sj11)*(x1605)*(x1606)))+(((cj11)*(cj14)*(r21)))+(((sj11)*(x1607)))+(((sj14)*(x1604)))+(((cj13)*(r20)*(x1608)))))), ((gconst8)*(((((r21)*(x1608)))+(((IkReal(-1.00000000000000))*(cj14)*(x1604)*(x1605)))+(((IkReal(-1.00000000000000))*(cj11)*(x1607)))+(((r20)*(sj11)*(sj14)))+(((cj11)*(cj13)*(x1606)))))));
7009 sj10array[0]=IKsin(j10array[0]);
7010 cj10array[0]=IKcos(j10array[0]);
7011 if( j10array[0] > IKPI )
7012 {
7013  j10array[0]-=IK2PI;
7014 }
7015 else if( j10array[0] < -IKPI )
7016 { j10array[0]+=IK2PI;
7017 }
7018 j10valid[0] = true;
7019 for(int ij10 = 0; ij10 < 1; ++ij10)
7020 {
7021 if( !j10valid[ij10] )
7022 {
7023  continue;
7024 }
7025 _ij10[0] = ij10; _ij10[1] = -1;
7026 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
7027 {
7028 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
7029 {
7030  j10valid[iij10]=false; _ij10[1] = iij10; break;
7031 }
7032 }
7033 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
7034 {
7035 IkReal evalcond[4];
7036 IkReal x1609=IKsin(j10);
7037 IkReal x1610=IKcos(j10);
7038 IkReal x1611=((cj13)*(sj14));
7039 IkReal x1612=((cj13)*(cj14));
7040 IkReal x1613=((r10)*(sj9));
7041 IkReal x1614=((IkReal(1.00000000000000))*(cj9));
7042 IkReal x1615=((sj11)*(x1609));
7043 IkReal x1616=((IkReal(1.00000000000000))*(x1610));
7044 IkReal x1617=((cj11)*(x1609));
7045 IkReal x1618=((IkReal(1.00000000000000))*(r11)*(sj9));
7046 IkReal x1619=((cj11)*(x1616));
7047 evalcond[0]=((((IkReal(-1.00000000000000))*(x1617)))+(((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x1616))));
7048 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x1612)))+(((r21)*(x1611)))+(x1615)+(((IkReal(-1.00000000000000))*(x1619)))+(((IkReal(-1.00000000000000))*(r22)*(sj13))));
7049 evalcond[2]=((((IkReal(-1.00000000000000))*(sj14)*(x1613)))+(x1615)+(((IkReal(-1.00000000000000))*(x1619)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1614)))+(((IkReal(-1.00000000000000))*(cj14)*(x1618)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x1614))));
7050 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x1611)*(x1614)))+(((IkReal(-1.00000000000000))*(x1611)*(x1618)))+(x1617)+(((cj9)*(r02)*(sj13)))+(((sj11)*(x1610)))+(((cj9)*(r00)*(x1612)))+(((x1612)*(x1613)))+(((r12)*(sj13)*(sj9))));
7051 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
7052 {
7053 continue;
7054 }
7055 }
7056 
7057 {
7058 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7059 vinfos[0].jointtype = 1;
7060 vinfos[0].foffset = j9;
7061 vinfos[0].indices[0] = _ij9[0];
7062 vinfos[0].indices[1] = _ij9[1];
7063 vinfos[0].maxsolutions = _nj9;
7064 vinfos[1].jointtype = 1;
7065 vinfos[1].foffset = j10;
7066 vinfos[1].indices[0] = _ij10[0];
7067 vinfos[1].indices[1] = _ij10[1];
7068 vinfos[1].maxsolutions = _nj10;
7069 vinfos[2].jointtype = 1;
7070 vinfos[2].foffset = j11;
7071 vinfos[2].indices[0] = _ij11[0];
7072 vinfos[2].indices[1] = _ij11[1];
7073 vinfos[2].maxsolutions = _nj11;
7074 vinfos[3].jointtype = 1;
7075 vinfos[3].foffset = j12;
7076 vinfos[3].indices[0] = _ij12[0];
7077 vinfos[3].indices[1] = _ij12[1];
7078 vinfos[3].maxsolutions = _nj12;
7079 vinfos[4].jointtype = 1;
7080 vinfos[4].foffset = j13;
7081 vinfos[4].indices[0] = _ij13[0];
7082 vinfos[4].indices[1] = _ij13[1];
7083 vinfos[4].maxsolutions = _nj13;
7084 vinfos[5].jointtype = 1;
7085 vinfos[5].foffset = j14;
7086 vinfos[5].indices[0] = _ij14[0];
7087 vinfos[5].indices[1] = _ij14[1];
7088 vinfos[5].maxsolutions = _nj14;
7089 std::vector<int> vfree(0);
7090 solutions.AddSolution(vinfos,vfree);
7091 }
7092 }
7093 }
7094 
7095 }
7096 
7097 }
7098 
7099 } else
7100 {
7101 IkReal x1620=((r00)*(sj9));
7102 IkReal x1621=((IkReal(1.00000000000000))*(sj13));
7103 IkReal x1622=((cj13)*(sj14));
7104 IkReal x1623=((cj9)*(sj14));
7105 IkReal x1624=((cj13)*(r02));
7106 IkReal x1625=((sj13)*(sj14));
7107 IkReal x1626=((r01)*(sj9));
7108 IkReal x1627=((cj9)*(sj13));
7109 IkReal x1628=((IkReal(1.00000000000000))*(cj9));
7110 IkReal x1629=((cj14)*(r10));
7111 IkReal x1630=((cj14)*(npx));
7112 IkReal x1631=((IkReal(1.00000000000000))*(cj13));
7113 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
7114 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((cj14)*(npy)))+(((npx)*(sj14))));
7115 evalcond[2]=((IkReal(0.235000000000000))+(((npy)*(x1622)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(x1630)*(x1631)))+(((IkReal(-1.00000000000000))*(npz)*(x1621))));
7116 evalcond[3]=((((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1628)))+(((sj14)*(x1620)))+(((IkReal(-1.00000000000000))*(r10)*(x1623)))+(((cj14)*(x1626))));
7117 evalcond[4]=((((r21)*(x1625)))+(((IkReal(-1.00000000000000))*(cj14)*(r20)*(x1621)))+(((cj13)*(r22))));
7118 evalcond[5]=((IkReal(-0.0950000000000000))+(((npy)*(x1625)))+(((IkReal(-1.00000000000000))*(x1621)*(x1630)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
7119 evalcond[6]=((((cj13)*(cj9)*(x1629)))+(((IkReal(-1.00000000000000))*(r11)*(x1622)*(x1628)))+(((x1622)*(x1626)))+(((r12)*(x1627)))+(((IkReal(-1.00000000000000))*(cj14)*(x1620)*(x1631)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x1621))));
7120 evalcond[7]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(cj14)*(x1620)*(x1621)))+(((x1627)*(x1629)))+(((IkReal(-1.00000000000000))*(r11)*(x1621)*(x1623)))+(((sj9)*(x1624)))+(((x1625)*(x1626)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x1628))));
7121 evalcond[8]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1631)))+(((IkReal(-1.00000000000000))*(r01)*(x1621)*(x1623)))+(((cj14)*(r00)*(x1627)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x1621)))+(((IkReal(-1.00000000000000))*(x1624)*(x1628)))+(((sj13)*(sj9)*(x1629))));
7122 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 )
7123 {
7124 {
7125 IkReal dummyeval[1];
7126 IkReal gconst10;
7127 gconst10=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
7128 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
7129 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
7130 {
7131 {
7132 IkReal dummyeval[1];
7133 IkReal gconst11;
7134 gconst11=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
7135 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
7136 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
7137 {
7138 continue;
7139 
7140 } else
7141 {
7142 {
7143 IkReal j10array[1], cj10array[1], sj10array[1];
7144 bool j10valid[1]={false};
7145 _nj10 = 1;
7146 IkReal x1632=((IkReal(1.00000000000000))*(sj11));
7147 IkReal x1633=((cj14)*(r21));
7148 IkReal x1634=((IkReal(1.00000000000000))*(cj11));
7149 IkReal x1635=((r20)*(sj14));
7150 IkReal x1636=((cj9)*(r00)*(sj14));
7151 IkReal x1637=((cj14)*(r11)*(sj9));
7152 IkReal x1638=((cj14)*(cj9)*(r01));
7153 IkReal x1639=((r10)*(sj14)*(sj9));
7154 if( IKabs(((gconst11)*(((((IkReal(-1.00000000000000))*(x1634)*(x1635)))+(((IkReal(-1.00000000000000))*(x1633)*(x1634)))+(((IkReal(-1.00000000000000))*(x1632)*(x1639)))+(((IkReal(-1.00000000000000))*(x1632)*(x1638)))+(((IkReal(-1.00000000000000))*(x1632)*(x1637)))+(((IkReal(-1.00000000000000))*(x1632)*(x1636))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((((cj11)*(x1636)))+(((cj11)*(x1637)))+(((cj11)*(x1638)))+(((cj11)*(x1639)))+(((IkReal(-1.00000000000000))*(x1632)*(x1635)))+(((IkReal(-1.00000000000000))*(x1632)*(x1633))))))) < IKFAST_ATAN2_MAGTHRESH )
7155  continue;
7156 j10array[0]=IKatan2(((gconst11)*(((((IkReal(-1.00000000000000))*(x1634)*(x1635)))+(((IkReal(-1.00000000000000))*(x1633)*(x1634)))+(((IkReal(-1.00000000000000))*(x1632)*(x1639)))+(((IkReal(-1.00000000000000))*(x1632)*(x1638)))+(((IkReal(-1.00000000000000))*(x1632)*(x1637)))+(((IkReal(-1.00000000000000))*(x1632)*(x1636)))))), ((gconst11)*(((((cj11)*(x1636)))+(((cj11)*(x1637)))+(((cj11)*(x1638)))+(((cj11)*(x1639)))+(((IkReal(-1.00000000000000))*(x1632)*(x1635)))+(((IkReal(-1.00000000000000))*(x1632)*(x1633)))))));
7157 sj10array[0]=IKsin(j10array[0]);
7158 cj10array[0]=IKcos(j10array[0]);
7159 if( j10array[0] > IKPI )
7160 {
7161  j10array[0]-=IK2PI;
7162 }
7163 else if( j10array[0] < -IKPI )
7164 { j10array[0]+=IK2PI;
7165 }
7166 j10valid[0] = true;
7167 for(int ij10 = 0; ij10 < 1; ++ij10)
7168 {
7169 if( !j10valid[ij10] )
7170 {
7171  continue;
7172 }
7173 _ij10[0] = ij10; _ij10[1] = -1;
7174 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
7175 {
7176 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
7177 {
7178  j10valid[iij10]=false; _ij10[1] = iij10; break;
7179 }
7180 }
7181 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
7182 {
7183 IkReal evalcond[4];
7184 IkReal x1640=IKsin(j10);
7185 IkReal x1641=IKcos(j10);
7186 IkReal x1642=((cj14)*(sj9));
7187 IkReal x1643=((IkReal(1.00000000000000))*(r11));
7188 IkReal x1644=((cj13)*(sj14));
7189 IkReal x1645=((IkReal(1.00000000000000))*(cj9));
7190 IkReal x1646=((cj13)*(cj14));
7191 IkReal x1647=((cj11)*(x1640));
7192 IkReal x1648=((sj11)*(x1641));
7193 IkReal x1649=((cj11)*(x1641));
7194 IkReal x1650=((sj11)*(x1640));
7195 IkReal x1651=((x1647)+(x1648));
7196 evalcond[0]=((((cj14)*(r21)))+(x1651)+(((r20)*(sj14))));
7197 evalcond[1]=((((r21)*(x1644)))+(x1650)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x1649)))+(((IkReal(-1.00000000000000))*(r20)*(x1646))));
7198 evalcond[2]=((x1649)+(((IkReal(-1.00000000000000))*(x1650)))+(((IkReal(-1.00000000000000))*(x1642)*(x1643)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1645)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x1645))));
7199 evalcond[3]=((((IkReal(-1.00000000000000))*(sj9)*(x1643)*(x1644)))+(x1651)+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x1644)*(x1645)))+(((cj13)*(r10)*(x1642)))+(((cj9)*(r00)*(x1646)))+(((r12)*(sj13)*(sj9))));
7200 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
7201 {
7202 continue;
7203 }
7204 }
7205 
7206 {
7207 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7208 vinfos[0].jointtype = 1;
7209 vinfos[0].foffset = j9;
7210 vinfos[0].indices[0] = _ij9[0];
7211 vinfos[0].indices[1] = _ij9[1];
7212 vinfos[0].maxsolutions = _nj9;
7213 vinfos[1].jointtype = 1;
7214 vinfos[1].foffset = j10;
7215 vinfos[1].indices[0] = _ij10[0];
7216 vinfos[1].indices[1] = _ij10[1];
7217 vinfos[1].maxsolutions = _nj10;
7218 vinfos[2].jointtype = 1;
7219 vinfos[2].foffset = j11;
7220 vinfos[2].indices[0] = _ij11[0];
7221 vinfos[2].indices[1] = _ij11[1];
7222 vinfos[2].maxsolutions = _nj11;
7223 vinfos[3].jointtype = 1;
7224 vinfos[3].foffset = j12;
7225 vinfos[3].indices[0] = _ij12[0];
7226 vinfos[3].indices[1] = _ij12[1];
7227 vinfos[3].maxsolutions = _nj12;
7228 vinfos[4].jointtype = 1;
7229 vinfos[4].foffset = j13;
7230 vinfos[4].indices[0] = _ij13[0];
7231 vinfos[4].indices[1] = _ij13[1];
7232 vinfos[4].maxsolutions = _nj13;
7233 vinfos[5].jointtype = 1;
7234 vinfos[5].foffset = j14;
7235 vinfos[5].indices[0] = _ij14[0];
7236 vinfos[5].indices[1] = _ij14[1];
7237 vinfos[5].maxsolutions = _nj14;
7238 std::vector<int> vfree(0);
7239 solutions.AddSolution(vinfos,vfree);
7240 }
7241 }
7242 }
7243 
7244 }
7245 
7246 }
7247 
7248 } else
7249 {
7250 {
7251 IkReal j10array[1], cj10array[1], sj10array[1];
7252 bool j10valid[1]={false};
7253 _nj10 = 1;
7254 IkReal x1652=((cj13)*(sj11));
7255 IkReal x1653=((r21)*(sj14));
7256 IkReal x1654=((cj14)*(r20));
7257 IkReal x1655=((cj11)*(cj13));
7258 IkReal x1656=((r22)*(sj13));
7259 IkReal x1657=((r20)*(sj14));
7260 IkReal x1658=((cj14)*(r21));
7261 if( IKabs(((gconst10)*(((((IkReal(-1.00000000000000))*(sj11)*(x1656)))+(((IkReal(-1.00000000000000))*(x1652)*(x1654)))+(((cj11)*(x1658)))+(((cj11)*(x1657)))+(((x1652)*(x1653))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((((sj11)*(x1657)))+(((sj11)*(x1658)))+(((x1654)*(x1655)))+(((IkReal(-1.00000000000000))*(x1653)*(x1655)))+(((cj11)*(x1656))))))) < IKFAST_ATAN2_MAGTHRESH )
7262  continue;
7263 j10array[0]=IKatan2(((gconst10)*(((((IkReal(-1.00000000000000))*(sj11)*(x1656)))+(((IkReal(-1.00000000000000))*(x1652)*(x1654)))+(((cj11)*(x1658)))+(((cj11)*(x1657)))+(((x1652)*(x1653)))))), ((gconst10)*(((((sj11)*(x1657)))+(((sj11)*(x1658)))+(((x1654)*(x1655)))+(((IkReal(-1.00000000000000))*(x1653)*(x1655)))+(((cj11)*(x1656)))))));
7264 sj10array[0]=IKsin(j10array[0]);
7265 cj10array[0]=IKcos(j10array[0]);
7266 if( j10array[0] > IKPI )
7267 {
7268  j10array[0]-=IK2PI;
7269 }
7270 else if( j10array[0] < -IKPI )
7271 { j10array[0]+=IK2PI;
7272 }
7273 j10valid[0] = true;
7274 for(int ij10 = 0; ij10 < 1; ++ij10)
7275 {
7276 if( !j10valid[ij10] )
7277 {
7278  continue;
7279 }
7280 _ij10[0] = ij10; _ij10[1] = -1;
7281 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
7282 {
7283 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
7284 {
7285  j10valid[iij10]=false; _ij10[1] = iij10; break;
7286 }
7287 }
7288 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
7289 {
7290 IkReal evalcond[4];
7291 IkReal x1659=IKsin(j10);
7292 IkReal x1660=IKcos(j10);
7293 IkReal x1661=((cj14)*(sj9));
7294 IkReal x1662=((IkReal(1.00000000000000))*(r11));
7295 IkReal x1663=((cj13)*(sj14));
7296 IkReal x1664=((IkReal(1.00000000000000))*(cj9));
7297 IkReal x1665=((cj13)*(cj14));
7298 IkReal x1666=((cj11)*(x1659));
7299 IkReal x1667=((sj11)*(x1660));
7300 IkReal x1668=((cj11)*(x1660));
7301 IkReal x1669=((sj11)*(x1659));
7302 IkReal x1670=((x1667)+(x1666));
7303 evalcond[0]=((((cj14)*(r21)))+(x1670)+(((r20)*(sj14))));
7304 evalcond[1]=((x1669)+(((IkReal(-1.00000000000000))*(x1668)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x1665)))+(((r21)*(x1663))));
7305 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(sj14)*(x1664)))+(((IkReal(-1.00000000000000))*(x1661)*(x1662)))+(x1668)+(((IkReal(-1.00000000000000))*(x1669)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x1664))));
7306 evalcond[3]=((((IkReal(-1.00000000000000))*(sj9)*(x1662)*(x1663)))+(((IkReal(-1.00000000000000))*(r01)*(x1663)*(x1664)))+(x1670)+(((cj9)*(r02)*(sj13)))+(((cj9)*(r00)*(x1665)))+(((r12)*(sj13)*(sj9)))+(((cj13)*(r10)*(x1661))));
7307 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
7308 {
7309 continue;
7310 }
7311 }
7312 
7313 {
7314 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7315 vinfos[0].jointtype = 1;
7316 vinfos[0].foffset = j9;
7317 vinfos[0].indices[0] = _ij9[0];
7318 vinfos[0].indices[1] = _ij9[1];
7319 vinfos[0].maxsolutions = _nj9;
7320 vinfos[1].jointtype = 1;
7321 vinfos[1].foffset = j10;
7322 vinfos[1].indices[0] = _ij10[0];
7323 vinfos[1].indices[1] = _ij10[1];
7324 vinfos[1].maxsolutions = _nj10;
7325 vinfos[2].jointtype = 1;
7326 vinfos[2].foffset = j11;
7327 vinfos[2].indices[0] = _ij11[0];
7328 vinfos[2].indices[1] = _ij11[1];
7329 vinfos[2].maxsolutions = _nj11;
7330 vinfos[3].jointtype = 1;
7331 vinfos[3].foffset = j12;
7332 vinfos[3].indices[0] = _ij12[0];
7333 vinfos[3].indices[1] = _ij12[1];
7334 vinfos[3].maxsolutions = _nj12;
7335 vinfos[4].jointtype = 1;
7336 vinfos[4].foffset = j13;
7337 vinfos[4].indices[0] = _ij13[0];
7338 vinfos[4].indices[1] = _ij13[1];
7339 vinfos[4].maxsolutions = _nj13;
7340 vinfos[5].jointtype = 1;
7341 vinfos[5].foffset = j14;
7342 vinfos[5].indices[0] = _ij14[0];
7343 vinfos[5].indices[1] = _ij14[1];
7344 vinfos[5].maxsolutions = _nj14;
7345 std::vector<int> vfree(0);
7346 solutions.AddSolution(vinfos,vfree);
7347 }
7348 }
7349 }
7350 
7351 }
7352 
7353 }
7354 
7355 } else
7356 {
7357 IkReal x1671=((cj9)*(r10));
7358 IkReal x1672=((cj13)*(cj14));
7359 IkReal x1673=((sj14)*(sj9));
7360 IkReal x1674=((IkReal(1.00000000000000))*(sj14));
7361 IkReal x1675=((cj9)*(sj13));
7362 IkReal x1676=((r02)*(sj9));
7363 IkReal x1677=((cj13)*(cj9));
7364 IkReal x1678=((cj14)*(r01));
7365 IkReal x1679=((IkReal(1.00000000000000))*(npx));
7366 IkReal x1680=((cj14)*(sj13));
7367 IkReal x1681=((IkReal(1.00000000000000))*(cj9));
7368 IkReal x1682=((npy)*(sj14));
7369 IkReal x1683=((IkReal(1.00000000000000))*(sj13));
7370 IkReal x1684=((IkReal(1.00000000000000))*(cj14)*(sj9));
7371 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
7372 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
7373 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
7374 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(x1672)*(x1679)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((cj13)*(x1682)))+(((IkReal(-1.00000000000000))*(npz)*(x1683))));
7375 evalcond[4]=((IkReal(1.00000000000000))+(((r00)*(x1673)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1681)))+(((IkReal(-1.00000000000000))*(x1671)*(x1674)))+(((sj9)*(x1678))));
7376 evalcond[5]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((IkReal(-1.00000000000000))*(x1679)*(x1680)))+(((cj13)*(npz)))+(((sj13)*(x1682)))+(((IkReal(0.0900000000000000))*(sj13))));
7377 evalcond[6]=((((IkReal(-1.00000000000000))*(r11)*(x1684)))+(((IkReal(-1.00000000000000))*(r10)*(x1673)))+(((IkReal(-1.00000000000000))*(x1678)*(x1681)))+(((IkReal(-1.00000000000000))*(cj9)*(r00)*(x1674))));
7378 evalcond[7]=((((IkReal(-1.00000000000000))*(r00)*(sj9)*(x1672)))+(((cj13)*(r01)*(x1673)))+(((IkReal(-1.00000000000000))*(x1676)*(x1683)))+(((IkReal(-1.00000000000000))*(r11)*(x1674)*(x1677)))+(((x1671)*(x1672)))+(((r12)*(x1675))));
7379 evalcond[8]=((((r01)*(sj13)*(x1673)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x1680)))+(((x1671)*(x1680)))+(((IkReal(-1.00000000000000))*(r11)*(x1674)*(x1675)))+(((IkReal(-1.00000000000000))*(r12)*(x1677)))+(((cj13)*(x1676))));
7380 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 )
7381 {
7382 {
7383 IkReal dummyeval[1];
7384 IkReal gconst12;
7385 gconst12=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
7386 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
7387 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
7388 {
7389 {
7390 IkReal dummyeval[1];
7391 IkReal gconst13;
7392 gconst13=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
7393 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
7394 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
7395 {
7396 continue;
7397 
7398 } else
7399 {
7400 {
7401 IkReal j10array[1], cj10array[1], sj10array[1];
7402 bool j10valid[1]={false};
7403 _nj10 = 1;
7404 IkReal x1685=((cj13)*(sj14));
7405 IkReal x1686=((IkReal(1.00000000000000))*(cj11));
7406 IkReal x1687=((r11)*(sj9));
7407 IkReal x1688=((IkReal(1.00000000000000))*(sj11));
7408 IkReal x1689=((cj13)*(cj14));
7409 IkReal x1690=((cj9)*(r01));
7410 IkReal x1691=((cj9)*(sj11));
7411 IkReal x1692=((r02)*(sj13));
7412 IkReal x1693=((r10)*(sj9));
7413 IkReal x1694=((cj11)*(cj9));
7414 IkReal x1695=((r22)*(sj13));
7415 IkReal x1696=((r12)*(sj13)*(sj9));
7416 if( IKabs(((gconst13)*(((((IkReal(-1.00000000000000))*(x1688)*(x1695)))+(((IkReal(-1.00000000000000))*(x1685)*(x1686)*(x1690)))+(((IkReal(-1.00000000000000))*(r20)*(x1688)*(x1689)))+(((IkReal(-1.00000000000000))*(x1685)*(x1686)*(x1687)))+(((r00)*(x1689)*(x1694)))+(((x1692)*(x1694)))+(((cj11)*(x1696)))+(((r21)*(sj11)*(x1685)))+(((cj11)*(x1689)*(x1693))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst13)*(((((sj11)*(x1696)))+(((r00)*(x1689)*(x1691)))+(((x1691)*(x1692)))+(((sj11)*(x1689)*(x1693)))+(((cj11)*(x1695)))+(((cj11)*(r20)*(x1689)))+(((IkReal(-1.00000000000000))*(x1685)*(x1687)*(x1688)))+(((IkReal(-1.00000000000000))*(r21)*(x1685)*(x1686)))+(((IkReal(-1.00000000000000))*(x1685)*(x1688)*(x1690))))))) < IKFAST_ATAN2_MAGTHRESH )
7417  continue;
7418 j10array[0]=IKatan2(((gconst13)*(((((IkReal(-1.00000000000000))*(x1688)*(x1695)))+(((IkReal(-1.00000000000000))*(x1685)*(x1686)*(x1690)))+(((IkReal(-1.00000000000000))*(r20)*(x1688)*(x1689)))+(((IkReal(-1.00000000000000))*(x1685)*(x1686)*(x1687)))+(((r00)*(x1689)*(x1694)))+(((x1692)*(x1694)))+(((cj11)*(x1696)))+(((r21)*(sj11)*(x1685)))+(((cj11)*(x1689)*(x1693)))))), ((gconst13)*(((((sj11)*(x1696)))+(((r00)*(x1689)*(x1691)))+(((x1691)*(x1692)))+(((sj11)*(x1689)*(x1693)))+(((cj11)*(x1695)))+(((cj11)*(r20)*(x1689)))+(((IkReal(-1.00000000000000))*(x1685)*(x1687)*(x1688)))+(((IkReal(-1.00000000000000))*(r21)*(x1685)*(x1686)))+(((IkReal(-1.00000000000000))*(x1685)*(x1688)*(x1690)))))));
7419 sj10array[0]=IKsin(j10array[0]);
7420 cj10array[0]=IKcos(j10array[0]);
7421 if( j10array[0] > IKPI )
7422 {
7423  j10array[0]-=IK2PI;
7424 }
7425 else if( j10array[0] < -IKPI )
7426 { j10array[0]+=IK2PI;
7427 }
7428 j10valid[0] = true;
7429 for(int ij10 = 0; ij10 < 1; ++ij10)
7430 {
7431 if( !j10valid[ij10] )
7432 {
7433  continue;
7434 }
7435 _ij10[0] = ij10; _ij10[1] = -1;
7436 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
7437 {
7438 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
7439 {
7440  j10valid[iij10]=false; _ij10[1] = iij10; break;
7441 }
7442 }
7443 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
7444 {
7445 IkReal evalcond[4];
7446 IkReal x1697=IKsin(j10);
7447 IkReal x1698=IKcos(j10);
7448 IkReal x1699=((IkReal(1.00000000000000))*(sj13));
7449 IkReal x1700=((r11)*(sj9));
7450 IkReal x1701=((cj9)*(r01));
7451 IkReal x1702=((r21)*(sj14));
7452 IkReal x1703=((cj9)*(r02));
7453 IkReal x1704=((sj13)*(sj9));
7454 IkReal x1705=((cj14)*(r10));
7455 IkReal x1706=((IkReal(1.00000000000000))*(cj13));
7456 IkReal x1707=((cj14)*(r20));
7457 IkReal x1708=((cj11)*(x1697));
7458 IkReal x1709=((sj11)*(x1698));
7459 IkReal x1710=((sj14)*(x1706));
7460 IkReal x1711=((sj11)*(x1697));
7461 IkReal x1712=((cj11)*(x1698));
7462 IkReal x1713=((cj14)*(cj9)*(r00));
7463 IkReal x1714=((x1708)+(x1709));
7464 evalcond[0]=((x1711)+(((IkReal(-1.00000000000000))*(x1712)))+(((IkReal(-1.00000000000000))*(r22)*(x1699)))+(((IkReal(-1.00000000000000))*(x1706)*(x1707)))+(((cj13)*(x1702))));
7465 evalcond[1]=((x1714)+(((IkReal(-1.00000000000000))*(x1699)*(x1707)))+(((sj13)*(x1702)))+(((cj13)*(r22))));
7466 evalcond[2]=((((IkReal(-1.00000000000000))*(x1701)*(x1710)))+(((cj13)*(x1713)))+(((r12)*(x1704)))+(x1714)+(((sj13)*(x1703)))+(((IkReal(-1.00000000000000))*(x1700)*(x1710)))+(((cj13)*(sj9)*(x1705))));
7467 evalcond[3]=((x1712)+(((sj13)*(x1713)))+(((IkReal(-1.00000000000000))*(x1711)))+(((x1704)*(x1705)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1706)))+(((IkReal(-1.00000000000000))*(x1703)*(x1706)))+(((IkReal(-1.00000000000000))*(sj14)*(x1699)*(x1700)))+(((IkReal(-1.00000000000000))*(sj14)*(x1699)*(x1701))));
7468 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
7469 {
7470 continue;
7471 }
7472 }
7473 
7474 {
7475 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7476 vinfos[0].jointtype = 1;
7477 vinfos[0].foffset = j9;
7478 vinfos[0].indices[0] = _ij9[0];
7479 vinfos[0].indices[1] = _ij9[1];
7480 vinfos[0].maxsolutions = _nj9;
7481 vinfos[1].jointtype = 1;
7482 vinfos[1].foffset = j10;
7483 vinfos[1].indices[0] = _ij10[0];
7484 vinfos[1].indices[1] = _ij10[1];
7485 vinfos[1].maxsolutions = _nj10;
7486 vinfos[2].jointtype = 1;
7487 vinfos[2].foffset = j11;
7488 vinfos[2].indices[0] = _ij11[0];
7489 vinfos[2].indices[1] = _ij11[1];
7490 vinfos[2].maxsolutions = _nj11;
7491 vinfos[3].jointtype = 1;
7492 vinfos[3].foffset = j12;
7493 vinfos[3].indices[0] = _ij12[0];
7494 vinfos[3].indices[1] = _ij12[1];
7495 vinfos[3].maxsolutions = _nj12;
7496 vinfos[4].jointtype = 1;
7497 vinfos[4].foffset = j13;
7498 vinfos[4].indices[0] = _ij13[0];
7499 vinfos[4].indices[1] = _ij13[1];
7500 vinfos[4].maxsolutions = _nj13;
7501 vinfos[5].jointtype = 1;
7502 vinfos[5].foffset = j14;
7503 vinfos[5].indices[0] = _ij14[0];
7504 vinfos[5].indices[1] = _ij14[1];
7505 vinfos[5].maxsolutions = _nj14;
7506 std::vector<int> vfree(0);
7507 solutions.AddSolution(vinfos,vfree);
7508 }
7509 }
7510 }
7511 
7512 }
7513 
7514 }
7515 
7516 } else
7517 {
7518 {
7519 IkReal j10array[1], cj10array[1], sj10array[1];
7520 bool j10valid[1]={false};
7521 _nj10 = 1;
7522 IkReal x1715=((IkReal(1.00000000000000))*(cj11));
7523 IkReal x1716=((cj11)*(r22));
7524 IkReal x1717=((sj11)*(sj13));
7525 IkReal x1718=((r21)*(sj14));
7526 IkReal x1719=((cj13)*(sj11));
7527 IkReal x1720=((cj14)*(r20));
7528 IkReal x1721=((cj13)*(x1718));
7529 IkReal x1722=((IkReal(1.00000000000000))*(x1720));
7530 if( IKabs(((gconst12)*(((((cj13)*(x1716)))+(((cj11)*(sj13)*(x1718)))+(((IkReal(-1.00000000000000))*(r22)*(x1717)))+(((IkReal(-1.00000000000000))*(sj13)*(x1715)*(x1720)))+(((IkReal(-1.00000000000000))*(x1719)*(x1722)))+(((x1718)*(x1719))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst12)*(((((r22)*(x1719)))+(((IkReal(-1.00000000000000))*(x1715)*(x1721)))+(((sj13)*(x1716)))+(((IkReal(-1.00000000000000))*(x1717)*(x1722)))+(((cj11)*(cj13)*(x1720)))+(((x1717)*(x1718))))))) < IKFAST_ATAN2_MAGTHRESH )
7531  continue;
7532 j10array[0]=IKatan2(((gconst12)*(((((cj13)*(x1716)))+(((cj11)*(sj13)*(x1718)))+(((IkReal(-1.00000000000000))*(r22)*(x1717)))+(((IkReal(-1.00000000000000))*(sj13)*(x1715)*(x1720)))+(((IkReal(-1.00000000000000))*(x1719)*(x1722)))+(((x1718)*(x1719)))))), ((gconst12)*(((((r22)*(x1719)))+(((IkReal(-1.00000000000000))*(x1715)*(x1721)))+(((sj13)*(x1716)))+(((IkReal(-1.00000000000000))*(x1717)*(x1722)))+(((cj11)*(cj13)*(x1720)))+(((x1717)*(x1718)))))));
7533 sj10array[0]=IKsin(j10array[0]);
7534 cj10array[0]=IKcos(j10array[0]);
7535 if( j10array[0] > IKPI )
7536 {
7537  j10array[0]-=IK2PI;
7538 }
7539 else if( j10array[0] < -IKPI )
7540 { j10array[0]+=IK2PI;
7541 }
7542 j10valid[0] = true;
7543 for(int ij10 = 0; ij10 < 1; ++ij10)
7544 {
7545 if( !j10valid[ij10] )
7546 {
7547  continue;
7548 }
7549 _ij10[0] = ij10; _ij10[1] = -1;
7550 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
7551 {
7552 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
7553 {
7554  j10valid[iij10]=false; _ij10[1] = iij10; break;
7555 }
7556 }
7557 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
7558 {
7559 IkReal evalcond[4];
7560 IkReal x1723=IKsin(j10);
7561 IkReal x1724=IKcos(j10);
7562 IkReal x1725=((IkReal(1.00000000000000))*(sj13));
7563 IkReal x1726=((r11)*(sj9));
7564 IkReal x1727=((cj9)*(r01));
7565 IkReal x1728=((r21)*(sj14));
7566 IkReal x1729=((cj9)*(r02));
7567 IkReal x1730=((sj13)*(sj9));
7568 IkReal x1731=((cj14)*(r10));
7569 IkReal x1732=((IkReal(1.00000000000000))*(cj13));
7570 IkReal x1733=((cj14)*(r20));
7571 IkReal x1734=((cj11)*(x1723));
7572 IkReal x1735=((sj11)*(x1724));
7573 IkReal x1736=((sj14)*(x1732));
7574 IkReal x1737=((sj11)*(x1723));
7575 IkReal x1738=((cj11)*(x1724));
7576 IkReal x1739=((cj14)*(cj9)*(r00));
7577 IkReal x1740=((x1735)+(x1734));
7578 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1725)))+(x1737)+(((cj13)*(x1728)))+(((IkReal(-1.00000000000000))*(x1738)))+(((IkReal(-1.00000000000000))*(x1732)*(x1733))));
7579 evalcond[1]=((x1740)+(((IkReal(-1.00000000000000))*(x1725)*(x1733)))+(((cj13)*(r22)))+(((sj13)*(x1728))));
7580 evalcond[2]=((((cj13)*(sj9)*(x1731)))+(x1740)+(((IkReal(-1.00000000000000))*(x1726)*(x1736)))+(((cj13)*(x1739)))+(((r12)*(x1730)))+(((IkReal(-1.00000000000000))*(x1727)*(x1736)))+(((sj13)*(x1729))));
7581 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1732)))+(((x1730)*(x1731)))+(x1738)+(((IkReal(-1.00000000000000))*(sj14)*(x1725)*(x1727)))+(((IkReal(-1.00000000000000))*(sj14)*(x1725)*(x1726)))+(((IkReal(-1.00000000000000))*(x1729)*(x1732)))+(((IkReal(-1.00000000000000))*(x1737)))+(((sj13)*(x1739))));
7582 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
7583 {
7584 continue;
7585 }
7586 }
7587 
7588 {
7589 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7590 vinfos[0].jointtype = 1;
7591 vinfos[0].foffset = j9;
7592 vinfos[0].indices[0] = _ij9[0];
7593 vinfos[0].indices[1] = _ij9[1];
7594 vinfos[0].maxsolutions = _nj9;
7595 vinfos[1].jointtype = 1;
7596 vinfos[1].foffset = j10;
7597 vinfos[1].indices[0] = _ij10[0];
7598 vinfos[1].indices[1] = _ij10[1];
7599 vinfos[1].maxsolutions = _nj10;
7600 vinfos[2].jointtype = 1;
7601 vinfos[2].foffset = j11;
7602 vinfos[2].indices[0] = _ij11[0];
7603 vinfos[2].indices[1] = _ij11[1];
7604 vinfos[2].maxsolutions = _nj11;
7605 vinfos[3].jointtype = 1;
7606 vinfos[3].foffset = j12;
7607 vinfos[3].indices[0] = _ij12[0];
7608 vinfos[3].indices[1] = _ij12[1];
7609 vinfos[3].maxsolutions = _nj12;
7610 vinfos[4].jointtype = 1;
7611 vinfos[4].foffset = j13;
7612 vinfos[4].indices[0] = _ij13[0];
7613 vinfos[4].indices[1] = _ij13[1];
7614 vinfos[4].maxsolutions = _nj13;
7615 vinfos[5].jointtype = 1;
7616 vinfos[5].foffset = j14;
7617 vinfos[5].indices[0] = _ij14[0];
7618 vinfos[5].indices[1] = _ij14[1];
7619 vinfos[5].maxsolutions = _nj14;
7620 std::vector<int> vfree(0);
7621 solutions.AddSolution(vinfos,vfree);
7622 }
7623 }
7624 }
7625 
7626 }
7627 
7628 }
7629 
7630 } else
7631 {
7632 IkReal x1741=((cj9)*(r10));
7633 IkReal x1742=((cj13)*(cj14));
7634 IkReal x1743=((sj14)*(sj9));
7635 IkReal x1744=((IkReal(1.00000000000000))*(sj14));
7636 IkReal x1745=((cj9)*(sj13));
7637 IkReal x1746=((r02)*(sj9));
7638 IkReal x1747=((cj13)*(cj9));
7639 IkReal x1748=((cj14)*(r01));
7640 IkReal x1749=((IkReal(1.00000000000000))*(npx));
7641 IkReal x1750=((cj14)*(sj13));
7642 IkReal x1751=((IkReal(1.00000000000000))*(cj9));
7643 IkReal x1752=((npy)*(sj14));
7644 IkReal x1753=((IkReal(1.00000000000000))*(sj13));
7645 IkReal x1754=((IkReal(1.00000000000000))*(cj14)*(sj9));
7646 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
7647 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
7648 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
7649 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x1753)))+(((IkReal(0.250000000000000))*(cj11)))+(((cj13)*(x1752)))+(((IkReal(-1.00000000000000))*(x1742)*(x1749))));
7650 evalcond[4]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x1741)*(x1744)))+(((sj9)*(x1748)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1751)))+(((r00)*(x1743))));
7651 evalcond[5]=((IkReal(-0.0300000000000000))+(((IkReal(-1.00000000000000))*(x1749)*(x1750)))+(((cj13)*(npz)))+(((sj13)*(x1752)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(0.250000000000000))*(sj11))));
7652 evalcond[6]=((((IkReal(-1.00000000000000))*(r11)*(x1754)))+(((IkReal(-1.00000000000000))*(r10)*(x1743)))+(((IkReal(-1.00000000000000))*(x1748)*(x1751)))+(((IkReal(-1.00000000000000))*(cj9)*(r00)*(x1744))));
7653 evalcond[7]=((((r12)*(x1745)))+(((cj13)*(r01)*(x1743)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x1742)))+(((IkReal(-1.00000000000000))*(x1746)*(x1753)))+(((IkReal(-1.00000000000000))*(r11)*(x1744)*(x1747)))+(((x1741)*(x1742))));
7654 evalcond[8]=((((IkReal(-1.00000000000000))*(r00)*(sj9)*(x1750)))+(((IkReal(-1.00000000000000))*(r12)*(x1747)))+(((cj13)*(x1746)))+(((r01)*(sj13)*(x1743)))+(((IkReal(-1.00000000000000))*(r11)*(x1744)*(x1745)))+(((x1741)*(x1750))));
7655 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 )
7656 {
7657 {
7658 IkReal dummyeval[1];
7659 IkReal gconst14;
7660 gconst14=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
7661 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
7662 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
7663 {
7664 {
7665 IkReal dummyeval[1];
7666 IkReal gconst15;
7667 gconst15=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
7668 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
7669 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
7670 {
7671 continue;
7672 
7673 } else
7674 {
7675 {
7676 IkReal j10array[1], cj10array[1], sj10array[1];
7677 bool j10valid[1]={false};
7678 _nj10 = 1;
7679 IkReal x1755=((cj13)*(sj14));
7680 IkReal x1756=((IkReal(1.00000000000000))*(cj11));
7681 IkReal x1757=((r11)*(sj9));
7682 IkReal x1758=((IkReal(1.00000000000000))*(sj11));
7683 IkReal x1759=((cj13)*(cj14));
7684 IkReal x1760=((cj11)*(sj13));
7685 IkReal x1761=((r12)*(sj9));
7686 IkReal x1762=((cj9)*(r01));
7687 IkReal x1763=((sj11)*(sj13));
7688 IkReal x1764=((cj9)*(r02));
7689 IkReal x1765=((r10)*(sj9));
7690 IkReal x1766=((cj9)*(r00));
7691 if( IKabs(((gconst15)*(((((IkReal(-1.00000000000000))*(x1755)*(x1756)*(x1757)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1758)))+(((r21)*(sj11)*(x1755)))+(((IkReal(-1.00000000000000))*(r20)*(x1758)*(x1759)))+(((IkReal(-1.00000000000000))*(x1755)*(x1756)*(x1762)))+(((cj11)*(x1759)*(x1765)))+(((cj11)*(x1759)*(x1766)))+(((x1760)*(x1761)))+(((x1760)*(x1764))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst15)*(((((cj11)*(r20)*(x1759)))+(((IkReal(-1.00000000000000))*(x1755)*(x1758)*(x1762)))+(((IkReal(-1.00000000000000))*(r21)*(x1755)*(x1756)))+(((r22)*(x1760)))+(((IkReal(-1.00000000000000))*(x1755)*(x1757)*(x1758)))+(((x1761)*(x1763)))+(((sj11)*(x1759)*(x1766)))+(((sj11)*(x1759)*(x1765)))+(((x1763)*(x1764))))))) < IKFAST_ATAN2_MAGTHRESH )
7692  continue;
7693 j10array[0]=IKatan2(((gconst15)*(((((IkReal(-1.00000000000000))*(x1755)*(x1756)*(x1757)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x1758)))+(((r21)*(sj11)*(x1755)))+(((IkReal(-1.00000000000000))*(r20)*(x1758)*(x1759)))+(((IkReal(-1.00000000000000))*(x1755)*(x1756)*(x1762)))+(((cj11)*(x1759)*(x1765)))+(((cj11)*(x1759)*(x1766)))+(((x1760)*(x1761)))+(((x1760)*(x1764)))))), ((gconst15)*(((((cj11)*(r20)*(x1759)))+(((IkReal(-1.00000000000000))*(x1755)*(x1758)*(x1762)))+(((IkReal(-1.00000000000000))*(r21)*(x1755)*(x1756)))+(((r22)*(x1760)))+(((IkReal(-1.00000000000000))*(x1755)*(x1757)*(x1758)))+(((x1761)*(x1763)))+(((sj11)*(x1759)*(x1766)))+(((sj11)*(x1759)*(x1765)))+(((x1763)*(x1764)))))));
7694 sj10array[0]=IKsin(j10array[0]);
7695 cj10array[0]=IKcos(j10array[0]);
7696 if( j10array[0] > IKPI )
7697 {
7698  j10array[0]-=IK2PI;
7699 }
7700 else if( j10array[0] < -IKPI )
7701 { j10array[0]+=IK2PI;
7702 }
7703 j10valid[0] = true;
7704 for(int ij10 = 0; ij10 < 1; ++ij10)
7705 {
7706 if( !j10valid[ij10] )
7707 {
7708  continue;
7709 }
7710 _ij10[0] = ij10; _ij10[1] = -1;
7711 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
7712 {
7713 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
7714 {
7715  j10valid[iij10]=false; _ij10[1] = iij10; break;
7716 }
7717 }
7718 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
7719 {
7720 IkReal evalcond[4];
7721 IkReal x1767=IKsin(j10);
7722 IkReal x1768=IKcos(j10);
7723 IkReal x1769=((IkReal(1.00000000000000))*(sj13));
7724 IkReal x1770=((r11)*(sj9));
7725 IkReal x1771=((cj9)*(r01));
7726 IkReal x1772=((IkReal(1.00000000000000))*(cj11));
7727 IkReal x1773=((r21)*(sj14));
7728 IkReal x1774=((cj9)*(r02));
7729 IkReal x1775=((sj13)*(sj9));
7730 IkReal x1776=((cj14)*(r10));
7731 IkReal x1777=((IkReal(1.00000000000000))*(cj13));
7732 IkReal x1778=((cj14)*(r20));
7733 IkReal x1779=((sj11)*(x1767));
7734 IkReal x1780=((sj14)*(x1777));
7735 IkReal x1781=((sj11)*(x1768));
7736 IkReal x1782=((cj14)*(cj9)*(r00));
7737 IkReal x1783=((x1768)*(x1772));
7738 evalcond[0]=((((IkReal(-1.00000000000000))*(x1783)))+(x1779)+(((IkReal(-1.00000000000000))*(r22)*(x1769)))+(((IkReal(-1.00000000000000))*(x1777)*(x1778)))+(((cj13)*(x1773))));
7739 evalcond[1]=((((IkReal(-1.00000000000000))*(x1767)*(x1772)))+(((sj13)*(x1773)))+(((IkReal(-1.00000000000000))*(x1781)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x1769)*(x1778))));
7740 evalcond[2]=((((IkReal(-1.00000000000000))*(x1770)*(x1780)))+(((r12)*(x1775)))+(x1781)+(((cj11)*(x1767)))+(((sj13)*(x1774)))+(((cj13)*(x1782)))+(((cj13)*(sj9)*(x1776)))+(((IkReal(-1.00000000000000))*(x1771)*(x1780))));
7741 evalcond[3]=((((IkReal(-1.00000000000000))*(x1783)))+(x1779)+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1777)))+(((x1775)*(x1776)))+(((IkReal(-1.00000000000000))*(x1774)*(x1777)))+(((sj13)*(x1782)))+(((IkReal(-1.00000000000000))*(sj14)*(x1769)*(x1771)))+(((IkReal(-1.00000000000000))*(sj14)*(x1769)*(x1770))));
7742 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
7743 {
7744 continue;
7745 }
7746 }
7747 
7748 {
7749 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7750 vinfos[0].jointtype = 1;
7751 vinfos[0].foffset = j9;
7752 vinfos[0].indices[0] = _ij9[0];
7753 vinfos[0].indices[1] = _ij9[1];
7754 vinfos[0].maxsolutions = _nj9;
7755 vinfos[1].jointtype = 1;
7756 vinfos[1].foffset = j10;
7757 vinfos[1].indices[0] = _ij10[0];
7758 vinfos[1].indices[1] = _ij10[1];
7759 vinfos[1].maxsolutions = _nj10;
7760 vinfos[2].jointtype = 1;
7761 vinfos[2].foffset = j11;
7762 vinfos[2].indices[0] = _ij11[0];
7763 vinfos[2].indices[1] = _ij11[1];
7764 vinfos[2].maxsolutions = _nj11;
7765 vinfos[3].jointtype = 1;
7766 vinfos[3].foffset = j12;
7767 vinfos[3].indices[0] = _ij12[0];
7768 vinfos[3].indices[1] = _ij12[1];
7769 vinfos[3].maxsolutions = _nj12;
7770 vinfos[4].jointtype = 1;
7771 vinfos[4].foffset = j13;
7772 vinfos[4].indices[0] = _ij13[0];
7773 vinfos[4].indices[1] = _ij13[1];
7774 vinfos[4].maxsolutions = _nj13;
7775 vinfos[5].jointtype = 1;
7776 vinfos[5].foffset = j14;
7777 vinfos[5].indices[0] = _ij14[0];
7778 vinfos[5].indices[1] = _ij14[1];
7779 vinfos[5].maxsolutions = _nj14;
7780 std::vector<int> vfree(0);
7781 solutions.AddSolution(vinfos,vfree);
7782 }
7783 }
7784 }
7785 
7786 }
7787 
7788 }
7789 
7790 } else
7791 {
7792 {
7793 IkReal j10array[1], cj10array[1], sj10array[1];
7794 bool j10valid[1]={false};
7795 _nj10 = 1;
7796 IkReal x1784=((IkReal(1.00000000000000))*(sj11));
7797 IkReal x1785=((cj11)*(r22));
7798 IkReal x1786=((IkReal(1.00000000000000))*(sj13));
7799 IkReal x1787=((sj11)*(sj13));
7800 IkReal x1788=((r21)*(sj14));
7801 IkReal x1789=((cj14)*(r20));
7802 IkReal x1790=((cj13)*(sj11));
7803 IkReal x1791=((cj13)*(x1788));
7804 if( IKabs(((gconst14)*(((((cj13)*(x1785)))+(((x1789)*(x1790)))+(((r22)*(x1787)))+(((IkReal(-1.00000000000000))*(x1784)*(x1791)))+(((cj11)*(sj13)*(x1788)))+(((IkReal(-1.00000000000000))*(cj11)*(x1786)*(x1789))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst14)*(((((IkReal(-1.00000000000000))*(sj13)*(x1784)*(x1789)))+(((x1787)*(x1788)))+(((IkReal(-1.00000000000000))*(x1785)*(x1786)))+(((r22)*(x1790)))+(((cj11)*(x1791)))+(((IkReal(-1.00000000000000))*(cj11)*(cj13)*(x1789))))))) < IKFAST_ATAN2_MAGTHRESH )
7805  continue;
7806 j10array[0]=IKatan2(((gconst14)*(((((cj13)*(x1785)))+(((x1789)*(x1790)))+(((r22)*(x1787)))+(((IkReal(-1.00000000000000))*(x1784)*(x1791)))+(((cj11)*(sj13)*(x1788)))+(((IkReal(-1.00000000000000))*(cj11)*(x1786)*(x1789)))))), ((gconst14)*(((((IkReal(-1.00000000000000))*(sj13)*(x1784)*(x1789)))+(((x1787)*(x1788)))+(((IkReal(-1.00000000000000))*(x1785)*(x1786)))+(((r22)*(x1790)))+(((cj11)*(x1791)))+(((IkReal(-1.00000000000000))*(cj11)*(cj13)*(x1789)))))));
7807 sj10array[0]=IKsin(j10array[0]);
7808 cj10array[0]=IKcos(j10array[0]);
7809 if( j10array[0] > IKPI )
7810 {
7811  j10array[0]-=IK2PI;
7812 }
7813 else if( j10array[0] < -IKPI )
7814 { j10array[0]+=IK2PI;
7815 }
7816 j10valid[0] = true;
7817 for(int ij10 = 0; ij10 < 1; ++ij10)
7818 {
7819 if( !j10valid[ij10] )
7820 {
7821  continue;
7822 }
7823 _ij10[0] = ij10; _ij10[1] = -1;
7824 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
7825 {
7826 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
7827 {
7828  j10valid[iij10]=false; _ij10[1] = iij10; break;
7829 }
7830 }
7831 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
7832 {
7833 IkReal evalcond[4];
7834 IkReal x1792=IKsin(j10);
7835 IkReal x1793=IKcos(j10);
7836 IkReal x1794=((IkReal(1.00000000000000))*(sj13));
7837 IkReal x1795=((r11)*(sj9));
7838 IkReal x1796=((cj9)*(r01));
7839 IkReal x1797=((IkReal(1.00000000000000))*(cj11));
7840 IkReal x1798=((r21)*(sj14));
7841 IkReal x1799=((cj9)*(r02));
7842 IkReal x1800=((sj13)*(sj9));
7843 IkReal x1801=((cj14)*(r10));
7844 IkReal x1802=((IkReal(1.00000000000000))*(cj13));
7845 IkReal x1803=((cj14)*(r20));
7846 IkReal x1804=((sj11)*(x1792));
7847 IkReal x1805=((sj14)*(x1802));
7848 IkReal x1806=((sj11)*(x1793));
7849 IkReal x1807=((cj14)*(cj9)*(r00));
7850 IkReal x1808=((x1793)*(x1797));
7851 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x1794)))+(((cj13)*(x1798)))+(((IkReal(-1.00000000000000))*(x1802)*(x1803)))+(((IkReal(-1.00000000000000))*(x1808)))+(x1804));
7852 evalcond[1]=((((sj13)*(x1798)))+(((IkReal(-1.00000000000000))*(x1792)*(x1797)))+(((IkReal(-1.00000000000000))*(x1794)*(x1803)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x1806))));
7853 evalcond[2]=((((sj13)*(x1799)))+(((IkReal(-1.00000000000000))*(x1795)*(x1805)))+(((IkReal(-1.00000000000000))*(x1796)*(x1805)))+(((cj13)*(x1807)))+(((r12)*(x1800)))+(x1806)+(((cj11)*(x1792)))+(((cj13)*(sj9)*(x1801))));
7854 evalcond[3]=((((x1800)*(x1801)))+(((IkReal(-1.00000000000000))*(x1799)*(x1802)))+(((IkReal(-1.00000000000000))*(x1808)))+(x1804)+(((IkReal(-1.00000000000000))*(sj14)*(x1794)*(x1795)))+(((IkReal(-1.00000000000000))*(sj14)*(x1794)*(x1796)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x1802)))+(((sj13)*(x1807))));
7855 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
7856 {
7857 continue;
7858 }
7859 }
7860 
7861 {
7862 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7863 vinfos[0].jointtype = 1;
7864 vinfos[0].foffset = j9;
7865 vinfos[0].indices[0] = _ij9[0];
7866 vinfos[0].indices[1] = _ij9[1];
7867 vinfos[0].maxsolutions = _nj9;
7868 vinfos[1].jointtype = 1;
7869 vinfos[1].foffset = j10;
7870 vinfos[1].indices[0] = _ij10[0];
7871 vinfos[1].indices[1] = _ij10[1];
7872 vinfos[1].maxsolutions = _nj10;
7873 vinfos[2].jointtype = 1;
7874 vinfos[2].foffset = j11;
7875 vinfos[2].indices[0] = _ij11[0];
7876 vinfos[2].indices[1] = _ij11[1];
7877 vinfos[2].maxsolutions = _nj11;
7878 vinfos[3].jointtype = 1;
7879 vinfos[3].foffset = j12;
7880 vinfos[3].indices[0] = _ij12[0];
7881 vinfos[3].indices[1] = _ij12[1];
7882 vinfos[3].maxsolutions = _nj12;
7883 vinfos[4].jointtype = 1;
7884 vinfos[4].foffset = j13;
7885 vinfos[4].indices[0] = _ij13[0];
7886 vinfos[4].indices[1] = _ij13[1];
7887 vinfos[4].maxsolutions = _nj13;
7888 vinfos[5].jointtype = 1;
7889 vinfos[5].foffset = j14;
7890 vinfos[5].indices[0] = _ij14[0];
7891 vinfos[5].indices[1] = _ij14[1];
7892 vinfos[5].maxsolutions = _nj14;
7893 std::vector<int> vfree(0);
7894 solutions.AddSolution(vinfos,vfree);
7895 }
7896 }
7897 }
7898 
7899 }
7900 
7901 }
7902 
7903 } else
7904 {
7905 if( 1 )
7906 {
7907 continue;
7908 
7909 } else
7910 {
7911 }
7912 }
7913 }
7914 }
7915 }
7916 }
7917 
7918 } else
7919 {
7920 {
7921 IkReal j10array[1], cj10array[1], sj10array[1];
7922 bool j10valid[1]={false};
7923 _nj10 = 1;
7924 IkReal x1809=((r21)*(sj14));
7925 IkReal x1810=((cj12)*(cj13));
7926 IkReal x1811=((sj11)*(sj13));
7927 IkReal x1812=((cj14)*(r20));
7928 IkReal x1813=((IkReal(1.00000000000000))*(sj11));
7929 IkReal x1814=((cj12)*(r22));
7930 IkReal x1815=((IkReal(1.00000000000000))*(cj11));
7931 IkReal x1816=((cj13)*(r22));
7932 IkReal x1817=((sj13)*(x1815));
7933 if( IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x1809)*(x1817)))+(((sj11)*(x1810)*(x1812)))+(((IkReal(-1.00000000000000))*(x1809)*(x1810)*(x1813)))+(((cj11)*(sj13)*(x1812)))+(((IkReal(-1.00000000000000))*(x1815)*(x1816)))+(((x1811)*(x1814))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst7)*(((((IkReal(-1.00000000000000))*(x1810)*(x1812)*(x1815)))+(((IkReal(-1.00000000000000))*(x1813)*(x1816)))+(((IkReal(-1.00000000000000))*(x1809)*(x1811)))+(((IkReal(-1.00000000000000))*(x1814)*(x1817)))+(((x1811)*(x1812)))+(((cj11)*(x1809)*(x1810))))))) < IKFAST_ATAN2_MAGTHRESH )
7934  continue;
7935 j10array[0]=IKatan2(((gconst7)*(((((IkReal(-1.00000000000000))*(x1809)*(x1817)))+(((sj11)*(x1810)*(x1812)))+(((IkReal(-1.00000000000000))*(x1809)*(x1810)*(x1813)))+(((cj11)*(sj13)*(x1812)))+(((IkReal(-1.00000000000000))*(x1815)*(x1816)))+(((x1811)*(x1814)))))), ((gconst7)*(((((IkReal(-1.00000000000000))*(x1810)*(x1812)*(x1815)))+(((IkReal(-1.00000000000000))*(x1813)*(x1816)))+(((IkReal(-1.00000000000000))*(x1809)*(x1811)))+(((IkReal(-1.00000000000000))*(x1814)*(x1817)))+(((x1811)*(x1812)))+(((cj11)*(x1809)*(x1810)))))));
7936 sj10array[0]=IKsin(j10array[0]);
7937 cj10array[0]=IKcos(j10array[0]);
7938 if( j10array[0] > IKPI )
7939 {
7940  j10array[0]-=IK2PI;
7941 }
7942 else if( j10array[0] < -IKPI )
7943 { j10array[0]+=IK2PI;
7944 }
7945 j10valid[0] = true;
7946 for(int ij10 = 0; ij10 < 1; ++ij10)
7947 {
7948 if( !j10valid[ij10] )
7949 {
7950  continue;
7951 }
7952 _ij10[0] = ij10; _ij10[1] = -1;
7953 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
7954 {
7955 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
7956 {
7957  j10valid[iij10]=false; _ij10[1] = iij10; break;
7958 }
7959 }
7960 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
7961 {
7962 IkReal evalcond[6];
7963 IkReal x1818=IKsin(j10);
7964 IkReal x1819=IKcos(j10);
7965 IkReal x1820=((IkReal(1.00000000000000))*(cj13));
7966 IkReal x1821=((cj9)*(r02));
7967 IkReal x1822=((IkReal(1.00000000000000))*(sj13));
7968 IkReal x1823=((r11)*(sj9));
7969 IkReal x1824=((IkReal(1.00000000000000))*(cj14));
7970 IkReal x1825=((cj9)*(r01));
7971 IkReal x1826=((r21)*(sj14));
7972 IkReal x1827=((IkReal(1.00000000000000))*(sj12));
7973 IkReal x1828=((r10)*(sj9));
7974 IkReal x1829=((cj14)*(sj13));
7975 IkReal x1830=((cj14)*(r20));
7976 IkReal x1831=((IkReal(1.00000000000000))*(sj14));
7977 IkReal x1832=((r12)*(sj9));
7978 IkReal x1833=((cj13)*(cj14));
7979 IkReal x1834=((cj9)*(r00));
7980 IkReal x1835=((sj11)*(x1818));
7981 IkReal x1836=((cj11)*(x1818));
7982 IkReal x1837=((sj11)*(x1819));
7983 IkReal x1838=((cj11)*(x1819));
7984 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x1827)*(x1836)))+(((IkReal(-1.00000000000000))*(x1827)*(x1837))));
7985 evalcond[1]=((((IkReal(-1.00000000000000))*(x1838)))+(((cj13)*(x1826)))+(((IkReal(-1.00000000000000))*(r22)*(x1822)))+(x1835)+(((IkReal(-1.00000000000000))*(x1820)*(x1830))));
7986 evalcond[2]=((((sj13)*(x1826)))+(((cj12)*(x1836)))+(((cj12)*(x1837)))+(((IkReal(-1.00000000000000))*(x1822)*(x1830)))+(((cj13)*(r22))));
7987 evalcond[3]=((((IkReal(-1.00000000000000))*(x1828)*(x1831)))+(((IkReal(-1.00000000000000))*(x1827)*(x1838)))+(((IkReal(-1.00000000000000))*(x1823)*(x1824)))+(((IkReal(-1.00000000000000))*(x1831)*(x1834)))+(((IkReal(-1.00000000000000))*(x1824)*(x1825)))+(((sj12)*(x1835))));
7988 evalcond[4]=((((sj13)*(x1821)))+(x1836)+(x1837)+(((IkReal(-1.00000000000000))*(sj14)*(x1820)*(x1823)))+(((IkReal(-1.00000000000000))*(sj14)*(x1820)*(x1825)))+(((x1833)*(x1834)))+(((x1828)*(x1833)))+(((sj13)*(x1832))));
7989 evalcond[5]=((((x1829)*(x1834)))+(((IkReal(-1.00000000000000))*(sj14)*(x1822)*(x1825)))+(((IkReal(-1.00000000000000))*(sj14)*(x1822)*(x1823)))+(((cj12)*(x1838)))+(((IkReal(-1.00000000000000))*(cj12)*(x1835)))+(((x1828)*(x1829)))+(((IkReal(-1.00000000000000))*(x1820)*(x1832)))+(((IkReal(-1.00000000000000))*(x1820)*(x1821))));
7990 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 )
7991 {
7992 continue;
7993 }
7994 }
7995 
7996 {
7997 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
7998 vinfos[0].jointtype = 1;
7999 vinfos[0].foffset = j9;
8000 vinfos[0].indices[0] = _ij9[0];
8001 vinfos[0].indices[1] = _ij9[1];
8002 vinfos[0].maxsolutions = _nj9;
8003 vinfos[1].jointtype = 1;
8004 vinfos[1].foffset = j10;
8005 vinfos[1].indices[0] = _ij10[0];
8006 vinfos[1].indices[1] = _ij10[1];
8007 vinfos[1].maxsolutions = _nj10;
8008 vinfos[2].jointtype = 1;
8009 vinfos[2].foffset = j11;
8010 vinfos[2].indices[0] = _ij11[0];
8011 vinfos[2].indices[1] = _ij11[1];
8012 vinfos[2].maxsolutions = _nj11;
8013 vinfos[3].jointtype = 1;
8014 vinfos[3].foffset = j12;
8015 vinfos[3].indices[0] = _ij12[0];
8016 vinfos[3].indices[1] = _ij12[1];
8017 vinfos[3].maxsolutions = _nj12;
8018 vinfos[4].jointtype = 1;
8019 vinfos[4].foffset = j13;
8020 vinfos[4].indices[0] = _ij13[0];
8021 vinfos[4].indices[1] = _ij13[1];
8022 vinfos[4].maxsolutions = _nj13;
8023 vinfos[5].jointtype = 1;
8024 vinfos[5].foffset = j14;
8025 vinfos[5].indices[0] = _ij14[0];
8026 vinfos[5].indices[1] = _ij14[1];
8027 vinfos[5].maxsolutions = _nj14;
8028 std::vector<int> vfree(0);
8029 solutions.AddSolution(vinfos,vfree);
8030 }
8031 }
8032 }
8033 
8034 }
8035 
8036 }
8037 
8038 } else
8039 {
8040 {
8041 IkReal j10array[1], cj10array[1], sj10array[1];
8042 bool j10valid[1]={false};
8043 _nj10 = 1;
8044 IkReal x1839=((cj11)*(sj12));
8045 IkReal x1840=((r22)*(sj13));
8046 IkReal x1841=((r20)*(sj14));
8047 IkReal x1842=((cj14)*(sj11));
8048 IkReal x1843=((cj13)*(r20));
8049 IkReal x1844=((sj11)*(sj12));
8050 IkReal x1845=((cj13)*(r21)*(sj14));
8051 if( IKabs(((gconst6)*(((((cj11)*(cj14)*(r21)))+(((sj12)*(x1842)*(x1843)))+(((x1840)*(x1844)))+(((IkReal(-1.00000000000000))*(x1844)*(x1845)))+(((cj11)*(x1841))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((((sj11)*(x1841)))+(((IkReal(-1.00000000000000))*(x1839)*(x1840)))+(((x1839)*(x1845)))+(((r21)*(x1842)))+(((IkReal(-1.00000000000000))*(cj14)*(x1839)*(x1843))))))) < IKFAST_ATAN2_MAGTHRESH )
8052  continue;
8053 j10array[0]=IKatan2(((gconst6)*(((((cj11)*(cj14)*(r21)))+(((sj12)*(x1842)*(x1843)))+(((x1840)*(x1844)))+(((IkReal(-1.00000000000000))*(x1844)*(x1845)))+(((cj11)*(x1841)))))), ((gconst6)*(((((sj11)*(x1841)))+(((IkReal(-1.00000000000000))*(x1839)*(x1840)))+(((x1839)*(x1845)))+(((r21)*(x1842)))+(((IkReal(-1.00000000000000))*(cj14)*(x1839)*(x1843)))))));
8054 sj10array[0]=IKsin(j10array[0]);
8055 cj10array[0]=IKcos(j10array[0]);
8056 if( j10array[0] > IKPI )
8057 {
8058  j10array[0]-=IK2PI;
8059 }
8060 else if( j10array[0] < -IKPI )
8061 { j10array[0]+=IK2PI;
8062 }
8063 j10valid[0] = true;
8064 for(int ij10 = 0; ij10 < 1; ++ij10)
8065 {
8066 if( !j10valid[ij10] )
8067 {
8068  continue;
8069 }
8070 _ij10[0] = ij10; _ij10[1] = -1;
8071 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
8072 {
8073 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
8074 {
8075  j10valid[iij10]=false; _ij10[1] = iij10; break;
8076 }
8077 }
8078 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
8079 {
8080 IkReal evalcond[6];
8081 IkReal x1846=IKsin(j10);
8082 IkReal x1847=IKcos(j10);
8083 IkReal x1848=((IkReal(1.00000000000000))*(cj13));
8084 IkReal x1849=((cj9)*(r02));
8085 IkReal x1850=((IkReal(1.00000000000000))*(sj13));
8086 IkReal x1851=((r11)*(sj9));
8087 IkReal x1852=((IkReal(1.00000000000000))*(cj14));
8088 IkReal x1853=((cj9)*(r01));
8089 IkReal x1854=((r21)*(sj14));
8090 IkReal x1855=((IkReal(1.00000000000000))*(sj12));
8091 IkReal x1856=((r10)*(sj9));
8092 IkReal x1857=((cj14)*(sj13));
8093 IkReal x1858=((cj14)*(r20));
8094 IkReal x1859=((IkReal(1.00000000000000))*(sj14));
8095 IkReal x1860=((r12)*(sj9));
8096 IkReal x1861=((cj13)*(cj14));
8097 IkReal x1862=((cj9)*(r00));
8098 IkReal x1863=((sj11)*(x1846));
8099 IkReal x1864=((cj11)*(x1846));
8100 IkReal x1865=((sj11)*(x1847));
8101 IkReal x1866=((cj11)*(x1847));
8102 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x1855)*(x1864)))+(((IkReal(-1.00000000000000))*(x1855)*(x1865)))+(((r20)*(sj14))));
8103 evalcond[1]=((((IkReal(-1.00000000000000))*(x1848)*(x1858)))+(((cj13)*(x1854)))+(((IkReal(-1.00000000000000))*(r22)*(x1850)))+(x1863)+(((IkReal(-1.00000000000000))*(x1866))));
8104 evalcond[2]=((((sj13)*(x1854)))+(((IkReal(-1.00000000000000))*(x1850)*(x1858)))+(((cj13)*(r22)))+(((cj12)*(x1864)))+(((cj12)*(x1865))));
8105 evalcond[3]=((((sj12)*(x1863)))+(((IkReal(-1.00000000000000))*(x1855)*(x1866)))+(((IkReal(-1.00000000000000))*(x1852)*(x1853)))+(((IkReal(-1.00000000000000))*(x1851)*(x1852)))+(((IkReal(-1.00000000000000))*(x1856)*(x1859)))+(((IkReal(-1.00000000000000))*(x1859)*(x1862))));
8106 evalcond[4]=((((sj13)*(x1860)))+(((IkReal(-1.00000000000000))*(sj14)*(x1848)*(x1853)))+(((IkReal(-1.00000000000000))*(sj14)*(x1848)*(x1851)))+(((x1856)*(x1861)))+(x1865)+(x1864)+(((sj13)*(x1849)))+(((x1861)*(x1862))));
8107 evalcond[5]=((((x1856)*(x1857)))+(((IkReal(-1.00000000000000))*(x1848)*(x1860)))+(((IkReal(-1.00000000000000))*(sj14)*(x1850)*(x1851)))+(((IkReal(-1.00000000000000))*(sj14)*(x1850)*(x1853)))+(((IkReal(-1.00000000000000))*(x1848)*(x1849)))+(((x1857)*(x1862)))+(((IkReal(-1.00000000000000))*(cj12)*(x1863)))+(((cj12)*(x1866))));
8108 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 )
8109 {
8110 continue;
8111 }
8112 }
8113 
8114 {
8115 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8116 vinfos[0].jointtype = 1;
8117 vinfos[0].foffset = j9;
8118 vinfos[0].indices[0] = _ij9[0];
8119 vinfos[0].indices[1] = _ij9[1];
8120 vinfos[0].maxsolutions = _nj9;
8121 vinfos[1].jointtype = 1;
8122 vinfos[1].foffset = j10;
8123 vinfos[1].indices[0] = _ij10[0];
8124 vinfos[1].indices[1] = _ij10[1];
8125 vinfos[1].maxsolutions = _nj10;
8126 vinfos[2].jointtype = 1;
8127 vinfos[2].foffset = j11;
8128 vinfos[2].indices[0] = _ij11[0];
8129 vinfos[2].indices[1] = _ij11[1];
8130 vinfos[2].maxsolutions = _nj11;
8131 vinfos[3].jointtype = 1;
8132 vinfos[3].foffset = j12;
8133 vinfos[3].indices[0] = _ij12[0];
8134 vinfos[3].indices[1] = _ij12[1];
8135 vinfos[3].maxsolutions = _nj12;
8136 vinfos[4].jointtype = 1;
8137 vinfos[4].foffset = j13;
8138 vinfos[4].indices[0] = _ij13[0];
8139 vinfos[4].indices[1] = _ij13[1];
8140 vinfos[4].maxsolutions = _nj13;
8141 vinfos[5].jointtype = 1;
8142 vinfos[5].foffset = j14;
8143 vinfos[5].indices[0] = _ij14[0];
8144 vinfos[5].indices[1] = _ij14[1];
8145 vinfos[5].maxsolutions = _nj14;
8146 std::vector<int> vfree(0);
8147 solutions.AddSolution(vinfos,vfree);
8148 }
8149 }
8150 }
8151 
8152 }
8153 
8154 }
8155 }
8156 }
8157 
8158 }
8159 
8160 }
8161 
8162 } else
8163 {
8164 {
8165 IkReal j10array[1], cj10array[1], sj10array[1];
8166 bool j10valid[1]={false};
8167 _nj10 = 1;
8168 IkReal x1867=((sj11)*(sj13));
8169 IkReal x1868=((cj14)*(r20));
8170 IkReal x1869=((IkReal(1.00000000000000))*(sj11));
8171 IkReal x1870=((cj12)*(r22));
8172 IkReal x1871=((r21)*(sj14));
8173 IkReal x1872=((cj12)*(cj13));
8174 IkReal x1873=((IkReal(1.00000000000000))*(cj11));
8175 IkReal x1874=((cj13)*(r22));
8176 IkReal x1875=((sj13)*(x1873));
8177 IkReal x1876=((x1871)*(x1872));
8178 if( IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(x1873)*(x1874)))+(((IkReal(-1.00000000000000))*(x1869)*(x1876)))+(((IkReal(-1.00000000000000))*(x1871)*(x1875)))+(((cj11)*(sj13)*(x1868)))+(((sj11)*(x1868)*(x1872)))+(((x1867)*(x1870))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((((x1867)*(x1868)))+(((IkReal(-1.00000000000000))*(x1869)*(x1874)))+(((IkReal(-1.00000000000000))*(x1870)*(x1875)))+(((cj11)*(x1876)))+(((IkReal(-1.00000000000000))*(x1867)*(x1871)))+(((IkReal(-1.00000000000000))*(x1868)*(x1872)*(x1873))))))) < IKFAST_ATAN2_MAGTHRESH )
8179  continue;
8180 j10array[0]=IKatan2(((gconst5)*(((((IkReal(-1.00000000000000))*(x1873)*(x1874)))+(((IkReal(-1.00000000000000))*(x1869)*(x1876)))+(((IkReal(-1.00000000000000))*(x1871)*(x1875)))+(((cj11)*(sj13)*(x1868)))+(((sj11)*(x1868)*(x1872)))+(((x1867)*(x1870)))))), ((gconst5)*(((((x1867)*(x1868)))+(((IkReal(-1.00000000000000))*(x1869)*(x1874)))+(((IkReal(-1.00000000000000))*(x1870)*(x1875)))+(((cj11)*(x1876)))+(((IkReal(-1.00000000000000))*(x1867)*(x1871)))+(((IkReal(-1.00000000000000))*(x1868)*(x1872)*(x1873)))))));
8181 sj10array[0]=IKsin(j10array[0]);
8182 cj10array[0]=IKcos(j10array[0]);
8183 if( j10array[0] > IKPI )
8184 {
8185  j10array[0]-=IK2PI;
8186 }
8187 else if( j10array[0] < -IKPI )
8188 { j10array[0]+=IK2PI;
8189 }
8190 j10valid[0] = true;
8191 for(int ij10 = 0; ij10 < 1; ++ij10)
8192 {
8193 if( !j10valid[ij10] )
8194 {
8195  continue;
8196 }
8197 _ij10[0] = ij10; _ij10[1] = -1;
8198 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
8199 {
8200 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
8201 {
8202  j10valid[iij10]=false; _ij10[1] = iij10; break;
8203 }
8204 }
8205 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
8206 {
8207 IkReal evalcond[3];
8208 IkReal x1877=IKsin(j10);
8209 IkReal x1878=IKcos(j10);
8210 IkReal x1879=((IkReal(1.00000000000000))*(sj13));
8211 IkReal x1880=((cj14)*(r20));
8212 IkReal x1881=((r21)*(sj14));
8213 IkReal x1882=((IkReal(1.00000000000000))*(x1878));
8214 IkReal x1883=((cj11)*(x1877));
8215 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj12)*(x1883)))+(((IkReal(-1.00000000000000))*(sj11)*(sj12)*(x1882))));
8216 evalcond[1]=((((IkReal(-1.00000000000000))*(cj13)*(x1880)))+(((sj11)*(x1877)))+(((IkReal(-1.00000000000000))*(r22)*(x1879)))+(((cj13)*(x1881)))+(((IkReal(-1.00000000000000))*(cj11)*(x1882))));
8217 evalcond[2]=((((IkReal(-1.00000000000000))*(x1879)*(x1880)))+(((cj12)*(x1883)))+(((sj13)*(x1881)))+(((cj12)*(sj11)*(x1878)))+(((cj13)*(r22))));
8218 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
8219 {
8220 continue;
8221 }
8222 }
8223 
8224 {
8225 IkReal dummyeval[1];
8226 IkReal gconst16;
8227 IkReal x1884=(cj14)*(cj14);
8228 IkReal x1885=(sj14)*(sj14);
8229 IkReal x1886=((IkReal(1.00000000000000))*(r01));
8230 IkReal x1887=((sj13)*(sj14));
8231 IkReal x1888=((cj14)*(sj13));
8232 IkReal x1889=((r00)*(r11));
8233 IkReal x1890=((cj13)*(x1884));
8234 IkReal x1891=((cj13)*(x1885));
8235 gconst16=IKsign(((((IkReal(-1.00000000000000))*(r00)*(r12)*(x1887)))+(((r02)*(r10)*(x1887)))+(((x1889)*(x1891)))+(((x1889)*(x1890)))+(((r02)*(r11)*(x1888)))+(((IkReal(-1.00000000000000))*(r12)*(x1886)*(x1888)))+(((IkReal(-1.00000000000000))*(r10)*(x1886)*(x1890)))+(((IkReal(-1.00000000000000))*(r10)*(x1886)*(x1891)))));
8236 IkReal x1892=(cj14)*(cj14);
8237 IkReal x1893=(sj14)*(sj14);
8238 IkReal x1894=((IkReal(1.00000000000000))*(r01));
8239 IkReal x1895=((sj13)*(sj14));
8240 IkReal x1896=((cj14)*(sj13));
8241 IkReal x1897=((r00)*(r11));
8242 IkReal x1898=((cj13)*(x1892));
8243 IkReal x1899=((cj13)*(x1893));
8244 dummyeval[0]=((((x1897)*(x1899)))+(((x1897)*(x1898)))+(((IkReal(-1.00000000000000))*(r10)*(x1894)*(x1899)))+(((IkReal(-1.00000000000000))*(r10)*(x1894)*(x1898)))+(((IkReal(-1.00000000000000))*(r12)*(x1894)*(x1896)))+(((r02)*(r11)*(x1896)))+(((r02)*(r10)*(x1895)))+(((IkReal(-1.00000000000000))*(r00)*(r12)*(x1895))));
8245 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
8246 {
8247 {
8248 IkReal dummyeval[1];
8249 IkReal gconst17;
8250 IkReal x1900=(sj14)*(sj14);
8251 IkReal x1901=(cj14)*(cj14);
8252 IkReal x1902=((cj13)*(r12));
8253 IkReal x1903=((IkReal(1.00000000000000))*(r10));
8254 IkReal x1904=((cj13)*(r02));
8255 IkReal x1905=((r01)*(sj13));
8256 IkReal x1906=((r00)*(r11)*(sj13));
8257 gconst17=IKsign(((((x1901)*(x1906)))+(((IkReal(-1.00000000000000))*(sj14)*(x1903)*(x1904)))+(((IkReal(-1.00000000000000))*(x1901)*(x1903)*(x1905)))+(((r00)*(sj14)*(x1902)))+(((IkReal(-1.00000000000000))*(x1900)*(x1903)*(x1905)))+(((x1900)*(x1906)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1904)))+(((cj14)*(r01)*(x1902)))));
8258 IkReal x1907=(sj14)*(sj14);
8259 IkReal x1908=(cj14)*(cj14);
8260 IkReal x1909=((cj13)*(r12));
8261 IkReal x1910=((IkReal(1.00000000000000))*(r10));
8262 IkReal x1911=((cj13)*(r02));
8263 IkReal x1912=((r01)*(sj13));
8264 IkReal x1913=((r00)*(r11)*(sj13));
8265 dummyeval[0]=((((x1907)*(x1913)))+(((IkReal(-1.00000000000000))*(x1907)*(x1910)*(x1912)))+(((r00)*(sj14)*(x1909)))+(((IkReal(-1.00000000000000))*(x1908)*(x1910)*(x1912)))+(((x1908)*(x1913)))+(((IkReal(-1.00000000000000))*(sj14)*(x1910)*(x1911)))+(((cj14)*(r01)*(x1909)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1911))));
8266 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
8267 {
8268 continue;
8269 
8270 } else
8271 {
8272 {
8273 IkReal j9array[1], cj9array[1], sj9array[1];
8274 bool j9valid[1]={false};
8275 _nj9 = 1;
8276 IkReal x1914=((sj12)*(sj14));
8277 IkReal x1915=((cj14)*(sj12));
8278 IkReal x1916=((cj12)*(cj14)*(sj13));
8279 IkReal x1917=((IkReal(1.00000000000000))*(cj12)*(cj13));
8280 IkReal x1918=((IkReal(1.00000000000000))*(cj12)*(sj13)*(sj14));
8281 if( IKabs(((gconst17)*(((((r10)*(x1914)))+(((r10)*(x1916)))+(((IkReal(-1.00000000000000))*(r12)*(x1917)))+(((IkReal(-1.00000000000000))*(r11)*(x1918)))+(((r11)*(x1915))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst17)*(((((IkReal(-1.00000000000000))*(r01)*(x1918)))+(((IkReal(-1.00000000000000))*(r02)*(x1917)))+(((r00)*(x1916)))+(((r00)*(x1914)))+(((r01)*(x1915))))))) < IKFAST_ATAN2_MAGTHRESH )
8282  continue;
8283 j9array[0]=IKatan2(((gconst17)*(((((r10)*(x1914)))+(((r10)*(x1916)))+(((IkReal(-1.00000000000000))*(r12)*(x1917)))+(((IkReal(-1.00000000000000))*(r11)*(x1918)))+(((r11)*(x1915)))))), ((gconst17)*(((((IkReal(-1.00000000000000))*(r01)*(x1918)))+(((IkReal(-1.00000000000000))*(r02)*(x1917)))+(((r00)*(x1916)))+(((r00)*(x1914)))+(((r01)*(x1915)))))));
8284 sj9array[0]=IKsin(j9array[0]);
8285 cj9array[0]=IKcos(j9array[0]);
8286 if( j9array[0] > IKPI )
8287 {
8288  j9array[0]-=IK2PI;
8289 }
8290 else if( j9array[0] < -IKPI )
8291 { j9array[0]+=IK2PI;
8292 }
8293 j9valid[0] = true;
8294 for(int ij9 = 0; ij9 < 1; ++ij9)
8295 {
8296 if( !j9valid[ij9] )
8297 {
8298  continue;
8299 }
8300 _ij9[0] = ij9; _ij9[1] = -1;
8301 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
8302 {
8303 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
8304 {
8305  j9valid[iij9]=false; _ij9[1] = iij9; break;
8306 }
8307 }
8308 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
8309 {
8310 IkReal evalcond[6];
8311 IkReal x1919=IKsin(j9);
8312 IkReal x1920=IKcos(j9);
8313 IkReal x1921=((cj10)*(cj11));
8314 IkReal x1922=((IkReal(1.00000000000000))*(cj14));
8315 IkReal x1923=((IkReal(1.00000000000000))*(sj14));
8316 IkReal x1924=((cj13)*(cj14));
8317 IkReal x1925=((cj14)*(r10));
8318 IkReal x1926=((r01)*(sj14));
8319 IkReal x1927=((sj10)*(sj11));
8320 IkReal x1928=((IkReal(1.00000000000000))*(r12));
8321 IkReal x1929=((sj13)*(x1920));
8322 IkReal x1930=((r02)*(x1919));
8323 IkReal x1931=((r11)*(x1919));
8324 IkReal x1932=((r10)*(x1920));
8325 IkReal x1933=((r01)*(x1920));
8326 IkReal x1934=((sj13)*(x1919));
8327 IkReal x1935=((r11)*(x1920));
8328 IkReal x1936=((cj13)*(x1919));
8329 IkReal x1937=((r10)*(x1919));
8330 IkReal x1938=((r00)*(x1920));
8331 IkReal x1939=((cj13)*(x1920));
8332 evalcond[0]=((((r00)*(sj14)*(x1919)))+(((IkReal(-1.00000000000000))*(x1923)*(x1932)))+(cj12)+(((cj14)*(r01)*(x1919)))+(((IkReal(-1.00000000000000))*(x1922)*(x1935))));
8333 evalcond[1]=((((IkReal(-1.00000000000000))*(sj12)*(x1921)))+(((IkReal(-1.00000000000000))*(x1923)*(x1938)))+(((IkReal(-1.00000000000000))*(x1923)*(x1937)))+(((IkReal(-1.00000000000000))*(x1922)*(x1931)))+(((IkReal(-1.00000000000000))*(x1922)*(x1933)))+(((sj12)*(x1927))));
8334 evalcond[2]=((((IkReal(-1.00000000000000))*(sj13)*(x1930)))+(((IkReal(-1.00000000000000))*(r00)*(x1922)*(x1936)))+(((x1924)*(x1932)))+(((x1926)*(x1936)))+(((IkReal(-1.00000000000000))*(cj13)*(x1923)*(x1935)))+(((r12)*(x1929))));
8335 evalcond[3]=((((IkReal(-1.00000000000000))*(x1928)*(x1939)))+(((IkReal(-1.00000000000000))*(r00)*(x1922)*(x1934)))+(((cj13)*(x1930)))+(((IkReal(-1.00000000000000))*(r11)*(x1923)*(x1929)))+(((x1926)*(x1934)))+(sj12)+(((x1925)*(x1929))));
8336 evalcond[4]=((((cj10)*(sj11)))+(((x1924)*(x1938)))+(((x1924)*(x1937)))+(((r12)*(x1934)))+(((r02)*(x1929)))+(((IkReal(-1.00000000000000))*(cj13)*(x1923)*(x1933)))+(((IkReal(-1.00000000000000))*(cj13)*(x1923)*(x1931)))+(((cj11)*(sj10))));
8337 evalcond[5]=((((IkReal(-1.00000000000000))*(x1928)*(x1936)))+(((IkReal(-1.00000000000000))*(sj13)*(x1923)*(x1931)))+(((IkReal(-1.00000000000000))*(r01)*(x1923)*(x1929)))+(((x1925)*(x1934)))+(((cj12)*(x1921)))+(((IkReal(-1.00000000000000))*(cj12)*(x1927)))+(((cj14)*(r00)*(x1929)))+(((IkReal(-1.00000000000000))*(r02)*(x1939))));
8338 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 )
8339 {
8340 continue;
8341 }
8342 }
8343 
8344 {
8345 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8346 vinfos[0].jointtype = 1;
8347 vinfos[0].foffset = j9;
8348 vinfos[0].indices[0] = _ij9[0];
8349 vinfos[0].indices[1] = _ij9[1];
8350 vinfos[0].maxsolutions = _nj9;
8351 vinfos[1].jointtype = 1;
8352 vinfos[1].foffset = j10;
8353 vinfos[1].indices[0] = _ij10[0];
8354 vinfos[1].indices[1] = _ij10[1];
8355 vinfos[1].maxsolutions = _nj10;
8356 vinfos[2].jointtype = 1;
8357 vinfos[2].foffset = j11;
8358 vinfos[2].indices[0] = _ij11[0];
8359 vinfos[2].indices[1] = _ij11[1];
8360 vinfos[2].maxsolutions = _nj11;
8361 vinfos[3].jointtype = 1;
8362 vinfos[3].foffset = j12;
8363 vinfos[3].indices[0] = _ij12[0];
8364 vinfos[3].indices[1] = _ij12[1];
8365 vinfos[3].maxsolutions = _nj12;
8366 vinfos[4].jointtype = 1;
8367 vinfos[4].foffset = j13;
8368 vinfos[4].indices[0] = _ij13[0];
8369 vinfos[4].indices[1] = _ij13[1];
8370 vinfos[4].maxsolutions = _nj13;
8371 vinfos[5].jointtype = 1;
8372 vinfos[5].foffset = j14;
8373 vinfos[5].indices[0] = _ij14[0];
8374 vinfos[5].indices[1] = _ij14[1];
8375 vinfos[5].maxsolutions = _nj14;
8376 std::vector<int> vfree(0);
8377 solutions.AddSolution(vinfos,vfree);
8378 }
8379 }
8380 }
8381 
8382 }
8383 
8384 }
8385 
8386 } else
8387 {
8388 {
8389 IkReal j9array[1], cj9array[1], sj9array[1];
8390 bool j9valid[1]={false};
8391 _nj9 = 1;
8392 IkReal x1940=((cj12)*(sj13));
8393 IkReal x1941=((cj12)*(cj13));
8394 IkReal x1942=((IkReal(1.00000000000000))*(sj14));
8395 if( IKabs(((gconst16)*(((((r12)*(x1940)))+(((cj14)*(r10)*(x1941)))+(((IkReal(-1.00000000000000))*(r11)*(x1941)*(x1942))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(r01)*(x1941)*(x1942)))+(((cj14)*(r00)*(x1941)))+(((r02)*(x1940))))))) < IKFAST_ATAN2_MAGTHRESH )
8396  continue;
8397 j9array[0]=IKatan2(((gconst16)*(((((r12)*(x1940)))+(((cj14)*(r10)*(x1941)))+(((IkReal(-1.00000000000000))*(r11)*(x1941)*(x1942)))))), ((gconst16)*(((((IkReal(-1.00000000000000))*(r01)*(x1941)*(x1942)))+(((cj14)*(r00)*(x1941)))+(((r02)*(x1940)))))));
8398 sj9array[0]=IKsin(j9array[0]);
8399 cj9array[0]=IKcos(j9array[0]);
8400 if( j9array[0] > IKPI )
8401 {
8402  j9array[0]-=IK2PI;
8403 }
8404 else if( j9array[0] < -IKPI )
8405 { j9array[0]+=IK2PI;
8406 }
8407 j9valid[0] = true;
8408 for(int ij9 = 0; ij9 < 1; ++ij9)
8409 {
8410 if( !j9valid[ij9] )
8411 {
8412  continue;
8413 }
8414 _ij9[0] = ij9; _ij9[1] = -1;
8415 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
8416 {
8417 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
8418 {
8419  j9valid[iij9]=false; _ij9[1] = iij9; break;
8420 }
8421 }
8422 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
8423 {
8424 IkReal evalcond[6];
8425 IkReal x1943=IKsin(j9);
8426 IkReal x1944=IKcos(j9);
8427 IkReal x1945=((cj10)*(cj11));
8428 IkReal x1946=((IkReal(1.00000000000000))*(cj14));
8429 IkReal x1947=((IkReal(1.00000000000000))*(sj14));
8430 IkReal x1948=((cj13)*(cj14));
8431 IkReal x1949=((cj14)*(r10));
8432 IkReal x1950=((r01)*(sj14));
8433 IkReal x1951=((sj10)*(sj11));
8434 IkReal x1952=((IkReal(1.00000000000000))*(r12));
8435 IkReal x1953=((sj13)*(x1944));
8436 IkReal x1954=((r02)*(x1943));
8437 IkReal x1955=((r11)*(x1943));
8438 IkReal x1956=((r10)*(x1944));
8439 IkReal x1957=((r01)*(x1944));
8440 IkReal x1958=((sj13)*(x1943));
8441 IkReal x1959=((r11)*(x1944));
8442 IkReal x1960=((cj13)*(x1943));
8443 IkReal x1961=((r10)*(x1943));
8444 IkReal x1962=((r00)*(x1944));
8445 IkReal x1963=((cj13)*(x1944));
8446 evalcond[0]=((((cj14)*(r01)*(x1943)))+(((IkReal(-1.00000000000000))*(x1947)*(x1956)))+(cj12)+(((IkReal(-1.00000000000000))*(x1946)*(x1959)))+(((r00)*(sj14)*(x1943))));
8447 evalcond[1]=((((sj12)*(x1951)))+(((IkReal(-1.00000000000000))*(x1947)*(x1962)))+(((IkReal(-1.00000000000000))*(x1947)*(x1961)))+(((IkReal(-1.00000000000000))*(x1946)*(x1957)))+(((IkReal(-1.00000000000000))*(x1946)*(x1955)))+(((IkReal(-1.00000000000000))*(sj12)*(x1945))));
8448 evalcond[2]=((((r12)*(x1953)))+(((IkReal(-1.00000000000000))*(cj13)*(x1947)*(x1959)))+(((x1950)*(x1960)))+(((x1948)*(x1956)))+(((IkReal(-1.00000000000000))*(sj13)*(x1954)))+(((IkReal(-1.00000000000000))*(r00)*(x1946)*(x1960))));
8449 evalcond[3]=((((IkReal(-1.00000000000000))*(x1952)*(x1963)))+(((IkReal(-1.00000000000000))*(r11)*(x1947)*(x1953)))+(((IkReal(-1.00000000000000))*(r00)*(x1946)*(x1958)))+(((cj13)*(x1954)))+(((x1950)*(x1958)))+(sj12)+(((x1949)*(x1953))));
8450 evalcond[4]=((((r12)*(x1958)))+(((IkReal(-1.00000000000000))*(cj13)*(x1947)*(x1955)))+(((IkReal(-1.00000000000000))*(cj13)*(x1947)*(x1957)))+(((cj10)*(sj11)))+(((r02)*(x1953)))+(((cj11)*(sj10)))+(((x1948)*(x1961)))+(((x1948)*(x1962))));
8451 evalcond[5]=((((IkReal(-1.00000000000000))*(x1952)*(x1960)))+(((IkReal(-1.00000000000000))*(r01)*(x1947)*(x1953)))+(((cj12)*(x1945)))+(((IkReal(-1.00000000000000))*(sj13)*(x1947)*(x1955)))+(((IkReal(-1.00000000000000))*(cj12)*(x1951)))+(((cj14)*(r00)*(x1953)))+(((IkReal(-1.00000000000000))*(r02)*(x1963)))+(((x1949)*(x1958))));
8452 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 )
8453 {
8454 continue;
8455 }
8456 }
8457 
8458 {
8459 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8460 vinfos[0].jointtype = 1;
8461 vinfos[0].foffset = j9;
8462 vinfos[0].indices[0] = _ij9[0];
8463 vinfos[0].indices[1] = _ij9[1];
8464 vinfos[0].maxsolutions = _nj9;
8465 vinfos[1].jointtype = 1;
8466 vinfos[1].foffset = j10;
8467 vinfos[1].indices[0] = _ij10[0];
8468 vinfos[1].indices[1] = _ij10[1];
8469 vinfos[1].maxsolutions = _nj10;
8470 vinfos[2].jointtype = 1;
8471 vinfos[2].foffset = j11;
8472 vinfos[2].indices[0] = _ij11[0];
8473 vinfos[2].indices[1] = _ij11[1];
8474 vinfos[2].maxsolutions = _nj11;
8475 vinfos[3].jointtype = 1;
8476 vinfos[3].foffset = j12;
8477 vinfos[3].indices[0] = _ij12[0];
8478 vinfos[3].indices[1] = _ij12[1];
8479 vinfos[3].maxsolutions = _nj12;
8480 vinfos[4].jointtype = 1;
8481 vinfos[4].foffset = j13;
8482 vinfos[4].indices[0] = _ij13[0];
8483 vinfos[4].indices[1] = _ij13[1];
8484 vinfos[4].maxsolutions = _nj13;
8485 vinfos[5].jointtype = 1;
8486 vinfos[5].foffset = j14;
8487 vinfos[5].indices[0] = _ij14[0];
8488 vinfos[5].indices[1] = _ij14[1];
8489 vinfos[5].maxsolutions = _nj14;
8490 std::vector<int> vfree(0);
8491 solutions.AddSolution(vinfos,vfree);
8492 }
8493 }
8494 }
8495 
8496 }
8497 
8498 }
8499 }
8500 }
8501 
8502 }
8503 
8504 }
8505 
8506 } else
8507 {
8508 {
8509 IkReal j10array[1], cj10array[1], sj10array[1];
8510 bool j10valid[1]={false};
8511 _nj10 = 1;
8512 IkReal x1964=((cj11)*(sj12));
8513 IkReal x1965=((r22)*(sj13));
8514 IkReal x1966=((r20)*(sj14));
8515 IkReal x1967=((cj14)*(sj11));
8516 IkReal x1968=((cj13)*(r20));
8517 IkReal x1969=((sj11)*(sj12));
8518 IkReal x1970=((cj13)*(r21)*(sj14));
8519 if( IKabs(((gconst4)*(((((cj11)*(cj14)*(r21)))+(((sj12)*(x1967)*(x1968)))+(((IkReal(-1.00000000000000))*(x1969)*(x1970)))+(((cj11)*(x1966)))+(((x1965)*(x1969))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((((x1964)*(x1970)))+(((IkReal(-1.00000000000000))*(x1964)*(x1965)))+(((r21)*(x1967)))+(((sj11)*(x1966)))+(((IkReal(-1.00000000000000))*(cj14)*(x1964)*(x1968))))))) < IKFAST_ATAN2_MAGTHRESH )
8520  continue;
8521 j10array[0]=IKatan2(((gconst4)*(((((cj11)*(cj14)*(r21)))+(((sj12)*(x1967)*(x1968)))+(((IkReal(-1.00000000000000))*(x1969)*(x1970)))+(((cj11)*(x1966)))+(((x1965)*(x1969)))))), ((gconst4)*(((((x1964)*(x1970)))+(((IkReal(-1.00000000000000))*(x1964)*(x1965)))+(((r21)*(x1967)))+(((sj11)*(x1966)))+(((IkReal(-1.00000000000000))*(cj14)*(x1964)*(x1968)))))));
8522 sj10array[0]=IKsin(j10array[0]);
8523 cj10array[0]=IKcos(j10array[0]);
8524 if( j10array[0] > IKPI )
8525 {
8526  j10array[0]-=IK2PI;
8527 }
8528 else if( j10array[0] < -IKPI )
8529 { j10array[0]+=IK2PI;
8530 }
8531 j10valid[0] = true;
8532 for(int ij10 = 0; ij10 < 1; ++ij10)
8533 {
8534 if( !j10valid[ij10] )
8535 {
8536  continue;
8537 }
8538 _ij10[0] = ij10; _ij10[1] = -1;
8539 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
8540 {
8541 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
8542 {
8543  j10valid[iij10]=false; _ij10[1] = iij10; break;
8544 }
8545 }
8546 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
8547 {
8548 IkReal evalcond[3];
8549 IkReal x1971=IKsin(j10);
8550 IkReal x1972=IKcos(j10);
8551 IkReal x1973=((IkReal(1.00000000000000))*(sj13));
8552 IkReal x1974=((cj14)*(r20));
8553 IkReal x1975=((r21)*(sj14));
8554 IkReal x1976=((IkReal(1.00000000000000))*(x1972));
8555 IkReal x1977=((cj11)*(x1971));
8556 evalcond[0]=((((IkReal(-1.00000000000000))*(sj11)*(sj12)*(x1976)))+(((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj12)*(x1977))));
8557 evalcond[1]=((((cj13)*(x1975)))+(((IkReal(-1.00000000000000))*(cj11)*(x1976)))+(((IkReal(-1.00000000000000))*(r22)*(x1973)))+(((IkReal(-1.00000000000000))*(cj13)*(x1974)))+(((sj11)*(x1971))));
8558 evalcond[2]=((((cj12)*(sj11)*(x1972)))+(((cj12)*(x1977)))+(((IkReal(-1.00000000000000))*(x1973)*(x1974)))+(((sj13)*(x1975)))+(((cj13)*(r22))));
8559 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
8560 {
8561 continue;
8562 }
8563 }
8564 
8565 {
8566 IkReal dummyeval[1];
8567 IkReal gconst16;
8568 IkReal x1978=(cj14)*(cj14);
8569 IkReal x1979=(sj14)*(sj14);
8570 IkReal x1980=((IkReal(1.00000000000000))*(r01));
8571 IkReal x1981=((sj13)*(sj14));
8572 IkReal x1982=((cj14)*(sj13));
8573 IkReal x1983=((r00)*(r11));
8574 IkReal x1984=((cj13)*(x1978));
8575 IkReal x1985=((cj13)*(x1979));
8576 gconst16=IKsign(((((IkReal(-1.00000000000000))*(r00)*(r12)*(x1981)))+(((IkReal(-1.00000000000000))*(r10)*(x1980)*(x1985)))+(((IkReal(-1.00000000000000))*(r10)*(x1980)*(x1984)))+(((r02)*(r11)*(x1982)))+(((x1983)*(x1984)))+(((x1983)*(x1985)))+(((IkReal(-1.00000000000000))*(r12)*(x1980)*(x1982)))+(((r02)*(r10)*(x1981)))));
8577 IkReal x1986=(cj14)*(cj14);
8578 IkReal x1987=(sj14)*(sj14);
8579 IkReal x1988=((IkReal(1.00000000000000))*(r01));
8580 IkReal x1989=((sj13)*(sj14));
8581 IkReal x1990=((cj14)*(sj13));
8582 IkReal x1991=((r00)*(r11));
8583 IkReal x1992=((cj13)*(x1986));
8584 IkReal x1993=((cj13)*(x1987));
8585 dummyeval[0]=((((IkReal(-1.00000000000000))*(r00)*(r12)*(x1989)))+(((x1991)*(x1993)))+(((x1991)*(x1992)))+(((IkReal(-1.00000000000000))*(r12)*(x1988)*(x1990)))+(((IkReal(-1.00000000000000))*(r10)*(x1988)*(x1992)))+(((IkReal(-1.00000000000000))*(r10)*(x1988)*(x1993)))+(((r02)*(r11)*(x1990)))+(((r02)*(r10)*(x1989))));
8586 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
8587 {
8588 {
8589 IkReal dummyeval[1];
8590 IkReal gconst17;
8591 IkReal x1994=(sj14)*(sj14);
8592 IkReal x1995=(cj14)*(cj14);
8593 IkReal x1996=((cj13)*(r12));
8594 IkReal x1997=((IkReal(1.00000000000000))*(r10));
8595 IkReal x1998=((cj13)*(r02));
8596 IkReal x1999=((r01)*(sj13));
8597 IkReal x2000=((r00)*(r11)*(sj13));
8598 gconst17=IKsign(((((IkReal(-1.00000000000000))*(cj14)*(r11)*(x1998)))+(((cj14)*(r01)*(x1996)))+(((IkReal(-1.00000000000000))*(x1995)*(x1997)*(x1999)))+(((x1994)*(x2000)))+(((x1995)*(x2000)))+(((IkReal(-1.00000000000000))*(sj14)*(x1997)*(x1998)))+(((r00)*(sj14)*(x1996)))+(((IkReal(-1.00000000000000))*(x1994)*(x1997)*(x1999)))));
8599 IkReal x2001=(sj14)*(sj14);
8600 IkReal x2002=(cj14)*(cj14);
8601 IkReal x2003=((cj13)*(r12));
8602 IkReal x2004=((IkReal(1.00000000000000))*(r10));
8603 IkReal x2005=((cj13)*(r02));
8604 IkReal x2006=((r01)*(sj13));
8605 IkReal x2007=((r00)*(r11)*(sj13));
8606 dummyeval[0]=((((x2002)*(x2007)))+(((IkReal(-1.00000000000000))*(x2002)*(x2004)*(x2006)))+(((cj14)*(r01)*(x2003)))+(((IkReal(-1.00000000000000))*(x2001)*(x2004)*(x2006)))+(((IkReal(-1.00000000000000))*(sj14)*(x2004)*(x2005)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2005)))+(((r00)*(sj14)*(x2003)))+(((x2001)*(x2007))));
8607 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
8608 {
8609 continue;
8610 
8611 } else
8612 {
8613 {
8614 IkReal j9array[1], cj9array[1], sj9array[1];
8615 bool j9valid[1]={false};
8616 _nj9 = 1;
8617 IkReal x2008=((sj12)*(sj14));
8618 IkReal x2009=((cj14)*(sj12));
8619 IkReal x2010=((cj12)*(cj14)*(sj13));
8620 IkReal x2011=((IkReal(1.00000000000000))*(cj12)*(cj13));
8621 IkReal x2012=((IkReal(1.00000000000000))*(cj12)*(sj13)*(sj14));
8622 if( IKabs(((gconst17)*(((((r11)*(x2009)))+(((r10)*(x2008)))+(((r10)*(x2010)))+(((IkReal(-1.00000000000000))*(r12)*(x2011)))+(((IkReal(-1.00000000000000))*(r11)*(x2012))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst17)*(((((r01)*(x2009)))+(((r00)*(x2008)))+(((r00)*(x2010)))+(((IkReal(-1.00000000000000))*(r02)*(x2011)))+(((IkReal(-1.00000000000000))*(r01)*(x2012))))))) < IKFAST_ATAN2_MAGTHRESH )
8623  continue;
8624 j9array[0]=IKatan2(((gconst17)*(((((r11)*(x2009)))+(((r10)*(x2008)))+(((r10)*(x2010)))+(((IkReal(-1.00000000000000))*(r12)*(x2011)))+(((IkReal(-1.00000000000000))*(r11)*(x2012)))))), ((gconst17)*(((((r01)*(x2009)))+(((r00)*(x2008)))+(((r00)*(x2010)))+(((IkReal(-1.00000000000000))*(r02)*(x2011)))+(((IkReal(-1.00000000000000))*(r01)*(x2012)))))));
8625 sj9array[0]=IKsin(j9array[0]);
8626 cj9array[0]=IKcos(j9array[0]);
8627 if( j9array[0] > IKPI )
8628 {
8629  j9array[0]-=IK2PI;
8630 }
8631 else if( j9array[0] < -IKPI )
8632 { j9array[0]+=IK2PI;
8633 }
8634 j9valid[0] = true;
8635 for(int ij9 = 0; ij9 < 1; ++ij9)
8636 {
8637 if( !j9valid[ij9] )
8638 {
8639  continue;
8640 }
8641 _ij9[0] = ij9; _ij9[1] = -1;
8642 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
8643 {
8644 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
8645 {
8646  j9valid[iij9]=false; _ij9[1] = iij9; break;
8647 }
8648 }
8649 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
8650 {
8651 IkReal evalcond[6];
8652 IkReal x2013=IKsin(j9);
8653 IkReal x2014=IKcos(j9);
8654 IkReal x2015=((cj10)*(cj11));
8655 IkReal x2016=((IkReal(1.00000000000000))*(cj14));
8656 IkReal x2017=((IkReal(1.00000000000000))*(sj14));
8657 IkReal x2018=((cj13)*(cj14));
8658 IkReal x2019=((cj14)*(r10));
8659 IkReal x2020=((r01)*(sj14));
8660 IkReal x2021=((sj10)*(sj11));
8661 IkReal x2022=((IkReal(1.00000000000000))*(r12));
8662 IkReal x2023=((sj13)*(x2014));
8663 IkReal x2024=((r02)*(x2013));
8664 IkReal x2025=((r11)*(x2013));
8665 IkReal x2026=((r10)*(x2014));
8666 IkReal x2027=((r01)*(x2014));
8667 IkReal x2028=((sj13)*(x2013));
8668 IkReal x2029=((r11)*(x2014));
8669 IkReal x2030=((cj13)*(x2013));
8670 IkReal x2031=((r10)*(x2013));
8671 IkReal x2032=((r00)*(x2014));
8672 IkReal x2033=((cj13)*(x2014));
8673 evalcond[0]=((((r00)*(sj14)*(x2013)))+(((IkReal(-1.00000000000000))*(x2016)*(x2029)))+(((IkReal(-1.00000000000000))*(x2017)*(x2026)))+(cj12)+(((cj14)*(r01)*(x2013))));
8674 evalcond[1]=((((IkReal(-1.00000000000000))*(x2016)*(x2025)))+(((IkReal(-1.00000000000000))*(x2016)*(x2027)))+(((sj12)*(x2021)))+(((IkReal(-1.00000000000000))*(sj12)*(x2015)))+(((IkReal(-1.00000000000000))*(x2017)*(x2032)))+(((IkReal(-1.00000000000000))*(x2017)*(x2031))));
8675 evalcond[2]=((((x2018)*(x2026)))+(((IkReal(-1.00000000000000))*(r00)*(x2016)*(x2030)))+(((IkReal(-1.00000000000000))*(sj13)*(x2024)))+(((x2020)*(x2030)))+(((r12)*(x2023)))+(((IkReal(-1.00000000000000))*(cj13)*(x2017)*(x2029))));
8676 evalcond[3]=((((x2020)*(x2028)))+(((IkReal(-1.00000000000000))*(r11)*(x2017)*(x2023)))+(((cj13)*(x2024)))+(((x2019)*(x2023)))+(((IkReal(-1.00000000000000))*(x2022)*(x2033)))+(sj12)+(((IkReal(-1.00000000000000))*(r00)*(x2016)*(x2028))));
8677 evalcond[4]=((((x2018)*(x2031)))+(((x2018)*(x2032)))+(((cj10)*(sj11)))+(((r02)*(x2023)))+(((cj11)*(sj10)))+(((r12)*(x2028)))+(((IkReal(-1.00000000000000))*(cj13)*(x2017)*(x2025)))+(((IkReal(-1.00000000000000))*(cj13)*(x2017)*(x2027))));
8678 evalcond[5]=((((x2019)*(x2028)))+(((IkReal(-1.00000000000000))*(cj12)*(x2021)))+(((IkReal(-1.00000000000000))*(x2022)*(x2030)))+(((IkReal(-1.00000000000000))*(sj13)*(x2017)*(x2025)))+(((IkReal(-1.00000000000000))*(r01)*(x2017)*(x2023)))+(((cj14)*(r00)*(x2023)))+(((cj12)*(x2015)))+(((IkReal(-1.00000000000000))*(r02)*(x2033))));
8679 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 )
8680 {
8681 continue;
8682 }
8683 }
8684 
8685 {
8686 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8687 vinfos[0].jointtype = 1;
8688 vinfos[0].foffset = j9;
8689 vinfos[0].indices[0] = _ij9[0];
8690 vinfos[0].indices[1] = _ij9[1];
8691 vinfos[0].maxsolutions = _nj9;
8692 vinfos[1].jointtype = 1;
8693 vinfos[1].foffset = j10;
8694 vinfos[1].indices[0] = _ij10[0];
8695 vinfos[1].indices[1] = _ij10[1];
8696 vinfos[1].maxsolutions = _nj10;
8697 vinfos[2].jointtype = 1;
8698 vinfos[2].foffset = j11;
8699 vinfos[2].indices[0] = _ij11[0];
8700 vinfos[2].indices[1] = _ij11[1];
8701 vinfos[2].maxsolutions = _nj11;
8702 vinfos[3].jointtype = 1;
8703 vinfos[3].foffset = j12;
8704 vinfos[3].indices[0] = _ij12[0];
8705 vinfos[3].indices[1] = _ij12[1];
8706 vinfos[3].maxsolutions = _nj12;
8707 vinfos[4].jointtype = 1;
8708 vinfos[4].foffset = j13;
8709 vinfos[4].indices[0] = _ij13[0];
8710 vinfos[4].indices[1] = _ij13[1];
8711 vinfos[4].maxsolutions = _nj13;
8712 vinfos[5].jointtype = 1;
8713 vinfos[5].foffset = j14;
8714 vinfos[5].indices[0] = _ij14[0];
8715 vinfos[5].indices[1] = _ij14[1];
8716 vinfos[5].maxsolutions = _nj14;
8717 std::vector<int> vfree(0);
8718 solutions.AddSolution(vinfos,vfree);
8719 }
8720 }
8721 }
8722 
8723 }
8724 
8725 }
8726 
8727 } else
8728 {
8729 {
8730 IkReal j9array[1], cj9array[1], sj9array[1];
8731 bool j9valid[1]={false};
8732 _nj9 = 1;
8733 IkReal x2034=((cj12)*(sj13));
8734 IkReal x2035=((cj12)*(cj13));
8735 IkReal x2036=((IkReal(1.00000000000000))*(sj14));
8736 if( IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(r11)*(x2035)*(x2036)))+(((r12)*(x2034)))+(((cj14)*(r10)*(x2035))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst16)*(((((IkReal(-1.00000000000000))*(r01)*(x2035)*(x2036)))+(((r02)*(x2034)))+(((cj14)*(r00)*(x2035))))))) < IKFAST_ATAN2_MAGTHRESH )
8737  continue;
8738 j9array[0]=IKatan2(((gconst16)*(((((IkReal(-1.00000000000000))*(r11)*(x2035)*(x2036)))+(((r12)*(x2034)))+(((cj14)*(r10)*(x2035)))))), ((gconst16)*(((((IkReal(-1.00000000000000))*(r01)*(x2035)*(x2036)))+(((r02)*(x2034)))+(((cj14)*(r00)*(x2035)))))));
8739 sj9array[0]=IKsin(j9array[0]);
8740 cj9array[0]=IKcos(j9array[0]);
8741 if( j9array[0] > IKPI )
8742 {
8743  j9array[0]-=IK2PI;
8744 }
8745 else if( j9array[0] < -IKPI )
8746 { j9array[0]+=IK2PI;
8747 }
8748 j9valid[0] = true;
8749 for(int ij9 = 0; ij9 < 1; ++ij9)
8750 {
8751 if( !j9valid[ij9] )
8752 {
8753  continue;
8754 }
8755 _ij9[0] = ij9; _ij9[1] = -1;
8756 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
8757 {
8758 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
8759 {
8760  j9valid[iij9]=false; _ij9[1] = iij9; break;
8761 }
8762 }
8763 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
8764 {
8765 IkReal evalcond[6];
8766 IkReal x2037=IKsin(j9);
8767 IkReal x2038=IKcos(j9);
8768 IkReal x2039=((cj10)*(cj11));
8769 IkReal x2040=((IkReal(1.00000000000000))*(cj14));
8770 IkReal x2041=((IkReal(1.00000000000000))*(sj14));
8771 IkReal x2042=((cj13)*(cj14));
8772 IkReal x2043=((cj14)*(r10));
8773 IkReal x2044=((r01)*(sj14));
8774 IkReal x2045=((sj10)*(sj11));
8775 IkReal x2046=((IkReal(1.00000000000000))*(r12));
8776 IkReal x2047=((sj13)*(x2038));
8777 IkReal x2048=((r02)*(x2037));
8778 IkReal x2049=((r11)*(x2037));
8779 IkReal x2050=((r10)*(x2038));
8780 IkReal x2051=((r01)*(x2038));
8781 IkReal x2052=((sj13)*(x2037));
8782 IkReal x2053=((r11)*(x2038));
8783 IkReal x2054=((cj13)*(x2037));
8784 IkReal x2055=((r10)*(x2037));
8785 IkReal x2056=((r00)*(x2038));
8786 IkReal x2057=((cj13)*(x2038));
8787 evalcond[0]=((((cj14)*(r01)*(x2037)))+(cj12)+(((IkReal(-1.00000000000000))*(x2040)*(x2053)))+(((IkReal(-1.00000000000000))*(x2041)*(x2050)))+(((r00)*(sj14)*(x2037))));
8788 evalcond[1]=((((sj12)*(x2045)))+(((IkReal(-1.00000000000000))*(sj12)*(x2039)))+(((IkReal(-1.00000000000000))*(x2040)*(x2049)))+(((IkReal(-1.00000000000000))*(x2040)*(x2051)))+(((IkReal(-1.00000000000000))*(x2041)*(x2055)))+(((IkReal(-1.00000000000000))*(x2041)*(x2056))));
8789 evalcond[2]=((((IkReal(-1.00000000000000))*(sj13)*(x2048)))+(((x2042)*(x2050)))+(((r12)*(x2047)))+(((IkReal(-1.00000000000000))*(r00)*(x2040)*(x2054)))+(((x2044)*(x2054)))+(((IkReal(-1.00000000000000))*(cj13)*(x2041)*(x2053))));
8790 evalcond[3]=((((x2043)*(x2047)))+(((cj13)*(x2048)))+(((IkReal(-1.00000000000000))*(r00)*(x2040)*(x2052)))+(((x2044)*(x2052)))+(((IkReal(-1.00000000000000))*(x2046)*(x2057)))+(sj12)+(((IkReal(-1.00000000000000))*(r11)*(x2041)*(x2047))));
8791 evalcond[4]=((((r12)*(x2052)))+(((x2042)*(x2055)))+(((x2042)*(x2056)))+(((r02)*(x2047)))+(((cj10)*(sj11)))+(((IkReal(-1.00000000000000))*(cj13)*(x2041)*(x2051)))+(((cj11)*(sj10)))+(((IkReal(-1.00000000000000))*(cj13)*(x2041)*(x2049))));
8792 evalcond[5]=((((IkReal(-1.00000000000000))*(r01)*(x2041)*(x2047)))+(((IkReal(-1.00000000000000))*(r02)*(x2057)))+(((cj12)*(x2039)))+(((x2043)*(x2052)))+(((IkReal(-1.00000000000000))*(sj13)*(x2041)*(x2049)))+(((IkReal(-1.00000000000000))*(x2046)*(x2054)))+(((cj14)*(r00)*(x2047)))+(((IkReal(-1.00000000000000))*(cj12)*(x2045))));
8793 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 )
8794 {
8795 continue;
8796 }
8797 }
8798 
8799 {
8800 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
8801 vinfos[0].jointtype = 1;
8802 vinfos[0].foffset = j9;
8803 vinfos[0].indices[0] = _ij9[0];
8804 vinfos[0].indices[1] = _ij9[1];
8805 vinfos[0].maxsolutions = _nj9;
8806 vinfos[1].jointtype = 1;
8807 vinfos[1].foffset = j10;
8808 vinfos[1].indices[0] = _ij10[0];
8809 vinfos[1].indices[1] = _ij10[1];
8810 vinfos[1].maxsolutions = _nj10;
8811 vinfos[2].jointtype = 1;
8812 vinfos[2].foffset = j11;
8813 vinfos[2].indices[0] = _ij11[0];
8814 vinfos[2].indices[1] = _ij11[1];
8815 vinfos[2].maxsolutions = _nj11;
8816 vinfos[3].jointtype = 1;
8817 vinfos[3].foffset = j12;
8818 vinfos[3].indices[0] = _ij12[0];
8819 vinfos[3].indices[1] = _ij12[1];
8820 vinfos[3].maxsolutions = _nj12;
8821 vinfos[4].jointtype = 1;
8822 vinfos[4].foffset = j13;
8823 vinfos[4].indices[0] = _ij13[0];
8824 vinfos[4].indices[1] = _ij13[1];
8825 vinfos[4].maxsolutions = _nj13;
8826 vinfos[5].jointtype = 1;
8827 vinfos[5].foffset = j14;
8828 vinfos[5].indices[0] = _ij14[0];
8829 vinfos[5].indices[1] = _ij14[1];
8830 vinfos[5].maxsolutions = _nj14;
8831 std::vector<int> vfree(0);
8832 solutions.AddSolution(vinfos,vfree);
8833 }
8834 }
8835 }
8836 
8837 }
8838 
8839 }
8840 }
8841 }
8842 
8843 }
8844 
8845 }
8846 }
8847 }
8848 
8849 }
8850 
8851 }
8852 
8853 } else
8854 {
8855 {
8856 IkReal j9array[1], cj9array[1], sj9array[1];
8857 bool j9valid[1]={false};
8858 _nj9 = 1;
8859 IkReal x2058=((sj12)*(sj14));
8860 IkReal x2059=((cj14)*(sj12));
8861 IkReal x2060=((cj12)*(cj14)*(sj13));
8862 IkReal x2061=((IkReal(1.00000000000000))*(cj12)*(cj13));
8863 IkReal x2062=((IkReal(1.00000000000000))*(cj12)*(sj13)*(sj14));
8864 if( IKabs(((gconst1)*(((((r10)*(x2058)))+(((IkReal(-1.00000000000000))*(r12)*(x2061)))+(((r11)*(x2059)))+(((IkReal(-1.00000000000000))*(r11)*(x2062)))+(((r10)*(x2060))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((r01)*(x2059)))+(((IkReal(-1.00000000000000))*(r02)*(x2061)))+(((r00)*(x2060)))+(((IkReal(-1.00000000000000))*(r01)*(x2062)))+(((r00)*(x2058))))))) < IKFAST_ATAN2_MAGTHRESH )
8865  continue;
8866 j9array[0]=IKatan2(((gconst1)*(((((r10)*(x2058)))+(((IkReal(-1.00000000000000))*(r12)*(x2061)))+(((r11)*(x2059)))+(((IkReal(-1.00000000000000))*(r11)*(x2062)))+(((r10)*(x2060)))))), ((gconst1)*(((((r01)*(x2059)))+(((IkReal(-1.00000000000000))*(r02)*(x2061)))+(((r00)*(x2060)))+(((IkReal(-1.00000000000000))*(r01)*(x2062)))+(((r00)*(x2058)))))));
8867 sj9array[0]=IKsin(j9array[0]);
8868 cj9array[0]=IKcos(j9array[0]);
8869 if( j9array[0] > IKPI )
8870 {
8871  j9array[0]-=IK2PI;
8872 }
8873 else if( j9array[0] < -IKPI )
8874 { j9array[0]+=IK2PI;
8875 }
8876 j9valid[0] = true;
8877 for(int ij9 = 0; ij9 < 1; ++ij9)
8878 {
8879 if( !j9valid[ij9] )
8880 {
8881  continue;
8882 }
8883 _ij9[0] = ij9; _ij9[1] = -1;
8884 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
8885 {
8886 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
8887 {
8888  j9valid[iij9]=false; _ij9[1] = iij9; break;
8889 }
8890 }
8891 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
8892 {
8893 IkReal evalcond[3];
8894 IkReal x2063=IKsin(j9);
8895 IkReal x2064=IKcos(j9);
8896 IkReal x2065=((IkReal(1.00000000000000))*(sj14));
8897 IkReal x2066=((sj13)*(x2064));
8898 IkReal x2067=((cj13)*(x2063));
8899 IkReal x2068=((IkReal(1.00000000000000))*(cj14)*(r00));
8900 IkReal x2069=((r01)*(x2063));
8901 IkReal x2070=((r10)*(x2064));
8902 IkReal x2071=((sj13)*(x2063));
8903 IkReal x2072=((cj13)*(x2064));
8904 evalcond[0]=((((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2064)))+(((cj14)*(x2069)))+(cj12)+(((r00)*(sj14)*(x2063)))+(((IkReal(-1.00000000000000))*(x2065)*(x2070))));
8905 evalcond[1]=((((cj13)*(cj14)*(x2070)))+(((IkReal(-1.00000000000000))*(r11)*(x2065)*(x2072)))+(((r12)*(x2066)))+(((IkReal(-1.00000000000000))*(r02)*(x2071)))+(((r01)*(sj14)*(x2067)))+(((IkReal(-1.00000000000000))*(x2067)*(x2068))));
8906 evalcond[2]=((((IkReal(-1.00000000000000))*(r11)*(x2065)*(x2066)))+(((r02)*(x2067)))+(sj12)+(((cj14)*(r10)*(x2066)))+(((IkReal(-1.00000000000000))*(r12)*(x2072)))+(((sj13)*(sj14)*(x2069)))+(((IkReal(-1.00000000000000))*(x2068)*(x2071))));
8907 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
8908 {
8909 continue;
8910 }
8911 }
8912 
8913 {
8914 IkReal dummyeval[1];
8915 dummyeval[0]=sj12;
8916 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
8917 {
8918 {
8919 IkReal dummyeval[1];
8920 dummyeval[0]=cj12;
8921 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
8922 {
8923 {
8924 IkReal evalcond[7];
8925 IkReal x2073=((IkReal(1.00000000000000))*(sj13));
8926 IkReal x2074=((cj14)*(sj9));
8927 IkReal x2075=((cj9)*(sj14));
8928 IkReal x2076=((cj13)*(sj9));
8929 IkReal x2077=((sj13)*(sj14));
8930 IkReal x2078=((cj9)*(sj13));
8931 IkReal x2079=((IkReal(1.00000000000000))*(cj13));
8932 IkReal x2080=((cj14)*(r10));
8933 IkReal x2081=((sj14)*(sj9));
8934 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
8935 evalcond[1]=((((IkReal(-1.00000000000000))*(r10)*(x2075)))+(((IkReal(-1.00000000000000))*(cj14)*(cj9)*(r11)))+(((r01)*(x2074)))+(((r00)*(x2081))));
8936 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2073)))+(((cj13)*(r22)))+(((r21)*(x2077))));
8937 evalcond[3]=((IkReal(0.0950000000000000))+(((npy)*(x2077)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(cj14)*(npx)*(x2073))));
8938 evalcond[4]=((((r12)*(x2078)))+(((IkReal(-1.00000000000000))*(r00)*(x2074)*(x2079)))+(((cj13)*(cj9)*(x2080)))+(((IkReal(-1.00000000000000))*(r11)*(x2075)*(x2079)))+(((r01)*(sj14)*(x2076)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2073))));
8939 evalcond[5]=((IkReal(1.00000000000000))+(((x2078)*(x2080)))+(((r02)*(x2076)))+(((IkReal(-1.00000000000000))*(r00)*(x2073)*(x2074)))+(((IkReal(-1.00000000000000))*(cj9)*(r12)*(x2079)))+(((IkReal(-1.00000000000000))*(r11)*(x2073)*(x2075)))+(((r01)*(sj9)*(x2077))));
8940 evalcond[6]=((((IkReal(-1.00000000000000))*(r11)*(x2073)*(x2081)))+(((IkReal(-1.00000000000000))*(r01)*(x2073)*(x2075)))+(((cj14)*(r00)*(x2078)))+(((r10)*(sj13)*(x2074)))+(((IkReal(-1.00000000000000))*(r12)*(x2076)))+(((IkReal(-1.00000000000000))*(cj9)*(r02)*(x2079))));
8941 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 )
8942 {
8943 {
8944 IkReal j11array[1], cj11array[1], sj11array[1];
8945 bool j11valid[1]={false};
8946 _nj11 = 1;
8947 IkReal x2082=((IkReal(4.00000000000000))*(sj14));
8948 IkReal x2083=((IkReal(4.00000000000000))*(cj14));
8949 if( IKabs(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npy)*(x2083)))+(((IkReal(-1.00000000000000))*(npx)*(x2082))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2082)))+(((cj13)*(npx)*(x2083)))+(((IkReal(-0.360000000000000))*(cj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npy)*(x2083)))+(((IkReal(-1.00000000000000))*(npx)*(x2082)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2082)))+(((cj13)*(npx)*(x2083)))+(((IkReal(-0.360000000000000))*(cj13)))))-1) <= IKFAST_SINCOS_THRESH )
8950  continue;
8951 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npy)*(x2083)))+(((IkReal(-1.00000000000000))*(npx)*(x2082)))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2082)))+(((cj13)*(npx)*(x2083)))+(((IkReal(-0.360000000000000))*(cj13)))));
8952 sj11array[0]=IKsin(j11array[0]);
8953 cj11array[0]=IKcos(j11array[0]);
8954 if( j11array[0] > IKPI )
8955 {
8956  j11array[0]-=IK2PI;
8957 }
8958 else if( j11array[0] < -IKPI )
8959 { j11array[0]+=IK2PI;
8960 }
8961 j11valid[0] = true;
8962 for(int ij11 = 0; ij11 < 1; ++ij11)
8963 {
8964 if( !j11valid[ij11] )
8965 {
8966  continue;
8967 }
8968 _ij11[0] = ij11; _ij11[1] = -1;
8969 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
8970 {
8971 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
8972 {
8973  j11valid[iij11]=false; _ij11[1] = iij11; break;
8974 }
8975 }
8976 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
8977 {
8978 IkReal evalcond[2];
8979 evalcond[0]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(IKsin(j11)))));
8980 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(npx)))+(((cj13)*(npy)*(sj14)))+(((IkReal(-1.00000000000000))*(npz)*(sj13))));
8981 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
8982 {
8983 continue;
8984 }
8985 }
8986 
8987 {
8988 IkReal dummyeval[1];
8989 IkReal gconst56;
8990 gconst56=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
8991 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
8992 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
8993 {
8994 {
8995 IkReal dummyeval[1];
8996 IkReal gconst57;
8997 gconst57=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
8998 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
8999 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
9000 {
9001 continue;
9002 
9003 } else
9004 {
9005 {
9006 IkReal j10array[1], cj10array[1], sj10array[1];
9007 bool j10valid[1]={false};
9008 _nj10 = 1;
9009 IkReal x2084=((IkReal(1.00000000000000))*(cj11));
9010 IkReal x2085=((r20)*(sj14));
9011 IkReal x2086=((cj14)*(r21));
9012 IkReal x2087=((cj14)*(cj9)*(r01));
9013 IkReal x2088=((r10)*(sj14)*(sj9));
9014 IkReal x2089=((cj9)*(r00)*(sj14));
9015 IkReal x2090=((cj14)*(r11)*(sj9));
9016 if( IKabs(((gconst57)*(((((cj11)*(x2086)))+(((cj11)*(x2085)))+(((sj11)*(x2087)))+(((sj11)*(x2089)))+(((sj11)*(x2088)))+(((sj11)*(x2090))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst57)*(((((IkReal(-1.00000000000000))*(x2084)*(x2087)))+(((IkReal(-1.00000000000000))*(x2084)*(x2088)))+(((IkReal(-1.00000000000000))*(x2084)*(x2089)))+(((sj11)*(x2086)))+(((sj11)*(x2085)))+(((IkReal(-1.00000000000000))*(x2084)*(x2090))))))) < IKFAST_ATAN2_MAGTHRESH )
9017  continue;
9018 j10array[0]=IKatan2(((gconst57)*(((((cj11)*(x2086)))+(((cj11)*(x2085)))+(((sj11)*(x2087)))+(((sj11)*(x2089)))+(((sj11)*(x2088)))+(((sj11)*(x2090)))))), ((gconst57)*(((((IkReal(-1.00000000000000))*(x2084)*(x2087)))+(((IkReal(-1.00000000000000))*(x2084)*(x2088)))+(((IkReal(-1.00000000000000))*(x2084)*(x2089)))+(((sj11)*(x2086)))+(((sj11)*(x2085)))+(((IkReal(-1.00000000000000))*(x2084)*(x2090)))))));
9019 sj10array[0]=IKsin(j10array[0]);
9020 cj10array[0]=IKcos(j10array[0]);
9021 if( j10array[0] > IKPI )
9022 {
9023  j10array[0]-=IK2PI;
9024 }
9025 else if( j10array[0] < -IKPI )
9026 { j10array[0]+=IK2PI;
9027 }
9028 j10valid[0] = true;
9029 for(int ij10 = 0; ij10 < 1; ++ij10)
9030 {
9031 if( !j10valid[ij10] )
9032 {
9033  continue;
9034 }
9035 _ij10[0] = ij10; _ij10[1] = -1;
9036 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
9037 {
9038 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
9039 {
9040  j10valid[iij10]=false; _ij10[1] = iij10; break;
9041 }
9042 }
9043 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
9044 {
9045 IkReal evalcond[4];
9046 IkReal x2091=IKsin(j10);
9047 IkReal x2092=IKcos(j10);
9048 IkReal x2093=((cj13)*(sj14));
9049 IkReal x2094=((cj13)*(cj14));
9050 IkReal x2095=((r10)*(sj9));
9051 IkReal x2096=((IkReal(1.00000000000000))*(cj9));
9052 IkReal x2097=((sj11)*(x2091));
9053 IkReal x2098=((IkReal(1.00000000000000))*(x2092));
9054 IkReal x2099=((cj11)*(x2091));
9055 IkReal x2100=((IkReal(1.00000000000000))*(r11)*(sj9));
9056 IkReal x2101=((cj11)*(x2098));
9057 evalcond[0]=((((IkReal(-1.00000000000000))*(sj11)*(x2098)))+(((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x2099))));
9058 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x2094)))+(((IkReal(-1.00000000000000))*(x2101)))+(x2097)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x2093))));
9059 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(x2100)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2096)))+(((IkReal(-1.00000000000000))*(x2101)))+(x2097)+(((IkReal(-1.00000000000000))*(sj14)*(x2095)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2096))));
9060 evalcond[3]=((((IkReal(-1.00000000000000))*(x2093)*(x2100)))+(((x2094)*(x2095)))+(x2099)+(((cj9)*(r02)*(sj13)))+(((sj11)*(x2092)))+(((IkReal(-1.00000000000000))*(r01)*(x2093)*(x2096)))+(((cj9)*(r00)*(x2094)))+(((r12)*(sj13)*(sj9))));
9061 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
9062 {
9063 continue;
9064 }
9065 }
9066 
9067 {
9068 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9069 vinfos[0].jointtype = 1;
9070 vinfos[0].foffset = j9;
9071 vinfos[0].indices[0] = _ij9[0];
9072 vinfos[0].indices[1] = _ij9[1];
9073 vinfos[0].maxsolutions = _nj9;
9074 vinfos[1].jointtype = 1;
9075 vinfos[1].foffset = j10;
9076 vinfos[1].indices[0] = _ij10[0];
9077 vinfos[1].indices[1] = _ij10[1];
9078 vinfos[1].maxsolutions = _nj10;
9079 vinfos[2].jointtype = 1;
9080 vinfos[2].foffset = j11;
9081 vinfos[2].indices[0] = _ij11[0];
9082 vinfos[2].indices[1] = _ij11[1];
9083 vinfos[2].maxsolutions = _nj11;
9084 vinfos[3].jointtype = 1;
9085 vinfos[3].foffset = j12;
9086 vinfos[3].indices[0] = _ij12[0];
9087 vinfos[3].indices[1] = _ij12[1];
9088 vinfos[3].maxsolutions = _nj12;
9089 vinfos[4].jointtype = 1;
9090 vinfos[4].foffset = j13;
9091 vinfos[4].indices[0] = _ij13[0];
9092 vinfos[4].indices[1] = _ij13[1];
9093 vinfos[4].maxsolutions = _nj13;
9094 vinfos[5].jointtype = 1;
9095 vinfos[5].foffset = j14;
9096 vinfos[5].indices[0] = _ij14[0];
9097 vinfos[5].indices[1] = _ij14[1];
9098 vinfos[5].maxsolutions = _nj14;
9099 std::vector<int> vfree(0);
9100 solutions.AddSolution(vinfos,vfree);
9101 }
9102 }
9103 }
9104 
9105 }
9106 
9107 }
9108 
9109 } else
9110 {
9111 {
9112 IkReal j10array[1], cj10array[1], sj10array[1];
9113 bool j10valid[1]={false};
9114 _nj10 = 1;
9115 IkReal x2102=((r20)*(sj14));
9116 IkReal x2103=((IkReal(1.00000000000000))*(cj13));
9117 IkReal x2104=((cj14)*(r20));
9118 IkReal x2105=((r21)*(sj14));
9119 IkReal x2106=((r22)*(sj13));
9120 IkReal x2107=((cj14)*(r21));
9121 if( IKabs(((gconst56)*(((((cj13)*(sj11)*(x2104)))+(((IkReal(-1.00000000000000))*(sj11)*(x2103)*(x2105)))+(((cj11)*(x2102)))+(((cj11)*(x2107)))+(((sj11)*(x2106))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst56)*(((((IkReal(-1.00000000000000))*(cj11)*(x2106)))+(((cj11)*(cj13)*(x2105)))+(((IkReal(-1.00000000000000))*(cj11)*(x2103)*(x2104)))+(((sj11)*(x2102)))+(((sj11)*(x2107))))))) < IKFAST_ATAN2_MAGTHRESH )
9122  continue;
9123 j10array[0]=IKatan2(((gconst56)*(((((cj13)*(sj11)*(x2104)))+(((IkReal(-1.00000000000000))*(sj11)*(x2103)*(x2105)))+(((cj11)*(x2102)))+(((cj11)*(x2107)))+(((sj11)*(x2106)))))), ((gconst56)*(((((IkReal(-1.00000000000000))*(cj11)*(x2106)))+(((cj11)*(cj13)*(x2105)))+(((IkReal(-1.00000000000000))*(cj11)*(x2103)*(x2104)))+(((sj11)*(x2102)))+(((sj11)*(x2107)))))));
9124 sj10array[0]=IKsin(j10array[0]);
9125 cj10array[0]=IKcos(j10array[0]);
9126 if( j10array[0] > IKPI )
9127 {
9128  j10array[0]-=IK2PI;
9129 }
9130 else if( j10array[0] < -IKPI )
9131 { j10array[0]+=IK2PI;
9132 }
9133 j10valid[0] = true;
9134 for(int ij10 = 0; ij10 < 1; ++ij10)
9135 {
9136 if( !j10valid[ij10] )
9137 {
9138  continue;
9139 }
9140 _ij10[0] = ij10; _ij10[1] = -1;
9141 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
9142 {
9143 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
9144 {
9145  j10valid[iij10]=false; _ij10[1] = iij10; break;
9146 }
9147 }
9148 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
9149 {
9150 IkReal evalcond[4];
9151 IkReal x2108=IKsin(j10);
9152 IkReal x2109=IKcos(j10);
9153 IkReal x2110=((cj13)*(sj14));
9154 IkReal x2111=((cj13)*(cj14));
9155 IkReal x2112=((r10)*(sj9));
9156 IkReal x2113=((IkReal(1.00000000000000))*(cj9));
9157 IkReal x2114=((sj11)*(x2108));
9158 IkReal x2115=((IkReal(1.00000000000000))*(x2109));
9159 IkReal x2116=((cj11)*(x2108));
9160 IkReal x2117=((IkReal(1.00000000000000))*(r11)*(sj9));
9161 IkReal x2118=((cj11)*(x2115));
9162 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x2116)))+(((IkReal(-1.00000000000000))*(sj11)*(x2115))));
9163 evalcond[1]=((((r21)*(x2110)))+(x2114)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x2118)))+(((IkReal(-1.00000000000000))*(r20)*(x2111))));
9164 evalcond[2]=((x2114)+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2113)))+(((IkReal(-1.00000000000000))*(cj14)*(x2117)))+(((IkReal(-1.00000000000000))*(x2118)))+(((IkReal(-1.00000000000000))*(sj14)*(x2112)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2113))));
9165 evalcond[3]=((((cj9)*(r02)*(sj13)))+(x2116)+(((x2111)*(x2112)))+(((cj9)*(r00)*(x2111)))+(((IkReal(-1.00000000000000))*(x2110)*(x2117)))+(((IkReal(-1.00000000000000))*(r01)*(x2110)*(x2113)))+(((r12)*(sj13)*(sj9)))+(((sj11)*(x2109))));
9166 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
9167 {
9168 continue;
9169 }
9170 }
9171 
9172 {
9173 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9174 vinfos[0].jointtype = 1;
9175 vinfos[0].foffset = j9;
9176 vinfos[0].indices[0] = _ij9[0];
9177 vinfos[0].indices[1] = _ij9[1];
9178 vinfos[0].maxsolutions = _nj9;
9179 vinfos[1].jointtype = 1;
9180 vinfos[1].foffset = j10;
9181 vinfos[1].indices[0] = _ij10[0];
9182 vinfos[1].indices[1] = _ij10[1];
9183 vinfos[1].maxsolutions = _nj10;
9184 vinfos[2].jointtype = 1;
9185 vinfos[2].foffset = j11;
9186 vinfos[2].indices[0] = _ij11[0];
9187 vinfos[2].indices[1] = _ij11[1];
9188 vinfos[2].maxsolutions = _nj11;
9189 vinfos[3].jointtype = 1;
9190 vinfos[3].foffset = j12;
9191 vinfos[3].indices[0] = _ij12[0];
9192 vinfos[3].indices[1] = _ij12[1];
9193 vinfos[3].maxsolutions = _nj12;
9194 vinfos[4].jointtype = 1;
9195 vinfos[4].foffset = j13;
9196 vinfos[4].indices[0] = _ij13[0];
9197 vinfos[4].indices[1] = _ij13[1];
9198 vinfos[4].maxsolutions = _nj13;
9199 vinfos[5].jointtype = 1;
9200 vinfos[5].foffset = j14;
9201 vinfos[5].indices[0] = _ij14[0];
9202 vinfos[5].indices[1] = _ij14[1];
9203 vinfos[5].maxsolutions = _nj14;
9204 std::vector<int> vfree(0);
9205 solutions.AddSolution(vinfos,vfree);
9206 }
9207 }
9208 }
9209 
9210 }
9211 
9212 }
9213 }
9214 }
9215 
9216 } else
9217 {
9218 IkReal x2119=((IkReal(1.00000000000000))*(sj13));
9219 IkReal x2120=((cj14)*(sj9));
9220 IkReal x2121=((cj9)*(sj14));
9221 IkReal x2122=((cj13)*(sj9));
9222 IkReal x2123=((sj13)*(sj14));
9223 IkReal x2124=((cj9)*(sj13));
9224 IkReal x2125=((IkReal(1.00000000000000))*(cj13));
9225 IkReal x2126=((cj14)*(r10));
9226 IkReal x2127=((sj14)*(sj9));
9227 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
9228 evalcond[1]=((((IkReal(-1.00000000000000))*(cj14)*(cj9)*(r11)))+(((r01)*(x2120)))+(((IkReal(-1.00000000000000))*(r10)*(x2121)))+(((r00)*(x2127))));
9229 evalcond[2]=((((r21)*(x2123)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2119))));
9230 evalcond[3]=((IkReal(-0.0950000000000000))+(((npy)*(x2123)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(cj14)*(npx)*(x2119))));
9231 evalcond[4]=((((r12)*(x2124)))+(((r01)*(sj14)*(x2122)))+(((cj13)*(cj9)*(x2126)))+(((IkReal(-1.00000000000000))*(r00)*(x2120)*(x2125)))+(((IkReal(-1.00000000000000))*(r11)*(x2121)*(x2125)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2119))));
9232 evalcond[5]=((IkReal(-1.00000000000000))+(((r01)*(sj9)*(x2123)))+(((IkReal(-1.00000000000000))*(r00)*(x2119)*(x2120)))+(((IkReal(-1.00000000000000))*(r11)*(x2119)*(x2121)))+(((r02)*(x2122)))+(((x2124)*(x2126)))+(((IkReal(-1.00000000000000))*(cj9)*(r12)*(x2125))));
9233 evalcond[6]=((((IkReal(-1.00000000000000))*(r01)*(x2119)*(x2121)))+(((IkReal(-1.00000000000000))*(r11)*(x2119)*(x2127)))+(((IkReal(-1.00000000000000))*(r12)*(x2122)))+(((IkReal(-1.00000000000000))*(cj9)*(r02)*(x2125)))+(((cj14)*(r00)*(x2124)))+(((r10)*(sj13)*(x2120))));
9234 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 )
9235 {
9236 {
9237 IkReal j11array[1], cj11array[1], sj11array[1];
9238 bool j11valid[1]={false};
9239 _nj11 = 1;
9240 IkReal x2128=((IkReal(4.00000000000000))*(sj14));
9241 IkReal x2129=((IkReal(4.00000000000000))*(cj14));
9242 if( IKabs(((IkReal(0.120000000000000))+(((npx)*(x2128)))+(((npy)*(x2129))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2128)))+(((cj13)*(npx)*(x2129)))+(((IkReal(-0.360000000000000))*(cj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((npx)*(x2128)))+(((npy)*(x2129)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2128)))+(((cj13)*(npx)*(x2129)))+(((IkReal(-0.360000000000000))*(cj13)))))-1) <= IKFAST_SINCOS_THRESH )
9243  continue;
9244 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((npx)*(x2128)))+(((npy)*(x2129)))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2128)))+(((cj13)*(npx)*(x2129)))+(((IkReal(-0.360000000000000))*(cj13)))));
9245 sj11array[0]=IKsin(j11array[0]);
9246 cj11array[0]=IKcos(j11array[0]);
9247 if( j11array[0] > IKPI )
9248 {
9249  j11array[0]-=IK2PI;
9250 }
9251 else if( j11array[0] < -IKPI )
9252 { j11array[0]+=IK2PI;
9253 }
9254 j11valid[0] = true;
9255 for(int ij11 = 0; ij11 < 1; ++ij11)
9256 {
9257 if( !j11valid[ij11] )
9258 {
9259  continue;
9260 }
9261 _ij11[0] = ij11; _ij11[1] = -1;
9262 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
9263 {
9264 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
9265 {
9266  j11valid[iij11]=false; _ij11[1] = iij11; break;
9267 }
9268 }
9269 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
9270 {
9271 IkReal evalcond[2];
9272 evalcond[0]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(IKsin(j11))))+(((cj14)*(npy)))+(((npx)*(sj14))));
9273 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(npx)))+(((cj13)*(npy)*(sj14)))+(((IkReal(-1.00000000000000))*(npz)*(sj13))));
9274 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
9275 {
9276 continue;
9277 }
9278 }
9279 
9280 {
9281 IkReal dummyeval[1];
9282 IkReal gconst58;
9283 gconst58=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
9284 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
9285 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
9286 {
9287 {
9288 IkReal dummyeval[1];
9289 IkReal gconst59;
9290 gconst59=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
9291 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
9292 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
9293 {
9294 continue;
9295 
9296 } else
9297 {
9298 {
9299 IkReal j10array[1], cj10array[1], sj10array[1];
9300 bool j10valid[1]={false};
9301 _nj10 = 1;
9302 IkReal x2130=((IkReal(1.00000000000000))*(sj11));
9303 IkReal x2131=((cj14)*(r21));
9304 IkReal x2132=((IkReal(1.00000000000000))*(cj11));
9305 IkReal x2133=((r20)*(sj14));
9306 IkReal x2134=((cj9)*(r00)*(sj14));
9307 IkReal x2135=((cj14)*(r11)*(sj9));
9308 IkReal x2136=((cj14)*(cj9)*(r01));
9309 IkReal x2137=((r10)*(sj14)*(sj9));
9310 if( IKabs(((gconst59)*(((((IkReal(-1.00000000000000))*(x2132)*(x2133)))+(((IkReal(-1.00000000000000))*(x2130)*(x2137)))+(((IkReal(-1.00000000000000))*(x2130)*(x2136)))+(((IkReal(-1.00000000000000))*(x2130)*(x2135)))+(((IkReal(-1.00000000000000))*(x2130)*(x2134)))+(((IkReal(-1.00000000000000))*(x2131)*(x2132))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst59)*(((((cj11)*(x2136)))+(((cj11)*(x2137)))+(((cj11)*(x2134)))+(((cj11)*(x2135)))+(((IkReal(-1.00000000000000))*(x2130)*(x2133)))+(((IkReal(-1.00000000000000))*(x2130)*(x2131))))))) < IKFAST_ATAN2_MAGTHRESH )
9311  continue;
9312 j10array[0]=IKatan2(((gconst59)*(((((IkReal(-1.00000000000000))*(x2132)*(x2133)))+(((IkReal(-1.00000000000000))*(x2130)*(x2137)))+(((IkReal(-1.00000000000000))*(x2130)*(x2136)))+(((IkReal(-1.00000000000000))*(x2130)*(x2135)))+(((IkReal(-1.00000000000000))*(x2130)*(x2134)))+(((IkReal(-1.00000000000000))*(x2131)*(x2132)))))), ((gconst59)*(((((cj11)*(x2136)))+(((cj11)*(x2137)))+(((cj11)*(x2134)))+(((cj11)*(x2135)))+(((IkReal(-1.00000000000000))*(x2130)*(x2133)))+(((IkReal(-1.00000000000000))*(x2130)*(x2131)))))));
9313 sj10array[0]=IKsin(j10array[0]);
9314 cj10array[0]=IKcos(j10array[0]);
9315 if( j10array[0] > IKPI )
9316 {
9317  j10array[0]-=IK2PI;
9318 }
9319 else if( j10array[0] < -IKPI )
9320 { j10array[0]+=IK2PI;
9321 }
9322 j10valid[0] = true;
9323 for(int ij10 = 0; ij10 < 1; ++ij10)
9324 {
9325 if( !j10valid[ij10] )
9326 {
9327  continue;
9328 }
9329 _ij10[0] = ij10; _ij10[1] = -1;
9330 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
9331 {
9332 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
9333 {
9334  j10valid[iij10]=false; _ij10[1] = iij10; break;
9335 }
9336 }
9337 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
9338 {
9339 IkReal evalcond[4];
9340 IkReal x2138=IKsin(j10);
9341 IkReal x2139=IKcos(j10);
9342 IkReal x2140=((cj14)*(sj9));
9343 IkReal x2141=((IkReal(1.00000000000000))*(r11));
9344 IkReal x2142=((cj13)*(sj14));
9345 IkReal x2143=((IkReal(1.00000000000000))*(cj9));
9346 IkReal x2144=((cj13)*(cj14));
9347 IkReal x2145=((cj11)*(x2138));
9348 IkReal x2146=((sj11)*(x2139));
9349 IkReal x2147=((cj11)*(x2139));
9350 IkReal x2148=((sj11)*(x2138));
9351 IkReal x2149=((x2146)+(x2145));
9352 evalcond[0]=((((cj14)*(r21)))+(x2149)+(((r20)*(sj14))));
9353 evalcond[1]=((((IkReal(-1.00000000000000))*(x2147)))+(((IkReal(-1.00000000000000))*(r20)*(x2144)))+(x2148)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x2142))));
9354 evalcond[2]=((((IkReal(-1.00000000000000))*(x2148)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2143)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2143)))+(x2147)+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x2140)*(x2141))));
9355 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x2142)*(x2143)))+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(sj9)*(x2141)*(x2142)))+(x2149)+(((cj9)*(r00)*(x2144)))+(((r12)*(sj13)*(sj9)))+(((cj13)*(r10)*(x2140))));
9356 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
9357 {
9358 continue;
9359 }
9360 }
9361 
9362 {
9363 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9364 vinfos[0].jointtype = 1;
9365 vinfos[0].foffset = j9;
9366 vinfos[0].indices[0] = _ij9[0];
9367 vinfos[0].indices[1] = _ij9[1];
9368 vinfos[0].maxsolutions = _nj9;
9369 vinfos[1].jointtype = 1;
9370 vinfos[1].foffset = j10;
9371 vinfos[1].indices[0] = _ij10[0];
9372 vinfos[1].indices[1] = _ij10[1];
9373 vinfos[1].maxsolutions = _nj10;
9374 vinfos[2].jointtype = 1;
9375 vinfos[2].foffset = j11;
9376 vinfos[2].indices[0] = _ij11[0];
9377 vinfos[2].indices[1] = _ij11[1];
9378 vinfos[2].maxsolutions = _nj11;
9379 vinfos[3].jointtype = 1;
9380 vinfos[3].foffset = j12;
9381 vinfos[3].indices[0] = _ij12[0];
9382 vinfos[3].indices[1] = _ij12[1];
9383 vinfos[3].maxsolutions = _nj12;
9384 vinfos[4].jointtype = 1;
9385 vinfos[4].foffset = j13;
9386 vinfos[4].indices[0] = _ij13[0];
9387 vinfos[4].indices[1] = _ij13[1];
9388 vinfos[4].maxsolutions = _nj13;
9389 vinfos[5].jointtype = 1;
9390 vinfos[5].foffset = j14;
9391 vinfos[5].indices[0] = _ij14[0];
9392 vinfos[5].indices[1] = _ij14[1];
9393 vinfos[5].maxsolutions = _nj14;
9394 std::vector<int> vfree(0);
9395 solutions.AddSolution(vinfos,vfree);
9396 }
9397 }
9398 }
9399 
9400 }
9401 
9402 }
9403 
9404 } else
9405 {
9406 {
9407 IkReal j10array[1], cj10array[1], sj10array[1];
9408 bool j10valid[1]={false};
9409 _nj10 = 1;
9410 IkReal x2150=((cj13)*(sj11));
9411 IkReal x2151=((r21)*(sj14));
9412 IkReal x2152=((cj14)*(r20));
9413 IkReal x2153=((cj11)*(cj13));
9414 IkReal x2154=((r22)*(sj13));
9415 IkReal x2155=((r20)*(sj14));
9416 IkReal x2156=((cj14)*(r21));
9417 if( IKabs(((gconst58)*(((((IkReal(-1.00000000000000))*(sj11)*(x2154)))+(((IkReal(-1.00000000000000))*(x2150)*(x2152)))+(((cj11)*(x2155)))+(((cj11)*(x2156)))+(((x2150)*(x2151))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst58)*(((((x2152)*(x2153)))+(((cj11)*(x2154)))+(((IkReal(-1.00000000000000))*(x2151)*(x2153)))+(((sj11)*(x2156)))+(((sj11)*(x2155))))))) < IKFAST_ATAN2_MAGTHRESH )
9418  continue;
9419 j10array[0]=IKatan2(((gconst58)*(((((IkReal(-1.00000000000000))*(sj11)*(x2154)))+(((IkReal(-1.00000000000000))*(x2150)*(x2152)))+(((cj11)*(x2155)))+(((cj11)*(x2156)))+(((x2150)*(x2151)))))), ((gconst58)*(((((x2152)*(x2153)))+(((cj11)*(x2154)))+(((IkReal(-1.00000000000000))*(x2151)*(x2153)))+(((sj11)*(x2156)))+(((sj11)*(x2155)))))));
9420 sj10array[0]=IKsin(j10array[0]);
9421 cj10array[0]=IKcos(j10array[0]);
9422 if( j10array[0] > IKPI )
9423 {
9424  j10array[0]-=IK2PI;
9425 }
9426 else if( j10array[0] < -IKPI )
9427 { j10array[0]+=IK2PI;
9428 }
9429 j10valid[0] = true;
9430 for(int ij10 = 0; ij10 < 1; ++ij10)
9431 {
9432 if( !j10valid[ij10] )
9433 {
9434  continue;
9435 }
9436 _ij10[0] = ij10; _ij10[1] = -1;
9437 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
9438 {
9439 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
9440 {
9441  j10valid[iij10]=false; _ij10[1] = iij10; break;
9442 }
9443 }
9444 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
9445 {
9446 IkReal evalcond[4];
9447 IkReal x2157=IKsin(j10);
9448 IkReal x2158=IKcos(j10);
9449 IkReal x2159=((cj14)*(sj9));
9450 IkReal x2160=((IkReal(1.00000000000000))*(r11));
9451 IkReal x2161=((cj13)*(sj14));
9452 IkReal x2162=((IkReal(1.00000000000000))*(cj9));
9453 IkReal x2163=((cj13)*(cj14));
9454 IkReal x2164=((cj11)*(x2157));
9455 IkReal x2165=((sj11)*(x2158));
9456 IkReal x2166=((cj11)*(x2158));
9457 IkReal x2167=((sj11)*(x2157));
9458 IkReal x2168=((x2165)+(x2164));
9459 evalcond[0]=((((cj14)*(r21)))+(x2168)+(((r20)*(sj14))));
9460 evalcond[1]=((x2167)+(((r21)*(x2161)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x2166)))+(((IkReal(-1.00000000000000))*(r20)*(x2163))));
9461 evalcond[2]=((x2166)+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2162)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x2159)*(x2160)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2162)))+(((IkReal(-1.00000000000000))*(x2167))));
9462 evalcond[3]=((((cj13)*(r10)*(x2159)))+(((cj9)*(r02)*(sj13)))+(x2168)+(((cj9)*(r00)*(x2163)))+(((IkReal(-1.00000000000000))*(sj9)*(x2160)*(x2161)))+(((IkReal(-1.00000000000000))*(r01)*(x2161)*(x2162)))+(((r12)*(sj13)*(sj9))));
9463 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
9464 {
9465 continue;
9466 }
9467 }
9468 
9469 {
9470 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9471 vinfos[0].jointtype = 1;
9472 vinfos[0].foffset = j9;
9473 vinfos[0].indices[0] = _ij9[0];
9474 vinfos[0].indices[1] = _ij9[1];
9475 vinfos[0].maxsolutions = _nj9;
9476 vinfos[1].jointtype = 1;
9477 vinfos[1].foffset = j10;
9478 vinfos[1].indices[0] = _ij10[0];
9479 vinfos[1].indices[1] = _ij10[1];
9480 vinfos[1].maxsolutions = _nj10;
9481 vinfos[2].jointtype = 1;
9482 vinfos[2].foffset = j11;
9483 vinfos[2].indices[0] = _ij11[0];
9484 vinfos[2].indices[1] = _ij11[1];
9485 vinfos[2].maxsolutions = _nj11;
9486 vinfos[3].jointtype = 1;
9487 vinfos[3].foffset = j12;
9488 vinfos[3].indices[0] = _ij12[0];
9489 vinfos[3].indices[1] = _ij12[1];
9490 vinfos[3].maxsolutions = _nj12;
9491 vinfos[4].jointtype = 1;
9492 vinfos[4].foffset = j13;
9493 vinfos[4].indices[0] = _ij13[0];
9494 vinfos[4].indices[1] = _ij13[1];
9495 vinfos[4].maxsolutions = _nj13;
9496 vinfos[5].jointtype = 1;
9497 vinfos[5].foffset = j14;
9498 vinfos[5].indices[0] = _ij14[0];
9499 vinfos[5].indices[1] = _ij14[1];
9500 vinfos[5].maxsolutions = _nj14;
9501 std::vector<int> vfree(0);
9502 solutions.AddSolution(vinfos,vfree);
9503 }
9504 }
9505 }
9506 
9507 }
9508 
9509 }
9510 }
9511 }
9512 
9513 } else
9514 {
9515 IkReal x2169=((cj13)*(sj9));
9516 IkReal x2170=((r01)*(sj14));
9517 IkReal x2171=((cj9)*(sj13));
9518 IkReal x2172=((sj13)*(sj9));
9519 IkReal x2173=((cj14)*(r01));
9520 IkReal x2174=((cj14)*(r10));
9521 IkReal x2175=((cj13)*(cj9));
9522 IkReal x2176=((IkReal(1.00000000000000))*(cj9));
9523 IkReal x2177=((sj14)*(sj9));
9524 IkReal x2178=((IkReal(1.00000000000000))*(cj14)*(sj9));
9525 IkReal x2179=((sj14)*(x2176));
9526 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
9527 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
9528 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
9529 evalcond[3]=((IkReal(1.00000000000000))+(((r00)*(x2177)))+(((sj9)*(x2173)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2176)))+(((IkReal(-1.00000000000000))*(r10)*(x2179))));
9530 evalcond[4]=((((IkReal(-1.00000000000000))*(r10)*(x2177)))+(((IkReal(-1.00000000000000))*(x2173)*(x2176)))+(((IkReal(-1.00000000000000))*(r11)*(x2178)))+(((IkReal(-1.00000000000000))*(r00)*(x2179))));
9531 evalcond[5]=((((x2174)*(x2175)))+(((IkReal(-1.00000000000000))*(r02)*(x2172)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(x2175)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x2169)))+(((x2169)*(x2170)))+(((r12)*(x2171))));
9532 evalcond[6]=((((IkReal(-1.00000000000000))*(r12)*(x2175)))+(((x2170)*(x2172)))+(((x2171)*(x2174)))+(((r02)*(x2169)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(x2171)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x2172))));
9533 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 )
9534 {
9535 {
9536 IkReal j11array[1], cj11array[1], sj11array[1];
9537 bool j11valid[1]={false};
9538 _nj11 = 1;
9539 IkReal x2180=((IkReal(4.00000000000000))*(cj13));
9540 IkReal x2181=((npy)*(sj14));
9541 IkReal x2182=((cj14)*(npx));
9542 IkReal x2183=((IkReal(4.00000000000000))*(sj13));
9543 if( IKabs(((IkReal(0.120000000000000))+(((npz)*(x2180)))+(((IkReal(0.360000000000000))*(sj13)))+(((x2181)*(x2183)))+(((IkReal(-1.00000000000000))*(x2182)*(x2183))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((npz)*(x2183)))+(((IkReal(-1.00000000000000))*(x2180)*(x2181)))+(((x2180)*(x2182)))+(((IkReal(-0.360000000000000))*(cj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((npz)*(x2180)))+(((IkReal(0.360000000000000))*(sj13)))+(((x2181)*(x2183)))+(((IkReal(-1.00000000000000))*(x2182)*(x2183)))))+IKsqr(((IkReal(-0.940000000000000))+(((npz)*(x2183)))+(((IkReal(-1.00000000000000))*(x2180)*(x2181)))+(((x2180)*(x2182)))+(((IkReal(-0.360000000000000))*(cj13)))))-1) <= IKFAST_SINCOS_THRESH )
9544  continue;
9545 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((npz)*(x2180)))+(((IkReal(0.360000000000000))*(sj13)))+(((x2181)*(x2183)))+(((IkReal(-1.00000000000000))*(x2182)*(x2183)))), ((IkReal(-0.940000000000000))+(((npz)*(x2183)))+(((IkReal(-1.00000000000000))*(x2180)*(x2181)))+(((x2180)*(x2182)))+(((IkReal(-0.360000000000000))*(cj13)))));
9546 sj11array[0]=IKsin(j11array[0]);
9547 cj11array[0]=IKcos(j11array[0]);
9548 if( j11array[0] > IKPI )
9549 {
9550  j11array[0]-=IK2PI;
9551 }
9552 else if( j11array[0] < -IKPI )
9553 { j11array[0]+=IK2PI;
9554 }
9555 j11valid[0] = true;
9556 for(int ij11 = 0; ij11 < 1; ++ij11)
9557 {
9558 if( !j11valid[ij11] )
9559 {
9560  continue;
9561 }
9562 _ij11[0] = ij11; _ij11[1] = -1;
9563 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
9564 {
9565 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
9566 {
9567  j11valid[iij11]=false; _ij11[1] = iij11; break;
9568 }
9569 }
9570 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
9571 {
9572 IkReal evalcond[2];
9573 IkReal x2184=((IkReal(1.00000000000000))*(sj13));
9574 IkReal x2185=((cj14)*(npx));
9575 IkReal x2186=((npy)*(sj14));
9576 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x2184)))+(((cj13)*(x2186)))+(((IkReal(-1.00000000000000))*(cj13)*(x2185))));
9577 evalcond[1]=((IkReal(0.0300000000000000))+(((sj13)*(x2186)))+(((IkReal(-0.250000000000000))*(IKsin(j11))))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x2184)*(x2185))));
9578 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
9579 {
9580 continue;
9581 }
9582 }
9583 
9584 {
9585 IkReal dummyeval[1];
9586 IkReal gconst60;
9587 gconst60=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
9588 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
9589 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
9590 {
9591 {
9592 IkReal dummyeval[1];
9593 IkReal gconst61;
9594 gconst61=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
9595 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
9596 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
9597 {
9598 continue;
9599 
9600 } else
9601 {
9602 {
9603 IkReal j10array[1], cj10array[1], sj10array[1];
9604 bool j10valid[1]={false};
9605 _nj10 = 1;
9606 IkReal x2187=((cj13)*(sj14));
9607 IkReal x2188=((IkReal(1.00000000000000))*(cj11));
9608 IkReal x2189=((r11)*(sj9));
9609 IkReal x2190=((cj11)*(sj13));
9610 IkReal x2191=((r12)*(sj9));
9611 IkReal x2192=((cj9)*(r00));
9612 IkReal x2193=((sj11)*(sj13));
9613 IkReal x2194=((cj9)*(r02));
9614 IkReal x2195=((cj9)*(r01));
9615 IkReal x2196=((IkReal(1.00000000000000))*(sj11));
9616 IkReal x2197=((r10)*(sj9));
9617 IkReal x2198=((cj13)*(cj14)*(sj11));
9618 IkReal x2199=((cj11)*(cj13)*(cj14));
9619 if( IKabs(((gconst61)*(((((IkReal(-1.00000000000000))*(x2187)*(x2188)*(x2189)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x2196)))+(((x2192)*(x2199)))+(((r21)*(sj11)*(x2187)))+(((x2190)*(x2194)))+(((x2190)*(x2191)))+(((IkReal(-1.00000000000000))*(r22)*(x2193)))+(((x2197)*(x2199)))+(((IkReal(-1.00000000000000))*(x2187)*(x2188)*(x2195))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst61)*(((((r22)*(x2190)))+(((x2191)*(x2193)))+(((x2193)*(x2194)))+(((x2192)*(x2198)))+(((r20)*(x2199)))+(((IkReal(-1.00000000000000))*(x2187)*(x2195)*(x2196)))+(((IkReal(-1.00000000000000))*(x2187)*(x2189)*(x2196)))+(((x2197)*(x2198)))+(((IkReal(-1.00000000000000))*(r21)*(x2187)*(x2188))))))) < IKFAST_ATAN2_MAGTHRESH )
9620  continue;
9621 j10array[0]=IKatan2(((gconst61)*(((((IkReal(-1.00000000000000))*(x2187)*(x2188)*(x2189)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x2196)))+(((x2192)*(x2199)))+(((r21)*(sj11)*(x2187)))+(((x2190)*(x2194)))+(((x2190)*(x2191)))+(((IkReal(-1.00000000000000))*(r22)*(x2193)))+(((x2197)*(x2199)))+(((IkReal(-1.00000000000000))*(x2187)*(x2188)*(x2195)))))), ((gconst61)*(((((r22)*(x2190)))+(((x2191)*(x2193)))+(((x2193)*(x2194)))+(((x2192)*(x2198)))+(((r20)*(x2199)))+(((IkReal(-1.00000000000000))*(x2187)*(x2195)*(x2196)))+(((IkReal(-1.00000000000000))*(x2187)*(x2189)*(x2196)))+(((x2197)*(x2198)))+(((IkReal(-1.00000000000000))*(r21)*(x2187)*(x2188)))))));
9622 sj10array[0]=IKsin(j10array[0]);
9623 cj10array[0]=IKcos(j10array[0]);
9624 if( j10array[0] > IKPI )
9625 {
9626  j10array[0]-=IK2PI;
9627 }
9628 else if( j10array[0] < -IKPI )
9629 { j10array[0]+=IK2PI;
9630 }
9631 j10valid[0] = true;
9632 for(int ij10 = 0; ij10 < 1; ++ij10)
9633 {
9634 if( !j10valid[ij10] )
9635 {
9636  continue;
9637 }
9638 _ij10[0] = ij10; _ij10[1] = -1;
9639 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
9640 {
9641 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
9642 {
9643  j10valid[iij10]=false; _ij10[1] = iij10; break;
9644 }
9645 }
9646 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
9647 {
9648 IkReal evalcond[4];
9649 IkReal x2200=IKsin(j10);
9650 IkReal x2201=IKcos(j10);
9651 IkReal x2202=((IkReal(1.00000000000000))*(sj13));
9652 IkReal x2203=((r11)*(sj9));
9653 IkReal x2204=((cj9)*(r01));
9654 IkReal x2205=((r21)*(sj14));
9655 IkReal x2206=((cj9)*(r02));
9656 IkReal x2207=((sj13)*(sj9));
9657 IkReal x2208=((cj14)*(r10));
9658 IkReal x2209=((IkReal(1.00000000000000))*(cj13));
9659 IkReal x2210=((cj14)*(r20));
9660 IkReal x2211=((cj11)*(x2200));
9661 IkReal x2212=((sj11)*(x2201));
9662 IkReal x2213=((sj14)*(x2209));
9663 IkReal x2214=((sj11)*(x2200));
9664 IkReal x2215=((cj11)*(x2201));
9665 IkReal x2216=((cj14)*(cj9)*(r00));
9666 IkReal x2217=((x2212)+(x2211));
9667 evalcond[0]=((((IkReal(-1.00000000000000))*(x2215)))+(x2214)+(((IkReal(-1.00000000000000))*(r22)*(x2202)))+(((cj13)*(x2205)))+(((IkReal(-1.00000000000000))*(x2209)*(x2210))));
9668 evalcond[1]=((x2217)+(((IkReal(-1.00000000000000))*(x2202)*(x2210)))+(((sj13)*(x2205)))+(((cj13)*(r22))));
9669 evalcond[2]=((x2217)+(((IkReal(-1.00000000000000))*(x2204)*(x2213)))+(((r12)*(x2207)))+(((cj13)*(x2216)))+(((cj13)*(sj9)*(x2208)))+(((sj13)*(x2206)))+(((IkReal(-1.00000000000000))*(x2203)*(x2213))));
9670 evalcond[3]=((((IkReal(-1.00000000000000))*(x2214)))+(x2215)+(((IkReal(-1.00000000000000))*(sj14)*(x2202)*(x2204)))+(((IkReal(-1.00000000000000))*(sj14)*(x2202)*(x2203)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2209)))+(((IkReal(-1.00000000000000))*(x2206)*(x2209)))+(((x2207)*(x2208)))+(((sj13)*(x2216))));
9671 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
9672 {
9673 continue;
9674 }
9675 }
9676 
9677 {
9678 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9679 vinfos[0].jointtype = 1;
9680 vinfos[0].foffset = j9;
9681 vinfos[0].indices[0] = _ij9[0];
9682 vinfos[0].indices[1] = _ij9[1];
9683 vinfos[0].maxsolutions = _nj9;
9684 vinfos[1].jointtype = 1;
9685 vinfos[1].foffset = j10;
9686 vinfos[1].indices[0] = _ij10[0];
9687 vinfos[1].indices[1] = _ij10[1];
9688 vinfos[1].maxsolutions = _nj10;
9689 vinfos[2].jointtype = 1;
9690 vinfos[2].foffset = j11;
9691 vinfos[2].indices[0] = _ij11[0];
9692 vinfos[2].indices[1] = _ij11[1];
9693 vinfos[2].maxsolutions = _nj11;
9694 vinfos[3].jointtype = 1;
9695 vinfos[3].foffset = j12;
9696 vinfos[3].indices[0] = _ij12[0];
9697 vinfos[3].indices[1] = _ij12[1];
9698 vinfos[3].maxsolutions = _nj12;
9699 vinfos[4].jointtype = 1;
9700 vinfos[4].foffset = j13;
9701 vinfos[4].indices[0] = _ij13[0];
9702 vinfos[4].indices[1] = _ij13[1];
9703 vinfos[4].maxsolutions = _nj13;
9704 vinfos[5].jointtype = 1;
9705 vinfos[5].foffset = j14;
9706 vinfos[5].indices[0] = _ij14[0];
9707 vinfos[5].indices[1] = _ij14[1];
9708 vinfos[5].maxsolutions = _nj14;
9709 std::vector<int> vfree(0);
9710 solutions.AddSolution(vinfos,vfree);
9711 }
9712 }
9713 }
9714 
9715 }
9716 
9717 }
9718 
9719 } else
9720 {
9721 {
9722 IkReal j10array[1], cj10array[1], sj10array[1];
9723 bool j10valid[1]={false};
9724 _nj10 = 1;
9725 IkReal x2218=((cj13)*(sj11));
9726 IkReal x2219=((r21)*(sj14));
9727 IkReal x2220=((cj11)*(cj13));
9728 IkReal x2221=((sj11)*(sj13));
9729 IkReal x2222=((cj11)*(sj13));
9730 IkReal x2223=((IkReal(1.00000000000000))*(cj14)*(r20));
9731 if( IKabs(((gconst60)*(((((IkReal(-1.00000000000000))*(x2222)*(x2223)))+(((r22)*(x2220)))+(((IkReal(-1.00000000000000))*(r22)*(x2221)))+(((x2219)*(x2222)))+(((IkReal(-1.00000000000000))*(x2218)*(x2223)))+(((x2218)*(x2219))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst60)*(((((r22)*(x2222)))+(((IkReal(-1.00000000000000))*(x2219)*(x2220)))+(((IkReal(-1.00000000000000))*(x2221)*(x2223)))+(((x2219)*(x2221)))+(((r22)*(x2218)))+(((cj14)*(r20)*(x2220))))))) < IKFAST_ATAN2_MAGTHRESH )
9732  continue;
9733 j10array[0]=IKatan2(((gconst60)*(((((IkReal(-1.00000000000000))*(x2222)*(x2223)))+(((r22)*(x2220)))+(((IkReal(-1.00000000000000))*(r22)*(x2221)))+(((x2219)*(x2222)))+(((IkReal(-1.00000000000000))*(x2218)*(x2223)))+(((x2218)*(x2219)))))), ((gconst60)*(((((r22)*(x2222)))+(((IkReal(-1.00000000000000))*(x2219)*(x2220)))+(((IkReal(-1.00000000000000))*(x2221)*(x2223)))+(((x2219)*(x2221)))+(((r22)*(x2218)))+(((cj14)*(r20)*(x2220)))))));
9734 sj10array[0]=IKsin(j10array[0]);
9735 cj10array[0]=IKcos(j10array[0]);
9736 if( j10array[0] > IKPI )
9737 {
9738  j10array[0]-=IK2PI;
9739 }
9740 else if( j10array[0] < -IKPI )
9741 { j10array[0]+=IK2PI;
9742 }
9743 j10valid[0] = true;
9744 for(int ij10 = 0; ij10 < 1; ++ij10)
9745 {
9746 if( !j10valid[ij10] )
9747 {
9748  continue;
9749 }
9750 _ij10[0] = ij10; _ij10[1] = -1;
9751 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
9752 {
9753 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
9754 {
9755  j10valid[iij10]=false; _ij10[1] = iij10; break;
9756 }
9757 }
9758 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
9759 {
9760 IkReal evalcond[4];
9761 IkReal x2224=IKsin(j10);
9762 IkReal x2225=IKcos(j10);
9763 IkReal x2226=((IkReal(1.00000000000000))*(sj13));
9764 IkReal x2227=((r11)*(sj9));
9765 IkReal x2228=((cj9)*(r01));
9766 IkReal x2229=((r21)*(sj14));
9767 IkReal x2230=((cj9)*(r02));
9768 IkReal x2231=((sj13)*(sj9));
9769 IkReal x2232=((cj14)*(r10));
9770 IkReal x2233=((IkReal(1.00000000000000))*(cj13));
9771 IkReal x2234=((cj14)*(r20));
9772 IkReal x2235=((cj11)*(x2224));
9773 IkReal x2236=((sj11)*(x2225));
9774 IkReal x2237=((sj14)*(x2233));
9775 IkReal x2238=((sj11)*(x2224));
9776 IkReal x2239=((cj11)*(x2225));
9777 IkReal x2240=((cj14)*(cj9)*(r00));
9778 IkReal x2241=((x2235)+(x2236));
9779 evalcond[0]=((((cj13)*(x2229)))+(x2238)+(((IkReal(-1.00000000000000))*(x2239)))+(((IkReal(-1.00000000000000))*(r22)*(x2226)))+(((IkReal(-1.00000000000000))*(x2233)*(x2234))));
9780 evalcond[1]=((x2241)+(((IkReal(-1.00000000000000))*(x2226)*(x2234)))+(((sj13)*(x2229)))+(((cj13)*(r22))));
9781 evalcond[2]=((x2241)+(((sj13)*(x2230)))+(((IkReal(-1.00000000000000))*(x2227)*(x2237)))+(((cj13)*(x2240)))+(((IkReal(-1.00000000000000))*(x2228)*(x2237)))+(((cj13)*(sj9)*(x2232)))+(((r12)*(x2231))));
9782 evalcond[3]=((x2239)+(((IkReal(-1.00000000000000))*(x2238)))+(((x2231)*(x2232)))+(((IkReal(-1.00000000000000))*(x2230)*(x2233)))+(((IkReal(-1.00000000000000))*(sj14)*(x2226)*(x2227)))+(((IkReal(-1.00000000000000))*(sj14)*(x2226)*(x2228)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2233)))+(((sj13)*(x2240))));
9783 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
9784 {
9785 continue;
9786 }
9787 }
9788 
9789 {
9790 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9791 vinfos[0].jointtype = 1;
9792 vinfos[0].foffset = j9;
9793 vinfos[0].indices[0] = _ij9[0];
9794 vinfos[0].indices[1] = _ij9[1];
9795 vinfos[0].maxsolutions = _nj9;
9796 vinfos[1].jointtype = 1;
9797 vinfos[1].foffset = j10;
9798 vinfos[1].indices[0] = _ij10[0];
9799 vinfos[1].indices[1] = _ij10[1];
9800 vinfos[1].maxsolutions = _nj10;
9801 vinfos[2].jointtype = 1;
9802 vinfos[2].foffset = j11;
9803 vinfos[2].indices[0] = _ij11[0];
9804 vinfos[2].indices[1] = _ij11[1];
9805 vinfos[2].maxsolutions = _nj11;
9806 vinfos[3].jointtype = 1;
9807 vinfos[3].foffset = j12;
9808 vinfos[3].indices[0] = _ij12[0];
9809 vinfos[3].indices[1] = _ij12[1];
9810 vinfos[3].maxsolutions = _nj12;
9811 vinfos[4].jointtype = 1;
9812 vinfos[4].foffset = j13;
9813 vinfos[4].indices[0] = _ij13[0];
9814 vinfos[4].indices[1] = _ij13[1];
9815 vinfos[4].maxsolutions = _nj13;
9816 vinfos[5].jointtype = 1;
9817 vinfos[5].foffset = j14;
9818 vinfos[5].indices[0] = _ij14[0];
9819 vinfos[5].indices[1] = _ij14[1];
9820 vinfos[5].maxsolutions = _nj14;
9821 std::vector<int> vfree(0);
9822 solutions.AddSolution(vinfos,vfree);
9823 }
9824 }
9825 }
9826 
9827 }
9828 
9829 }
9830 }
9831 }
9832 
9833 } else
9834 {
9835 IkReal x2242=((cj13)*(sj9));
9836 IkReal x2243=((r01)*(sj14));
9837 IkReal x2244=((cj9)*(sj13));
9838 IkReal x2245=((sj13)*(sj9));
9839 IkReal x2246=((cj14)*(r01));
9840 IkReal x2247=((cj14)*(r10));
9841 IkReal x2248=((cj13)*(cj9));
9842 IkReal x2249=((IkReal(1.00000000000000))*(cj9));
9843 IkReal x2250=((sj14)*(sj9));
9844 IkReal x2251=((IkReal(1.00000000000000))*(cj14)*(sj9));
9845 IkReal x2252=((sj14)*(x2249));
9846 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
9847 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
9848 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
9849 evalcond[3]=((IkReal(-1.00000000000000))+(((sj9)*(x2246)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2249)))+(((IkReal(-1.00000000000000))*(r10)*(x2252)))+(((r00)*(x2250))));
9850 evalcond[4]=((((IkReal(-1.00000000000000))*(r10)*(x2250)))+(((IkReal(-1.00000000000000))*(r00)*(x2252)))+(((IkReal(-1.00000000000000))*(r11)*(x2251)))+(((IkReal(-1.00000000000000))*(x2246)*(x2249))));
9851 evalcond[5]=((((IkReal(-1.00000000000000))*(r11)*(sj14)*(x2248)))+(((IkReal(-1.00000000000000))*(r02)*(x2245)))+(((x2247)*(x2248)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x2242)))+(((x2242)*(x2243)))+(((r12)*(x2244))));
9852 evalcond[6]=((((IkReal(-1.00000000000000))*(r11)*(sj14)*(x2244)))+(((x2243)*(x2245)))+(((x2244)*(x2247)))+(((IkReal(-1.00000000000000))*(r12)*(x2248)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x2245)))+(((r02)*(x2242))));
9853 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 )
9854 {
9855 {
9856 IkReal j11array[1], cj11array[1], sj11array[1];
9857 bool j11valid[1]={false};
9858 _nj11 = 1;
9859 IkReal x2253=((IkReal(4.00000000000000))*(cj13));
9860 IkReal x2254=((npy)*(sj14));
9861 IkReal x2255=((cj14)*(npx));
9862 IkReal x2256=((IkReal(4.00000000000000))*(sj13));
9863 if( IKabs(((IkReal(0.120000000000000))+(((x2255)*(x2256)))+(((IkReal(-1.00000000000000))*(npz)*(x2253)))+(((IkReal(-1.00000000000000))*(x2254)*(x2256)))+(((IkReal(-0.360000000000000))*(sj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((x2253)*(x2255)))+(((IkReal(-1.00000000000000))*(x2253)*(x2254)))+(((npz)*(x2256)))+(((IkReal(-0.360000000000000))*(cj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((x2255)*(x2256)))+(((IkReal(-1.00000000000000))*(npz)*(x2253)))+(((IkReal(-1.00000000000000))*(x2254)*(x2256)))+(((IkReal(-0.360000000000000))*(sj13)))))+IKsqr(((IkReal(-0.940000000000000))+(((x2253)*(x2255)))+(((IkReal(-1.00000000000000))*(x2253)*(x2254)))+(((npz)*(x2256)))+(((IkReal(-0.360000000000000))*(cj13)))))-1) <= IKFAST_SINCOS_THRESH )
9864  continue;
9865 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((x2255)*(x2256)))+(((IkReal(-1.00000000000000))*(npz)*(x2253)))+(((IkReal(-1.00000000000000))*(x2254)*(x2256)))+(((IkReal(-0.360000000000000))*(sj13)))), ((IkReal(-0.940000000000000))+(((x2253)*(x2255)))+(((IkReal(-1.00000000000000))*(x2253)*(x2254)))+(((npz)*(x2256)))+(((IkReal(-0.360000000000000))*(cj13)))));
9866 sj11array[0]=IKsin(j11array[0]);
9867 cj11array[0]=IKcos(j11array[0]);
9868 if( j11array[0] > IKPI )
9869 {
9870  j11array[0]-=IK2PI;
9871 }
9872 else if( j11array[0] < -IKPI )
9873 { j11array[0]+=IK2PI;
9874 }
9875 j11valid[0] = true;
9876 for(int ij11 = 0; ij11 < 1; ++ij11)
9877 {
9878 if( !j11valid[ij11] )
9879 {
9880  continue;
9881 }
9882 _ij11[0] = ij11; _ij11[1] = -1;
9883 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
9884 {
9885 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
9886 {
9887  j11valid[iij11]=false; _ij11[1] = iij11; break;
9888 }
9889 }
9890 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
9891 {
9892 IkReal evalcond[2];
9893 IkReal x2257=((IkReal(1.00000000000000))*(sj13));
9894 IkReal x2258=((cj14)*(npx));
9895 IkReal x2259=((npy)*(sj14));
9896 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x2257)))+(((IkReal(-1.00000000000000))*(cj13)*(x2258)))+(((cj13)*(x2259))));
9897 evalcond[1]=((IkReal(-0.0300000000000000))+(((IkReal(-1.00000000000000))*(x2257)*(x2258)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((sj13)*(x2259)))+(((IkReal(0.250000000000000))*(IKsin(j11)))));
9898 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
9899 {
9900 continue;
9901 }
9902 }
9903 
9904 {
9905 IkReal dummyeval[1];
9906 IkReal gconst62;
9907 gconst62=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
9908 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
9909 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
9910 {
9911 {
9912 IkReal dummyeval[1];
9913 IkReal gconst63;
9914 gconst63=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
9915 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
9916 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
9917 {
9918 continue;
9919 
9920 } else
9921 {
9922 {
9923 IkReal j10array[1], cj10array[1], sj10array[1];
9924 bool j10valid[1]={false};
9925 _nj10 = 1;
9926 IkReal x2260=((cj13)*(sj14));
9927 IkReal x2261=((IkReal(1.00000000000000))*(cj11));
9928 IkReal x2262=((r11)*(sj9));
9929 IkReal x2263=((IkReal(1.00000000000000))*(sj11));
9930 IkReal x2264=((cj13)*(cj14));
9931 IkReal x2265=((cj11)*(sj13));
9932 IkReal x2266=((r12)*(sj9));
9933 IkReal x2267=((sj11)*(sj13));
9934 IkReal x2268=((cj9)*(r02));
9935 IkReal x2269=((cj9)*(r01));
9936 IkReal x2270=((r10)*(sj9));
9937 IkReal x2271=((cj9)*(r00));
9938 if( IKabs(((gconst63)*(((((r21)*(sj11)*(x2260)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2263)))+(((IkReal(-1.00000000000000))*(x2260)*(x2261)*(x2269)))+(((IkReal(-1.00000000000000))*(x2260)*(x2261)*(x2262)))+(((IkReal(-1.00000000000000))*(r20)*(x2263)*(x2264)))+(((cj11)*(x2264)*(x2271)))+(((cj11)*(x2264)*(x2270)))+(((x2265)*(x2266)))+(((x2265)*(x2268))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst63)*(((((IkReal(-1.00000000000000))*(x2260)*(x2262)*(x2263)))+(((IkReal(-1.00000000000000))*(x2260)*(x2263)*(x2269)))+(((r22)*(x2265)))+(((sj11)*(x2264)*(x2270)))+(((sj11)*(x2264)*(x2271)))+(((IkReal(-1.00000000000000))*(r21)*(x2260)*(x2261)))+(((x2266)*(x2267)))+(((x2267)*(x2268)))+(((cj11)*(r20)*(x2264))))))) < IKFAST_ATAN2_MAGTHRESH )
9939  continue;
9940 j10array[0]=IKatan2(((gconst63)*(((((r21)*(sj11)*(x2260)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2263)))+(((IkReal(-1.00000000000000))*(x2260)*(x2261)*(x2269)))+(((IkReal(-1.00000000000000))*(x2260)*(x2261)*(x2262)))+(((IkReal(-1.00000000000000))*(r20)*(x2263)*(x2264)))+(((cj11)*(x2264)*(x2271)))+(((cj11)*(x2264)*(x2270)))+(((x2265)*(x2266)))+(((x2265)*(x2268)))))), ((gconst63)*(((((IkReal(-1.00000000000000))*(x2260)*(x2262)*(x2263)))+(((IkReal(-1.00000000000000))*(x2260)*(x2263)*(x2269)))+(((r22)*(x2265)))+(((sj11)*(x2264)*(x2270)))+(((sj11)*(x2264)*(x2271)))+(((IkReal(-1.00000000000000))*(r21)*(x2260)*(x2261)))+(((x2266)*(x2267)))+(((x2267)*(x2268)))+(((cj11)*(r20)*(x2264)))))));
9941 sj10array[0]=IKsin(j10array[0]);
9942 cj10array[0]=IKcos(j10array[0]);
9943 if( j10array[0] > IKPI )
9944 {
9945  j10array[0]-=IK2PI;
9946 }
9947 else if( j10array[0] < -IKPI )
9948 { j10array[0]+=IK2PI;
9949 }
9950 j10valid[0] = true;
9951 for(int ij10 = 0; ij10 < 1; ++ij10)
9952 {
9953 if( !j10valid[ij10] )
9954 {
9955  continue;
9956 }
9957 _ij10[0] = ij10; _ij10[1] = -1;
9958 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
9959 {
9960 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
9961 {
9962  j10valid[iij10]=false; _ij10[1] = iij10; break;
9963 }
9964 }
9965 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
9966 {
9967 IkReal evalcond[4];
9968 IkReal x2272=IKsin(j10);
9969 IkReal x2273=IKcos(j10);
9970 IkReal x2274=((IkReal(1.00000000000000))*(sj13));
9971 IkReal x2275=((r11)*(sj9));
9972 IkReal x2276=((cj9)*(r01));
9973 IkReal x2277=((IkReal(1.00000000000000))*(cj11));
9974 IkReal x2278=((r21)*(sj14));
9975 IkReal x2279=((cj9)*(r02));
9976 IkReal x2280=((sj13)*(sj9));
9977 IkReal x2281=((cj14)*(r10));
9978 IkReal x2282=((IkReal(1.00000000000000))*(cj13));
9979 IkReal x2283=((cj14)*(r20));
9980 IkReal x2284=((sj11)*(x2272));
9981 IkReal x2285=((sj14)*(x2282));
9982 IkReal x2286=((sj11)*(x2273));
9983 IkReal x2287=((cj14)*(cj9)*(r00));
9984 IkReal x2288=((x2273)*(x2277));
9985 evalcond[0]=((((IkReal(-1.00000000000000))*(x2282)*(x2283)))+(x2284)+(((cj13)*(x2278)))+(((IkReal(-1.00000000000000))*(x2288)))+(((IkReal(-1.00000000000000))*(r22)*(x2274))));
9986 evalcond[1]=((((IkReal(-1.00000000000000))*(x2286)))+(((sj13)*(x2278)))+(((IkReal(-1.00000000000000))*(x2274)*(x2283)))+(((IkReal(-1.00000000000000))*(x2272)*(x2277)))+(((cj13)*(r22))));
9987 evalcond[2]=((((IkReal(-1.00000000000000))*(x2276)*(x2285)))+(x2286)+(((cj13)*(sj9)*(x2281)))+(((r12)*(x2280)))+(((cj11)*(x2272)))+(((sj13)*(x2279)))+(((cj13)*(x2287)))+(((IkReal(-1.00000000000000))*(x2275)*(x2285))));
9988 evalcond[3]=((x2284)+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2282)))+(((IkReal(-1.00000000000000))*(x2288)))+(((x2280)*(x2281)))+(((IkReal(-1.00000000000000))*(x2279)*(x2282)))+(((IkReal(-1.00000000000000))*(sj14)*(x2274)*(x2275)))+(((IkReal(-1.00000000000000))*(sj14)*(x2274)*(x2276)))+(((sj13)*(x2287))));
9989 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
9990 {
9991 continue;
9992 }
9993 }
9994 
9995 {
9996 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
9997 vinfos[0].jointtype = 1;
9998 vinfos[0].foffset = j9;
9999 vinfos[0].indices[0] = _ij9[0];
10000 vinfos[0].indices[1] = _ij9[1];
10001 vinfos[0].maxsolutions = _nj9;
10002 vinfos[1].jointtype = 1;
10003 vinfos[1].foffset = j10;
10004 vinfos[1].indices[0] = _ij10[0];
10005 vinfos[1].indices[1] = _ij10[1];
10006 vinfos[1].maxsolutions = _nj10;
10007 vinfos[2].jointtype = 1;
10008 vinfos[2].foffset = j11;
10009 vinfos[2].indices[0] = _ij11[0];
10010 vinfos[2].indices[1] = _ij11[1];
10011 vinfos[2].maxsolutions = _nj11;
10012 vinfos[3].jointtype = 1;
10013 vinfos[3].foffset = j12;
10014 vinfos[3].indices[0] = _ij12[0];
10015 vinfos[3].indices[1] = _ij12[1];
10016 vinfos[3].maxsolutions = _nj12;
10017 vinfos[4].jointtype = 1;
10018 vinfos[4].foffset = j13;
10019 vinfos[4].indices[0] = _ij13[0];
10020 vinfos[4].indices[1] = _ij13[1];
10021 vinfos[4].maxsolutions = _nj13;
10022 vinfos[5].jointtype = 1;
10023 vinfos[5].foffset = j14;
10024 vinfos[5].indices[0] = _ij14[0];
10025 vinfos[5].indices[1] = _ij14[1];
10026 vinfos[5].maxsolutions = _nj14;
10027 std::vector<int> vfree(0);
10028 solutions.AddSolution(vinfos,vfree);
10029 }
10030 }
10031 }
10032 
10033 }
10034 
10035 }
10036 
10037 } else
10038 {
10039 {
10040 IkReal j10array[1], cj10array[1], sj10array[1];
10041 bool j10valid[1]={false};
10042 _nj10 = 1;
10043 IkReal x2289=((cj13)*(r22));
10044 IkReal x2290=((IkReal(1.00000000000000))*(sj11));
10045 IkReal x2291=((IkReal(1.00000000000000))*(cj11));
10046 IkReal x2292=((r22)*(sj13));
10047 IkReal x2293=((cj14)*(r20));
10048 IkReal x2294=((cj13)*(r21)*(sj14));
10049 IkReal x2295=((r21)*(sj13)*(sj14));
10050 if( IKabs(((gconst62)*(((((sj11)*(x2292)))+(((cj13)*(sj11)*(x2293)))+(((IkReal(-1.00000000000000))*(sj13)*(x2291)*(x2293)))+(((cj11)*(x2295)))+(((IkReal(-1.00000000000000))*(x2290)*(x2294)))+(((cj11)*(x2289))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst62)*(((((sj11)*(x2295)))+(((sj11)*(x2289)))+(((IkReal(-1.00000000000000))*(cj13)*(x2291)*(x2293)))+(((IkReal(-1.00000000000000))*(x2291)*(x2292)))+(((cj11)*(x2294)))+(((IkReal(-1.00000000000000))*(sj13)*(x2290)*(x2293))))))) < IKFAST_ATAN2_MAGTHRESH )
10051  continue;
10052 j10array[0]=IKatan2(((gconst62)*(((((sj11)*(x2292)))+(((cj13)*(sj11)*(x2293)))+(((IkReal(-1.00000000000000))*(sj13)*(x2291)*(x2293)))+(((cj11)*(x2295)))+(((IkReal(-1.00000000000000))*(x2290)*(x2294)))+(((cj11)*(x2289)))))), ((gconst62)*(((((sj11)*(x2295)))+(((sj11)*(x2289)))+(((IkReal(-1.00000000000000))*(cj13)*(x2291)*(x2293)))+(((IkReal(-1.00000000000000))*(x2291)*(x2292)))+(((cj11)*(x2294)))+(((IkReal(-1.00000000000000))*(sj13)*(x2290)*(x2293)))))));
10053 sj10array[0]=IKsin(j10array[0]);
10054 cj10array[0]=IKcos(j10array[0]);
10055 if( j10array[0] > IKPI )
10056 {
10057  j10array[0]-=IK2PI;
10058 }
10059 else if( j10array[0] < -IKPI )
10060 { j10array[0]+=IK2PI;
10061 }
10062 j10valid[0] = true;
10063 for(int ij10 = 0; ij10 < 1; ++ij10)
10064 {
10065 if( !j10valid[ij10] )
10066 {
10067  continue;
10068 }
10069 _ij10[0] = ij10; _ij10[1] = -1;
10070 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10071 {
10072 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10073 {
10074  j10valid[iij10]=false; _ij10[1] = iij10; break;
10075 }
10076 }
10077 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10078 {
10079 IkReal evalcond[4];
10080 IkReal x2296=IKsin(j10);
10081 IkReal x2297=IKcos(j10);
10082 IkReal x2298=((IkReal(1.00000000000000))*(sj13));
10083 IkReal x2299=((r11)*(sj9));
10084 IkReal x2300=((cj9)*(r01));
10085 IkReal x2301=((IkReal(1.00000000000000))*(cj11));
10086 IkReal x2302=((r21)*(sj14));
10087 IkReal x2303=((cj9)*(r02));
10088 IkReal x2304=((sj13)*(sj9));
10089 IkReal x2305=((cj14)*(r10));
10090 IkReal x2306=((IkReal(1.00000000000000))*(cj13));
10091 IkReal x2307=((cj14)*(r20));
10092 IkReal x2308=((sj11)*(x2296));
10093 IkReal x2309=((sj14)*(x2306));
10094 IkReal x2310=((sj11)*(x2297));
10095 IkReal x2311=((cj14)*(cj9)*(r00));
10096 IkReal x2312=((x2297)*(x2301));
10097 evalcond[0]=((((IkReal(-1.00000000000000))*(x2306)*(x2307)))+(((cj13)*(x2302)))+(x2308)+(((IkReal(-1.00000000000000))*(r22)*(x2298)))+(((IkReal(-1.00000000000000))*(x2312))));
10098 evalcond[1]=((((IkReal(-1.00000000000000))*(x2310)))+(((sj13)*(x2302)))+(((IkReal(-1.00000000000000))*(x2296)*(x2301)))+(((IkReal(-1.00000000000000))*(x2298)*(x2307)))+(((cj13)*(r22))));
10099 evalcond[2]=((((r12)*(x2304)))+(((sj13)*(x2303)))+(((IkReal(-1.00000000000000))*(x2300)*(x2309)))+(x2310)+(((IkReal(-1.00000000000000))*(x2299)*(x2309)))+(((cj11)*(x2296)))+(((cj13)*(x2311)))+(((cj13)*(sj9)*(x2305))));
10100 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2306)))+(((x2304)*(x2305)))+(x2308)+(((IkReal(-1.00000000000000))*(x2303)*(x2306)))+(((IkReal(-1.00000000000000))*(sj14)*(x2298)*(x2299)))+(((sj13)*(x2311)))+(((IkReal(-1.00000000000000))*(sj14)*(x2298)*(x2300)))+(((IkReal(-1.00000000000000))*(x2312))));
10101 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10102 {
10103 continue;
10104 }
10105 }
10106 
10107 {
10108 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10109 vinfos[0].jointtype = 1;
10110 vinfos[0].foffset = j9;
10111 vinfos[0].indices[0] = _ij9[0];
10112 vinfos[0].indices[1] = _ij9[1];
10113 vinfos[0].maxsolutions = _nj9;
10114 vinfos[1].jointtype = 1;
10115 vinfos[1].foffset = j10;
10116 vinfos[1].indices[0] = _ij10[0];
10117 vinfos[1].indices[1] = _ij10[1];
10118 vinfos[1].maxsolutions = _nj10;
10119 vinfos[2].jointtype = 1;
10120 vinfos[2].foffset = j11;
10121 vinfos[2].indices[0] = _ij11[0];
10122 vinfos[2].indices[1] = _ij11[1];
10123 vinfos[2].maxsolutions = _nj11;
10124 vinfos[3].jointtype = 1;
10125 vinfos[3].foffset = j12;
10126 vinfos[3].indices[0] = _ij12[0];
10127 vinfos[3].indices[1] = _ij12[1];
10128 vinfos[3].maxsolutions = _nj12;
10129 vinfos[4].jointtype = 1;
10130 vinfos[4].foffset = j13;
10131 vinfos[4].indices[0] = _ij13[0];
10132 vinfos[4].indices[1] = _ij13[1];
10133 vinfos[4].maxsolutions = _nj13;
10134 vinfos[5].jointtype = 1;
10135 vinfos[5].foffset = j14;
10136 vinfos[5].indices[0] = _ij14[0];
10137 vinfos[5].indices[1] = _ij14[1];
10138 vinfos[5].maxsolutions = _nj14;
10139 std::vector<int> vfree(0);
10140 solutions.AddSolution(vinfos,vfree);
10141 }
10142 }
10143 }
10144 
10145 }
10146 
10147 }
10148 }
10149 }
10150 
10151 } else
10152 {
10153 if( 1 )
10154 {
10155 continue;
10156 
10157 } else
10158 {
10159 }
10160 }
10161 }
10162 }
10163 }
10164 }
10165 
10166 } else
10167 {
10168 {
10169 IkReal j11array[1], cj11array[1], sj11array[1];
10170 bool j11valid[1]={false};
10171 _nj11 = 1;
10172 IkReal x2313=((IkReal(4.00000000000000))*(cj13));
10173 IkReal x2314=((npy)*(sj14));
10174 IkReal x2315=((IkReal(4.00000000000000))*(sj13));
10175 IkReal x2316=((cj14)*(npx));
10176 if( IKabs(((((IKabs(cj12) != 0)?((IkReal)1/(cj12)):(IkReal)1.0e30))*(((((npz)*(x2313)))+(((IkReal(0.380000000000000))*(sj12)))+(((IkReal(-1.00000000000000))*(x2315)*(x2316)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(0.120000000000000))*(cj12)))+(((x2314)*(x2315))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((npz)*(x2315)))+(((IkReal(-1.00000000000000))*(x2313)*(x2314)))+(((IkReal(-0.360000000000000))*(cj13)))+(((x2313)*(x2316))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(cj12) != 0)?((IkReal)1/(cj12)):(IkReal)1.0e30))*(((((npz)*(x2313)))+(((IkReal(0.380000000000000))*(sj12)))+(((IkReal(-1.00000000000000))*(x2315)*(x2316)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(0.120000000000000))*(cj12)))+(((x2314)*(x2315)))))))+IKsqr(((IkReal(-0.940000000000000))+(((npz)*(x2315)))+(((IkReal(-1.00000000000000))*(x2313)*(x2314)))+(((IkReal(-0.360000000000000))*(cj13)))+(((x2313)*(x2316)))))-1) <= IKFAST_SINCOS_THRESH )
10177  continue;
10178 j11array[0]=IKatan2(((((IKabs(cj12) != 0)?((IkReal)1/(cj12)):(IkReal)1.0e30))*(((((npz)*(x2313)))+(((IkReal(0.380000000000000))*(sj12)))+(((IkReal(-1.00000000000000))*(x2315)*(x2316)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(0.120000000000000))*(cj12)))+(((x2314)*(x2315)))))), ((IkReal(-0.940000000000000))+(((npz)*(x2315)))+(((IkReal(-1.00000000000000))*(x2313)*(x2314)))+(((IkReal(-0.360000000000000))*(cj13)))+(((x2313)*(x2316)))));
10179 sj11array[0]=IKsin(j11array[0]);
10180 cj11array[0]=IKcos(j11array[0]);
10181 if( j11array[0] > IKPI )
10182 {
10183  j11array[0]-=IK2PI;
10184 }
10185 else if( j11array[0] < -IKPI )
10186 { j11array[0]+=IK2PI;
10187 }
10188 j11valid[0] = true;
10189 for(int ij11 = 0; ij11 < 1; ++ij11)
10190 {
10191 if( !j11valid[ij11] )
10192 {
10193  continue;
10194 }
10195 _ij11[0] = ij11; _ij11[1] = -1;
10196 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
10197 {
10198 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
10199 {
10200  j11valid[iij11]=false; _ij11[1] = iij11; break;
10201 }
10202 }
10203 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
10204 {
10205 IkReal evalcond[3];
10206 IkReal x2317=IKsin(j11);
10207 IkReal x2318=((IkReal(1.00000000000000))*(sj13));
10208 IkReal x2319=((cj14)*(npx));
10209 IkReal x2320=((npy)*(sj14));
10210 IkReal x2321=((IkReal(0.250000000000000))*(x2317));
10211 evalcond[0]=((((IkReal(0.0950000000000000))*(cj12)))+(((cj14)*(npy)))+(((IkReal(-0.0300000000000000))*(sj12)))+(((npx)*(sj14)))+(((sj12)*(x2321))));
10212 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(x2319)))+(((IkReal(-1.00000000000000))*(npz)*(x2318)))+(((cj13)*(x2320))));
10213 evalcond[2]=((((IkReal(-1.00000000000000))*(x2318)*(x2319)))+(((IkReal(0.0950000000000000))*(sj12)))+(((cj13)*(npz)))+(((IkReal(0.0300000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(cj12)*(x2321)))+(((IkReal(0.0900000000000000))*(sj13)))+(((sj13)*(x2320))));
10214 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
10215 {
10216 continue;
10217 }
10218 }
10219 
10220 {
10221 IkReal dummyeval[1];
10222 IkReal gconst46;
10223 gconst46=IKsign(((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11))))));
10224 dummyeval[0]=((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11)))));
10225 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10226 {
10227 {
10228 IkReal dummyeval[1];
10229 IkReal gconst47;
10230 gconst47=IKsign(((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11))))));
10231 dummyeval[0]=((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11)))));
10232 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10233 {
10234 {
10235 IkReal evalcond[9];
10236 IkReal x2322=((r00)*(sj9));
10237 IkReal x2323=((IkReal(1.00000000000000))*(sj13));
10238 IkReal x2324=((cj13)*(sj14));
10239 IkReal x2325=((cj9)*(sj14));
10240 IkReal x2326=((cj13)*(r02));
10241 IkReal x2327=((sj13)*(sj14));
10242 IkReal x2328=((r01)*(sj9));
10243 IkReal x2329=((cj9)*(sj13));
10244 IkReal x2330=((IkReal(1.00000000000000))*(cj9));
10245 IkReal x2331=((cj14)*(r10));
10246 IkReal x2332=((cj14)*(npx));
10247 IkReal x2333=((IkReal(1.00000000000000))*(cj13));
10248 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
10249 evalcond[1]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(sj11))));
10250 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x2332)*(x2333)))+(((npy)*(x2324)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(npz)*(x2323))));
10251 evalcond[3]=((((sj14)*(x2322)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2330)))+(((cj14)*(x2328)))+(((IkReal(-1.00000000000000))*(r10)*(x2325))));
10252 evalcond[4]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2323)))+(((r21)*(x2327)))+(((cj13)*(r22))));
10253 evalcond[5]=((IkReal(0.0950000000000000))+(((npy)*(x2327)))+(((cj13)*(npz)))+(((IkReal(-1.00000000000000))*(x2323)*(x2332)))+(((IkReal(0.0900000000000000))*(sj13))));
10254 evalcond[6]=((((r12)*(x2329)))+(((IkReal(-1.00000000000000))*(cj14)*(x2322)*(x2333)))+(((cj13)*(cj9)*(x2331)))+(((IkReal(-1.00000000000000))*(r11)*(x2324)*(x2330)))+(((x2324)*(x2328)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2323))));
10255 evalcond[7]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(r11)*(x2323)*(x2325)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x2330)))+(((x2329)*(x2331)))+(((x2327)*(x2328)))+(((IkReal(-1.00000000000000))*(cj14)*(x2322)*(x2323)))+(((sj9)*(x2326))));
10256 evalcond[8]=((((sj13)*(sj9)*(x2331)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x2323)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2333)))+(((cj14)*(r00)*(x2329)))+(((IkReal(-1.00000000000000))*(r01)*(x2323)*(x2325)))+(((IkReal(-1.00000000000000))*(x2326)*(x2330))));
10257 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 )
10258 {
10259 {
10260 IkReal dummyeval[1];
10261 IkReal gconst48;
10262 gconst48=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
10263 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
10264 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10265 {
10266 {
10267 IkReal dummyeval[1];
10268 IkReal gconst49;
10269 gconst49=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
10270 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
10271 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10272 {
10273 continue;
10274 
10275 } else
10276 {
10277 {
10278 IkReal j10array[1], cj10array[1], sj10array[1];
10279 bool j10valid[1]={false};
10280 _nj10 = 1;
10281 IkReal x2334=((IkReal(1.00000000000000))*(cj11));
10282 IkReal x2335=((sj11)*(sj14));
10283 IkReal x2336=((r10)*(sj9));
10284 IkReal x2337=((cj9)*(r00));
10285 IkReal x2338=((cj14)*(sj11));
10286 IkReal x2339=((r11)*(sj9));
10287 IkReal x2340=((cj14)*(cj9)*(r01));
10288 if( IKabs(((gconst49)*(((((cj11)*(cj14)*(r21)))+(((cj9)*(r01)*(x2338)))+(((cj11)*(r20)*(sj14)))+(((x2335)*(x2337)))+(((x2335)*(x2336)))+(((x2338)*(x2339))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst49)*(((((r21)*(x2338)))+(((IkReal(-1.00000000000000))*(sj14)*(x2334)*(x2337)))+(((IkReal(-1.00000000000000))*(sj14)*(x2334)*(x2336)))+(((IkReal(-1.00000000000000))*(x2334)*(x2340)))+(((IkReal(-1.00000000000000))*(cj14)*(x2334)*(x2339)))+(((r20)*(x2335))))))) < IKFAST_ATAN2_MAGTHRESH )
10289  continue;
10290 j10array[0]=IKatan2(((gconst49)*(((((cj11)*(cj14)*(r21)))+(((cj9)*(r01)*(x2338)))+(((cj11)*(r20)*(sj14)))+(((x2335)*(x2337)))+(((x2335)*(x2336)))+(((x2338)*(x2339)))))), ((gconst49)*(((((r21)*(x2338)))+(((IkReal(-1.00000000000000))*(sj14)*(x2334)*(x2337)))+(((IkReal(-1.00000000000000))*(sj14)*(x2334)*(x2336)))+(((IkReal(-1.00000000000000))*(x2334)*(x2340)))+(((IkReal(-1.00000000000000))*(cj14)*(x2334)*(x2339)))+(((r20)*(x2335)))))));
10291 sj10array[0]=IKsin(j10array[0]);
10292 cj10array[0]=IKcos(j10array[0]);
10293 if( j10array[0] > IKPI )
10294 {
10295  j10array[0]-=IK2PI;
10296 }
10297 else if( j10array[0] < -IKPI )
10298 { j10array[0]+=IK2PI;
10299 }
10300 j10valid[0] = true;
10301 for(int ij10 = 0; ij10 < 1; ++ij10)
10302 {
10303 if( !j10valid[ij10] )
10304 {
10305  continue;
10306 }
10307 _ij10[0] = ij10; _ij10[1] = -1;
10308 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10309 {
10310 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10311 {
10312  j10valid[iij10]=false; _ij10[1] = iij10; break;
10313 }
10314 }
10315 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10316 {
10317 IkReal evalcond[4];
10318 IkReal x2341=IKsin(j10);
10319 IkReal x2342=IKcos(j10);
10320 IkReal x2343=((cj13)*(sj14));
10321 IkReal x2344=((cj13)*(cj14));
10322 IkReal x2345=((r10)*(sj9));
10323 IkReal x2346=((IkReal(1.00000000000000))*(cj9));
10324 IkReal x2347=((sj11)*(x2341));
10325 IkReal x2348=((IkReal(1.00000000000000))*(x2342));
10326 IkReal x2349=((cj11)*(x2341));
10327 IkReal x2350=((IkReal(1.00000000000000))*(r11)*(sj9));
10328 IkReal x2351=((cj11)*(x2348));
10329 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x2349)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x2348))));
10330 evalcond[1]=((x2347)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x2344)))+(((r21)*(x2343)))+(((IkReal(-1.00000000000000))*(x2351))));
10331 evalcond[2]=((x2347)+(((IkReal(-1.00000000000000))*(sj14)*(x2345)))+(((IkReal(-1.00000000000000))*(cj14)*(x2350)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2346)))+(((IkReal(-1.00000000000000))*(x2351)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2346))));
10332 evalcond[3]=((x2349)+(((cj9)*(r00)*(x2344)))+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x2343)*(x2346)))+(((x2344)*(x2345)))+(((IkReal(-1.00000000000000))*(x2343)*(x2350)))+(((r12)*(sj13)*(sj9)))+(((sj11)*(x2342))));
10333 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10334 {
10335 continue;
10336 }
10337 }
10338 
10339 {
10340 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10341 vinfos[0].jointtype = 1;
10342 vinfos[0].foffset = j9;
10343 vinfos[0].indices[0] = _ij9[0];
10344 vinfos[0].indices[1] = _ij9[1];
10345 vinfos[0].maxsolutions = _nj9;
10346 vinfos[1].jointtype = 1;
10347 vinfos[1].foffset = j10;
10348 vinfos[1].indices[0] = _ij10[0];
10349 vinfos[1].indices[1] = _ij10[1];
10350 vinfos[1].maxsolutions = _nj10;
10351 vinfos[2].jointtype = 1;
10352 vinfos[2].foffset = j11;
10353 vinfos[2].indices[0] = _ij11[0];
10354 vinfos[2].indices[1] = _ij11[1];
10355 vinfos[2].maxsolutions = _nj11;
10356 vinfos[3].jointtype = 1;
10357 vinfos[3].foffset = j12;
10358 vinfos[3].indices[0] = _ij12[0];
10359 vinfos[3].indices[1] = _ij12[1];
10360 vinfos[3].maxsolutions = _nj12;
10361 vinfos[4].jointtype = 1;
10362 vinfos[4].foffset = j13;
10363 vinfos[4].indices[0] = _ij13[0];
10364 vinfos[4].indices[1] = _ij13[1];
10365 vinfos[4].maxsolutions = _nj13;
10366 vinfos[5].jointtype = 1;
10367 vinfos[5].foffset = j14;
10368 vinfos[5].indices[0] = _ij14[0];
10369 vinfos[5].indices[1] = _ij14[1];
10370 vinfos[5].maxsolutions = _nj14;
10371 std::vector<int> vfree(0);
10372 solutions.AddSolution(vinfos,vfree);
10373 }
10374 }
10375 }
10376 
10377 }
10378 
10379 }
10380 
10381 } else
10382 {
10383 {
10384 IkReal j10array[1], cj10array[1], sj10array[1];
10385 bool j10valid[1]={false};
10386 _nj10 = 1;
10387 IkReal x2352=((cj11)*(r20));
10388 IkReal x2353=((IkReal(1.00000000000000))*(cj13));
10389 IkReal x2354=((r21)*(sj14));
10390 IkReal x2355=((r22)*(sj13));
10391 IkReal x2356=((r20)*(sj11));
10392 IkReal x2357=((cj14)*(r21));
10393 if( IKabs(((gconst48)*(((((cj11)*(x2357)))+(((IkReal(-1.00000000000000))*(sj11)*(x2353)*(x2354)))+(((sj11)*(x2355)))+(((cj13)*(cj14)*(x2356)))+(((sj14)*(x2352))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst48)*(((((sj11)*(x2357)))+(((cj11)*(cj13)*(x2354)))+(((IkReal(-1.00000000000000))*(cj11)*(x2355)))+(((IkReal(-1.00000000000000))*(cj14)*(x2352)*(x2353)))+(((sj14)*(x2356))))))) < IKFAST_ATAN2_MAGTHRESH )
10394  continue;
10395 j10array[0]=IKatan2(((gconst48)*(((((cj11)*(x2357)))+(((IkReal(-1.00000000000000))*(sj11)*(x2353)*(x2354)))+(((sj11)*(x2355)))+(((cj13)*(cj14)*(x2356)))+(((sj14)*(x2352)))))), ((gconst48)*(((((sj11)*(x2357)))+(((cj11)*(cj13)*(x2354)))+(((IkReal(-1.00000000000000))*(cj11)*(x2355)))+(((IkReal(-1.00000000000000))*(cj14)*(x2352)*(x2353)))+(((sj14)*(x2356)))))));
10396 sj10array[0]=IKsin(j10array[0]);
10397 cj10array[0]=IKcos(j10array[0]);
10398 if( j10array[0] > IKPI )
10399 {
10400  j10array[0]-=IK2PI;
10401 }
10402 else if( j10array[0] < -IKPI )
10403 { j10array[0]+=IK2PI;
10404 }
10405 j10valid[0] = true;
10406 for(int ij10 = 0; ij10 < 1; ++ij10)
10407 {
10408 if( !j10valid[ij10] )
10409 {
10410  continue;
10411 }
10412 _ij10[0] = ij10; _ij10[1] = -1;
10413 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10414 {
10415 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10416 {
10417  j10valid[iij10]=false; _ij10[1] = iij10; break;
10418 }
10419 }
10420 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10421 {
10422 IkReal evalcond[4];
10423 IkReal x2358=IKsin(j10);
10424 IkReal x2359=IKcos(j10);
10425 IkReal x2360=((cj13)*(sj14));
10426 IkReal x2361=((cj13)*(cj14));
10427 IkReal x2362=((r10)*(sj9));
10428 IkReal x2363=((IkReal(1.00000000000000))*(cj9));
10429 IkReal x2364=((sj11)*(x2358));
10430 IkReal x2365=((IkReal(1.00000000000000))*(x2359));
10431 IkReal x2366=((cj11)*(x2358));
10432 IkReal x2367=((IkReal(1.00000000000000))*(r11)*(sj9));
10433 IkReal x2368=((cj11)*(x2365));
10434 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x2366)))+(((IkReal(-1.00000000000000))*(sj11)*(x2365))));
10435 evalcond[1]=((x2364)+(((IkReal(-1.00000000000000))*(x2368)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x2360)))+(((IkReal(-1.00000000000000))*(r20)*(x2361))));
10436 evalcond[2]=((x2364)+(((IkReal(-1.00000000000000))*(x2368)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2363)))+(((IkReal(-1.00000000000000))*(cj14)*(x2367)))+(((IkReal(-1.00000000000000))*(sj14)*(x2362)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2363))));
10437 evalcond[3]=((((sj11)*(x2359)))+(x2366)+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x2360)*(x2363)))+(((x2361)*(x2362)))+(((cj9)*(r00)*(x2361)))+(((IkReal(-1.00000000000000))*(x2360)*(x2367)))+(((r12)*(sj13)*(sj9))));
10438 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10439 {
10440 continue;
10441 }
10442 }
10443 
10444 {
10445 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10446 vinfos[0].jointtype = 1;
10447 vinfos[0].foffset = j9;
10448 vinfos[0].indices[0] = _ij9[0];
10449 vinfos[0].indices[1] = _ij9[1];
10450 vinfos[0].maxsolutions = _nj9;
10451 vinfos[1].jointtype = 1;
10452 vinfos[1].foffset = j10;
10453 vinfos[1].indices[0] = _ij10[0];
10454 vinfos[1].indices[1] = _ij10[1];
10455 vinfos[1].maxsolutions = _nj10;
10456 vinfos[2].jointtype = 1;
10457 vinfos[2].foffset = j11;
10458 vinfos[2].indices[0] = _ij11[0];
10459 vinfos[2].indices[1] = _ij11[1];
10460 vinfos[2].maxsolutions = _nj11;
10461 vinfos[3].jointtype = 1;
10462 vinfos[3].foffset = j12;
10463 vinfos[3].indices[0] = _ij12[0];
10464 vinfos[3].indices[1] = _ij12[1];
10465 vinfos[3].maxsolutions = _nj12;
10466 vinfos[4].jointtype = 1;
10467 vinfos[4].foffset = j13;
10468 vinfos[4].indices[0] = _ij13[0];
10469 vinfos[4].indices[1] = _ij13[1];
10470 vinfos[4].maxsolutions = _nj13;
10471 vinfos[5].jointtype = 1;
10472 vinfos[5].foffset = j14;
10473 vinfos[5].indices[0] = _ij14[0];
10474 vinfos[5].indices[1] = _ij14[1];
10475 vinfos[5].maxsolutions = _nj14;
10476 std::vector<int> vfree(0);
10477 solutions.AddSolution(vinfos,vfree);
10478 }
10479 }
10480 }
10481 
10482 }
10483 
10484 }
10485 
10486 } else
10487 {
10488 IkReal x2369=((r00)*(sj9));
10489 IkReal x2370=((IkReal(1.00000000000000))*(sj13));
10490 IkReal x2371=((cj13)*(sj14));
10491 IkReal x2372=((cj9)*(sj14));
10492 IkReal x2373=((cj13)*(r02));
10493 IkReal x2374=((sj13)*(sj14));
10494 IkReal x2375=((r01)*(sj9));
10495 IkReal x2376=((cj9)*(sj13));
10496 IkReal x2377=((IkReal(1.00000000000000))*(cj9));
10497 IkReal x2378=((cj14)*(r10));
10498 IkReal x2379=((cj14)*(npx));
10499 IkReal x2380=((IkReal(1.00000000000000))*(cj13));
10500 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
10501 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((cj14)*(npy)))+(((npx)*(sj14))));
10502 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(npz)*(x2370)))+(((IkReal(-1.00000000000000))*(x2379)*(x2380)))+(((npy)*(x2371))));
10503 evalcond[3]=((((cj14)*(x2375)))+(((IkReal(-1.00000000000000))*(r10)*(x2372)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2377)))+(((sj14)*(x2369))));
10504 evalcond[4]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2370)))+(((cj13)*(r22)))+(((r21)*(x2374))));
10505 evalcond[5]=((IkReal(-0.0950000000000000))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x2370)*(x2379)))+(((npy)*(x2374))));
10506 evalcond[6]=((((r12)*(x2376)))+(((x2371)*(x2375)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2370)))+(((IkReal(-1.00000000000000))*(cj14)*(x2369)*(x2380)))+(((IkReal(-1.00000000000000))*(r11)*(x2371)*(x2377)))+(((cj13)*(cj9)*(x2378))));
10507 evalcond[7]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(cj14)*(x2369)*(x2370)))+(((IkReal(-1.00000000000000))*(r11)*(x2370)*(x2372)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x2377)))+(((x2374)*(x2375)))+(((sj9)*(x2373)))+(((x2376)*(x2378))));
10508 evalcond[8]=((((sj13)*(sj9)*(x2378)))+(((cj14)*(r00)*(x2376)))+(((IkReal(-1.00000000000000))*(r01)*(x2370)*(x2372)))+(((IkReal(-1.00000000000000))*(x2373)*(x2377)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2380)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x2370))));
10509 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 )
10510 {
10511 {
10512 IkReal dummyeval[1];
10513 IkReal gconst50;
10514 gconst50=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
10515 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
10516 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10517 {
10518 {
10519 IkReal dummyeval[1];
10520 IkReal gconst51;
10521 gconst51=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
10522 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
10523 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10524 {
10525 continue;
10526 
10527 } else
10528 {
10529 {
10530 IkReal j10array[1], cj10array[1], sj10array[1];
10531 bool j10valid[1]={false};
10532 _nj10 = 1;
10533 IkReal x2381=((IkReal(1.00000000000000))*(sj11));
10534 IkReal x2382=((cj14)*(r21));
10535 IkReal x2383=((IkReal(1.00000000000000))*(cj11));
10536 IkReal x2384=((r20)*(sj14));
10537 IkReal x2385=((cj9)*(r00)*(sj14));
10538 IkReal x2386=((cj14)*(r11)*(sj9));
10539 IkReal x2387=((cj14)*(cj9)*(r01));
10540 IkReal x2388=((r10)*(sj14)*(sj9));
10541 if( IKabs(((gconst51)*(((((IkReal(-1.00000000000000))*(x2381)*(x2386)))+(((IkReal(-1.00000000000000))*(x2381)*(x2388)))+(((IkReal(-1.00000000000000))*(x2381)*(x2385)))+(((IkReal(-1.00000000000000))*(x2381)*(x2387)))+(((IkReal(-1.00000000000000))*(x2382)*(x2383)))+(((IkReal(-1.00000000000000))*(x2383)*(x2384))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst51)*(((((IkReal(-1.00000000000000))*(x2381)*(x2382)))+(((IkReal(-1.00000000000000))*(x2381)*(x2384)))+(((cj11)*(x2385)))+(((cj11)*(x2387)))+(((cj11)*(x2386)))+(((cj11)*(x2388))))))) < IKFAST_ATAN2_MAGTHRESH )
10542  continue;
10543 j10array[0]=IKatan2(((gconst51)*(((((IkReal(-1.00000000000000))*(x2381)*(x2386)))+(((IkReal(-1.00000000000000))*(x2381)*(x2388)))+(((IkReal(-1.00000000000000))*(x2381)*(x2385)))+(((IkReal(-1.00000000000000))*(x2381)*(x2387)))+(((IkReal(-1.00000000000000))*(x2382)*(x2383)))+(((IkReal(-1.00000000000000))*(x2383)*(x2384)))))), ((gconst51)*(((((IkReal(-1.00000000000000))*(x2381)*(x2382)))+(((IkReal(-1.00000000000000))*(x2381)*(x2384)))+(((cj11)*(x2385)))+(((cj11)*(x2387)))+(((cj11)*(x2386)))+(((cj11)*(x2388)))))));
10544 sj10array[0]=IKsin(j10array[0]);
10545 cj10array[0]=IKcos(j10array[0]);
10546 if( j10array[0] > IKPI )
10547 {
10548  j10array[0]-=IK2PI;
10549 }
10550 else if( j10array[0] < -IKPI )
10551 { j10array[0]+=IK2PI;
10552 }
10553 j10valid[0] = true;
10554 for(int ij10 = 0; ij10 < 1; ++ij10)
10555 {
10556 if( !j10valid[ij10] )
10557 {
10558  continue;
10559 }
10560 _ij10[0] = ij10; _ij10[1] = -1;
10561 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10562 {
10563 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10564 {
10565  j10valid[iij10]=false; _ij10[1] = iij10; break;
10566 }
10567 }
10568 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10569 {
10570 IkReal evalcond[4];
10571 IkReal x2389=IKsin(j10);
10572 IkReal x2390=IKcos(j10);
10573 IkReal x2391=((cj14)*(sj9));
10574 IkReal x2392=((IkReal(1.00000000000000))*(r11));
10575 IkReal x2393=((cj13)*(sj14));
10576 IkReal x2394=((IkReal(1.00000000000000))*(cj9));
10577 IkReal x2395=((cj13)*(cj14));
10578 IkReal x2396=((cj11)*(x2389));
10579 IkReal x2397=((sj11)*(x2390));
10580 IkReal x2398=((cj11)*(x2390));
10581 IkReal x2399=((sj11)*(x2389));
10582 IkReal x2400=((x2396)+(x2397));
10583 evalcond[0]=((x2400)+(((cj14)*(r21)))+(((r20)*(sj14))));
10584 evalcond[1]=((x2399)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x2398)))+(((IkReal(-1.00000000000000))*(r20)*(x2395)))+(((r21)*(x2393))));
10585 evalcond[2]=((x2398)+(((IkReal(-1.00000000000000))*(x2399)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2394)))+(((IkReal(-1.00000000000000))*(x2391)*(x2392)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2394))));
10586 evalcond[3]=((x2400)+(((cj9)*(r02)*(sj13)))+(((cj13)*(r10)*(x2391)))+(((IkReal(-1.00000000000000))*(sj9)*(x2392)*(x2393)))+(((IkReal(-1.00000000000000))*(r01)*(x2393)*(x2394)))+(((cj9)*(r00)*(x2395)))+(((r12)*(sj13)*(sj9))));
10587 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10588 {
10589 continue;
10590 }
10591 }
10592 
10593 {
10594 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10595 vinfos[0].jointtype = 1;
10596 vinfos[0].foffset = j9;
10597 vinfos[0].indices[0] = _ij9[0];
10598 vinfos[0].indices[1] = _ij9[1];
10599 vinfos[0].maxsolutions = _nj9;
10600 vinfos[1].jointtype = 1;
10601 vinfos[1].foffset = j10;
10602 vinfos[1].indices[0] = _ij10[0];
10603 vinfos[1].indices[1] = _ij10[1];
10604 vinfos[1].maxsolutions = _nj10;
10605 vinfos[2].jointtype = 1;
10606 vinfos[2].foffset = j11;
10607 vinfos[2].indices[0] = _ij11[0];
10608 vinfos[2].indices[1] = _ij11[1];
10609 vinfos[2].maxsolutions = _nj11;
10610 vinfos[3].jointtype = 1;
10611 vinfos[3].foffset = j12;
10612 vinfos[3].indices[0] = _ij12[0];
10613 vinfos[3].indices[1] = _ij12[1];
10614 vinfos[3].maxsolutions = _nj12;
10615 vinfos[4].jointtype = 1;
10616 vinfos[4].foffset = j13;
10617 vinfos[4].indices[0] = _ij13[0];
10618 vinfos[4].indices[1] = _ij13[1];
10619 vinfos[4].maxsolutions = _nj13;
10620 vinfos[5].jointtype = 1;
10621 vinfos[5].foffset = j14;
10622 vinfos[5].indices[0] = _ij14[0];
10623 vinfos[5].indices[1] = _ij14[1];
10624 vinfos[5].maxsolutions = _nj14;
10625 std::vector<int> vfree(0);
10626 solutions.AddSolution(vinfos,vfree);
10627 }
10628 }
10629 }
10630 
10631 }
10632 
10633 }
10634 
10635 } else
10636 {
10637 {
10638 IkReal j10array[1], cj10array[1], sj10array[1];
10639 bool j10valid[1]={false};
10640 _nj10 = 1;
10641 IkReal x2401=((cj13)*(sj11));
10642 IkReal x2402=((r21)*(sj14));
10643 IkReal x2403=((cj14)*(r20));
10644 IkReal x2404=((cj11)*(cj13));
10645 IkReal x2405=((r22)*(sj13));
10646 IkReal x2406=((r20)*(sj14));
10647 IkReal x2407=((cj14)*(r21));
10648 if( IKabs(((gconst50)*(((((cj11)*(x2406)))+(((cj11)*(x2407)))+(((x2401)*(x2402)))+(((IkReal(-1.00000000000000))*(sj11)*(x2405)))+(((IkReal(-1.00000000000000))*(x2401)*(x2403))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst50)*(((((x2403)*(x2404)))+(((cj11)*(x2405)))+(((IkReal(-1.00000000000000))*(x2402)*(x2404)))+(((sj11)*(x2406)))+(((sj11)*(x2407))))))) < IKFAST_ATAN2_MAGTHRESH )
10649  continue;
10650 j10array[0]=IKatan2(((gconst50)*(((((cj11)*(x2406)))+(((cj11)*(x2407)))+(((x2401)*(x2402)))+(((IkReal(-1.00000000000000))*(sj11)*(x2405)))+(((IkReal(-1.00000000000000))*(x2401)*(x2403)))))), ((gconst50)*(((((x2403)*(x2404)))+(((cj11)*(x2405)))+(((IkReal(-1.00000000000000))*(x2402)*(x2404)))+(((sj11)*(x2406)))+(((sj11)*(x2407)))))));
10651 sj10array[0]=IKsin(j10array[0]);
10652 cj10array[0]=IKcos(j10array[0]);
10653 if( j10array[0] > IKPI )
10654 {
10655  j10array[0]-=IK2PI;
10656 }
10657 else if( j10array[0] < -IKPI )
10658 { j10array[0]+=IK2PI;
10659 }
10660 j10valid[0] = true;
10661 for(int ij10 = 0; ij10 < 1; ++ij10)
10662 {
10663 if( !j10valid[ij10] )
10664 {
10665  continue;
10666 }
10667 _ij10[0] = ij10; _ij10[1] = -1;
10668 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10669 {
10670 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10671 {
10672  j10valid[iij10]=false; _ij10[1] = iij10; break;
10673 }
10674 }
10675 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10676 {
10677 IkReal evalcond[4];
10678 IkReal x2408=IKsin(j10);
10679 IkReal x2409=IKcos(j10);
10680 IkReal x2410=((cj14)*(sj9));
10681 IkReal x2411=((IkReal(1.00000000000000))*(r11));
10682 IkReal x2412=((cj13)*(sj14));
10683 IkReal x2413=((IkReal(1.00000000000000))*(cj9));
10684 IkReal x2414=((cj13)*(cj14));
10685 IkReal x2415=((cj11)*(x2408));
10686 IkReal x2416=((sj11)*(x2409));
10687 IkReal x2417=((cj11)*(x2409));
10688 IkReal x2418=((sj11)*(x2408));
10689 IkReal x2419=((x2415)+(x2416));
10690 evalcond[0]=((x2419)+(((cj14)*(r21)))+(((r20)*(sj14))));
10691 evalcond[1]=((x2418)+(((r21)*(x2412)))+(((IkReal(-1.00000000000000))*(x2417)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x2414))));
10692 evalcond[2]=((x2417)+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2413)))+(((IkReal(-1.00000000000000))*(x2418)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x2410)*(x2411)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2413))));
10693 evalcond[3]=((((cj13)*(r10)*(x2410)))+(x2419)+(((cj9)*(r00)*(x2414)))+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x2412)*(x2413)))+(((r12)*(sj13)*(sj9)))+(((IkReal(-1.00000000000000))*(sj9)*(x2411)*(x2412))));
10694 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10695 {
10696 continue;
10697 }
10698 }
10699 
10700 {
10701 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10702 vinfos[0].jointtype = 1;
10703 vinfos[0].foffset = j9;
10704 vinfos[0].indices[0] = _ij9[0];
10705 vinfos[0].indices[1] = _ij9[1];
10706 vinfos[0].maxsolutions = _nj9;
10707 vinfos[1].jointtype = 1;
10708 vinfos[1].foffset = j10;
10709 vinfos[1].indices[0] = _ij10[0];
10710 vinfos[1].indices[1] = _ij10[1];
10711 vinfos[1].maxsolutions = _nj10;
10712 vinfos[2].jointtype = 1;
10713 vinfos[2].foffset = j11;
10714 vinfos[2].indices[0] = _ij11[0];
10715 vinfos[2].indices[1] = _ij11[1];
10716 vinfos[2].maxsolutions = _nj11;
10717 vinfos[3].jointtype = 1;
10718 vinfos[3].foffset = j12;
10719 vinfos[3].indices[0] = _ij12[0];
10720 vinfos[3].indices[1] = _ij12[1];
10721 vinfos[3].maxsolutions = _nj12;
10722 vinfos[4].jointtype = 1;
10723 vinfos[4].foffset = j13;
10724 vinfos[4].indices[0] = _ij13[0];
10725 vinfos[4].indices[1] = _ij13[1];
10726 vinfos[4].maxsolutions = _nj13;
10727 vinfos[5].jointtype = 1;
10728 vinfos[5].foffset = j14;
10729 vinfos[5].indices[0] = _ij14[0];
10730 vinfos[5].indices[1] = _ij14[1];
10731 vinfos[5].maxsolutions = _nj14;
10732 std::vector<int> vfree(0);
10733 solutions.AddSolution(vinfos,vfree);
10734 }
10735 }
10736 }
10737 
10738 }
10739 
10740 }
10741 
10742 } else
10743 {
10744 IkReal x2420=((cj9)*(r10));
10745 IkReal x2421=((cj13)*(cj14));
10746 IkReal x2422=((sj14)*(sj9));
10747 IkReal x2423=((IkReal(1.00000000000000))*(sj14));
10748 IkReal x2424=((cj9)*(sj13));
10749 IkReal x2425=((r02)*(sj9));
10750 IkReal x2426=((cj13)*(cj9));
10751 IkReal x2427=((cj14)*(r01));
10752 IkReal x2428=((IkReal(1.00000000000000))*(npx));
10753 IkReal x2429=((cj14)*(sj13));
10754 IkReal x2430=((IkReal(1.00000000000000))*(cj9));
10755 IkReal x2431=((npy)*(sj14));
10756 IkReal x2432=((IkReal(1.00000000000000))*(sj13));
10757 IkReal x2433=((IkReal(1.00000000000000))*(cj14)*(sj9));
10758 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
10759 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
10760 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
10761 evalcond[3]=((IkReal(0.235000000000000))+(((cj13)*(x2431)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x2432)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(x2421)*(x2428))));
10762 evalcond[4]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2430)))+(((IkReal(-1.00000000000000))*(x2420)*(x2423)))+(((r00)*(x2422)))+(((sj9)*(x2427))));
10763 evalcond[5]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((IkReal(-1.00000000000000))*(x2428)*(x2429)))+(((sj13)*(x2431)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
10764 evalcond[6]=((((IkReal(-1.00000000000000))*(x2427)*(x2430)))+(((IkReal(-1.00000000000000))*(r10)*(x2422)))+(((IkReal(-1.00000000000000))*(cj9)*(r00)*(x2423)))+(((IkReal(-1.00000000000000))*(r11)*(x2433))));
10765 evalcond[7]=((((IkReal(-1.00000000000000))*(x2425)*(x2432)))+(((x2420)*(x2421)))+(((cj13)*(r01)*(x2422)))+(((r12)*(x2424)))+(((IkReal(-1.00000000000000))*(r11)*(x2423)*(x2426)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x2421))));
10766 evalcond[8]=((((IkReal(-1.00000000000000))*(r12)*(x2426)))+(((x2420)*(x2429)))+(((r01)*(sj13)*(x2422)))+(((cj13)*(x2425)))+(((IkReal(-1.00000000000000))*(r11)*(x2423)*(x2424)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x2429))));
10767 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 )
10768 {
10769 {
10770 IkReal dummyeval[1];
10771 IkReal gconst52;
10772 gconst52=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
10773 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
10774 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10775 {
10776 {
10777 IkReal dummyeval[1];
10778 IkReal gconst53;
10779 gconst53=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
10780 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
10781 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
10782 {
10783 continue;
10784 
10785 } else
10786 {
10787 {
10788 IkReal j10array[1], cj10array[1], sj10array[1];
10789 bool j10valid[1]={false};
10790 _nj10 = 1;
10791 IkReal x2434=((cj13)*(sj14));
10792 IkReal x2435=((IkReal(1.00000000000000))*(cj11));
10793 IkReal x2436=((r11)*(sj9));
10794 IkReal x2437=((IkReal(1.00000000000000))*(sj11));
10795 IkReal x2438=((cj13)*(cj14));
10796 IkReal x2439=((cj11)*(sj13));
10797 IkReal x2440=((r12)*(sj9));
10798 IkReal x2441=((sj11)*(sj13));
10799 IkReal x2442=((cj9)*(r02));
10800 IkReal x2443=((cj9)*(r01));
10801 IkReal x2444=((r10)*(sj9));
10802 IkReal x2445=((cj9)*(r00));
10803 if( IKabs(((gconst53)*(((((IkReal(-1.00000000000000))*(x2434)*(x2435)*(x2436)))+(((IkReal(-1.00000000000000))*(r20)*(x2437)*(x2438)))+(((cj11)*(x2438)*(x2445)))+(((cj11)*(x2438)*(x2444)))+(((r21)*(sj11)*(x2434)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2437)))+(((IkReal(-1.00000000000000))*(x2434)*(x2435)*(x2443)))+(((x2439)*(x2440)))+(((x2439)*(x2442))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst53)*(((((sj11)*(x2438)*(x2444)))+(((sj11)*(x2438)*(x2445)))+(((IkReal(-1.00000000000000))*(x2434)*(x2436)*(x2437)))+(((x2440)*(x2441)))+(((IkReal(-1.00000000000000))*(r21)*(x2434)*(x2435)))+(((r22)*(x2439)))+(((IkReal(-1.00000000000000))*(x2434)*(x2437)*(x2443)))+(((cj11)*(r20)*(x2438)))+(((x2441)*(x2442))))))) < IKFAST_ATAN2_MAGTHRESH )
10804  continue;
10805 j10array[0]=IKatan2(((gconst53)*(((((IkReal(-1.00000000000000))*(x2434)*(x2435)*(x2436)))+(((IkReal(-1.00000000000000))*(r20)*(x2437)*(x2438)))+(((cj11)*(x2438)*(x2445)))+(((cj11)*(x2438)*(x2444)))+(((r21)*(sj11)*(x2434)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2437)))+(((IkReal(-1.00000000000000))*(x2434)*(x2435)*(x2443)))+(((x2439)*(x2440)))+(((x2439)*(x2442)))))), ((gconst53)*(((((sj11)*(x2438)*(x2444)))+(((sj11)*(x2438)*(x2445)))+(((IkReal(-1.00000000000000))*(x2434)*(x2436)*(x2437)))+(((x2440)*(x2441)))+(((IkReal(-1.00000000000000))*(r21)*(x2434)*(x2435)))+(((r22)*(x2439)))+(((IkReal(-1.00000000000000))*(x2434)*(x2437)*(x2443)))+(((cj11)*(r20)*(x2438)))+(((x2441)*(x2442)))))));
10806 sj10array[0]=IKsin(j10array[0]);
10807 cj10array[0]=IKcos(j10array[0]);
10808 if( j10array[0] > IKPI )
10809 {
10810  j10array[0]-=IK2PI;
10811 }
10812 else if( j10array[0] < -IKPI )
10813 { j10array[0]+=IK2PI;
10814 }
10815 j10valid[0] = true;
10816 for(int ij10 = 0; ij10 < 1; ++ij10)
10817 {
10818 if( !j10valid[ij10] )
10819 {
10820  continue;
10821 }
10822 _ij10[0] = ij10; _ij10[1] = -1;
10823 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10824 {
10825 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10826 {
10827  j10valid[iij10]=false; _ij10[1] = iij10; break;
10828 }
10829 }
10830 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10831 {
10832 IkReal evalcond[4];
10833 IkReal x2446=IKsin(j10);
10834 IkReal x2447=IKcos(j10);
10835 IkReal x2448=((IkReal(1.00000000000000))*(sj13));
10836 IkReal x2449=((r11)*(sj9));
10837 IkReal x2450=((cj9)*(r01));
10838 IkReal x2451=((r21)*(sj14));
10839 IkReal x2452=((cj9)*(r02));
10840 IkReal x2453=((sj13)*(sj9));
10841 IkReal x2454=((cj14)*(r10));
10842 IkReal x2455=((IkReal(1.00000000000000))*(cj13));
10843 IkReal x2456=((cj14)*(r20));
10844 IkReal x2457=((cj11)*(x2446));
10845 IkReal x2458=((sj11)*(x2447));
10846 IkReal x2459=((sj14)*(x2455));
10847 IkReal x2460=((sj11)*(x2446));
10848 IkReal x2461=((cj11)*(x2447));
10849 IkReal x2462=((cj14)*(cj9)*(r00));
10850 IkReal x2463=((x2458)+(x2457));
10851 evalcond[0]=((x2460)+(((IkReal(-1.00000000000000))*(x2461)))+(((cj13)*(x2451)))+(((IkReal(-1.00000000000000))*(r22)*(x2448)))+(((IkReal(-1.00000000000000))*(x2455)*(x2456))));
10852 evalcond[1]=((x2463)+(((sj13)*(x2451)))+(((IkReal(-1.00000000000000))*(x2448)*(x2456)))+(((cj13)*(r22))));
10853 evalcond[2]=((((IkReal(-1.00000000000000))*(x2450)*(x2459)))+(x2463)+(((sj13)*(x2452)))+(((cj13)*(x2462)))+(((IkReal(-1.00000000000000))*(x2449)*(x2459)))+(((r12)*(x2453)))+(((cj13)*(sj9)*(x2454))));
10854 evalcond[3]=((x2461)+(((IkReal(-1.00000000000000))*(x2452)*(x2455)))+(((x2453)*(x2454)))+(((IkReal(-1.00000000000000))*(x2460)))+(((IkReal(-1.00000000000000))*(sj14)*(x2448)*(x2450)))+(((sj13)*(x2462)))+(((IkReal(-1.00000000000000))*(sj14)*(x2448)*(x2449)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2455))));
10855 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10856 {
10857 continue;
10858 }
10859 }
10860 
10861 {
10862 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10863 vinfos[0].jointtype = 1;
10864 vinfos[0].foffset = j9;
10865 vinfos[0].indices[0] = _ij9[0];
10866 vinfos[0].indices[1] = _ij9[1];
10867 vinfos[0].maxsolutions = _nj9;
10868 vinfos[1].jointtype = 1;
10869 vinfos[1].foffset = j10;
10870 vinfos[1].indices[0] = _ij10[0];
10871 vinfos[1].indices[1] = _ij10[1];
10872 vinfos[1].maxsolutions = _nj10;
10873 vinfos[2].jointtype = 1;
10874 vinfos[2].foffset = j11;
10875 vinfos[2].indices[0] = _ij11[0];
10876 vinfos[2].indices[1] = _ij11[1];
10877 vinfos[2].maxsolutions = _nj11;
10878 vinfos[3].jointtype = 1;
10879 vinfos[3].foffset = j12;
10880 vinfos[3].indices[0] = _ij12[0];
10881 vinfos[3].indices[1] = _ij12[1];
10882 vinfos[3].maxsolutions = _nj12;
10883 vinfos[4].jointtype = 1;
10884 vinfos[4].foffset = j13;
10885 vinfos[4].indices[0] = _ij13[0];
10886 vinfos[4].indices[1] = _ij13[1];
10887 vinfos[4].maxsolutions = _nj13;
10888 vinfos[5].jointtype = 1;
10889 vinfos[5].foffset = j14;
10890 vinfos[5].indices[0] = _ij14[0];
10891 vinfos[5].indices[1] = _ij14[1];
10892 vinfos[5].maxsolutions = _nj14;
10893 std::vector<int> vfree(0);
10894 solutions.AddSolution(vinfos,vfree);
10895 }
10896 }
10897 }
10898 
10899 }
10900 
10901 }
10902 
10903 } else
10904 {
10905 {
10906 IkReal j10array[1], cj10array[1], sj10array[1];
10907 bool j10valid[1]={false};
10908 _nj10 = 1;
10909 IkReal x2464=((cj13)*(sj11));
10910 IkReal x2465=((r21)*(sj14));
10911 IkReal x2466=((cj11)*(cj13));
10912 IkReal x2467=((cj11)*(sj13));
10913 IkReal x2468=((sj11)*(sj13));
10914 IkReal x2469=((IkReal(1.00000000000000))*(cj14)*(r20));
10915 if( IKabs(((gconst52)*(((((IkReal(-1.00000000000000))*(x2464)*(x2469)))+(((x2464)*(x2465)))+(((x2465)*(x2467)))+(((r22)*(x2466)))+(((IkReal(-1.00000000000000))*(x2467)*(x2469)))+(((IkReal(-1.00000000000000))*(r22)*(x2468))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst52)*(((((cj14)*(r20)*(x2466)))+(((IkReal(-1.00000000000000))*(x2468)*(x2469)))+(((x2465)*(x2468)))+(((IkReal(-1.00000000000000))*(x2465)*(x2466)))+(((r22)*(x2464)))+(((r22)*(x2467))))))) < IKFAST_ATAN2_MAGTHRESH )
10916  continue;
10917 j10array[0]=IKatan2(((gconst52)*(((((IkReal(-1.00000000000000))*(x2464)*(x2469)))+(((x2464)*(x2465)))+(((x2465)*(x2467)))+(((r22)*(x2466)))+(((IkReal(-1.00000000000000))*(x2467)*(x2469)))+(((IkReal(-1.00000000000000))*(r22)*(x2468)))))), ((gconst52)*(((((cj14)*(r20)*(x2466)))+(((IkReal(-1.00000000000000))*(x2468)*(x2469)))+(((x2465)*(x2468)))+(((IkReal(-1.00000000000000))*(x2465)*(x2466)))+(((r22)*(x2464)))+(((r22)*(x2467)))))));
10918 sj10array[0]=IKsin(j10array[0]);
10919 cj10array[0]=IKcos(j10array[0]);
10920 if( j10array[0] > IKPI )
10921 {
10922  j10array[0]-=IK2PI;
10923 }
10924 else if( j10array[0] < -IKPI )
10925 { j10array[0]+=IK2PI;
10926 }
10927 j10valid[0] = true;
10928 for(int ij10 = 0; ij10 < 1; ++ij10)
10929 {
10930 if( !j10valid[ij10] )
10931 {
10932  continue;
10933 }
10934 _ij10[0] = ij10; _ij10[1] = -1;
10935 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
10936 {
10937 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
10938 {
10939  j10valid[iij10]=false; _ij10[1] = iij10; break;
10940 }
10941 }
10942 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
10943 {
10944 IkReal evalcond[4];
10945 IkReal x2470=IKsin(j10);
10946 IkReal x2471=IKcos(j10);
10947 IkReal x2472=((IkReal(1.00000000000000))*(sj13));
10948 IkReal x2473=((r11)*(sj9));
10949 IkReal x2474=((cj9)*(r01));
10950 IkReal x2475=((r21)*(sj14));
10951 IkReal x2476=((cj9)*(r02));
10952 IkReal x2477=((sj13)*(sj9));
10953 IkReal x2478=((cj14)*(r10));
10954 IkReal x2479=((IkReal(1.00000000000000))*(cj13));
10955 IkReal x2480=((cj14)*(r20));
10956 IkReal x2481=((cj11)*(x2470));
10957 IkReal x2482=((sj11)*(x2471));
10958 IkReal x2483=((sj14)*(x2479));
10959 IkReal x2484=((sj11)*(x2470));
10960 IkReal x2485=((cj11)*(x2471));
10961 IkReal x2486=((cj14)*(cj9)*(r00));
10962 IkReal x2487=((x2482)+(x2481));
10963 evalcond[0]=((x2484)+(((IkReal(-1.00000000000000))*(x2485)))+(((cj13)*(x2475)))+(((IkReal(-1.00000000000000))*(r22)*(x2472)))+(((IkReal(-1.00000000000000))*(x2479)*(x2480))));
10964 evalcond[1]=((x2487)+(((IkReal(-1.00000000000000))*(x2472)*(x2480)))+(((sj13)*(x2475)))+(((cj13)*(r22))));
10965 evalcond[2]=((((r12)*(x2477)))+(x2487)+(((cj13)*(sj9)*(x2478)))+(((cj13)*(x2486)))+(((IkReal(-1.00000000000000))*(x2473)*(x2483)))+(((IkReal(-1.00000000000000))*(x2474)*(x2483)))+(((sj13)*(x2476))));
10966 evalcond[3]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2479)))+(x2485)+(((IkReal(-1.00000000000000))*(x2484)))+(((sj13)*(x2486)))+(((x2477)*(x2478)))+(((IkReal(-1.00000000000000))*(x2476)*(x2479)))+(((IkReal(-1.00000000000000))*(sj14)*(x2472)*(x2474)))+(((IkReal(-1.00000000000000))*(sj14)*(x2472)*(x2473))));
10967 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
10968 {
10969 continue;
10970 }
10971 }
10972 
10973 {
10974 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
10975 vinfos[0].jointtype = 1;
10976 vinfos[0].foffset = j9;
10977 vinfos[0].indices[0] = _ij9[0];
10978 vinfos[0].indices[1] = _ij9[1];
10979 vinfos[0].maxsolutions = _nj9;
10980 vinfos[1].jointtype = 1;
10981 vinfos[1].foffset = j10;
10982 vinfos[1].indices[0] = _ij10[0];
10983 vinfos[1].indices[1] = _ij10[1];
10984 vinfos[1].maxsolutions = _nj10;
10985 vinfos[2].jointtype = 1;
10986 vinfos[2].foffset = j11;
10987 vinfos[2].indices[0] = _ij11[0];
10988 vinfos[2].indices[1] = _ij11[1];
10989 vinfos[2].maxsolutions = _nj11;
10990 vinfos[3].jointtype = 1;
10991 vinfos[3].foffset = j12;
10992 vinfos[3].indices[0] = _ij12[0];
10993 vinfos[3].indices[1] = _ij12[1];
10994 vinfos[3].maxsolutions = _nj12;
10995 vinfos[4].jointtype = 1;
10996 vinfos[4].foffset = j13;
10997 vinfos[4].indices[0] = _ij13[0];
10998 vinfos[4].indices[1] = _ij13[1];
10999 vinfos[4].maxsolutions = _nj13;
11000 vinfos[5].jointtype = 1;
11001 vinfos[5].foffset = j14;
11002 vinfos[5].indices[0] = _ij14[0];
11003 vinfos[5].indices[1] = _ij14[1];
11004 vinfos[5].maxsolutions = _nj14;
11005 std::vector<int> vfree(0);
11006 solutions.AddSolution(vinfos,vfree);
11007 }
11008 }
11009 }
11010 
11011 }
11012 
11013 }
11014 
11015 } else
11016 {
11017 IkReal x2488=((cj9)*(r10));
11018 IkReal x2489=((cj13)*(cj14));
11019 IkReal x2490=((sj14)*(sj9));
11020 IkReal x2491=((IkReal(1.00000000000000))*(sj14));
11021 IkReal x2492=((cj9)*(sj13));
11022 IkReal x2493=((r02)*(sj9));
11023 IkReal x2494=((cj13)*(cj9));
11024 IkReal x2495=((cj14)*(r01));
11025 IkReal x2496=((IkReal(1.00000000000000))*(npx));
11026 IkReal x2497=((cj14)*(sj13));
11027 IkReal x2498=((IkReal(1.00000000000000))*(cj9));
11028 IkReal x2499=((npy)*(sj14));
11029 IkReal x2500=((IkReal(1.00000000000000))*(sj13));
11030 IkReal x2501=((IkReal(1.00000000000000))*(cj14)*(sj9));
11031 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
11032 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
11033 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
11034 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x2499)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(x2489)*(x2496)))+(((IkReal(-1.00000000000000))*(npz)*(x2500))));
11035 evalcond[4]=((IkReal(-1.00000000000000))+(((r00)*(x2490)))+(((sj9)*(x2495)))+(((IkReal(-1.00000000000000))*(x2488)*(x2491)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2498))));
11036 evalcond[5]=((IkReal(-0.0300000000000000))+(((IkReal(-1.00000000000000))*(x2496)*(x2497)))+(((cj13)*(npz)))+(((sj13)*(x2499)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(0.250000000000000))*(sj11))));
11037 evalcond[6]=((((IkReal(-1.00000000000000))*(x2495)*(x2498)))+(((IkReal(-1.00000000000000))*(cj9)*(r00)*(x2491)))+(((IkReal(-1.00000000000000))*(r11)*(x2501)))+(((IkReal(-1.00000000000000))*(r10)*(x2490))));
11038 evalcond[7]=((((cj13)*(r01)*(x2490)))+(((IkReal(-1.00000000000000))*(r11)*(x2491)*(x2494)))+(((x2488)*(x2489)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x2489)))+(((IkReal(-1.00000000000000))*(x2493)*(x2500)))+(((r12)*(x2492))));
11039 evalcond[8]=((((r01)*(sj13)*(x2490)))+(((IkReal(-1.00000000000000))*(r12)*(x2494)))+(((x2488)*(x2497)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x2497)))+(((IkReal(-1.00000000000000))*(r11)*(x2491)*(x2492)))+(((cj13)*(x2493))));
11040 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 )
11041 {
11042 {
11043 IkReal dummyeval[1];
11044 IkReal gconst54;
11045 gconst54=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
11046 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
11047 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
11048 {
11049 {
11050 IkReal dummyeval[1];
11051 IkReal gconst55;
11052 gconst55=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
11053 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
11054 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
11055 {
11056 continue;
11057 
11058 } else
11059 {
11060 {
11061 IkReal j10array[1], cj10array[1], sj10array[1];
11062 bool j10valid[1]={false};
11063 _nj10 = 1;
11064 IkReal x2502=((cj13)*(sj14));
11065 IkReal x2503=((IkReal(1.00000000000000))*(cj11));
11066 IkReal x2504=((r11)*(sj9));
11067 IkReal x2505=((cj11)*(sj13));
11068 IkReal x2506=((r12)*(sj9));
11069 IkReal x2507=((r10)*(sj9));
11070 IkReal x2508=((sj11)*(sj13));
11071 IkReal x2509=((cj9)*(r02));
11072 IkReal x2510=((IkReal(1.00000000000000))*(sj11));
11073 IkReal x2511=((cj9)*(r01));
11074 IkReal x2512=((cj9)*(r00));
11075 IkReal x2513=((cj13)*(cj14)*(sj11));
11076 IkReal x2514=((cj11)*(cj13)*(cj14));
11077 if( IKabs(((gconst55)*(((((x2512)*(x2514)))+(((x2505)*(x2506)))+(((x2505)*(x2509)))+(((r21)*(sj11)*(x2502)))+(((IkReal(-1.00000000000000))*(r22)*(x2508)))+(((x2507)*(x2514)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x2510)))+(((IkReal(-1.00000000000000))*(x2502)*(x2503)*(x2504)))+(((IkReal(-1.00000000000000))*(x2502)*(x2503)*(x2511))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst55)*(((((x2512)*(x2513)))+(((r22)*(x2505)))+(((x2506)*(x2508)))+(((IkReal(-1.00000000000000))*(x2502)*(x2504)*(x2510)))+(((x2508)*(x2509)))+(((x2507)*(x2513)))+(((IkReal(-1.00000000000000))*(x2502)*(x2510)*(x2511)))+(((IkReal(-1.00000000000000))*(r21)*(x2502)*(x2503)))+(((r20)*(x2514))))))) < IKFAST_ATAN2_MAGTHRESH )
11078  continue;
11079 j10array[0]=IKatan2(((gconst55)*(((((x2512)*(x2514)))+(((x2505)*(x2506)))+(((x2505)*(x2509)))+(((r21)*(sj11)*(x2502)))+(((IkReal(-1.00000000000000))*(r22)*(x2508)))+(((x2507)*(x2514)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x2510)))+(((IkReal(-1.00000000000000))*(x2502)*(x2503)*(x2504)))+(((IkReal(-1.00000000000000))*(x2502)*(x2503)*(x2511)))))), ((gconst55)*(((((x2512)*(x2513)))+(((r22)*(x2505)))+(((x2506)*(x2508)))+(((IkReal(-1.00000000000000))*(x2502)*(x2504)*(x2510)))+(((x2508)*(x2509)))+(((x2507)*(x2513)))+(((IkReal(-1.00000000000000))*(x2502)*(x2510)*(x2511)))+(((IkReal(-1.00000000000000))*(r21)*(x2502)*(x2503)))+(((r20)*(x2514)))))));
11080 sj10array[0]=IKsin(j10array[0]);
11081 cj10array[0]=IKcos(j10array[0]);
11082 if( j10array[0] > IKPI )
11083 {
11084  j10array[0]-=IK2PI;
11085 }
11086 else if( j10array[0] < -IKPI )
11087 { j10array[0]+=IK2PI;
11088 }
11089 j10valid[0] = true;
11090 for(int ij10 = 0; ij10 < 1; ++ij10)
11091 {
11092 if( !j10valid[ij10] )
11093 {
11094  continue;
11095 }
11096 _ij10[0] = ij10; _ij10[1] = -1;
11097 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11098 {
11099 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11100 {
11101  j10valid[iij10]=false; _ij10[1] = iij10; break;
11102 }
11103 }
11104 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11105 {
11106 IkReal evalcond[4];
11107 IkReal x2515=IKsin(j10);
11108 IkReal x2516=IKcos(j10);
11109 IkReal x2517=((IkReal(1.00000000000000))*(sj13));
11110 IkReal x2518=((r11)*(sj9));
11111 IkReal x2519=((cj9)*(r01));
11112 IkReal x2520=((IkReal(1.00000000000000))*(cj11));
11113 IkReal x2521=((r21)*(sj14));
11114 IkReal x2522=((cj9)*(r02));
11115 IkReal x2523=((sj13)*(sj9));
11116 IkReal x2524=((cj14)*(r10));
11117 IkReal x2525=((IkReal(1.00000000000000))*(cj13));
11118 IkReal x2526=((cj14)*(r20));
11119 IkReal x2527=((sj11)*(x2515));
11120 IkReal x2528=((sj14)*(x2525));
11121 IkReal x2529=((sj11)*(x2516));
11122 IkReal x2530=((cj14)*(cj9)*(r00));
11123 IkReal x2531=((x2516)*(x2520));
11124 evalcond[0]=((x2527)+(((cj13)*(x2521)))+(((IkReal(-1.00000000000000))*(x2531)))+(((IkReal(-1.00000000000000))*(r22)*(x2517)))+(((IkReal(-1.00000000000000))*(x2525)*(x2526))));
11125 evalcond[1]=((((IkReal(-1.00000000000000))*(x2515)*(x2520)))+(((IkReal(-1.00000000000000))*(x2529)))+(((IkReal(-1.00000000000000))*(x2517)*(x2526)))+(((sj13)*(x2521)))+(((cj13)*(r22))));
11126 evalcond[2]=((x2529)+(((cj11)*(x2515)))+(((cj13)*(sj9)*(x2524)))+(((r12)*(x2523)))+(((sj13)*(x2522)))+(((cj13)*(x2530)))+(((IkReal(-1.00000000000000))*(x2519)*(x2528)))+(((IkReal(-1.00000000000000))*(x2518)*(x2528))));
11127 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x2517)*(x2518)))+(((IkReal(-1.00000000000000))*(sj14)*(x2517)*(x2519)))+(x2527)+(((sj13)*(x2530)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2525)))+(((IkReal(-1.00000000000000))*(x2531)))+(((IkReal(-1.00000000000000))*(x2522)*(x2525)))+(((x2523)*(x2524))));
11128 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
11129 {
11130 continue;
11131 }
11132 }
11133 
11134 {
11135 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11136 vinfos[0].jointtype = 1;
11137 vinfos[0].foffset = j9;
11138 vinfos[0].indices[0] = _ij9[0];
11139 vinfos[0].indices[1] = _ij9[1];
11140 vinfos[0].maxsolutions = _nj9;
11141 vinfos[1].jointtype = 1;
11142 vinfos[1].foffset = j10;
11143 vinfos[1].indices[0] = _ij10[0];
11144 vinfos[1].indices[1] = _ij10[1];
11145 vinfos[1].maxsolutions = _nj10;
11146 vinfos[2].jointtype = 1;
11147 vinfos[2].foffset = j11;
11148 vinfos[2].indices[0] = _ij11[0];
11149 vinfos[2].indices[1] = _ij11[1];
11150 vinfos[2].maxsolutions = _nj11;
11151 vinfos[3].jointtype = 1;
11152 vinfos[3].foffset = j12;
11153 vinfos[3].indices[0] = _ij12[0];
11154 vinfos[3].indices[1] = _ij12[1];
11155 vinfos[3].maxsolutions = _nj12;
11156 vinfos[4].jointtype = 1;
11157 vinfos[4].foffset = j13;
11158 vinfos[4].indices[0] = _ij13[0];
11159 vinfos[4].indices[1] = _ij13[1];
11160 vinfos[4].maxsolutions = _nj13;
11161 vinfos[5].jointtype = 1;
11162 vinfos[5].foffset = j14;
11163 vinfos[5].indices[0] = _ij14[0];
11164 vinfos[5].indices[1] = _ij14[1];
11165 vinfos[5].maxsolutions = _nj14;
11166 std::vector<int> vfree(0);
11167 solutions.AddSolution(vinfos,vfree);
11168 }
11169 }
11170 }
11171 
11172 }
11173 
11174 }
11175 
11176 } else
11177 {
11178 {
11179 IkReal j10array[1], cj10array[1], sj10array[1];
11180 bool j10valid[1]={false};
11181 _nj10 = 1;
11182 IkReal x2532=((r22)*(sj11));
11183 IkReal x2533=((IkReal(1.00000000000000))*(sj11));
11184 IkReal x2534=((IkReal(1.00000000000000))*(cj11));
11185 IkReal x2535=((cj14)*(r20));
11186 IkReal x2536=((cj13)*(r21)*(sj14));
11187 IkReal x2537=((r21)*(sj13)*(sj14));
11188 if( IKabs(((gconst54)*(((((IkReal(-1.00000000000000))*(x2533)*(x2536)))+(((cj11)*(x2537)))+(((sj13)*(x2532)))+(((cj11)*(cj13)*(r22)))+(((cj13)*(sj11)*(x2535)))+(((IkReal(-1.00000000000000))*(sj13)*(x2534)*(x2535))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst54)*(((((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2534)))+(((cj11)*(x2536)))+(((IkReal(-1.00000000000000))*(cj13)*(x2534)*(x2535)))+(((sj11)*(x2537)))+(((cj13)*(x2532)))+(((IkReal(-1.00000000000000))*(sj13)*(x2533)*(x2535))))))) < IKFAST_ATAN2_MAGTHRESH )
11189  continue;
11190 j10array[0]=IKatan2(((gconst54)*(((((IkReal(-1.00000000000000))*(x2533)*(x2536)))+(((cj11)*(x2537)))+(((sj13)*(x2532)))+(((cj11)*(cj13)*(r22)))+(((cj13)*(sj11)*(x2535)))+(((IkReal(-1.00000000000000))*(sj13)*(x2534)*(x2535)))))), ((gconst54)*(((((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2534)))+(((cj11)*(x2536)))+(((IkReal(-1.00000000000000))*(cj13)*(x2534)*(x2535)))+(((sj11)*(x2537)))+(((cj13)*(x2532)))+(((IkReal(-1.00000000000000))*(sj13)*(x2533)*(x2535)))))));
11191 sj10array[0]=IKsin(j10array[0]);
11192 cj10array[0]=IKcos(j10array[0]);
11193 if( j10array[0] > IKPI )
11194 {
11195  j10array[0]-=IK2PI;
11196 }
11197 else if( j10array[0] < -IKPI )
11198 { j10array[0]+=IK2PI;
11199 }
11200 j10valid[0] = true;
11201 for(int ij10 = 0; ij10 < 1; ++ij10)
11202 {
11203 if( !j10valid[ij10] )
11204 {
11205  continue;
11206 }
11207 _ij10[0] = ij10; _ij10[1] = -1;
11208 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11209 {
11210 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11211 {
11212  j10valid[iij10]=false; _ij10[1] = iij10; break;
11213 }
11214 }
11215 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11216 {
11217 IkReal evalcond[4];
11218 IkReal x2538=IKsin(j10);
11219 IkReal x2539=IKcos(j10);
11220 IkReal x2540=((IkReal(1.00000000000000))*(sj13));
11221 IkReal x2541=((r11)*(sj9));
11222 IkReal x2542=((cj9)*(r01));
11223 IkReal x2543=((IkReal(1.00000000000000))*(cj11));
11224 IkReal x2544=((r21)*(sj14));
11225 IkReal x2545=((cj9)*(r02));
11226 IkReal x2546=((sj13)*(sj9));
11227 IkReal x2547=((cj14)*(r10));
11228 IkReal x2548=((IkReal(1.00000000000000))*(cj13));
11229 IkReal x2549=((cj14)*(r20));
11230 IkReal x2550=((sj11)*(x2538));
11231 IkReal x2551=((sj14)*(x2548));
11232 IkReal x2552=((sj11)*(x2539));
11233 IkReal x2553=((cj14)*(cj9)*(r00));
11234 IkReal x2554=((x2539)*(x2543));
11235 evalcond[0]=((((IkReal(-1.00000000000000))*(x2548)*(x2549)))+(x2550)+(((cj13)*(x2544)))+(((IkReal(-1.00000000000000))*(r22)*(x2540)))+(((IkReal(-1.00000000000000))*(x2554))));
11236 evalcond[1]=((((IkReal(-1.00000000000000))*(x2552)))+(((sj13)*(x2544)))+(((IkReal(-1.00000000000000))*(x2538)*(x2543)))+(((IkReal(-1.00000000000000))*(x2540)*(x2549)))+(((cj13)*(r22))));
11237 evalcond[2]=((((cj13)*(x2553)))+(x2552)+(((IkReal(-1.00000000000000))*(x2542)*(x2551)))+(((cj11)*(x2538)))+(((cj13)*(sj9)*(x2547)))+(((r12)*(x2546)))+(((sj13)*(x2545)))+(((IkReal(-1.00000000000000))*(x2541)*(x2551))));
11238 evalcond[3]=((x2550)+(((IkReal(-1.00000000000000))*(x2545)*(x2548)))+(((IkReal(-1.00000000000000))*(sj14)*(x2540)*(x2542)))+(((IkReal(-1.00000000000000))*(sj14)*(x2540)*(x2541)))+(((sj13)*(x2553)))+(((x2546)*(x2547)))+(((IkReal(-1.00000000000000))*(x2554)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2548))));
11239 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
11240 {
11241 continue;
11242 }
11243 }
11244 
11245 {
11246 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11247 vinfos[0].jointtype = 1;
11248 vinfos[0].foffset = j9;
11249 vinfos[0].indices[0] = _ij9[0];
11250 vinfos[0].indices[1] = _ij9[1];
11251 vinfos[0].maxsolutions = _nj9;
11252 vinfos[1].jointtype = 1;
11253 vinfos[1].foffset = j10;
11254 vinfos[1].indices[0] = _ij10[0];
11255 vinfos[1].indices[1] = _ij10[1];
11256 vinfos[1].maxsolutions = _nj10;
11257 vinfos[2].jointtype = 1;
11258 vinfos[2].foffset = j11;
11259 vinfos[2].indices[0] = _ij11[0];
11260 vinfos[2].indices[1] = _ij11[1];
11261 vinfos[2].maxsolutions = _nj11;
11262 vinfos[3].jointtype = 1;
11263 vinfos[3].foffset = j12;
11264 vinfos[3].indices[0] = _ij12[0];
11265 vinfos[3].indices[1] = _ij12[1];
11266 vinfos[3].maxsolutions = _nj12;
11267 vinfos[4].jointtype = 1;
11268 vinfos[4].foffset = j13;
11269 vinfos[4].indices[0] = _ij13[0];
11270 vinfos[4].indices[1] = _ij13[1];
11271 vinfos[4].maxsolutions = _nj13;
11272 vinfos[5].jointtype = 1;
11273 vinfos[5].foffset = j14;
11274 vinfos[5].indices[0] = _ij14[0];
11275 vinfos[5].indices[1] = _ij14[1];
11276 vinfos[5].maxsolutions = _nj14;
11277 std::vector<int> vfree(0);
11278 solutions.AddSolution(vinfos,vfree);
11279 }
11280 }
11281 }
11282 
11283 }
11284 
11285 }
11286 
11287 } else
11288 {
11289 if( 1 )
11290 {
11291 continue;
11292 
11293 } else
11294 {
11295 }
11296 }
11297 }
11298 }
11299 }
11300 }
11301 
11302 } else
11303 {
11304 {
11305 IkReal j10array[1], cj10array[1], sj10array[1];
11306 bool j10valid[1]={false};
11307 _nj10 = 1;
11308 IkReal x2555=((r21)*(sj14));
11309 IkReal x2556=((cj12)*(cj13));
11310 IkReal x2557=((sj11)*(sj13));
11311 IkReal x2558=((cj14)*(r20));
11312 IkReal x2559=((IkReal(1.00000000000000))*(sj11));
11313 IkReal x2560=((cj12)*(r22));
11314 IkReal x2561=((IkReal(1.00000000000000))*(cj11));
11315 IkReal x2562=((cj13)*(r22));
11316 IkReal x2563=((sj13)*(x2561));
11317 if( IKabs(((gconst47)*(((((sj11)*(x2556)*(x2558)))+(((IkReal(-1.00000000000000))*(x2555)*(x2563)))+(((x2557)*(x2560)))+(((IkReal(-1.00000000000000))*(x2555)*(x2556)*(x2559)))+(((IkReal(-1.00000000000000))*(x2561)*(x2562)))+(((cj11)*(sj13)*(x2558))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst47)*(((((IkReal(-1.00000000000000))*(x2555)*(x2557)))+(((cj11)*(x2555)*(x2556)))+(((IkReal(-1.00000000000000))*(x2556)*(x2558)*(x2561)))+(((IkReal(-1.00000000000000))*(x2560)*(x2563)))+(((IkReal(-1.00000000000000))*(x2559)*(x2562)))+(((x2557)*(x2558))))))) < IKFAST_ATAN2_MAGTHRESH )
11318  continue;
11319 j10array[0]=IKatan2(((gconst47)*(((((sj11)*(x2556)*(x2558)))+(((IkReal(-1.00000000000000))*(x2555)*(x2563)))+(((x2557)*(x2560)))+(((IkReal(-1.00000000000000))*(x2555)*(x2556)*(x2559)))+(((IkReal(-1.00000000000000))*(x2561)*(x2562)))+(((cj11)*(sj13)*(x2558)))))), ((gconst47)*(((((IkReal(-1.00000000000000))*(x2555)*(x2557)))+(((cj11)*(x2555)*(x2556)))+(((IkReal(-1.00000000000000))*(x2556)*(x2558)*(x2561)))+(((IkReal(-1.00000000000000))*(x2560)*(x2563)))+(((IkReal(-1.00000000000000))*(x2559)*(x2562)))+(((x2557)*(x2558)))))));
11320 sj10array[0]=IKsin(j10array[0]);
11321 cj10array[0]=IKcos(j10array[0]);
11322 if( j10array[0] > IKPI )
11323 {
11324  j10array[0]-=IK2PI;
11325 }
11326 else if( j10array[0] < -IKPI )
11327 { j10array[0]+=IK2PI;
11328 }
11329 j10valid[0] = true;
11330 for(int ij10 = 0; ij10 < 1; ++ij10)
11331 {
11332 if( !j10valid[ij10] )
11333 {
11334  continue;
11335 }
11336 _ij10[0] = ij10; _ij10[1] = -1;
11337 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11338 {
11339 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11340 {
11341  j10valid[iij10]=false; _ij10[1] = iij10; break;
11342 }
11343 }
11344 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11345 {
11346 IkReal evalcond[6];
11347 IkReal x2564=IKsin(j10);
11348 IkReal x2565=IKcos(j10);
11349 IkReal x2566=((IkReal(1.00000000000000))*(cj13));
11350 IkReal x2567=((cj9)*(r02));
11351 IkReal x2568=((IkReal(1.00000000000000))*(sj13));
11352 IkReal x2569=((r11)*(sj9));
11353 IkReal x2570=((IkReal(1.00000000000000))*(cj14));
11354 IkReal x2571=((cj9)*(r01));
11355 IkReal x2572=((r21)*(sj14));
11356 IkReal x2573=((IkReal(1.00000000000000))*(sj12));
11357 IkReal x2574=((r10)*(sj9));
11358 IkReal x2575=((cj14)*(sj13));
11359 IkReal x2576=((cj14)*(r20));
11360 IkReal x2577=((IkReal(1.00000000000000))*(sj14));
11361 IkReal x2578=((r12)*(sj9));
11362 IkReal x2579=((cj13)*(cj14));
11363 IkReal x2580=((cj9)*(r00));
11364 IkReal x2581=((sj11)*(x2564));
11365 IkReal x2582=((cj11)*(x2564));
11366 IkReal x2583=((sj11)*(x2565));
11367 IkReal x2584=((cj11)*(x2565));
11368 evalcond[0]=((((IkReal(-1.00000000000000))*(x2573)*(x2582)))+(((IkReal(-1.00000000000000))*(x2573)*(x2583)))+(((cj14)*(r21)))+(((r20)*(sj14))));
11369 evalcond[1]=((x2581)+(((IkReal(-1.00000000000000))*(r22)*(x2568)))+(((IkReal(-1.00000000000000))*(x2566)*(x2576)))+(((cj13)*(x2572)))+(((IkReal(-1.00000000000000))*(x2584))));
11370 evalcond[2]=((((cj12)*(x2582)))+(((cj12)*(x2583)))+(((IkReal(-1.00000000000000))*(x2568)*(x2576)))+(((sj13)*(x2572)))+(((cj13)*(r22))));
11371 evalcond[3]=((((IkReal(-1.00000000000000))*(x2569)*(x2570)))+(((IkReal(-1.00000000000000))*(x2573)*(x2584)))+(((sj12)*(x2581)))+(((IkReal(-1.00000000000000))*(x2570)*(x2571)))+(((IkReal(-1.00000000000000))*(x2577)*(x2580)))+(((IkReal(-1.00000000000000))*(x2574)*(x2577))));
11372 evalcond[4]=((((IkReal(-1.00000000000000))*(sj14)*(x2566)*(x2569)))+(x2583)+(x2582)+(((IkReal(-1.00000000000000))*(sj14)*(x2566)*(x2571)))+(((x2574)*(x2579)))+(((sj13)*(x2567)))+(((sj13)*(x2578)))+(((x2579)*(x2580))));
11373 evalcond[5]=((((IkReal(-1.00000000000000))*(x2566)*(x2578)))+(((cj12)*(x2584)))+(((x2574)*(x2575)))+(((IkReal(-1.00000000000000))*(x2566)*(x2567)))+(((IkReal(-1.00000000000000))*(sj14)*(x2568)*(x2569)))+(((x2575)*(x2580)))+(((IkReal(-1.00000000000000))*(sj14)*(x2568)*(x2571)))+(((IkReal(-1.00000000000000))*(cj12)*(x2581))));
11374 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 )
11375 {
11376 continue;
11377 }
11378 }
11379 
11380 {
11381 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11382 vinfos[0].jointtype = 1;
11383 vinfos[0].foffset = j9;
11384 vinfos[0].indices[0] = _ij9[0];
11385 vinfos[0].indices[1] = _ij9[1];
11386 vinfos[0].maxsolutions = _nj9;
11387 vinfos[1].jointtype = 1;
11388 vinfos[1].foffset = j10;
11389 vinfos[1].indices[0] = _ij10[0];
11390 vinfos[1].indices[1] = _ij10[1];
11391 vinfos[1].maxsolutions = _nj10;
11392 vinfos[2].jointtype = 1;
11393 vinfos[2].foffset = j11;
11394 vinfos[2].indices[0] = _ij11[0];
11395 vinfos[2].indices[1] = _ij11[1];
11396 vinfos[2].maxsolutions = _nj11;
11397 vinfos[3].jointtype = 1;
11398 vinfos[3].foffset = j12;
11399 vinfos[3].indices[0] = _ij12[0];
11400 vinfos[3].indices[1] = _ij12[1];
11401 vinfos[3].maxsolutions = _nj12;
11402 vinfos[4].jointtype = 1;
11403 vinfos[4].foffset = j13;
11404 vinfos[4].indices[0] = _ij13[0];
11405 vinfos[4].indices[1] = _ij13[1];
11406 vinfos[4].maxsolutions = _nj13;
11407 vinfos[5].jointtype = 1;
11408 vinfos[5].foffset = j14;
11409 vinfos[5].indices[0] = _ij14[0];
11410 vinfos[5].indices[1] = _ij14[1];
11411 vinfos[5].maxsolutions = _nj14;
11412 std::vector<int> vfree(0);
11413 solutions.AddSolution(vinfos,vfree);
11414 }
11415 }
11416 }
11417 
11418 }
11419 
11420 }
11421 
11422 } else
11423 {
11424 {
11425 IkReal j10array[1], cj10array[1], sj10array[1];
11426 bool j10valid[1]={false};
11427 _nj10 = 1;
11428 IkReal x2585=((cj11)*(sj12));
11429 IkReal x2586=((r22)*(sj13));
11430 IkReal x2587=((r20)*(sj14));
11431 IkReal x2588=((cj14)*(sj11));
11432 IkReal x2589=((cj13)*(r20));
11433 IkReal x2590=((sj11)*(sj12));
11434 IkReal x2591=((cj13)*(r21)*(sj14));
11435 if( IKabs(((gconst46)*(((((cj11)*(cj14)*(r21)))+(((x2586)*(x2590)))+(((IkReal(-1.00000000000000))*(x2590)*(x2591)))+(((sj12)*(x2588)*(x2589)))+(((cj11)*(x2587))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst46)*(((((IkReal(-1.00000000000000))*(cj14)*(x2585)*(x2589)))+(((sj11)*(x2587)))+(((IkReal(-1.00000000000000))*(x2585)*(x2586)))+(((x2585)*(x2591)))+(((r21)*(x2588))))))) < IKFAST_ATAN2_MAGTHRESH )
11436  continue;
11437 j10array[0]=IKatan2(((gconst46)*(((((cj11)*(cj14)*(r21)))+(((x2586)*(x2590)))+(((IkReal(-1.00000000000000))*(x2590)*(x2591)))+(((sj12)*(x2588)*(x2589)))+(((cj11)*(x2587)))))), ((gconst46)*(((((IkReal(-1.00000000000000))*(cj14)*(x2585)*(x2589)))+(((sj11)*(x2587)))+(((IkReal(-1.00000000000000))*(x2585)*(x2586)))+(((x2585)*(x2591)))+(((r21)*(x2588)))))));
11438 sj10array[0]=IKsin(j10array[0]);
11439 cj10array[0]=IKcos(j10array[0]);
11440 if( j10array[0] > IKPI )
11441 {
11442  j10array[0]-=IK2PI;
11443 }
11444 else if( j10array[0] < -IKPI )
11445 { j10array[0]+=IK2PI;
11446 }
11447 j10valid[0] = true;
11448 for(int ij10 = 0; ij10 < 1; ++ij10)
11449 {
11450 if( !j10valid[ij10] )
11451 {
11452  continue;
11453 }
11454 _ij10[0] = ij10; _ij10[1] = -1;
11455 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11456 {
11457 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11458 {
11459  j10valid[iij10]=false; _ij10[1] = iij10; break;
11460 }
11461 }
11462 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11463 {
11464 IkReal evalcond[6];
11465 IkReal x2592=IKsin(j10);
11466 IkReal x2593=IKcos(j10);
11467 IkReal x2594=((IkReal(1.00000000000000))*(cj13));
11468 IkReal x2595=((cj9)*(r02));
11469 IkReal x2596=((IkReal(1.00000000000000))*(sj13));
11470 IkReal x2597=((r11)*(sj9));
11471 IkReal x2598=((IkReal(1.00000000000000))*(cj14));
11472 IkReal x2599=((cj9)*(r01));
11473 IkReal x2600=((r21)*(sj14));
11474 IkReal x2601=((IkReal(1.00000000000000))*(sj12));
11475 IkReal x2602=((r10)*(sj9));
11476 IkReal x2603=((cj14)*(sj13));
11477 IkReal x2604=((cj14)*(r20));
11478 IkReal x2605=((IkReal(1.00000000000000))*(sj14));
11479 IkReal x2606=((r12)*(sj9));
11480 IkReal x2607=((cj13)*(cj14));
11481 IkReal x2608=((cj9)*(r00));
11482 IkReal x2609=((sj11)*(x2592));
11483 IkReal x2610=((cj11)*(x2592));
11484 IkReal x2611=((sj11)*(x2593));
11485 IkReal x2612=((cj11)*(x2593));
11486 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x2601)*(x2610)))+(((IkReal(-1.00000000000000))*(x2601)*(x2611))));
11487 evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x2596)))+(((IkReal(-1.00000000000000))*(x2612)))+(((cj13)*(x2600)))+(((IkReal(-1.00000000000000))*(x2594)*(x2604)))+(x2609));
11488 evalcond[2]=((((IkReal(-1.00000000000000))*(x2596)*(x2604)))+(((sj13)*(x2600)))+(((cj12)*(x2610)))+(((cj12)*(x2611)))+(((cj13)*(r22))));
11489 evalcond[3]=((((IkReal(-1.00000000000000))*(x2598)*(x2599)))+(((sj12)*(x2609)))+(((IkReal(-1.00000000000000))*(x2597)*(x2598)))+(((IkReal(-1.00000000000000))*(x2602)*(x2605)))+(((IkReal(-1.00000000000000))*(x2605)*(x2608)))+(((IkReal(-1.00000000000000))*(x2601)*(x2612))));
11490 evalcond[4]=((((x2607)*(x2608)))+(((sj13)*(x2606)))+(((x2602)*(x2607)))+(((sj13)*(x2595)))+(((IkReal(-1.00000000000000))*(sj14)*(x2594)*(x2599)))+(((IkReal(-1.00000000000000))*(sj14)*(x2594)*(x2597)))+(x2611)+(x2610));
11491 evalcond[5]=((((x2603)*(x2608)))+(((IkReal(-1.00000000000000))*(x2594)*(x2595)))+(((x2602)*(x2603)))+(((IkReal(-1.00000000000000))*(sj14)*(x2596)*(x2597)))+(((IkReal(-1.00000000000000))*(sj14)*(x2596)*(x2599)))+(((IkReal(-1.00000000000000))*(cj12)*(x2609)))+(((cj12)*(x2612)))+(((IkReal(-1.00000000000000))*(x2594)*(x2606))));
11492 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 )
11493 {
11494 continue;
11495 }
11496 }
11497 
11498 {
11499 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11500 vinfos[0].jointtype = 1;
11501 vinfos[0].foffset = j9;
11502 vinfos[0].indices[0] = _ij9[0];
11503 vinfos[0].indices[1] = _ij9[1];
11504 vinfos[0].maxsolutions = _nj9;
11505 vinfos[1].jointtype = 1;
11506 vinfos[1].foffset = j10;
11507 vinfos[1].indices[0] = _ij10[0];
11508 vinfos[1].indices[1] = _ij10[1];
11509 vinfos[1].maxsolutions = _nj10;
11510 vinfos[2].jointtype = 1;
11511 vinfos[2].foffset = j11;
11512 vinfos[2].indices[0] = _ij11[0];
11513 vinfos[2].indices[1] = _ij11[1];
11514 vinfos[2].maxsolutions = _nj11;
11515 vinfos[3].jointtype = 1;
11516 vinfos[3].foffset = j12;
11517 vinfos[3].indices[0] = _ij12[0];
11518 vinfos[3].indices[1] = _ij12[1];
11519 vinfos[3].maxsolutions = _nj12;
11520 vinfos[4].jointtype = 1;
11521 vinfos[4].foffset = j13;
11522 vinfos[4].indices[0] = _ij13[0];
11523 vinfos[4].indices[1] = _ij13[1];
11524 vinfos[4].maxsolutions = _nj13;
11525 vinfos[5].jointtype = 1;
11526 vinfos[5].foffset = j14;
11527 vinfos[5].indices[0] = _ij14[0];
11528 vinfos[5].indices[1] = _ij14[1];
11529 vinfos[5].maxsolutions = _nj14;
11530 std::vector<int> vfree(0);
11531 solutions.AddSolution(vinfos,vfree);
11532 }
11533 }
11534 }
11535 
11536 }
11537 
11538 }
11539 }
11540 }
11541 
11542 }
11543 
11544 }
11545 
11546 } else
11547 {
11548 {
11549 IkReal j11array[1], cj11array[1], sj11array[1];
11550 bool j11valid[1]={false};
11551 _nj11 = 1;
11552 IkReal x2613=((IkReal(4.00000000000000))*(sj14));
11553 IkReal x2614=((IkReal(4.00000000000000))*(cj14));
11554 if( IKabs(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npx)*(x2613)))+(((IkReal(-1.00000000000000))*(npy)*(x2614))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2613)))+(((IkReal(-0.360000000000000))*(cj13)))+(((cj13)*(npx)*(x2614))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npx)*(x2613)))+(((IkReal(-1.00000000000000))*(npy)*(x2614)))))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2613)))+(((IkReal(-0.360000000000000))*(cj13)))+(((cj13)*(npx)*(x2614)))))-1) <= IKFAST_SINCOS_THRESH )
11555  continue;
11556 j11array[0]=IKatan2(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npx)*(x2613)))+(((IkReal(-1.00000000000000))*(npy)*(x2614)))))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2613)))+(((IkReal(-0.360000000000000))*(cj13)))+(((cj13)*(npx)*(x2614)))));
11557 sj11array[0]=IKsin(j11array[0]);
11558 cj11array[0]=IKcos(j11array[0]);
11559 if( j11array[0] > IKPI )
11560 {
11561  j11array[0]-=IK2PI;
11562 }
11563 else if( j11array[0] < -IKPI )
11564 { j11array[0]+=IK2PI;
11565 }
11566 j11valid[0] = true;
11567 for(int ij11 = 0; ij11 < 1; ++ij11)
11568 {
11569 if( !j11valid[ij11] )
11570 {
11571  continue;
11572 }
11573 _ij11[0] = ij11; _ij11[1] = -1;
11574 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
11575 {
11576 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
11577 {
11578  j11valid[iij11]=false; _ij11[1] = iij11; break;
11579 }
11580 }
11581 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
11582 {
11583 IkReal evalcond[3];
11584 IkReal x2615=IKsin(j11);
11585 IkReal x2616=((IkReal(1.00000000000000))*(sj13));
11586 IkReal x2617=((cj14)*(npx));
11587 IkReal x2618=((npy)*(sj14));
11588 IkReal x2619=((IkReal(0.250000000000000))*(x2615));
11589 evalcond[0]=((((IkReal(0.0950000000000000))*(cj12)))+(((sj12)*(x2619)))+(((cj14)*(npy)))+(((IkReal(-0.0300000000000000))*(sj12)))+(((npx)*(sj14))));
11590 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x2616)))+(((IkReal(-1.00000000000000))*(cj13)*(x2617)))+(((cj13)*(x2618))));
11591 evalcond[2]=((((sj13)*(x2618)))+(((IkReal(0.0950000000000000))*(sj12)))+(((cj13)*(npz)))+(((IkReal(0.0300000000000000))*(cj12)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x2616)*(x2617)))+(((IkReal(-1.00000000000000))*(cj12)*(x2619))));
11592 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
11593 {
11594 continue;
11595 }
11596 }
11597 
11598 {
11599 IkReal dummyeval[1];
11600 IkReal gconst46;
11601 gconst46=IKsign(((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11))))));
11602 dummyeval[0]=((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11)))));
11603 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
11604 {
11605 {
11606 IkReal dummyeval[1];
11607 IkReal gconst47;
11608 gconst47=IKsign(((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11))))));
11609 dummyeval[0]=((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11)))));
11610 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
11611 {
11612 {
11613 IkReal evalcond[9];
11614 IkReal x2620=((r00)*(sj9));
11615 IkReal x2621=((IkReal(1.00000000000000))*(sj13));
11616 IkReal x2622=((cj13)*(sj14));
11617 IkReal x2623=((cj9)*(sj14));
11618 IkReal x2624=((cj13)*(r02));
11619 IkReal x2625=((sj13)*(sj14));
11620 IkReal x2626=((r01)*(sj9));
11621 IkReal x2627=((cj9)*(sj13));
11622 IkReal x2628=((IkReal(1.00000000000000))*(cj9));
11623 IkReal x2629=((cj14)*(r10));
11624 IkReal x2630=((cj14)*(npx));
11625 IkReal x2631=((IkReal(1.00000000000000))*(cj13));
11626 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
11627 evalcond[1]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(sj11))));
11628 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(npz)*(x2621)))+(((IkReal(-1.00000000000000))*(x2630)*(x2631)))+(((npy)*(x2622))));
11629 evalcond[3]=((((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2628)))+(((IkReal(-1.00000000000000))*(r10)*(x2623)))+(((cj14)*(x2626)))+(((sj14)*(x2620))));
11630 evalcond[4]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2621)))+(((r21)*(x2625)))+(((cj13)*(r22))));
11631 evalcond[5]=((IkReal(0.0950000000000000))+(((cj13)*(npz)))+(((IkReal(-1.00000000000000))*(x2621)*(x2630)))+(((IkReal(0.0900000000000000))*(sj13)))+(((npy)*(x2625))));
11632 evalcond[6]=((((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2621)))+(((cj13)*(cj9)*(x2629)))+(((IkReal(-1.00000000000000))*(r11)*(x2622)*(x2628)))+(((IkReal(-1.00000000000000))*(cj14)*(x2620)*(x2631)))+(((x2622)*(x2626)))+(((r12)*(x2627))));
11633 evalcond[7]=((IkReal(1.00000000000000))+(((sj9)*(x2624)))+(((x2627)*(x2629)))+(((IkReal(-1.00000000000000))*(cj14)*(x2620)*(x2621)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x2628)))+(((IkReal(-1.00000000000000))*(r11)*(x2621)*(x2623)))+(((x2625)*(x2626))));
11634 evalcond[8]=((((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x2621)))+(((IkReal(-1.00000000000000))*(x2624)*(x2628)))+(((sj13)*(sj9)*(x2629)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2631)))+(((IkReal(-1.00000000000000))*(r01)*(x2621)*(x2623)))+(((cj14)*(r00)*(x2627))));
11635 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 )
11636 {
11637 {
11638 IkReal dummyeval[1];
11639 IkReal gconst48;
11640 gconst48=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
11641 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
11642 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
11643 {
11644 {
11645 IkReal dummyeval[1];
11646 IkReal gconst49;
11647 gconst49=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
11648 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
11649 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
11650 {
11651 continue;
11652 
11653 } else
11654 {
11655 {
11656 IkReal j10array[1], cj10array[1], sj10array[1];
11657 bool j10valid[1]={false};
11658 _nj10 = 1;
11659 IkReal x2632=((IkReal(1.00000000000000))*(cj11));
11660 IkReal x2633=((sj11)*(sj14));
11661 IkReal x2634=((r10)*(sj9));
11662 IkReal x2635=((cj9)*(r00));
11663 IkReal x2636=((cj14)*(sj11));
11664 IkReal x2637=((r11)*(sj9));
11665 IkReal x2638=((cj14)*(cj9)*(r01));
11666 if( IKabs(((gconst49)*(((((cj9)*(r01)*(x2636)))+(((cj11)*(cj14)*(r21)))+(((cj11)*(r20)*(sj14)))+(((x2633)*(x2635)))+(((x2633)*(x2634)))+(((x2636)*(x2637))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst49)*(((((IkReal(-1.00000000000000))*(sj14)*(x2632)*(x2634)))+(((IkReal(-1.00000000000000))*(sj14)*(x2632)*(x2635)))+(((r21)*(x2636)))+(((IkReal(-1.00000000000000))*(x2632)*(x2638)))+(((r20)*(x2633)))+(((IkReal(-1.00000000000000))*(cj14)*(x2632)*(x2637))))))) < IKFAST_ATAN2_MAGTHRESH )
11667  continue;
11668 j10array[0]=IKatan2(((gconst49)*(((((cj9)*(r01)*(x2636)))+(((cj11)*(cj14)*(r21)))+(((cj11)*(r20)*(sj14)))+(((x2633)*(x2635)))+(((x2633)*(x2634)))+(((x2636)*(x2637)))))), ((gconst49)*(((((IkReal(-1.00000000000000))*(sj14)*(x2632)*(x2634)))+(((IkReal(-1.00000000000000))*(sj14)*(x2632)*(x2635)))+(((r21)*(x2636)))+(((IkReal(-1.00000000000000))*(x2632)*(x2638)))+(((r20)*(x2633)))+(((IkReal(-1.00000000000000))*(cj14)*(x2632)*(x2637)))))));
11669 sj10array[0]=IKsin(j10array[0]);
11670 cj10array[0]=IKcos(j10array[0]);
11671 if( j10array[0] > IKPI )
11672 {
11673  j10array[0]-=IK2PI;
11674 }
11675 else if( j10array[0] < -IKPI )
11676 { j10array[0]+=IK2PI;
11677 }
11678 j10valid[0] = true;
11679 for(int ij10 = 0; ij10 < 1; ++ij10)
11680 {
11681 if( !j10valid[ij10] )
11682 {
11683  continue;
11684 }
11685 _ij10[0] = ij10; _ij10[1] = -1;
11686 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11687 {
11688 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11689 {
11690  j10valid[iij10]=false; _ij10[1] = iij10; break;
11691 }
11692 }
11693 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11694 {
11695 IkReal evalcond[4];
11696 IkReal x2639=IKsin(j10);
11697 IkReal x2640=IKcos(j10);
11698 IkReal x2641=((cj13)*(sj14));
11699 IkReal x2642=((cj13)*(cj14));
11700 IkReal x2643=((r10)*(sj9));
11701 IkReal x2644=((IkReal(1.00000000000000))*(cj9));
11702 IkReal x2645=((sj11)*(x2639));
11703 IkReal x2646=((IkReal(1.00000000000000))*(x2640));
11704 IkReal x2647=((cj11)*(x2639));
11705 IkReal x2648=((IkReal(1.00000000000000))*(r11)*(sj9));
11706 IkReal x2649=((cj11)*(x2646));
11707 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x2646)))+(((IkReal(-1.00000000000000))*(x2647))));
11708 evalcond[1]=((((r21)*(x2641)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x2649)))+(((IkReal(-1.00000000000000))*(r20)*(x2642)))+(x2645));
11709 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(x2648)))+(((IkReal(-1.00000000000000))*(sj14)*(x2643)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2644)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2644)))+(((IkReal(-1.00000000000000))*(x2649)))+(x2645));
11710 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x2641)*(x2644)))+(((cj9)*(r00)*(x2642)))+(((cj9)*(r02)*(sj13)))+(((sj11)*(x2640)))+(((IkReal(-1.00000000000000))*(x2641)*(x2648)))+(((x2642)*(x2643)))+(((r12)*(sj13)*(sj9)))+(x2647));
11711 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
11712 {
11713 continue;
11714 }
11715 }
11716 
11717 {
11718 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11719 vinfos[0].jointtype = 1;
11720 vinfos[0].foffset = j9;
11721 vinfos[0].indices[0] = _ij9[0];
11722 vinfos[0].indices[1] = _ij9[1];
11723 vinfos[0].maxsolutions = _nj9;
11724 vinfos[1].jointtype = 1;
11725 vinfos[1].foffset = j10;
11726 vinfos[1].indices[0] = _ij10[0];
11727 vinfos[1].indices[1] = _ij10[1];
11728 vinfos[1].maxsolutions = _nj10;
11729 vinfos[2].jointtype = 1;
11730 vinfos[2].foffset = j11;
11731 vinfos[2].indices[0] = _ij11[0];
11732 vinfos[2].indices[1] = _ij11[1];
11733 vinfos[2].maxsolutions = _nj11;
11734 vinfos[3].jointtype = 1;
11735 vinfos[3].foffset = j12;
11736 vinfos[3].indices[0] = _ij12[0];
11737 vinfos[3].indices[1] = _ij12[1];
11738 vinfos[3].maxsolutions = _nj12;
11739 vinfos[4].jointtype = 1;
11740 vinfos[4].foffset = j13;
11741 vinfos[4].indices[0] = _ij13[0];
11742 vinfos[4].indices[1] = _ij13[1];
11743 vinfos[4].maxsolutions = _nj13;
11744 vinfos[5].jointtype = 1;
11745 vinfos[5].foffset = j14;
11746 vinfos[5].indices[0] = _ij14[0];
11747 vinfos[5].indices[1] = _ij14[1];
11748 vinfos[5].maxsolutions = _nj14;
11749 std::vector<int> vfree(0);
11750 solutions.AddSolution(vinfos,vfree);
11751 }
11752 }
11753 }
11754 
11755 }
11756 
11757 }
11758 
11759 } else
11760 {
11761 {
11762 IkReal j10array[1], cj10array[1], sj10array[1];
11763 bool j10valid[1]={false};
11764 _nj10 = 1;
11765 IkReal x2650=((cj11)*(r20));
11766 IkReal x2651=((IkReal(1.00000000000000))*(cj13));
11767 IkReal x2652=((r21)*(sj14));
11768 IkReal x2653=((r22)*(sj13));
11769 IkReal x2654=((r20)*(sj11));
11770 IkReal x2655=((cj14)*(r21));
11771 if( IKabs(((gconst48)*(((((sj14)*(x2650)))+(((IkReal(-1.00000000000000))*(sj11)*(x2651)*(x2652)))+(((cj13)*(cj14)*(x2654)))+(((sj11)*(x2653)))+(((cj11)*(x2655))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst48)*(((((sj14)*(x2654)))+(((sj11)*(x2655)))+(((IkReal(-1.00000000000000))*(cj14)*(x2650)*(x2651)))+(((IkReal(-1.00000000000000))*(cj11)*(x2653)))+(((cj11)*(cj13)*(x2652))))))) < IKFAST_ATAN2_MAGTHRESH )
11772  continue;
11773 j10array[0]=IKatan2(((gconst48)*(((((sj14)*(x2650)))+(((IkReal(-1.00000000000000))*(sj11)*(x2651)*(x2652)))+(((cj13)*(cj14)*(x2654)))+(((sj11)*(x2653)))+(((cj11)*(x2655)))))), ((gconst48)*(((((sj14)*(x2654)))+(((sj11)*(x2655)))+(((IkReal(-1.00000000000000))*(cj14)*(x2650)*(x2651)))+(((IkReal(-1.00000000000000))*(cj11)*(x2653)))+(((cj11)*(cj13)*(x2652)))))));
11774 sj10array[0]=IKsin(j10array[0]);
11775 cj10array[0]=IKcos(j10array[0]);
11776 if( j10array[0] > IKPI )
11777 {
11778  j10array[0]-=IK2PI;
11779 }
11780 else if( j10array[0] < -IKPI )
11781 { j10array[0]+=IK2PI;
11782 }
11783 j10valid[0] = true;
11784 for(int ij10 = 0; ij10 < 1; ++ij10)
11785 {
11786 if( !j10valid[ij10] )
11787 {
11788  continue;
11789 }
11790 _ij10[0] = ij10; _ij10[1] = -1;
11791 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11792 {
11793 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11794 {
11795  j10valid[iij10]=false; _ij10[1] = iij10; break;
11796 }
11797 }
11798 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11799 {
11800 IkReal evalcond[4];
11801 IkReal x2656=IKsin(j10);
11802 IkReal x2657=IKcos(j10);
11803 IkReal x2658=((cj13)*(sj14));
11804 IkReal x2659=((cj13)*(cj14));
11805 IkReal x2660=((r10)*(sj9));
11806 IkReal x2661=((IkReal(1.00000000000000))*(cj9));
11807 IkReal x2662=((sj11)*(x2656));
11808 IkReal x2663=((IkReal(1.00000000000000))*(x2657));
11809 IkReal x2664=((cj11)*(x2656));
11810 IkReal x2665=((IkReal(1.00000000000000))*(r11)*(sj9));
11811 IkReal x2666=((cj11)*(x2663));
11812 evalcond[0]=((((IkReal(-1.00000000000000))*(x2664)))+(((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x2663))));
11813 evalcond[1]=((((r21)*(x2658)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x2666)))+(((IkReal(-1.00000000000000))*(r20)*(x2659)))+(x2662));
11814 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(x2665)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2661)))+(((IkReal(-1.00000000000000))*(x2666)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2661)))+(x2662)+(((IkReal(-1.00000000000000))*(sj14)*(x2660))));
11815 evalcond[3]=((((x2659)*(x2660)))+(((cj9)*(r00)*(x2659)))+(((sj11)*(x2657)))+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(x2658)*(x2665)))+(((IkReal(-1.00000000000000))*(r01)*(x2658)*(x2661)))+(((r12)*(sj13)*(sj9)))+(x2664));
11816 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
11817 {
11818 continue;
11819 }
11820 }
11821 
11822 {
11823 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11824 vinfos[0].jointtype = 1;
11825 vinfos[0].foffset = j9;
11826 vinfos[0].indices[0] = _ij9[0];
11827 vinfos[0].indices[1] = _ij9[1];
11828 vinfos[0].maxsolutions = _nj9;
11829 vinfos[1].jointtype = 1;
11830 vinfos[1].foffset = j10;
11831 vinfos[1].indices[0] = _ij10[0];
11832 vinfos[1].indices[1] = _ij10[1];
11833 vinfos[1].maxsolutions = _nj10;
11834 vinfos[2].jointtype = 1;
11835 vinfos[2].foffset = j11;
11836 vinfos[2].indices[0] = _ij11[0];
11837 vinfos[2].indices[1] = _ij11[1];
11838 vinfos[2].maxsolutions = _nj11;
11839 vinfos[3].jointtype = 1;
11840 vinfos[3].foffset = j12;
11841 vinfos[3].indices[0] = _ij12[0];
11842 vinfos[3].indices[1] = _ij12[1];
11843 vinfos[3].maxsolutions = _nj12;
11844 vinfos[4].jointtype = 1;
11845 vinfos[4].foffset = j13;
11846 vinfos[4].indices[0] = _ij13[0];
11847 vinfos[4].indices[1] = _ij13[1];
11848 vinfos[4].maxsolutions = _nj13;
11849 vinfos[5].jointtype = 1;
11850 vinfos[5].foffset = j14;
11851 vinfos[5].indices[0] = _ij14[0];
11852 vinfos[5].indices[1] = _ij14[1];
11853 vinfos[5].maxsolutions = _nj14;
11854 std::vector<int> vfree(0);
11855 solutions.AddSolution(vinfos,vfree);
11856 }
11857 }
11858 }
11859 
11860 }
11861 
11862 }
11863 
11864 } else
11865 {
11866 IkReal x2667=((r00)*(sj9));
11867 IkReal x2668=((IkReal(1.00000000000000))*(sj13));
11868 IkReal x2669=((cj13)*(sj14));
11869 IkReal x2670=((cj9)*(sj14));
11870 IkReal x2671=((cj13)*(r02));
11871 IkReal x2672=((sj13)*(sj14));
11872 IkReal x2673=((r01)*(sj9));
11873 IkReal x2674=((cj9)*(sj13));
11874 IkReal x2675=((IkReal(1.00000000000000))*(cj9));
11875 IkReal x2676=((cj14)*(r10));
11876 IkReal x2677=((cj14)*(npx));
11877 IkReal x2678=((IkReal(1.00000000000000))*(cj13));
11878 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
11879 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((cj14)*(npy)))+(((npx)*(sj14))));
11880 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((npy)*(x2669)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(x2677)*(x2678)))+(((IkReal(-1.00000000000000))*(npz)*(x2668))));
11881 evalcond[3]=((((sj14)*(x2667)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2675)))+(((IkReal(-1.00000000000000))*(r10)*(x2670)))+(((cj14)*(x2673))));
11882 evalcond[4]=((((r21)*(x2672)))+(((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2668)))+(((cj13)*(r22))));
11883 evalcond[5]=((IkReal(-0.0950000000000000))+(((npy)*(x2672)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x2668)*(x2677))));
11884 evalcond[6]=((((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2668)))+(((r12)*(x2674)))+(((IkReal(-1.00000000000000))*(cj14)*(x2667)*(x2678)))+(((IkReal(-1.00000000000000))*(r11)*(x2669)*(x2675)))+(((cj13)*(cj9)*(x2676)))+(((x2669)*(x2673))));
11885 evalcond[7]=((IkReal(-1.00000000000000))+(((x2672)*(x2673)))+(((IkReal(-1.00000000000000))*(cj14)*(x2667)*(x2668)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x2675)))+(((sj9)*(x2671)))+(((IkReal(-1.00000000000000))*(r11)*(x2668)*(x2670)))+(((x2674)*(x2676))));
11886 evalcond[8]=((((sj13)*(sj9)*(x2676)))+(((IkReal(-1.00000000000000))*(r01)*(x2668)*(x2670)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2678)))+(((IkReal(-1.00000000000000))*(x2671)*(x2675)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x2668)))+(((cj14)*(r00)*(x2674))));
11887 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 )
11888 {
11889 {
11890 IkReal dummyeval[1];
11891 IkReal gconst50;
11892 gconst50=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
11893 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
11894 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
11895 {
11896 {
11897 IkReal dummyeval[1];
11898 IkReal gconst51;
11899 gconst51=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
11900 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
11901 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
11902 {
11903 continue;
11904 
11905 } else
11906 {
11907 {
11908 IkReal j10array[1], cj10array[1], sj10array[1];
11909 bool j10valid[1]={false};
11910 _nj10 = 1;
11911 IkReal x2679=((IkReal(1.00000000000000))*(sj11));
11912 IkReal x2680=((cj14)*(r21));
11913 IkReal x2681=((IkReal(1.00000000000000))*(cj11));
11914 IkReal x2682=((r20)*(sj14));
11915 IkReal x2683=((cj9)*(r00)*(sj14));
11916 IkReal x2684=((cj14)*(r11)*(sj9));
11917 IkReal x2685=((cj14)*(cj9)*(r01));
11918 IkReal x2686=((r10)*(sj14)*(sj9));
11919 if( IKabs(((gconst51)*(((((IkReal(-1.00000000000000))*(x2680)*(x2681)))+(((IkReal(-1.00000000000000))*(x2679)*(x2683)))+(((IkReal(-1.00000000000000))*(x2679)*(x2686)))+(((IkReal(-1.00000000000000))*(x2679)*(x2685)))+(((IkReal(-1.00000000000000))*(x2679)*(x2684)))+(((IkReal(-1.00000000000000))*(x2681)*(x2682))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst51)*(((((IkReal(-1.00000000000000))*(x2679)*(x2682)))+(((IkReal(-1.00000000000000))*(x2679)*(x2680)))+(((cj11)*(x2683)))+(((cj11)*(x2684)))+(((cj11)*(x2685)))+(((cj11)*(x2686))))))) < IKFAST_ATAN2_MAGTHRESH )
11920  continue;
11921 j10array[0]=IKatan2(((gconst51)*(((((IkReal(-1.00000000000000))*(x2680)*(x2681)))+(((IkReal(-1.00000000000000))*(x2679)*(x2683)))+(((IkReal(-1.00000000000000))*(x2679)*(x2686)))+(((IkReal(-1.00000000000000))*(x2679)*(x2685)))+(((IkReal(-1.00000000000000))*(x2679)*(x2684)))+(((IkReal(-1.00000000000000))*(x2681)*(x2682)))))), ((gconst51)*(((((IkReal(-1.00000000000000))*(x2679)*(x2682)))+(((IkReal(-1.00000000000000))*(x2679)*(x2680)))+(((cj11)*(x2683)))+(((cj11)*(x2684)))+(((cj11)*(x2685)))+(((cj11)*(x2686)))))));
11922 sj10array[0]=IKsin(j10array[0]);
11923 cj10array[0]=IKcos(j10array[0]);
11924 if( j10array[0] > IKPI )
11925 {
11926  j10array[0]-=IK2PI;
11927 }
11928 else if( j10array[0] < -IKPI )
11929 { j10array[0]+=IK2PI;
11930 }
11931 j10valid[0] = true;
11932 for(int ij10 = 0; ij10 < 1; ++ij10)
11933 {
11934 if( !j10valid[ij10] )
11935 {
11936  continue;
11937 }
11938 _ij10[0] = ij10; _ij10[1] = -1;
11939 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
11940 {
11941 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
11942 {
11943  j10valid[iij10]=false; _ij10[1] = iij10; break;
11944 }
11945 }
11946 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
11947 {
11948 IkReal evalcond[4];
11949 IkReal x2687=IKsin(j10);
11950 IkReal x2688=IKcos(j10);
11951 IkReal x2689=((cj14)*(sj9));
11952 IkReal x2690=((IkReal(1.00000000000000))*(r11));
11953 IkReal x2691=((cj13)*(sj14));
11954 IkReal x2692=((IkReal(1.00000000000000))*(cj9));
11955 IkReal x2693=((cj13)*(cj14));
11956 IkReal x2694=((cj11)*(x2687));
11957 IkReal x2695=((sj11)*(x2688));
11958 IkReal x2696=((cj11)*(x2688));
11959 IkReal x2697=((sj11)*(x2687));
11960 IkReal x2698=((x2695)+(x2694));
11961 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(x2698));
11962 evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x2693)))+(((r21)*(x2691)))+(((IkReal(-1.00000000000000))*(x2696)))+(x2697));
11963 evalcond[2]=((((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2692)))+(((IkReal(-1.00000000000000))*(x2689)*(x2690)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2692)))+(((IkReal(-1.00000000000000))*(x2697)))+(x2696));
11964 evalcond[3]=((((cj9)*(r00)*(x2693)))+(((cj9)*(r02)*(sj13)))+(((cj13)*(r10)*(x2689)))+(((IkReal(-1.00000000000000))*(sj9)*(x2690)*(x2691)))+(((IkReal(-1.00000000000000))*(r01)*(x2691)*(x2692)))+(((r12)*(sj13)*(sj9)))+(x2698));
11965 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
11966 {
11967 continue;
11968 }
11969 }
11970 
11971 {
11972 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
11973 vinfos[0].jointtype = 1;
11974 vinfos[0].foffset = j9;
11975 vinfos[0].indices[0] = _ij9[0];
11976 vinfos[0].indices[1] = _ij9[1];
11977 vinfos[0].maxsolutions = _nj9;
11978 vinfos[1].jointtype = 1;
11979 vinfos[1].foffset = j10;
11980 vinfos[1].indices[0] = _ij10[0];
11981 vinfos[1].indices[1] = _ij10[1];
11982 vinfos[1].maxsolutions = _nj10;
11983 vinfos[2].jointtype = 1;
11984 vinfos[2].foffset = j11;
11985 vinfos[2].indices[0] = _ij11[0];
11986 vinfos[2].indices[1] = _ij11[1];
11987 vinfos[2].maxsolutions = _nj11;
11988 vinfos[3].jointtype = 1;
11989 vinfos[3].foffset = j12;
11990 vinfos[3].indices[0] = _ij12[0];
11991 vinfos[3].indices[1] = _ij12[1];
11992 vinfos[3].maxsolutions = _nj12;
11993 vinfos[4].jointtype = 1;
11994 vinfos[4].foffset = j13;
11995 vinfos[4].indices[0] = _ij13[0];
11996 vinfos[4].indices[1] = _ij13[1];
11997 vinfos[4].maxsolutions = _nj13;
11998 vinfos[5].jointtype = 1;
11999 vinfos[5].foffset = j14;
12000 vinfos[5].indices[0] = _ij14[0];
12001 vinfos[5].indices[1] = _ij14[1];
12002 vinfos[5].maxsolutions = _nj14;
12003 std::vector<int> vfree(0);
12004 solutions.AddSolution(vinfos,vfree);
12005 }
12006 }
12007 }
12008 
12009 }
12010 
12011 }
12012 
12013 } else
12014 {
12015 {
12016 IkReal j10array[1], cj10array[1], sj10array[1];
12017 bool j10valid[1]={false};
12018 _nj10 = 1;
12019 IkReal x2699=((cj13)*(sj11));
12020 IkReal x2700=((r21)*(sj14));
12021 IkReal x2701=((cj14)*(r20));
12022 IkReal x2702=((cj11)*(cj13));
12023 IkReal x2703=((r22)*(sj13));
12024 IkReal x2704=((r20)*(sj14));
12025 IkReal x2705=((cj14)*(r21));
12026 if( IKabs(((gconst50)*(((((x2699)*(x2700)))+(((IkReal(-1.00000000000000))*(sj11)*(x2703)))+(((IkReal(-1.00000000000000))*(x2699)*(x2701)))+(((cj11)*(x2705)))+(((cj11)*(x2704))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst50)*(((((IkReal(-1.00000000000000))*(x2700)*(x2702)))+(((sj11)*(x2704)))+(((sj11)*(x2705)))+(((x2701)*(x2702)))+(((cj11)*(x2703))))))) < IKFAST_ATAN2_MAGTHRESH )
12027  continue;
12028 j10array[0]=IKatan2(((gconst50)*(((((x2699)*(x2700)))+(((IkReal(-1.00000000000000))*(sj11)*(x2703)))+(((IkReal(-1.00000000000000))*(x2699)*(x2701)))+(((cj11)*(x2705)))+(((cj11)*(x2704)))))), ((gconst50)*(((((IkReal(-1.00000000000000))*(x2700)*(x2702)))+(((sj11)*(x2704)))+(((sj11)*(x2705)))+(((x2701)*(x2702)))+(((cj11)*(x2703)))))));
12029 sj10array[0]=IKsin(j10array[0]);
12030 cj10array[0]=IKcos(j10array[0]);
12031 if( j10array[0] > IKPI )
12032 {
12033  j10array[0]-=IK2PI;
12034 }
12035 else if( j10array[0] < -IKPI )
12036 { j10array[0]+=IK2PI;
12037 }
12038 j10valid[0] = true;
12039 for(int ij10 = 0; ij10 < 1; ++ij10)
12040 {
12041 if( !j10valid[ij10] )
12042 {
12043  continue;
12044 }
12045 _ij10[0] = ij10; _ij10[1] = -1;
12046 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12047 {
12048 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12049 {
12050  j10valid[iij10]=false; _ij10[1] = iij10; break;
12051 }
12052 }
12053 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12054 {
12055 IkReal evalcond[4];
12056 IkReal x2706=IKsin(j10);
12057 IkReal x2707=IKcos(j10);
12058 IkReal x2708=((cj14)*(sj9));
12059 IkReal x2709=((IkReal(1.00000000000000))*(r11));
12060 IkReal x2710=((cj13)*(sj14));
12061 IkReal x2711=((IkReal(1.00000000000000))*(cj9));
12062 IkReal x2712=((cj13)*(cj14));
12063 IkReal x2713=((cj11)*(x2706));
12064 IkReal x2714=((sj11)*(x2707));
12065 IkReal x2715=((cj11)*(x2707));
12066 IkReal x2716=((sj11)*(x2706));
12067 IkReal x2717=((x2713)+(x2714));
12068 evalcond[0]=((x2717)+(((cj14)*(r21)))+(((r20)*(sj14))));
12069 evalcond[1]=((x2716)+(((IkReal(-1.00000000000000))*(x2715)))+(((IkReal(-1.00000000000000))*(r20)*(x2712)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x2710))));
12070 evalcond[2]=((x2715)+(((IkReal(-1.00000000000000))*(x2716)))+(((IkReal(-1.00000000000000))*(x2708)*(x2709)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2711)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2711))));
12071 evalcond[3]=((x2717)+(((cj9)*(r02)*(sj13)))+(((cj9)*(r00)*(x2712)))+(((IkReal(-1.00000000000000))*(sj9)*(x2709)*(x2710)))+(((IkReal(-1.00000000000000))*(r01)*(x2710)*(x2711)))+(((cj13)*(r10)*(x2708)))+(((r12)*(sj13)*(sj9))));
12072 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
12073 {
12074 continue;
12075 }
12076 }
12077 
12078 {
12079 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12080 vinfos[0].jointtype = 1;
12081 vinfos[0].foffset = j9;
12082 vinfos[0].indices[0] = _ij9[0];
12083 vinfos[0].indices[1] = _ij9[1];
12084 vinfos[0].maxsolutions = _nj9;
12085 vinfos[1].jointtype = 1;
12086 vinfos[1].foffset = j10;
12087 vinfos[1].indices[0] = _ij10[0];
12088 vinfos[1].indices[1] = _ij10[1];
12089 vinfos[1].maxsolutions = _nj10;
12090 vinfos[2].jointtype = 1;
12091 vinfos[2].foffset = j11;
12092 vinfos[2].indices[0] = _ij11[0];
12093 vinfos[2].indices[1] = _ij11[1];
12094 vinfos[2].maxsolutions = _nj11;
12095 vinfos[3].jointtype = 1;
12096 vinfos[3].foffset = j12;
12097 vinfos[3].indices[0] = _ij12[0];
12098 vinfos[3].indices[1] = _ij12[1];
12099 vinfos[3].maxsolutions = _nj12;
12100 vinfos[4].jointtype = 1;
12101 vinfos[4].foffset = j13;
12102 vinfos[4].indices[0] = _ij13[0];
12103 vinfos[4].indices[1] = _ij13[1];
12104 vinfos[4].maxsolutions = _nj13;
12105 vinfos[5].jointtype = 1;
12106 vinfos[5].foffset = j14;
12107 vinfos[5].indices[0] = _ij14[0];
12108 vinfos[5].indices[1] = _ij14[1];
12109 vinfos[5].maxsolutions = _nj14;
12110 std::vector<int> vfree(0);
12111 solutions.AddSolution(vinfos,vfree);
12112 }
12113 }
12114 }
12115 
12116 }
12117 
12118 }
12119 
12120 } else
12121 {
12122 IkReal x2718=((cj9)*(r10));
12123 IkReal x2719=((cj13)*(cj14));
12124 IkReal x2720=((sj14)*(sj9));
12125 IkReal x2721=((IkReal(1.00000000000000))*(sj14));
12126 IkReal x2722=((cj9)*(sj13));
12127 IkReal x2723=((r02)*(sj9));
12128 IkReal x2724=((cj13)*(cj9));
12129 IkReal x2725=((cj14)*(r01));
12130 IkReal x2726=((IkReal(1.00000000000000))*(npx));
12131 IkReal x2727=((cj14)*(sj13));
12132 IkReal x2728=((IkReal(1.00000000000000))*(cj9));
12133 IkReal x2729=((npy)*(sj14));
12134 IkReal x2730=((IkReal(1.00000000000000))*(sj13));
12135 IkReal x2731=((IkReal(1.00000000000000))*(cj14)*(sj9));
12136 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
12137 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
12138 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
12139 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x2719)*(x2726)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(npz)*(x2730)))+(((cj13)*(x2729))));
12140 evalcond[4]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x2718)*(x2721)))+(((sj9)*(x2725)))+(((r00)*(x2720)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2728))));
12141 evalcond[5]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((IkReal(-1.00000000000000))*(x2726)*(x2727)))+(((sj13)*(x2729)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
12142 evalcond[6]=((((IkReal(-1.00000000000000))*(x2725)*(x2728)))+(((IkReal(-1.00000000000000))*(r11)*(x2731)))+(((IkReal(-1.00000000000000))*(cj9)*(r00)*(x2721)))+(((IkReal(-1.00000000000000))*(r10)*(x2720))));
12143 evalcond[7]=((((IkReal(-1.00000000000000))*(r11)*(x2721)*(x2724)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x2719)))+(((IkReal(-1.00000000000000))*(x2723)*(x2730)))+(((x2718)*(x2719)))+(((r12)*(x2722)))+(((cj13)*(r01)*(x2720))));
12144 evalcond[8]=((((IkReal(-1.00000000000000))*(r11)*(x2721)*(x2722)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x2727)))+(((IkReal(-1.00000000000000))*(r12)*(x2724)))+(((r01)*(sj13)*(x2720)))+(((x2718)*(x2727)))+(((cj13)*(x2723))));
12145 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 )
12146 {
12147 {
12148 IkReal dummyeval[1];
12149 IkReal gconst52;
12150 gconst52=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
12151 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
12152 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
12153 {
12154 {
12155 IkReal dummyeval[1];
12156 IkReal gconst53;
12157 gconst53=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
12158 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
12159 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
12160 {
12161 continue;
12162 
12163 } else
12164 {
12165 {
12166 IkReal j10array[1], cj10array[1], sj10array[1];
12167 bool j10valid[1]={false};
12168 _nj10 = 1;
12169 IkReal x2732=((cj13)*(sj14));
12170 IkReal x2733=((IkReal(1.00000000000000))*(cj11));
12171 IkReal x2734=((r11)*(sj9));
12172 IkReal x2735=((IkReal(1.00000000000000))*(sj11));
12173 IkReal x2736=((cj13)*(cj14));
12174 IkReal x2737=((cj11)*(sj13));
12175 IkReal x2738=((r12)*(sj9));
12176 IkReal x2739=((sj11)*(sj13));
12177 IkReal x2740=((cj9)*(r02));
12178 IkReal x2741=((cj9)*(r01));
12179 IkReal x2742=((r10)*(sj9));
12180 IkReal x2743=((cj9)*(r00));
12181 if( IKabs(((gconst53)*(((((IkReal(-1.00000000000000))*(x2732)*(x2733)*(x2741)))+(((cj11)*(x2736)*(x2743)))+(((cj11)*(x2736)*(x2742)))+(((IkReal(-1.00000000000000))*(r20)*(x2735)*(x2736)))+(((IkReal(-1.00000000000000))*(x2732)*(x2733)*(x2734)))+(((r21)*(sj11)*(x2732)))+(((x2737)*(x2738)))+(((x2737)*(x2740)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2735))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst53)*(((((cj11)*(r20)*(x2736)))+(((r22)*(x2737)))+(((x2739)*(x2740)))+(((IkReal(-1.00000000000000))*(x2732)*(x2735)*(x2741)))+(((IkReal(-1.00000000000000))*(r21)*(x2732)*(x2733)))+(((sj11)*(x2736)*(x2743)))+(((sj11)*(x2736)*(x2742)))+(((x2738)*(x2739)))+(((IkReal(-1.00000000000000))*(x2732)*(x2734)*(x2735))))))) < IKFAST_ATAN2_MAGTHRESH )
12182  continue;
12183 j10array[0]=IKatan2(((gconst53)*(((((IkReal(-1.00000000000000))*(x2732)*(x2733)*(x2741)))+(((cj11)*(x2736)*(x2743)))+(((cj11)*(x2736)*(x2742)))+(((IkReal(-1.00000000000000))*(r20)*(x2735)*(x2736)))+(((IkReal(-1.00000000000000))*(x2732)*(x2733)*(x2734)))+(((r21)*(sj11)*(x2732)))+(((x2737)*(x2738)))+(((x2737)*(x2740)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2735)))))), ((gconst53)*(((((cj11)*(r20)*(x2736)))+(((r22)*(x2737)))+(((x2739)*(x2740)))+(((IkReal(-1.00000000000000))*(x2732)*(x2735)*(x2741)))+(((IkReal(-1.00000000000000))*(r21)*(x2732)*(x2733)))+(((sj11)*(x2736)*(x2743)))+(((sj11)*(x2736)*(x2742)))+(((x2738)*(x2739)))+(((IkReal(-1.00000000000000))*(x2732)*(x2734)*(x2735)))))));
12184 sj10array[0]=IKsin(j10array[0]);
12185 cj10array[0]=IKcos(j10array[0]);
12186 if( j10array[0] > IKPI )
12187 {
12188  j10array[0]-=IK2PI;
12189 }
12190 else if( j10array[0] < -IKPI )
12191 { j10array[0]+=IK2PI;
12192 }
12193 j10valid[0] = true;
12194 for(int ij10 = 0; ij10 < 1; ++ij10)
12195 {
12196 if( !j10valid[ij10] )
12197 {
12198  continue;
12199 }
12200 _ij10[0] = ij10; _ij10[1] = -1;
12201 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12202 {
12203 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12204 {
12205  j10valid[iij10]=false; _ij10[1] = iij10; break;
12206 }
12207 }
12208 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12209 {
12210 IkReal evalcond[4];
12211 IkReal x2744=IKsin(j10);
12212 IkReal x2745=IKcos(j10);
12213 IkReal x2746=((IkReal(1.00000000000000))*(sj13));
12214 IkReal x2747=((r11)*(sj9));
12215 IkReal x2748=((cj9)*(r01));
12216 IkReal x2749=((r21)*(sj14));
12217 IkReal x2750=((cj9)*(r02));
12218 IkReal x2751=((sj13)*(sj9));
12219 IkReal x2752=((cj14)*(r10));
12220 IkReal x2753=((IkReal(1.00000000000000))*(cj13));
12221 IkReal x2754=((cj14)*(r20));
12222 IkReal x2755=((cj11)*(x2744));
12223 IkReal x2756=((sj11)*(x2745));
12224 IkReal x2757=((sj14)*(x2753));
12225 IkReal x2758=((sj11)*(x2744));
12226 IkReal x2759=((cj11)*(x2745));
12227 IkReal x2760=((cj14)*(cj9)*(r00));
12228 IkReal x2761=((x2756)+(x2755));
12229 evalcond[0]=((x2758)+(((IkReal(-1.00000000000000))*(x2753)*(x2754)))+(((IkReal(-1.00000000000000))*(x2759)))+(((cj13)*(x2749)))+(((IkReal(-1.00000000000000))*(r22)*(x2746))));
12230 evalcond[1]=((x2761)+(((sj13)*(x2749)))+(((IkReal(-1.00000000000000))*(x2746)*(x2754)))+(((cj13)*(r22))));
12231 evalcond[2]=((x2761)+(((r12)*(x2751)))+(((sj13)*(x2750)))+(((IkReal(-1.00000000000000))*(x2748)*(x2757)))+(((cj13)*(sj9)*(x2752)))+(((cj13)*(x2760)))+(((IkReal(-1.00000000000000))*(x2747)*(x2757))));
12232 evalcond[3]=((x2759)+(((IkReal(-1.00000000000000))*(sj14)*(x2746)*(x2747)))+(((IkReal(-1.00000000000000))*(sj14)*(x2746)*(x2748)))+(((sj13)*(x2760)))+(((IkReal(-1.00000000000000))*(x2750)*(x2753)))+(((IkReal(-1.00000000000000))*(x2758)))+(((x2751)*(x2752)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2753))));
12233 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
12234 {
12235 continue;
12236 }
12237 }
12238 
12239 {
12240 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12241 vinfos[0].jointtype = 1;
12242 vinfos[0].foffset = j9;
12243 vinfos[0].indices[0] = _ij9[0];
12244 vinfos[0].indices[1] = _ij9[1];
12245 vinfos[0].maxsolutions = _nj9;
12246 vinfos[1].jointtype = 1;
12247 vinfos[1].foffset = j10;
12248 vinfos[1].indices[0] = _ij10[0];
12249 vinfos[1].indices[1] = _ij10[1];
12250 vinfos[1].maxsolutions = _nj10;
12251 vinfos[2].jointtype = 1;
12252 vinfos[2].foffset = j11;
12253 vinfos[2].indices[0] = _ij11[0];
12254 vinfos[2].indices[1] = _ij11[1];
12255 vinfos[2].maxsolutions = _nj11;
12256 vinfos[3].jointtype = 1;
12257 vinfos[3].foffset = j12;
12258 vinfos[3].indices[0] = _ij12[0];
12259 vinfos[3].indices[1] = _ij12[1];
12260 vinfos[3].maxsolutions = _nj12;
12261 vinfos[4].jointtype = 1;
12262 vinfos[4].foffset = j13;
12263 vinfos[4].indices[0] = _ij13[0];
12264 vinfos[4].indices[1] = _ij13[1];
12265 vinfos[4].maxsolutions = _nj13;
12266 vinfos[5].jointtype = 1;
12267 vinfos[5].foffset = j14;
12268 vinfos[5].indices[0] = _ij14[0];
12269 vinfos[5].indices[1] = _ij14[1];
12270 vinfos[5].maxsolutions = _nj14;
12271 std::vector<int> vfree(0);
12272 solutions.AddSolution(vinfos,vfree);
12273 }
12274 }
12275 }
12276 
12277 }
12278 
12279 }
12280 
12281 } else
12282 {
12283 {
12284 IkReal j10array[1], cj10array[1], sj10array[1];
12285 bool j10valid[1]={false};
12286 _nj10 = 1;
12287 IkReal x2762=((cj13)*(sj11));
12288 IkReal x2763=((r21)*(sj14));
12289 IkReal x2764=((cj11)*(cj13));
12290 IkReal x2765=((cj11)*(sj13));
12291 IkReal x2766=((sj11)*(sj13));
12292 IkReal x2767=((IkReal(1.00000000000000))*(cj14)*(r20));
12293 if( IKabs(((gconst52)*(((((IkReal(-1.00000000000000))*(r22)*(x2766)))+(((x2762)*(x2763)))+(((IkReal(-1.00000000000000))*(x2762)*(x2767)))+(((r22)*(x2764)))+(((x2763)*(x2765)))+(((IkReal(-1.00000000000000))*(x2765)*(x2767))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst52)*(((((IkReal(-1.00000000000000))*(x2763)*(x2764)))+(((cj14)*(r20)*(x2764)))+(((r22)*(x2762)))+(((r22)*(x2765)))+(((IkReal(-1.00000000000000))*(x2766)*(x2767)))+(((x2763)*(x2766))))))) < IKFAST_ATAN2_MAGTHRESH )
12294  continue;
12295 j10array[0]=IKatan2(((gconst52)*(((((IkReal(-1.00000000000000))*(r22)*(x2766)))+(((x2762)*(x2763)))+(((IkReal(-1.00000000000000))*(x2762)*(x2767)))+(((r22)*(x2764)))+(((x2763)*(x2765)))+(((IkReal(-1.00000000000000))*(x2765)*(x2767)))))), ((gconst52)*(((((IkReal(-1.00000000000000))*(x2763)*(x2764)))+(((cj14)*(r20)*(x2764)))+(((r22)*(x2762)))+(((r22)*(x2765)))+(((IkReal(-1.00000000000000))*(x2766)*(x2767)))+(((x2763)*(x2766)))))));
12296 sj10array[0]=IKsin(j10array[0]);
12297 cj10array[0]=IKcos(j10array[0]);
12298 if( j10array[0] > IKPI )
12299 {
12300  j10array[0]-=IK2PI;
12301 }
12302 else if( j10array[0] < -IKPI )
12303 { j10array[0]+=IK2PI;
12304 }
12305 j10valid[0] = true;
12306 for(int ij10 = 0; ij10 < 1; ++ij10)
12307 {
12308 if( !j10valid[ij10] )
12309 {
12310  continue;
12311 }
12312 _ij10[0] = ij10; _ij10[1] = -1;
12313 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12314 {
12315 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12316 {
12317  j10valid[iij10]=false; _ij10[1] = iij10; break;
12318 }
12319 }
12320 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12321 {
12322 IkReal evalcond[4];
12323 IkReal x2768=IKsin(j10);
12324 IkReal x2769=IKcos(j10);
12325 IkReal x2770=((IkReal(1.00000000000000))*(sj13));
12326 IkReal x2771=((r11)*(sj9));
12327 IkReal x2772=((cj9)*(r01));
12328 IkReal x2773=((r21)*(sj14));
12329 IkReal x2774=((cj9)*(r02));
12330 IkReal x2775=((sj13)*(sj9));
12331 IkReal x2776=((cj14)*(r10));
12332 IkReal x2777=((IkReal(1.00000000000000))*(cj13));
12333 IkReal x2778=((cj14)*(r20));
12334 IkReal x2779=((cj11)*(x2768));
12335 IkReal x2780=((sj11)*(x2769));
12336 IkReal x2781=((sj14)*(x2777));
12337 IkReal x2782=((sj11)*(x2768));
12338 IkReal x2783=((cj11)*(x2769));
12339 IkReal x2784=((cj14)*(cj9)*(r00));
12340 IkReal x2785=((x2779)+(x2780));
12341 evalcond[0]=((x2782)+(((IkReal(-1.00000000000000))*(x2777)*(x2778)))+(((cj13)*(x2773)))+(((IkReal(-1.00000000000000))*(x2783)))+(((IkReal(-1.00000000000000))*(r22)*(x2770))));
12342 evalcond[1]=((x2785)+(((IkReal(-1.00000000000000))*(x2770)*(x2778)))+(((sj13)*(x2773)))+(((cj13)*(r22))));
12343 evalcond[2]=((x2785)+(((cj13)*(x2784)))+(((sj13)*(x2774)))+(((cj13)*(sj9)*(x2776)))+(((IkReal(-1.00000000000000))*(x2772)*(x2781)))+(((IkReal(-1.00000000000000))*(x2771)*(x2781)))+(((r12)*(x2775))));
12344 evalcond[3]=((x2783)+(((IkReal(-1.00000000000000))*(sj14)*(x2770)*(x2771)))+(((IkReal(-1.00000000000000))*(sj14)*(x2770)*(x2772)))+(((x2775)*(x2776)))+(((IkReal(-1.00000000000000))*(x2782)))+(((IkReal(-1.00000000000000))*(x2774)*(x2777)))+(((sj13)*(x2784)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2777))));
12345 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
12346 {
12347 continue;
12348 }
12349 }
12350 
12351 {
12352 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12353 vinfos[0].jointtype = 1;
12354 vinfos[0].foffset = j9;
12355 vinfos[0].indices[0] = _ij9[0];
12356 vinfos[0].indices[1] = _ij9[1];
12357 vinfos[0].maxsolutions = _nj9;
12358 vinfos[1].jointtype = 1;
12359 vinfos[1].foffset = j10;
12360 vinfos[1].indices[0] = _ij10[0];
12361 vinfos[1].indices[1] = _ij10[1];
12362 vinfos[1].maxsolutions = _nj10;
12363 vinfos[2].jointtype = 1;
12364 vinfos[2].foffset = j11;
12365 vinfos[2].indices[0] = _ij11[0];
12366 vinfos[2].indices[1] = _ij11[1];
12367 vinfos[2].maxsolutions = _nj11;
12368 vinfos[3].jointtype = 1;
12369 vinfos[3].foffset = j12;
12370 vinfos[3].indices[0] = _ij12[0];
12371 vinfos[3].indices[1] = _ij12[1];
12372 vinfos[3].maxsolutions = _nj12;
12373 vinfos[4].jointtype = 1;
12374 vinfos[4].foffset = j13;
12375 vinfos[4].indices[0] = _ij13[0];
12376 vinfos[4].indices[1] = _ij13[1];
12377 vinfos[4].maxsolutions = _nj13;
12378 vinfos[5].jointtype = 1;
12379 vinfos[5].foffset = j14;
12380 vinfos[5].indices[0] = _ij14[0];
12381 vinfos[5].indices[1] = _ij14[1];
12382 vinfos[5].maxsolutions = _nj14;
12383 std::vector<int> vfree(0);
12384 solutions.AddSolution(vinfos,vfree);
12385 }
12386 }
12387 }
12388 
12389 }
12390 
12391 }
12392 
12393 } else
12394 {
12395 IkReal x2786=((cj9)*(r10));
12396 IkReal x2787=((cj13)*(cj14));
12397 IkReal x2788=((sj14)*(sj9));
12398 IkReal x2789=((IkReal(1.00000000000000))*(sj14));
12399 IkReal x2790=((cj9)*(sj13));
12400 IkReal x2791=((r02)*(sj9));
12401 IkReal x2792=((cj13)*(cj9));
12402 IkReal x2793=((cj14)*(r01));
12403 IkReal x2794=((IkReal(1.00000000000000))*(npx));
12404 IkReal x2795=((cj14)*(sj13));
12405 IkReal x2796=((IkReal(1.00000000000000))*(cj9));
12406 IkReal x2797=((npy)*(sj14));
12407 IkReal x2798=((IkReal(1.00000000000000))*(sj13));
12408 IkReal x2799=((IkReal(1.00000000000000))*(cj14)*(sj9));
12409 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
12410 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
12411 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
12412 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x2798)))+(((IkReal(-1.00000000000000))*(x2787)*(x2794)))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x2797)))+(((IkReal(0.250000000000000))*(cj11))));
12413 evalcond[4]=((IkReal(-1.00000000000000))+(((sj9)*(x2793)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2796)))+(((IkReal(-1.00000000000000))*(x2786)*(x2789)))+(((r00)*(x2788))));
12414 evalcond[5]=((IkReal(-0.0300000000000000))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((sj13)*(x2797)))+(((IkReal(0.250000000000000))*(sj11)))+(((IkReal(-1.00000000000000))*(x2794)*(x2795))));
12415 evalcond[6]=((((IkReal(-1.00000000000000))*(cj9)*(r00)*(x2789)))+(((IkReal(-1.00000000000000))*(r10)*(x2788)))+(((IkReal(-1.00000000000000))*(x2793)*(x2796)))+(((IkReal(-1.00000000000000))*(r11)*(x2799))));
12416 evalcond[7]=((((IkReal(-1.00000000000000))*(r11)*(x2789)*(x2792)))+(((IkReal(-1.00000000000000))*(x2791)*(x2798)))+(((r12)*(x2790)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x2787)))+(((cj13)*(r01)*(x2788)))+(((x2786)*(x2787))));
12417 evalcond[8]=((((x2786)*(x2795)))+(((IkReal(-1.00000000000000))*(r11)*(x2789)*(x2790)))+(((cj13)*(x2791)))+(((r01)*(sj13)*(x2788)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x2795)))+(((IkReal(-1.00000000000000))*(r12)*(x2792))));
12418 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 )
12419 {
12420 {
12421 IkReal dummyeval[1];
12422 IkReal gconst54;
12423 gconst54=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
12424 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
12425 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
12426 {
12427 {
12428 IkReal dummyeval[1];
12429 IkReal gconst55;
12430 gconst55=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
12431 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
12432 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
12433 {
12434 continue;
12435 
12436 } else
12437 {
12438 {
12439 IkReal j10array[1], cj10array[1], sj10array[1];
12440 bool j10valid[1]={false};
12441 _nj10 = 1;
12442 IkReal x2800=((cj13)*(sj14));
12443 IkReal x2801=((IkReal(1.00000000000000))*(cj11));
12444 IkReal x2802=((r11)*(sj9));
12445 IkReal x2803=((cj11)*(sj13));
12446 IkReal x2804=((r12)*(sj9));
12447 IkReal x2805=((r10)*(sj9));
12448 IkReal x2806=((sj11)*(sj13));
12449 IkReal x2807=((cj9)*(r02));
12450 IkReal x2808=((IkReal(1.00000000000000))*(sj11));
12451 IkReal x2809=((cj9)*(r01));
12452 IkReal x2810=((cj9)*(r00));
12453 IkReal x2811=((cj13)*(cj14)*(sj11));
12454 IkReal x2812=((cj11)*(cj13)*(cj14));
12455 if( IKabs(((gconst55)*(((((x2810)*(x2812)))+(((IkReal(-1.00000000000000))*(r22)*(x2806)))+(((x2803)*(x2804)))+(((x2803)*(x2807)))+(((r21)*(sj11)*(x2800)))+(((x2805)*(x2812)))+(((IkReal(-1.00000000000000))*(x2800)*(x2801)*(x2809)))+(((IkReal(-1.00000000000000))*(x2800)*(x2801)*(x2802)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x2808))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst55)*(((((x2810)*(x2811)))+(((x2804)*(x2806)))+(((IkReal(-1.00000000000000))*(x2800)*(x2802)*(x2808)))+(((IkReal(-1.00000000000000))*(x2800)*(x2808)*(x2809)))+(((IkReal(-1.00000000000000))*(r21)*(x2800)*(x2801)))+(((x2805)*(x2811)))+(((r20)*(x2812)))+(((x2806)*(x2807)))+(((r22)*(x2803))))))) < IKFAST_ATAN2_MAGTHRESH )
12456  continue;
12457 j10array[0]=IKatan2(((gconst55)*(((((x2810)*(x2812)))+(((IkReal(-1.00000000000000))*(r22)*(x2806)))+(((x2803)*(x2804)))+(((x2803)*(x2807)))+(((r21)*(sj11)*(x2800)))+(((x2805)*(x2812)))+(((IkReal(-1.00000000000000))*(x2800)*(x2801)*(x2809)))+(((IkReal(-1.00000000000000))*(x2800)*(x2801)*(x2802)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x2808)))))), ((gconst55)*(((((x2810)*(x2811)))+(((x2804)*(x2806)))+(((IkReal(-1.00000000000000))*(x2800)*(x2802)*(x2808)))+(((IkReal(-1.00000000000000))*(x2800)*(x2808)*(x2809)))+(((IkReal(-1.00000000000000))*(r21)*(x2800)*(x2801)))+(((x2805)*(x2811)))+(((r20)*(x2812)))+(((x2806)*(x2807)))+(((r22)*(x2803)))))));
12458 sj10array[0]=IKsin(j10array[0]);
12459 cj10array[0]=IKcos(j10array[0]);
12460 if( j10array[0] > IKPI )
12461 {
12462  j10array[0]-=IK2PI;
12463 }
12464 else if( j10array[0] < -IKPI )
12465 { j10array[0]+=IK2PI;
12466 }
12467 j10valid[0] = true;
12468 for(int ij10 = 0; ij10 < 1; ++ij10)
12469 {
12470 if( !j10valid[ij10] )
12471 {
12472  continue;
12473 }
12474 _ij10[0] = ij10; _ij10[1] = -1;
12475 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12476 {
12477 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12478 {
12479  j10valid[iij10]=false; _ij10[1] = iij10; break;
12480 }
12481 }
12482 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12483 {
12484 IkReal evalcond[4];
12485 IkReal x2813=IKsin(j10);
12486 IkReal x2814=IKcos(j10);
12487 IkReal x2815=((IkReal(1.00000000000000))*(sj13));
12488 IkReal x2816=((r11)*(sj9));
12489 IkReal x2817=((cj9)*(r01));
12490 IkReal x2818=((IkReal(1.00000000000000))*(cj11));
12491 IkReal x2819=((r21)*(sj14));
12492 IkReal x2820=((cj9)*(r02));
12493 IkReal x2821=((sj13)*(sj9));
12494 IkReal x2822=((cj14)*(r10));
12495 IkReal x2823=((IkReal(1.00000000000000))*(cj13));
12496 IkReal x2824=((cj14)*(r20));
12497 IkReal x2825=((sj11)*(x2813));
12498 IkReal x2826=((sj14)*(x2823));
12499 IkReal x2827=((sj11)*(x2814));
12500 IkReal x2828=((cj14)*(cj9)*(r00));
12501 IkReal x2829=((x2814)*(x2818));
12502 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x2815)))+(((cj13)*(x2819)))+(((IkReal(-1.00000000000000))*(x2823)*(x2824)))+(((IkReal(-1.00000000000000))*(x2829)))+(x2825));
12503 evalcond[1]=((((IkReal(-1.00000000000000))*(x2813)*(x2818)))+(((IkReal(-1.00000000000000))*(x2827)))+(((cj13)*(r22)))+(((sj13)*(x2819)))+(((IkReal(-1.00000000000000))*(x2815)*(x2824))));
12504 evalcond[2]=((((IkReal(-1.00000000000000))*(x2816)*(x2826)))+(((cj13)*(x2828)))+(((sj13)*(x2820)))+(((IkReal(-1.00000000000000))*(x2817)*(x2826)))+(((cj11)*(x2813)))+(((cj13)*(sj9)*(x2822)))+(x2827)+(((r12)*(x2821))));
12505 evalcond[3]=((((IkReal(-1.00000000000000))*(x2820)*(x2823)))+(((x2821)*(x2822)))+(((sj13)*(x2828)))+(((IkReal(-1.00000000000000))*(sj14)*(x2815)*(x2817)))+(((IkReal(-1.00000000000000))*(sj14)*(x2815)*(x2816)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2823)))+(((IkReal(-1.00000000000000))*(x2829)))+(x2825));
12506 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
12507 {
12508 continue;
12509 }
12510 }
12511 
12512 {
12513 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12514 vinfos[0].jointtype = 1;
12515 vinfos[0].foffset = j9;
12516 vinfos[0].indices[0] = _ij9[0];
12517 vinfos[0].indices[1] = _ij9[1];
12518 vinfos[0].maxsolutions = _nj9;
12519 vinfos[1].jointtype = 1;
12520 vinfos[1].foffset = j10;
12521 vinfos[1].indices[0] = _ij10[0];
12522 vinfos[1].indices[1] = _ij10[1];
12523 vinfos[1].maxsolutions = _nj10;
12524 vinfos[2].jointtype = 1;
12525 vinfos[2].foffset = j11;
12526 vinfos[2].indices[0] = _ij11[0];
12527 vinfos[2].indices[1] = _ij11[1];
12528 vinfos[2].maxsolutions = _nj11;
12529 vinfos[3].jointtype = 1;
12530 vinfos[3].foffset = j12;
12531 vinfos[3].indices[0] = _ij12[0];
12532 vinfos[3].indices[1] = _ij12[1];
12533 vinfos[3].maxsolutions = _nj12;
12534 vinfos[4].jointtype = 1;
12535 vinfos[4].foffset = j13;
12536 vinfos[4].indices[0] = _ij13[0];
12537 vinfos[4].indices[1] = _ij13[1];
12538 vinfos[4].maxsolutions = _nj13;
12539 vinfos[5].jointtype = 1;
12540 vinfos[5].foffset = j14;
12541 vinfos[5].indices[0] = _ij14[0];
12542 vinfos[5].indices[1] = _ij14[1];
12543 vinfos[5].maxsolutions = _nj14;
12544 std::vector<int> vfree(0);
12545 solutions.AddSolution(vinfos,vfree);
12546 }
12547 }
12548 }
12549 
12550 }
12551 
12552 }
12553 
12554 } else
12555 {
12556 {
12557 IkReal j10array[1], cj10array[1], sj10array[1];
12558 bool j10valid[1]={false};
12559 _nj10 = 1;
12560 IkReal x2830=((r22)*(sj11));
12561 IkReal x2831=((IkReal(1.00000000000000))*(sj11));
12562 IkReal x2832=((IkReal(1.00000000000000))*(cj11));
12563 IkReal x2833=((cj14)*(r20));
12564 IkReal x2834=((cj13)*(r21)*(sj14));
12565 IkReal x2835=((r21)*(sj13)*(sj14));
12566 if( IKabs(((gconst54)*(((((cj13)*(sj11)*(x2833)))+(((sj13)*(x2830)))+(((cj11)*(cj13)*(r22)))+(((cj11)*(x2835)))+(((IkReal(-1.00000000000000))*(sj13)*(x2832)*(x2833)))+(((IkReal(-1.00000000000000))*(x2831)*(x2834))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst54)*(((((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2832)))+(((IkReal(-1.00000000000000))*(sj13)*(x2831)*(x2833)))+(((cj13)*(x2830)))+(((cj11)*(x2834)))+(((IkReal(-1.00000000000000))*(cj13)*(x2832)*(x2833)))+(((sj11)*(x2835))))))) < IKFAST_ATAN2_MAGTHRESH )
12567  continue;
12568 j10array[0]=IKatan2(((gconst54)*(((((cj13)*(sj11)*(x2833)))+(((sj13)*(x2830)))+(((cj11)*(cj13)*(r22)))+(((cj11)*(x2835)))+(((IkReal(-1.00000000000000))*(sj13)*(x2832)*(x2833)))+(((IkReal(-1.00000000000000))*(x2831)*(x2834)))))), ((gconst54)*(((((IkReal(-1.00000000000000))*(r22)*(sj13)*(x2832)))+(((IkReal(-1.00000000000000))*(sj13)*(x2831)*(x2833)))+(((cj13)*(x2830)))+(((cj11)*(x2834)))+(((IkReal(-1.00000000000000))*(cj13)*(x2832)*(x2833)))+(((sj11)*(x2835)))))));
12569 sj10array[0]=IKsin(j10array[0]);
12570 cj10array[0]=IKcos(j10array[0]);
12571 if( j10array[0] > IKPI )
12572 {
12573  j10array[0]-=IK2PI;
12574 }
12575 else if( j10array[0] < -IKPI )
12576 { j10array[0]+=IK2PI;
12577 }
12578 j10valid[0] = true;
12579 for(int ij10 = 0; ij10 < 1; ++ij10)
12580 {
12581 if( !j10valid[ij10] )
12582 {
12583  continue;
12584 }
12585 _ij10[0] = ij10; _ij10[1] = -1;
12586 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12587 {
12588 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12589 {
12590  j10valid[iij10]=false; _ij10[1] = iij10; break;
12591 }
12592 }
12593 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12594 {
12595 IkReal evalcond[4];
12596 IkReal x2836=IKsin(j10);
12597 IkReal x2837=IKcos(j10);
12598 IkReal x2838=((IkReal(1.00000000000000))*(sj13));
12599 IkReal x2839=((r11)*(sj9));
12600 IkReal x2840=((cj9)*(r01));
12601 IkReal x2841=((IkReal(1.00000000000000))*(cj11));
12602 IkReal x2842=((r21)*(sj14));
12603 IkReal x2843=((cj9)*(r02));
12604 IkReal x2844=((sj13)*(sj9));
12605 IkReal x2845=((cj14)*(r10));
12606 IkReal x2846=((IkReal(1.00000000000000))*(cj13));
12607 IkReal x2847=((cj14)*(r20));
12608 IkReal x2848=((sj11)*(x2836));
12609 IkReal x2849=((sj14)*(x2846));
12610 IkReal x2850=((sj11)*(x2837));
12611 IkReal x2851=((cj14)*(cj9)*(r00));
12612 IkReal x2852=((x2837)*(x2841));
12613 evalcond[0]=((((IkReal(-1.00000000000000))*(x2846)*(x2847)))+(((cj13)*(x2842)))+(((IkReal(-1.00000000000000))*(r22)*(x2838)))+(x2848)+(((IkReal(-1.00000000000000))*(x2852))));
12614 evalcond[1]=((((sj13)*(x2842)))+(((IkReal(-1.00000000000000))*(x2836)*(x2841)))+(((IkReal(-1.00000000000000))*(x2838)*(x2847)))+(((IkReal(-1.00000000000000))*(x2850)))+(((cj13)*(r22))));
12615 evalcond[2]=((((cj13)*(x2851)))+(((IkReal(-1.00000000000000))*(x2840)*(x2849)))+(((sj13)*(x2843)))+(((r12)*(x2844)))+(((IkReal(-1.00000000000000))*(x2839)*(x2849)))+(((cj11)*(x2836)))+(x2850)+(((cj13)*(sj9)*(x2845))));
12616 evalcond[3]=((((IkReal(-1.00000000000000))*(x2843)*(x2846)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x2846)))+(((x2844)*(x2845)))+(((IkReal(-1.00000000000000))*(sj14)*(x2838)*(x2839)))+(((sj13)*(x2851)))+(((IkReal(-1.00000000000000))*(sj14)*(x2838)*(x2840)))+(x2848)+(((IkReal(-1.00000000000000))*(x2852))));
12617 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
12618 {
12619 continue;
12620 }
12621 }
12622 
12623 {
12624 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12625 vinfos[0].jointtype = 1;
12626 vinfos[0].foffset = j9;
12627 vinfos[0].indices[0] = _ij9[0];
12628 vinfos[0].indices[1] = _ij9[1];
12629 vinfos[0].maxsolutions = _nj9;
12630 vinfos[1].jointtype = 1;
12631 vinfos[1].foffset = j10;
12632 vinfos[1].indices[0] = _ij10[0];
12633 vinfos[1].indices[1] = _ij10[1];
12634 vinfos[1].maxsolutions = _nj10;
12635 vinfos[2].jointtype = 1;
12636 vinfos[2].foffset = j11;
12637 vinfos[2].indices[0] = _ij11[0];
12638 vinfos[2].indices[1] = _ij11[1];
12639 vinfos[2].maxsolutions = _nj11;
12640 vinfos[3].jointtype = 1;
12641 vinfos[3].foffset = j12;
12642 vinfos[3].indices[0] = _ij12[0];
12643 vinfos[3].indices[1] = _ij12[1];
12644 vinfos[3].maxsolutions = _nj12;
12645 vinfos[4].jointtype = 1;
12646 vinfos[4].foffset = j13;
12647 vinfos[4].indices[0] = _ij13[0];
12648 vinfos[4].indices[1] = _ij13[1];
12649 vinfos[4].maxsolutions = _nj13;
12650 vinfos[5].jointtype = 1;
12651 vinfos[5].foffset = j14;
12652 vinfos[5].indices[0] = _ij14[0];
12653 vinfos[5].indices[1] = _ij14[1];
12654 vinfos[5].maxsolutions = _nj14;
12655 std::vector<int> vfree(0);
12656 solutions.AddSolution(vinfos,vfree);
12657 }
12658 }
12659 }
12660 
12661 }
12662 
12663 }
12664 
12665 } else
12666 {
12667 if( 1 )
12668 {
12669 continue;
12670 
12671 } else
12672 {
12673 }
12674 }
12675 }
12676 }
12677 }
12678 }
12679 
12680 } else
12681 {
12682 {
12683 IkReal j10array[1], cj10array[1], sj10array[1];
12684 bool j10valid[1]={false};
12685 _nj10 = 1;
12686 IkReal x2853=((r21)*(sj14));
12687 IkReal x2854=((cj12)*(cj13));
12688 IkReal x2855=((sj11)*(sj13));
12689 IkReal x2856=((cj14)*(r20));
12690 IkReal x2857=((IkReal(1.00000000000000))*(sj11));
12691 IkReal x2858=((cj12)*(r22));
12692 IkReal x2859=((IkReal(1.00000000000000))*(cj11));
12693 IkReal x2860=((cj13)*(r22));
12694 IkReal x2861=((sj13)*(x2859));
12695 if( IKabs(((gconst47)*(((((x2855)*(x2858)))+(((sj11)*(x2854)*(x2856)))+(((IkReal(-1.00000000000000))*(x2853)*(x2854)*(x2857)))+(((IkReal(-1.00000000000000))*(x2853)*(x2861)))+(((IkReal(-1.00000000000000))*(x2859)*(x2860)))+(((cj11)*(sj13)*(x2856))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst47)*(((((x2855)*(x2856)))+(((IkReal(-1.00000000000000))*(x2854)*(x2856)*(x2859)))+(((IkReal(-1.00000000000000))*(x2857)*(x2860)))+(((IkReal(-1.00000000000000))*(x2853)*(x2855)))+(((IkReal(-1.00000000000000))*(x2858)*(x2861)))+(((cj11)*(x2853)*(x2854))))))) < IKFAST_ATAN2_MAGTHRESH )
12696  continue;
12697 j10array[0]=IKatan2(((gconst47)*(((((x2855)*(x2858)))+(((sj11)*(x2854)*(x2856)))+(((IkReal(-1.00000000000000))*(x2853)*(x2854)*(x2857)))+(((IkReal(-1.00000000000000))*(x2853)*(x2861)))+(((IkReal(-1.00000000000000))*(x2859)*(x2860)))+(((cj11)*(sj13)*(x2856)))))), ((gconst47)*(((((x2855)*(x2856)))+(((IkReal(-1.00000000000000))*(x2854)*(x2856)*(x2859)))+(((IkReal(-1.00000000000000))*(x2857)*(x2860)))+(((IkReal(-1.00000000000000))*(x2853)*(x2855)))+(((IkReal(-1.00000000000000))*(x2858)*(x2861)))+(((cj11)*(x2853)*(x2854)))))));
12698 sj10array[0]=IKsin(j10array[0]);
12699 cj10array[0]=IKcos(j10array[0]);
12700 if( j10array[0] > IKPI )
12701 {
12702  j10array[0]-=IK2PI;
12703 }
12704 else if( j10array[0] < -IKPI )
12705 { j10array[0]+=IK2PI;
12706 }
12707 j10valid[0] = true;
12708 for(int ij10 = 0; ij10 < 1; ++ij10)
12709 {
12710 if( !j10valid[ij10] )
12711 {
12712  continue;
12713 }
12714 _ij10[0] = ij10; _ij10[1] = -1;
12715 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12716 {
12717 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12718 {
12719  j10valid[iij10]=false; _ij10[1] = iij10; break;
12720 }
12721 }
12722 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12723 {
12724 IkReal evalcond[6];
12725 IkReal x2862=IKsin(j10);
12726 IkReal x2863=IKcos(j10);
12727 IkReal x2864=((IkReal(1.00000000000000))*(cj13));
12728 IkReal x2865=((cj9)*(r02));
12729 IkReal x2866=((IkReal(1.00000000000000))*(sj13));
12730 IkReal x2867=((r11)*(sj9));
12731 IkReal x2868=((IkReal(1.00000000000000))*(cj14));
12732 IkReal x2869=((cj9)*(r01));
12733 IkReal x2870=((r21)*(sj14));
12734 IkReal x2871=((IkReal(1.00000000000000))*(sj12));
12735 IkReal x2872=((r10)*(sj9));
12736 IkReal x2873=((cj14)*(sj13));
12737 IkReal x2874=((cj14)*(r20));
12738 IkReal x2875=((IkReal(1.00000000000000))*(sj14));
12739 IkReal x2876=((r12)*(sj9));
12740 IkReal x2877=((cj13)*(cj14));
12741 IkReal x2878=((cj9)*(r00));
12742 IkReal x2879=((sj11)*(x2862));
12743 IkReal x2880=((cj11)*(x2862));
12744 IkReal x2881=((sj11)*(x2863));
12745 IkReal x2882=((cj11)*(x2863));
12746 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x2871)*(x2880)))+(((IkReal(-1.00000000000000))*(x2871)*(x2881)))+(((r20)*(sj14))));
12747 evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x2866)))+(((IkReal(-1.00000000000000))*(x2864)*(x2874)))+(((IkReal(-1.00000000000000))*(x2882)))+(((cj13)*(x2870)))+(x2879));
12748 evalcond[2]=((((cj12)*(x2881)))+(((cj12)*(x2880)))+(((sj13)*(x2870)))+(((IkReal(-1.00000000000000))*(x2866)*(x2874)))+(((cj13)*(r22))));
12749 evalcond[3]=((((IkReal(-1.00000000000000))*(x2871)*(x2882)))+(((IkReal(-1.00000000000000))*(x2872)*(x2875)))+(((IkReal(-1.00000000000000))*(x2867)*(x2868)))+(((IkReal(-1.00000000000000))*(x2868)*(x2869)))+(((IkReal(-1.00000000000000))*(x2875)*(x2878)))+(((sj12)*(x2879))));
12750 evalcond[4]=((((x2877)*(x2878)))+(((x2872)*(x2877)))+(((IkReal(-1.00000000000000))*(sj14)*(x2864)*(x2867)))+(((IkReal(-1.00000000000000))*(sj14)*(x2864)*(x2869)))+(((sj13)*(x2876)))+(x2880)+(x2881)+(((sj13)*(x2865))));
12751 evalcond[5]=((((IkReal(-1.00000000000000))*(x2864)*(x2865)))+(((IkReal(-1.00000000000000))*(cj12)*(x2879)))+(((x2872)*(x2873)))+(((IkReal(-1.00000000000000))*(x2864)*(x2876)))+(((x2873)*(x2878)))+(((cj12)*(x2882)))+(((IkReal(-1.00000000000000))*(sj14)*(x2866)*(x2869)))+(((IkReal(-1.00000000000000))*(sj14)*(x2866)*(x2867))));
12752 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 )
12753 {
12754 continue;
12755 }
12756 }
12757 
12758 {
12759 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12760 vinfos[0].jointtype = 1;
12761 vinfos[0].foffset = j9;
12762 vinfos[0].indices[0] = _ij9[0];
12763 vinfos[0].indices[1] = _ij9[1];
12764 vinfos[0].maxsolutions = _nj9;
12765 vinfos[1].jointtype = 1;
12766 vinfos[1].foffset = j10;
12767 vinfos[1].indices[0] = _ij10[0];
12768 vinfos[1].indices[1] = _ij10[1];
12769 vinfos[1].maxsolutions = _nj10;
12770 vinfos[2].jointtype = 1;
12771 vinfos[2].foffset = j11;
12772 vinfos[2].indices[0] = _ij11[0];
12773 vinfos[2].indices[1] = _ij11[1];
12774 vinfos[2].maxsolutions = _nj11;
12775 vinfos[3].jointtype = 1;
12776 vinfos[3].foffset = j12;
12777 vinfos[3].indices[0] = _ij12[0];
12778 vinfos[3].indices[1] = _ij12[1];
12779 vinfos[3].maxsolutions = _nj12;
12780 vinfos[4].jointtype = 1;
12781 vinfos[4].foffset = j13;
12782 vinfos[4].indices[0] = _ij13[0];
12783 vinfos[4].indices[1] = _ij13[1];
12784 vinfos[4].maxsolutions = _nj13;
12785 vinfos[5].jointtype = 1;
12786 vinfos[5].foffset = j14;
12787 vinfos[5].indices[0] = _ij14[0];
12788 vinfos[5].indices[1] = _ij14[1];
12789 vinfos[5].maxsolutions = _nj14;
12790 std::vector<int> vfree(0);
12791 solutions.AddSolution(vinfos,vfree);
12792 }
12793 }
12794 }
12795 
12796 }
12797 
12798 }
12799 
12800 } else
12801 {
12802 {
12803 IkReal j10array[1], cj10array[1], sj10array[1];
12804 bool j10valid[1]={false};
12805 _nj10 = 1;
12806 IkReal x2883=((cj11)*(sj12));
12807 IkReal x2884=((r22)*(sj13));
12808 IkReal x2885=((r20)*(sj14));
12809 IkReal x2886=((cj14)*(sj11));
12810 IkReal x2887=((cj13)*(r20));
12811 IkReal x2888=((sj11)*(sj12));
12812 IkReal x2889=((cj13)*(r21)*(sj14));
12813 if( IKabs(((gconst46)*(((((x2884)*(x2888)))+(((cj11)*(x2885)))+(((cj11)*(cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x2888)*(x2889)))+(((sj12)*(x2886)*(x2887))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst46)*(((((IkReal(-1.00000000000000))*(cj14)*(x2883)*(x2887)))+(((r21)*(x2886)))+(((sj11)*(x2885)))+(((IkReal(-1.00000000000000))*(x2883)*(x2884)))+(((x2883)*(x2889))))))) < IKFAST_ATAN2_MAGTHRESH )
12814  continue;
12815 j10array[0]=IKatan2(((gconst46)*(((((x2884)*(x2888)))+(((cj11)*(x2885)))+(((cj11)*(cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x2888)*(x2889)))+(((sj12)*(x2886)*(x2887)))))), ((gconst46)*(((((IkReal(-1.00000000000000))*(cj14)*(x2883)*(x2887)))+(((r21)*(x2886)))+(((sj11)*(x2885)))+(((IkReal(-1.00000000000000))*(x2883)*(x2884)))+(((x2883)*(x2889)))))));
12816 sj10array[0]=IKsin(j10array[0]);
12817 cj10array[0]=IKcos(j10array[0]);
12818 if( j10array[0] > IKPI )
12819 {
12820  j10array[0]-=IK2PI;
12821 }
12822 else if( j10array[0] < -IKPI )
12823 { j10array[0]+=IK2PI;
12824 }
12825 j10valid[0] = true;
12826 for(int ij10 = 0; ij10 < 1; ++ij10)
12827 {
12828 if( !j10valid[ij10] )
12829 {
12830  continue;
12831 }
12832 _ij10[0] = ij10; _ij10[1] = -1;
12833 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
12834 {
12835 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
12836 {
12837  j10valid[iij10]=false; _ij10[1] = iij10; break;
12838 }
12839 }
12840 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
12841 {
12842 IkReal evalcond[6];
12843 IkReal x2890=IKsin(j10);
12844 IkReal x2891=IKcos(j10);
12845 IkReal x2892=((IkReal(1.00000000000000))*(cj13));
12846 IkReal x2893=((cj9)*(r02));
12847 IkReal x2894=((IkReal(1.00000000000000))*(sj13));
12848 IkReal x2895=((r11)*(sj9));
12849 IkReal x2896=((IkReal(1.00000000000000))*(cj14));
12850 IkReal x2897=((cj9)*(r01));
12851 IkReal x2898=((r21)*(sj14));
12852 IkReal x2899=((IkReal(1.00000000000000))*(sj12));
12853 IkReal x2900=((r10)*(sj9));
12854 IkReal x2901=((cj14)*(sj13));
12855 IkReal x2902=((cj14)*(r20));
12856 IkReal x2903=((IkReal(1.00000000000000))*(sj14));
12857 IkReal x2904=((r12)*(sj9));
12858 IkReal x2905=((cj13)*(cj14));
12859 IkReal x2906=((cj9)*(r00));
12860 IkReal x2907=((sj11)*(x2890));
12861 IkReal x2908=((cj11)*(x2890));
12862 IkReal x2909=((sj11)*(x2891));
12863 IkReal x2910=((cj11)*(x2891));
12864 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x2899)*(x2908)))+(((IkReal(-1.00000000000000))*(x2899)*(x2909))));
12865 evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x2894)))+(((IkReal(-1.00000000000000))*(x2892)*(x2902)))+(((IkReal(-1.00000000000000))*(x2910)))+(((cj13)*(x2898)))+(x2907));
12866 evalcond[2]=((((sj13)*(x2898)))+(((cj12)*(x2908)))+(((cj12)*(x2909)))+(((IkReal(-1.00000000000000))*(x2894)*(x2902)))+(((cj13)*(r22))));
12867 evalcond[3]=((((IkReal(-1.00000000000000))*(x2899)*(x2910)))+(((IkReal(-1.00000000000000))*(x2900)*(x2903)))+(((IkReal(-1.00000000000000))*(x2895)*(x2896)))+(((IkReal(-1.00000000000000))*(x2896)*(x2897)))+(((IkReal(-1.00000000000000))*(x2903)*(x2906)))+(((sj12)*(x2907))));
12868 evalcond[4]=((((sj13)*(x2893)))+(((x2905)*(x2906)))+(((x2900)*(x2905)))+(((IkReal(-1.00000000000000))*(sj14)*(x2892)*(x2895)))+(((IkReal(-1.00000000000000))*(sj14)*(x2892)*(x2897)))+(((sj13)*(x2904)))+(x2909)+(x2908));
12869 evalcond[5]=((((IkReal(-1.00000000000000))*(cj12)*(x2907)))+(((x2901)*(x2906)))+(((x2900)*(x2901)))+(((IkReal(-1.00000000000000))*(x2892)*(x2893)))+(((IkReal(-1.00000000000000))*(sj14)*(x2894)*(x2895)))+(((IkReal(-1.00000000000000))*(sj14)*(x2894)*(x2897)))+(((IkReal(-1.00000000000000))*(x2892)*(x2904)))+(((cj12)*(x2910))));
12870 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 )
12871 {
12872 continue;
12873 }
12874 }
12875 
12876 {
12877 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
12878 vinfos[0].jointtype = 1;
12879 vinfos[0].foffset = j9;
12880 vinfos[0].indices[0] = _ij9[0];
12881 vinfos[0].indices[1] = _ij9[1];
12882 vinfos[0].maxsolutions = _nj9;
12883 vinfos[1].jointtype = 1;
12884 vinfos[1].foffset = j10;
12885 vinfos[1].indices[0] = _ij10[0];
12886 vinfos[1].indices[1] = _ij10[1];
12887 vinfos[1].maxsolutions = _nj10;
12888 vinfos[2].jointtype = 1;
12889 vinfos[2].foffset = j11;
12890 vinfos[2].indices[0] = _ij11[0];
12891 vinfos[2].indices[1] = _ij11[1];
12892 vinfos[2].maxsolutions = _nj11;
12893 vinfos[3].jointtype = 1;
12894 vinfos[3].foffset = j12;
12895 vinfos[3].indices[0] = _ij12[0];
12896 vinfos[3].indices[1] = _ij12[1];
12897 vinfos[3].maxsolutions = _nj12;
12898 vinfos[4].jointtype = 1;
12899 vinfos[4].foffset = j13;
12900 vinfos[4].indices[0] = _ij13[0];
12901 vinfos[4].indices[1] = _ij13[1];
12902 vinfos[4].maxsolutions = _nj13;
12903 vinfos[5].jointtype = 1;
12904 vinfos[5].foffset = j14;
12905 vinfos[5].indices[0] = _ij14[0];
12906 vinfos[5].indices[1] = _ij14[1];
12907 vinfos[5].maxsolutions = _nj14;
12908 std::vector<int> vfree(0);
12909 solutions.AddSolution(vinfos,vfree);
12910 }
12911 }
12912 }
12913 
12914 }
12915 
12916 }
12917 }
12918 }
12919 
12920 }
12921 
12922 }
12923 }
12924 }
12925 
12926 }
12927 
12928 }
12929 
12930 } else
12931 {
12932 {
12933 IkReal j9array[1], cj9array[1], sj9array[1];
12934 bool j9valid[1]={false};
12935 _nj9 = 1;
12936 IkReal x2911=((cj12)*(sj13));
12937 IkReal x2912=((cj12)*(cj13));
12938 IkReal x2913=((IkReal(1.00000000000000))*(sj14));
12939 if( IKabs(((gconst0)*(((((r12)*(x2911)))+(((cj14)*(r10)*(x2912)))+(((IkReal(-1.00000000000000))*(r11)*(x2912)*(x2913))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((cj14)*(r00)*(x2912)))+(((IkReal(-1.00000000000000))*(r01)*(x2912)*(x2913)))+(((r02)*(x2911))))))) < IKFAST_ATAN2_MAGTHRESH )
12940  continue;
12941 j9array[0]=IKatan2(((gconst0)*(((((r12)*(x2911)))+(((cj14)*(r10)*(x2912)))+(((IkReal(-1.00000000000000))*(r11)*(x2912)*(x2913)))))), ((gconst0)*(((((cj14)*(r00)*(x2912)))+(((IkReal(-1.00000000000000))*(r01)*(x2912)*(x2913)))+(((r02)*(x2911)))))));
12942 sj9array[0]=IKsin(j9array[0]);
12943 cj9array[0]=IKcos(j9array[0]);
12944 if( j9array[0] > IKPI )
12945 {
12946  j9array[0]-=IK2PI;
12947 }
12948 else if( j9array[0] < -IKPI )
12949 { j9array[0]+=IK2PI;
12950 }
12951 j9valid[0] = true;
12952 for(int ij9 = 0; ij9 < 1; ++ij9)
12953 {
12954 if( !j9valid[ij9] )
12955 {
12956  continue;
12957 }
12958 _ij9[0] = ij9; _ij9[1] = -1;
12959 for(int iij9 = ij9+1; iij9 < 1; ++iij9)
12960 {
12961 if( j9valid[iij9] && IKabs(cj9array[ij9]-cj9array[iij9]) < IKFAST_SOLUTION_THRESH && IKabs(sj9array[ij9]-sj9array[iij9]) < IKFAST_SOLUTION_THRESH )
12962 {
12963  j9valid[iij9]=false; _ij9[1] = iij9; break;
12964 }
12965 }
12966 j9 = j9array[ij9]; cj9 = cj9array[ij9]; sj9 = sj9array[ij9];
12967 {
12968 IkReal evalcond[3];
12969 IkReal x2914=IKsin(j9);
12970 IkReal x2915=IKcos(j9);
12971 IkReal x2916=((IkReal(1.00000000000000))*(sj14));
12972 IkReal x2917=((sj13)*(x2915));
12973 IkReal x2918=((cj13)*(x2914));
12974 IkReal x2919=((IkReal(1.00000000000000))*(cj14)*(r00));
12975 IkReal x2920=((r01)*(x2914));
12976 IkReal x2921=((r10)*(x2915));
12977 IkReal x2922=((sj13)*(x2914));
12978 IkReal x2923=((cj13)*(x2915));
12979 evalcond[0]=((((IkReal(-1.00000000000000))*(x2916)*(x2921)))+(cj12)+(((cj14)*(x2920)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x2915)))+(((r00)*(sj14)*(x2914))));
12980 evalcond[1]=((((r12)*(x2917)))+(((cj13)*(cj14)*(x2921)))+(((IkReal(-1.00000000000000))*(r11)*(x2916)*(x2923)))+(((r01)*(sj14)*(x2918)))+(((IkReal(-1.00000000000000))*(x2918)*(x2919)))+(((IkReal(-1.00000000000000))*(r02)*(x2922))));
12981 evalcond[2]=((((IkReal(-1.00000000000000))*(r11)*(x2916)*(x2917)))+(((cj14)*(r10)*(x2917)))+(((IkReal(-1.00000000000000))*(x2919)*(x2922)))+(((IkReal(-1.00000000000000))*(r12)*(x2923)))+(sj12)+(((r02)*(x2918)))+(((sj13)*(sj14)*(x2920))));
12982 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
12983 {
12984 continue;
12985 }
12986 }
12987 
12988 {
12989 IkReal dummyeval[1];
12990 dummyeval[0]=sj12;
12991 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
12992 {
12993 {
12994 IkReal dummyeval[1];
12995 dummyeval[0]=cj12;
12996 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
12997 {
12998 {
12999 IkReal evalcond[7];
13000 IkReal x2924=((IkReal(1.00000000000000))*(sj13));
13001 IkReal x2925=((cj14)*(sj9));
13002 IkReal x2926=((cj9)*(sj14));
13003 IkReal x2927=((cj13)*(sj9));
13004 IkReal x2928=((sj13)*(sj14));
13005 IkReal x2929=((cj9)*(sj13));
13006 IkReal x2930=((IkReal(1.00000000000000))*(cj13));
13007 IkReal x2931=((cj14)*(r10));
13008 IkReal x2932=((sj14)*(sj9));
13009 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
13010 evalcond[1]=((((r00)*(x2932)))+(((IkReal(-1.00000000000000))*(cj14)*(cj9)*(r11)))+(((r01)*(x2925)))+(((IkReal(-1.00000000000000))*(r10)*(x2926))));
13011 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2924)))+(((r21)*(x2928)))+(((cj13)*(r22))));
13012 evalcond[3]=((IkReal(0.0950000000000000))+(((IkReal(-1.00000000000000))*(cj14)*(npx)*(x2924)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((npy)*(x2928))));
13013 evalcond[4]=((((cj13)*(cj9)*(x2931)))+(((IkReal(-1.00000000000000))*(r11)*(x2926)*(x2930)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2924)))+(((r01)*(sj14)*(x2927)))+(((IkReal(-1.00000000000000))*(r00)*(x2925)*(x2930)))+(((r12)*(x2929))));
13014 evalcond[5]=((IkReal(1.00000000000000))+(((x2929)*(x2931)))+(((IkReal(-1.00000000000000))*(cj9)*(r12)*(x2930)))+(((IkReal(-1.00000000000000))*(r11)*(x2924)*(x2926)))+(((r01)*(sj9)*(x2928)))+(((IkReal(-1.00000000000000))*(r00)*(x2924)*(x2925)))+(((r02)*(x2927))));
13015 evalcond[6]=((((IkReal(-1.00000000000000))*(r01)*(x2924)*(x2926)))+(((IkReal(-1.00000000000000))*(r11)*(x2924)*(x2932)))+(((r10)*(sj13)*(x2925)))+(((IkReal(-1.00000000000000))*(cj9)*(r02)*(x2930)))+(((IkReal(-1.00000000000000))*(r12)*(x2927)))+(((cj14)*(r00)*(x2929))));
13016 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 )
13017 {
13018 {
13019 IkReal j11array[1], cj11array[1], sj11array[1];
13020 bool j11valid[1]={false};
13021 _nj11 = 1;
13022 IkReal x2933=((IkReal(4.00000000000000))*(sj14));
13023 IkReal x2934=((IkReal(4.00000000000000))*(cj14));
13024 if( IKabs(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npy)*(x2934)))+(((IkReal(-1.00000000000000))*(npx)*(x2933))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x2934)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2933)))+(((IkReal(-0.360000000000000))*(cj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npy)*(x2934)))+(((IkReal(-1.00000000000000))*(npx)*(x2933)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x2934)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2933)))+(((IkReal(-0.360000000000000))*(cj13)))))-1) <= IKFAST_SINCOS_THRESH )
13025  continue;
13026 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npy)*(x2934)))+(((IkReal(-1.00000000000000))*(npx)*(x2933)))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x2934)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2933)))+(((IkReal(-0.360000000000000))*(cj13)))));
13027 sj11array[0]=IKsin(j11array[0]);
13028 cj11array[0]=IKcos(j11array[0]);
13029 if( j11array[0] > IKPI )
13030 {
13031  j11array[0]-=IK2PI;
13032 }
13033 else if( j11array[0] < -IKPI )
13034 { j11array[0]+=IK2PI;
13035 }
13036 j11valid[0] = true;
13037 for(int ij11 = 0; ij11 < 1; ++ij11)
13038 {
13039 if( !j11valid[ij11] )
13040 {
13041  continue;
13042 }
13043 _ij11[0] = ij11; _ij11[1] = -1;
13044 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
13045 {
13046 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
13047 {
13048  j11valid[iij11]=false; _ij11[1] = iij11; break;
13049 }
13050 }
13051 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
13052 {
13053 IkReal evalcond[2];
13054 evalcond[0]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(IKsin(j11)))));
13055 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(npx)))+(((cj13)*(npy)*(sj14)))+(((IkReal(-1.00000000000000))*(npz)*(sj13))));
13056 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
13057 {
13058 continue;
13059 }
13060 }
13061 
13062 {
13063 IkReal dummyeval[1];
13064 IkReal gconst56;
13065 gconst56=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
13066 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
13067 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
13068 {
13069 {
13070 IkReal dummyeval[1];
13071 IkReal gconst57;
13072 gconst57=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
13073 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
13074 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
13075 {
13076 continue;
13077 
13078 } else
13079 {
13080 {
13081 IkReal j10array[1], cj10array[1], sj10array[1];
13082 bool j10valid[1]={false};
13083 _nj10 = 1;
13084 IkReal x2935=((IkReal(1.00000000000000))*(cj11));
13085 IkReal x2936=((r20)*(sj14));
13086 IkReal x2937=((cj14)*(r21));
13087 IkReal x2938=((cj14)*(cj9)*(r01));
13088 IkReal x2939=((r10)*(sj14)*(sj9));
13089 IkReal x2940=((cj9)*(r00)*(sj14));
13090 IkReal x2941=((cj14)*(r11)*(sj9));
13091 if( IKabs(((gconst57)*(((((sj11)*(x2940)))+(((sj11)*(x2941)))+(((sj11)*(x2939)))+(((sj11)*(x2938)))+(((cj11)*(x2936)))+(((cj11)*(x2937))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst57)*(((((IkReal(-1.00000000000000))*(x2935)*(x2941)))+(((IkReal(-1.00000000000000))*(x2935)*(x2940)))+(((sj11)*(x2936)))+(((sj11)*(x2937)))+(((IkReal(-1.00000000000000))*(x2935)*(x2938)))+(((IkReal(-1.00000000000000))*(x2935)*(x2939))))))) < IKFAST_ATAN2_MAGTHRESH )
13092  continue;
13093 j10array[0]=IKatan2(((gconst57)*(((((sj11)*(x2940)))+(((sj11)*(x2941)))+(((sj11)*(x2939)))+(((sj11)*(x2938)))+(((cj11)*(x2936)))+(((cj11)*(x2937)))))), ((gconst57)*(((((IkReal(-1.00000000000000))*(x2935)*(x2941)))+(((IkReal(-1.00000000000000))*(x2935)*(x2940)))+(((sj11)*(x2936)))+(((sj11)*(x2937)))+(((IkReal(-1.00000000000000))*(x2935)*(x2938)))+(((IkReal(-1.00000000000000))*(x2935)*(x2939)))))));
13094 sj10array[0]=IKsin(j10array[0]);
13095 cj10array[0]=IKcos(j10array[0]);
13096 if( j10array[0] > IKPI )
13097 {
13098  j10array[0]-=IK2PI;
13099 }
13100 else if( j10array[0] < -IKPI )
13101 { j10array[0]+=IK2PI;
13102 }
13103 j10valid[0] = true;
13104 for(int ij10 = 0; ij10 < 1; ++ij10)
13105 {
13106 if( !j10valid[ij10] )
13107 {
13108  continue;
13109 }
13110 _ij10[0] = ij10; _ij10[1] = -1;
13111 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
13112 {
13113 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
13114 {
13115  j10valid[iij10]=false; _ij10[1] = iij10; break;
13116 }
13117 }
13118 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
13119 {
13120 IkReal evalcond[4];
13121 IkReal x2942=IKsin(j10);
13122 IkReal x2943=IKcos(j10);
13123 IkReal x2944=((cj13)*(sj14));
13124 IkReal x2945=((cj13)*(cj14));
13125 IkReal x2946=((r10)*(sj9));
13126 IkReal x2947=((IkReal(1.00000000000000))*(cj9));
13127 IkReal x2948=((sj11)*(x2942));
13128 IkReal x2949=((IkReal(1.00000000000000))*(x2943));
13129 IkReal x2950=((cj11)*(x2942));
13130 IkReal x2951=((IkReal(1.00000000000000))*(r11)*(sj9));
13131 IkReal x2952=((cj11)*(x2949));
13132 evalcond[0]=((((IkReal(-1.00000000000000))*(x2950)))+(((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x2949))));
13133 evalcond[1]=((((r21)*(x2944)))+(((IkReal(-1.00000000000000))*(x2952)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x2945)))+(x2948));
13134 evalcond[2]=((((IkReal(-1.00000000000000))*(x2952)))+(((IkReal(-1.00000000000000))*(sj14)*(x2946)))+(((IkReal(-1.00000000000000))*(cj14)*(x2951)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2947)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2947)))+(x2948));
13135 evalcond[3]=((((sj11)*(x2943)))+(((x2945)*(x2946)))+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(x2944)*(x2951)))+(((IkReal(-1.00000000000000))*(r01)*(x2944)*(x2947)))+(((cj9)*(r00)*(x2945)))+(x2950)+(((r12)*(sj13)*(sj9))));
13136 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
13137 {
13138 continue;
13139 }
13140 }
13141 
13142 {
13143 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13144 vinfos[0].jointtype = 1;
13145 vinfos[0].foffset = j9;
13146 vinfos[0].indices[0] = _ij9[0];
13147 vinfos[0].indices[1] = _ij9[1];
13148 vinfos[0].maxsolutions = _nj9;
13149 vinfos[1].jointtype = 1;
13150 vinfos[1].foffset = j10;
13151 vinfos[1].indices[0] = _ij10[0];
13152 vinfos[1].indices[1] = _ij10[1];
13153 vinfos[1].maxsolutions = _nj10;
13154 vinfos[2].jointtype = 1;
13155 vinfos[2].foffset = j11;
13156 vinfos[2].indices[0] = _ij11[0];
13157 vinfos[2].indices[1] = _ij11[1];
13158 vinfos[2].maxsolutions = _nj11;
13159 vinfos[3].jointtype = 1;
13160 vinfos[3].foffset = j12;
13161 vinfos[3].indices[0] = _ij12[0];
13162 vinfos[3].indices[1] = _ij12[1];
13163 vinfos[3].maxsolutions = _nj12;
13164 vinfos[4].jointtype = 1;
13165 vinfos[4].foffset = j13;
13166 vinfos[4].indices[0] = _ij13[0];
13167 vinfos[4].indices[1] = _ij13[1];
13168 vinfos[4].maxsolutions = _nj13;
13169 vinfos[5].jointtype = 1;
13170 vinfos[5].foffset = j14;
13171 vinfos[5].indices[0] = _ij14[0];
13172 vinfos[5].indices[1] = _ij14[1];
13173 vinfos[5].maxsolutions = _nj14;
13174 std::vector<int> vfree(0);
13175 solutions.AddSolution(vinfos,vfree);
13176 }
13177 }
13178 }
13179 
13180 }
13181 
13182 }
13183 
13184 } else
13185 {
13186 {
13187 IkReal j10array[1], cj10array[1], sj10array[1];
13188 bool j10valid[1]={false};
13189 _nj10 = 1;
13190 IkReal x2953=((r20)*(sj14));
13191 IkReal x2954=((IkReal(1.00000000000000))*(cj13));
13192 IkReal x2955=((cj14)*(r20));
13193 IkReal x2956=((r21)*(sj14));
13194 IkReal x2957=((r22)*(sj13));
13195 IkReal x2958=((cj14)*(r21));
13196 if( IKabs(((gconst56)*(((((sj11)*(x2957)))+(((cj13)*(sj11)*(x2955)))+(((cj11)*(x2958)))+(((cj11)*(x2953)))+(((IkReal(-1.00000000000000))*(sj11)*(x2954)*(x2956))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst56)*(((((IkReal(-1.00000000000000))*(cj11)*(x2957)))+(((sj11)*(x2958)))+(((sj11)*(x2953)))+(((cj11)*(cj13)*(x2956)))+(((IkReal(-1.00000000000000))*(cj11)*(x2954)*(x2955))))))) < IKFAST_ATAN2_MAGTHRESH )
13197  continue;
13198 j10array[0]=IKatan2(((gconst56)*(((((sj11)*(x2957)))+(((cj13)*(sj11)*(x2955)))+(((cj11)*(x2958)))+(((cj11)*(x2953)))+(((IkReal(-1.00000000000000))*(sj11)*(x2954)*(x2956)))))), ((gconst56)*(((((IkReal(-1.00000000000000))*(cj11)*(x2957)))+(((sj11)*(x2958)))+(((sj11)*(x2953)))+(((cj11)*(cj13)*(x2956)))+(((IkReal(-1.00000000000000))*(cj11)*(x2954)*(x2955)))))));
13199 sj10array[0]=IKsin(j10array[0]);
13200 cj10array[0]=IKcos(j10array[0]);
13201 if( j10array[0] > IKPI )
13202 {
13203  j10array[0]-=IK2PI;
13204 }
13205 else if( j10array[0] < -IKPI )
13206 { j10array[0]+=IK2PI;
13207 }
13208 j10valid[0] = true;
13209 for(int ij10 = 0; ij10 < 1; ++ij10)
13210 {
13211 if( !j10valid[ij10] )
13212 {
13213  continue;
13214 }
13215 _ij10[0] = ij10; _ij10[1] = -1;
13216 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
13217 {
13218 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
13219 {
13220  j10valid[iij10]=false; _ij10[1] = iij10; break;
13221 }
13222 }
13223 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
13224 {
13225 IkReal evalcond[4];
13226 IkReal x2959=IKsin(j10);
13227 IkReal x2960=IKcos(j10);
13228 IkReal x2961=((cj13)*(sj14));
13229 IkReal x2962=((cj13)*(cj14));
13230 IkReal x2963=((r10)*(sj9));
13231 IkReal x2964=((IkReal(1.00000000000000))*(cj9));
13232 IkReal x2965=((sj11)*(x2959));
13233 IkReal x2966=((IkReal(1.00000000000000))*(x2960));
13234 IkReal x2967=((cj11)*(x2959));
13235 IkReal x2968=((IkReal(1.00000000000000))*(r11)*(sj9));
13236 IkReal x2969=((cj11)*(x2966));
13237 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x2967)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x2966))));
13238 evalcond[1]=((((r21)*(x2961)))+(((IkReal(-1.00000000000000))*(r20)*(x2962)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x2969)))+(x2965));
13239 evalcond[2]=((((IkReal(-1.00000000000000))*(sj14)*(x2963)))+(((IkReal(-1.00000000000000))*(x2969)))+(((IkReal(-1.00000000000000))*(cj14)*(x2968)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2964)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2964)))+(x2965));
13240 evalcond[3]=((((sj11)*(x2960)))+(((cj9)*(r02)*(sj13)))+(((x2962)*(x2963)))+(((IkReal(-1.00000000000000))*(x2961)*(x2968)))+(((IkReal(-1.00000000000000))*(r01)*(x2961)*(x2964)))+(x2967)+(((r12)*(sj13)*(sj9)))+(((cj9)*(r00)*(x2962))));
13241 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
13242 {
13243 continue;
13244 }
13245 }
13246 
13247 {
13248 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13249 vinfos[0].jointtype = 1;
13250 vinfos[0].foffset = j9;
13251 vinfos[0].indices[0] = _ij9[0];
13252 vinfos[0].indices[1] = _ij9[1];
13253 vinfos[0].maxsolutions = _nj9;
13254 vinfos[1].jointtype = 1;
13255 vinfos[1].foffset = j10;
13256 vinfos[1].indices[0] = _ij10[0];
13257 vinfos[1].indices[1] = _ij10[1];
13258 vinfos[1].maxsolutions = _nj10;
13259 vinfos[2].jointtype = 1;
13260 vinfos[2].foffset = j11;
13261 vinfos[2].indices[0] = _ij11[0];
13262 vinfos[2].indices[1] = _ij11[1];
13263 vinfos[2].maxsolutions = _nj11;
13264 vinfos[3].jointtype = 1;
13265 vinfos[3].foffset = j12;
13266 vinfos[3].indices[0] = _ij12[0];
13267 vinfos[3].indices[1] = _ij12[1];
13268 vinfos[3].maxsolutions = _nj12;
13269 vinfos[4].jointtype = 1;
13270 vinfos[4].foffset = j13;
13271 vinfos[4].indices[0] = _ij13[0];
13272 vinfos[4].indices[1] = _ij13[1];
13273 vinfos[4].maxsolutions = _nj13;
13274 vinfos[5].jointtype = 1;
13275 vinfos[5].foffset = j14;
13276 vinfos[5].indices[0] = _ij14[0];
13277 vinfos[5].indices[1] = _ij14[1];
13278 vinfos[5].maxsolutions = _nj14;
13279 std::vector<int> vfree(0);
13280 solutions.AddSolution(vinfos,vfree);
13281 }
13282 }
13283 }
13284 
13285 }
13286 
13287 }
13288 }
13289 }
13290 
13291 } else
13292 {
13293 IkReal x2970=((IkReal(1.00000000000000))*(sj13));
13294 IkReal x2971=((cj14)*(sj9));
13295 IkReal x2972=((cj9)*(sj14));
13296 IkReal x2973=((cj13)*(sj9));
13297 IkReal x2974=((sj13)*(sj14));
13298 IkReal x2975=((cj9)*(sj13));
13299 IkReal x2976=((IkReal(1.00000000000000))*(cj13));
13300 IkReal x2977=((cj14)*(r10));
13301 IkReal x2978=((sj14)*(sj9));
13302 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
13303 evalcond[1]=((((r01)*(x2971)))+(((IkReal(-1.00000000000000))*(cj14)*(cj9)*(r11)))+(((r00)*(x2978)))+(((IkReal(-1.00000000000000))*(r10)*(x2972))));
13304 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x2970)))+(((cj13)*(r22)))+(((r21)*(x2974))));
13305 evalcond[3]=((IkReal(-0.0950000000000000))+(((npy)*(x2974)))+(((cj13)*(npz)))+(((IkReal(-1.00000000000000))*(cj14)*(npx)*(x2970)))+(((IkReal(0.0900000000000000))*(sj13))));
13306 evalcond[4]=((((IkReal(-1.00000000000000))*(r11)*(x2972)*(x2976)))+(((cj13)*(cj9)*(x2977)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x2970)))+(((r12)*(x2975)))+(((IkReal(-1.00000000000000))*(r00)*(x2971)*(x2976)))+(((r01)*(sj14)*(x2973))));
13307 evalcond[5]=((IkReal(-1.00000000000000))+(((r02)*(x2973)))+(((IkReal(-1.00000000000000))*(cj9)*(r12)*(x2976)))+(((x2975)*(x2977)))+(((r01)*(sj9)*(x2974)))+(((IkReal(-1.00000000000000))*(r00)*(x2970)*(x2971)))+(((IkReal(-1.00000000000000))*(r11)*(x2970)*(x2972))));
13308 evalcond[6]=((((IkReal(-1.00000000000000))*(cj9)*(r02)*(x2976)))+(((r10)*(sj13)*(x2971)))+(((cj14)*(r00)*(x2975)))+(((IkReal(-1.00000000000000))*(r01)*(x2970)*(x2972)))+(((IkReal(-1.00000000000000))*(r12)*(x2973)))+(((IkReal(-1.00000000000000))*(r11)*(x2970)*(x2978))));
13309 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 )
13310 {
13311 {
13312 IkReal j11array[1], cj11array[1], sj11array[1];
13313 bool j11valid[1]={false};
13314 _nj11 = 1;
13315 IkReal x2979=((IkReal(4.00000000000000))*(sj14));
13316 IkReal x2980=((IkReal(4.00000000000000))*(cj14));
13317 if( IKabs(((IkReal(0.120000000000000))+(((npx)*(x2979)))+(((npy)*(x2980))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2979)))+(((IkReal(-0.360000000000000))*(cj13)))+(((cj13)*(npx)*(x2980))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((npx)*(x2979)))+(((npy)*(x2980)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2979)))+(((IkReal(-0.360000000000000))*(cj13)))+(((cj13)*(npx)*(x2980)))))-1) <= IKFAST_SINCOS_THRESH )
13318  continue;
13319 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((npx)*(x2979)))+(((npy)*(x2980)))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x2979)))+(((IkReal(-0.360000000000000))*(cj13)))+(((cj13)*(npx)*(x2980)))));
13320 sj11array[0]=IKsin(j11array[0]);
13321 cj11array[0]=IKcos(j11array[0]);
13322 if( j11array[0] > IKPI )
13323 {
13324  j11array[0]-=IK2PI;
13325 }
13326 else if( j11array[0] < -IKPI )
13327 { j11array[0]+=IK2PI;
13328 }
13329 j11valid[0] = true;
13330 for(int ij11 = 0; ij11 < 1; ++ij11)
13331 {
13332 if( !j11valid[ij11] )
13333 {
13334  continue;
13335 }
13336 _ij11[0] = ij11; _ij11[1] = -1;
13337 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
13338 {
13339 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
13340 {
13341  j11valid[iij11]=false; _ij11[1] = iij11; break;
13342 }
13343 }
13344 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
13345 {
13346 IkReal evalcond[2];
13347 evalcond[0]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(IKsin(j11))))+(((cj14)*(npy)))+(((npx)*(sj14))));
13348 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(npx)))+(((cj13)*(npy)*(sj14)))+(((IkReal(-1.00000000000000))*(npz)*(sj13))));
13349 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
13350 {
13351 continue;
13352 }
13353 }
13354 
13355 {
13356 IkReal dummyeval[1];
13357 IkReal gconst58;
13358 gconst58=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
13359 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
13360 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
13361 {
13362 {
13363 IkReal dummyeval[1];
13364 IkReal gconst59;
13365 gconst59=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
13366 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
13367 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
13368 {
13369 continue;
13370 
13371 } else
13372 {
13373 {
13374 IkReal j10array[1], cj10array[1], sj10array[1];
13375 bool j10valid[1]={false};
13376 _nj10 = 1;
13377 IkReal x2981=((IkReal(1.00000000000000))*(sj11));
13378 IkReal x2982=((cj14)*(r21));
13379 IkReal x2983=((IkReal(1.00000000000000))*(cj11));
13380 IkReal x2984=((r20)*(sj14));
13381 IkReal x2985=((cj9)*(r00)*(sj14));
13382 IkReal x2986=((cj14)*(r11)*(sj9));
13383 IkReal x2987=((cj14)*(cj9)*(r01));
13384 IkReal x2988=((r10)*(sj14)*(sj9));
13385 if( IKabs(((gconst59)*(((((IkReal(-1.00000000000000))*(x2982)*(x2983)))+(((IkReal(-1.00000000000000))*(x2981)*(x2987)))+(((IkReal(-1.00000000000000))*(x2981)*(x2988)))+(((IkReal(-1.00000000000000))*(x2981)*(x2985)))+(((IkReal(-1.00000000000000))*(x2981)*(x2986)))+(((IkReal(-1.00000000000000))*(x2983)*(x2984))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst59)*(((((cj11)*(x2988)))+(((cj11)*(x2987)))+(((cj11)*(x2986)))+(((cj11)*(x2985)))+(((IkReal(-1.00000000000000))*(x2981)*(x2982)))+(((IkReal(-1.00000000000000))*(x2981)*(x2984))))))) < IKFAST_ATAN2_MAGTHRESH )
13386  continue;
13387 j10array[0]=IKatan2(((gconst59)*(((((IkReal(-1.00000000000000))*(x2982)*(x2983)))+(((IkReal(-1.00000000000000))*(x2981)*(x2987)))+(((IkReal(-1.00000000000000))*(x2981)*(x2988)))+(((IkReal(-1.00000000000000))*(x2981)*(x2985)))+(((IkReal(-1.00000000000000))*(x2981)*(x2986)))+(((IkReal(-1.00000000000000))*(x2983)*(x2984)))))), ((gconst59)*(((((cj11)*(x2988)))+(((cj11)*(x2987)))+(((cj11)*(x2986)))+(((cj11)*(x2985)))+(((IkReal(-1.00000000000000))*(x2981)*(x2982)))+(((IkReal(-1.00000000000000))*(x2981)*(x2984)))))));
13388 sj10array[0]=IKsin(j10array[0]);
13389 cj10array[0]=IKcos(j10array[0]);
13390 if( j10array[0] > IKPI )
13391 {
13392  j10array[0]-=IK2PI;
13393 }
13394 else if( j10array[0] < -IKPI )
13395 { j10array[0]+=IK2PI;
13396 }
13397 j10valid[0] = true;
13398 for(int ij10 = 0; ij10 < 1; ++ij10)
13399 {
13400 if( !j10valid[ij10] )
13401 {
13402  continue;
13403 }
13404 _ij10[0] = ij10; _ij10[1] = -1;
13405 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
13406 {
13407 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
13408 {
13409  j10valid[iij10]=false; _ij10[1] = iij10; break;
13410 }
13411 }
13412 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
13413 {
13414 IkReal evalcond[4];
13415 IkReal x2989=IKsin(j10);
13416 IkReal x2990=IKcos(j10);
13417 IkReal x2991=((cj14)*(sj9));
13418 IkReal x2992=((IkReal(1.00000000000000))*(r11));
13419 IkReal x2993=((cj13)*(sj14));
13420 IkReal x2994=((IkReal(1.00000000000000))*(cj9));
13421 IkReal x2995=((cj13)*(cj14));
13422 IkReal x2996=((cj11)*(x2989));
13423 IkReal x2997=((sj11)*(x2990));
13424 IkReal x2998=((cj11)*(x2990));
13425 IkReal x2999=((sj11)*(x2989));
13426 IkReal x3000=((x2996)+(x2997));
13427 evalcond[0]=((((cj14)*(r21)))+(x3000)+(((r20)*(sj14))));
13428 evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x2998)))+(((r21)*(x2993)))+(((IkReal(-1.00000000000000))*(r20)*(x2995)))+(x2999));
13429 evalcond[2]=((((IkReal(-1.00000000000000))*(r00)*(sj14)*(x2994)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x2994)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x2999)))+(((IkReal(-1.00000000000000))*(x2991)*(x2992)))+(x2998));
13430 evalcond[3]=((x3000)+(((IkReal(-1.00000000000000))*(r01)*(x2993)*(x2994)))+(((cj9)*(r02)*(sj13)))+(((cj9)*(r00)*(x2995)))+(((r12)*(sj13)*(sj9)))+(((IkReal(-1.00000000000000))*(sj9)*(x2992)*(x2993)))+(((cj13)*(r10)*(x2991))));
13431 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
13432 {
13433 continue;
13434 }
13435 }
13436 
13437 {
13438 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13439 vinfos[0].jointtype = 1;
13440 vinfos[0].foffset = j9;
13441 vinfos[0].indices[0] = _ij9[0];
13442 vinfos[0].indices[1] = _ij9[1];
13443 vinfos[0].maxsolutions = _nj9;
13444 vinfos[1].jointtype = 1;
13445 vinfos[1].foffset = j10;
13446 vinfos[1].indices[0] = _ij10[0];
13447 vinfos[1].indices[1] = _ij10[1];
13448 vinfos[1].maxsolutions = _nj10;
13449 vinfos[2].jointtype = 1;
13450 vinfos[2].foffset = j11;
13451 vinfos[2].indices[0] = _ij11[0];
13452 vinfos[2].indices[1] = _ij11[1];
13453 vinfos[2].maxsolutions = _nj11;
13454 vinfos[3].jointtype = 1;
13455 vinfos[3].foffset = j12;
13456 vinfos[3].indices[0] = _ij12[0];
13457 vinfos[3].indices[1] = _ij12[1];
13458 vinfos[3].maxsolutions = _nj12;
13459 vinfos[4].jointtype = 1;
13460 vinfos[4].foffset = j13;
13461 vinfos[4].indices[0] = _ij13[0];
13462 vinfos[4].indices[1] = _ij13[1];
13463 vinfos[4].maxsolutions = _nj13;
13464 vinfos[5].jointtype = 1;
13465 vinfos[5].foffset = j14;
13466 vinfos[5].indices[0] = _ij14[0];
13467 vinfos[5].indices[1] = _ij14[1];
13468 vinfos[5].maxsolutions = _nj14;
13469 std::vector<int> vfree(0);
13470 solutions.AddSolution(vinfos,vfree);
13471 }
13472 }
13473 }
13474 
13475 }
13476 
13477 }
13478 
13479 } else
13480 {
13481 {
13482 IkReal j10array[1], cj10array[1], sj10array[1];
13483 bool j10valid[1]={false};
13484 _nj10 = 1;
13485 IkReal x3001=((cj13)*(sj11));
13486 IkReal x3002=((r21)*(sj14));
13487 IkReal x3003=((cj14)*(r20));
13488 IkReal x3004=((cj11)*(cj13));
13489 IkReal x3005=((r22)*(sj13));
13490 IkReal x3006=((r20)*(sj14));
13491 IkReal x3007=((cj14)*(r21));
13492 if( IKabs(((gconst58)*(((((IkReal(-1.00000000000000))*(sj11)*(x3005)))+(((cj11)*(x3007)))+(((cj11)*(x3006)))+(((IkReal(-1.00000000000000))*(x3001)*(x3003)))+(((x3001)*(x3002))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst58)*(((((sj11)*(x3006)))+(((sj11)*(x3007)))+(((IkReal(-1.00000000000000))*(x3002)*(x3004)))+(((cj11)*(x3005)))+(((x3003)*(x3004))))))) < IKFAST_ATAN2_MAGTHRESH )
13493  continue;
13494 j10array[0]=IKatan2(((gconst58)*(((((IkReal(-1.00000000000000))*(sj11)*(x3005)))+(((cj11)*(x3007)))+(((cj11)*(x3006)))+(((IkReal(-1.00000000000000))*(x3001)*(x3003)))+(((x3001)*(x3002)))))), ((gconst58)*(((((sj11)*(x3006)))+(((sj11)*(x3007)))+(((IkReal(-1.00000000000000))*(x3002)*(x3004)))+(((cj11)*(x3005)))+(((x3003)*(x3004)))))));
13495 sj10array[0]=IKsin(j10array[0]);
13496 cj10array[0]=IKcos(j10array[0]);
13497 if( j10array[0] > IKPI )
13498 {
13499  j10array[0]-=IK2PI;
13500 }
13501 else if( j10array[0] < -IKPI )
13502 { j10array[0]+=IK2PI;
13503 }
13504 j10valid[0] = true;
13505 for(int ij10 = 0; ij10 < 1; ++ij10)
13506 {
13507 if( !j10valid[ij10] )
13508 {
13509  continue;
13510 }
13511 _ij10[0] = ij10; _ij10[1] = -1;
13512 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
13513 {
13514 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
13515 {
13516  j10valid[iij10]=false; _ij10[1] = iij10; break;
13517 }
13518 }
13519 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
13520 {
13521 IkReal evalcond[4];
13522 IkReal x3008=IKsin(j10);
13523 IkReal x3009=IKcos(j10);
13524 IkReal x3010=((cj14)*(sj9));
13525 IkReal x3011=((IkReal(1.00000000000000))*(r11));
13526 IkReal x3012=((cj13)*(sj14));
13527 IkReal x3013=((IkReal(1.00000000000000))*(cj9));
13528 IkReal x3014=((cj13)*(cj14));
13529 IkReal x3015=((cj11)*(x3008));
13530 IkReal x3016=((sj11)*(x3009));
13531 IkReal x3017=((cj11)*(x3009));
13532 IkReal x3018=((sj11)*(x3008));
13533 IkReal x3019=((x3016)+(x3015));
13534 evalcond[0]=((((cj14)*(r21)))+(x3019)+(((r20)*(sj14))));
13535 evalcond[1]=((x3018)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x3014)))+(((r21)*(x3012)))+(((IkReal(-1.00000000000000))*(x3017))));
13536 evalcond[2]=((x3017)+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x3013)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x3013)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x3010)*(x3011)))+(((IkReal(-1.00000000000000))*(x3018))));
13537 evalcond[3]=((((cj9)*(r00)*(x3014)))+(x3019)+(((IkReal(-1.00000000000000))*(sj9)*(x3011)*(x3012)))+(((cj9)*(r02)*(sj13)))+(((IkReal(-1.00000000000000))*(r01)*(x3012)*(x3013)))+(((cj13)*(r10)*(x3010)))+(((r12)*(sj13)*(sj9))));
13538 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
13539 {
13540 continue;
13541 }
13542 }
13543 
13544 {
13545 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13546 vinfos[0].jointtype = 1;
13547 vinfos[0].foffset = j9;
13548 vinfos[0].indices[0] = _ij9[0];
13549 vinfos[0].indices[1] = _ij9[1];
13550 vinfos[0].maxsolutions = _nj9;
13551 vinfos[1].jointtype = 1;
13552 vinfos[1].foffset = j10;
13553 vinfos[1].indices[0] = _ij10[0];
13554 vinfos[1].indices[1] = _ij10[1];
13555 vinfos[1].maxsolutions = _nj10;
13556 vinfos[2].jointtype = 1;
13557 vinfos[2].foffset = j11;
13558 vinfos[2].indices[0] = _ij11[0];
13559 vinfos[2].indices[1] = _ij11[1];
13560 vinfos[2].maxsolutions = _nj11;
13561 vinfos[3].jointtype = 1;
13562 vinfos[3].foffset = j12;
13563 vinfos[3].indices[0] = _ij12[0];
13564 vinfos[3].indices[1] = _ij12[1];
13565 vinfos[3].maxsolutions = _nj12;
13566 vinfos[4].jointtype = 1;
13567 vinfos[4].foffset = j13;
13568 vinfos[4].indices[0] = _ij13[0];
13569 vinfos[4].indices[1] = _ij13[1];
13570 vinfos[4].maxsolutions = _nj13;
13571 vinfos[5].jointtype = 1;
13572 vinfos[5].foffset = j14;
13573 vinfos[5].indices[0] = _ij14[0];
13574 vinfos[5].indices[1] = _ij14[1];
13575 vinfos[5].maxsolutions = _nj14;
13576 std::vector<int> vfree(0);
13577 solutions.AddSolution(vinfos,vfree);
13578 }
13579 }
13580 }
13581 
13582 }
13583 
13584 }
13585 }
13586 }
13587 
13588 } else
13589 {
13590 IkReal x3020=((cj13)*(sj9));
13591 IkReal x3021=((r01)*(sj14));
13592 IkReal x3022=((cj9)*(sj13));
13593 IkReal x3023=((sj13)*(sj9));
13594 IkReal x3024=((cj14)*(r01));
13595 IkReal x3025=((cj14)*(r10));
13596 IkReal x3026=((cj13)*(cj9));
13597 IkReal x3027=((IkReal(1.00000000000000))*(cj9));
13598 IkReal x3028=((sj14)*(sj9));
13599 IkReal x3029=((IkReal(1.00000000000000))*(cj14)*(sj9));
13600 IkReal x3030=((sj14)*(x3027));
13601 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
13602 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
13603 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
13604 evalcond[3]=((IkReal(1.00000000000000))+(((r00)*(x3028)))+(((sj9)*(x3024)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x3027)))+(((IkReal(-1.00000000000000))*(r10)*(x3030))));
13605 evalcond[4]=((((IkReal(-1.00000000000000))*(r11)*(x3029)))+(((IkReal(-1.00000000000000))*(r00)*(x3030)))+(((IkReal(-1.00000000000000))*(x3024)*(x3027)))+(((IkReal(-1.00000000000000))*(r10)*(x3028))));
13606 evalcond[5]=((((r12)*(x3022)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(x3026)))+(((x3020)*(x3021)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x3020)))+(((x3025)*(x3026)))+(((IkReal(-1.00000000000000))*(r02)*(x3023))));
13607 evalcond[6]=((((r02)*(x3020)))+(((x3021)*(x3023)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(x3022)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x3023)))+(((IkReal(-1.00000000000000))*(r12)*(x3026)))+(((x3022)*(x3025))));
13608 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 )
13609 {
13610 {
13611 IkReal j11array[1], cj11array[1], sj11array[1];
13612 bool j11valid[1]={false};
13613 _nj11 = 1;
13614 IkReal x3031=((IkReal(4.00000000000000))*(cj13));
13615 IkReal x3032=((npy)*(sj14));
13616 IkReal x3033=((cj14)*(npx));
13617 IkReal x3034=((IkReal(4.00000000000000))*(sj13));
13618 if( IKabs(((IkReal(0.120000000000000))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x3033)*(x3034)))+(((x3032)*(x3034)))+(((npz)*(x3031))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(-0.360000000000000))*(cj13)))+(((npz)*(x3034)))+(((x3031)*(x3033)))+(((IkReal(-1.00000000000000))*(x3031)*(x3032))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x3033)*(x3034)))+(((x3032)*(x3034)))+(((npz)*(x3031)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(-0.360000000000000))*(cj13)))+(((npz)*(x3034)))+(((x3031)*(x3033)))+(((IkReal(-1.00000000000000))*(x3031)*(x3032)))))-1) <= IKFAST_SINCOS_THRESH )
13619  continue;
13620 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x3033)*(x3034)))+(((x3032)*(x3034)))+(((npz)*(x3031)))), ((IkReal(-0.940000000000000))+(((IkReal(-0.360000000000000))*(cj13)))+(((npz)*(x3034)))+(((x3031)*(x3033)))+(((IkReal(-1.00000000000000))*(x3031)*(x3032)))));
13621 sj11array[0]=IKsin(j11array[0]);
13622 cj11array[0]=IKcos(j11array[0]);
13623 if( j11array[0] > IKPI )
13624 {
13625  j11array[0]-=IK2PI;
13626 }
13627 else if( j11array[0] < -IKPI )
13628 { j11array[0]+=IK2PI;
13629 }
13630 j11valid[0] = true;
13631 for(int ij11 = 0; ij11 < 1; ++ij11)
13632 {
13633 if( !j11valid[ij11] )
13634 {
13635  continue;
13636 }
13637 _ij11[0] = ij11; _ij11[1] = -1;
13638 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
13639 {
13640 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
13641 {
13642  j11valid[iij11]=false; _ij11[1] = iij11; break;
13643 }
13644 }
13645 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
13646 {
13647 IkReal evalcond[2];
13648 IkReal x3035=((IkReal(1.00000000000000))*(sj13));
13649 IkReal x3036=((cj14)*(npx));
13650 IkReal x3037=((npy)*(sj14));
13651 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x3035)))+(((IkReal(-1.00000000000000))*(cj13)*(x3036)))+(((cj13)*(x3037))));
13652 evalcond[1]=((IkReal(0.0300000000000000))+(((sj13)*(x3037)))+(((IkReal(-0.250000000000000))*(IKsin(j11))))+(((IkReal(-1.00000000000000))*(x3035)*(x3036)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
13653 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
13654 {
13655 continue;
13656 }
13657 }
13658 
13659 {
13660 IkReal dummyeval[1];
13661 IkReal gconst60;
13662 gconst60=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
13663 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
13664 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
13665 {
13666 {
13667 IkReal dummyeval[1];
13668 IkReal gconst61;
13669 gconst61=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
13670 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
13671 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
13672 {
13673 continue;
13674 
13675 } else
13676 {
13677 {
13678 IkReal j10array[1], cj10array[1], sj10array[1];
13679 bool j10valid[1]={false};
13680 _nj10 = 1;
13681 IkReal x3038=((cj13)*(sj14));
13682 IkReal x3039=((IkReal(1.00000000000000))*(cj11));
13683 IkReal x3040=((r11)*(sj9));
13684 IkReal x3041=((cj11)*(sj13));
13685 IkReal x3042=((r12)*(sj9));
13686 IkReal x3043=((cj9)*(r00));
13687 IkReal x3044=((sj11)*(sj13));
13688 IkReal x3045=((cj9)*(r02));
13689 IkReal x3046=((cj9)*(r01));
13690 IkReal x3047=((IkReal(1.00000000000000))*(sj11));
13691 IkReal x3048=((r10)*(sj9));
13692 IkReal x3049=((cj13)*(cj14)*(sj11));
13693 IkReal x3050=((cj11)*(cj13)*(cj14));
13694 if( IKabs(((gconst61)*(((((x3048)*(x3050)))+(((IkReal(-1.00000000000000))*(r22)*(x3044)))+(((IkReal(-1.00000000000000))*(x3038)*(x3039)*(x3040)))+(((IkReal(-1.00000000000000))*(x3038)*(x3039)*(x3046)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x3047)))+(((x3043)*(x3050)))+(((r21)*(sj11)*(x3038)))+(((x3041)*(x3042)))+(((x3041)*(x3045))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst61)*(((((r22)*(x3041)))+(((x3043)*(x3049)))+(((x3042)*(x3044)))+(((IkReal(-1.00000000000000))*(x3038)*(x3040)*(x3047)))+(((IkReal(-1.00000000000000))*(x3038)*(x3046)*(x3047)))+(((IkReal(-1.00000000000000))*(r21)*(x3038)*(x3039)))+(((x3048)*(x3049)))+(((x3044)*(x3045)))+(((r20)*(x3050))))))) < IKFAST_ATAN2_MAGTHRESH )
13695  continue;
13696 j10array[0]=IKatan2(((gconst61)*(((((x3048)*(x3050)))+(((IkReal(-1.00000000000000))*(r22)*(x3044)))+(((IkReal(-1.00000000000000))*(x3038)*(x3039)*(x3040)))+(((IkReal(-1.00000000000000))*(x3038)*(x3039)*(x3046)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x3047)))+(((x3043)*(x3050)))+(((r21)*(sj11)*(x3038)))+(((x3041)*(x3042)))+(((x3041)*(x3045)))))), ((gconst61)*(((((r22)*(x3041)))+(((x3043)*(x3049)))+(((x3042)*(x3044)))+(((IkReal(-1.00000000000000))*(x3038)*(x3040)*(x3047)))+(((IkReal(-1.00000000000000))*(x3038)*(x3046)*(x3047)))+(((IkReal(-1.00000000000000))*(r21)*(x3038)*(x3039)))+(((x3048)*(x3049)))+(((x3044)*(x3045)))+(((r20)*(x3050)))))));
13697 sj10array[0]=IKsin(j10array[0]);
13698 cj10array[0]=IKcos(j10array[0]);
13699 if( j10array[0] > IKPI )
13700 {
13701  j10array[0]-=IK2PI;
13702 }
13703 else if( j10array[0] < -IKPI )
13704 { j10array[0]+=IK2PI;
13705 }
13706 j10valid[0] = true;
13707 for(int ij10 = 0; ij10 < 1; ++ij10)
13708 {
13709 if( !j10valid[ij10] )
13710 {
13711  continue;
13712 }
13713 _ij10[0] = ij10; _ij10[1] = -1;
13714 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
13715 {
13716 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
13717 {
13718  j10valid[iij10]=false; _ij10[1] = iij10; break;
13719 }
13720 }
13721 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
13722 {
13723 IkReal evalcond[4];
13724 IkReal x3051=IKsin(j10);
13725 IkReal x3052=IKcos(j10);
13726 IkReal x3053=((IkReal(1.00000000000000))*(sj13));
13727 IkReal x3054=((r11)*(sj9));
13728 IkReal x3055=((cj9)*(r01));
13729 IkReal x3056=((r21)*(sj14));
13730 IkReal x3057=((cj9)*(r02));
13731 IkReal x3058=((sj13)*(sj9));
13732 IkReal x3059=((cj14)*(r10));
13733 IkReal x3060=((IkReal(1.00000000000000))*(cj13));
13734 IkReal x3061=((cj14)*(r20));
13735 IkReal x3062=((cj11)*(x3051));
13736 IkReal x3063=((sj11)*(x3052));
13737 IkReal x3064=((sj14)*(x3060));
13738 IkReal x3065=((sj11)*(x3051));
13739 IkReal x3066=((cj11)*(x3052));
13740 IkReal x3067=((cj14)*(cj9)*(r00));
13741 IkReal x3068=((x3063)+(x3062));
13742 evalcond[0]=((((IkReal(-1.00000000000000))*(x3066)))+(((IkReal(-1.00000000000000))*(x3060)*(x3061)))+(x3065)+(((cj13)*(x3056)))+(((IkReal(-1.00000000000000))*(r22)*(x3053))));
13743 evalcond[1]=((((sj13)*(x3056)))+(x3068)+(((IkReal(-1.00000000000000))*(x3053)*(x3061)))+(((cj13)*(r22))));
13744 evalcond[2]=((((cj13)*(x3067)))+(((sj13)*(x3057)))+(((IkReal(-1.00000000000000))*(x3054)*(x3064)))+(x3068)+(((r12)*(x3058)))+(((IkReal(-1.00000000000000))*(x3055)*(x3064)))+(((cj13)*(sj9)*(x3059))));
13745 evalcond[3]=((((IkReal(-1.00000000000000))*(x3065)))+(x3066)+(((IkReal(-1.00000000000000))*(sj14)*(x3053)*(x3054)))+(((IkReal(-1.00000000000000))*(sj14)*(x3053)*(x3055)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3060)))+(((sj13)*(x3067)))+(((x3058)*(x3059)))+(((IkReal(-1.00000000000000))*(x3057)*(x3060))));
13746 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
13747 {
13748 continue;
13749 }
13750 }
13751 
13752 {
13753 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13754 vinfos[0].jointtype = 1;
13755 vinfos[0].foffset = j9;
13756 vinfos[0].indices[0] = _ij9[0];
13757 vinfos[0].indices[1] = _ij9[1];
13758 vinfos[0].maxsolutions = _nj9;
13759 vinfos[1].jointtype = 1;
13760 vinfos[1].foffset = j10;
13761 vinfos[1].indices[0] = _ij10[0];
13762 vinfos[1].indices[1] = _ij10[1];
13763 vinfos[1].maxsolutions = _nj10;
13764 vinfos[2].jointtype = 1;
13765 vinfos[2].foffset = j11;
13766 vinfos[2].indices[0] = _ij11[0];
13767 vinfos[2].indices[1] = _ij11[1];
13768 vinfos[2].maxsolutions = _nj11;
13769 vinfos[3].jointtype = 1;
13770 vinfos[3].foffset = j12;
13771 vinfos[3].indices[0] = _ij12[0];
13772 vinfos[3].indices[1] = _ij12[1];
13773 vinfos[3].maxsolutions = _nj12;
13774 vinfos[4].jointtype = 1;
13775 vinfos[4].foffset = j13;
13776 vinfos[4].indices[0] = _ij13[0];
13777 vinfos[4].indices[1] = _ij13[1];
13778 vinfos[4].maxsolutions = _nj13;
13779 vinfos[5].jointtype = 1;
13780 vinfos[5].foffset = j14;
13781 vinfos[5].indices[0] = _ij14[0];
13782 vinfos[5].indices[1] = _ij14[1];
13783 vinfos[5].maxsolutions = _nj14;
13784 std::vector<int> vfree(0);
13785 solutions.AddSolution(vinfos,vfree);
13786 }
13787 }
13788 }
13789 
13790 }
13791 
13792 }
13793 
13794 } else
13795 {
13796 {
13797 IkReal j10array[1], cj10array[1], sj10array[1];
13798 bool j10valid[1]={false};
13799 _nj10 = 1;
13800 IkReal x3069=((cj13)*(sj11));
13801 IkReal x3070=((r21)*(sj14));
13802 IkReal x3071=((cj11)*(cj13));
13803 IkReal x3072=((sj11)*(sj13));
13804 IkReal x3073=((cj11)*(sj13));
13805 IkReal x3074=((IkReal(1.00000000000000))*(cj14)*(r20));
13806 if( IKabs(((gconst60)*(((((IkReal(-1.00000000000000))*(x3073)*(x3074)))+(((r22)*(x3071)))+(((IkReal(-1.00000000000000))*(x3069)*(x3074)))+(((x3070)*(x3073)))+(((x3069)*(x3070)))+(((IkReal(-1.00000000000000))*(r22)*(x3072))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst60)*(((((IkReal(-1.00000000000000))*(x3072)*(x3074)))+(((r22)*(x3069)))+(((r22)*(x3073)))+(((x3070)*(x3072)))+(((IkReal(-1.00000000000000))*(x3070)*(x3071)))+(((cj14)*(r20)*(x3071))))))) < IKFAST_ATAN2_MAGTHRESH )
13807  continue;
13808 j10array[0]=IKatan2(((gconst60)*(((((IkReal(-1.00000000000000))*(x3073)*(x3074)))+(((r22)*(x3071)))+(((IkReal(-1.00000000000000))*(x3069)*(x3074)))+(((x3070)*(x3073)))+(((x3069)*(x3070)))+(((IkReal(-1.00000000000000))*(r22)*(x3072)))))), ((gconst60)*(((((IkReal(-1.00000000000000))*(x3072)*(x3074)))+(((r22)*(x3069)))+(((r22)*(x3073)))+(((x3070)*(x3072)))+(((IkReal(-1.00000000000000))*(x3070)*(x3071)))+(((cj14)*(r20)*(x3071)))))));
13809 sj10array[0]=IKsin(j10array[0]);
13810 cj10array[0]=IKcos(j10array[0]);
13811 if( j10array[0] > IKPI )
13812 {
13813  j10array[0]-=IK2PI;
13814 }
13815 else if( j10array[0] < -IKPI )
13816 { j10array[0]+=IK2PI;
13817 }
13818 j10valid[0] = true;
13819 for(int ij10 = 0; ij10 < 1; ++ij10)
13820 {
13821 if( !j10valid[ij10] )
13822 {
13823  continue;
13824 }
13825 _ij10[0] = ij10; _ij10[1] = -1;
13826 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
13827 {
13828 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
13829 {
13830  j10valid[iij10]=false; _ij10[1] = iij10; break;
13831 }
13832 }
13833 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
13834 {
13835 IkReal evalcond[4];
13836 IkReal x3075=IKsin(j10);
13837 IkReal x3076=IKcos(j10);
13838 IkReal x3077=((IkReal(1.00000000000000))*(sj13));
13839 IkReal x3078=((r11)*(sj9));
13840 IkReal x3079=((cj9)*(r01));
13841 IkReal x3080=((r21)*(sj14));
13842 IkReal x3081=((cj9)*(r02));
13843 IkReal x3082=((sj13)*(sj9));
13844 IkReal x3083=((cj14)*(r10));
13845 IkReal x3084=((IkReal(1.00000000000000))*(cj13));
13846 IkReal x3085=((cj14)*(r20));
13847 IkReal x3086=((cj11)*(x3075));
13848 IkReal x3087=((sj11)*(x3076));
13849 IkReal x3088=((sj14)*(x3084));
13850 IkReal x3089=((sj11)*(x3075));
13851 IkReal x3090=((cj11)*(x3076));
13852 IkReal x3091=((cj14)*(cj9)*(r00));
13853 IkReal x3092=((x3087)+(x3086));
13854 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x3077)))+(x3089)+(((cj13)*(x3080)))+(((IkReal(-1.00000000000000))*(x3084)*(x3085)))+(((IkReal(-1.00000000000000))*(x3090))));
13855 evalcond[1]=((x3092)+(((sj13)*(x3080)))+(((IkReal(-1.00000000000000))*(x3077)*(x3085)))+(((cj13)*(r22))));
13856 evalcond[2]=((((IkReal(-1.00000000000000))*(x3079)*(x3088)))+(x3092)+(((r12)*(x3082)))+(((sj13)*(x3081)))+(((IkReal(-1.00000000000000))*(x3078)*(x3088)))+(((cj13)*(sj9)*(x3083)))+(((cj13)*(x3091))));
13857 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x3077)*(x3078)))+(((IkReal(-1.00000000000000))*(sj14)*(x3077)*(x3079)))+(x3090)+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3084)))+(((x3082)*(x3083)))+(((IkReal(-1.00000000000000))*(x3089)))+(((sj13)*(x3091)))+(((IkReal(-1.00000000000000))*(x3081)*(x3084))));
13858 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
13859 {
13860 continue;
13861 }
13862 }
13863 
13864 {
13865 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
13866 vinfos[0].jointtype = 1;
13867 vinfos[0].foffset = j9;
13868 vinfos[0].indices[0] = _ij9[0];
13869 vinfos[0].indices[1] = _ij9[1];
13870 vinfos[0].maxsolutions = _nj9;
13871 vinfos[1].jointtype = 1;
13872 vinfos[1].foffset = j10;
13873 vinfos[1].indices[0] = _ij10[0];
13874 vinfos[1].indices[1] = _ij10[1];
13875 vinfos[1].maxsolutions = _nj10;
13876 vinfos[2].jointtype = 1;
13877 vinfos[2].foffset = j11;
13878 vinfos[2].indices[0] = _ij11[0];
13879 vinfos[2].indices[1] = _ij11[1];
13880 vinfos[2].maxsolutions = _nj11;
13881 vinfos[3].jointtype = 1;
13882 vinfos[3].foffset = j12;
13883 vinfos[3].indices[0] = _ij12[0];
13884 vinfos[3].indices[1] = _ij12[1];
13885 vinfos[3].maxsolutions = _nj12;
13886 vinfos[4].jointtype = 1;
13887 vinfos[4].foffset = j13;
13888 vinfos[4].indices[0] = _ij13[0];
13889 vinfos[4].indices[1] = _ij13[1];
13890 vinfos[4].maxsolutions = _nj13;
13891 vinfos[5].jointtype = 1;
13892 vinfos[5].foffset = j14;
13893 vinfos[5].indices[0] = _ij14[0];
13894 vinfos[5].indices[1] = _ij14[1];
13895 vinfos[5].maxsolutions = _nj14;
13896 std::vector<int> vfree(0);
13897 solutions.AddSolution(vinfos,vfree);
13898 }
13899 }
13900 }
13901 
13902 }
13903 
13904 }
13905 }
13906 }
13907 
13908 } else
13909 {
13910 IkReal x3093=((cj13)*(sj9));
13911 IkReal x3094=((r01)*(sj14));
13912 IkReal x3095=((cj9)*(sj13));
13913 IkReal x3096=((sj13)*(sj9));
13914 IkReal x3097=((cj14)*(r01));
13915 IkReal x3098=((cj14)*(r10));
13916 IkReal x3099=((cj13)*(cj9));
13917 IkReal x3100=((IkReal(1.00000000000000))*(cj9));
13918 IkReal x3101=((sj14)*(sj9));
13919 IkReal x3102=((IkReal(1.00000000000000))*(cj14)*(sj9));
13920 IkReal x3103=((sj14)*(x3100));
13921 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
13922 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
13923 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
13924 evalcond[3]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(r10)*(x3103)))+(((sj9)*(x3097)))+(((r00)*(x3101)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x3100))));
13925 evalcond[4]=((((IkReal(-1.00000000000000))*(r10)*(x3101)))+(((IkReal(-1.00000000000000))*(r11)*(x3102)))+(((IkReal(-1.00000000000000))*(r00)*(x3103)))+(((IkReal(-1.00000000000000))*(x3097)*(x3100))));
13926 evalcond[5]=((((x3098)*(x3099)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(x3099)))+(((IkReal(-1.00000000000000))*(r02)*(x3096)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x3093)))+(((x3093)*(x3094)))+(((r12)*(x3095))));
13927 evalcond[6]=((((IkReal(-1.00000000000000))*(r11)*(sj14)*(x3095)))+(((IkReal(-1.00000000000000))*(r12)*(x3099)))+(((r02)*(x3093)))+(((IkReal(-1.00000000000000))*(cj14)*(r00)*(x3096)))+(((x3095)*(x3098)))+(((x3094)*(x3096))));
13928 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 )
13929 {
13930 {
13931 IkReal j11array[1], cj11array[1], sj11array[1];
13932 bool j11valid[1]={false};
13933 _nj11 = 1;
13934 IkReal x3104=((IkReal(4.00000000000000))*(cj13));
13935 IkReal x3105=((npy)*(sj14));
13936 IkReal x3106=((cj14)*(npx));
13937 IkReal x3107=((IkReal(4.00000000000000))*(sj13));
13938 if( IKabs(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x3104)))+(((IkReal(-1.00000000000000))*(x3105)*(x3107)))+(((x3106)*(x3107)))+(((IkReal(-0.360000000000000))*(sj13))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x3104)*(x3105)))+(((npz)*(x3107)))+(((x3104)*(x3106))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x3104)))+(((IkReal(-1.00000000000000))*(x3105)*(x3107)))+(((x3106)*(x3107)))+(((IkReal(-0.360000000000000))*(sj13)))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x3104)*(x3105)))+(((npz)*(x3107)))+(((x3104)*(x3106)))))-1) <= IKFAST_SINCOS_THRESH )
13939  continue;
13940 j11array[0]=IKatan2(((IkReal(0.120000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x3104)))+(((IkReal(-1.00000000000000))*(x3105)*(x3107)))+(((x3106)*(x3107)))+(((IkReal(-0.360000000000000))*(sj13)))), ((IkReal(-0.940000000000000))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x3104)*(x3105)))+(((npz)*(x3107)))+(((x3104)*(x3106)))));
13941 sj11array[0]=IKsin(j11array[0]);
13942 cj11array[0]=IKcos(j11array[0]);
13943 if( j11array[0] > IKPI )
13944 {
13945  j11array[0]-=IK2PI;
13946 }
13947 else if( j11array[0] < -IKPI )
13948 { j11array[0]+=IK2PI;
13949 }
13950 j11valid[0] = true;
13951 for(int ij11 = 0; ij11 < 1; ++ij11)
13952 {
13953 if( !j11valid[ij11] )
13954 {
13955  continue;
13956 }
13957 _ij11[0] = ij11; _ij11[1] = -1;
13958 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
13959 {
13960 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
13961 {
13962  j11valid[iij11]=false; _ij11[1] = iij11; break;
13963 }
13964 }
13965 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
13966 {
13967 IkReal evalcond[2];
13968 IkReal x3108=((IkReal(1.00000000000000))*(sj13));
13969 IkReal x3109=((cj14)*(npx));
13970 IkReal x3110=((npy)*(sj14));
13971 evalcond[0]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x3110)))+(((IkReal(-1.00000000000000))*(npz)*(x3108)))+(((IkReal(-1.00000000000000))*(cj13)*(x3109))));
13972 evalcond[1]=((IkReal(-0.0300000000000000))+(((sj13)*(x3110)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x3108)*(x3109)))+(((IkReal(0.250000000000000))*(IKsin(j11)))));
13973 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
13974 {
13975 continue;
13976 }
13977 }
13978 
13979 {
13980 IkReal dummyeval[1];
13981 IkReal gconst62;
13982 gconst62=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
13983 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
13984 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
13985 {
13986 {
13987 IkReal dummyeval[1];
13988 IkReal gconst63;
13989 gconst63=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
13990 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
13991 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
13992 {
13993 continue;
13994 
13995 } else
13996 {
13997 {
13998 IkReal j10array[1], cj10array[1], sj10array[1];
13999 bool j10valid[1]={false};
14000 _nj10 = 1;
14001 IkReal x3111=((cj13)*(sj14));
14002 IkReal x3112=((IkReal(1.00000000000000))*(cj11));
14003 IkReal x3113=((r11)*(sj9));
14004 IkReal x3114=((IkReal(1.00000000000000))*(sj11));
14005 IkReal x3115=((cj13)*(cj14));
14006 IkReal x3116=((cj11)*(sj13));
14007 IkReal x3117=((r12)*(sj9));
14008 IkReal x3118=((sj11)*(sj13));
14009 IkReal x3119=((cj9)*(r02));
14010 IkReal x3120=((cj9)*(r01));
14011 IkReal x3121=((r10)*(sj9));
14012 IkReal x3122=((cj9)*(r00));
14013 if( IKabs(((gconst63)*(((((IkReal(-1.00000000000000))*(r20)*(x3114)*(x3115)))+(((cj11)*(x3115)*(x3121)))+(((cj11)*(x3115)*(x3122)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x3114)))+(((IkReal(-1.00000000000000))*(x3111)*(x3112)*(x3113)))+(((r21)*(sj11)*(x3111)))+(((x3116)*(x3117)))+(((x3116)*(x3119)))+(((IkReal(-1.00000000000000))*(x3111)*(x3112)*(x3120))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst63)*(((((cj11)*(r20)*(x3115)))+(((r22)*(x3116)))+(((x3118)*(x3119)))+(((IkReal(-1.00000000000000))*(r21)*(x3111)*(x3112)))+(((sj11)*(x3115)*(x3121)))+(((sj11)*(x3115)*(x3122)))+(((IkReal(-1.00000000000000))*(x3111)*(x3113)*(x3114)))+(((IkReal(-1.00000000000000))*(x3111)*(x3114)*(x3120)))+(((x3117)*(x3118))))))) < IKFAST_ATAN2_MAGTHRESH )
14014  continue;
14015 j10array[0]=IKatan2(((gconst63)*(((((IkReal(-1.00000000000000))*(r20)*(x3114)*(x3115)))+(((cj11)*(x3115)*(x3121)))+(((cj11)*(x3115)*(x3122)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x3114)))+(((IkReal(-1.00000000000000))*(x3111)*(x3112)*(x3113)))+(((r21)*(sj11)*(x3111)))+(((x3116)*(x3117)))+(((x3116)*(x3119)))+(((IkReal(-1.00000000000000))*(x3111)*(x3112)*(x3120)))))), ((gconst63)*(((((cj11)*(r20)*(x3115)))+(((r22)*(x3116)))+(((x3118)*(x3119)))+(((IkReal(-1.00000000000000))*(r21)*(x3111)*(x3112)))+(((sj11)*(x3115)*(x3121)))+(((sj11)*(x3115)*(x3122)))+(((IkReal(-1.00000000000000))*(x3111)*(x3113)*(x3114)))+(((IkReal(-1.00000000000000))*(x3111)*(x3114)*(x3120)))+(((x3117)*(x3118)))))));
14016 sj10array[0]=IKsin(j10array[0]);
14017 cj10array[0]=IKcos(j10array[0]);
14018 if( j10array[0] > IKPI )
14019 {
14020  j10array[0]-=IK2PI;
14021 }
14022 else if( j10array[0] < -IKPI )
14023 { j10array[0]+=IK2PI;
14024 }
14025 j10valid[0] = true;
14026 for(int ij10 = 0; ij10 < 1; ++ij10)
14027 {
14028 if( !j10valid[ij10] )
14029 {
14030  continue;
14031 }
14032 _ij10[0] = ij10; _ij10[1] = -1;
14033 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14034 {
14035 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14036 {
14037  j10valid[iij10]=false; _ij10[1] = iij10; break;
14038 }
14039 }
14040 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14041 {
14042 IkReal evalcond[4];
14043 IkReal x3123=IKsin(j10);
14044 IkReal x3124=IKcos(j10);
14045 IkReal x3125=((IkReal(1.00000000000000))*(sj13));
14046 IkReal x3126=((r11)*(sj9));
14047 IkReal x3127=((cj9)*(r01));
14048 IkReal x3128=((IkReal(1.00000000000000))*(cj11));
14049 IkReal x3129=((r21)*(sj14));
14050 IkReal x3130=((cj9)*(r02));
14051 IkReal x3131=((sj13)*(sj9));
14052 IkReal x3132=((cj14)*(r10));
14053 IkReal x3133=((IkReal(1.00000000000000))*(cj13));
14054 IkReal x3134=((cj14)*(r20));
14055 IkReal x3135=((sj11)*(x3123));
14056 IkReal x3136=((sj14)*(x3133));
14057 IkReal x3137=((sj11)*(x3124));
14058 IkReal x3138=((cj14)*(cj9)*(r00));
14059 IkReal x3139=((x3124)*(x3128));
14060 evalcond[0]=((x3135)+(((cj13)*(x3129)))+(((IkReal(-1.00000000000000))*(x3133)*(x3134)))+(((IkReal(-1.00000000000000))*(r22)*(x3125)))+(((IkReal(-1.00000000000000))*(x3139))));
14061 evalcond[1]=((((IkReal(-1.00000000000000))*(x3125)*(x3134)))+(((IkReal(-1.00000000000000))*(x3137)))+(((sj13)*(x3129)))+(((IkReal(-1.00000000000000))*(x3123)*(x3128)))+(((cj13)*(r22))));
14062 evalcond[2]=((x3137)+(((cj13)*(x3138)))+(((cj13)*(sj9)*(x3132)))+(((sj13)*(x3130)))+(((cj11)*(x3123)))+(((r12)*(x3131)))+(((IkReal(-1.00000000000000))*(x3126)*(x3136)))+(((IkReal(-1.00000000000000))*(x3127)*(x3136))));
14063 evalcond[3]=((x3135)+(((x3131)*(x3132)))+(((IkReal(-1.00000000000000))*(x3130)*(x3133)))+(((sj13)*(x3138)))+(((IkReal(-1.00000000000000))*(sj14)*(x3125)*(x3127)))+(((IkReal(-1.00000000000000))*(sj14)*(x3125)*(x3126)))+(((IkReal(-1.00000000000000))*(x3139)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3133))));
14064 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
14065 {
14066 continue;
14067 }
14068 }
14069 
14070 {
14071 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14072 vinfos[0].jointtype = 1;
14073 vinfos[0].foffset = j9;
14074 vinfos[0].indices[0] = _ij9[0];
14075 vinfos[0].indices[1] = _ij9[1];
14076 vinfos[0].maxsolutions = _nj9;
14077 vinfos[1].jointtype = 1;
14078 vinfos[1].foffset = j10;
14079 vinfos[1].indices[0] = _ij10[0];
14080 vinfos[1].indices[1] = _ij10[1];
14081 vinfos[1].maxsolutions = _nj10;
14082 vinfos[2].jointtype = 1;
14083 vinfos[2].foffset = j11;
14084 vinfos[2].indices[0] = _ij11[0];
14085 vinfos[2].indices[1] = _ij11[1];
14086 vinfos[2].maxsolutions = _nj11;
14087 vinfos[3].jointtype = 1;
14088 vinfos[3].foffset = j12;
14089 vinfos[3].indices[0] = _ij12[0];
14090 vinfos[3].indices[1] = _ij12[1];
14091 vinfos[3].maxsolutions = _nj12;
14092 vinfos[4].jointtype = 1;
14093 vinfos[4].foffset = j13;
14094 vinfos[4].indices[0] = _ij13[0];
14095 vinfos[4].indices[1] = _ij13[1];
14096 vinfos[4].maxsolutions = _nj13;
14097 vinfos[5].jointtype = 1;
14098 vinfos[5].foffset = j14;
14099 vinfos[5].indices[0] = _ij14[0];
14100 vinfos[5].indices[1] = _ij14[1];
14101 vinfos[5].maxsolutions = _nj14;
14102 std::vector<int> vfree(0);
14103 solutions.AddSolution(vinfos,vfree);
14104 }
14105 }
14106 }
14107 
14108 }
14109 
14110 }
14111 
14112 } else
14113 {
14114 {
14115 IkReal j10array[1], cj10array[1], sj10array[1];
14116 bool j10valid[1]={false};
14117 _nj10 = 1;
14118 IkReal x3140=((cj13)*(r22));
14119 IkReal x3141=((IkReal(1.00000000000000))*(sj11));
14120 IkReal x3142=((IkReal(1.00000000000000))*(cj11));
14121 IkReal x3143=((r22)*(sj13));
14122 IkReal x3144=((cj14)*(r20));
14123 IkReal x3145=((cj13)*(r21)*(sj14));
14124 IkReal x3146=((r21)*(sj13)*(sj14));
14125 if( IKabs(((gconst62)*(((((cj11)*(x3140)))+(((cj11)*(x3146)))+(((IkReal(-1.00000000000000))*(sj13)*(x3142)*(x3144)))+(((IkReal(-1.00000000000000))*(x3141)*(x3145)))+(((sj11)*(x3143)))+(((cj13)*(sj11)*(x3144))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst62)*(((((IkReal(-1.00000000000000))*(cj13)*(x3142)*(x3144)))+(((cj11)*(x3145)))+(((IkReal(-1.00000000000000))*(x3142)*(x3143)))+(((IkReal(-1.00000000000000))*(sj13)*(x3141)*(x3144)))+(((sj11)*(x3140)))+(((sj11)*(x3146))))))) < IKFAST_ATAN2_MAGTHRESH )
14126  continue;
14127 j10array[0]=IKatan2(((gconst62)*(((((cj11)*(x3140)))+(((cj11)*(x3146)))+(((IkReal(-1.00000000000000))*(sj13)*(x3142)*(x3144)))+(((IkReal(-1.00000000000000))*(x3141)*(x3145)))+(((sj11)*(x3143)))+(((cj13)*(sj11)*(x3144)))))), ((gconst62)*(((((IkReal(-1.00000000000000))*(cj13)*(x3142)*(x3144)))+(((cj11)*(x3145)))+(((IkReal(-1.00000000000000))*(x3142)*(x3143)))+(((IkReal(-1.00000000000000))*(sj13)*(x3141)*(x3144)))+(((sj11)*(x3140)))+(((sj11)*(x3146)))))));
14128 sj10array[0]=IKsin(j10array[0]);
14129 cj10array[0]=IKcos(j10array[0]);
14130 if( j10array[0] > IKPI )
14131 {
14132  j10array[0]-=IK2PI;
14133 }
14134 else if( j10array[0] < -IKPI )
14135 { j10array[0]+=IK2PI;
14136 }
14137 j10valid[0] = true;
14138 for(int ij10 = 0; ij10 < 1; ++ij10)
14139 {
14140 if( !j10valid[ij10] )
14141 {
14142  continue;
14143 }
14144 _ij10[0] = ij10; _ij10[1] = -1;
14145 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14146 {
14147 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14148 {
14149  j10valid[iij10]=false; _ij10[1] = iij10; break;
14150 }
14151 }
14152 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14153 {
14154 IkReal evalcond[4];
14155 IkReal x3147=IKsin(j10);
14156 IkReal x3148=IKcos(j10);
14157 IkReal x3149=((IkReal(1.00000000000000))*(sj13));
14158 IkReal x3150=((r11)*(sj9));
14159 IkReal x3151=((cj9)*(r01));
14160 IkReal x3152=((IkReal(1.00000000000000))*(cj11));
14161 IkReal x3153=((r21)*(sj14));
14162 IkReal x3154=((cj9)*(r02));
14163 IkReal x3155=((sj13)*(sj9));
14164 IkReal x3156=((cj14)*(r10));
14165 IkReal x3157=((IkReal(1.00000000000000))*(cj13));
14166 IkReal x3158=((cj14)*(r20));
14167 IkReal x3159=((sj11)*(x3147));
14168 IkReal x3160=((sj14)*(x3157));
14169 IkReal x3161=((sj11)*(x3148));
14170 IkReal x3162=((cj14)*(cj9)*(r00));
14171 IkReal x3163=((x3148)*(x3152));
14172 evalcond[0]=((((IkReal(-1.00000000000000))*(x3157)*(x3158)))+(x3159)+(((IkReal(-1.00000000000000))*(r22)*(x3149)))+(((cj13)*(x3153)))+(((IkReal(-1.00000000000000))*(x3163))));
14173 evalcond[1]=((((IkReal(-1.00000000000000))*(x3161)))+(((IkReal(-1.00000000000000))*(x3149)*(x3158)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x3147)*(x3152)))+(((sj13)*(x3153))));
14174 evalcond[2]=((x3161)+(((IkReal(-1.00000000000000))*(x3150)*(x3160)))+(((cj11)*(x3147)))+(((r12)*(x3155)))+(((cj13)*(sj9)*(x3156)))+(((cj13)*(x3162)))+(((IkReal(-1.00000000000000))*(x3151)*(x3160)))+(((sj13)*(x3154))));
14175 evalcond[3]=((((sj13)*(x3162)))+(x3159)+(((x3155)*(x3156)))+(((IkReal(-1.00000000000000))*(sj14)*(x3149)*(x3151)))+(((IkReal(-1.00000000000000))*(sj14)*(x3149)*(x3150)))+(((IkReal(-1.00000000000000))*(x3154)*(x3157)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3157)))+(((IkReal(-1.00000000000000))*(x3163))));
14176 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
14177 {
14178 continue;
14179 }
14180 }
14181 
14182 {
14183 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14184 vinfos[0].jointtype = 1;
14185 vinfos[0].foffset = j9;
14186 vinfos[0].indices[0] = _ij9[0];
14187 vinfos[0].indices[1] = _ij9[1];
14188 vinfos[0].maxsolutions = _nj9;
14189 vinfos[1].jointtype = 1;
14190 vinfos[1].foffset = j10;
14191 vinfos[1].indices[0] = _ij10[0];
14192 vinfos[1].indices[1] = _ij10[1];
14193 vinfos[1].maxsolutions = _nj10;
14194 vinfos[2].jointtype = 1;
14195 vinfos[2].foffset = j11;
14196 vinfos[2].indices[0] = _ij11[0];
14197 vinfos[2].indices[1] = _ij11[1];
14198 vinfos[2].maxsolutions = _nj11;
14199 vinfos[3].jointtype = 1;
14200 vinfos[3].foffset = j12;
14201 vinfos[3].indices[0] = _ij12[0];
14202 vinfos[3].indices[1] = _ij12[1];
14203 vinfos[3].maxsolutions = _nj12;
14204 vinfos[4].jointtype = 1;
14205 vinfos[4].foffset = j13;
14206 vinfos[4].indices[0] = _ij13[0];
14207 vinfos[4].indices[1] = _ij13[1];
14208 vinfos[4].maxsolutions = _nj13;
14209 vinfos[5].jointtype = 1;
14210 vinfos[5].foffset = j14;
14211 vinfos[5].indices[0] = _ij14[0];
14212 vinfos[5].indices[1] = _ij14[1];
14213 vinfos[5].maxsolutions = _nj14;
14214 std::vector<int> vfree(0);
14215 solutions.AddSolution(vinfos,vfree);
14216 }
14217 }
14218 }
14219 
14220 }
14221 
14222 }
14223 }
14224 }
14225 
14226 } else
14227 {
14228 if( 1 )
14229 {
14230 continue;
14231 
14232 } else
14233 {
14234 }
14235 }
14236 }
14237 }
14238 }
14239 }
14240 
14241 } else
14242 {
14243 {
14244 IkReal j11array[1], cj11array[1], sj11array[1];
14245 bool j11valid[1]={false};
14246 _nj11 = 1;
14247 IkReal x3164=((IkReal(4.00000000000000))*(cj13));
14248 IkReal x3165=((npy)*(sj14));
14249 IkReal x3166=((IkReal(4.00000000000000))*(sj13));
14250 IkReal x3167=((cj14)*(npx));
14251 if( IKabs(((((IKabs(cj12) != 0)?((IkReal)1/(cj12)):(IkReal)1.0e30))*(((((npz)*(x3164)))+(((IkReal(0.380000000000000))*(sj12)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(0.120000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(x3166)*(x3167)))+(((x3165)*(x3166))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((npz)*(x3166)))+(((IkReal(-1.00000000000000))*(x3164)*(x3165)))+(((IkReal(-0.360000000000000))*(cj13)))+(((x3164)*(x3167))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(cj12) != 0)?((IkReal)1/(cj12)):(IkReal)1.0e30))*(((((npz)*(x3164)))+(((IkReal(0.380000000000000))*(sj12)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(0.120000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(x3166)*(x3167)))+(((x3165)*(x3166)))))))+IKsqr(((IkReal(-0.940000000000000))+(((npz)*(x3166)))+(((IkReal(-1.00000000000000))*(x3164)*(x3165)))+(((IkReal(-0.360000000000000))*(cj13)))+(((x3164)*(x3167)))))-1) <= IKFAST_SINCOS_THRESH )
14252  continue;
14253 j11array[0]=IKatan2(((((IKabs(cj12) != 0)?((IkReal)1/(cj12)):(IkReal)1.0e30))*(((((npz)*(x3164)))+(((IkReal(0.380000000000000))*(sj12)))+(((IkReal(0.360000000000000))*(sj13)))+(((IkReal(0.120000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(x3166)*(x3167)))+(((x3165)*(x3166)))))), ((IkReal(-0.940000000000000))+(((npz)*(x3166)))+(((IkReal(-1.00000000000000))*(x3164)*(x3165)))+(((IkReal(-0.360000000000000))*(cj13)))+(((x3164)*(x3167)))));
14254 sj11array[0]=IKsin(j11array[0]);
14255 cj11array[0]=IKcos(j11array[0]);
14256 if( j11array[0] > IKPI )
14257 {
14258  j11array[0]-=IK2PI;
14259 }
14260 else if( j11array[0] < -IKPI )
14261 { j11array[0]+=IK2PI;
14262 }
14263 j11valid[0] = true;
14264 for(int ij11 = 0; ij11 < 1; ++ij11)
14265 {
14266 if( !j11valid[ij11] )
14267 {
14268  continue;
14269 }
14270 _ij11[0] = ij11; _ij11[1] = -1;
14271 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
14272 {
14273 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
14274 {
14275  j11valid[iij11]=false; _ij11[1] = iij11; break;
14276 }
14277 }
14278 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
14279 {
14280 IkReal evalcond[3];
14281 IkReal x3168=IKsin(j11);
14282 IkReal x3169=((IkReal(1.00000000000000))*(sj13));
14283 IkReal x3170=((cj14)*(npx));
14284 IkReal x3171=((npy)*(sj14));
14285 IkReal x3172=((IkReal(0.250000000000000))*(x3168));
14286 evalcond[0]=((((IkReal(0.0950000000000000))*(cj12)))+(((sj12)*(x3172)))+(((cj14)*(npy)))+(((IkReal(-0.0300000000000000))*(sj12)))+(((npx)*(sj14))));
14287 evalcond[1]=((IkReal(0.235000000000000))+(((cj13)*(x3171)))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x3169)))+(((IkReal(-1.00000000000000))*(cj13)*(x3170))));
14288 evalcond[2]=((((IkReal(-1.00000000000000))*(x3169)*(x3170)))+(((IkReal(0.0950000000000000))*(sj12)))+(((sj13)*(x3171)))+(((cj13)*(npz)))+(((IkReal(0.0300000000000000))*(cj12)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(cj12)*(x3172))));
14289 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
14290 {
14291 continue;
14292 }
14293 }
14294 
14295 {
14296 IkReal dummyeval[1];
14297 IkReal gconst46;
14298 gconst46=IKsign(((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11))))));
14299 dummyeval[0]=((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11)))));
14300 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
14301 {
14302 {
14303 IkReal dummyeval[1];
14304 IkReal gconst47;
14305 gconst47=IKsign(((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11))))));
14306 dummyeval[0]=((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11)))));
14307 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
14308 {
14309 {
14310 IkReal evalcond[9];
14311 IkReal x3173=((r00)*(sj9));
14312 IkReal x3174=((IkReal(1.00000000000000))*(sj13));
14313 IkReal x3175=((cj13)*(sj14));
14314 IkReal x3176=((cj9)*(sj14));
14315 IkReal x3177=((cj13)*(r02));
14316 IkReal x3178=((sj13)*(sj14));
14317 IkReal x3179=((r01)*(sj9));
14318 IkReal x3180=((cj9)*(sj13));
14319 IkReal x3181=((IkReal(1.00000000000000))*(cj9));
14320 IkReal x3182=((cj14)*(r10));
14321 IkReal x3183=((cj14)*(npx));
14322 IkReal x3184=((IkReal(1.00000000000000))*(cj13));
14323 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
14324 evalcond[1]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(sj11))));
14325 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x3174)))+(((IkReal(-1.00000000000000))*(x3183)*(x3184)))+(((IkReal(0.250000000000000))*(cj11)))+(((npy)*(x3175))));
14326 evalcond[3]=((((sj14)*(x3173)))+(((IkReal(-1.00000000000000))*(r10)*(x3176)))+(((cj14)*(x3179)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x3181))));
14327 evalcond[4]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x3174)))+(((r21)*(x3178)))+(((cj13)*(r22))));
14328 evalcond[5]=((IkReal(0.0950000000000000))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x3174)*(x3183)))+(((npy)*(x3178))));
14329 evalcond[6]=((((IkReal(-1.00000000000000))*(cj14)*(x3173)*(x3184)))+(((IkReal(-1.00000000000000))*(r11)*(x3175)*(x3181)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x3174)))+(((r12)*(x3180)))+(((cj13)*(cj9)*(x3182)))+(((x3175)*(x3179))));
14330 evalcond[7]=((IkReal(1.00000000000000))+(((sj9)*(x3177)))+(((x3180)*(x3182)))+(((x3178)*(x3179)))+(((IkReal(-1.00000000000000))*(r11)*(x3174)*(x3176)))+(((IkReal(-1.00000000000000))*(cj14)*(x3173)*(x3174)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x3181))));
14331 evalcond[8]=((((cj14)*(r00)*(x3180)))+(((IkReal(-1.00000000000000))*(x3177)*(x3181)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x3174)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3184)))+(((IkReal(-1.00000000000000))*(r01)*(x3174)*(x3176)))+(((sj13)*(sj9)*(x3182))));
14332 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 )
14333 {
14334 {
14335 IkReal dummyeval[1];
14336 IkReal gconst48;
14337 gconst48=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
14338 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
14339 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
14340 {
14341 {
14342 IkReal dummyeval[1];
14343 IkReal gconst49;
14344 gconst49=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
14345 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
14346 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
14347 {
14348 continue;
14349 
14350 } else
14351 {
14352 {
14353 IkReal j10array[1], cj10array[1], sj10array[1];
14354 bool j10valid[1]={false};
14355 _nj10 = 1;
14356 IkReal x3185=((IkReal(1.00000000000000))*(cj11));
14357 IkReal x3186=((sj11)*(sj14));
14358 IkReal x3187=((r10)*(sj9));
14359 IkReal x3188=((cj9)*(r00));
14360 IkReal x3189=((cj14)*(sj11));
14361 IkReal x3190=((r11)*(sj9));
14362 IkReal x3191=((cj14)*(cj9)*(r01));
14363 if( IKabs(((gconst49)*(((((cj9)*(r01)*(x3189)))+(((cj11)*(cj14)*(r21)))+(((cj11)*(r20)*(sj14)))+(((x3186)*(x3188)))+(((x3186)*(x3187)))+(((x3189)*(x3190))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst49)*(((((IkReal(-1.00000000000000))*(x3185)*(x3191)))+(((IkReal(-1.00000000000000))*(sj14)*(x3185)*(x3187)))+(((IkReal(-1.00000000000000))*(sj14)*(x3185)*(x3188)))+(((r20)*(x3186)))+(((IkReal(-1.00000000000000))*(cj14)*(x3185)*(x3190)))+(((r21)*(x3189))))))) < IKFAST_ATAN2_MAGTHRESH )
14364  continue;
14365 j10array[0]=IKatan2(((gconst49)*(((((cj9)*(r01)*(x3189)))+(((cj11)*(cj14)*(r21)))+(((cj11)*(r20)*(sj14)))+(((x3186)*(x3188)))+(((x3186)*(x3187)))+(((x3189)*(x3190)))))), ((gconst49)*(((((IkReal(-1.00000000000000))*(x3185)*(x3191)))+(((IkReal(-1.00000000000000))*(sj14)*(x3185)*(x3187)))+(((IkReal(-1.00000000000000))*(sj14)*(x3185)*(x3188)))+(((r20)*(x3186)))+(((IkReal(-1.00000000000000))*(cj14)*(x3185)*(x3190)))+(((r21)*(x3189)))))));
14366 sj10array[0]=IKsin(j10array[0]);
14367 cj10array[0]=IKcos(j10array[0]);
14368 if( j10array[0] > IKPI )
14369 {
14370  j10array[0]-=IK2PI;
14371 }
14372 else if( j10array[0] < -IKPI )
14373 { j10array[0]+=IK2PI;
14374 }
14375 j10valid[0] = true;
14376 for(int ij10 = 0; ij10 < 1; ++ij10)
14377 {
14378 if( !j10valid[ij10] )
14379 {
14380  continue;
14381 }
14382 _ij10[0] = ij10; _ij10[1] = -1;
14383 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14384 {
14385 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14386 {
14387  j10valid[iij10]=false; _ij10[1] = iij10; break;
14388 }
14389 }
14390 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14391 {
14392 IkReal evalcond[4];
14393 IkReal x3192=IKsin(j10);
14394 IkReal x3193=IKcos(j10);
14395 IkReal x3194=((cj13)*(sj14));
14396 IkReal x3195=((cj13)*(cj14));
14397 IkReal x3196=((r10)*(sj9));
14398 IkReal x3197=((IkReal(1.00000000000000))*(cj9));
14399 IkReal x3198=((sj11)*(x3192));
14400 IkReal x3199=((IkReal(1.00000000000000))*(x3193));
14401 IkReal x3200=((cj11)*(x3192));
14402 IkReal x3201=((IkReal(1.00000000000000))*(r11)*(sj9));
14403 IkReal x3202=((cj11)*(x3199));
14404 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(sj11)*(x3199)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x3200))));
14405 evalcond[1]=((x3198)+(((r21)*(x3194)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x3202)))+(((IkReal(-1.00000000000000))*(r20)*(x3195))));
14406 evalcond[2]=((x3198)+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x3197)))+(((IkReal(-1.00000000000000))*(sj14)*(x3196)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x3197)))+(((IkReal(-1.00000000000000))*(x3202)))+(((IkReal(-1.00000000000000))*(cj14)*(x3201))));
14407 evalcond[3]=((((IkReal(-1.00000000000000))*(x3194)*(x3201)))+(((IkReal(-1.00000000000000))*(r01)*(x3194)*(x3197)))+(((cj9)*(r02)*(sj13)))+(x3200)+(((x3195)*(x3196)))+(((sj11)*(x3193)))+(((r12)*(sj13)*(sj9)))+(((cj9)*(r00)*(x3195))));
14408 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
14409 {
14410 continue;
14411 }
14412 }
14413 
14414 {
14415 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14416 vinfos[0].jointtype = 1;
14417 vinfos[0].foffset = j9;
14418 vinfos[0].indices[0] = _ij9[0];
14419 vinfos[0].indices[1] = _ij9[1];
14420 vinfos[0].maxsolutions = _nj9;
14421 vinfos[1].jointtype = 1;
14422 vinfos[1].foffset = j10;
14423 vinfos[1].indices[0] = _ij10[0];
14424 vinfos[1].indices[1] = _ij10[1];
14425 vinfos[1].maxsolutions = _nj10;
14426 vinfos[2].jointtype = 1;
14427 vinfos[2].foffset = j11;
14428 vinfos[2].indices[0] = _ij11[0];
14429 vinfos[2].indices[1] = _ij11[1];
14430 vinfos[2].maxsolutions = _nj11;
14431 vinfos[3].jointtype = 1;
14432 vinfos[3].foffset = j12;
14433 vinfos[3].indices[0] = _ij12[0];
14434 vinfos[3].indices[1] = _ij12[1];
14435 vinfos[3].maxsolutions = _nj12;
14436 vinfos[4].jointtype = 1;
14437 vinfos[4].foffset = j13;
14438 vinfos[4].indices[0] = _ij13[0];
14439 vinfos[4].indices[1] = _ij13[1];
14440 vinfos[4].maxsolutions = _nj13;
14441 vinfos[5].jointtype = 1;
14442 vinfos[5].foffset = j14;
14443 vinfos[5].indices[0] = _ij14[0];
14444 vinfos[5].indices[1] = _ij14[1];
14445 vinfos[5].maxsolutions = _nj14;
14446 std::vector<int> vfree(0);
14447 solutions.AddSolution(vinfos,vfree);
14448 }
14449 }
14450 }
14451 
14452 }
14453 
14454 }
14455 
14456 } else
14457 {
14458 {
14459 IkReal j10array[1], cj10array[1], sj10array[1];
14460 bool j10valid[1]={false};
14461 _nj10 = 1;
14462 IkReal x3203=((cj11)*(r20));
14463 IkReal x3204=((IkReal(1.00000000000000))*(cj13));
14464 IkReal x3205=((r21)*(sj14));
14465 IkReal x3206=((r22)*(sj13));
14466 IkReal x3207=((r20)*(sj11));
14467 IkReal x3208=((cj14)*(r21));
14468 if( IKabs(((gconst48)*(((((sj11)*(x3206)))+(((sj14)*(x3203)))+(((cj13)*(cj14)*(x3207)))+(((cj11)*(x3208)))+(((IkReal(-1.00000000000000))*(sj11)*(x3204)*(x3205))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst48)*(((((sj11)*(x3208)))+(((sj14)*(x3207)))+(((IkReal(-1.00000000000000))*(cj11)*(x3206)))+(((cj11)*(cj13)*(x3205)))+(((IkReal(-1.00000000000000))*(cj14)*(x3203)*(x3204))))))) < IKFAST_ATAN2_MAGTHRESH )
14469  continue;
14470 j10array[0]=IKatan2(((gconst48)*(((((sj11)*(x3206)))+(((sj14)*(x3203)))+(((cj13)*(cj14)*(x3207)))+(((cj11)*(x3208)))+(((IkReal(-1.00000000000000))*(sj11)*(x3204)*(x3205)))))), ((gconst48)*(((((sj11)*(x3208)))+(((sj14)*(x3207)))+(((IkReal(-1.00000000000000))*(cj11)*(x3206)))+(((cj11)*(cj13)*(x3205)))+(((IkReal(-1.00000000000000))*(cj14)*(x3203)*(x3204)))))));
14471 sj10array[0]=IKsin(j10array[0]);
14472 cj10array[0]=IKcos(j10array[0]);
14473 if( j10array[0] > IKPI )
14474 {
14475  j10array[0]-=IK2PI;
14476 }
14477 else if( j10array[0] < -IKPI )
14478 { j10array[0]+=IK2PI;
14479 }
14480 j10valid[0] = true;
14481 for(int ij10 = 0; ij10 < 1; ++ij10)
14482 {
14483 if( !j10valid[ij10] )
14484 {
14485  continue;
14486 }
14487 _ij10[0] = ij10; _ij10[1] = -1;
14488 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14489 {
14490 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14491 {
14492  j10valid[iij10]=false; _ij10[1] = iij10; break;
14493 }
14494 }
14495 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14496 {
14497 IkReal evalcond[4];
14498 IkReal x3209=IKsin(j10);
14499 IkReal x3210=IKcos(j10);
14500 IkReal x3211=((cj13)*(sj14));
14501 IkReal x3212=((cj13)*(cj14));
14502 IkReal x3213=((r10)*(sj9));
14503 IkReal x3214=((IkReal(1.00000000000000))*(cj9));
14504 IkReal x3215=((sj11)*(x3209));
14505 IkReal x3216=((IkReal(1.00000000000000))*(x3210));
14506 IkReal x3217=((cj11)*(x3209));
14507 IkReal x3218=((IkReal(1.00000000000000))*(r11)*(sj9));
14508 IkReal x3219=((cj11)*(x3216));
14509 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x3217)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x3216))));
14510 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x3212)))+(x3215)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x3219)))+(((r21)*(x3211))));
14511 evalcond[2]=((x3215)+(((IkReal(-1.00000000000000))*(cj14)*(x3218)))+(((IkReal(-1.00000000000000))*(x3219)))+(((IkReal(-1.00000000000000))*(sj14)*(x3213)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x3214)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x3214))));
14512 evalcond[3]=((((x3212)*(x3213)))+(((IkReal(-1.00000000000000))*(r01)*(x3211)*(x3214)))+(((IkReal(-1.00000000000000))*(x3211)*(x3218)))+(((sj11)*(x3210)))+(((cj9)*(r02)*(sj13)))+(x3217)+(((cj9)*(r00)*(x3212)))+(((r12)*(sj13)*(sj9))));
14513 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
14514 {
14515 continue;
14516 }
14517 }
14518 
14519 {
14520 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14521 vinfos[0].jointtype = 1;
14522 vinfos[0].foffset = j9;
14523 vinfos[0].indices[0] = _ij9[0];
14524 vinfos[0].indices[1] = _ij9[1];
14525 vinfos[0].maxsolutions = _nj9;
14526 vinfos[1].jointtype = 1;
14527 vinfos[1].foffset = j10;
14528 vinfos[1].indices[0] = _ij10[0];
14529 vinfos[1].indices[1] = _ij10[1];
14530 vinfos[1].maxsolutions = _nj10;
14531 vinfos[2].jointtype = 1;
14532 vinfos[2].foffset = j11;
14533 vinfos[2].indices[0] = _ij11[0];
14534 vinfos[2].indices[1] = _ij11[1];
14535 vinfos[2].maxsolutions = _nj11;
14536 vinfos[3].jointtype = 1;
14537 vinfos[3].foffset = j12;
14538 vinfos[3].indices[0] = _ij12[0];
14539 vinfos[3].indices[1] = _ij12[1];
14540 vinfos[3].maxsolutions = _nj12;
14541 vinfos[4].jointtype = 1;
14542 vinfos[4].foffset = j13;
14543 vinfos[4].indices[0] = _ij13[0];
14544 vinfos[4].indices[1] = _ij13[1];
14545 vinfos[4].maxsolutions = _nj13;
14546 vinfos[5].jointtype = 1;
14547 vinfos[5].foffset = j14;
14548 vinfos[5].indices[0] = _ij14[0];
14549 vinfos[5].indices[1] = _ij14[1];
14550 vinfos[5].maxsolutions = _nj14;
14551 std::vector<int> vfree(0);
14552 solutions.AddSolution(vinfos,vfree);
14553 }
14554 }
14555 }
14556 
14557 }
14558 
14559 }
14560 
14561 } else
14562 {
14563 IkReal x3220=((r00)*(sj9));
14564 IkReal x3221=((IkReal(1.00000000000000))*(sj13));
14565 IkReal x3222=((cj13)*(sj14));
14566 IkReal x3223=((cj9)*(sj14));
14567 IkReal x3224=((cj13)*(r02));
14568 IkReal x3225=((sj13)*(sj14));
14569 IkReal x3226=((r01)*(sj9));
14570 IkReal x3227=((cj9)*(sj13));
14571 IkReal x3228=((IkReal(1.00000000000000))*(cj9));
14572 IkReal x3229=((cj14)*(r10));
14573 IkReal x3230=((cj14)*(npx));
14574 IkReal x3231=((IkReal(1.00000000000000))*(cj13));
14575 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
14576 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((cj14)*(npy)))+(((npx)*(sj14))));
14577 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((npy)*(x3222)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(x3230)*(x3231)))+(((IkReal(-1.00000000000000))*(npz)*(x3221))));
14578 evalcond[3]=((((cj14)*(x3226)))+(((IkReal(-1.00000000000000))*(r10)*(x3223)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x3228)))+(((sj14)*(x3220))));
14579 evalcond[4]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x3221)))+(((r21)*(x3225)))+(((cj13)*(r22))));
14580 evalcond[5]=((IkReal(-0.0950000000000000))+(((npy)*(x3225)))+(((IkReal(-1.00000000000000))*(x3221)*(x3230)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
14581 evalcond[6]=((((r12)*(x3227)))+(((x3222)*(x3226)))+(((cj13)*(cj9)*(x3229)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x3221)))+(((IkReal(-1.00000000000000))*(cj14)*(x3220)*(x3231)))+(((IkReal(-1.00000000000000))*(r11)*(x3222)*(x3228))));
14582 evalcond[7]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(cj14)*(x3220)*(x3221)))+(((x3227)*(x3229)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x3228)))+(((x3225)*(x3226)))+(((IkReal(-1.00000000000000))*(r11)*(x3221)*(x3223)))+(((sj9)*(x3224))));
14583 evalcond[8]=((((sj13)*(sj9)*(x3229)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3231)))+(((cj14)*(r00)*(x3227)))+(((IkReal(-1.00000000000000))*(x3224)*(x3228)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x3221)))+(((IkReal(-1.00000000000000))*(r01)*(x3221)*(x3223))));
14584 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 )
14585 {
14586 {
14587 IkReal dummyeval[1];
14588 IkReal gconst50;
14589 gconst50=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
14590 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
14591 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
14592 {
14593 {
14594 IkReal dummyeval[1];
14595 IkReal gconst51;
14596 gconst51=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
14597 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
14598 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
14599 {
14600 continue;
14601 
14602 } else
14603 {
14604 {
14605 IkReal j10array[1], cj10array[1], sj10array[1];
14606 bool j10valid[1]={false};
14607 _nj10 = 1;
14608 IkReal x3232=((IkReal(1.00000000000000))*(sj11));
14609 IkReal x3233=((cj14)*(r21));
14610 IkReal x3234=((IkReal(1.00000000000000))*(cj11));
14611 IkReal x3235=((r20)*(sj14));
14612 IkReal x3236=((cj9)*(r00)*(sj14));
14613 IkReal x3237=((cj14)*(r11)*(sj9));
14614 IkReal x3238=((cj14)*(cj9)*(r01));
14615 IkReal x3239=((r10)*(sj14)*(sj9));
14616 if( IKabs(((gconst51)*(((((IkReal(-1.00000000000000))*(x3233)*(x3234)))+(((IkReal(-1.00000000000000))*(x3232)*(x3239)))+(((IkReal(-1.00000000000000))*(x3232)*(x3238)))+(((IkReal(-1.00000000000000))*(x3232)*(x3236)))+(((IkReal(-1.00000000000000))*(x3232)*(x3237)))+(((IkReal(-1.00000000000000))*(x3234)*(x3235))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst51)*(((((cj11)*(x3236)))+(((cj11)*(x3237)))+(((cj11)*(x3238)))+(((cj11)*(x3239)))+(((IkReal(-1.00000000000000))*(x3232)*(x3233)))+(((IkReal(-1.00000000000000))*(x3232)*(x3235))))))) < IKFAST_ATAN2_MAGTHRESH )
14617  continue;
14618 j10array[0]=IKatan2(((gconst51)*(((((IkReal(-1.00000000000000))*(x3233)*(x3234)))+(((IkReal(-1.00000000000000))*(x3232)*(x3239)))+(((IkReal(-1.00000000000000))*(x3232)*(x3238)))+(((IkReal(-1.00000000000000))*(x3232)*(x3236)))+(((IkReal(-1.00000000000000))*(x3232)*(x3237)))+(((IkReal(-1.00000000000000))*(x3234)*(x3235)))))), ((gconst51)*(((((cj11)*(x3236)))+(((cj11)*(x3237)))+(((cj11)*(x3238)))+(((cj11)*(x3239)))+(((IkReal(-1.00000000000000))*(x3232)*(x3233)))+(((IkReal(-1.00000000000000))*(x3232)*(x3235)))))));
14619 sj10array[0]=IKsin(j10array[0]);
14620 cj10array[0]=IKcos(j10array[0]);
14621 if( j10array[0] > IKPI )
14622 {
14623  j10array[0]-=IK2PI;
14624 }
14625 else if( j10array[0] < -IKPI )
14626 { j10array[0]+=IK2PI;
14627 }
14628 j10valid[0] = true;
14629 for(int ij10 = 0; ij10 < 1; ++ij10)
14630 {
14631 if( !j10valid[ij10] )
14632 {
14633  continue;
14634 }
14635 _ij10[0] = ij10; _ij10[1] = -1;
14636 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14637 {
14638 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14639 {
14640  j10valid[iij10]=false; _ij10[1] = iij10; break;
14641 }
14642 }
14643 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14644 {
14645 IkReal evalcond[4];
14646 IkReal x3240=IKsin(j10);
14647 IkReal x3241=IKcos(j10);
14648 IkReal x3242=((cj14)*(sj9));
14649 IkReal x3243=((IkReal(1.00000000000000))*(r11));
14650 IkReal x3244=((cj13)*(sj14));
14651 IkReal x3245=((IkReal(1.00000000000000))*(cj9));
14652 IkReal x3246=((cj13)*(cj14));
14653 IkReal x3247=((cj11)*(x3240));
14654 IkReal x3248=((sj11)*(x3241));
14655 IkReal x3249=((cj11)*(x3241));
14656 IkReal x3250=((sj11)*(x3240));
14657 IkReal x3251=((x3248)+(x3247));
14658 evalcond[0]=((((cj14)*(r21)))+(x3251)+(((r20)*(sj14))));
14659 evalcond[1]=((((r21)*(x3244)))+(((IkReal(-1.00000000000000))*(x3249)))+(((IkReal(-1.00000000000000))*(r20)*(x3246)))+(x3250)+(((IkReal(-1.00000000000000))*(r22)*(sj13))));
14660 evalcond[2]=((x3249)+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x3245)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x3245)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x3250)))+(((IkReal(-1.00000000000000))*(x3242)*(x3243))));
14661 evalcond[3]=((((cj9)*(r00)*(x3246)))+(((cj9)*(r02)*(sj13)))+(x3251)+(((IkReal(-1.00000000000000))*(sj9)*(x3243)*(x3244)))+(((cj13)*(r10)*(x3242)))+(((IkReal(-1.00000000000000))*(r01)*(x3244)*(x3245)))+(((r12)*(sj13)*(sj9))));
14662 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
14663 {
14664 continue;
14665 }
14666 }
14667 
14668 {
14669 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14670 vinfos[0].jointtype = 1;
14671 vinfos[0].foffset = j9;
14672 vinfos[0].indices[0] = _ij9[0];
14673 vinfos[0].indices[1] = _ij9[1];
14674 vinfos[0].maxsolutions = _nj9;
14675 vinfos[1].jointtype = 1;
14676 vinfos[1].foffset = j10;
14677 vinfos[1].indices[0] = _ij10[0];
14678 vinfos[1].indices[1] = _ij10[1];
14679 vinfos[1].maxsolutions = _nj10;
14680 vinfos[2].jointtype = 1;
14681 vinfos[2].foffset = j11;
14682 vinfos[2].indices[0] = _ij11[0];
14683 vinfos[2].indices[1] = _ij11[1];
14684 vinfos[2].maxsolutions = _nj11;
14685 vinfos[3].jointtype = 1;
14686 vinfos[3].foffset = j12;
14687 vinfos[3].indices[0] = _ij12[0];
14688 vinfos[3].indices[1] = _ij12[1];
14689 vinfos[3].maxsolutions = _nj12;
14690 vinfos[4].jointtype = 1;
14691 vinfos[4].foffset = j13;
14692 vinfos[4].indices[0] = _ij13[0];
14693 vinfos[4].indices[1] = _ij13[1];
14694 vinfos[4].maxsolutions = _nj13;
14695 vinfos[5].jointtype = 1;
14696 vinfos[5].foffset = j14;
14697 vinfos[5].indices[0] = _ij14[0];
14698 vinfos[5].indices[1] = _ij14[1];
14699 vinfos[5].maxsolutions = _nj14;
14700 std::vector<int> vfree(0);
14701 solutions.AddSolution(vinfos,vfree);
14702 }
14703 }
14704 }
14705 
14706 }
14707 
14708 }
14709 
14710 } else
14711 {
14712 {
14713 IkReal j10array[1], cj10array[1], sj10array[1];
14714 bool j10valid[1]={false};
14715 _nj10 = 1;
14716 IkReal x3252=((cj13)*(sj11));
14717 IkReal x3253=((r21)*(sj14));
14718 IkReal x3254=((cj14)*(r20));
14719 IkReal x3255=((cj11)*(cj13));
14720 IkReal x3256=((r22)*(sj13));
14721 IkReal x3257=((r20)*(sj14));
14722 IkReal x3258=((cj14)*(r21));
14723 if( IKabs(((gconst50)*(((((IkReal(-1.00000000000000))*(x3252)*(x3254)))+(((cj11)*(x3258)))+(((cj11)*(x3257)))+(((x3252)*(x3253)))+(((IkReal(-1.00000000000000))*(sj11)*(x3256))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst50)*(((((sj11)*(x3257)))+(((sj11)*(x3258)))+(((cj11)*(x3256)))+(((IkReal(-1.00000000000000))*(x3253)*(x3255)))+(((x3254)*(x3255))))))) < IKFAST_ATAN2_MAGTHRESH )
14724  continue;
14725 j10array[0]=IKatan2(((gconst50)*(((((IkReal(-1.00000000000000))*(x3252)*(x3254)))+(((cj11)*(x3258)))+(((cj11)*(x3257)))+(((x3252)*(x3253)))+(((IkReal(-1.00000000000000))*(sj11)*(x3256)))))), ((gconst50)*(((((sj11)*(x3257)))+(((sj11)*(x3258)))+(((cj11)*(x3256)))+(((IkReal(-1.00000000000000))*(x3253)*(x3255)))+(((x3254)*(x3255)))))));
14726 sj10array[0]=IKsin(j10array[0]);
14727 cj10array[0]=IKcos(j10array[0]);
14728 if( j10array[0] > IKPI )
14729 {
14730  j10array[0]-=IK2PI;
14731 }
14732 else if( j10array[0] < -IKPI )
14733 { j10array[0]+=IK2PI;
14734 }
14735 j10valid[0] = true;
14736 for(int ij10 = 0; ij10 < 1; ++ij10)
14737 {
14738 if( !j10valid[ij10] )
14739 {
14740  continue;
14741 }
14742 _ij10[0] = ij10; _ij10[1] = -1;
14743 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14744 {
14745 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14746 {
14747  j10valid[iij10]=false; _ij10[1] = iij10; break;
14748 }
14749 }
14750 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14751 {
14752 IkReal evalcond[4];
14753 IkReal x3259=IKsin(j10);
14754 IkReal x3260=IKcos(j10);
14755 IkReal x3261=((cj14)*(sj9));
14756 IkReal x3262=((IkReal(1.00000000000000))*(r11));
14757 IkReal x3263=((cj13)*(sj14));
14758 IkReal x3264=((IkReal(1.00000000000000))*(cj9));
14759 IkReal x3265=((cj13)*(cj14));
14760 IkReal x3266=((cj11)*(x3259));
14761 IkReal x3267=((sj11)*(x3260));
14762 IkReal x3268=((cj11)*(x3260));
14763 IkReal x3269=((sj11)*(x3259));
14764 IkReal x3270=((x3267)+(x3266));
14765 evalcond[0]=((((cj14)*(r21)))+(x3270)+(((r20)*(sj14))));
14766 evalcond[1]=((((r21)*(x3263)))+(((IkReal(-1.00000000000000))*(x3268)))+(x3269)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x3265))));
14767 evalcond[2]=((((IkReal(-1.00000000000000))*(x3261)*(x3262)))+(((IkReal(-1.00000000000000))*(x3269)))+(x3268)+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x3264)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x3264))));
14768 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x3263)*(x3264)))+(((cj9)*(r02)*(sj13)))+(x3270)+(((cj9)*(r00)*(x3265)))+(((cj13)*(r10)*(x3261)))+(((r12)*(sj13)*(sj9)))+(((IkReal(-1.00000000000000))*(sj9)*(x3262)*(x3263))));
14769 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
14770 {
14771 continue;
14772 }
14773 }
14774 
14775 {
14776 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14777 vinfos[0].jointtype = 1;
14778 vinfos[0].foffset = j9;
14779 vinfos[0].indices[0] = _ij9[0];
14780 vinfos[0].indices[1] = _ij9[1];
14781 vinfos[0].maxsolutions = _nj9;
14782 vinfos[1].jointtype = 1;
14783 vinfos[1].foffset = j10;
14784 vinfos[1].indices[0] = _ij10[0];
14785 vinfos[1].indices[1] = _ij10[1];
14786 vinfos[1].maxsolutions = _nj10;
14787 vinfos[2].jointtype = 1;
14788 vinfos[2].foffset = j11;
14789 vinfos[2].indices[0] = _ij11[0];
14790 vinfos[2].indices[1] = _ij11[1];
14791 vinfos[2].maxsolutions = _nj11;
14792 vinfos[3].jointtype = 1;
14793 vinfos[3].foffset = j12;
14794 vinfos[3].indices[0] = _ij12[0];
14795 vinfos[3].indices[1] = _ij12[1];
14796 vinfos[3].maxsolutions = _nj12;
14797 vinfos[4].jointtype = 1;
14798 vinfos[4].foffset = j13;
14799 vinfos[4].indices[0] = _ij13[0];
14800 vinfos[4].indices[1] = _ij13[1];
14801 vinfos[4].maxsolutions = _nj13;
14802 vinfos[5].jointtype = 1;
14803 vinfos[5].foffset = j14;
14804 vinfos[5].indices[0] = _ij14[0];
14805 vinfos[5].indices[1] = _ij14[1];
14806 vinfos[5].maxsolutions = _nj14;
14807 std::vector<int> vfree(0);
14808 solutions.AddSolution(vinfos,vfree);
14809 }
14810 }
14811 }
14812 
14813 }
14814 
14815 }
14816 
14817 } else
14818 {
14819 IkReal x3271=((cj9)*(r10));
14820 IkReal x3272=((cj13)*(cj14));
14821 IkReal x3273=((sj14)*(sj9));
14822 IkReal x3274=((IkReal(1.00000000000000))*(sj14));
14823 IkReal x3275=((cj9)*(sj13));
14824 IkReal x3276=((r02)*(sj9));
14825 IkReal x3277=((cj13)*(cj9));
14826 IkReal x3278=((cj14)*(r01));
14827 IkReal x3279=((IkReal(1.00000000000000))*(npx));
14828 IkReal x3280=((cj14)*(sj13));
14829 IkReal x3281=((IkReal(1.00000000000000))*(cj9));
14830 IkReal x3282=((npy)*(sj14));
14831 IkReal x3283=((IkReal(1.00000000000000))*(sj13));
14832 IkReal x3284=((IkReal(1.00000000000000))*(cj14)*(sj9));
14833 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
14834 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
14835 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
14836 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(npz)*(x3283)))+(((IkReal(-1.00000000000000))*(x3272)*(x3279)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((cj13)*(x3282))));
14837 evalcond[4]=((IkReal(1.00000000000000))+(((r00)*(x3273)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x3281)))+(((sj9)*(x3278)))+(((IkReal(-1.00000000000000))*(x3271)*(x3274))));
14838 evalcond[5]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((IkReal(-1.00000000000000))*(x3279)*(x3280)))+(((sj13)*(x3282)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
14839 evalcond[6]=((((IkReal(-1.00000000000000))*(cj9)*(r00)*(x3274)))+(((IkReal(-1.00000000000000))*(r10)*(x3273)))+(((IkReal(-1.00000000000000))*(x3278)*(x3281)))+(((IkReal(-1.00000000000000))*(r11)*(x3284))));
14840 evalcond[7]=((((IkReal(-1.00000000000000))*(r00)*(sj9)*(x3272)))+(((cj13)*(r01)*(x3273)))+(((IkReal(-1.00000000000000))*(x3276)*(x3283)))+(((IkReal(-1.00000000000000))*(r11)*(x3274)*(x3277)))+(((r12)*(x3275)))+(((x3271)*(x3272))));
14841 evalcond[8]=((((cj13)*(x3276)))+(((IkReal(-1.00000000000000))*(r12)*(x3277)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x3280)))+(((r01)*(sj13)*(x3273)))+(((IkReal(-1.00000000000000))*(r11)*(x3274)*(x3275)))+(((x3271)*(x3280))));
14842 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 )
14843 {
14844 {
14845 IkReal dummyeval[1];
14846 IkReal gconst52;
14847 gconst52=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
14848 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
14849 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
14850 {
14851 {
14852 IkReal dummyeval[1];
14853 IkReal gconst53;
14854 gconst53=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
14855 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
14856 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
14857 {
14858 continue;
14859 
14860 } else
14861 {
14862 {
14863 IkReal j10array[1], cj10array[1], sj10array[1];
14864 bool j10valid[1]={false};
14865 _nj10 = 1;
14866 IkReal x3285=((cj13)*(sj14));
14867 IkReal x3286=((IkReal(1.00000000000000))*(cj11));
14868 IkReal x3287=((r11)*(sj9));
14869 IkReal x3288=((IkReal(1.00000000000000))*(sj11));
14870 IkReal x3289=((cj13)*(cj14));
14871 IkReal x3290=((cj11)*(sj13));
14872 IkReal x3291=((r12)*(sj9));
14873 IkReal x3292=((sj11)*(sj13));
14874 IkReal x3293=((cj9)*(r02));
14875 IkReal x3294=((cj9)*(r01));
14876 IkReal x3295=((r10)*(sj9));
14877 IkReal x3296=((cj9)*(r00));
14878 if( IKabs(((gconst53)*(((((r21)*(sj11)*(x3285)))+(((IkReal(-1.00000000000000))*(x3285)*(x3286)*(x3287)))+(((IkReal(-1.00000000000000))*(x3285)*(x3286)*(x3294)))+(((x3290)*(x3293)))+(((x3290)*(x3291)))+(((cj11)*(x3289)*(x3295)))+(((cj11)*(x3289)*(x3296)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x3288)))+(((IkReal(-1.00000000000000))*(r20)*(x3288)*(x3289))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst53)*(((((sj11)*(x3289)*(x3295)))+(((sj11)*(x3289)*(x3296)))+(((x3292)*(x3293)))+(((x3291)*(x3292)))+(((r22)*(x3290)))+(((IkReal(-1.00000000000000))*(x3285)*(x3288)*(x3294)))+(((IkReal(-1.00000000000000))*(r21)*(x3285)*(x3286)))+(((IkReal(-1.00000000000000))*(x3285)*(x3287)*(x3288)))+(((cj11)*(r20)*(x3289))))))) < IKFAST_ATAN2_MAGTHRESH )
14879  continue;
14880 j10array[0]=IKatan2(((gconst53)*(((((r21)*(sj11)*(x3285)))+(((IkReal(-1.00000000000000))*(x3285)*(x3286)*(x3287)))+(((IkReal(-1.00000000000000))*(x3285)*(x3286)*(x3294)))+(((x3290)*(x3293)))+(((x3290)*(x3291)))+(((cj11)*(x3289)*(x3295)))+(((cj11)*(x3289)*(x3296)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x3288)))+(((IkReal(-1.00000000000000))*(r20)*(x3288)*(x3289)))))), ((gconst53)*(((((sj11)*(x3289)*(x3295)))+(((sj11)*(x3289)*(x3296)))+(((x3292)*(x3293)))+(((x3291)*(x3292)))+(((r22)*(x3290)))+(((IkReal(-1.00000000000000))*(x3285)*(x3288)*(x3294)))+(((IkReal(-1.00000000000000))*(r21)*(x3285)*(x3286)))+(((IkReal(-1.00000000000000))*(x3285)*(x3287)*(x3288)))+(((cj11)*(r20)*(x3289)))))));
14881 sj10array[0]=IKsin(j10array[0]);
14882 cj10array[0]=IKcos(j10array[0]);
14883 if( j10array[0] > IKPI )
14884 {
14885  j10array[0]-=IK2PI;
14886 }
14887 else if( j10array[0] < -IKPI )
14888 { j10array[0]+=IK2PI;
14889 }
14890 j10valid[0] = true;
14891 for(int ij10 = 0; ij10 < 1; ++ij10)
14892 {
14893 if( !j10valid[ij10] )
14894 {
14895  continue;
14896 }
14897 _ij10[0] = ij10; _ij10[1] = -1;
14898 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
14899 {
14900 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
14901 {
14902  j10valid[iij10]=false; _ij10[1] = iij10; break;
14903 }
14904 }
14905 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
14906 {
14907 IkReal evalcond[4];
14908 IkReal x3297=IKsin(j10);
14909 IkReal x3298=IKcos(j10);
14910 IkReal x3299=((IkReal(1.00000000000000))*(sj13));
14911 IkReal x3300=((r11)*(sj9));
14912 IkReal x3301=((cj9)*(r01));
14913 IkReal x3302=((r21)*(sj14));
14914 IkReal x3303=((cj9)*(r02));
14915 IkReal x3304=((sj13)*(sj9));
14916 IkReal x3305=((cj14)*(r10));
14917 IkReal x3306=((IkReal(1.00000000000000))*(cj13));
14918 IkReal x3307=((cj14)*(r20));
14919 IkReal x3308=((cj11)*(x3297));
14920 IkReal x3309=((sj11)*(x3298));
14921 IkReal x3310=((sj14)*(x3306));
14922 IkReal x3311=((sj11)*(x3297));
14923 IkReal x3312=((cj11)*(x3298));
14924 IkReal x3313=((cj14)*(cj9)*(r00));
14925 IkReal x3314=((x3308)+(x3309));
14926 evalcond[0]=((x3311)+(((IkReal(-1.00000000000000))*(r22)*(x3299)))+(((IkReal(-1.00000000000000))*(x3306)*(x3307)))+(((cj13)*(x3302)))+(((IkReal(-1.00000000000000))*(x3312))));
14927 evalcond[1]=((x3314)+(((IkReal(-1.00000000000000))*(x3299)*(x3307)))+(((cj13)*(r22)))+(((sj13)*(x3302))));
14928 evalcond[2]=((((cj13)*(sj9)*(x3305)))+(x3314)+(((cj13)*(x3313)))+(((r12)*(x3304)))+(((IkReal(-1.00000000000000))*(x3301)*(x3310)))+(((IkReal(-1.00000000000000))*(x3300)*(x3310)))+(((sj13)*(x3303))));
14929 evalcond[3]=((x3312)+(((x3304)*(x3305)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3306)))+(((IkReal(-1.00000000000000))*(x3311)))+(((IkReal(-1.00000000000000))*(sj14)*(x3299)*(x3300)))+(((IkReal(-1.00000000000000))*(sj14)*(x3299)*(x3301)))+(((IkReal(-1.00000000000000))*(x3303)*(x3306)))+(((sj13)*(x3313))));
14930 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
14931 {
14932 continue;
14933 }
14934 }
14935 
14936 {
14937 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
14938 vinfos[0].jointtype = 1;
14939 vinfos[0].foffset = j9;
14940 vinfos[0].indices[0] = _ij9[0];
14941 vinfos[0].indices[1] = _ij9[1];
14942 vinfos[0].maxsolutions = _nj9;
14943 vinfos[1].jointtype = 1;
14944 vinfos[1].foffset = j10;
14945 vinfos[1].indices[0] = _ij10[0];
14946 vinfos[1].indices[1] = _ij10[1];
14947 vinfos[1].maxsolutions = _nj10;
14948 vinfos[2].jointtype = 1;
14949 vinfos[2].foffset = j11;
14950 vinfos[2].indices[0] = _ij11[0];
14951 vinfos[2].indices[1] = _ij11[1];
14952 vinfos[2].maxsolutions = _nj11;
14953 vinfos[3].jointtype = 1;
14954 vinfos[3].foffset = j12;
14955 vinfos[3].indices[0] = _ij12[0];
14956 vinfos[3].indices[1] = _ij12[1];
14957 vinfos[3].maxsolutions = _nj12;
14958 vinfos[4].jointtype = 1;
14959 vinfos[4].foffset = j13;
14960 vinfos[4].indices[0] = _ij13[0];
14961 vinfos[4].indices[1] = _ij13[1];
14962 vinfos[4].maxsolutions = _nj13;
14963 vinfos[5].jointtype = 1;
14964 vinfos[5].foffset = j14;
14965 vinfos[5].indices[0] = _ij14[0];
14966 vinfos[5].indices[1] = _ij14[1];
14967 vinfos[5].maxsolutions = _nj14;
14968 std::vector<int> vfree(0);
14969 solutions.AddSolution(vinfos,vfree);
14970 }
14971 }
14972 }
14973 
14974 }
14975 
14976 }
14977 
14978 } else
14979 {
14980 {
14981 IkReal j10array[1], cj10array[1], sj10array[1];
14982 bool j10valid[1]={false};
14983 _nj10 = 1;
14984 IkReal x3315=((cj13)*(sj11));
14985 IkReal x3316=((r21)*(sj14));
14986 IkReal x3317=((cj11)*(cj13));
14987 IkReal x3318=((cj11)*(sj13));
14988 IkReal x3319=((sj11)*(sj13));
14989 IkReal x3320=((IkReal(1.00000000000000))*(cj14)*(r20));
14990 if( IKabs(((gconst52)*(((((x3316)*(x3318)))+(((IkReal(-1.00000000000000))*(r22)*(x3319)))+(((x3315)*(x3316)))+(((r22)*(x3317)))+(((IkReal(-1.00000000000000))*(x3318)*(x3320)))+(((IkReal(-1.00000000000000))*(x3315)*(x3320))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst52)*(((((x3316)*(x3319)))+(((IkReal(-1.00000000000000))*(x3316)*(x3317)))+(((IkReal(-1.00000000000000))*(x3319)*(x3320)))+(((r22)*(x3315)))+(((r22)*(x3318)))+(((cj14)*(r20)*(x3317))))))) < IKFAST_ATAN2_MAGTHRESH )
14991  continue;
14992 j10array[0]=IKatan2(((gconst52)*(((((x3316)*(x3318)))+(((IkReal(-1.00000000000000))*(r22)*(x3319)))+(((x3315)*(x3316)))+(((r22)*(x3317)))+(((IkReal(-1.00000000000000))*(x3318)*(x3320)))+(((IkReal(-1.00000000000000))*(x3315)*(x3320)))))), ((gconst52)*(((((x3316)*(x3319)))+(((IkReal(-1.00000000000000))*(x3316)*(x3317)))+(((IkReal(-1.00000000000000))*(x3319)*(x3320)))+(((r22)*(x3315)))+(((r22)*(x3318)))+(((cj14)*(r20)*(x3317)))))));
14993 sj10array[0]=IKsin(j10array[0]);
14994 cj10array[0]=IKcos(j10array[0]);
14995 if( j10array[0] > IKPI )
14996 {
14997  j10array[0]-=IK2PI;
14998 }
14999 else if( j10array[0] < -IKPI )
15000 { j10array[0]+=IK2PI;
15001 }
15002 j10valid[0] = true;
15003 for(int ij10 = 0; ij10 < 1; ++ij10)
15004 {
15005 if( !j10valid[ij10] )
15006 {
15007  continue;
15008 }
15009 _ij10[0] = ij10; _ij10[1] = -1;
15010 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15011 {
15012 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15013 {
15014  j10valid[iij10]=false; _ij10[1] = iij10; break;
15015 }
15016 }
15017 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15018 {
15019 IkReal evalcond[4];
15020 IkReal x3321=IKsin(j10);
15021 IkReal x3322=IKcos(j10);
15022 IkReal x3323=((IkReal(1.00000000000000))*(sj13));
15023 IkReal x3324=((r11)*(sj9));
15024 IkReal x3325=((cj9)*(r01));
15025 IkReal x3326=((r21)*(sj14));
15026 IkReal x3327=((cj9)*(r02));
15027 IkReal x3328=((sj13)*(sj9));
15028 IkReal x3329=((cj14)*(r10));
15029 IkReal x3330=((IkReal(1.00000000000000))*(cj13));
15030 IkReal x3331=((cj14)*(r20));
15031 IkReal x3332=((cj11)*(x3321));
15032 IkReal x3333=((sj11)*(x3322));
15033 IkReal x3334=((sj14)*(x3330));
15034 IkReal x3335=((sj11)*(x3321));
15035 IkReal x3336=((cj11)*(x3322));
15036 IkReal x3337=((cj14)*(cj9)*(r00));
15037 IkReal x3338=((x3333)+(x3332));
15038 evalcond[0]=((((IkReal(-1.00000000000000))*(x3330)*(x3331)))+(x3335)+(((IkReal(-1.00000000000000))*(x3336)))+(((IkReal(-1.00000000000000))*(r22)*(x3323)))+(((cj13)*(x3326))));
15039 evalcond[1]=((x3338)+(((IkReal(-1.00000000000000))*(x3323)*(x3331)))+(((sj13)*(x3326)))+(((cj13)*(r22))));
15040 evalcond[2]=((((r12)*(x3328)))+(x3338)+(((cj13)*(x3337)))+(((sj13)*(x3327)))+(((IkReal(-1.00000000000000))*(x3324)*(x3334)))+(((cj13)*(sj9)*(x3329)))+(((IkReal(-1.00000000000000))*(x3325)*(x3334))));
15041 evalcond[3]=((((x3328)*(x3329)))+(((IkReal(-1.00000000000000))*(sj14)*(x3323)*(x3324)))+(((IkReal(-1.00000000000000))*(sj14)*(x3323)*(x3325)))+(((sj13)*(x3337)))+(x3336)+(((IkReal(-1.00000000000000))*(x3335)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3330)))+(((IkReal(-1.00000000000000))*(x3327)*(x3330))));
15042 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
15043 {
15044 continue;
15045 }
15046 }
15047 
15048 {
15049 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15050 vinfos[0].jointtype = 1;
15051 vinfos[0].foffset = j9;
15052 vinfos[0].indices[0] = _ij9[0];
15053 vinfos[0].indices[1] = _ij9[1];
15054 vinfos[0].maxsolutions = _nj9;
15055 vinfos[1].jointtype = 1;
15056 vinfos[1].foffset = j10;
15057 vinfos[1].indices[0] = _ij10[0];
15058 vinfos[1].indices[1] = _ij10[1];
15059 vinfos[1].maxsolutions = _nj10;
15060 vinfos[2].jointtype = 1;
15061 vinfos[2].foffset = j11;
15062 vinfos[2].indices[0] = _ij11[0];
15063 vinfos[2].indices[1] = _ij11[1];
15064 vinfos[2].maxsolutions = _nj11;
15065 vinfos[3].jointtype = 1;
15066 vinfos[3].foffset = j12;
15067 vinfos[3].indices[0] = _ij12[0];
15068 vinfos[3].indices[1] = _ij12[1];
15069 vinfos[3].maxsolutions = _nj12;
15070 vinfos[4].jointtype = 1;
15071 vinfos[4].foffset = j13;
15072 vinfos[4].indices[0] = _ij13[0];
15073 vinfos[4].indices[1] = _ij13[1];
15074 vinfos[4].maxsolutions = _nj13;
15075 vinfos[5].jointtype = 1;
15076 vinfos[5].foffset = j14;
15077 vinfos[5].indices[0] = _ij14[0];
15078 vinfos[5].indices[1] = _ij14[1];
15079 vinfos[5].maxsolutions = _nj14;
15080 std::vector<int> vfree(0);
15081 solutions.AddSolution(vinfos,vfree);
15082 }
15083 }
15084 }
15085 
15086 }
15087 
15088 }
15089 
15090 } else
15091 {
15092 IkReal x3339=((cj9)*(r10));
15093 IkReal x3340=((cj13)*(cj14));
15094 IkReal x3341=((sj14)*(sj9));
15095 IkReal x3342=((IkReal(1.00000000000000))*(sj14));
15096 IkReal x3343=((cj9)*(sj13));
15097 IkReal x3344=((r02)*(sj9));
15098 IkReal x3345=((cj13)*(cj9));
15099 IkReal x3346=((cj14)*(r01));
15100 IkReal x3347=((IkReal(1.00000000000000))*(npx));
15101 IkReal x3348=((cj14)*(sj13));
15102 IkReal x3349=((IkReal(1.00000000000000))*(cj9));
15103 IkReal x3350=((npy)*(sj14));
15104 IkReal x3351=((IkReal(1.00000000000000))*(sj13));
15105 IkReal x3352=((IkReal(1.00000000000000))*(cj14)*(sj9));
15106 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
15107 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
15108 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
15109 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(-1.00000000000000))*(x3340)*(x3347)))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(npz)*(x3351)))+(((cj13)*(x3350))));
15110 evalcond[4]=((IkReal(-1.00000000000000))+(((r00)*(x3341)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x3349)))+(((sj9)*(x3346)))+(((IkReal(-1.00000000000000))*(x3339)*(x3342))));
15111 evalcond[5]=((IkReal(-0.0300000000000000))+(((IkReal(-1.00000000000000))*(x3347)*(x3348)))+(((sj13)*(x3350)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(0.250000000000000))*(sj11))));
15112 evalcond[6]=((((IkReal(-1.00000000000000))*(x3346)*(x3349)))+(((IkReal(-1.00000000000000))*(cj9)*(r00)*(x3342)))+(((IkReal(-1.00000000000000))*(r11)*(x3352)))+(((IkReal(-1.00000000000000))*(r10)*(x3341))));
15113 evalcond[7]=((((cj13)*(r01)*(x3341)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x3340)))+(((IkReal(-1.00000000000000))*(x3344)*(x3351)))+(((IkReal(-1.00000000000000))*(r11)*(x3342)*(x3345)))+(((x3339)*(x3340)))+(((r12)*(x3343))));
15114 evalcond[8]=((((IkReal(-1.00000000000000))*(r00)*(sj9)*(x3348)))+(((IkReal(-1.00000000000000))*(r12)*(x3345)))+(((cj13)*(x3344)))+(((r01)*(sj13)*(x3341)))+(((IkReal(-1.00000000000000))*(r11)*(x3342)*(x3343)))+(((x3339)*(x3348))));
15115 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 )
15116 {
15117 {
15118 IkReal dummyeval[1];
15119 IkReal gconst54;
15120 gconst54=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
15121 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
15122 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
15123 {
15124 {
15125 IkReal dummyeval[1];
15126 IkReal gconst55;
15127 gconst55=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
15128 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
15129 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
15130 {
15131 continue;
15132 
15133 } else
15134 {
15135 {
15136 IkReal j10array[1], cj10array[1], sj10array[1];
15137 bool j10valid[1]={false};
15138 _nj10 = 1;
15139 IkReal x3353=((cj13)*(sj14));
15140 IkReal x3354=((IkReal(1.00000000000000))*(cj11));
15141 IkReal x3355=((r11)*(sj9));
15142 IkReal x3356=((cj11)*(sj13));
15143 IkReal x3357=((r12)*(sj9));
15144 IkReal x3358=((r10)*(sj9));
15145 IkReal x3359=((sj11)*(sj13));
15146 IkReal x3360=((cj9)*(r02));
15147 IkReal x3361=((IkReal(1.00000000000000))*(sj11));
15148 IkReal x3362=((cj9)*(r01));
15149 IkReal x3363=((cj9)*(r00));
15150 IkReal x3364=((cj13)*(cj14)*(sj11));
15151 IkReal x3365=((cj11)*(cj13)*(cj14));
15152 if( IKabs(((gconst55)*(((((x3356)*(x3360)))+(((IkReal(-1.00000000000000))*(x3353)*(x3354)*(x3362)))+(((x3363)*(x3365)))+(((IkReal(-1.00000000000000))*(r22)*(x3359)))+(((x3358)*(x3365)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x3361)))+(((x3356)*(x3357)))+(((IkReal(-1.00000000000000))*(x3353)*(x3354)*(x3355)))+(((r21)*(sj11)*(x3353))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst55)*(((((x3363)*(x3364)))+(((IkReal(-1.00000000000000))*(x3353)*(x3355)*(x3361)))+(((IkReal(-1.00000000000000))*(x3353)*(x3361)*(x3362)))+(((x3358)*(x3364)))+(((r20)*(x3365)))+(((x3359)*(x3360)))+(((IkReal(-1.00000000000000))*(r21)*(x3353)*(x3354)))+(((x3357)*(x3359)))+(((r22)*(x3356))))))) < IKFAST_ATAN2_MAGTHRESH )
15153  continue;
15154 j10array[0]=IKatan2(((gconst55)*(((((x3356)*(x3360)))+(((IkReal(-1.00000000000000))*(x3353)*(x3354)*(x3362)))+(((x3363)*(x3365)))+(((IkReal(-1.00000000000000))*(r22)*(x3359)))+(((x3358)*(x3365)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x3361)))+(((x3356)*(x3357)))+(((IkReal(-1.00000000000000))*(x3353)*(x3354)*(x3355)))+(((r21)*(sj11)*(x3353)))))), ((gconst55)*(((((x3363)*(x3364)))+(((IkReal(-1.00000000000000))*(x3353)*(x3355)*(x3361)))+(((IkReal(-1.00000000000000))*(x3353)*(x3361)*(x3362)))+(((x3358)*(x3364)))+(((r20)*(x3365)))+(((x3359)*(x3360)))+(((IkReal(-1.00000000000000))*(r21)*(x3353)*(x3354)))+(((x3357)*(x3359)))+(((r22)*(x3356)))))));
15155 sj10array[0]=IKsin(j10array[0]);
15156 cj10array[0]=IKcos(j10array[0]);
15157 if( j10array[0] > IKPI )
15158 {
15159  j10array[0]-=IK2PI;
15160 }
15161 else if( j10array[0] < -IKPI )
15162 { j10array[0]+=IK2PI;
15163 }
15164 j10valid[0] = true;
15165 for(int ij10 = 0; ij10 < 1; ++ij10)
15166 {
15167 if( !j10valid[ij10] )
15168 {
15169  continue;
15170 }
15171 _ij10[0] = ij10; _ij10[1] = -1;
15172 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15173 {
15174 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15175 {
15176  j10valid[iij10]=false; _ij10[1] = iij10; break;
15177 }
15178 }
15179 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15180 {
15181 IkReal evalcond[4];
15182 IkReal x3366=IKsin(j10);
15183 IkReal x3367=IKcos(j10);
15184 IkReal x3368=((IkReal(1.00000000000000))*(sj13));
15185 IkReal x3369=((r11)*(sj9));
15186 IkReal x3370=((cj9)*(r01));
15187 IkReal x3371=((IkReal(1.00000000000000))*(cj11));
15188 IkReal x3372=((r21)*(sj14));
15189 IkReal x3373=((cj9)*(r02));
15190 IkReal x3374=((sj13)*(sj9));
15191 IkReal x3375=((cj14)*(r10));
15192 IkReal x3376=((IkReal(1.00000000000000))*(cj13));
15193 IkReal x3377=((cj14)*(r20));
15194 IkReal x3378=((sj11)*(x3366));
15195 IkReal x3379=((sj14)*(x3376));
15196 IkReal x3380=((sj11)*(x3367));
15197 IkReal x3381=((cj14)*(cj9)*(r00));
15198 IkReal x3382=((x3367)*(x3371));
15199 evalcond[0]=((x3378)+(((IkReal(-1.00000000000000))*(r22)*(x3368)))+(((cj13)*(x3372)))+(((IkReal(-1.00000000000000))*(x3376)*(x3377)))+(((IkReal(-1.00000000000000))*(x3382))));
15200 evalcond[1]=((((IkReal(-1.00000000000000))*(x3366)*(x3371)))+(((IkReal(-1.00000000000000))*(x3380)))+(((sj13)*(x3372)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x3368)*(x3377))));
15201 evalcond[2]=((((r12)*(x3374)))+(((cj13)*(sj9)*(x3375)))+(x3380)+(((IkReal(-1.00000000000000))*(x3370)*(x3379)))+(((cj13)*(x3381)))+(((sj13)*(x3373)))+(((IkReal(-1.00000000000000))*(x3369)*(x3379)))+(((cj11)*(x3366))));
15202 evalcond[3]=((((IkReal(-1.00000000000000))*(sj14)*(x3368)*(x3369)))+(((IkReal(-1.00000000000000))*(x3373)*(x3376)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3376)))+(x3378)+(((IkReal(-1.00000000000000))*(sj14)*(x3368)*(x3370)))+(((x3374)*(x3375)))+(((IkReal(-1.00000000000000))*(x3382)))+(((sj13)*(x3381))));
15203 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
15204 {
15205 continue;
15206 }
15207 }
15208 
15209 {
15210 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15211 vinfos[0].jointtype = 1;
15212 vinfos[0].foffset = j9;
15213 vinfos[0].indices[0] = _ij9[0];
15214 vinfos[0].indices[1] = _ij9[1];
15215 vinfos[0].maxsolutions = _nj9;
15216 vinfos[1].jointtype = 1;
15217 vinfos[1].foffset = j10;
15218 vinfos[1].indices[0] = _ij10[0];
15219 vinfos[1].indices[1] = _ij10[1];
15220 vinfos[1].maxsolutions = _nj10;
15221 vinfos[2].jointtype = 1;
15222 vinfos[2].foffset = j11;
15223 vinfos[2].indices[0] = _ij11[0];
15224 vinfos[2].indices[1] = _ij11[1];
15225 vinfos[2].maxsolutions = _nj11;
15226 vinfos[3].jointtype = 1;
15227 vinfos[3].foffset = j12;
15228 vinfos[3].indices[0] = _ij12[0];
15229 vinfos[3].indices[1] = _ij12[1];
15230 vinfos[3].maxsolutions = _nj12;
15231 vinfos[4].jointtype = 1;
15232 vinfos[4].foffset = j13;
15233 vinfos[4].indices[0] = _ij13[0];
15234 vinfos[4].indices[1] = _ij13[1];
15235 vinfos[4].maxsolutions = _nj13;
15236 vinfos[5].jointtype = 1;
15237 vinfos[5].foffset = j14;
15238 vinfos[5].indices[0] = _ij14[0];
15239 vinfos[5].indices[1] = _ij14[1];
15240 vinfos[5].maxsolutions = _nj14;
15241 std::vector<int> vfree(0);
15242 solutions.AddSolution(vinfos,vfree);
15243 }
15244 }
15245 }
15246 
15247 }
15248 
15249 }
15250 
15251 } else
15252 {
15253 {
15254 IkReal j10array[1], cj10array[1], sj10array[1];
15255 bool j10valid[1]={false};
15256 _nj10 = 1;
15257 IkReal x3383=((r22)*(sj11));
15258 IkReal x3384=((IkReal(1.00000000000000))*(sj11));
15259 IkReal x3385=((IkReal(1.00000000000000))*(cj11));
15260 IkReal x3386=((cj14)*(r20));
15261 IkReal x3387=((cj13)*(r21)*(sj14));
15262 IkReal x3388=((r21)*(sj13)*(sj14));
15263 if( IKabs(((gconst54)*(((((cj13)*(sj11)*(x3386)))+(((IkReal(-1.00000000000000))*(x3384)*(x3387)))+(((IkReal(-1.00000000000000))*(sj13)*(x3385)*(x3386)))+(((cj11)*(cj13)*(r22)))+(((sj13)*(x3383)))+(((cj11)*(x3388))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst54)*(((((IkReal(-1.00000000000000))*(sj13)*(x3384)*(x3386)))+(((sj11)*(x3388)))+(((IkReal(-1.00000000000000))*(cj13)*(x3385)*(x3386)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x3385)))+(((cj13)*(x3383)))+(((cj11)*(x3387))))))) < IKFAST_ATAN2_MAGTHRESH )
15264  continue;
15265 j10array[0]=IKatan2(((gconst54)*(((((cj13)*(sj11)*(x3386)))+(((IkReal(-1.00000000000000))*(x3384)*(x3387)))+(((IkReal(-1.00000000000000))*(sj13)*(x3385)*(x3386)))+(((cj11)*(cj13)*(r22)))+(((sj13)*(x3383)))+(((cj11)*(x3388)))))), ((gconst54)*(((((IkReal(-1.00000000000000))*(sj13)*(x3384)*(x3386)))+(((sj11)*(x3388)))+(((IkReal(-1.00000000000000))*(cj13)*(x3385)*(x3386)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x3385)))+(((cj13)*(x3383)))+(((cj11)*(x3387)))))));
15266 sj10array[0]=IKsin(j10array[0]);
15267 cj10array[0]=IKcos(j10array[0]);
15268 if( j10array[0] > IKPI )
15269 {
15270  j10array[0]-=IK2PI;
15271 }
15272 else if( j10array[0] < -IKPI )
15273 { j10array[0]+=IK2PI;
15274 }
15275 j10valid[0] = true;
15276 for(int ij10 = 0; ij10 < 1; ++ij10)
15277 {
15278 if( !j10valid[ij10] )
15279 {
15280  continue;
15281 }
15282 _ij10[0] = ij10; _ij10[1] = -1;
15283 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15284 {
15285 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15286 {
15287  j10valid[iij10]=false; _ij10[1] = iij10; break;
15288 }
15289 }
15290 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15291 {
15292 IkReal evalcond[4];
15293 IkReal x3389=IKsin(j10);
15294 IkReal x3390=IKcos(j10);
15295 IkReal x3391=((IkReal(1.00000000000000))*(sj13));
15296 IkReal x3392=((r11)*(sj9));
15297 IkReal x3393=((cj9)*(r01));
15298 IkReal x3394=((IkReal(1.00000000000000))*(cj11));
15299 IkReal x3395=((r21)*(sj14));
15300 IkReal x3396=((cj9)*(r02));
15301 IkReal x3397=((sj13)*(sj9));
15302 IkReal x3398=((cj14)*(r10));
15303 IkReal x3399=((IkReal(1.00000000000000))*(cj13));
15304 IkReal x3400=((cj14)*(r20));
15305 IkReal x3401=((sj11)*(x3389));
15306 IkReal x3402=((sj14)*(x3399));
15307 IkReal x3403=((sj11)*(x3390));
15308 IkReal x3404=((cj14)*(cj9)*(r00));
15309 IkReal x3405=((x3390)*(x3394));
15310 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x3391)))+(((IkReal(-1.00000000000000))*(x3399)*(x3400)))+(x3401)+(((cj13)*(x3395)))+(((IkReal(-1.00000000000000))*(x3405))));
15311 evalcond[1]=((((IkReal(-1.00000000000000))*(x3403)))+(((IkReal(-1.00000000000000))*(x3389)*(x3394)))+(((sj13)*(x3395)))+(((IkReal(-1.00000000000000))*(x3391)*(x3400)))+(((cj13)*(r22))));
15312 evalcond[2]=((((IkReal(-1.00000000000000))*(x3392)*(x3402)))+(x3403)+(((cj13)*(sj9)*(x3398)))+(((cj13)*(x3404)))+(((sj13)*(x3396)))+(((r12)*(x3397)))+(((IkReal(-1.00000000000000))*(x3393)*(x3402)))+(((cj11)*(x3389))));
15313 evalcond[3]=((((IkReal(-1.00000000000000))*(x3396)*(x3399)))+(x3401)+(((IkReal(-1.00000000000000))*(sj14)*(x3391)*(x3392)))+(((IkReal(-1.00000000000000))*(sj14)*(x3391)*(x3393)))+(((sj13)*(x3404)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3399)))+(((x3397)*(x3398)))+(((IkReal(-1.00000000000000))*(x3405))));
15314 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
15315 {
15316 continue;
15317 }
15318 }
15319 
15320 {
15321 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15322 vinfos[0].jointtype = 1;
15323 vinfos[0].foffset = j9;
15324 vinfos[0].indices[0] = _ij9[0];
15325 vinfos[0].indices[1] = _ij9[1];
15326 vinfos[0].maxsolutions = _nj9;
15327 vinfos[1].jointtype = 1;
15328 vinfos[1].foffset = j10;
15329 vinfos[1].indices[0] = _ij10[0];
15330 vinfos[1].indices[1] = _ij10[1];
15331 vinfos[1].maxsolutions = _nj10;
15332 vinfos[2].jointtype = 1;
15333 vinfos[2].foffset = j11;
15334 vinfos[2].indices[0] = _ij11[0];
15335 vinfos[2].indices[1] = _ij11[1];
15336 vinfos[2].maxsolutions = _nj11;
15337 vinfos[3].jointtype = 1;
15338 vinfos[3].foffset = j12;
15339 vinfos[3].indices[0] = _ij12[0];
15340 vinfos[3].indices[1] = _ij12[1];
15341 vinfos[3].maxsolutions = _nj12;
15342 vinfos[4].jointtype = 1;
15343 vinfos[4].foffset = j13;
15344 vinfos[4].indices[0] = _ij13[0];
15345 vinfos[4].indices[1] = _ij13[1];
15346 vinfos[4].maxsolutions = _nj13;
15347 vinfos[5].jointtype = 1;
15348 vinfos[5].foffset = j14;
15349 vinfos[5].indices[0] = _ij14[0];
15350 vinfos[5].indices[1] = _ij14[1];
15351 vinfos[5].maxsolutions = _nj14;
15352 std::vector<int> vfree(0);
15353 solutions.AddSolution(vinfos,vfree);
15354 }
15355 }
15356 }
15357 
15358 }
15359 
15360 }
15361 
15362 } else
15363 {
15364 if( 1 )
15365 {
15366 continue;
15367 
15368 } else
15369 {
15370 }
15371 }
15372 }
15373 }
15374 }
15375 }
15376 
15377 } else
15378 {
15379 {
15380 IkReal j10array[1], cj10array[1], sj10array[1];
15381 bool j10valid[1]={false};
15382 _nj10 = 1;
15383 IkReal x3406=((r21)*(sj14));
15384 IkReal x3407=((cj12)*(cj13));
15385 IkReal x3408=((sj11)*(sj13));
15386 IkReal x3409=((cj14)*(r20));
15387 IkReal x3410=((IkReal(1.00000000000000))*(sj11));
15388 IkReal x3411=((cj12)*(r22));
15389 IkReal x3412=((IkReal(1.00000000000000))*(cj11));
15390 IkReal x3413=((cj13)*(r22));
15391 IkReal x3414=((sj13)*(x3412));
15392 if( IKabs(((gconst47)*(((((IkReal(-1.00000000000000))*(x3412)*(x3413)))+(((x3408)*(x3411)))+(((IkReal(-1.00000000000000))*(x3406)*(x3407)*(x3410)))+(((sj11)*(x3407)*(x3409)))+(((cj11)*(sj13)*(x3409)))+(((IkReal(-1.00000000000000))*(x3406)*(x3414))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst47)*(((((x3408)*(x3409)))+(((IkReal(-1.00000000000000))*(x3407)*(x3409)*(x3412)))+(((IkReal(-1.00000000000000))*(x3406)*(x3408)))+(((IkReal(-1.00000000000000))*(x3411)*(x3414)))+(((cj11)*(x3406)*(x3407)))+(((IkReal(-1.00000000000000))*(x3410)*(x3413))))))) < IKFAST_ATAN2_MAGTHRESH )
15393  continue;
15394 j10array[0]=IKatan2(((gconst47)*(((((IkReal(-1.00000000000000))*(x3412)*(x3413)))+(((x3408)*(x3411)))+(((IkReal(-1.00000000000000))*(x3406)*(x3407)*(x3410)))+(((sj11)*(x3407)*(x3409)))+(((cj11)*(sj13)*(x3409)))+(((IkReal(-1.00000000000000))*(x3406)*(x3414)))))), ((gconst47)*(((((x3408)*(x3409)))+(((IkReal(-1.00000000000000))*(x3407)*(x3409)*(x3412)))+(((IkReal(-1.00000000000000))*(x3406)*(x3408)))+(((IkReal(-1.00000000000000))*(x3411)*(x3414)))+(((cj11)*(x3406)*(x3407)))+(((IkReal(-1.00000000000000))*(x3410)*(x3413)))))));
15395 sj10array[0]=IKsin(j10array[0]);
15396 cj10array[0]=IKcos(j10array[0]);
15397 if( j10array[0] > IKPI )
15398 {
15399  j10array[0]-=IK2PI;
15400 }
15401 else if( j10array[0] < -IKPI )
15402 { j10array[0]+=IK2PI;
15403 }
15404 j10valid[0] = true;
15405 for(int ij10 = 0; ij10 < 1; ++ij10)
15406 {
15407 if( !j10valid[ij10] )
15408 {
15409  continue;
15410 }
15411 _ij10[0] = ij10; _ij10[1] = -1;
15412 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15413 {
15414 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15415 {
15416  j10valid[iij10]=false; _ij10[1] = iij10; break;
15417 }
15418 }
15419 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15420 {
15421 IkReal evalcond[6];
15422 IkReal x3415=IKsin(j10);
15423 IkReal x3416=IKcos(j10);
15424 IkReal x3417=((IkReal(1.00000000000000))*(cj13));
15425 IkReal x3418=((cj9)*(r02));
15426 IkReal x3419=((IkReal(1.00000000000000))*(sj13));
15427 IkReal x3420=((r11)*(sj9));
15428 IkReal x3421=((IkReal(1.00000000000000))*(cj14));
15429 IkReal x3422=((cj9)*(r01));
15430 IkReal x3423=((r21)*(sj14));
15431 IkReal x3424=((IkReal(1.00000000000000))*(sj12));
15432 IkReal x3425=((r10)*(sj9));
15433 IkReal x3426=((cj14)*(sj13));
15434 IkReal x3427=((cj14)*(r20));
15435 IkReal x3428=((IkReal(1.00000000000000))*(sj14));
15436 IkReal x3429=((r12)*(sj9));
15437 IkReal x3430=((cj13)*(cj14));
15438 IkReal x3431=((cj9)*(r00));
15439 IkReal x3432=((sj11)*(x3415));
15440 IkReal x3433=((cj11)*(x3415));
15441 IkReal x3434=((sj11)*(x3416));
15442 IkReal x3435=((cj11)*(x3416));
15443 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x3424)*(x3434)))+(((IkReal(-1.00000000000000))*(x3424)*(x3433))));
15444 evalcond[1]=((((IkReal(-1.00000000000000))*(r22)*(x3419)))+(((cj13)*(x3423)))+(((IkReal(-1.00000000000000))*(x3417)*(x3427)))+(x3432)+(((IkReal(-1.00000000000000))*(x3435))));
15445 evalcond[2]=((((sj13)*(x3423)))+(((cj12)*(x3434)))+(((cj12)*(x3433)))+(((IkReal(-1.00000000000000))*(x3419)*(x3427)))+(((cj13)*(r22))));
15446 evalcond[3]=((((IkReal(-1.00000000000000))*(x3428)*(x3431)))+(((IkReal(-1.00000000000000))*(x3420)*(x3421)))+(((sj12)*(x3432)))+(((IkReal(-1.00000000000000))*(x3424)*(x3435)))+(((IkReal(-1.00000000000000))*(x3425)*(x3428)))+(((IkReal(-1.00000000000000))*(x3421)*(x3422))));
15447 evalcond[4]=((((x3425)*(x3430)))+(((sj13)*(x3429)))+(((x3430)*(x3431)))+(x3433)+(x3434)+(((sj13)*(x3418)))+(((IkReal(-1.00000000000000))*(sj14)*(x3417)*(x3422)))+(((IkReal(-1.00000000000000))*(sj14)*(x3417)*(x3420))));
15448 evalcond[5]=((((x3425)*(x3426)))+(((cj12)*(x3435)))+(((IkReal(-1.00000000000000))*(x3417)*(x3429)))+(((IkReal(-1.00000000000000))*(cj12)*(x3432)))+(((IkReal(-1.00000000000000))*(sj14)*(x3419)*(x3422)))+(((IkReal(-1.00000000000000))*(sj14)*(x3419)*(x3420)))+(((IkReal(-1.00000000000000))*(x3417)*(x3418)))+(((x3426)*(x3431))));
15449 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 )
15450 {
15451 continue;
15452 }
15453 }
15454 
15455 {
15456 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15457 vinfos[0].jointtype = 1;
15458 vinfos[0].foffset = j9;
15459 vinfos[0].indices[0] = _ij9[0];
15460 vinfos[0].indices[1] = _ij9[1];
15461 vinfos[0].maxsolutions = _nj9;
15462 vinfos[1].jointtype = 1;
15463 vinfos[1].foffset = j10;
15464 vinfos[1].indices[0] = _ij10[0];
15465 vinfos[1].indices[1] = _ij10[1];
15466 vinfos[1].maxsolutions = _nj10;
15467 vinfos[2].jointtype = 1;
15468 vinfos[2].foffset = j11;
15469 vinfos[2].indices[0] = _ij11[0];
15470 vinfos[2].indices[1] = _ij11[1];
15471 vinfos[2].maxsolutions = _nj11;
15472 vinfos[3].jointtype = 1;
15473 vinfos[3].foffset = j12;
15474 vinfos[3].indices[0] = _ij12[0];
15475 vinfos[3].indices[1] = _ij12[1];
15476 vinfos[3].maxsolutions = _nj12;
15477 vinfos[4].jointtype = 1;
15478 vinfos[4].foffset = j13;
15479 vinfos[4].indices[0] = _ij13[0];
15480 vinfos[4].indices[1] = _ij13[1];
15481 vinfos[4].maxsolutions = _nj13;
15482 vinfos[5].jointtype = 1;
15483 vinfos[5].foffset = j14;
15484 vinfos[5].indices[0] = _ij14[0];
15485 vinfos[5].indices[1] = _ij14[1];
15486 vinfos[5].maxsolutions = _nj14;
15487 std::vector<int> vfree(0);
15488 solutions.AddSolution(vinfos,vfree);
15489 }
15490 }
15491 }
15492 
15493 }
15494 
15495 }
15496 
15497 } else
15498 {
15499 {
15500 IkReal j10array[1], cj10array[1], sj10array[1];
15501 bool j10valid[1]={false};
15502 _nj10 = 1;
15503 IkReal x3436=((cj11)*(sj12));
15504 IkReal x3437=((r22)*(sj13));
15505 IkReal x3438=((r20)*(sj14));
15506 IkReal x3439=((cj14)*(sj11));
15507 IkReal x3440=((cj13)*(r20));
15508 IkReal x3441=((sj11)*(sj12));
15509 IkReal x3442=((cj13)*(r21)*(sj14));
15510 if( IKabs(((gconst46)*(((((IkReal(-1.00000000000000))*(x3441)*(x3442)))+(((cj11)*(x3438)))+(((cj11)*(cj14)*(r21)))+(((sj12)*(x3439)*(x3440)))+(((x3437)*(x3441))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst46)*(((((IkReal(-1.00000000000000))*(cj14)*(x3436)*(x3440)))+(((IkReal(-1.00000000000000))*(x3436)*(x3437)))+(((x3436)*(x3442)))+(((r21)*(x3439)))+(((sj11)*(x3438))))))) < IKFAST_ATAN2_MAGTHRESH )
15511  continue;
15512 j10array[0]=IKatan2(((gconst46)*(((((IkReal(-1.00000000000000))*(x3441)*(x3442)))+(((cj11)*(x3438)))+(((cj11)*(cj14)*(r21)))+(((sj12)*(x3439)*(x3440)))+(((x3437)*(x3441)))))), ((gconst46)*(((((IkReal(-1.00000000000000))*(cj14)*(x3436)*(x3440)))+(((IkReal(-1.00000000000000))*(x3436)*(x3437)))+(((x3436)*(x3442)))+(((r21)*(x3439)))+(((sj11)*(x3438)))))));
15513 sj10array[0]=IKsin(j10array[0]);
15514 cj10array[0]=IKcos(j10array[0]);
15515 if( j10array[0] > IKPI )
15516 {
15517  j10array[0]-=IK2PI;
15518 }
15519 else if( j10array[0] < -IKPI )
15520 { j10array[0]+=IK2PI;
15521 }
15522 j10valid[0] = true;
15523 for(int ij10 = 0; ij10 < 1; ++ij10)
15524 {
15525 if( !j10valid[ij10] )
15526 {
15527  continue;
15528 }
15529 _ij10[0] = ij10; _ij10[1] = -1;
15530 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15531 {
15532 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15533 {
15534  j10valid[iij10]=false; _ij10[1] = iij10; break;
15535 }
15536 }
15537 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15538 {
15539 IkReal evalcond[6];
15540 IkReal x3443=IKsin(j10);
15541 IkReal x3444=IKcos(j10);
15542 IkReal x3445=((IkReal(1.00000000000000))*(cj13));
15543 IkReal x3446=((cj9)*(r02));
15544 IkReal x3447=((IkReal(1.00000000000000))*(sj13));
15545 IkReal x3448=((r11)*(sj9));
15546 IkReal x3449=((IkReal(1.00000000000000))*(cj14));
15547 IkReal x3450=((cj9)*(r01));
15548 IkReal x3451=((r21)*(sj14));
15549 IkReal x3452=((IkReal(1.00000000000000))*(sj12));
15550 IkReal x3453=((r10)*(sj9));
15551 IkReal x3454=((cj14)*(sj13));
15552 IkReal x3455=((cj14)*(r20));
15553 IkReal x3456=((IkReal(1.00000000000000))*(sj14));
15554 IkReal x3457=((r12)*(sj9));
15555 IkReal x3458=((cj13)*(cj14));
15556 IkReal x3459=((cj9)*(r00));
15557 IkReal x3460=((sj11)*(x3443));
15558 IkReal x3461=((cj11)*(x3443));
15559 IkReal x3462=((sj11)*(x3444));
15560 IkReal x3463=((cj11)*(x3444));
15561 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x3452)*(x3462)))+(((IkReal(-1.00000000000000))*(x3452)*(x3461))));
15562 evalcond[1]=((((cj13)*(x3451)))+(x3460)+(((IkReal(-1.00000000000000))*(x3445)*(x3455)))+(((IkReal(-1.00000000000000))*(x3463)))+(((IkReal(-1.00000000000000))*(r22)*(x3447))));
15563 evalcond[2]=((((IkReal(-1.00000000000000))*(x3447)*(x3455)))+(((cj12)*(x3462)))+(((cj12)*(x3461)))+(((sj13)*(x3451)))+(((cj13)*(r22))));
15564 evalcond[3]=((((IkReal(-1.00000000000000))*(x3453)*(x3456)))+(((sj12)*(x3460)))+(((IkReal(-1.00000000000000))*(x3452)*(x3463)))+(((IkReal(-1.00000000000000))*(x3448)*(x3449)))+(((IkReal(-1.00000000000000))*(x3449)*(x3450)))+(((IkReal(-1.00000000000000))*(x3456)*(x3459))));
15565 evalcond[4]=((((x3453)*(x3458)))+(((IkReal(-1.00000000000000))*(sj14)*(x3445)*(x3448)))+(((IkReal(-1.00000000000000))*(sj14)*(x3445)*(x3450)))+(x3462)+(x3461)+(((x3458)*(x3459)))+(((sj13)*(x3457)))+(((sj13)*(x3446))));
15566 evalcond[5]=((((x3453)*(x3454)))+(((cj12)*(x3463)))+(((IkReal(-1.00000000000000))*(cj12)*(x3460)))+(((IkReal(-1.00000000000000))*(x3445)*(x3457)))+(((IkReal(-1.00000000000000))*(x3445)*(x3446)))+(((x3454)*(x3459)))+(((IkReal(-1.00000000000000))*(sj14)*(x3447)*(x3450)))+(((IkReal(-1.00000000000000))*(sj14)*(x3447)*(x3448))));
15567 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 )
15568 {
15569 continue;
15570 }
15571 }
15572 
15573 {
15574 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15575 vinfos[0].jointtype = 1;
15576 vinfos[0].foffset = j9;
15577 vinfos[0].indices[0] = _ij9[0];
15578 vinfos[0].indices[1] = _ij9[1];
15579 vinfos[0].maxsolutions = _nj9;
15580 vinfos[1].jointtype = 1;
15581 vinfos[1].foffset = j10;
15582 vinfos[1].indices[0] = _ij10[0];
15583 vinfos[1].indices[1] = _ij10[1];
15584 vinfos[1].maxsolutions = _nj10;
15585 vinfos[2].jointtype = 1;
15586 vinfos[2].foffset = j11;
15587 vinfos[2].indices[0] = _ij11[0];
15588 vinfos[2].indices[1] = _ij11[1];
15589 vinfos[2].maxsolutions = _nj11;
15590 vinfos[3].jointtype = 1;
15591 vinfos[3].foffset = j12;
15592 vinfos[3].indices[0] = _ij12[0];
15593 vinfos[3].indices[1] = _ij12[1];
15594 vinfos[3].maxsolutions = _nj12;
15595 vinfos[4].jointtype = 1;
15596 vinfos[4].foffset = j13;
15597 vinfos[4].indices[0] = _ij13[0];
15598 vinfos[4].indices[1] = _ij13[1];
15599 vinfos[4].maxsolutions = _nj13;
15600 vinfos[5].jointtype = 1;
15601 vinfos[5].foffset = j14;
15602 vinfos[5].indices[0] = _ij14[0];
15603 vinfos[5].indices[1] = _ij14[1];
15604 vinfos[5].maxsolutions = _nj14;
15605 std::vector<int> vfree(0);
15606 solutions.AddSolution(vinfos,vfree);
15607 }
15608 }
15609 }
15610 
15611 }
15612 
15613 }
15614 }
15615 }
15616 
15617 }
15618 
15619 }
15620 
15621 } else
15622 {
15623 {
15624 IkReal j11array[1], cj11array[1], sj11array[1];
15625 bool j11valid[1]={false};
15626 _nj11 = 1;
15627 IkReal x3464=((IkReal(4.00000000000000))*(sj14));
15628 IkReal x3465=((IkReal(4.00000000000000))*(cj14));
15629 if( IKabs(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npy)*(x3465)))+(((IkReal(-1.00000000000000))*(npx)*(x3464))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x3465)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x3464))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npy)*(x3465)))+(((IkReal(-1.00000000000000))*(npx)*(x3464)))))))+IKsqr(((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x3465)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x3464)))))-1) <= IKFAST_SINCOS_THRESH )
15630  continue;
15631 j11array[0]=IKatan2(((((IKabs(sj12) != 0)?((IkReal)1/(sj12)):(IkReal)1.0e30))*(((((IkReal(0.120000000000000))*(sj12)))+(((IkReal(-0.380000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(npy)*(x3465)))+(((IkReal(-1.00000000000000))*(npx)*(x3464)))))), ((IkReal(-0.940000000000000))+(((IkReal(4.00000000000000))*(npz)*(sj13)))+(((cj13)*(npx)*(x3465)))+(((IkReal(-0.360000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(cj13)*(npy)*(x3464)))));
15632 sj11array[0]=IKsin(j11array[0]);
15633 cj11array[0]=IKcos(j11array[0]);
15634 if( j11array[0] > IKPI )
15635 {
15636  j11array[0]-=IK2PI;
15637 }
15638 else if( j11array[0] < -IKPI )
15639 { j11array[0]+=IK2PI;
15640 }
15641 j11valid[0] = true;
15642 for(int ij11 = 0; ij11 < 1; ++ij11)
15643 {
15644 if( !j11valid[ij11] )
15645 {
15646  continue;
15647 }
15648 _ij11[0] = ij11; _ij11[1] = -1;
15649 for(int iij11 = ij11+1; iij11 < 1; ++iij11)
15650 {
15651 if( j11valid[iij11] && IKabs(cj11array[ij11]-cj11array[iij11]) < IKFAST_SOLUTION_THRESH && IKabs(sj11array[ij11]-sj11array[iij11]) < IKFAST_SOLUTION_THRESH )
15652 {
15653  j11valid[iij11]=false; _ij11[1] = iij11; break;
15654 }
15655 }
15656 j11 = j11array[ij11]; cj11 = cj11array[ij11]; sj11 = sj11array[ij11];
15657 {
15658 IkReal evalcond[3];
15659 IkReal x3466=IKsin(j11);
15660 IkReal x3467=((IkReal(1.00000000000000))*(sj13));
15661 IkReal x3468=((cj14)*(npx));
15662 IkReal x3469=((npy)*(sj14));
15663 IkReal x3470=((IkReal(0.250000000000000))*(x3466));
15664 evalcond[0]=((((IkReal(0.0950000000000000))*(cj12)))+(((sj12)*(x3470)))+(((cj14)*(npy)))+(((IkReal(-0.0300000000000000))*(sj12)))+(((npx)*(sj14))));
15665 evalcond[1]=((IkReal(0.235000000000000))+(((IkReal(0.250000000000000))*(IKcos(j11))))+(((IkReal(0.0900000000000000))*(cj13)))+(((cj13)*(x3469)))+(((IkReal(-1.00000000000000))*(npz)*(x3467)))+(((IkReal(-1.00000000000000))*(cj13)*(x3468))));
15666 evalcond[2]=((((IkReal(-1.00000000000000))*(x3467)*(x3468)))+(((IkReal(0.0950000000000000))*(sj12)))+(((sj13)*(x3469)))+(((cj13)*(npz)))+(((IkReal(0.0300000000000000))*(cj12)))+(((IkReal(-1.00000000000000))*(cj12)*(x3470)))+(((IkReal(0.0900000000000000))*(sj13))));
15667 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 )
15668 {
15669 continue;
15670 }
15671 }
15672 
15673 {
15674 IkReal dummyeval[1];
15675 IkReal gconst46;
15676 gconst46=IKsign(((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11))))));
15677 dummyeval[0]=((((sj12)*((sj11)*(sj11))))+(((sj12)*((cj11)*(cj11)))));
15678 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
15679 {
15680 {
15681 IkReal dummyeval[1];
15682 IkReal gconst47;
15683 gconst47=IKsign(((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11))))));
15684 dummyeval[0]=((((cj12)*((sj11)*(sj11))))+(((cj12)*((cj11)*(cj11)))));
15685 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
15686 {
15687 {
15688 IkReal evalcond[9];
15689 IkReal x3471=((r00)*(sj9));
15690 IkReal x3472=((IkReal(1.00000000000000))*(sj13));
15691 IkReal x3473=((cj13)*(sj14));
15692 IkReal x3474=((cj9)*(sj14));
15693 IkReal x3475=((cj13)*(r02));
15694 IkReal x3476=((sj13)*(sj14));
15695 IkReal x3477=((r01)*(sj9));
15696 IkReal x3478=((cj9)*(sj13));
15697 IkReal x3479=((IkReal(1.00000000000000))*(cj9));
15698 IkReal x3480=((cj14)*(r10));
15699 IkReal x3481=((cj14)*(npx));
15700 IkReal x3482=((IkReal(1.00000000000000))*(cj13));
15701 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j12)), IkReal(6.28318530717959))));
15702 evalcond[1]=((IkReal(-0.0300000000000000))+(((cj14)*(npy)))+(((npx)*(sj14)))+(((IkReal(0.250000000000000))*(sj11))));
15703 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(x3481)*(x3482)))+(((IkReal(0.250000000000000))*(cj11)))+(((npy)*(x3473)))+(((IkReal(-1.00000000000000))*(npz)*(x3472))));
15704 evalcond[3]=((((IkReal(-1.00000000000000))*(cj14)*(r11)*(x3479)))+(((IkReal(-1.00000000000000))*(r10)*(x3474)))+(((cj14)*(x3477)))+(((sj14)*(x3471))));
15705 evalcond[4]=((((IkReal(-1.00000000000000))*(cj14)*(r20)*(x3472)))+(((r21)*(x3476)))+(((cj13)*(r22))));
15706 evalcond[5]=((IkReal(0.0950000000000000))+(((IkReal(-1.00000000000000))*(x3472)*(x3481)))+(((npy)*(x3476)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13))));
15707 evalcond[6]=((((x3473)*(x3477)))+(((r12)*(x3478)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x3472)))+(((IkReal(-1.00000000000000))*(r11)*(x3473)*(x3479)))+(((IkReal(-1.00000000000000))*(cj14)*(x3471)*(x3482)))+(((cj13)*(cj9)*(x3480))));
15708 evalcond[7]=((IkReal(1.00000000000000))+(((sj9)*(x3475)))+(((x3476)*(x3477)))+(((IkReal(-1.00000000000000))*(cj14)*(x3471)*(x3472)))+(((x3478)*(x3480)))+(((IkReal(-1.00000000000000))*(r11)*(x3472)*(x3474)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x3479))));
15709 evalcond[8]=((((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3482)))+(((cj14)*(r00)*(x3478)))+(((sj13)*(sj9)*(x3480)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x3472)))+(((IkReal(-1.00000000000000))*(x3475)*(x3479)))+(((IkReal(-1.00000000000000))*(r01)*(x3472)*(x3474))));
15710 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 )
15711 {
15712 {
15713 IkReal dummyeval[1];
15714 IkReal gconst48;
15715 gconst48=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
15716 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
15717 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
15718 {
15719 {
15720 IkReal dummyeval[1];
15721 IkReal gconst49;
15722 gconst49=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
15723 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
15724 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
15725 {
15726 continue;
15727 
15728 } else
15729 {
15730 {
15731 IkReal j10array[1], cj10array[1], sj10array[1];
15732 bool j10valid[1]={false};
15733 _nj10 = 1;
15734 IkReal x3483=((IkReal(1.00000000000000))*(cj11));
15735 IkReal x3484=((sj11)*(sj14));
15736 IkReal x3485=((r10)*(sj9));
15737 IkReal x3486=((cj9)*(r00));
15738 IkReal x3487=((cj14)*(sj11));
15739 IkReal x3488=((r11)*(sj9));
15740 IkReal x3489=((cj14)*(cj9)*(r01));
15741 if( IKabs(((gconst49)*(((((x3484)*(x3486)))+(((x3484)*(x3485)))+(((cj11)*(cj14)*(r21)))+(((x3487)*(x3488)))+(((cj11)*(r20)*(sj14)))+(((cj9)*(r01)*(x3487))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst49)*(((((IkReal(-1.00000000000000))*(x3483)*(x3489)))+(((IkReal(-1.00000000000000))*(cj14)*(x3483)*(x3488)))+(((IkReal(-1.00000000000000))*(sj14)*(x3483)*(x3486)))+(((IkReal(-1.00000000000000))*(sj14)*(x3483)*(x3485)))+(((r20)*(x3484)))+(((r21)*(x3487))))))) < IKFAST_ATAN2_MAGTHRESH )
15742  continue;
15743 j10array[0]=IKatan2(((gconst49)*(((((x3484)*(x3486)))+(((x3484)*(x3485)))+(((cj11)*(cj14)*(r21)))+(((x3487)*(x3488)))+(((cj11)*(r20)*(sj14)))+(((cj9)*(r01)*(x3487)))))), ((gconst49)*(((((IkReal(-1.00000000000000))*(x3483)*(x3489)))+(((IkReal(-1.00000000000000))*(cj14)*(x3483)*(x3488)))+(((IkReal(-1.00000000000000))*(sj14)*(x3483)*(x3486)))+(((IkReal(-1.00000000000000))*(sj14)*(x3483)*(x3485)))+(((r20)*(x3484)))+(((r21)*(x3487)))))));
15744 sj10array[0]=IKsin(j10array[0]);
15745 cj10array[0]=IKcos(j10array[0]);
15746 if( j10array[0] > IKPI )
15747 {
15748  j10array[0]-=IK2PI;
15749 }
15750 else if( j10array[0] < -IKPI )
15751 { j10array[0]+=IK2PI;
15752 }
15753 j10valid[0] = true;
15754 for(int ij10 = 0; ij10 < 1; ++ij10)
15755 {
15756 if( !j10valid[ij10] )
15757 {
15758  continue;
15759 }
15760 _ij10[0] = ij10; _ij10[1] = -1;
15761 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15762 {
15763 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15764 {
15765  j10valid[iij10]=false; _ij10[1] = iij10; break;
15766 }
15767 }
15768 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15769 {
15770 IkReal evalcond[4];
15771 IkReal x3490=IKsin(j10);
15772 IkReal x3491=IKcos(j10);
15773 IkReal x3492=((cj13)*(sj14));
15774 IkReal x3493=((cj13)*(cj14));
15775 IkReal x3494=((r10)*(sj9));
15776 IkReal x3495=((IkReal(1.00000000000000))*(cj9));
15777 IkReal x3496=((sj11)*(x3490));
15778 IkReal x3497=((IkReal(1.00000000000000))*(x3491));
15779 IkReal x3498=((cj11)*(x3490));
15780 IkReal x3499=((IkReal(1.00000000000000))*(r11)*(sj9));
15781 IkReal x3500=((cj11)*(x3497));
15782 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x3497)))+(((IkReal(-1.00000000000000))*(x3498))));
15783 evalcond[1]=((((IkReal(-1.00000000000000))*(x3500)))+(x3496)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(r20)*(x3493)))+(((r21)*(x3492))));
15784 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(x3499)))+(((IkReal(-1.00000000000000))*(x3500)))+(x3496)+(((IkReal(-1.00000000000000))*(cj14)*(r01)*(x3495)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x3495)))+(((IkReal(-1.00000000000000))*(sj14)*(x3494))));
15785 evalcond[3]=((((IkReal(-1.00000000000000))*(r01)*(x3492)*(x3495)))+(((cj9)*(r02)*(sj13)))+(((cj9)*(r00)*(x3493)))+(x3498)+(((IkReal(-1.00000000000000))*(x3492)*(x3499)))+(((sj11)*(x3491)))+(((x3493)*(x3494)))+(((r12)*(sj13)*(sj9))));
15786 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
15787 {
15788 continue;
15789 }
15790 }
15791 
15792 {
15793 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15794 vinfos[0].jointtype = 1;
15795 vinfos[0].foffset = j9;
15796 vinfos[0].indices[0] = _ij9[0];
15797 vinfos[0].indices[1] = _ij9[1];
15798 vinfos[0].maxsolutions = _nj9;
15799 vinfos[1].jointtype = 1;
15800 vinfos[1].foffset = j10;
15801 vinfos[1].indices[0] = _ij10[0];
15802 vinfos[1].indices[1] = _ij10[1];
15803 vinfos[1].maxsolutions = _nj10;
15804 vinfos[2].jointtype = 1;
15805 vinfos[2].foffset = j11;
15806 vinfos[2].indices[0] = _ij11[0];
15807 vinfos[2].indices[1] = _ij11[1];
15808 vinfos[2].maxsolutions = _nj11;
15809 vinfos[3].jointtype = 1;
15810 vinfos[3].foffset = j12;
15811 vinfos[3].indices[0] = _ij12[0];
15812 vinfos[3].indices[1] = _ij12[1];
15813 vinfos[3].maxsolutions = _nj12;
15814 vinfos[4].jointtype = 1;
15815 vinfos[4].foffset = j13;
15816 vinfos[4].indices[0] = _ij13[0];
15817 vinfos[4].indices[1] = _ij13[1];
15818 vinfos[4].maxsolutions = _nj13;
15819 vinfos[5].jointtype = 1;
15820 vinfos[5].foffset = j14;
15821 vinfos[5].indices[0] = _ij14[0];
15822 vinfos[5].indices[1] = _ij14[1];
15823 vinfos[5].maxsolutions = _nj14;
15824 std::vector<int> vfree(0);
15825 solutions.AddSolution(vinfos,vfree);
15826 }
15827 }
15828 }
15829 
15830 }
15831 
15832 }
15833 
15834 } else
15835 {
15836 {
15837 IkReal j10array[1], cj10array[1], sj10array[1];
15838 bool j10valid[1]={false};
15839 _nj10 = 1;
15840 IkReal x3501=((cj11)*(r20));
15841 IkReal x3502=((IkReal(1.00000000000000))*(cj13));
15842 IkReal x3503=((r21)*(sj14));
15843 IkReal x3504=((r22)*(sj13));
15844 IkReal x3505=((r20)*(sj11));
15845 IkReal x3506=((cj14)*(r21));
15846 if( IKabs(((gconst48)*(((((cj13)*(cj14)*(x3505)))+(((IkReal(-1.00000000000000))*(sj11)*(x3502)*(x3503)))+(((cj11)*(x3506)))+(((sj14)*(x3501)))+(((sj11)*(x3504))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst48)*(((((IkReal(-1.00000000000000))*(cj11)*(x3504)))+(((IkReal(-1.00000000000000))*(cj14)*(x3501)*(x3502)))+(((sj14)*(x3505)))+(((sj11)*(x3506)))+(((cj11)*(cj13)*(x3503))))))) < IKFAST_ATAN2_MAGTHRESH )
15847  continue;
15848 j10array[0]=IKatan2(((gconst48)*(((((cj13)*(cj14)*(x3505)))+(((IkReal(-1.00000000000000))*(sj11)*(x3502)*(x3503)))+(((cj11)*(x3506)))+(((sj14)*(x3501)))+(((sj11)*(x3504)))))), ((gconst48)*(((((IkReal(-1.00000000000000))*(cj11)*(x3504)))+(((IkReal(-1.00000000000000))*(cj14)*(x3501)*(x3502)))+(((sj14)*(x3505)))+(((sj11)*(x3506)))+(((cj11)*(cj13)*(x3503)))))));
15849 sj10array[0]=IKsin(j10array[0]);
15850 cj10array[0]=IKcos(j10array[0]);
15851 if( j10array[0] > IKPI )
15852 {
15853  j10array[0]-=IK2PI;
15854 }
15855 else if( j10array[0] < -IKPI )
15856 { j10array[0]+=IK2PI;
15857 }
15858 j10valid[0] = true;
15859 for(int ij10 = 0; ij10 < 1; ++ij10)
15860 {
15861 if( !j10valid[ij10] )
15862 {
15863  continue;
15864 }
15865 _ij10[0] = ij10; _ij10[1] = -1;
15866 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
15867 {
15868 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
15869 {
15870  j10valid[iij10]=false; _ij10[1] = iij10; break;
15871 }
15872 }
15873 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
15874 {
15875 IkReal evalcond[4];
15876 IkReal x3507=IKsin(j10);
15877 IkReal x3508=IKcos(j10);
15878 IkReal x3509=((cj13)*(sj14));
15879 IkReal x3510=((cj13)*(cj14));
15880 IkReal x3511=((r10)*(sj9));
15881 IkReal x3512=((IkReal(1.00000000000000))*(cj9));
15882 IkReal x3513=((sj11)*(x3507));
15883 IkReal x3514=((IkReal(1.00000000000000))*(x3508));
15884 IkReal x3515=((cj11)*(x3507));
15885 IkReal x3516=((IkReal(1.00000000000000))*(r11)*(sj9));
15886 IkReal x3517=((cj11)*(x3514));
15887 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(sj11)*(x3514)))+(((IkReal(-1.00000000000000))*(x3515))));
15888 evalcond[1]=((((IkReal(-1.00000000000000))*(x3517)))+(x3513)+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((r21)*(x3509)))+(((IkReal(-1.00000000000000))*(r20)*(x3510))));
15889 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(r01)*(x3512)))+(((IkReal(-1.00000000000000))*(x3517)))+(x3513)+(((IkReal(-1.00000000000000))*(sj14)*(x3511)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x3512)))+(((IkReal(-1.00000000000000))*(cj14)*(x3516))));
15890 evalcond[3]=((((IkReal(-1.00000000000000))*(x3509)*(x3516)))+(((x3510)*(x3511)))+(((cj9)*(r02)*(sj13)))+(x3515)+(((IkReal(-1.00000000000000))*(r01)*(x3509)*(x3512)))+(((cj9)*(r00)*(x3510)))+(((sj11)*(x3508)))+(((r12)*(sj13)*(sj9))));
15891 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
15892 {
15893 continue;
15894 }
15895 }
15896 
15897 {
15898 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
15899 vinfos[0].jointtype = 1;
15900 vinfos[0].foffset = j9;
15901 vinfos[0].indices[0] = _ij9[0];
15902 vinfos[0].indices[1] = _ij9[1];
15903 vinfos[0].maxsolutions = _nj9;
15904 vinfos[1].jointtype = 1;
15905 vinfos[1].foffset = j10;
15906 vinfos[1].indices[0] = _ij10[0];
15907 vinfos[1].indices[1] = _ij10[1];
15908 vinfos[1].maxsolutions = _nj10;
15909 vinfos[2].jointtype = 1;
15910 vinfos[2].foffset = j11;
15911 vinfos[2].indices[0] = _ij11[0];
15912 vinfos[2].indices[1] = _ij11[1];
15913 vinfos[2].maxsolutions = _nj11;
15914 vinfos[3].jointtype = 1;
15915 vinfos[3].foffset = j12;
15916 vinfos[3].indices[0] = _ij12[0];
15917 vinfos[3].indices[1] = _ij12[1];
15918 vinfos[3].maxsolutions = _nj12;
15919 vinfos[4].jointtype = 1;
15920 vinfos[4].foffset = j13;
15921 vinfos[4].indices[0] = _ij13[0];
15922 vinfos[4].indices[1] = _ij13[1];
15923 vinfos[4].maxsolutions = _nj13;
15924 vinfos[5].jointtype = 1;
15925 vinfos[5].foffset = j14;
15926 vinfos[5].indices[0] = _ij14[0];
15927 vinfos[5].indices[1] = _ij14[1];
15928 vinfos[5].maxsolutions = _nj14;
15929 std::vector<int> vfree(0);
15930 solutions.AddSolution(vinfos,vfree);
15931 }
15932 }
15933 }
15934 
15935 }
15936 
15937 }
15938 
15939 } else
15940 {
15941 IkReal x3518=((r00)*(sj9));
15942 IkReal x3519=((IkReal(1.00000000000000))*(sj13));
15943 IkReal x3520=((cj13)*(sj14));
15944 IkReal x3521=((cj9)*(sj14));
15945 IkReal x3522=((cj13)*(r02));
15946 IkReal x3523=((sj13)*(sj14));
15947 IkReal x3524=((r01)*(sj9));
15948 IkReal x3525=((cj9)*(sj13));
15949 IkReal x3526=((IkReal(1.00000000000000))*(cj9));
15950 IkReal x3527=((cj14)*(r10));
15951 IkReal x3528=((cj14)*(npx));
15952 IkReal x3529=((IkReal(1.00000000000000))*(cj13));
15953 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j12)), IkReal(6.28318530717959))));
15954 evalcond[1]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((cj14)*(npy)))+(((npx)*(sj14))));
15955 evalcond[2]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x3519)))+(((IkReal(0.250000000000000))*(cj11)))+(((npy)*(x3520)))+(((IkReal(-1.00000000000000))*(x3528)*(x3529))));
15956 evalcond[3]=((((IkReal(-1.00000000000000))*(cj14)*(r11)*(x3526)))+(((cj14)*(x3524)))+(((sj14)*(x3518)))+(((IkReal(-1.00000000000000))*(r10)*(x3521))));
15957 evalcond[4]=((((r21)*(x3523)))+(((IkReal(-1.00000000000000))*(cj14)*(r20)*(x3519)))+(((cj13)*(r22))));
15958 evalcond[5]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(x3519)*(x3528)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((npy)*(x3523))));
15959 evalcond[6]=((((cj13)*(cj9)*(x3527)))+(((r12)*(x3525)))+(((IkReal(-1.00000000000000))*(r11)*(x3520)*(x3526)))+(((x3520)*(x3524)))+(((IkReal(-1.00000000000000))*(r02)*(sj9)*(x3519)))+(((IkReal(-1.00000000000000))*(cj14)*(x3518)*(x3529))));
15960 evalcond[7]=((IkReal(-1.00000000000000))+(((x3525)*(x3527)))+(((IkReal(-1.00000000000000))*(cj13)*(r12)*(x3526)))+(((sj9)*(x3522)))+(((x3523)*(x3524)))+(((IkReal(-1.00000000000000))*(r11)*(x3519)*(x3521)))+(((IkReal(-1.00000000000000))*(cj14)*(x3518)*(x3519))));
15961 evalcond[8]=((((IkReal(-1.00000000000000))*(x3522)*(x3526)))+(((IkReal(-1.00000000000000))*(r01)*(x3519)*(x3521)))+(((IkReal(-1.00000000000000))*(r11)*(sj14)*(sj9)*(x3519)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3529)))+(((sj13)*(sj9)*(x3527)))+(((cj14)*(r00)*(x3525))));
15962 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 )
15963 {
15964 {
15965 IkReal dummyeval[1];
15966 IkReal gconst50;
15967 gconst50=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
15968 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
15969 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
15970 {
15971 {
15972 IkReal dummyeval[1];
15973 IkReal gconst51;
15974 gconst51=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
15975 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
15976 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
15977 {
15978 continue;
15979 
15980 } else
15981 {
15982 {
15983 IkReal j10array[1], cj10array[1], sj10array[1];
15984 bool j10valid[1]={false};
15985 _nj10 = 1;
15986 IkReal x3530=((IkReal(1.00000000000000))*(sj11));
15987 IkReal x3531=((cj14)*(r21));
15988 IkReal x3532=((IkReal(1.00000000000000))*(cj11));
15989 IkReal x3533=((r20)*(sj14));
15990 IkReal x3534=((cj9)*(r00)*(sj14));
15991 IkReal x3535=((cj14)*(r11)*(sj9));
15992 IkReal x3536=((cj14)*(cj9)*(r01));
15993 IkReal x3537=((r10)*(sj14)*(sj9));
15994 if( IKabs(((gconst51)*(((((IkReal(-1.00000000000000))*(x3531)*(x3532)))+(((IkReal(-1.00000000000000))*(x3530)*(x3535)))+(((IkReal(-1.00000000000000))*(x3530)*(x3534)))+(((IkReal(-1.00000000000000))*(x3530)*(x3537)))+(((IkReal(-1.00000000000000))*(x3530)*(x3536)))+(((IkReal(-1.00000000000000))*(x3532)*(x3533))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst51)*(((((cj11)*(x3535)))+(((cj11)*(x3534)))+(((cj11)*(x3537)))+(((cj11)*(x3536)))+(((IkReal(-1.00000000000000))*(x3530)*(x3531)))+(((IkReal(-1.00000000000000))*(x3530)*(x3533))))))) < IKFAST_ATAN2_MAGTHRESH )
15995  continue;
15996 j10array[0]=IKatan2(((gconst51)*(((((IkReal(-1.00000000000000))*(x3531)*(x3532)))+(((IkReal(-1.00000000000000))*(x3530)*(x3535)))+(((IkReal(-1.00000000000000))*(x3530)*(x3534)))+(((IkReal(-1.00000000000000))*(x3530)*(x3537)))+(((IkReal(-1.00000000000000))*(x3530)*(x3536)))+(((IkReal(-1.00000000000000))*(x3532)*(x3533)))))), ((gconst51)*(((((cj11)*(x3535)))+(((cj11)*(x3534)))+(((cj11)*(x3537)))+(((cj11)*(x3536)))+(((IkReal(-1.00000000000000))*(x3530)*(x3531)))+(((IkReal(-1.00000000000000))*(x3530)*(x3533)))))));
15997 sj10array[0]=IKsin(j10array[0]);
15998 cj10array[0]=IKcos(j10array[0]);
15999 if( j10array[0] > IKPI )
16000 {
16001  j10array[0]-=IK2PI;
16002 }
16003 else if( j10array[0] < -IKPI )
16004 { j10array[0]+=IK2PI;
16005 }
16006 j10valid[0] = true;
16007 for(int ij10 = 0; ij10 < 1; ++ij10)
16008 {
16009 if( !j10valid[ij10] )
16010 {
16011  continue;
16012 }
16013 _ij10[0] = ij10; _ij10[1] = -1;
16014 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16015 {
16016 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16017 {
16018  j10valid[iij10]=false; _ij10[1] = iij10; break;
16019 }
16020 }
16021 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16022 {
16023 IkReal evalcond[4];
16024 IkReal x3538=IKsin(j10);
16025 IkReal x3539=IKcos(j10);
16026 IkReal x3540=((cj14)*(sj9));
16027 IkReal x3541=((IkReal(1.00000000000000))*(r11));
16028 IkReal x3542=((cj13)*(sj14));
16029 IkReal x3543=((IkReal(1.00000000000000))*(cj9));
16030 IkReal x3544=((cj13)*(cj14));
16031 IkReal x3545=((cj11)*(x3538));
16032 IkReal x3546=((sj11)*(x3539));
16033 IkReal x3547=((cj11)*(x3539));
16034 IkReal x3548=((sj11)*(x3538));
16035 IkReal x3549=((x3545)+(x3546));
16036 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(x3549));
16037 evalcond[1]=((((r21)*(x3542)))+(x3548)+(((IkReal(-1.00000000000000))*(r20)*(x3544)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x3547))));
16038 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(r01)*(x3543)))+(x3547)+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x3548)))+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x3543)))+(((IkReal(-1.00000000000000))*(x3540)*(x3541))));
16039 evalcond[3]=((((cj9)*(r02)*(sj13)))+(x3549)+(((cj9)*(r00)*(x3544)))+(((IkReal(-1.00000000000000))*(r01)*(x3542)*(x3543)))+(((IkReal(-1.00000000000000))*(sj9)*(x3541)*(x3542)))+(((cj13)*(r10)*(x3540)))+(((r12)*(sj13)*(sj9))));
16040 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
16041 {
16042 continue;
16043 }
16044 }
16045 
16046 {
16047 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16048 vinfos[0].jointtype = 1;
16049 vinfos[0].foffset = j9;
16050 vinfos[0].indices[0] = _ij9[0];
16051 vinfos[0].indices[1] = _ij9[1];
16052 vinfos[0].maxsolutions = _nj9;
16053 vinfos[1].jointtype = 1;
16054 vinfos[1].foffset = j10;
16055 vinfos[1].indices[0] = _ij10[0];
16056 vinfos[1].indices[1] = _ij10[1];
16057 vinfos[1].maxsolutions = _nj10;
16058 vinfos[2].jointtype = 1;
16059 vinfos[2].foffset = j11;
16060 vinfos[2].indices[0] = _ij11[0];
16061 vinfos[2].indices[1] = _ij11[1];
16062 vinfos[2].maxsolutions = _nj11;
16063 vinfos[3].jointtype = 1;
16064 vinfos[3].foffset = j12;
16065 vinfos[3].indices[0] = _ij12[0];
16066 vinfos[3].indices[1] = _ij12[1];
16067 vinfos[3].maxsolutions = _nj12;
16068 vinfos[4].jointtype = 1;
16069 vinfos[4].foffset = j13;
16070 vinfos[4].indices[0] = _ij13[0];
16071 vinfos[4].indices[1] = _ij13[1];
16072 vinfos[4].maxsolutions = _nj13;
16073 vinfos[5].jointtype = 1;
16074 vinfos[5].foffset = j14;
16075 vinfos[5].indices[0] = _ij14[0];
16076 vinfos[5].indices[1] = _ij14[1];
16077 vinfos[5].maxsolutions = _nj14;
16078 std::vector<int> vfree(0);
16079 solutions.AddSolution(vinfos,vfree);
16080 }
16081 }
16082 }
16083 
16084 }
16085 
16086 }
16087 
16088 } else
16089 {
16090 {
16091 IkReal j10array[1], cj10array[1], sj10array[1];
16092 bool j10valid[1]={false};
16093 _nj10 = 1;
16094 IkReal x3550=((cj13)*(sj11));
16095 IkReal x3551=((r21)*(sj14));
16096 IkReal x3552=((cj14)*(r20));
16097 IkReal x3553=((cj11)*(cj13));
16098 IkReal x3554=((r22)*(sj13));
16099 IkReal x3555=((r20)*(sj14));
16100 IkReal x3556=((cj14)*(r21));
16101 if( IKabs(((gconst50)*(((((IkReal(-1.00000000000000))*(sj11)*(x3554)))+(((IkReal(-1.00000000000000))*(x3550)*(x3552)))+(((cj11)*(x3556)))+(((cj11)*(x3555)))+(((x3550)*(x3551))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst50)*(((((sj11)*(x3555)))+(((sj11)*(x3556)))+(((x3552)*(x3553)))+(((cj11)*(x3554)))+(((IkReal(-1.00000000000000))*(x3551)*(x3553))))))) < IKFAST_ATAN2_MAGTHRESH )
16102  continue;
16103 j10array[0]=IKatan2(((gconst50)*(((((IkReal(-1.00000000000000))*(sj11)*(x3554)))+(((IkReal(-1.00000000000000))*(x3550)*(x3552)))+(((cj11)*(x3556)))+(((cj11)*(x3555)))+(((x3550)*(x3551)))))), ((gconst50)*(((((sj11)*(x3555)))+(((sj11)*(x3556)))+(((x3552)*(x3553)))+(((cj11)*(x3554)))+(((IkReal(-1.00000000000000))*(x3551)*(x3553)))))));
16104 sj10array[0]=IKsin(j10array[0]);
16105 cj10array[0]=IKcos(j10array[0]);
16106 if( j10array[0] > IKPI )
16107 {
16108  j10array[0]-=IK2PI;
16109 }
16110 else if( j10array[0] < -IKPI )
16111 { j10array[0]+=IK2PI;
16112 }
16113 j10valid[0] = true;
16114 for(int ij10 = 0; ij10 < 1; ++ij10)
16115 {
16116 if( !j10valid[ij10] )
16117 {
16118  continue;
16119 }
16120 _ij10[0] = ij10; _ij10[1] = -1;
16121 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16122 {
16123 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16124 {
16125  j10valid[iij10]=false; _ij10[1] = iij10; break;
16126 }
16127 }
16128 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16129 {
16130 IkReal evalcond[4];
16131 IkReal x3557=IKsin(j10);
16132 IkReal x3558=IKcos(j10);
16133 IkReal x3559=((cj14)*(sj9));
16134 IkReal x3560=((IkReal(1.00000000000000))*(r11));
16135 IkReal x3561=((cj13)*(sj14));
16136 IkReal x3562=((IkReal(1.00000000000000))*(cj9));
16137 IkReal x3563=((cj13)*(cj14));
16138 IkReal x3564=((cj11)*(x3557));
16139 IkReal x3565=((sj11)*(x3558));
16140 IkReal x3566=((cj11)*(x3558));
16141 IkReal x3567=((sj11)*(x3557));
16142 IkReal x3568=((x3564)+(x3565));
16143 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(x3568));
16144 evalcond[1]=((((IkReal(-1.00000000000000))*(r20)*(x3563)))+(x3567)+(((r21)*(x3561)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)))+(((IkReal(-1.00000000000000))*(x3566))));
16145 evalcond[2]=((((IkReal(-1.00000000000000))*(cj14)*(r01)*(x3562)))+(x3566)+(((IkReal(-1.00000000000000))*(r00)*(sj14)*(x3562)))+(((IkReal(-1.00000000000000))*(r10)*(sj14)*(sj9)))+(((IkReal(-1.00000000000000))*(x3559)*(x3560)))+(((IkReal(-1.00000000000000))*(x3567))));
16146 evalcond[3]=((((cj9)*(r02)*(sj13)))+(x3568)+(((IkReal(-1.00000000000000))*(r01)*(x3561)*(x3562)))+(((cj13)*(r10)*(x3559)))+(((cj9)*(r00)*(x3563)))+(((IkReal(-1.00000000000000))*(sj9)*(x3560)*(x3561)))+(((r12)*(sj13)*(sj9))));
16147 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
16148 {
16149 continue;
16150 }
16151 }
16152 
16153 {
16154 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16155 vinfos[0].jointtype = 1;
16156 vinfos[0].foffset = j9;
16157 vinfos[0].indices[0] = _ij9[0];
16158 vinfos[0].indices[1] = _ij9[1];
16159 vinfos[0].maxsolutions = _nj9;
16160 vinfos[1].jointtype = 1;
16161 vinfos[1].foffset = j10;
16162 vinfos[1].indices[0] = _ij10[0];
16163 vinfos[1].indices[1] = _ij10[1];
16164 vinfos[1].maxsolutions = _nj10;
16165 vinfos[2].jointtype = 1;
16166 vinfos[2].foffset = j11;
16167 vinfos[2].indices[0] = _ij11[0];
16168 vinfos[2].indices[1] = _ij11[1];
16169 vinfos[2].maxsolutions = _nj11;
16170 vinfos[3].jointtype = 1;
16171 vinfos[3].foffset = j12;
16172 vinfos[3].indices[0] = _ij12[0];
16173 vinfos[3].indices[1] = _ij12[1];
16174 vinfos[3].maxsolutions = _nj12;
16175 vinfos[4].jointtype = 1;
16176 vinfos[4].foffset = j13;
16177 vinfos[4].indices[0] = _ij13[0];
16178 vinfos[4].indices[1] = _ij13[1];
16179 vinfos[4].maxsolutions = _nj13;
16180 vinfos[5].jointtype = 1;
16181 vinfos[5].foffset = j14;
16182 vinfos[5].indices[0] = _ij14[0];
16183 vinfos[5].indices[1] = _ij14[1];
16184 vinfos[5].maxsolutions = _nj14;
16185 std::vector<int> vfree(0);
16186 solutions.AddSolution(vinfos,vfree);
16187 }
16188 }
16189 }
16190 
16191 }
16192 
16193 }
16194 
16195 } else
16196 {
16197 IkReal x3569=((cj9)*(r10));
16198 IkReal x3570=((cj13)*(cj14));
16199 IkReal x3571=((sj14)*(sj9));
16200 IkReal x3572=((IkReal(1.00000000000000))*(sj14));
16201 IkReal x3573=((cj9)*(sj13));
16202 IkReal x3574=((r02)*(sj9));
16203 IkReal x3575=((cj13)*(cj9));
16204 IkReal x3576=((cj14)*(r01));
16205 IkReal x3577=((IkReal(1.00000000000000))*(npx));
16206 IkReal x3578=((cj14)*(sj13));
16207 IkReal x3579=((IkReal(1.00000000000000))*(cj9));
16208 IkReal x3580=((npy)*(sj14));
16209 IkReal x3581=((IkReal(1.00000000000000))*(sj13));
16210 IkReal x3582=((IkReal(1.00000000000000))*(cj14)*(sj9));
16211 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j12)), IkReal(6.28318530717959))));
16212 evalcond[1]=((IkReal(0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
16213 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
16214 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(-1.00000000000000))*(npz)*(x3581)))+(((IkReal(0.250000000000000))*(cj11)))+(((cj13)*(x3580)))+(((IkReal(-1.00000000000000))*(x3570)*(x3577))));
16215 evalcond[4]=((IkReal(1.00000000000000))+(((r00)*(x3571)))+(((sj9)*(x3576)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x3579)))+(((IkReal(-1.00000000000000))*(x3569)*(x3572))));
16216 evalcond[5]=((IkReal(0.0300000000000000))+(((IkReal(-0.250000000000000))*(sj11)))+(((sj13)*(x3580)))+(((cj13)*(npz)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(-1.00000000000000))*(x3577)*(x3578))));
16217 evalcond[6]=((((IkReal(-1.00000000000000))*(cj9)*(r00)*(x3572)))+(((IkReal(-1.00000000000000))*(x3576)*(x3579)))+(((IkReal(-1.00000000000000))*(r10)*(x3571)))+(((IkReal(-1.00000000000000))*(r11)*(x3582))));
16218 evalcond[7]=((((IkReal(-1.00000000000000))*(x3574)*(x3581)))+(((IkReal(-1.00000000000000))*(r11)*(x3572)*(x3575)))+(((x3569)*(x3570)))+(((r12)*(x3573)))+(((cj13)*(r01)*(x3571)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x3570))));
16219 evalcond[8]=((((IkReal(-1.00000000000000))*(r11)*(x3572)*(x3573)))+(((r01)*(sj13)*(x3571)))+(((x3569)*(x3578)))+(((IkReal(-1.00000000000000))*(r12)*(x3575)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x3578)))+(((cj13)*(x3574))));
16220 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 )
16221 {
16222 {
16223 IkReal dummyeval[1];
16224 IkReal gconst52;
16225 gconst52=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
16226 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
16227 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
16228 {
16229 {
16230 IkReal dummyeval[1];
16231 IkReal gconst53;
16232 gconst53=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
16233 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
16234 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
16235 {
16236 continue;
16237 
16238 } else
16239 {
16240 {
16241 IkReal j10array[1], cj10array[1], sj10array[1];
16242 bool j10valid[1]={false};
16243 _nj10 = 1;
16244 IkReal x3583=((cj13)*(sj14));
16245 IkReal x3584=((IkReal(1.00000000000000))*(cj11));
16246 IkReal x3585=((r11)*(sj9));
16247 IkReal x3586=((IkReal(1.00000000000000))*(sj11));
16248 IkReal x3587=((cj13)*(cj14));
16249 IkReal x3588=((cj11)*(sj13));
16250 IkReal x3589=((r12)*(sj9));
16251 IkReal x3590=((sj11)*(sj13));
16252 IkReal x3591=((cj9)*(r02));
16253 IkReal x3592=((cj9)*(r01));
16254 IkReal x3593=((r10)*(sj9));
16255 IkReal x3594=((cj9)*(r00));
16256 if( IKabs(((gconst53)*(((((IkReal(-1.00000000000000))*(r22)*(sj13)*(x3586)))+(((r21)*(sj11)*(x3583)))+(((x3588)*(x3589)))+(((IkReal(-1.00000000000000))*(r20)*(x3586)*(x3587)))+(((x3588)*(x3591)))+(((cj11)*(x3587)*(x3593)))+(((cj11)*(x3587)*(x3594)))+(((IkReal(-1.00000000000000))*(x3583)*(x3584)*(x3592)))+(((IkReal(-1.00000000000000))*(x3583)*(x3584)*(x3585))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst53)*(((((sj11)*(x3587)*(x3594)))+(((sj11)*(x3587)*(x3593)))+(((x3589)*(x3590)))+(((cj11)*(r20)*(x3587)))+(((x3590)*(x3591)))+(((IkReal(-1.00000000000000))*(x3583)*(x3586)*(x3592)))+(((IkReal(-1.00000000000000))*(x3583)*(x3585)*(x3586)))+(((IkReal(-1.00000000000000))*(r21)*(x3583)*(x3584)))+(((r22)*(x3588))))))) < IKFAST_ATAN2_MAGTHRESH )
16257  continue;
16258 j10array[0]=IKatan2(((gconst53)*(((((IkReal(-1.00000000000000))*(r22)*(sj13)*(x3586)))+(((r21)*(sj11)*(x3583)))+(((x3588)*(x3589)))+(((IkReal(-1.00000000000000))*(r20)*(x3586)*(x3587)))+(((x3588)*(x3591)))+(((cj11)*(x3587)*(x3593)))+(((cj11)*(x3587)*(x3594)))+(((IkReal(-1.00000000000000))*(x3583)*(x3584)*(x3592)))+(((IkReal(-1.00000000000000))*(x3583)*(x3584)*(x3585)))))), ((gconst53)*(((((sj11)*(x3587)*(x3594)))+(((sj11)*(x3587)*(x3593)))+(((x3589)*(x3590)))+(((cj11)*(r20)*(x3587)))+(((x3590)*(x3591)))+(((IkReal(-1.00000000000000))*(x3583)*(x3586)*(x3592)))+(((IkReal(-1.00000000000000))*(x3583)*(x3585)*(x3586)))+(((IkReal(-1.00000000000000))*(r21)*(x3583)*(x3584)))+(((r22)*(x3588)))))));
16259 sj10array[0]=IKsin(j10array[0]);
16260 cj10array[0]=IKcos(j10array[0]);
16261 if( j10array[0] > IKPI )
16262 {
16263  j10array[0]-=IK2PI;
16264 }
16265 else if( j10array[0] < -IKPI )
16266 { j10array[0]+=IK2PI;
16267 }
16268 j10valid[0] = true;
16269 for(int ij10 = 0; ij10 < 1; ++ij10)
16270 {
16271 if( !j10valid[ij10] )
16272 {
16273  continue;
16274 }
16275 _ij10[0] = ij10; _ij10[1] = -1;
16276 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16277 {
16278 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16279 {
16280  j10valid[iij10]=false; _ij10[1] = iij10; break;
16281 }
16282 }
16283 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16284 {
16285 IkReal evalcond[4];
16286 IkReal x3595=IKsin(j10);
16287 IkReal x3596=IKcos(j10);
16288 IkReal x3597=((IkReal(1.00000000000000))*(sj13));
16289 IkReal x3598=((r11)*(sj9));
16290 IkReal x3599=((cj9)*(r01));
16291 IkReal x3600=((r21)*(sj14));
16292 IkReal x3601=((cj9)*(r02));
16293 IkReal x3602=((sj13)*(sj9));
16294 IkReal x3603=((cj14)*(r10));
16295 IkReal x3604=((IkReal(1.00000000000000))*(cj13));
16296 IkReal x3605=((cj14)*(r20));
16297 IkReal x3606=((cj11)*(x3595));
16298 IkReal x3607=((sj11)*(x3596));
16299 IkReal x3608=((sj14)*(x3604));
16300 IkReal x3609=((sj11)*(x3595));
16301 IkReal x3610=((cj11)*(x3596));
16302 IkReal x3611=((cj14)*(cj9)*(r00));
16303 IkReal x3612=((x3607)+(x3606));
16304 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x3597)))+(((IkReal(-1.00000000000000))*(x3610)))+(x3609)+(((cj13)*(x3600)))+(((IkReal(-1.00000000000000))*(x3604)*(x3605))));
16305 evalcond[1]=((((sj13)*(x3600)))+(x3612)+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x3597)*(x3605))));
16306 evalcond[2]=((((sj13)*(x3601)))+(((IkReal(-1.00000000000000))*(x3598)*(x3608)))+(x3612)+(((cj13)*(x3611)))+(((IkReal(-1.00000000000000))*(x3599)*(x3608)))+(((cj13)*(sj9)*(x3603)))+(((r12)*(x3602))));
16307 evalcond[3]=((((sj13)*(x3611)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3604)))+(((IkReal(-1.00000000000000))*(x3601)*(x3604)))+(((IkReal(-1.00000000000000))*(x3609)))+(x3610)+(((x3602)*(x3603)))+(((IkReal(-1.00000000000000))*(sj14)*(x3597)*(x3598)))+(((IkReal(-1.00000000000000))*(sj14)*(x3597)*(x3599))));
16308 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
16309 {
16310 continue;
16311 }
16312 }
16313 
16314 {
16315 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16316 vinfos[0].jointtype = 1;
16317 vinfos[0].foffset = j9;
16318 vinfos[0].indices[0] = _ij9[0];
16319 vinfos[0].indices[1] = _ij9[1];
16320 vinfos[0].maxsolutions = _nj9;
16321 vinfos[1].jointtype = 1;
16322 vinfos[1].foffset = j10;
16323 vinfos[1].indices[0] = _ij10[0];
16324 vinfos[1].indices[1] = _ij10[1];
16325 vinfos[1].maxsolutions = _nj10;
16326 vinfos[2].jointtype = 1;
16327 vinfos[2].foffset = j11;
16328 vinfos[2].indices[0] = _ij11[0];
16329 vinfos[2].indices[1] = _ij11[1];
16330 vinfos[2].maxsolutions = _nj11;
16331 vinfos[3].jointtype = 1;
16332 vinfos[3].foffset = j12;
16333 vinfos[3].indices[0] = _ij12[0];
16334 vinfos[3].indices[1] = _ij12[1];
16335 vinfos[3].maxsolutions = _nj12;
16336 vinfos[4].jointtype = 1;
16337 vinfos[4].foffset = j13;
16338 vinfos[4].indices[0] = _ij13[0];
16339 vinfos[4].indices[1] = _ij13[1];
16340 vinfos[4].maxsolutions = _nj13;
16341 vinfos[5].jointtype = 1;
16342 vinfos[5].foffset = j14;
16343 vinfos[5].indices[0] = _ij14[0];
16344 vinfos[5].indices[1] = _ij14[1];
16345 vinfos[5].maxsolutions = _nj14;
16346 std::vector<int> vfree(0);
16347 solutions.AddSolution(vinfos,vfree);
16348 }
16349 }
16350 }
16351 
16352 }
16353 
16354 }
16355 
16356 } else
16357 {
16358 {
16359 IkReal j10array[1], cj10array[1], sj10array[1];
16360 bool j10valid[1]={false};
16361 _nj10 = 1;
16362 IkReal x3613=((cj13)*(sj11));
16363 IkReal x3614=((r21)*(sj14));
16364 IkReal x3615=((cj11)*(cj13));
16365 IkReal x3616=((cj11)*(sj13));
16366 IkReal x3617=((sj11)*(sj13));
16367 IkReal x3618=((IkReal(1.00000000000000))*(cj14)*(r20));
16368 if( IKabs(((gconst52)*(((((IkReal(-1.00000000000000))*(x3616)*(x3618)))+(((IkReal(-1.00000000000000))*(r22)*(x3617)))+(((x3613)*(x3614)))+(((r22)*(x3615)))+(((IkReal(-1.00000000000000))*(x3613)*(x3618)))+(((x3614)*(x3616))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst52)*(((((cj14)*(r20)*(x3615)))+(((IkReal(-1.00000000000000))*(x3617)*(x3618)))+(((r22)*(x3613)))+(((r22)*(x3616)))+(((IkReal(-1.00000000000000))*(x3614)*(x3615)))+(((x3614)*(x3617))))))) < IKFAST_ATAN2_MAGTHRESH )
16369  continue;
16370 j10array[0]=IKatan2(((gconst52)*(((((IkReal(-1.00000000000000))*(x3616)*(x3618)))+(((IkReal(-1.00000000000000))*(r22)*(x3617)))+(((x3613)*(x3614)))+(((r22)*(x3615)))+(((IkReal(-1.00000000000000))*(x3613)*(x3618)))+(((x3614)*(x3616)))))), ((gconst52)*(((((cj14)*(r20)*(x3615)))+(((IkReal(-1.00000000000000))*(x3617)*(x3618)))+(((r22)*(x3613)))+(((r22)*(x3616)))+(((IkReal(-1.00000000000000))*(x3614)*(x3615)))+(((x3614)*(x3617)))))));
16371 sj10array[0]=IKsin(j10array[0]);
16372 cj10array[0]=IKcos(j10array[0]);
16373 if( j10array[0] > IKPI )
16374 {
16375  j10array[0]-=IK2PI;
16376 }
16377 else if( j10array[0] < -IKPI )
16378 { j10array[0]+=IK2PI;
16379 }
16380 j10valid[0] = true;
16381 for(int ij10 = 0; ij10 < 1; ++ij10)
16382 {
16383 if( !j10valid[ij10] )
16384 {
16385  continue;
16386 }
16387 _ij10[0] = ij10; _ij10[1] = -1;
16388 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16389 {
16390 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16391 {
16392  j10valid[iij10]=false; _ij10[1] = iij10; break;
16393 }
16394 }
16395 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16396 {
16397 IkReal evalcond[4];
16398 IkReal x3619=IKsin(j10);
16399 IkReal x3620=IKcos(j10);
16400 IkReal x3621=((IkReal(1.00000000000000))*(sj13));
16401 IkReal x3622=((r11)*(sj9));
16402 IkReal x3623=((cj9)*(r01));
16403 IkReal x3624=((r21)*(sj14));
16404 IkReal x3625=((cj9)*(r02));
16405 IkReal x3626=((sj13)*(sj9));
16406 IkReal x3627=((cj14)*(r10));
16407 IkReal x3628=((IkReal(1.00000000000000))*(cj13));
16408 IkReal x3629=((cj14)*(r20));
16409 IkReal x3630=((cj11)*(x3619));
16410 IkReal x3631=((sj11)*(x3620));
16411 IkReal x3632=((sj14)*(x3628));
16412 IkReal x3633=((sj11)*(x3619));
16413 IkReal x3634=((cj11)*(x3620));
16414 IkReal x3635=((cj14)*(cj9)*(r00));
16415 IkReal x3636=((x3630)+(x3631));
16416 evalcond[0]=((((cj13)*(x3624)))+(x3633)+(((IkReal(-1.00000000000000))*(x3628)*(x3629)))+(((IkReal(-1.00000000000000))*(x3634)))+(((IkReal(-1.00000000000000))*(r22)*(x3621))));
16417 evalcond[1]=((((IkReal(-1.00000000000000))*(x3621)*(x3629)))+(x3636)+(((sj13)*(x3624)))+(((cj13)*(r22))));
16418 evalcond[2]=((((cj13)*(sj9)*(x3627)))+(x3636)+(((sj13)*(x3625)))+(((r12)*(x3626)))+(((cj13)*(x3635)))+(((IkReal(-1.00000000000000))*(x3623)*(x3632)))+(((IkReal(-1.00000000000000))*(x3622)*(x3632))));
16419 evalcond[3]=((((sj13)*(x3635)))+(((x3626)*(x3627)))+(((IkReal(-1.00000000000000))*(x3625)*(x3628)))+(((IkReal(-1.00000000000000))*(sj14)*(x3621)*(x3623)))+(((IkReal(-1.00000000000000))*(sj14)*(x3621)*(x3622)))+(x3634)+(((IkReal(-1.00000000000000))*(x3633)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3628))));
16420 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
16421 {
16422 continue;
16423 }
16424 }
16425 
16426 {
16427 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16428 vinfos[0].jointtype = 1;
16429 vinfos[0].foffset = j9;
16430 vinfos[0].indices[0] = _ij9[0];
16431 vinfos[0].indices[1] = _ij9[1];
16432 vinfos[0].maxsolutions = _nj9;
16433 vinfos[1].jointtype = 1;
16434 vinfos[1].foffset = j10;
16435 vinfos[1].indices[0] = _ij10[0];
16436 vinfos[1].indices[1] = _ij10[1];
16437 vinfos[1].maxsolutions = _nj10;
16438 vinfos[2].jointtype = 1;
16439 vinfos[2].foffset = j11;
16440 vinfos[2].indices[0] = _ij11[0];
16441 vinfos[2].indices[1] = _ij11[1];
16442 vinfos[2].maxsolutions = _nj11;
16443 vinfos[3].jointtype = 1;
16444 vinfos[3].foffset = j12;
16445 vinfos[3].indices[0] = _ij12[0];
16446 vinfos[3].indices[1] = _ij12[1];
16447 vinfos[3].maxsolutions = _nj12;
16448 vinfos[4].jointtype = 1;
16449 vinfos[4].foffset = j13;
16450 vinfos[4].indices[0] = _ij13[0];
16451 vinfos[4].indices[1] = _ij13[1];
16452 vinfos[4].maxsolutions = _nj13;
16453 vinfos[5].jointtype = 1;
16454 vinfos[5].foffset = j14;
16455 vinfos[5].indices[0] = _ij14[0];
16456 vinfos[5].indices[1] = _ij14[1];
16457 vinfos[5].maxsolutions = _nj14;
16458 std::vector<int> vfree(0);
16459 solutions.AddSolution(vinfos,vfree);
16460 }
16461 }
16462 }
16463 
16464 }
16465 
16466 }
16467 
16468 } else
16469 {
16470 IkReal x3637=((cj9)*(r10));
16471 IkReal x3638=((cj13)*(cj14));
16472 IkReal x3639=((sj14)*(sj9));
16473 IkReal x3640=((IkReal(1.00000000000000))*(sj14));
16474 IkReal x3641=((cj9)*(sj13));
16475 IkReal x3642=((r02)*(sj9));
16476 IkReal x3643=((cj13)*(cj9));
16477 IkReal x3644=((cj14)*(r01));
16478 IkReal x3645=((IkReal(1.00000000000000))*(npx));
16479 IkReal x3646=((cj14)*(sj13));
16480 IkReal x3647=((IkReal(1.00000000000000))*(cj9));
16481 IkReal x3648=((npy)*(sj14));
16482 IkReal x3649=((IkReal(1.00000000000000))*(sj13));
16483 IkReal x3650=((IkReal(1.00000000000000))*(cj14)*(sj9));
16484 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j12, IkReal(6.28318530717959))));
16485 evalcond[1]=((IkReal(-0.0950000000000000))+(((cj14)*(npy)))+(((npx)*(sj14))));
16486 evalcond[2]=((((cj14)*(r21)))+(((r20)*(sj14))));
16487 evalcond[3]=((IkReal(0.235000000000000))+(((IkReal(0.0900000000000000))*(cj13)))+(((IkReal(0.250000000000000))*(cj11)))+(((IkReal(-1.00000000000000))*(npz)*(x3649)))+(((cj13)*(x3648)))+(((IkReal(-1.00000000000000))*(x3638)*(x3645))));
16488 evalcond[4]=((IkReal(-1.00000000000000))+(((sj9)*(x3644)))+(((IkReal(-1.00000000000000))*(cj14)*(r11)*(x3647)))+(((IkReal(-1.00000000000000))*(x3637)*(x3640)))+(((r00)*(x3639))));
16489 evalcond[5]=((IkReal(-0.0300000000000000))+(((IkReal(-1.00000000000000))*(x3645)*(x3646)))+(((cj13)*(npz)))+(((sj13)*(x3648)))+(((IkReal(0.0900000000000000))*(sj13)))+(((IkReal(0.250000000000000))*(sj11))));
16490 evalcond[6]=((((IkReal(-1.00000000000000))*(cj9)*(r00)*(x3640)))+(((IkReal(-1.00000000000000))*(x3644)*(x3647)))+(((IkReal(-1.00000000000000))*(r11)*(x3650)))+(((IkReal(-1.00000000000000))*(r10)*(x3639))));
16491 evalcond[7]=((((IkReal(-1.00000000000000))*(r11)*(x3640)*(x3643)))+(((r12)*(x3641)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x3638)))+(((x3637)*(x3638)))+(((cj13)*(r01)*(x3639)))+(((IkReal(-1.00000000000000))*(x3642)*(x3649))));
16492 evalcond[8]=((((IkReal(-1.00000000000000))*(r11)*(x3640)*(x3641)))+(((cj13)*(x3642)))+(((x3637)*(x3646)))+(((IkReal(-1.00000000000000))*(r12)*(x3643)))+(((IkReal(-1.00000000000000))*(r00)*(sj9)*(x3646)))+(((r01)*(sj13)*(x3639))));
16493 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 )
16494 {
16495 {
16496 IkReal dummyeval[1];
16497 IkReal gconst54;
16498 gconst54=IKsign((((sj11)*(sj11))+((cj11)*(cj11))));
16499 dummyeval[0]=(((sj11)*(sj11))+((cj11)*(cj11)));
16500 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
16501 {
16502 {
16503 IkReal dummyeval[1];
16504 IkReal gconst55;
16505 gconst55=IKsign(((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11))))));
16506 dummyeval[0]=((((IkReal(-1.00000000000000))*((sj11)*(sj11))))+(((IkReal(-1.00000000000000))*((cj11)*(cj11)))));
16507 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
16508 {
16509 continue;
16510 
16511 } else
16512 {
16513 {
16514 IkReal j10array[1], cj10array[1], sj10array[1];
16515 bool j10valid[1]={false};
16516 _nj10 = 1;
16517 IkReal x3651=((cj13)*(sj14));
16518 IkReal x3652=((IkReal(1.00000000000000))*(cj11));
16519 IkReal x3653=((r11)*(sj9));
16520 IkReal x3654=((cj11)*(sj13));
16521 IkReal x3655=((r12)*(sj9));
16522 IkReal x3656=((r10)*(sj9));
16523 IkReal x3657=((sj11)*(sj13));
16524 IkReal x3658=((cj9)*(r02));
16525 IkReal x3659=((IkReal(1.00000000000000))*(sj11));
16526 IkReal x3660=((cj9)*(r01));
16527 IkReal x3661=((cj9)*(r00));
16528 IkReal x3662=((cj13)*(cj14)*(sj11));
16529 IkReal x3663=((cj11)*(cj13)*(cj14));
16530 if( IKabs(((gconst55)*(((((IkReal(-1.00000000000000))*(x3651)*(x3652)*(x3653)))+(((x3656)*(x3663)))+(((IkReal(-1.00000000000000))*(r22)*(x3657)))+(((IkReal(-1.00000000000000))*(x3651)*(x3652)*(x3660)))+(((r21)*(sj11)*(x3651)))+(((x3654)*(x3658)))+(((x3654)*(x3655)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x3659)))+(((x3661)*(x3663))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst55)*(((((IkReal(-1.00000000000000))*(r21)*(x3651)*(x3652)))+(((x3656)*(x3662)))+(((IkReal(-1.00000000000000))*(x3651)*(x3653)*(x3659)))+(((x3657)*(x3658)))+(((r20)*(x3663)))+(((x3655)*(x3657)))+(((IkReal(-1.00000000000000))*(x3651)*(x3659)*(x3660)))+(((x3661)*(x3662)))+(((r22)*(x3654))))))) < IKFAST_ATAN2_MAGTHRESH )
16531  continue;
16532 j10array[0]=IKatan2(((gconst55)*(((((IkReal(-1.00000000000000))*(x3651)*(x3652)*(x3653)))+(((x3656)*(x3663)))+(((IkReal(-1.00000000000000))*(r22)*(x3657)))+(((IkReal(-1.00000000000000))*(x3651)*(x3652)*(x3660)))+(((r21)*(sj11)*(x3651)))+(((x3654)*(x3658)))+(((x3654)*(x3655)))+(((IkReal(-1.00000000000000))*(cj13)*(cj14)*(r20)*(x3659)))+(((x3661)*(x3663)))))), ((gconst55)*(((((IkReal(-1.00000000000000))*(r21)*(x3651)*(x3652)))+(((x3656)*(x3662)))+(((IkReal(-1.00000000000000))*(x3651)*(x3653)*(x3659)))+(((x3657)*(x3658)))+(((r20)*(x3663)))+(((x3655)*(x3657)))+(((IkReal(-1.00000000000000))*(x3651)*(x3659)*(x3660)))+(((x3661)*(x3662)))+(((r22)*(x3654)))))));
16533 sj10array[0]=IKsin(j10array[0]);
16534 cj10array[0]=IKcos(j10array[0]);
16535 if( j10array[0] > IKPI )
16536 {
16537  j10array[0]-=IK2PI;
16538 }
16539 else if( j10array[0] < -IKPI )
16540 { j10array[0]+=IK2PI;
16541 }
16542 j10valid[0] = true;
16543 for(int ij10 = 0; ij10 < 1; ++ij10)
16544 {
16545 if( !j10valid[ij10] )
16546 {
16547  continue;
16548 }
16549 _ij10[0] = ij10; _ij10[1] = -1;
16550 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16551 {
16552 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16553 {
16554  j10valid[iij10]=false; _ij10[1] = iij10; break;
16555 }
16556 }
16557 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16558 {
16559 IkReal evalcond[4];
16560 IkReal x3664=IKsin(j10);
16561 IkReal x3665=IKcos(j10);
16562 IkReal x3666=((IkReal(1.00000000000000))*(sj13));
16563 IkReal x3667=((r11)*(sj9));
16564 IkReal x3668=((cj9)*(r01));
16565 IkReal x3669=((IkReal(1.00000000000000))*(cj11));
16566 IkReal x3670=((r21)*(sj14));
16567 IkReal x3671=((cj9)*(r02));
16568 IkReal x3672=((sj13)*(sj9));
16569 IkReal x3673=((cj14)*(r10));
16570 IkReal x3674=((IkReal(1.00000000000000))*(cj13));
16571 IkReal x3675=((cj14)*(r20));
16572 IkReal x3676=((sj11)*(x3664));
16573 IkReal x3677=((sj14)*(x3674));
16574 IkReal x3678=((sj11)*(x3665));
16575 IkReal x3679=((cj14)*(cj9)*(r00));
16576 IkReal x3680=((x3665)*(x3669));
16577 evalcond[0]=((((IkReal(-1.00000000000000))*(x3680)))+(((cj13)*(x3670)))+(x3676)+(((IkReal(-1.00000000000000))*(r22)*(x3666)))+(((IkReal(-1.00000000000000))*(x3674)*(x3675))));
16578 evalcond[1]=((((IkReal(-1.00000000000000))*(x3664)*(x3669)))+(((sj13)*(x3670)))+(((IkReal(-1.00000000000000))*(x3678)))+(((IkReal(-1.00000000000000))*(x3666)*(x3675)))+(((cj13)*(r22))));
16579 evalcond[2]=((((cj13)*(sj9)*(x3673)))+(((r12)*(x3672)))+(((cj13)*(x3679)))+(x3678)+(((IkReal(-1.00000000000000))*(x3667)*(x3677)))+(((sj13)*(x3671)))+(((cj11)*(x3664)))+(((IkReal(-1.00000000000000))*(x3668)*(x3677))));
16580 evalcond[3]=((((IkReal(-1.00000000000000))*(x3671)*(x3674)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3674)))+(((IkReal(-1.00000000000000))*(x3680)))+(x3676)+(((sj13)*(x3679)))+(((IkReal(-1.00000000000000))*(sj14)*(x3666)*(x3667)))+(((IkReal(-1.00000000000000))*(sj14)*(x3666)*(x3668)))+(((x3672)*(x3673))));
16581 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
16582 {
16583 continue;
16584 }
16585 }
16586 
16587 {
16588 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16589 vinfos[0].jointtype = 1;
16590 vinfos[0].foffset = j9;
16591 vinfos[0].indices[0] = _ij9[0];
16592 vinfos[0].indices[1] = _ij9[1];
16593 vinfos[0].maxsolutions = _nj9;
16594 vinfos[1].jointtype = 1;
16595 vinfos[1].foffset = j10;
16596 vinfos[1].indices[0] = _ij10[0];
16597 vinfos[1].indices[1] = _ij10[1];
16598 vinfos[1].maxsolutions = _nj10;
16599 vinfos[2].jointtype = 1;
16600 vinfos[2].foffset = j11;
16601 vinfos[2].indices[0] = _ij11[0];
16602 vinfos[2].indices[1] = _ij11[1];
16603 vinfos[2].maxsolutions = _nj11;
16604 vinfos[3].jointtype = 1;
16605 vinfos[3].foffset = j12;
16606 vinfos[3].indices[0] = _ij12[0];
16607 vinfos[3].indices[1] = _ij12[1];
16608 vinfos[3].maxsolutions = _nj12;
16609 vinfos[4].jointtype = 1;
16610 vinfos[4].foffset = j13;
16611 vinfos[4].indices[0] = _ij13[0];
16612 vinfos[4].indices[1] = _ij13[1];
16613 vinfos[4].maxsolutions = _nj13;
16614 vinfos[5].jointtype = 1;
16615 vinfos[5].foffset = j14;
16616 vinfos[5].indices[0] = _ij14[0];
16617 vinfos[5].indices[1] = _ij14[1];
16618 vinfos[5].maxsolutions = _nj14;
16619 std::vector<int> vfree(0);
16620 solutions.AddSolution(vinfos,vfree);
16621 }
16622 }
16623 }
16624 
16625 }
16626 
16627 }
16628 
16629 } else
16630 {
16631 {
16632 IkReal j10array[1], cj10array[1], sj10array[1];
16633 bool j10valid[1]={false};
16634 _nj10 = 1;
16635 IkReal x3681=((r22)*(sj11));
16636 IkReal x3682=((IkReal(1.00000000000000))*(sj11));
16637 IkReal x3683=((IkReal(1.00000000000000))*(cj11));
16638 IkReal x3684=((cj14)*(r20));
16639 IkReal x3685=((cj13)*(r21)*(sj14));
16640 IkReal x3686=((r21)*(sj13)*(sj14));
16641 if( IKabs(((gconst54)*(((((cj11)*(x3686)))+(((IkReal(-1.00000000000000))*(x3682)*(x3685)))+(((cj11)*(cj13)*(r22)))+(((sj13)*(x3681)))+(((IkReal(-1.00000000000000))*(sj13)*(x3683)*(x3684)))+(((cj13)*(sj11)*(x3684))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst54)*(((((cj11)*(x3685)))+(((cj13)*(x3681)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x3683)))+(((sj11)*(x3686)))+(((IkReal(-1.00000000000000))*(cj13)*(x3683)*(x3684)))+(((IkReal(-1.00000000000000))*(sj13)*(x3682)*(x3684))))))) < IKFAST_ATAN2_MAGTHRESH )
16642  continue;
16643 j10array[0]=IKatan2(((gconst54)*(((((cj11)*(x3686)))+(((IkReal(-1.00000000000000))*(x3682)*(x3685)))+(((cj11)*(cj13)*(r22)))+(((sj13)*(x3681)))+(((IkReal(-1.00000000000000))*(sj13)*(x3683)*(x3684)))+(((cj13)*(sj11)*(x3684)))))), ((gconst54)*(((((cj11)*(x3685)))+(((cj13)*(x3681)))+(((IkReal(-1.00000000000000))*(r22)*(sj13)*(x3683)))+(((sj11)*(x3686)))+(((IkReal(-1.00000000000000))*(cj13)*(x3683)*(x3684)))+(((IkReal(-1.00000000000000))*(sj13)*(x3682)*(x3684)))))));
16644 sj10array[0]=IKsin(j10array[0]);
16645 cj10array[0]=IKcos(j10array[0]);
16646 if( j10array[0] > IKPI )
16647 {
16648  j10array[0]-=IK2PI;
16649 }
16650 else if( j10array[0] < -IKPI )
16651 { j10array[0]+=IK2PI;
16652 }
16653 j10valid[0] = true;
16654 for(int ij10 = 0; ij10 < 1; ++ij10)
16655 {
16656 if( !j10valid[ij10] )
16657 {
16658  continue;
16659 }
16660 _ij10[0] = ij10; _ij10[1] = -1;
16661 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16662 {
16663 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16664 {
16665  j10valid[iij10]=false; _ij10[1] = iij10; break;
16666 }
16667 }
16668 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16669 {
16670 IkReal evalcond[4];
16671 IkReal x3687=IKsin(j10);
16672 IkReal x3688=IKcos(j10);
16673 IkReal x3689=((IkReal(1.00000000000000))*(sj13));
16674 IkReal x3690=((r11)*(sj9));
16675 IkReal x3691=((cj9)*(r01));
16676 IkReal x3692=((IkReal(1.00000000000000))*(cj11));
16677 IkReal x3693=((r21)*(sj14));
16678 IkReal x3694=((cj9)*(r02));
16679 IkReal x3695=((sj13)*(sj9));
16680 IkReal x3696=((cj14)*(r10));
16681 IkReal x3697=((IkReal(1.00000000000000))*(cj13));
16682 IkReal x3698=((cj14)*(r20));
16683 IkReal x3699=((sj11)*(x3687));
16684 IkReal x3700=((sj14)*(x3697));
16685 IkReal x3701=((sj11)*(x3688));
16686 IkReal x3702=((cj14)*(cj9)*(r00));
16687 IkReal x3703=((x3688)*(x3692));
16688 evalcond[0]=((((IkReal(-1.00000000000000))*(r22)*(x3689)))+(((IkReal(-1.00000000000000))*(x3703)))+(x3699)+(((IkReal(-1.00000000000000))*(x3697)*(x3698)))+(((cj13)*(x3693))));
16689 evalcond[1]=((((IkReal(-1.00000000000000))*(x3689)*(x3698)))+(((sj13)*(x3693)))+(((IkReal(-1.00000000000000))*(x3687)*(x3692)))+(((IkReal(-1.00000000000000))*(x3701)))+(((cj13)*(r22))));
16690 evalcond[2]=((((cj11)*(x3687)))+(((cj13)*(x3702)))+(((sj13)*(x3694)))+(((IkReal(-1.00000000000000))*(x3690)*(x3700)))+(x3701)+(((IkReal(-1.00000000000000))*(x3691)*(x3700)))+(((cj13)*(sj9)*(x3696)))+(((r12)*(x3695))));
16691 evalcond[3]=((((sj13)*(x3702)))+(((x3695)*(x3696)))+(((IkReal(-1.00000000000000))*(sj14)*(x3689)*(x3691)))+(((IkReal(-1.00000000000000))*(sj14)*(x3689)*(x3690)))+(((IkReal(-1.00000000000000))*(x3703)))+(x3699)+(((IkReal(-1.00000000000000))*(x3694)*(x3697)))+(((IkReal(-1.00000000000000))*(r12)*(sj9)*(x3697))));
16692 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
16693 {
16694 continue;
16695 }
16696 }
16697 
16698 {
16699 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16700 vinfos[0].jointtype = 1;
16701 vinfos[0].foffset = j9;
16702 vinfos[0].indices[0] = _ij9[0];
16703 vinfos[0].indices[1] = _ij9[1];
16704 vinfos[0].maxsolutions = _nj9;
16705 vinfos[1].jointtype = 1;
16706 vinfos[1].foffset = j10;
16707 vinfos[1].indices[0] = _ij10[0];
16708 vinfos[1].indices[1] = _ij10[1];
16709 vinfos[1].maxsolutions = _nj10;
16710 vinfos[2].jointtype = 1;
16711 vinfos[2].foffset = j11;
16712 vinfos[2].indices[0] = _ij11[0];
16713 vinfos[2].indices[1] = _ij11[1];
16714 vinfos[2].maxsolutions = _nj11;
16715 vinfos[3].jointtype = 1;
16716 vinfos[3].foffset = j12;
16717 vinfos[3].indices[0] = _ij12[0];
16718 vinfos[3].indices[1] = _ij12[1];
16719 vinfos[3].maxsolutions = _nj12;
16720 vinfos[4].jointtype = 1;
16721 vinfos[4].foffset = j13;
16722 vinfos[4].indices[0] = _ij13[0];
16723 vinfos[4].indices[1] = _ij13[1];
16724 vinfos[4].maxsolutions = _nj13;
16725 vinfos[5].jointtype = 1;
16726 vinfos[5].foffset = j14;
16727 vinfos[5].indices[0] = _ij14[0];
16728 vinfos[5].indices[1] = _ij14[1];
16729 vinfos[5].maxsolutions = _nj14;
16730 std::vector<int> vfree(0);
16731 solutions.AddSolution(vinfos,vfree);
16732 }
16733 }
16734 }
16735 
16736 }
16737 
16738 }
16739 
16740 } else
16741 {
16742 if( 1 )
16743 {
16744 continue;
16745 
16746 } else
16747 {
16748 }
16749 }
16750 }
16751 }
16752 }
16753 }
16754 
16755 } else
16756 {
16757 {
16758 IkReal j10array[1], cj10array[1], sj10array[1];
16759 bool j10valid[1]={false};
16760 _nj10 = 1;
16761 IkReal x3704=((r21)*(sj14));
16762 IkReal x3705=((cj12)*(cj13));
16763 IkReal x3706=((sj11)*(sj13));
16764 IkReal x3707=((cj14)*(r20));
16765 IkReal x3708=((IkReal(1.00000000000000))*(sj11));
16766 IkReal x3709=((cj12)*(r22));
16767 IkReal x3710=((IkReal(1.00000000000000))*(cj11));
16768 IkReal x3711=((cj13)*(r22));
16769 IkReal x3712=((sj13)*(x3710));
16770 if( IKabs(((gconst47)*(((((IkReal(-1.00000000000000))*(x3704)*(x3712)))+(((IkReal(-1.00000000000000))*(x3710)*(x3711)))+(((cj11)*(sj13)*(x3707)))+(((IkReal(-1.00000000000000))*(x3704)*(x3705)*(x3708)))+(((sj11)*(x3705)*(x3707)))+(((x3706)*(x3709))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst47)*(((((cj11)*(x3704)*(x3705)))+(((IkReal(-1.00000000000000))*(x3705)*(x3707)*(x3710)))+(((IkReal(-1.00000000000000))*(x3709)*(x3712)))+(((IkReal(-1.00000000000000))*(x3708)*(x3711)))+(((IkReal(-1.00000000000000))*(x3704)*(x3706)))+(((x3706)*(x3707))))))) < IKFAST_ATAN2_MAGTHRESH )
16771  continue;
16772 j10array[0]=IKatan2(((gconst47)*(((((IkReal(-1.00000000000000))*(x3704)*(x3712)))+(((IkReal(-1.00000000000000))*(x3710)*(x3711)))+(((cj11)*(sj13)*(x3707)))+(((IkReal(-1.00000000000000))*(x3704)*(x3705)*(x3708)))+(((sj11)*(x3705)*(x3707)))+(((x3706)*(x3709)))))), ((gconst47)*(((((cj11)*(x3704)*(x3705)))+(((IkReal(-1.00000000000000))*(x3705)*(x3707)*(x3710)))+(((IkReal(-1.00000000000000))*(x3709)*(x3712)))+(((IkReal(-1.00000000000000))*(x3708)*(x3711)))+(((IkReal(-1.00000000000000))*(x3704)*(x3706)))+(((x3706)*(x3707)))))));
16773 sj10array[0]=IKsin(j10array[0]);
16774 cj10array[0]=IKcos(j10array[0]);
16775 if( j10array[0] > IKPI )
16776 {
16777  j10array[0]-=IK2PI;
16778 }
16779 else if( j10array[0] < -IKPI )
16780 { j10array[0]+=IK2PI;
16781 }
16782 j10valid[0] = true;
16783 for(int ij10 = 0; ij10 < 1; ++ij10)
16784 {
16785 if( !j10valid[ij10] )
16786 {
16787  continue;
16788 }
16789 _ij10[0] = ij10; _ij10[1] = -1;
16790 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16791 {
16792 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16793 {
16794  j10valid[iij10]=false; _ij10[1] = iij10; break;
16795 }
16796 }
16797 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16798 {
16799 IkReal evalcond[6];
16800 IkReal x3713=IKsin(j10);
16801 IkReal x3714=IKcos(j10);
16802 IkReal x3715=((IkReal(1.00000000000000))*(cj13));
16803 IkReal x3716=((cj9)*(r02));
16804 IkReal x3717=((IkReal(1.00000000000000))*(sj13));
16805 IkReal x3718=((r11)*(sj9));
16806 IkReal x3719=((IkReal(1.00000000000000))*(cj14));
16807 IkReal x3720=((cj9)*(r01));
16808 IkReal x3721=((r21)*(sj14));
16809 IkReal x3722=((IkReal(1.00000000000000))*(sj12));
16810 IkReal x3723=((r10)*(sj9));
16811 IkReal x3724=((cj14)*(sj13));
16812 IkReal x3725=((cj14)*(r20));
16813 IkReal x3726=((IkReal(1.00000000000000))*(sj14));
16814 IkReal x3727=((r12)*(sj9));
16815 IkReal x3728=((cj13)*(cj14));
16816 IkReal x3729=((cj9)*(r00));
16817 IkReal x3730=((sj11)*(x3713));
16818 IkReal x3731=((cj11)*(x3713));
16819 IkReal x3732=((sj11)*(x3714));
16820 IkReal x3733=((cj11)*(x3714));
16821 evalcond[0]=((((cj14)*(r21)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x3722)*(x3731)))+(((IkReal(-1.00000000000000))*(x3722)*(x3732))));
16822 evalcond[1]=((((cj13)*(x3721)))+(x3730)+(((IkReal(-1.00000000000000))*(x3733)))+(((IkReal(-1.00000000000000))*(r22)*(x3717)))+(((IkReal(-1.00000000000000))*(x3715)*(x3725))));
16823 evalcond[2]=((((sj13)*(x3721)))+(((cj13)*(r22)))+(((IkReal(-1.00000000000000))*(x3717)*(x3725)))+(((cj12)*(x3731)))+(((cj12)*(x3732))));
16824 evalcond[3]=((((IkReal(-1.00000000000000))*(x3726)*(x3729)))+(((IkReal(-1.00000000000000))*(x3723)*(x3726)))+(((IkReal(-1.00000000000000))*(x3719)*(x3720)))+(((IkReal(-1.00000000000000))*(x3722)*(x3733)))+(((sj12)*(x3730)))+(((IkReal(-1.00000000000000))*(x3718)*(x3719))));
16825 evalcond[4]=((x3732)+(x3731)+(((IkReal(-1.00000000000000))*(sj14)*(x3715)*(x3718)))+(((IkReal(-1.00000000000000))*(sj14)*(x3715)*(x3720)))+(((x3723)*(x3728)))+(((sj13)*(x3727)))+(((x3728)*(x3729)))+(((sj13)*(x3716))));
16826 evalcond[5]=((((IkReal(-1.00000000000000))*(x3715)*(x3716)))+(((x3724)*(x3729)))+(((IkReal(-1.00000000000000))*(sj14)*(x3717)*(x3720)))+(((IkReal(-1.00000000000000))*(cj12)*(x3730)))+(((IkReal(-1.00000000000000))*(sj14)*(x3717)*(x3718)))+(((x3723)*(x3724)))+(((cj12)*(x3733)))+(((IkReal(-1.00000000000000))*(x3715)*(x3727))));
16827 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 )
16828 {
16829 continue;
16830 }
16831 }
16832 
16833 {
16834 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16835 vinfos[0].jointtype = 1;
16836 vinfos[0].foffset = j9;
16837 vinfos[0].indices[0] = _ij9[0];
16838 vinfos[0].indices[1] = _ij9[1];
16839 vinfos[0].maxsolutions = _nj9;
16840 vinfos[1].jointtype = 1;
16841 vinfos[1].foffset = j10;
16842 vinfos[1].indices[0] = _ij10[0];
16843 vinfos[1].indices[1] = _ij10[1];
16844 vinfos[1].maxsolutions = _nj10;
16845 vinfos[2].jointtype = 1;
16846 vinfos[2].foffset = j11;
16847 vinfos[2].indices[0] = _ij11[0];
16848 vinfos[2].indices[1] = _ij11[1];
16849 vinfos[2].maxsolutions = _nj11;
16850 vinfos[3].jointtype = 1;
16851 vinfos[3].foffset = j12;
16852 vinfos[3].indices[0] = _ij12[0];
16853 vinfos[3].indices[1] = _ij12[1];
16854 vinfos[3].maxsolutions = _nj12;
16855 vinfos[4].jointtype = 1;
16856 vinfos[4].foffset = j13;
16857 vinfos[4].indices[0] = _ij13[0];
16858 vinfos[4].indices[1] = _ij13[1];
16859 vinfos[4].maxsolutions = _nj13;
16860 vinfos[5].jointtype = 1;
16861 vinfos[5].foffset = j14;
16862 vinfos[5].indices[0] = _ij14[0];
16863 vinfos[5].indices[1] = _ij14[1];
16864 vinfos[5].maxsolutions = _nj14;
16865 std::vector<int> vfree(0);
16866 solutions.AddSolution(vinfos,vfree);
16867 }
16868 }
16869 }
16870 
16871 }
16872 
16873 }
16874 
16875 } else
16876 {
16877 {
16878 IkReal j10array[1], cj10array[1], sj10array[1];
16879 bool j10valid[1]={false};
16880 _nj10 = 1;
16881 IkReal x3734=((cj11)*(sj12));
16882 IkReal x3735=((r22)*(sj13));
16883 IkReal x3736=((r20)*(sj14));
16884 IkReal x3737=((cj14)*(sj11));
16885 IkReal x3738=((cj13)*(r20));
16886 IkReal x3739=((sj11)*(sj12));
16887 IkReal x3740=((cj13)*(r21)*(sj14));
16888 if( IKabs(((gconst46)*(((((cj11)*(cj14)*(r21)))+(((x3735)*(x3739)))+(((cj11)*(x3736)))+(((sj12)*(x3737)*(x3738)))+(((IkReal(-1.00000000000000))*(x3739)*(x3740))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst46)*(((((sj11)*(x3736)))+(((x3734)*(x3740)))+(((IkReal(-1.00000000000000))*(x3734)*(x3735)))+(((IkReal(-1.00000000000000))*(cj14)*(x3734)*(x3738)))+(((r21)*(x3737))))))) < IKFAST_ATAN2_MAGTHRESH )
16889  continue;
16890 j10array[0]=IKatan2(((gconst46)*(((((cj11)*(cj14)*(r21)))+(((x3735)*(x3739)))+(((cj11)*(x3736)))+(((sj12)*(x3737)*(x3738)))+(((IkReal(-1.00000000000000))*(x3739)*(x3740)))))), ((gconst46)*(((((sj11)*(x3736)))+(((x3734)*(x3740)))+(((IkReal(-1.00000000000000))*(x3734)*(x3735)))+(((IkReal(-1.00000000000000))*(cj14)*(x3734)*(x3738)))+(((r21)*(x3737)))))));
16891 sj10array[0]=IKsin(j10array[0]);
16892 cj10array[0]=IKcos(j10array[0]);
16893 if( j10array[0] > IKPI )
16894 {
16895  j10array[0]-=IK2PI;
16896 }
16897 else if( j10array[0] < -IKPI )
16898 { j10array[0]+=IK2PI;
16899 }
16900 j10valid[0] = true;
16901 for(int ij10 = 0; ij10 < 1; ++ij10)
16902 {
16903 if( !j10valid[ij10] )
16904 {
16905  continue;
16906 }
16907 _ij10[0] = ij10; _ij10[1] = -1;
16908 for(int iij10 = ij10+1; iij10 < 1; ++iij10)
16909 {
16910 if( j10valid[iij10] && IKabs(cj10array[ij10]-cj10array[iij10]) < IKFAST_SOLUTION_THRESH && IKabs(sj10array[ij10]-sj10array[iij10]) < IKFAST_SOLUTION_THRESH )
16911 {
16912  j10valid[iij10]=false; _ij10[1] = iij10; break;
16913 }
16914 }
16915 j10 = j10array[ij10]; cj10 = cj10array[ij10]; sj10 = sj10array[ij10];
16916 {
16917 IkReal evalcond[6];
16918 IkReal x3741=IKsin(j10);
16919 IkReal x3742=IKcos(j10);
16920 IkReal x3743=((IkReal(1.00000000000000))*(cj13));
16921 IkReal x3744=((cj9)*(r02));
16922 IkReal x3745=((IkReal(1.00000000000000))*(sj13));
16923 IkReal x3746=((r11)*(sj9));
16924 IkReal x3747=((IkReal(1.00000000000000))*(cj14));
16925 IkReal x3748=((cj9)*(r01));
16926 IkReal x3749=((r21)*(sj14));
16927 IkReal x3750=((IkReal(1.00000000000000))*(sj12));
16928 IkReal x3751=((r10)*(sj9));
16929 IkReal x3752=((cj14)*(sj13));
16930 IkReal x3753=((cj14)*(r20));
16931 IkReal x3754=((IkReal(1.00000000000000))*(sj14));
16932 IkReal x3755=((r12)*(sj9));
16933 IkReal x3756=((cj13)*(cj14));
16934 IkReal x3757=((cj9)*(r00));
16935 IkReal x3758=((sj11)*(x3741));
16936 IkReal x3759=((cj11)*(x3741));
16937 IkReal x3760=((sj11)*(x3742));
16938 IkReal x3761=((cj11)*(x3742));
16939 evalcond[0]=((((cj14)*(r21)))+(((IkReal(-1.00000000000000))*(x3750)*(x3759)))+(((r20)*(sj14)))+(((IkReal(-1.00000000000000))*(x3750)*(x3760))));
16940 evalcond[1]=((((cj13)*(x3749)))+(((IkReal(-1.00000000000000))*(r22)*(x3745)))+(((IkReal(-1.00000000000000))*(x3743)*(x3753)))+(((IkReal(-1.00000000000000))*(x3761)))+(x3758));
16941 evalcond[2]=((((cj12)*(x3759)))+(((IkReal(-1.00000000000000))*(x3745)*(x3753)))+(((cj12)*(x3760)))+(((sj13)*(x3749)))+(((cj13)*(r22))));
16942 evalcond[3]=((((IkReal(-1.00000000000000))*(x3746)*(x3747)))+(((IkReal(-1.00000000000000))*(x3747)*(x3748)))+(((IkReal(-1.00000000000000))*(x3754)*(x3757)))+(((IkReal(-1.00000000000000))*(x3751)*(x3754)))+(((IkReal(-1.00000000000000))*(x3750)*(x3761)))+(((sj12)*(x3758))));
16943 evalcond[4]=((((x3751)*(x3756)))+(((IkReal(-1.00000000000000))*(sj14)*(x3743)*(x3746)))+(((IkReal(-1.00000000000000))*(sj14)*(x3743)*(x3748)))+(x3760)+(x3759)+(((x3756)*(x3757)))+(((sj13)*(x3755)))+(((sj13)*(x3744))));
16944 evalcond[5]=((((IkReal(-1.00000000000000))*(x3743)*(x3755)))+(((x3751)*(x3752)))+(((IkReal(-1.00000000000000))*(x3743)*(x3744)))+(((IkReal(-1.00000000000000))*(sj14)*(x3745)*(x3746)))+(((IkReal(-1.00000000000000))*(sj14)*(x3745)*(x3748)))+(((x3752)*(x3757)))+(((IkReal(-1.00000000000000))*(cj12)*(x3758)))+(((cj12)*(x3761))));
16945 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 )
16946 {
16947 continue;
16948 }
16949 }
16950 
16951 {
16952 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
16953 vinfos[0].jointtype = 1;
16954 vinfos[0].foffset = j9;
16955 vinfos[0].indices[0] = _ij9[0];
16956 vinfos[0].indices[1] = _ij9[1];
16957 vinfos[0].maxsolutions = _nj9;
16958 vinfos[1].jointtype = 1;
16959 vinfos[1].foffset = j10;
16960 vinfos[1].indices[0] = _ij10[0];
16961 vinfos[1].indices[1] = _ij10[1];
16962 vinfos[1].maxsolutions = _nj10;
16963 vinfos[2].jointtype = 1;
16964 vinfos[2].foffset = j11;
16965 vinfos[2].indices[0] = _ij11[0];
16966 vinfos[2].indices[1] = _ij11[1];
16967 vinfos[2].maxsolutions = _nj11;
16968 vinfos[3].jointtype = 1;
16969 vinfos[3].foffset = j12;
16970 vinfos[3].indices[0] = _ij12[0];
16971 vinfos[3].indices[1] = _ij12[1];
16972 vinfos[3].maxsolutions = _nj12;
16973 vinfos[4].jointtype = 1;
16974 vinfos[4].foffset = j13;
16975 vinfos[4].indices[0] = _ij13[0];
16976 vinfos[4].indices[1] = _ij13[1];
16977 vinfos[4].maxsolutions = _nj13;
16978 vinfos[5].jointtype = 1;
16979 vinfos[5].foffset = j14;
16980 vinfos[5].indices[0] = _ij14[0];
16981 vinfos[5].indices[1] = _ij14[1];
16982 vinfos[5].maxsolutions = _nj14;
16983 std::vector<int> vfree(0);
16984 solutions.AddSolution(vinfos,vfree);
16985 }
16986 }
16987 }
16988 
16989 }
16990 
16991 }
16992 }
16993 }
16994 
16995 }
16996 
16997 }
16998 }
16999 }
17000 
17001 }
17002 
17003 }
17004  }
17005 }
17006 return solutions.GetNumSolutions()>0;
17007 }
17008 
17009 static inline bool checkconsistency8(const IkReal* Breal)
17010 {
17011  IkReal norm = 0.1;
17012  for(int i = 0; i < 7; ++i) {
17013  norm += IKabs(Breal[i]);
17014  }
17015  IkReal tol = 1e-5*norm; // have to increase the threshold since many computations are involved
17016  return IKabs(Breal[0]*Breal[1]-Breal[2]) < tol && IKabs(Breal[1]*Breal[1]-Breal[3]) < tol && IKabs(Breal[0]*Breal[3]-Breal[4]) < tol && IKabs(Breal[1]*Breal[3]-Breal[5]) < tol && IKabs(Breal[0]*Breal[5]-Breal[6]) < tol;
17017 }
17021 static inline void solvedialyticpoly8qep(const IkReal* matcoeffs, IkReal* rawroots, int& numroots)
17022 {
17023  const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
17024  IkReal IKFAST_ALIGNED16(M[16*16]) = {0};
17025  IkReal IKFAST_ALIGNED16(A[8*8]);
17026  IkReal IKFAST_ALIGNED16(work[16*16*15]);
17027  int ipiv[8];
17028  int info, coeffindex;
17029  const int worksize=16*16*15;
17030  const int matrixdim = 8;
17031  const int matrixdim2 = 16;
17032  numroots = 0;
17033  // first setup M = [0 I; -C -B] and A
17034  coeffindex = 0;
17035  for(int j = 0; j < 4; ++j) {
17036  for(int k = 0; k < 6; ++k) {
17037  M[matrixdim+(j+4)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+2)] = -matcoeffs[coeffindex++];
17038  }
17039  }
17040  for(int j = 0; j < 4; ++j) {
17041  for(int k = 0; k < 6; ++k) {
17042  M[matrixdim+(j+4)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+2)+matrixdim*2*matrixdim] = -matcoeffs[coeffindex++];
17043  }
17044  }
17045  for(int j = 0; j < 4; ++j) {
17046  for(int k = 0; k < 6; ++k) {
17047  A[(j+4)+matrixdim*k] = A[j+matrixdim*(k+2)] = matcoeffs[coeffindex++];
17048  }
17049  for(int k = 0; k < 2; ++k) {
17050  A[j+matrixdim*k] = A[(j+4)+matrixdim*(k+6)] = 0;
17051  }
17052  }
17053  const IkReal lfpossibilities[4][4] = {{1,-1,1,1},{1,0,-2,1},{1,1,2,0},{1,-1,4,1}};
17054  int lfindex = -1;
17055  bool bsingular = true;
17056  do {
17057  dgetrf_(&matrixdim,&matrixdim,A,&matrixdim,&ipiv[0],&info);
17058  if( info == 0 ) {
17059  bsingular = false;
17060  for(int j = 0; j < matrixdim; ++j) {
17061  if( IKabs(A[j*matrixdim+j]) < 100*tol ) {
17062  bsingular = true;
17063  break;
17064  }
17065  }
17066  if( !bsingular ) {
17067  break;
17068  }
17069  }
17070  if( lfindex == 3 ) {
17071  break;
17072  }
17073  // transform by the linear functional
17074  lfindex++;
17075  const IkReal* lf = lfpossibilities[lfindex];
17076  // have to reinitialize A
17077  coeffindex = 0;
17078  for(int j = 0; j < 4; ++j) {
17079  for(int k = 0; k < 6; ++k) {
17080  IkReal a = matcoeffs[coeffindex+48], b = matcoeffs[coeffindex+24], c = matcoeffs[coeffindex];
17081  A[(j+4)+matrixdim*k] = A[j+matrixdim*(k+2)] = lf[0]*lf[0]*a+lf[0]*lf[2]*b+lf[2]*lf[2]*c;
17082  M[matrixdim+(j+4)+2*matrixdim*k] = M[matrixdim+j+2*matrixdim*(k+2)] = -(lf[1]*lf[1]*a + lf[1]*lf[3]*b + lf[3]*lf[3]*c);
17083  M[matrixdim+(j+4)+2*matrixdim*k+matrixdim*2*matrixdim] = M[matrixdim+j+2*matrixdim*(k+2)+matrixdim*2*matrixdim] = -(2*lf[0]*lf[1]*a + (lf[0]*lf[3]+lf[1]*lf[2])*b + 2*lf[2]*lf[3]*c);
17084  coeffindex++;
17085  }
17086  for(int k = 0; k < 2; ++k) {
17087  A[j+matrixdim*k] = A[(j+4)+matrixdim*(k+6)] = 0;
17088  }
17089  }
17090  } while(lfindex<4);
17091 
17092  if( bsingular ) {
17093  return;
17094  }
17095  dgetrs_("No transpose", &matrixdim, &matrixdim2, A, &matrixdim, &ipiv[0], &M[matrixdim], &matrixdim2, &info);
17096  if( info != 0 ) {
17097  return;
17098  }
17099 
17100  // set identity in upper corner
17101  for(int j = 0; j < matrixdim; ++j) {
17102  M[matrixdim*2*matrixdim+j+matrixdim*2*j] = 1;
17103  }
17104  IkReal IKFAST_ALIGNED16(wr[16]);
17105  IkReal IKFAST_ALIGNED16(wi[16]);
17106  IkReal IKFAST_ALIGNED16(vr[16*16]);
17107  int one=1;
17108  dgeev_("N", "V", &matrixdim2, M, &matrixdim2, wr, wi,NULL, &one, vr, &matrixdim2, work, &worksize, &info);
17109  if( info != 0 ) {
17110  return;
17111  }
17112  IkReal Breal[matrixdim-1];
17113  for(int i = 0; i < matrixdim2; ++i) {
17114  if( IKabs(wi[i]) < tol*100 ) {
17115  IkReal* ev = vr+matrixdim2*i;
17116  if( IKabs(wr[i]) > 1 ) {
17117  ev += matrixdim;
17118  }
17119  // consistency has to be checked!!
17120  if( IKabs(ev[0]) < tol ) {
17121  continue;
17122  }
17123  IkReal iconst = 1/ev[0];
17124  for(int j = 1; j < matrixdim; ++j) {
17125  Breal[j-1] = ev[j]*iconst;
17126  }
17127  if( checkconsistency8(Breal) ) {
17128  if( lfindex >= 0 ) {
17129  const IkReal* lf = lfpossibilities[lfindex];
17130  rawroots[numroots++] = (wr[i]*lf[0]+lf[1])/(wr[i]*lf[2]+lf[3]);
17131  }
17132  else {
17133  rawroots[numroots++] = wr[i];
17134  }
17135  bool bsmall0=IKabs(ev[0]) > IKabs(ev[2]);
17136  bool bsmall1=IKabs(ev[0]) > IKabs(ev[1]);
17137  if( bsmall0 && bsmall1 ) {
17138  rawroots[numroots++] = ev[2]/ev[0];
17139  rawroots[numroots++] = ev[1]/ev[0];
17140  }
17141  else if( bsmall0 && !bsmall1 ) {
17142  rawroots[numroots++] = ev[3]/ev[1];
17143  rawroots[numroots++] = ev[1]/ev[0];
17144  }
17145  else if( !bsmall0 && bsmall1 ) {
17146  rawroots[numroots++] = ev[6]/ev[4];
17147  rawroots[numroots++] = ev[7]/ev[6];
17148  }
17149  else if( !bsmall0 && !bsmall1 ) {
17150  rawroots[numroots++] = ev[7]/ev[5];
17151  rawroots[numroots++] = ev[7]/ev[6];
17152  }
17153  }
17154  }
17155  }
17156 }};
17157 
17158 
17161 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
17162 IKSolver solver;
17163 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
17164 }
17165 
17166 IKFAST_API const char* GetKinematicsHash() { return "<robot:genericrobot - NextageOpen (53b6c6feea19292c552a263dfc45aa26)>"; }
17167 
17168 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
17169 
17170 #ifdef IKFAST_NAMESPACE
17171 } // end namespace
17172 #endif
17173 
17174 #ifndef IKFAST_NO_MAIN
17175 #include <stdio.h>
17176 #include <stdlib.h>
17177 #ifdef IKFAST_NAMESPACE
17178 using namespace IKFAST_NAMESPACE;
17179 #endif
17180 int main(int argc, char** argv)
17181 {
17182  if( argc != 12+GetNumFreeParameters()+1 ) {
17183  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
17184  "Returns the ik solutions given the transformation of the end effector specified by\n"
17185  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
17186  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
17187  return 1;
17188  }
17189 
17190  IkSolutionList<IkReal> solutions;
17191  std::vector<IkReal> vfree(GetNumFreeParameters());
17192  IkReal eerot[9],eetrans[3];
17193  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
17194  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
17195  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
17196  for(std::size_t i = 0; i < vfree.size(); ++i)
17197  vfree[i] = atof(argv[13+i]);
17198  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
17199 
17200  if( !bSuccess ) {
17201  fprintf(stderr,"Failed to get ik solution\n");
17202  return -1;
17203  }
17204 
17205  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
17206  std::vector<IkReal> solvalues(GetNumJoints());
17207  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
17208  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
17209  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
17210  std::vector<IkReal> vsolfree(sol.GetFree().size());
17211  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
17212  for( std::size_t j = 0; j < solvalues.size(); ++j)
17213  printf("%.15f, ", solvalues[j]);
17214  printf("\n");
17215  }
17216  return 0;
17217 }
17218 
17219 #endif
ikfast
Definition: ikfast.h:43
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: ikfast.h:239
IKPI_2
#define IKPI_2
Definition: nextage_right_arm_ikfast_solver.cpp:64
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: nextage_right_arm_ikfast_solver.cpp:99
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: nextage_right_arm_ikfast_solver.cpp:109
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: nextage_right_arm_ikfast_solver.cpp:308
IKFAST_ALIGNED16
#define IKFAST_ALIGNED16(x)
Definition: nextage_right_arm_ikfast_solver.cpp:59
log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
IKabs
float IKabs(float f)
Definition: nextage_right_arm_ikfast_solver.cpp:88
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: ikfast.h:249
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: ikfast.h:65
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
IKtan
float IKtan(float f)
Definition: nextage_right_arm_ikfast_solver.cpp:163
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: nextage_right_arm_ikfast_solver.cpp:310
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: nextage_right_arm_ikfast_solver.cpp:306
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...
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: nextage_right_arm_ikfast_solver.cpp:307
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: nextage_right_arm_ikfast_solver.cpp:52
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: nextage_right_arm_ikfast_solver.cpp:17161
IKsign
float IKsign(float f)
Definition: nextage_right_arm_ikfast_solver.cpp:188
IKPI
#define IKPI
Definition: nextage_right_arm_ikfast_solver.cpp:63
ikfast::IkSolutionListBase::AddSolution
virtual size_t AddSolution(const std::vector< IkSingleDOFSolutionBase< T > > &vinfos, const std::vector< int > &vfree)=0
add one solution and return its index for later retrieval
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: nextage_right_arm_ikfast_solver.cpp:104
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: nextage_right_arm_ikfast_solver.cpp:210
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)
atan
INLINE Rall1d< T, V, S > atan(const Rall1d< T, V, S > &x)
IKfmod
float IKfmod(float x, float y)
Definition: nextage_right_arm_ikfast_solver.cpp:128
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)
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: nextage_right_arm_ikfast_solver.cpp:17168
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
IKSolver::checkconsistency8
static bool checkconsistency8(const IkReal *Breal)
Definition: nextage_right_arm_ikfast_solver.cpp:17009
IKatan2
float IKatan2(float fy, float fx)
Definition: nextage_right_arm_ikfast_solver.cpp:167
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
IK2PI
#define IK2PI
Definition: nextage_right_arm_ikfast_solver.cpp:62
IKcos
float IKcos(float f)
Definition: nextage_right_arm_ikfast_solver.cpp:161
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
IKacos
float IKacos(float f)
Definition: nextage_right_arm_ikfast_solver.cpp:145
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: nextage_right_arm_ikfast_solver.cpp:319
IKSolver::solvedialyticpoly8qep
static void solvedialyticpoly8qep(const IkReal *matcoeffs, IkReal *rawroots, int &numroots)
Solve the det Ax^2+Bx+C = 0 problem using the Manocha and Canny method (1994)
Definition: nextage_right_arm_ikfast_solver.cpp:17021
acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: ikfast.h:229
main
int main(int argc, char **argv)
Definition: nextage_right_arm_ikfast_solver.cpp:17180
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: nextage_right_arm_ikfast_solver.cpp:17166
IKsin
float IKsin(float f)
Definition: nextage_right_arm_ikfast_solver.cpp:159
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: ikfast.h:41
GetIkType
IKFAST_API int GetIkType()
Definition: nextage_right_arm_ikfast_solver.cpp:312
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: nextage_right_arm_ikfast_solver.cpp:25
IKFAST_STRINGIZE
#define IKFAST_STRINGIZE(s)
Definition: nextage_right_arm_ikfast_solver.cpp:35
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)
IKsqrt
float IKsqrt(float f)
Definition: nextage_right_arm_ikfast_solver.cpp:165
IKsqr
float IKsqr(float f)
Definition: nextage_right_arm_ikfast_solver.cpp:91
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
ikfast::IkSolutionListBase
manages all the solutions
Definition: ikfast.h:93
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
IKasin
float IKasin(float f)
Definition: nextage_right_arm_ikfast_solver.cpp:112
IKlog
float IKlog(float f)
Definition: nextage_right_arm_ikfast_solver.cpp:94

nextage_ik_plugin
Author(s): TORK Developer 534o <534o@opensouce-robotics.tokyo.jp>
autogenerated on Wed Jun 17 2020 04:14:36