fanuc_m20ia10l_manipulator_ikfast_solver.cpp
Go to the documentation of this file.
1 #define IKFAST_HAS_LIBRARY
21 #include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
22 using namespace ikfast;
23 
24 // check if the included ikfast version matches what this file was compiled with
25 #define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
26 IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);
27 
28 #include <cmath>
29 #include <vector>
30 #include <limits>
31 #include <algorithm>
32 #include <complex>
33 
34 #define IKFAST_STRINGIZE2(s) #s
35 #define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)
36 
37 #ifndef IKFAST_ASSERT
38 #include <stdexcept>
39 #include <sstream>
40 #include <iostream>
41 
42 #ifdef _MSC_VER
43 #ifndef __PRETTY_FUNCTION__
44 #define __PRETTY_FUNCTION__ __FUNCDNAME__
45 #endif
46 #endif
47 
48 #ifndef __PRETTY_FUNCTION__
49 #define __PRETTY_FUNCTION__ __func__
50 #endif
51 
52 #define IKFAST_ASSERT(b) { if( !(b) ) { std::stringstream ss; ss << "ikfast exception: " << __FILE__ << ":" << __LINE__ << ": " <<__PRETTY_FUNCTION__ << ": Assertion '" << #b << "' failed"; throw std::runtime_error(ss.str()); } }
53 
54 #endif
55 
56 #if defined(_MSC_VER)
57 #define IKFAST_ALIGNED16(x) __declspec(align(16)) x
58 #else
59 #define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
60 #endif
61 
62 #define IK2PI ((IkReal)6.28318530717959)
63 #define IKPI ((IkReal)3.14159265358979)
64 #define IKPI_2 ((IkReal)1.57079632679490)
65 
66 #ifdef _MSC_VER
67 #ifndef isnan
68 #define isnan _isnan
69 #endif
70 #endif // _MSC_VER
71 
72 // lapack routines
73 extern "C" {
74  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
75  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
76  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
77  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
78  void dgetrs_(const char *trans, const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info);
79  void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi,double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info);
80 }
81 
82 using namespace std; // necessary to get std math routines
83 
84 #ifdef IKFAST_NAMESPACE
85 namespace IKFAST_NAMESPACE {
86 #endif
87 
88 inline float IKabs(float f) { return fabsf(f); }
89 inline double IKabs(double f) { return fabs(f); }
90 
91 inline float IKsqr(float f) { return f*f; }
92 inline double IKsqr(double f) { return f*f; }
93 
94 inline float IKlog(float f) { return logf(f); }
95 inline double IKlog(double f) { return log(f); }
96 
97 // allows asin and acos to exceed 1
98 #ifndef IKFAST_SINCOS_THRESH
99 #define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
100 #endif
101 
102 // used to check input to atan2 for degenerate cases
103 #ifndef IKFAST_ATAN2_MAGTHRESH
104 #define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
105 #endif
106 
107 // minimum distance of separate solutions
108 #ifndef IKFAST_SOLUTION_THRESH
109 #define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
110 #endif
111 
112 inline float IKasin(float f)
113 {
114 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
115 if( f <= -1 ) return float(-IKPI_2);
116 else if( f >= 1 ) return float(IKPI_2);
117 return asinf(f);
118 }
119 inline double IKasin(double f)
120 {
121 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
122 if( f <= -1 ) return -IKPI_2;
123 else if( f >= 1 ) return IKPI_2;
124 return asin(f);
125 }
126 
127 // return positive value in [0,y)
128 inline float IKfmod(float x, float y)
129 {
130  while(x < 0) {
131  x += y;
132  }
133  return fmodf(x,y);
134 }
135 
136 // return positive value in [0,y)
137 inline double IKfmod(double x, double y)
138 {
139  while(x < 0) {
140  x += y;
141  }
142  return fmod(x,y);
143 }
144 
145 inline float IKacos(float f)
146 {
147 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
148 if( f <= -1 ) return float(IKPI);
149 else if( f >= 1 ) return float(0);
150 return acosf(f);
151 }
152 inline double IKacos(double f)
153 {
154 IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
155 if( f <= -1 ) return IKPI;
156 else if( f >= 1 ) return 0;
157 return acos(f);
158 }
159 inline float IKsin(float f) { return sinf(f); }
160 inline double IKsin(double f) { return sin(f); }
161 inline float IKcos(float f) { return cosf(f); }
162 inline double IKcos(double f) { return cos(f); }
163 inline float IKtan(float f) { return tanf(f); }
164 inline double IKtan(double f) { return tan(f); }
165 inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
166 inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
167 inline float IKatan2(float fy, float fx) {
168  if( isnan(fy) ) {
169  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
170  return float(IKPI_2);
171  }
172  else if( isnan(fx) ) {
173  return 0;
174  }
175  return atan2f(fy,fx);
176 }
177 inline double IKatan2(double fy, double fx) {
178  if( isnan(fy) ) {
179  IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
180  return IKPI_2;
181  }
182  else if( isnan(fx) ) {
183  return 0;
184  }
185  return atan2(fy,fx);
186 }
187 
188 inline float IKsign(float f) {
189  if( f > 0 ) {
190  return float(1);
191  }
192  else if( f < 0 ) {
193  return float(-1);
194  }
195  return 0;
196 }
197 
198 inline double IKsign(double f) {
199  if( f > 0 ) {
200  return 1.0;
201  }
202  else if( f < 0 ) {
203  return -1.0;
204  }
205  return 0;
206 }
207 
210 IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
211 IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20,x21,x22,x23,x24,x25,x26,x27,x28,x29,x30,x31,x32,x33,x34,x35,x36,x37,x38,x39,x40,x41,x42,x43,x44,x45,x46,x47,x48;
212 x0=IKcos(j[0]);
213 x1=IKcos(j[2]);
214 x2=IKsin(j[1]);
215 x3=IKcos(j[1]);
216 x4=IKsin(j[2]);
217 x5=IKsin(j[3]);
218 x6=IKcos(j[3]);
219 x7=IKsin(j[0]);
220 x8=IKcos(j[5]);
221 x9=IKsin(j[5]);
222 x10=IKsin(j[4]);
223 x11=IKcos(j[4]);
224 x12=((IkReal(0.100000000000000))*(x4));
225 x13=((IkReal(0.100000000000000))*(x0));
226 x14=((IkReal(0.100000000000000))*(x6));
227 x15=((IkReal(0.250000000000000))*(x4));
228 x16=((IkReal(1.00000000000000))*(x7));
229 x17=((IkReal(1.04000000000000))*(x1));
230 x18=((IkReal(1.00000000000000))*(x4));
231 x19=((IkReal(1.00000000000000))*(x5));
232 x20=((x0)*(x2));
233 x21=((x5)*(x7));
234 x22=((x1)*(x2));
235 x23=((x0)*(x3));
236 x24=((x2)*(x4));
237 x25=((x3)*(x7));
238 x26=((x2)*(x7));
239 x27=((x1)*(x3));
240 x28=((x3)*(x4));
241 x29=((x0)*(x6));
242 x30=((x16)*(x6));
243 x31=((IkReal(1.00000000000000))*(x22));
244 x32=((IkReal(1.04000000000000))*(x24));
245 x33=((IkReal(1.00000000000000))*(x27));
246 x34=((x18)*(x23));
247 x35=((x16)*(x28));
248 x36=((x24)+(x27));
249 x37=((x36)*(x6));
250 x38=((((x1)*(x20)))+(((IkReal(-1.00000000000000))*(x34))));
251 x39=((((x22)*(x7)))+(((IkReal(-1.00000000000000))*(x35))));
252 x40=((x34)+(((IkReal(-1.00000000000000))*(x1)*(x20))));
253 x41=((((IkReal(-1.00000000000000))*(x16)*(x22)))+(x35));
254 x42=((x38)*(x6));
255 x43=((x41)*(x5));
256 x44=((x42)+(((IkReal(-1.00000000000000))*(x16)*(x5))));
257 x45=((((x39)*(x6)))+(((x0)*(x5))));
258 x46=((((x11)*(x37)))+(((x10)*(((x31)+(((IkReal(-1.00000000000000))*(x18)*(x3))))))));
259 x47=((((x11)*(x44)))+(((x10)*(((((IkReal(-1.00000000000000))*(x1)*(x23)))+(((IkReal(-1.00000000000000))*(x18)*(x20))))))));
260 IkReal x49=((IkReal(1.00000000000000))*(x16));
261 x48=((((x11)*(x45)))+(((x10)*(((((IkReal(-1.00000000000000))*(x24)*(x49)))+(((IkReal(-1.00000000000000))*(x27)*(x49))))))));
262 eerot[0]=((((x47)*(x8)))+(((x9)*(((((IkReal(-1.00000000000000))*(x30)))+(((x40)*(x5))))))));
263 eerot[1]=((((x8)*(((x30)+(((IkReal(-1.00000000000000))*(x19)*(x40)))))))+(((x47)*(x9))));
264 eerot[2]=((((x10)*(x44)))+(((x11)*(((((x20)*(x4)))+(((x1)*(x23))))))));
265 eetrans[0]=((((x11)*(((((x13)*(x27)))+(((x12)*(x20)))))))+(((x10)*(((((IkReal(-0.100000000000000))*(x21)))+(((x14)*(x38)))))))+(((x17)*(x23)))+(((IkReal(-1.00000000000000))*(x15)*(x23)))+(((IkReal(1.04000000000000))*(x20)*(x4)))+(((IkReal(0.150000000000000))*(x0)))+(((IkReal(0.250000000000000))*(x1)*(x20)))+(((IkReal(0.790000000000000))*(x20))));
266 eerot[3]=((((x9)*(((x29)+(x43)))))+(((x48)*(x8))));
267 eerot[4]=((((x8)*(((((IkReal(-1.00000000000000))*(x19)*(x41)))+(((IkReal(-1.00000000000000))*(x29)))))))+(((x48)*(x9))));
268 eerot[5]=((((x11)*(((((x1)*(x25)))+(((x24)*(x7)))))))+(((x10)*(x45))));
269 eetrans[1]=((((IkReal(0.150000000000000))*(x7)))+(((x10)*(((((x13)*(x5)))+(((x14)*(x39)))))))+(((x17)*(x25)))+(((IkReal(0.790000000000000))*(x26)))+(((x11)*(((((IkReal(0.100000000000000))*(x1)*(x25)))+(((x12)*(x26)))))))+(((x32)*(x7)))+(((IkReal(-1.00000000000000))*(x15)*(x25)))+(((IkReal(0.250000000000000))*(x22)*(x7))));
270 eerot[6]=((((x46)*(x8)))+(((x5)*(x9)*(((((IkReal(-1.00000000000000))*(x33)))+(((IkReal(-1.00000000000000))*(x18)*(x2))))))));
271 eerot[7]=((((x36)*(x5)*(x8)))+(((x46)*(x9))));
272 eerot[8]=((((x11)*(((x28)+(((IkReal(-1.00000000000000))*(x31)))))))+(((x10)*(x37))));
273 eetrans[2]=((IkReal(0.525000000000000))+(((x10)*(x6)*(((((x12)*(x2)))+(((IkReal(0.100000000000000))*(x27)))))))+(((IkReal(0.790000000000000))*(x3)))+(((IkReal(1.04000000000000))*(x28)))+(((IkReal(0.250000000000000))*(x27)))+(((x15)*(x2)))+(((x11)*(((((IkReal(-0.100000000000000))*(x22)))+(((x12)*(x3)))))))+(((IkReal(-1.00000000000000))*(x17)*(x2))));
274 }
275 
276 IKFAST_API int GetNumFreeParameters() { return 0; }
277 IKFAST_API int* GetFreeParameters() { return NULL; }
278 IKFAST_API int GetNumJoints() { return 6; }
279 
280 IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }
281 
282 IKFAST_API int GetIkType() { return 0x67000001; }
283 
284 class IKSolver {
285 public:
286 IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,j5,cj5,sj5,htj5,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;
287 unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4,_ij5[2], _nj5;
288 
289 bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
290 j0=numeric_limits<IkReal>::quiet_NaN(); _ij0[0] = -1; _ij0[1] = -1; _nj0 = -1; j1=numeric_limits<IkReal>::quiet_NaN(); _ij1[0] = -1; _ij1[1] = -1; _nj1 = -1; j2=numeric_limits<IkReal>::quiet_NaN(); _ij2[0] = -1; _ij2[1] = -1; _nj2 = -1; j3=numeric_limits<IkReal>::quiet_NaN(); _ij3[0] = -1; _ij3[1] = -1; _nj3 = -1; j4=numeric_limits<IkReal>::quiet_NaN(); _ij4[0] = -1; _ij4[1] = -1; _nj4 = -1; j5=numeric_limits<IkReal>::quiet_NaN(); _ij5[0] = -1; _ij5[1] = -1; _nj5 = -1;
291 for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
292  solutions.Clear();
293 r00 = eerot[0*3+0];
294 r01 = eerot[0*3+1];
295 r02 = eerot[0*3+2];
296 r10 = eerot[1*3+0];
297 r11 = eerot[1*3+1];
298 r12 = eerot[1*3+2];
299 r20 = eerot[2*3+0];
300 r21 = eerot[2*3+1];
301 r22 = eerot[2*3+2];
302 px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
303 
304 new_r00=r00;
305 new_r01=((IkReal(-1.00000000000000))*(r01));
306 new_r02=((IkReal(-1.00000000000000))*(r02));
307 new_px=((((IkReal(-0.100000000000000))*(r02)))+(px));
308 new_r10=r10;
309 new_r11=((IkReal(-1.00000000000000))*(r11));
310 new_r12=((IkReal(-1.00000000000000))*(r12));
311 new_py=((((IkReal(-0.100000000000000))*(r12)))+(py));
312 new_r20=r20;
313 new_r21=((IkReal(-1.00000000000000))*(r21));
314 new_r22=((IkReal(-1.00000000000000))*(r22));
315 new_pz=((IkReal(-0.525000000000000))+(((IkReal(-0.100000000000000))*(r22)))+(pz));
316 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;
317 pp=(((px)*(px))+((pz)*(pz))+((py)*(py)));
318 npx=((((py)*(r10)))+(((pz)*(r20)))+(((px)*(r00))));
319 npy=((((px)*(r01)))+(((pz)*(r21)))+(((py)*(r11))));
320 npz=((((py)*(r12)))+(((pz)*(r22)))+(((px)*(r02))));
321 rxp0_0=((((IkReal(-1.00000000000000))*(py)*(r20)))+(((pz)*(r10))));
322 rxp0_1=((((px)*(r20)))+(((IkReal(-1.00000000000000))*(pz)*(r00))));
323 rxp0_2=((((py)*(r00)))+(((IkReal(-1.00000000000000))*(px)*(r10))));
324 rxp1_0=((((pz)*(r11)))+(((IkReal(-1.00000000000000))*(py)*(r21))));
325 rxp1_1=((((IkReal(-1.00000000000000))*(pz)*(r01)))+(((px)*(r21))));
326 rxp1_2=((((py)*(r01)))+(((IkReal(-1.00000000000000))*(px)*(r11))));
327 rxp2_0=((((IkReal(-1.00000000000000))*(py)*(r22)))+(((pz)*(r12))));
328 rxp2_1=((((px)*(r22)))+(((IkReal(-1.00000000000000))*(pz)*(r02))));
329 rxp2_2=((((py)*(r02)))+(((IkReal(-1.00000000000000))*(px)*(r12))));
330 {
331 IkReal j0array[2], cj0array[2], sj0array[2];
332 bool j0valid[2]={false};
333 _nj0 = 2;
334 if( IKabs(py) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(px))) < IKFAST_ATAN2_MAGTHRESH )
335  continue;
336 IkReal x50=IKatan2(py, ((IkReal(-1.00000000000000))*(px)));
337 j0array[0]=((IkReal(-1.00000000000000))*(x50));
338 sj0array[0]=IKsin(j0array[0]);
339 cj0array[0]=IKcos(j0array[0]);
340 j0array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x50))));
341 sj0array[1]=IKsin(j0array[1]);
342 cj0array[1]=IKcos(j0array[1]);
343 if( j0array[0] > IKPI )
344 {
345  j0array[0]-=IK2PI;
346 }
347 else if( j0array[0] < -IKPI )
348 { j0array[0]+=IK2PI;
349 }
350 j0valid[0] = true;
351 if( j0array[1] > IKPI )
352 {
353  j0array[1]-=IK2PI;
354 }
355 else if( j0array[1] < -IKPI )
356 { j0array[1]+=IK2PI;
357 }
358 j0valid[1] = true;
359 for(int ij0 = 0; ij0 < 2; ++ij0)
360 {
361 if( !j0valid[ij0] )
362 {
363  continue;
364 }
365 _ij0[0] = ij0; _ij0[1] = -1;
366 for(int iij0 = ij0+1; iij0 < 2; ++iij0)
367 {
368 if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
369 {
370  j0valid[iij0]=false; _ij0[1] = iij0; break;
371 }
372 }
373 j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];
374 
375 {
376 IkReal j2array[2], cj2array[2], sj2array[2];
377 bool j2valid[2]={false};
378 _nj2 = 2;
379 if( (((IkReal(1.03295293068147))+(((IkReal(0.177513822079647))*(cj0)*(px)))+(((IkReal(0.177513822079647))*(py)*(sj0)))+(((IkReal(-0.591712740265492))*(pp))))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(1.03295293068147))+(((IkReal(0.177513822079647))*(cj0)*(px)))+(((IkReal(0.177513822079647))*(py)*(sj0)))+(((IkReal(-0.591712740265492))*(pp))))) > 1+IKFAST_SINCOS_THRESH )
380  continue;
381 IkReal x51=IKasin(((IkReal(1.03295293068147))+(((IkReal(0.177513822079647))*(cj0)*(px)))+(((IkReal(0.177513822079647))*(py)*(sj0)))+(((IkReal(-0.591712740265492))*(pp)))));
382 j2array[0]=((IkReal(-0.235908617068472))+(((IkReal(-1.00000000000000))*(x51))));
383 sj2array[0]=IKsin(j2array[0]);
384 cj2array[0]=IKcos(j2array[0]);
385 j2array[1]=((IkReal(2.90568403652132))+(x51));
386 sj2array[1]=IKsin(j2array[1]);
387 cj2array[1]=IKcos(j2array[1]);
388 if( j2array[0] > IKPI )
389 {
390  j2array[0]-=IK2PI;
391 }
392 else if( j2array[0] < -IKPI )
393 { j2array[0]+=IK2PI;
394 }
395 j2valid[0] = true;
396 if( j2array[1] > IKPI )
397 {
398  j2array[1]-=IK2PI;
399 }
400 else if( j2array[1] < -IKPI )
401 { j2array[1]+=IK2PI;
402 }
403 j2valid[1] = true;
404 for(int ij2 = 0; ij2 < 2; ++ij2)
405 {
406 if( !j2valid[ij2] )
407 {
408  continue;
409 }
410 _ij2[0] = ij2; _ij2[1] = -1;
411 for(int iij2 = ij2+1; iij2 < 2; ++iij2)
412 {
413 if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
414 {
415  j2valid[iij2]=false; _ij2[1] = iij2; break;
416 }
417 }
418 j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
419 
420 {
421 IkReal dummyeval[1];
422 IkReal gconst0;
423 IkReal x52=((IkReal(1.04000000000000))*(cj2));
424 IkReal x53=((py)*(sj0));
425 IkReal x54=((cj0)*(px));
426 IkReal x55=((IkReal(0.250000000000000))*(sj2));
427 gconst0=IKsign(((((IkReal(-1.00000000000000))*(x52)*(x53)))+(((IkReal(-0.0375000000000000))*(sj2)))+(((IkReal(0.250000000000000))*(cj2)*(pz)))+(((IkReal(0.156000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(x52)*(x54)))+(((IkReal(0.790000000000000))*(pz)))+(((x53)*(x55)))+(((IkReal(1.04000000000000))*(pz)*(sj2)))+(((x54)*(x55)))));
428 IkReal x56=((IkReal(27.7333333333333))*(cj2));
429 IkReal x57=((py)*(sj0));
430 IkReal x58=((cj0)*(px));
431 IkReal x59=((IkReal(6.66666666666667))*(sj2));
432 dummyeval[0]=((((IkReal(21.0666666666667))*(pz)))+(((IkReal(-1.00000000000000))*(x56)*(x57)))+(((IkReal(-1.00000000000000))*(sj2)))+(((IkReal(4.16000000000000))*(cj2)))+(((IkReal(6.66666666666667))*(cj2)*(pz)))+(((IkReal(-1.00000000000000))*(x56)*(x58)))+(((x58)*(x59)))+(((x57)*(x59)))+(((IkReal(27.7333333333333))*(pz)*(sj2))));
433 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
434 {
435 {
436 IkReal dummyeval[1];
437 IkReal gconst1;
438 IkReal x60=((py)*(sj0));
439 IkReal x61=((IkReal(1.04000000000000))*(sj2));
440 IkReal x62=((IkReal(0.250000000000000))*(cj2));
441 IkReal x63=((cj0)*(px));
442 gconst1=IKsign(((IkReal(0.118500000000000))+(((IkReal(-0.790000000000000))*(x60)))+(((IkReal(-1.00000000000000))*(x60)*(x61)))+(((IkReal(0.0375000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(x62)*(x63)))+(((IkReal(0.250000000000000))*(pz)*(sj2)))+(((IkReal(-1.04000000000000))*(cj2)*(pz)))+(((IkReal(0.156000000000000))*(sj2)))+(((IkReal(-1.00000000000000))*(x61)*(x63)))+(((IkReal(-0.790000000000000))*(x63)))+(((IkReal(-1.00000000000000))*(x60)*(x62)))));
443 IkReal x64=((py)*(sj0));
444 IkReal x65=((IkReal(27.7333333333333))*(sj2));
445 IkReal x66=((cj0)*(px));
446 IkReal x67=((IkReal(6.66666666666667))*(cj2));
447 dummyeval[0]=((IkReal(3.16000000000000))+(((IkReal(-21.0666666666667))*(x66)))+(((IkReal(-21.0666666666667))*(x64)))+(((IkReal(-1.00000000000000))*(x64)*(x65)))+(((IkReal(-1.00000000000000))*(x64)*(x67)))+(((IkReal(-1.00000000000000))*(x65)*(x66)))+(cj2)+(((IkReal(-1.00000000000000))*(x66)*(x67)))+(((IkReal(4.16000000000000))*(sj2)))+(((IkReal(-27.7333333333333))*(cj2)*(pz)))+(((IkReal(6.66666666666667))*(pz)*(sj2))));
448 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
449 {
450 continue;
451 
452 } else
453 {
454 {
455 IkReal j1array[1], cj1array[1], sj1array[1];
456 bool j1valid[1]={false};
457 _nj1 = 1;
458 IkReal x68=(sj2)*(sj2);
459 IkReal x69=(cj2)*(cj2);
460 IkReal x70=((IkReal(1.00000000000000))*(pz));
461 IkReal x71=((cj2)*(sj2));
462 if( IKabs(((gconst1)*(((IkReal(-0.624100000000000))+(((IkReal(-0.520000000000000))*(x71)))+(((IkReal(-1.64320000000000))*(sj2)))+((pz)*(pz))+(((IkReal(-0.395000000000000))*(cj2)))+(((IkReal(-1.08160000000000))*(x68)))+(((IkReal(-0.0625000000000000))*(x69))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(0.197500000000000))*(sj2)))+(((IkReal(0.150000000000000))*(pz)))+(((IkReal(-0.821600000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x70)))+(((IkReal(-1.01910000000000))*(x71)))+(((IkReal(0.260000000000000))*(x68)))+(((IkReal(-0.260000000000000))*(x69)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x70))))))) < IKFAST_ATAN2_MAGTHRESH )
463  continue;
464 j1array[0]=IKatan2(((gconst1)*(((IkReal(-0.624100000000000))+(((IkReal(-0.520000000000000))*(x71)))+(((IkReal(-1.64320000000000))*(sj2)))+((pz)*(pz))+(((IkReal(-0.395000000000000))*(cj2)))+(((IkReal(-1.08160000000000))*(x68)))+(((IkReal(-0.0625000000000000))*(x69)))))), ((gconst1)*(((((IkReal(0.197500000000000))*(sj2)))+(((IkReal(0.150000000000000))*(pz)))+(((IkReal(-0.821600000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x70)))+(((IkReal(-1.01910000000000))*(x71)))+(((IkReal(0.260000000000000))*(x68)))+(((IkReal(-0.260000000000000))*(x69)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x70)))))));
465 sj1array[0]=IKsin(j1array[0]);
466 cj1array[0]=IKcos(j1array[0]);
467 if( j1array[0] > IKPI )
468 {
469  j1array[0]-=IK2PI;
470 }
471 else if( j1array[0] < -IKPI )
472 { j1array[0]+=IK2PI;
473 }
474 j1valid[0] = true;
475 for(int ij1 = 0; ij1 < 1; ++ij1)
476 {
477 if( !j1valid[ij1] )
478 {
479  continue;
480 }
481 _ij1[0] = ij1; _ij1[1] = -1;
482 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
483 {
484 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
485 {
486  j1valid[iij1]=false; _ij1[1] = iij1; break;
487 }
488 }
489 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
490 {
491 IkReal evalcond[5];
492 IkReal x72=IKcos(j1);
493 IkReal x73=IKsin(j1);
494 IkReal x74=((IkReal(0.250000000000000))*(sj2));
495 IkReal x75=((cj0)*(px));
496 IkReal x76=((py)*(sj0));
497 IkReal x77=((IkReal(0.250000000000000))*(cj2));
498 IkReal x78=((IkReal(1.04000000000000))*(sj2));
499 IkReal x79=((IkReal(1.00000000000000))*(x72));
500 IkReal x80=((cj2)*(x72));
501 IkReal x81=((IkReal(1.04000000000000))*(x73));
502 IkReal x82=((x73)*(x75));
503 evalcond[0]=((((IkReal(-1.00000000000000))*(x76)*(x79)))+(((IkReal(-1.00000000000000))*(x74)))+(((IkReal(0.150000000000000))*(x72)))+(((IkReal(1.04000000000000))*(cj2)))+(((pz)*(x73)))+(((IkReal(-1.00000000000000))*(x75)*(x79))));
504 evalcond[1]=((((IkReal(-1.00000000000000))*(x72)*(x77)))+(((cj2)*(x81)))+(((IkReal(-1.00000000000000))*(x73)*(x74)))+(pz)+(((IkReal(-1.00000000000000))*(x72)*(x78)))+(((IkReal(-0.790000000000000))*(x72))));
505 evalcond[2]=((IkReal(0.790000000000000))+(((IkReal(-1.00000000000000))*(x82)))+(((IkReal(0.150000000000000))*(x73)))+(((IkReal(-1.00000000000000))*(x73)*(x76)))+(x77)+(x78)+(((IkReal(-1.00000000000000))*(pz)*(x79))));
506 evalcond[3]=((IkReal(0.497500000000000))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-0.237000000000000))*(x73)))+(((IkReal(0.300000000000000))*(x76)))+(((IkReal(1.58000000000000))*(x73)*(x76)))+(((IkReal(1.58000000000000))*(x82)))+(((IkReal(1.58000000000000))*(pz)*(x72)))+(((IkReal(0.300000000000000))*(x75))));
507 evalcond[4]=((IkReal(0.150000000000000))+(((x73)*(x77)))+(((x73)*(x78)))+(((IkReal(-1.00000000000000))*(x72)*(x74)))+(((IkReal(1.04000000000000))*(x80)))+(((IkReal(-1.00000000000000))*(x75)))+(((IkReal(0.790000000000000))*(x73)))+(((IkReal(-1.00000000000000))*(x76))));
508 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 )
509 {
510 continue;
511 }
512 }
513 
514 rotationfunction0(solutions);
515 }
516 }
517 
518 }
519 
520 }
521 
522 } else
523 {
524 {
525 IkReal j1array[1], cj1array[1], sj1array[1];
526 bool j1valid[1]={false};
527 _nj1 = 1;
528 IkReal x224=(cj2)*(cj2);
529 IkReal x225=(sj2)*(sj2);
530 IkReal x226=((cj2)*(sj2));
531 if( IKabs(((gconst0)*(((((IkReal(0.197500000000000))*(sj2)))+(((IkReal(-0.821600000000000))*(cj2)))+(((IkReal(0.260000000000000))*(x225)))+(((cj0)*(px)*(pz)))+(((IkReal(-1.01910000000000))*(x226)))+(((IkReal(-0.150000000000000))*(pz)))+(((IkReal(-0.260000000000000))*(x224)))+(((py)*(pz)*(sj0))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-1.08160000000000))*(x224)))+(((IkReal(-0.0625000000000000))*(x225)))+((pz)*(pz))+(((IkReal(0.520000000000000))*(x226))))))) < IKFAST_ATAN2_MAGTHRESH )
532  continue;
533 j1array[0]=IKatan2(((gconst0)*(((((IkReal(0.197500000000000))*(sj2)))+(((IkReal(-0.821600000000000))*(cj2)))+(((IkReal(0.260000000000000))*(x225)))+(((cj0)*(px)*(pz)))+(((IkReal(-1.01910000000000))*(x226)))+(((IkReal(-0.150000000000000))*(pz)))+(((IkReal(-0.260000000000000))*(x224)))+(((py)*(pz)*(sj0)))))), ((gconst0)*(((((IkReal(-1.08160000000000))*(x224)))+(((IkReal(-0.0625000000000000))*(x225)))+((pz)*(pz))+(((IkReal(0.520000000000000))*(x226)))))));
534 sj1array[0]=IKsin(j1array[0]);
535 cj1array[0]=IKcos(j1array[0]);
536 if( j1array[0] > IKPI )
537 {
538  j1array[0]-=IK2PI;
539 }
540 else if( j1array[0] < -IKPI )
541 { j1array[0]+=IK2PI;
542 }
543 j1valid[0] = true;
544 for(int ij1 = 0; ij1 < 1; ++ij1)
545 {
546 if( !j1valid[ij1] )
547 {
548  continue;
549 }
550 _ij1[0] = ij1; _ij1[1] = -1;
551 for(int iij1 = ij1+1; iij1 < 1; ++iij1)
552 {
553 if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
554 {
555  j1valid[iij1]=false; _ij1[1] = iij1; break;
556 }
557 }
558 j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
559 {
560 IkReal evalcond[5];
561 IkReal x227=IKcos(j1);
562 IkReal x228=IKsin(j1);
563 IkReal x229=((IkReal(0.250000000000000))*(sj2));
564 IkReal x230=((cj0)*(px));
565 IkReal x231=((py)*(sj0));
566 IkReal x232=((IkReal(0.250000000000000))*(cj2));
567 IkReal x233=((IkReal(1.04000000000000))*(sj2));
568 IkReal x234=((IkReal(1.00000000000000))*(x227));
569 IkReal x235=((cj2)*(x227));
570 IkReal x236=((IkReal(1.04000000000000))*(x228));
571 IkReal x237=((x228)*(x230));
572 evalcond[0]=((((IkReal(-1.00000000000000))*(x231)*(x234)))+(((pz)*(x228)))+(((IkReal(1.04000000000000))*(cj2)))+(((IkReal(-1.00000000000000))*(x230)*(x234)))+(((IkReal(0.150000000000000))*(x227)))+(((IkReal(-1.00000000000000))*(x229))));
573 evalcond[1]=((((cj2)*(x236)))+(((IkReal(-0.790000000000000))*(x227)))+(((IkReal(-1.00000000000000))*(x227)*(x233)))+(((IkReal(-1.00000000000000))*(x227)*(x232)))+(pz)+(((IkReal(-1.00000000000000))*(x228)*(x229))));
574 evalcond[2]=((IkReal(0.790000000000000))+(((IkReal(0.150000000000000))*(x228)))+(((IkReal(-1.00000000000000))*(pz)*(x234)))+(x232)+(x233)+(((IkReal(-1.00000000000000))*(x237)))+(((IkReal(-1.00000000000000))*(x228)*(x231))));
575 evalcond[3]=((IkReal(0.497500000000000))+(((IkReal(0.300000000000000))*(x231)))+(((IkReal(-0.237000000000000))*(x228)))+(((IkReal(1.58000000000000))*(pz)*(x227)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(1.58000000000000))*(x237)))+(((IkReal(1.58000000000000))*(x228)*(x231)))+(((IkReal(0.300000000000000))*(x230))));
576 evalcond[4]=((IkReal(0.150000000000000))+(((IkReal(-1.00000000000000))*(x231)))+(((x228)*(x232)))+(((x228)*(x233)))+(((IkReal(-1.00000000000000))*(x230)))+(((IkReal(1.04000000000000))*(x235)))+(((IkReal(-1.00000000000000))*(x227)*(x229)))+(((IkReal(0.790000000000000))*(x228))));
577 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 )
578 {
579 continue;
580 }
581 }
582 
583 rotationfunction0(solutions);
584 }
585 }
586 
587 }
588 
589 }
590 }
591 }
592 }
593 }
594 }
595 return solutions.GetNumSolutions()>0;
596 }
597 inline void rotationfunction0(IkSolutionListBase<IkReal>& solutions) {
598 for(int rotationiter = 0; rotationiter < 1; ++rotationiter) {
599 IkReal x83=((r11)*(sj0));
600 IkReal x84=((cj0)*(r02));
601 IkReal x85=((sj1)*(sj2));
602 IkReal x86=((r10)*(sj0));
603 IkReal x87=((IkReal(1.00000000000000))*(cj1));
604 IkReal x88=((IkReal(1.00000000000000))*(sj0));
605 IkReal x89=((cj0)*(r00));
606 IkReal x90=((r12)*(sj0));
607 IkReal x91=((cj0)*(r01));
608 IkReal x92=((((IkReal(-1.00000000000000))*(sj2)*(x87)))+(((cj2)*(sj1))));
609 IkReal x93=((((cj1)*(cj2)))+(x85));
610 IkReal x94=((sj0)*(x92));
611 IkReal x95=((cj0)*(x92));
612 IkReal x96=((((IkReal(-1.00000000000000))*(cj2)*(x87)))+(((IkReal(-1.00000000000000))*(x85))));
613 new_r00=((((x89)*(x92)))+(((r20)*(x93)))+(((x86)*(x92))));
614 new_r01=((((x83)*(x92)))+(((r21)*(x93)))+(((x91)*(x92))));
615 new_r02=((((x84)*(x92)))+(((r22)*(x93)))+(((x90)*(x92))));
616 new_r10=((((IkReal(-1.00000000000000))*(r00)*(x88)))+(((cj0)*(r10))));
617 new_r11=((((cj0)*(r11)))+(((IkReal(-1.00000000000000))*(r01)*(x88))));
618 new_r12=((((IkReal(-1.00000000000000))*(r02)*(x88)))+(((cj0)*(r12))));
619 new_r20=((((x89)*(x96)))+(((r20)*(x92)))+(((x86)*(x96))));
620 new_r21=((((x83)*(x96)))+(((r21)*(x92)))+(((x91)*(x96))));
621 new_r22=((((r22)*(x92)))+(((x90)*(x96)))+(((x84)*(x96))));
622 {
623 IkReal j4array[2], cj4array[2], sj4array[2];
624 bool j4valid[2]={false};
625 _nj4 = 2;
626 cj4array[0]=new_r22;
627 if( cj4array[0] >= -1-IKFAST_SINCOS_THRESH && cj4array[0] <= 1+IKFAST_SINCOS_THRESH )
628 {
629  j4valid[0] = j4valid[1] = true;
630  j4array[0] = IKacos(cj4array[0]);
631  sj4array[0] = IKsin(j4array[0]);
632  cj4array[1] = cj4array[0];
633  j4array[1] = -j4array[0];
634  sj4array[1] = -sj4array[0];
635 }
636 else if( isnan(cj4array[0]) )
637 {
638  // probably any value will work
639  j4valid[0] = true;
640  cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
641 }
642 for(int ij4 = 0; ij4 < 2; ++ij4)
643 {
644 if( !j4valid[ij4] )
645 {
646  continue;
647 }
648 _ij4[0] = ij4; _ij4[1] = -1;
649 for(int iij4 = ij4+1; iij4 < 2; ++iij4)
650 {
651 if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
652 {
653  j4valid[iij4]=false; _ij4[1] = iij4; break;
654 }
655 }
656 j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];
657 
658 {
659 IkReal dummyeval[1];
660 IkReal gconst4;
661 gconst4=IKsign(sj4);
662 dummyeval[0]=sj4;
663 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
664 {
665 {
666 IkReal dummyeval[1];
667 IkReal gconst2;
668 gconst2=IKsign(((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12))))));
669 dummyeval[0]=((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12)))));
670 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
671 {
672 {
673 IkReal dummyeval[1];
674 IkReal gconst3;
675 gconst3=IKsign(((((cj4)*((new_r02)*(new_r02))))+(((cj4)*((new_r12)*(new_r12))))));
676 dummyeval[0]=((((cj4)*((new_r02)*(new_r02))))+(((cj4)*((new_r12)*(new_r12)))));
677 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
678 {
679 {
680 IkReal evalcond[7];
681 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
682 evalcond[1]=new_r22;
683 evalcond[2]=new_r22;
684 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 )
685 {
686 {
687 IkReal j5array[1], cj5array[1], sj5array[1];
688 bool j5valid[1]={false};
689 _nj5 = 1;
690 if( IKabs(((IkReal(-1.00000000000000))*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r20) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-1.00000000000000))*(new_r21)))+IKsqr(new_r20)-1) <= IKFAST_SINCOS_THRESH )
691  continue;
692 j5array[0]=IKatan2(((IkReal(-1.00000000000000))*(new_r21)), new_r20);
693 sj5array[0]=IKsin(j5array[0]);
694 cj5array[0]=IKcos(j5array[0]);
695 if( j5array[0] > IKPI )
696 {
697  j5array[0]-=IK2PI;
698 }
699 else if( j5array[0] < -IKPI )
700 { j5array[0]+=IK2PI;
701 }
702 j5valid[0] = true;
703 for(int ij5 = 0; ij5 < 1; ++ij5)
704 {
705 if( !j5valid[ij5] )
706 {
707  continue;
708 }
709 _ij5[0] = ij5; _ij5[1] = -1;
710 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
711 {
712 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
713 {
714  j5valid[iij5]=false; _ij5[1] = iij5; break;
715 }
716 }
717 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
718 {
719 IkReal evalcond[2];
720 evalcond[0]=((new_r21)+(IKsin(j5)));
721 evalcond[1]=((((IkReal(-1.00000000000000))*(IKcos(j5))))+(new_r20));
722 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
723 {
724 continue;
725 }
726 }
727 
728 {
729 IkReal dummyeval[1];
730 IkReal gconst10;
731 gconst10=IKsign(((((IkReal(-1.00000000000000))*(new_r01)*(new_r12)))+(((new_r02)*(new_r11)))));
732 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r01)*(new_r12)))+(((new_r02)*(new_r11))));
733 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
734 {
735 {
736 IkReal dummyeval[1];
737 IkReal gconst11;
738 gconst11=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))));
739 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))));
740 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
741 {
742 continue;
743 
744 } else
745 {
746 {
747 IkReal j3array[1], cj3array[1], sj3array[1];
748 bool j3valid[1]={false};
749 _nj3 = 1;
750 if( IKabs(((gconst11)*(new_r00))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst11)*(new_r10))) < IKFAST_ATAN2_MAGTHRESH )
751  continue;
752 j3array[0]=IKatan2(((gconst11)*(new_r00)), ((IkReal(-1.00000000000000))*(gconst11)*(new_r10)));
753 sj3array[0]=IKsin(j3array[0]);
754 cj3array[0]=IKcos(j3array[0]);
755 if( j3array[0] > IKPI )
756 {
757  j3array[0]-=IK2PI;
758 }
759 else if( j3array[0] < -IKPI )
760 { j3array[0]+=IK2PI;
761 }
762 j3valid[0] = true;
763 for(int ij3 = 0; ij3 < 1; ++ij3)
764 {
765 if( !j3valid[ij3] )
766 {
767  continue;
768 }
769 _ij3[0] = ij3; _ij3[1] = -1;
770 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
771 {
772 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
773 {
774  j3valid[iij3]=false; _ij3[1] = iij3; break;
775 }
776 }
777 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
778 {
779 IkReal evalcond[6];
780 IkReal x97=IKsin(j3);
781 IkReal x98=IKcos(j3);
782 IkReal x99=((IkReal(1.00000000000000))*(x98));
783 evalcond[0]=((((new_r12)*(x98)))+(((IkReal(-1.00000000000000))*(new_r02)*(x97))));
784 evalcond[1]=((IkReal(1.00000000000000))+(((new_r02)*(x98)))+(((new_r12)*(x97))));
785 evalcond[2]=((sj5)+(((IkReal(-1.00000000000000))*(new_r10)*(x99)))+(((new_r00)*(x97))));
786 evalcond[3]=((((new_r01)*(x97)))+(((IkReal(-1.00000000000000))*(new_r11)*(x99)))+(cj5));
787 evalcond[4]=((((new_r01)*(x98)))+(((new_r11)*(x97))));
788 evalcond[5]=((((new_r00)*(x98)))+(((new_r10)*(x97))));
789 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 )
790 {
791 continue;
792 }
793 }
794 
795 {
796 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
797 vinfos[0].jointtype = 1;
798 vinfos[0].foffset = j0;
799 vinfos[0].indices[0] = _ij0[0];
800 vinfos[0].indices[1] = _ij0[1];
801 vinfos[0].maxsolutions = _nj0;
802 vinfos[1].jointtype = 1;
803 vinfos[1].foffset = j1;
804 vinfos[1].indices[0] = _ij1[0];
805 vinfos[1].indices[1] = _ij1[1];
806 vinfos[1].maxsolutions = _nj1;
807 vinfos[2].jointtype = 1;
808 vinfos[2].foffset = j2;
809 vinfos[2].indices[0] = _ij2[0];
810 vinfos[2].indices[1] = _ij2[1];
811 vinfos[2].maxsolutions = _nj2;
812 vinfos[3].jointtype = 1;
813 vinfos[3].foffset = j3;
814 vinfos[3].indices[0] = _ij3[0];
815 vinfos[3].indices[1] = _ij3[1];
816 vinfos[3].maxsolutions = _nj3;
817 vinfos[4].jointtype = 1;
818 vinfos[4].foffset = j4;
819 vinfos[4].indices[0] = _ij4[0];
820 vinfos[4].indices[1] = _ij4[1];
821 vinfos[4].maxsolutions = _nj4;
822 vinfos[5].jointtype = 1;
823 vinfos[5].foffset = j5;
824 vinfos[5].indices[0] = _ij5[0];
825 vinfos[5].indices[1] = _ij5[1];
826 vinfos[5].maxsolutions = _nj5;
827 std::vector<int> vfree(0);
828 solutions.AddSolution(vinfos,vfree);
829 }
830 }
831 }
832 
833 }
834 
835 }
836 
837 } else
838 {
839 {
840 IkReal j3array[1], cj3array[1], sj3array[1];
841 bool j3valid[1]={false};
842 _nj3 = 1;
843 if( IKabs(((gconst10)*(new_r01))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst10)*(new_r11))) < IKFAST_ATAN2_MAGTHRESH )
844  continue;
845 j3array[0]=IKatan2(((gconst10)*(new_r01)), ((IkReal(-1.00000000000000))*(gconst10)*(new_r11)));
846 sj3array[0]=IKsin(j3array[0]);
847 cj3array[0]=IKcos(j3array[0]);
848 if( j3array[0] > IKPI )
849 {
850  j3array[0]-=IK2PI;
851 }
852 else if( j3array[0] < -IKPI )
853 { j3array[0]+=IK2PI;
854 }
855 j3valid[0] = true;
856 for(int ij3 = 0; ij3 < 1; ++ij3)
857 {
858 if( !j3valid[ij3] )
859 {
860  continue;
861 }
862 _ij3[0] = ij3; _ij3[1] = -1;
863 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
864 {
865 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
866 {
867  j3valid[iij3]=false; _ij3[1] = iij3; break;
868 }
869 }
870 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
871 {
872 IkReal evalcond[6];
873 IkReal x100=IKsin(j3);
874 IkReal x101=IKcos(j3);
875 IkReal x102=((IkReal(1.00000000000000))*(x101));
876 evalcond[0]=((((new_r12)*(x101)))+(((IkReal(-1.00000000000000))*(new_r02)*(x100))));
877 evalcond[1]=((IkReal(1.00000000000000))+(((new_r12)*(x100)))+(((new_r02)*(x101))));
878 evalcond[2]=((sj5)+(((new_r00)*(x100)))+(((IkReal(-1.00000000000000))*(new_r10)*(x102))));
879 evalcond[3]=((((new_r01)*(x100)))+(cj5)+(((IkReal(-1.00000000000000))*(new_r11)*(x102))));
880 evalcond[4]=((((new_r01)*(x101)))+(((new_r11)*(x100))));
881 evalcond[5]=((((new_r00)*(x101)))+(((new_r10)*(x100))));
882 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 )
883 {
884 continue;
885 }
886 }
887 
888 {
889 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
890 vinfos[0].jointtype = 1;
891 vinfos[0].foffset = j0;
892 vinfos[0].indices[0] = _ij0[0];
893 vinfos[0].indices[1] = _ij0[1];
894 vinfos[0].maxsolutions = _nj0;
895 vinfos[1].jointtype = 1;
896 vinfos[1].foffset = j1;
897 vinfos[1].indices[0] = _ij1[0];
898 vinfos[1].indices[1] = _ij1[1];
899 vinfos[1].maxsolutions = _nj1;
900 vinfos[2].jointtype = 1;
901 vinfos[2].foffset = j2;
902 vinfos[2].indices[0] = _ij2[0];
903 vinfos[2].indices[1] = _ij2[1];
904 vinfos[2].maxsolutions = _nj2;
905 vinfos[3].jointtype = 1;
906 vinfos[3].foffset = j3;
907 vinfos[3].indices[0] = _ij3[0];
908 vinfos[3].indices[1] = _ij3[1];
909 vinfos[3].maxsolutions = _nj3;
910 vinfos[4].jointtype = 1;
911 vinfos[4].foffset = j4;
912 vinfos[4].indices[0] = _ij4[0];
913 vinfos[4].indices[1] = _ij4[1];
914 vinfos[4].maxsolutions = _nj4;
915 vinfos[5].jointtype = 1;
916 vinfos[5].foffset = j5;
917 vinfos[5].indices[0] = _ij5[0];
918 vinfos[5].indices[1] = _ij5[1];
919 vinfos[5].maxsolutions = _nj5;
920 std::vector<int> vfree(0);
921 solutions.AddSolution(vinfos,vfree);
922 }
923 }
924 }
925 
926 }
927 
928 }
929 }
930 }
931 
932 } else
933 {
934 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
935 evalcond[1]=new_r22;
936 evalcond[2]=((IkReal(-1.00000000000000))*(new_r22));
937 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 )
938 {
939 {
940 IkReal j5array[1], cj5array[1], sj5array[1];
941 bool j5valid[1]={false};
942 _nj5 = 1;
943 if( IKabs(new_r21) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(new_r20))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(new_r21)+IKsqr(((IkReal(-1.00000000000000))*(new_r20)))-1) <= IKFAST_SINCOS_THRESH )
944  continue;
945 j5array[0]=IKatan2(new_r21, ((IkReal(-1.00000000000000))*(new_r20)));
946 sj5array[0]=IKsin(j5array[0]);
947 cj5array[0]=IKcos(j5array[0]);
948 if( j5array[0] > IKPI )
949 {
950  j5array[0]-=IK2PI;
951 }
952 else if( j5array[0] < -IKPI )
953 { j5array[0]+=IK2PI;
954 }
955 j5valid[0] = true;
956 for(int ij5 = 0; ij5 < 1; ++ij5)
957 {
958 if( !j5valid[ij5] )
959 {
960  continue;
961 }
962 _ij5[0] = ij5; _ij5[1] = -1;
963 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
964 {
965 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
966 {
967  j5valid[iij5]=false; _ij5[1] = iij5; break;
968 }
969 }
970 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
971 {
972 IkReal evalcond[2];
973 evalcond[0]=((new_r21)+(((IkReal(-1.00000000000000))*(IKsin(j5)))));
974 evalcond[1]=((IKcos(j5))+(new_r20));
975 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
976 {
977 continue;
978 }
979 }
980 
981 {
982 IkReal dummyeval[1];
983 IkReal gconst14;
984 gconst14=IKsign(((((IkReal(-1.00000000000000))*(new_r02)*(new_r11)))+(((new_r01)*(new_r12)))));
985 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r02)*(new_r11)))+(((new_r01)*(new_r12))));
986 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
987 {
988 {
989 IkReal dummyeval[1];
990 IkReal gconst15;
991 gconst15=IKsign(((((new_r00)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(new_r10)))));
992 dummyeval[0]=((((new_r00)*(new_r12)))+(((IkReal(-1.00000000000000))*(new_r02)*(new_r10))));
993 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
994 {
995 continue;
996 
997 } else
998 {
999 {
1000 IkReal j3array[1], cj3array[1], sj3array[1];
1001 bool j3valid[1]={false};
1002 _nj3 = 1;
1003 if( IKabs(((gconst15)*(new_r00))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst15)*(new_r10))) < IKFAST_ATAN2_MAGTHRESH )
1004  continue;
1005 j3array[0]=IKatan2(((gconst15)*(new_r00)), ((IkReal(-1.00000000000000))*(gconst15)*(new_r10)));
1006 sj3array[0]=IKsin(j3array[0]);
1007 cj3array[0]=IKcos(j3array[0]);
1008 if( j3array[0] > IKPI )
1009 {
1010  j3array[0]-=IK2PI;
1011 }
1012 else if( j3array[0] < -IKPI )
1013 { j3array[0]+=IK2PI;
1014 }
1015 j3valid[0] = true;
1016 for(int ij3 = 0; ij3 < 1; ++ij3)
1017 {
1018 if( !j3valid[ij3] )
1019 {
1020  continue;
1021 }
1022 _ij3[0] = ij3; _ij3[1] = -1;
1023 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
1024 {
1025 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1026 {
1027  j3valid[iij3]=false; _ij3[1] = iij3; break;
1028 }
1029 }
1030 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1031 {
1032 IkReal evalcond[6];
1033 IkReal x103=IKsin(j3);
1034 IkReal x104=IKcos(j3);
1035 IkReal x105=((IkReal(1.00000000000000))*(x104));
1036 evalcond[0]=((((new_r12)*(x104)))+(((IkReal(-1.00000000000000))*(new_r02)*(x103))));
1037 evalcond[1]=((IkReal(-1.00000000000000))+(((new_r12)*(x103)))+(((new_r02)*(x104))));
1038 evalcond[2]=((((new_r00)*(x103)))+(sj5)+(((IkReal(-1.00000000000000))*(new_r10)*(x105))));
1039 evalcond[3]=((((new_r01)*(x103)))+(cj5)+(((IkReal(-1.00000000000000))*(new_r11)*(x105))));
1040 evalcond[4]=((((new_r11)*(x103)))+(((new_r01)*(x104))));
1041 evalcond[5]=((((new_r10)*(x103)))+(((new_r00)*(x104))));
1042 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 )
1043 {
1044 continue;
1045 }
1046 }
1047 
1048 {
1049 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1050 vinfos[0].jointtype = 1;
1051 vinfos[0].foffset = j0;
1052 vinfos[0].indices[0] = _ij0[0];
1053 vinfos[0].indices[1] = _ij0[1];
1054 vinfos[0].maxsolutions = _nj0;
1055 vinfos[1].jointtype = 1;
1056 vinfos[1].foffset = j1;
1057 vinfos[1].indices[0] = _ij1[0];
1058 vinfos[1].indices[1] = _ij1[1];
1059 vinfos[1].maxsolutions = _nj1;
1060 vinfos[2].jointtype = 1;
1061 vinfos[2].foffset = j2;
1062 vinfos[2].indices[0] = _ij2[0];
1063 vinfos[2].indices[1] = _ij2[1];
1064 vinfos[2].maxsolutions = _nj2;
1065 vinfos[3].jointtype = 1;
1066 vinfos[3].foffset = j3;
1067 vinfos[3].indices[0] = _ij3[0];
1068 vinfos[3].indices[1] = _ij3[1];
1069 vinfos[3].maxsolutions = _nj3;
1070 vinfos[4].jointtype = 1;
1071 vinfos[4].foffset = j4;
1072 vinfos[4].indices[0] = _ij4[0];
1073 vinfos[4].indices[1] = _ij4[1];
1074 vinfos[4].maxsolutions = _nj4;
1075 vinfos[5].jointtype = 1;
1076 vinfos[5].foffset = j5;
1077 vinfos[5].indices[0] = _ij5[0];
1078 vinfos[5].indices[1] = _ij5[1];
1079 vinfos[5].maxsolutions = _nj5;
1080 std::vector<int> vfree(0);
1081 solutions.AddSolution(vinfos,vfree);
1082 }
1083 }
1084 }
1085 
1086 }
1087 
1088 }
1089 
1090 } else
1091 {
1092 {
1093 IkReal j3array[1], cj3array[1], sj3array[1];
1094 bool j3valid[1]={false};
1095 _nj3 = 1;
1096 if( IKabs(((gconst14)*(new_r01))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(gconst14)*(new_r11))) < IKFAST_ATAN2_MAGTHRESH )
1097  continue;
1098 j3array[0]=IKatan2(((gconst14)*(new_r01)), ((IkReal(-1.00000000000000))*(gconst14)*(new_r11)));
1099 sj3array[0]=IKsin(j3array[0]);
1100 cj3array[0]=IKcos(j3array[0]);
1101 if( j3array[0] > IKPI )
1102 {
1103  j3array[0]-=IK2PI;
1104 }
1105 else if( j3array[0] < -IKPI )
1106 { j3array[0]+=IK2PI;
1107 }
1108 j3valid[0] = true;
1109 for(int ij3 = 0; ij3 < 1; ++ij3)
1110 {
1111 if( !j3valid[ij3] )
1112 {
1113  continue;
1114 }
1115 _ij3[0] = ij3; _ij3[1] = -1;
1116 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
1117 {
1118 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1119 {
1120  j3valid[iij3]=false; _ij3[1] = iij3; break;
1121 }
1122 }
1123 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1124 {
1125 IkReal evalcond[6];
1126 IkReal x106=IKsin(j3);
1127 IkReal x107=IKcos(j3);
1128 IkReal x108=((IkReal(1.00000000000000))*(x107));
1129 evalcond[0]=((((new_r12)*(x107)))+(((IkReal(-1.00000000000000))*(new_r02)*(x106))));
1130 evalcond[1]=((IkReal(-1.00000000000000))+(((new_r12)*(x106)))+(((new_r02)*(x107))));
1131 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r10)*(x108)))+(sj5)+(((new_r00)*(x106))));
1132 evalcond[3]=((cj5)+(((IkReal(-1.00000000000000))*(new_r11)*(x108)))+(((new_r01)*(x106))));
1133 evalcond[4]=((((new_r01)*(x107)))+(((new_r11)*(x106))));
1134 evalcond[5]=((((new_r10)*(x106)))+(((new_r00)*(x107))));
1135 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 )
1136 {
1137 continue;
1138 }
1139 }
1140 
1141 {
1142 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1143 vinfos[0].jointtype = 1;
1144 vinfos[0].foffset = j0;
1145 vinfos[0].indices[0] = _ij0[0];
1146 vinfos[0].indices[1] = _ij0[1];
1147 vinfos[0].maxsolutions = _nj0;
1148 vinfos[1].jointtype = 1;
1149 vinfos[1].foffset = j1;
1150 vinfos[1].indices[0] = _ij1[0];
1151 vinfos[1].indices[1] = _ij1[1];
1152 vinfos[1].maxsolutions = _nj1;
1153 vinfos[2].jointtype = 1;
1154 vinfos[2].foffset = j2;
1155 vinfos[2].indices[0] = _ij2[0];
1156 vinfos[2].indices[1] = _ij2[1];
1157 vinfos[2].maxsolutions = _nj2;
1158 vinfos[3].jointtype = 1;
1159 vinfos[3].foffset = j3;
1160 vinfos[3].indices[0] = _ij3[0];
1161 vinfos[3].indices[1] = _ij3[1];
1162 vinfos[3].maxsolutions = _nj3;
1163 vinfos[4].jointtype = 1;
1164 vinfos[4].foffset = j4;
1165 vinfos[4].indices[0] = _ij4[0];
1166 vinfos[4].indices[1] = _ij4[1];
1167 vinfos[4].maxsolutions = _nj4;
1168 vinfos[5].jointtype = 1;
1169 vinfos[5].foffset = j5;
1170 vinfos[5].indices[0] = _ij5[0];
1171 vinfos[5].indices[1] = _ij5[1];
1172 vinfos[5].maxsolutions = _nj5;
1173 std::vector<int> vfree(0);
1174 solutions.AddSolution(vinfos,vfree);
1175 }
1176 }
1177 }
1178 
1179 }
1180 
1181 }
1182 }
1183 }
1184 
1185 } else
1186 {
1187 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
1188 evalcond[1]=((IkReal(-1.00000000000000))+(new_r22));
1189 evalcond[2]=new_r21;
1190 evalcond[3]=new_r20;
1191 evalcond[4]=((IkReal(-1.00000000000000))*(new_r20));
1192 evalcond[5]=((IkReal(-1.00000000000000))*(new_r21));
1193 evalcond[6]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
1194 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 )
1195 {
1196 {
1197 IkReal j3array[2], cj3array[2], sj3array[2];
1198 bool j3valid[2]={false};
1199 _nj3 = 2;
1200 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
1201  continue;
1202 IkReal x109=IKatan2(new_r02, new_r12);
1203 j3array[0]=((IkReal(-1.00000000000000))*(x109));
1204 sj3array[0]=IKsin(j3array[0]);
1205 cj3array[0]=IKcos(j3array[0]);
1206 j3array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x109))));
1207 sj3array[1]=IKsin(j3array[1]);
1208 cj3array[1]=IKcos(j3array[1]);
1209 if( j3array[0] > IKPI )
1210 {
1211  j3array[0]-=IK2PI;
1212 }
1213 else if( j3array[0] < -IKPI )
1214 { j3array[0]+=IK2PI;
1215 }
1216 j3valid[0] = true;
1217 if( j3array[1] > IKPI )
1218 {
1219  j3array[1]-=IK2PI;
1220 }
1221 else if( j3array[1] < -IKPI )
1222 { j3array[1]+=IK2PI;
1223 }
1224 j3valid[1] = true;
1225 for(int ij3 = 0; ij3 < 2; ++ij3)
1226 {
1227 if( !j3valid[ij3] )
1228 {
1229  continue;
1230 }
1231 _ij3[0] = ij3; _ij3[1] = -1;
1232 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
1233 {
1234 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1235 {
1236  j3valid[iij3]=false; _ij3[1] = iij3; break;
1237 }
1238 }
1239 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1240 {
1241 IkReal evalcond[1];
1242 evalcond[0]=((((new_r12)*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j3)))));
1243 if( IKabs(evalcond[0]) > 0.000001 )
1244 {
1245 continue;
1246 }
1247 }
1248 
1249 {
1250 IkReal j5array[1], cj5array[1], sj5array[1];
1251 bool j5valid[1]={false};
1252 _nj5 = 1;
1253 IkReal x110=((IkReal(1.00000000000000))*(sj3));
1254 if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(x110)))+(((cj3)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(x110)))+(((cj3)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(x110)))+(((cj3)*(new_r10)))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(x110)))+(((cj3)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
1255  continue;
1256 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(x110)))+(((cj3)*(new_r10)))), ((((IkReal(-1.00000000000000))*(new_r01)*(x110)))+(((cj3)*(new_r11)))));
1257 sj5array[0]=IKsin(j5array[0]);
1258 cj5array[0]=IKcos(j5array[0]);
1259 if( j5array[0] > IKPI )
1260 {
1261  j5array[0]-=IK2PI;
1262 }
1263 else if( j5array[0] < -IKPI )
1264 { j5array[0]+=IK2PI;
1265 }
1266 j5valid[0] = true;
1267 for(int ij5 = 0; ij5 < 1; ++ij5)
1268 {
1269 if( !j5valid[ij5] )
1270 {
1271  continue;
1272 }
1273 _ij5[0] = ij5; _ij5[1] = -1;
1274 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1275 {
1276 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1277 {
1278  j5valid[iij5]=false; _ij5[1] = iij5; break;
1279 }
1280 }
1281 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1282 {
1283 IkReal evalcond[4];
1284 IkReal x111=IKsin(j5);
1285 IkReal x112=IKcos(j5);
1286 IkReal x113=((IkReal(1.00000000000000))*(cj3));
1287 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r10)*(x113)))+(x111)+(((new_r00)*(sj3))));
1288 evalcond[1]=((((new_r01)*(sj3)))+(((IkReal(-1.00000000000000))*(new_r11)*(x113)))+(x112));
1289 evalcond[2]=((((new_r11)*(sj3)))+(x111)+(((cj3)*(new_r01))));
1290 evalcond[3]=((((IkReal(-1.00000000000000))*(x112)))+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
1291 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1292 {
1293 continue;
1294 }
1295 }
1296 
1297 {
1298 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1299 vinfos[0].jointtype = 1;
1300 vinfos[0].foffset = j0;
1301 vinfos[0].indices[0] = _ij0[0];
1302 vinfos[0].indices[1] = _ij0[1];
1303 vinfos[0].maxsolutions = _nj0;
1304 vinfos[1].jointtype = 1;
1305 vinfos[1].foffset = j1;
1306 vinfos[1].indices[0] = _ij1[0];
1307 vinfos[1].indices[1] = _ij1[1];
1308 vinfos[1].maxsolutions = _nj1;
1309 vinfos[2].jointtype = 1;
1310 vinfos[2].foffset = j2;
1311 vinfos[2].indices[0] = _ij2[0];
1312 vinfos[2].indices[1] = _ij2[1];
1313 vinfos[2].maxsolutions = _nj2;
1314 vinfos[3].jointtype = 1;
1315 vinfos[3].foffset = j3;
1316 vinfos[3].indices[0] = _ij3[0];
1317 vinfos[3].indices[1] = _ij3[1];
1318 vinfos[3].maxsolutions = _nj3;
1319 vinfos[4].jointtype = 1;
1320 vinfos[4].foffset = j4;
1321 vinfos[4].indices[0] = _ij4[0];
1322 vinfos[4].indices[1] = _ij4[1];
1323 vinfos[4].maxsolutions = _nj4;
1324 vinfos[5].jointtype = 1;
1325 vinfos[5].foffset = j5;
1326 vinfos[5].indices[0] = _ij5[0];
1327 vinfos[5].indices[1] = _ij5[1];
1328 vinfos[5].maxsolutions = _nj5;
1329 std::vector<int> vfree(0);
1330 solutions.AddSolution(vinfos,vfree);
1331 }
1332 }
1333 }
1334 }
1335 }
1336 
1337 } else
1338 {
1339 IkReal x114=((IkReal(1.00000000000000))+(new_r22));
1340 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959))));
1341 evalcond[1]=x114;
1342 evalcond[2]=new_r21;
1343 evalcond[3]=new_r20;
1344 evalcond[4]=new_r20;
1345 evalcond[5]=new_r21;
1346 evalcond[6]=x114;
1347 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 )
1348 {
1349 {
1350 IkReal j3array[2], cj3array[2], sj3array[2];
1351 bool j3valid[2]={false};
1352 _nj3 = 2;
1353 if( IKabs(new_r02) < IKFAST_ATAN2_MAGTHRESH && IKabs(new_r12) < IKFAST_ATAN2_MAGTHRESH )
1354  continue;
1355 IkReal x115=IKatan2(new_r02, new_r12);
1356 j3array[0]=((IkReal(-1.00000000000000))*(x115));
1357 sj3array[0]=IKsin(j3array[0]);
1358 cj3array[0]=IKcos(j3array[0]);
1359 j3array[1]=((IkReal(3.14159265358979))+(((IkReal(-1.00000000000000))*(x115))));
1360 sj3array[1]=IKsin(j3array[1]);
1361 cj3array[1]=IKcos(j3array[1]);
1362 if( j3array[0] > IKPI )
1363 {
1364  j3array[0]-=IK2PI;
1365 }
1366 else if( j3array[0] < -IKPI )
1367 { j3array[0]+=IK2PI;
1368 }
1369 j3valid[0] = true;
1370 if( j3array[1] > IKPI )
1371 {
1372  j3array[1]-=IK2PI;
1373 }
1374 else if( j3array[1] < -IKPI )
1375 { j3array[1]+=IK2PI;
1376 }
1377 j3valid[1] = true;
1378 for(int ij3 = 0; ij3 < 2; ++ij3)
1379 {
1380 if( !j3valid[ij3] )
1381 {
1382  continue;
1383 }
1384 _ij3[0] = ij3; _ij3[1] = -1;
1385 for(int iij3 = ij3+1; iij3 < 2; ++iij3)
1386 {
1387 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1388 {
1389  j3valid[iij3]=false; _ij3[1] = iij3; break;
1390 }
1391 }
1392 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1393 {
1394 IkReal evalcond[1];
1395 evalcond[0]=((((new_r12)*(IKcos(j3))))+(((IkReal(-1.00000000000000))*(new_r02)*(IKsin(j3)))));
1396 if( IKabs(evalcond[0]) > 0.000001 )
1397 {
1398 continue;
1399 }
1400 }
1401 
1402 {
1403 IkReal j5array[1], cj5array[1], sj5array[1];
1404 bool j5valid[1]={false};
1405 _nj5 = 1;
1406 IkReal x116=((IkReal(1.00000000000000))*(sj3));
1407 if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(x116)))+(((cj3)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x116))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(x116)))+(((cj3)*(new_r10)))))+IKsqr(((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x116)))))-1) <= IKFAST_SINCOS_THRESH )
1408  continue;
1409 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(x116)))+(((cj3)*(new_r10)))), ((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x116)))));
1410 sj5array[0]=IKsin(j5array[0]);
1411 cj5array[0]=IKcos(j5array[0]);
1412 if( j5array[0] > IKPI )
1413 {
1414  j5array[0]-=IK2PI;
1415 }
1416 else if( j5array[0] < -IKPI )
1417 { j5array[0]+=IK2PI;
1418 }
1419 j5valid[0] = true;
1420 for(int ij5 = 0; ij5 < 1; ++ij5)
1421 {
1422 if( !j5valid[ij5] )
1423 {
1424  continue;
1425 }
1426 _ij5[0] = ij5; _ij5[1] = -1;
1427 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1428 {
1429 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1430 {
1431  j5valid[iij5]=false; _ij5[1] = iij5; break;
1432 }
1433 }
1434 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1435 {
1436 IkReal evalcond[4];
1437 IkReal x117=IKsin(j5);
1438 IkReal x118=IKcos(j5);
1439 IkReal x119=((IkReal(1.00000000000000))*(cj3));
1440 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r10)*(x119)))+(x117)+(((new_r00)*(sj3))));
1441 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r11)*(x119)))+(((new_r01)*(sj3)))+(x118));
1442 evalcond[2]=((((IkReal(-1.00000000000000))*(x117)))+(((new_r11)*(sj3)))+(((cj3)*(new_r01))));
1443 evalcond[3]=((((new_r10)*(sj3)))+(x118)+(((cj3)*(new_r00))));
1444 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1445 {
1446 continue;
1447 }
1448 }
1449 
1450 {
1451 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1452 vinfos[0].jointtype = 1;
1453 vinfos[0].foffset = j0;
1454 vinfos[0].indices[0] = _ij0[0];
1455 vinfos[0].indices[1] = _ij0[1];
1456 vinfos[0].maxsolutions = _nj0;
1457 vinfos[1].jointtype = 1;
1458 vinfos[1].foffset = j1;
1459 vinfos[1].indices[0] = _ij1[0];
1460 vinfos[1].indices[1] = _ij1[1];
1461 vinfos[1].maxsolutions = _nj1;
1462 vinfos[2].jointtype = 1;
1463 vinfos[2].foffset = j2;
1464 vinfos[2].indices[0] = _ij2[0];
1465 vinfos[2].indices[1] = _ij2[1];
1466 vinfos[2].maxsolutions = _nj2;
1467 vinfos[3].jointtype = 1;
1468 vinfos[3].foffset = j3;
1469 vinfos[3].indices[0] = _ij3[0];
1470 vinfos[3].indices[1] = _ij3[1];
1471 vinfos[3].maxsolutions = _nj3;
1472 vinfos[4].jointtype = 1;
1473 vinfos[4].foffset = j4;
1474 vinfos[4].indices[0] = _ij4[0];
1475 vinfos[4].indices[1] = _ij4[1];
1476 vinfos[4].maxsolutions = _nj4;
1477 vinfos[5].jointtype = 1;
1478 vinfos[5].foffset = j5;
1479 vinfos[5].indices[0] = _ij5[0];
1480 vinfos[5].indices[1] = _ij5[1];
1481 vinfos[5].maxsolutions = _nj5;
1482 std::vector<int> vfree(0);
1483 solutions.AddSolution(vinfos,vfree);
1484 }
1485 }
1486 }
1487 }
1488 }
1489 
1490 } else
1491 {
1492 if( 1 )
1493 {
1494 continue;
1495 
1496 } else
1497 {
1498 }
1499 }
1500 }
1501 }
1502 }
1503 }
1504 
1505 } else
1506 {
1507 {
1508 IkReal j3array[1], cj3array[1], sj3array[1];
1509 bool j3valid[1]={false};
1510 _nj3 = 1;
1511 IkReal x120=((IkReal(-1.00000000000000))*(gconst3)*(new_r22)*(sj4));
1512 if( IKabs(((new_r12)*(x120))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x120))) < IKFAST_ATAN2_MAGTHRESH )
1513  continue;
1514 j3array[0]=IKatan2(((new_r12)*(x120)), ((new_r02)*(x120)));
1515 sj3array[0]=IKsin(j3array[0]);
1516 cj3array[0]=IKcos(j3array[0]);
1517 if( j3array[0] > IKPI )
1518 {
1519  j3array[0]-=IK2PI;
1520 }
1521 else if( j3array[0] < -IKPI )
1522 { j3array[0]+=IK2PI;
1523 }
1524 j3valid[0] = true;
1525 for(int ij3 = 0; ij3 < 1; ++ij3)
1526 {
1527 if( !j3valid[ij3] )
1528 {
1529  continue;
1530 }
1531 _ij3[0] = ij3; _ij3[1] = -1;
1532 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
1533 {
1534 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
1535 {
1536  j3valid[iij3]=false; _ij3[1] = iij3; break;
1537 }
1538 }
1539 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
1540 {
1541 IkReal evalcond[6];
1542 IkReal x121=IKsin(j3);
1543 IkReal x122=IKcos(j3);
1544 IkReal x123=((IkReal(1.00000000000000))*(cj4));
1545 IkReal x124=((new_r02)*(x122));
1546 IkReal x125=((sj4)*(x121));
1547 IkReal x126=((sj4)*(x122));
1548 IkReal x127=((new_r12)*(x121));
1549 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x121)))+(((new_r12)*(x122))));
1550 evalcond[1]=((sj4)+(x127)+(x124));
1551 evalcond[2]=((((new_r22)*(sj4)))+(((cj4)*(x127)))+(((cj4)*(x124))));
1552 evalcond[3]=((((new_r00)*(x126)))+(((new_r10)*(x125)))+(((IkReal(-1.00000000000000))*(new_r20)*(x123))));
1553 evalcond[4]=((((new_r11)*(x125)))+(((new_r01)*(x126)))+(((IkReal(-1.00000000000000))*(new_r21)*(x123))));
1554 evalcond[5]=((IkReal(1.00000000000000))+(((new_r12)*(x125)))+(((IkReal(-1.00000000000000))*(new_r22)*(x123)))+(((sj4)*(x124))));
1555 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 )
1556 {
1557 continue;
1558 }
1559 }
1560 
1561 {
1562 IkReal dummyeval[1];
1563 IkReal gconst5;
1564 gconst5=IKsign(sj4);
1565 dummyeval[0]=sj4;
1566 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1567 {
1568 {
1569 IkReal dummyeval[1];
1570 dummyeval[0]=sj4;
1571 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1572 {
1573 {
1574 IkReal dummyeval[1];
1575 dummyeval[0]=sj4;
1576 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
1577 {
1578 {
1579 IkReal evalcond[11];
1580 IkReal x128=((cj3)*(new_r12));
1581 IkReal x129=((new_r02)*(sj3));
1582 IkReal x130=((((new_r12)*(sj3)))+(((cj3)*(new_r02))));
1583 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
1584 evalcond[1]=((IkReal(-1.00000000000000))+(new_r22));
1585 evalcond[2]=new_r21;
1586 evalcond[3]=new_r20;
1587 evalcond[4]=((((IkReal(-1.00000000000000))*(x129)))+(x128));
1588 evalcond[5]=((((IkReal(-1.00000000000000))*(x128)))+(x129));
1589 evalcond[6]=x130;
1590 evalcond[7]=x130;
1591 evalcond[8]=((IkReal(-1.00000000000000))*(new_r20));
1592 evalcond[9]=((IkReal(-1.00000000000000))*(new_r21));
1593 evalcond[10]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
1594 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 )
1595 {
1596 {
1597 IkReal j5array[1], cj5array[1], sj5array[1];
1598 bool j5valid[1]={false};
1599 _nj5 = 1;
1600 IkReal x131=((IkReal(1.00000000000000))*(sj3));
1601 if( IKabs(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x131))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(x131)))+(((cj3)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x131)))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(x131)))+(((cj3)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
1602  continue;
1603 j5array[0]=IKatan2(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x131)))), ((((IkReal(-1.00000000000000))*(new_r01)*(x131)))+(((cj3)*(new_r11)))));
1604 sj5array[0]=IKsin(j5array[0]);
1605 cj5array[0]=IKcos(j5array[0]);
1606 if( j5array[0] > IKPI )
1607 {
1608  j5array[0]-=IK2PI;
1609 }
1610 else if( j5array[0] < -IKPI )
1611 { j5array[0]+=IK2PI;
1612 }
1613 j5valid[0] = true;
1614 for(int ij5 = 0; ij5 < 1; ++ij5)
1615 {
1616 if( !j5valid[ij5] )
1617 {
1618  continue;
1619 }
1620 _ij5[0] = ij5; _ij5[1] = -1;
1621 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1622 {
1623 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1624 {
1625  j5valid[iij5]=false; _ij5[1] = iij5; break;
1626 }
1627 }
1628 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1629 {
1630 IkReal evalcond[4];
1631 IkReal x132=IKsin(j5);
1632 IkReal x133=IKcos(j5);
1633 IkReal x134=((IkReal(1.00000000000000))*(cj3));
1634 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r10)*(x134)))+(x132)+(((new_r00)*(sj3))));
1635 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r11)*(x134)))+(((new_r01)*(sj3)))+(x133));
1636 evalcond[2]=((((new_r11)*(sj3)))+(x132)+(((cj3)*(new_r01))));
1637 evalcond[3]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(x133)))+(((cj3)*(new_r00))));
1638 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1639 {
1640 continue;
1641 }
1642 }
1643 
1644 {
1645 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1646 vinfos[0].jointtype = 1;
1647 vinfos[0].foffset = j0;
1648 vinfos[0].indices[0] = _ij0[0];
1649 vinfos[0].indices[1] = _ij0[1];
1650 vinfos[0].maxsolutions = _nj0;
1651 vinfos[1].jointtype = 1;
1652 vinfos[1].foffset = j1;
1653 vinfos[1].indices[0] = _ij1[0];
1654 vinfos[1].indices[1] = _ij1[1];
1655 vinfos[1].maxsolutions = _nj1;
1656 vinfos[2].jointtype = 1;
1657 vinfos[2].foffset = j2;
1658 vinfos[2].indices[0] = _ij2[0];
1659 vinfos[2].indices[1] = _ij2[1];
1660 vinfos[2].maxsolutions = _nj2;
1661 vinfos[3].jointtype = 1;
1662 vinfos[3].foffset = j3;
1663 vinfos[3].indices[0] = _ij3[0];
1664 vinfos[3].indices[1] = _ij3[1];
1665 vinfos[3].maxsolutions = _nj3;
1666 vinfos[4].jointtype = 1;
1667 vinfos[4].foffset = j4;
1668 vinfos[4].indices[0] = _ij4[0];
1669 vinfos[4].indices[1] = _ij4[1];
1670 vinfos[4].maxsolutions = _nj4;
1671 vinfos[5].jointtype = 1;
1672 vinfos[5].foffset = j5;
1673 vinfos[5].indices[0] = _ij5[0];
1674 vinfos[5].indices[1] = _ij5[1];
1675 vinfos[5].maxsolutions = _nj5;
1676 std::vector<int> vfree(0);
1677 solutions.AddSolution(vinfos,vfree);
1678 }
1679 }
1680 }
1681 
1682 } else
1683 {
1684 IkReal x135=((IkReal(1.00000000000000))+(new_r22));
1685 IkReal x136=((cj3)*(new_r12));
1686 IkReal x137=((new_r12)*(sj3));
1687 IkReal x138=((new_r02)*(sj3));
1688 IkReal x139=((cj3)*(new_r02));
1689 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959))));
1690 evalcond[1]=x135;
1691 evalcond[2]=new_r21;
1692 evalcond[3]=new_r20;
1693 evalcond[4]=((((IkReal(-1.00000000000000))*(x138)))+(x136));
1694 evalcond[5]=((x138)+(((IkReal(-1.00000000000000))*(x136))));
1695 evalcond[6]=((x137)+(x139));
1696 evalcond[7]=((((IkReal(-1.00000000000000))*(x137)))+(((IkReal(-1.00000000000000))*(x139))));
1697 evalcond[8]=new_r20;
1698 evalcond[9]=new_r21;
1699 evalcond[10]=x135;
1700 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 )
1701 {
1702 {
1703 IkReal j5array[1], cj5array[1], sj5array[1];
1704 bool j5valid[1]={false};
1705 _nj5 = 1;
1706 IkReal x140=((IkReal(1.00000000000000))*(sj3));
1707 if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(x140)))+(((cj3)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x140))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(x140)))+(((cj3)*(new_r10)))))+IKsqr(((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x140)))))-1) <= IKFAST_SINCOS_THRESH )
1708  continue;
1709 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(x140)))+(((cj3)*(new_r10)))), ((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x140)))));
1710 sj5array[0]=IKsin(j5array[0]);
1711 cj5array[0]=IKcos(j5array[0]);
1712 if( j5array[0] > IKPI )
1713 {
1714  j5array[0]-=IK2PI;
1715 }
1716 else if( j5array[0] < -IKPI )
1717 { j5array[0]+=IK2PI;
1718 }
1719 j5valid[0] = true;
1720 for(int ij5 = 0; ij5 < 1; ++ij5)
1721 {
1722 if( !j5valid[ij5] )
1723 {
1724  continue;
1725 }
1726 _ij5[0] = ij5; _ij5[1] = -1;
1727 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1728 {
1729 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1730 {
1731  j5valid[iij5]=false; _ij5[1] = iij5; break;
1732 }
1733 }
1734 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1735 {
1736 IkReal evalcond[4];
1737 IkReal x141=IKsin(j5);
1738 IkReal x142=IKcos(j5);
1739 IkReal x143=((IkReal(1.00000000000000))*(cj3));
1740 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r10)*(x143)))+(x141)+(((new_r00)*(sj3))));
1741 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r11)*(x143)))+(((new_r01)*(sj3)))+(x142));
1742 evalcond[2]=((((new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(x141)))+(((cj3)*(new_r01))));
1743 evalcond[3]=((((new_r10)*(sj3)))+(x142)+(((cj3)*(new_r00))));
1744 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
1745 {
1746 continue;
1747 }
1748 }
1749 
1750 {
1751 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1752 vinfos[0].jointtype = 1;
1753 vinfos[0].foffset = j0;
1754 vinfos[0].indices[0] = _ij0[0];
1755 vinfos[0].indices[1] = _ij0[1];
1756 vinfos[0].maxsolutions = _nj0;
1757 vinfos[1].jointtype = 1;
1758 vinfos[1].foffset = j1;
1759 vinfos[1].indices[0] = _ij1[0];
1760 vinfos[1].indices[1] = _ij1[1];
1761 vinfos[1].maxsolutions = _nj1;
1762 vinfos[2].jointtype = 1;
1763 vinfos[2].foffset = j2;
1764 vinfos[2].indices[0] = _ij2[0];
1765 vinfos[2].indices[1] = _ij2[1];
1766 vinfos[2].maxsolutions = _nj2;
1767 vinfos[3].jointtype = 1;
1768 vinfos[3].foffset = j3;
1769 vinfos[3].indices[0] = _ij3[0];
1770 vinfos[3].indices[1] = _ij3[1];
1771 vinfos[3].maxsolutions = _nj3;
1772 vinfos[4].jointtype = 1;
1773 vinfos[4].foffset = j4;
1774 vinfos[4].indices[0] = _ij4[0];
1775 vinfos[4].indices[1] = _ij4[1];
1776 vinfos[4].maxsolutions = _nj4;
1777 vinfos[5].jointtype = 1;
1778 vinfos[5].foffset = j5;
1779 vinfos[5].indices[0] = _ij5[0];
1780 vinfos[5].indices[1] = _ij5[1];
1781 vinfos[5].maxsolutions = _nj5;
1782 std::vector<int> vfree(0);
1783 solutions.AddSolution(vinfos,vfree);
1784 }
1785 }
1786 }
1787 
1788 } else
1789 {
1790 if( 1 )
1791 {
1792 continue;
1793 
1794 } else
1795 {
1796 }
1797 }
1798 }
1799 }
1800 
1801 } else
1802 {
1803 {
1804 IkReal j5array[1], cj5array[1], sj5array[1];
1805 bool j5valid[1]={false};
1806 _nj5 = 1;
1807 if( IKabs(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
1808  continue;
1809 j5array[0]=IKatan2(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))));
1810 sj5array[0]=IKsin(j5array[0]);
1811 cj5array[0]=IKcos(j5array[0]);
1812 if( j5array[0] > IKPI )
1813 {
1814  j5array[0]-=IK2PI;
1815 }
1816 else if( j5array[0] < -IKPI )
1817 { j5array[0]+=IK2PI;
1818 }
1819 j5valid[0] = true;
1820 for(int ij5 = 0; ij5 < 1; ++ij5)
1821 {
1822 if( !j5valid[ij5] )
1823 {
1824  continue;
1825 }
1826 _ij5[0] = ij5; _ij5[1] = -1;
1827 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1828 {
1829 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1830 {
1831  j5valid[iij5]=false; _ij5[1] = iij5; break;
1832 }
1833 }
1834 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1835 {
1836 IkReal evalcond[8];
1837 IkReal x144=IKsin(j5);
1838 IkReal x145=IKcos(j5);
1839 IkReal x146=((IkReal(1.00000000000000))*(cj3));
1840 IkReal x147=((cj4)*(sj3));
1841 IkReal x148=((cj3)*(cj4));
1842 IkReal x149=((IkReal(1.00000000000000))*(x145));
1843 evalcond[0]=((new_r21)+(((sj4)*(x144))));
1844 evalcond[1]=((new_r20)+(((IkReal(-1.00000000000000))*(sj4)*(x149))));
1845 evalcond[2]=((((IkReal(-1.00000000000000))*(new_r10)*(x146)))+(x144)+(((new_r00)*(sj3))));
1846 evalcond[3]=((((new_r01)*(sj3)))+(((IkReal(-1.00000000000000))*(new_r11)*(x146)))+(x145));
1847 evalcond[4]=((((cj4)*(x144)))+(((new_r11)*(sj3)))+(((cj3)*(new_r01))));
1848 evalcond[5]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(cj4)*(x149)))+(((cj3)*(new_r00))));
1849 evalcond[6]=((((new_r21)*(sj4)))+(((new_r01)*(x148)))+(x144)+(((new_r11)*(x147))));
1850 evalcond[7]=((((new_r20)*(sj4)))+(((new_r00)*(x148)))+(((new_r10)*(x147)))+(((IkReal(-1.00000000000000))*(x149))));
1851 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 )
1852 {
1853 continue;
1854 }
1855 }
1856 
1857 {
1858 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1859 vinfos[0].jointtype = 1;
1860 vinfos[0].foffset = j0;
1861 vinfos[0].indices[0] = _ij0[0];
1862 vinfos[0].indices[1] = _ij0[1];
1863 vinfos[0].maxsolutions = _nj0;
1864 vinfos[1].jointtype = 1;
1865 vinfos[1].foffset = j1;
1866 vinfos[1].indices[0] = _ij1[0];
1867 vinfos[1].indices[1] = _ij1[1];
1868 vinfos[1].maxsolutions = _nj1;
1869 vinfos[2].jointtype = 1;
1870 vinfos[2].foffset = j2;
1871 vinfos[2].indices[0] = _ij2[0];
1872 vinfos[2].indices[1] = _ij2[1];
1873 vinfos[2].maxsolutions = _nj2;
1874 vinfos[3].jointtype = 1;
1875 vinfos[3].foffset = j3;
1876 vinfos[3].indices[0] = _ij3[0];
1877 vinfos[3].indices[1] = _ij3[1];
1878 vinfos[3].maxsolutions = _nj3;
1879 vinfos[4].jointtype = 1;
1880 vinfos[4].foffset = j4;
1881 vinfos[4].indices[0] = _ij4[0];
1882 vinfos[4].indices[1] = _ij4[1];
1883 vinfos[4].maxsolutions = _nj4;
1884 vinfos[5].jointtype = 1;
1885 vinfos[5].foffset = j5;
1886 vinfos[5].indices[0] = _ij5[0];
1887 vinfos[5].indices[1] = _ij5[1];
1888 vinfos[5].maxsolutions = _nj5;
1889 std::vector<int> vfree(0);
1890 solutions.AddSolution(vinfos,vfree);
1891 }
1892 }
1893 }
1894 
1895 }
1896 
1897 }
1898 
1899 } else
1900 {
1901 {
1902 IkReal j5array[1], cj5array[1], sj5array[1];
1903 bool j5valid[1]={false};
1904 _nj5 = 1;
1905 if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))))+IKsqr(((new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH )
1906  continue;
1907 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))), ((new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))));
1908 sj5array[0]=IKsin(j5array[0]);
1909 cj5array[0]=IKcos(j5array[0]);
1910 if( j5array[0] > IKPI )
1911 {
1912  j5array[0]-=IK2PI;
1913 }
1914 else if( j5array[0] < -IKPI )
1915 { j5array[0]+=IK2PI;
1916 }
1917 j5valid[0] = true;
1918 for(int ij5 = 0; ij5 < 1; ++ij5)
1919 {
1920 if( !j5valid[ij5] )
1921 {
1922  continue;
1923 }
1924 _ij5[0] = ij5; _ij5[1] = -1;
1925 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
1926 {
1927 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
1928 {
1929  j5valid[iij5]=false; _ij5[1] = iij5; break;
1930 }
1931 }
1932 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
1933 {
1934 IkReal evalcond[8];
1935 IkReal x150=IKsin(j5);
1936 IkReal x151=IKcos(j5);
1937 IkReal x152=((IkReal(1.00000000000000))*(cj3));
1938 IkReal x153=((cj4)*(sj3));
1939 IkReal x154=((cj3)*(cj4));
1940 IkReal x155=((IkReal(1.00000000000000))*(x151));
1941 evalcond[0]=((new_r21)+(((sj4)*(x150))));
1942 evalcond[1]=((new_r20)+(((IkReal(-1.00000000000000))*(sj4)*(x155))));
1943 evalcond[2]=((x150)+(((IkReal(-1.00000000000000))*(new_r10)*(x152)))+(((new_r00)*(sj3))));
1944 evalcond[3]=((x151)+(((new_r01)*(sj3)))+(((IkReal(-1.00000000000000))*(new_r11)*(x152))));
1945 evalcond[4]=((((new_r11)*(sj3)))+(((cj4)*(x150)))+(((cj3)*(new_r01))));
1946 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x155)))+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
1947 evalcond[6]=((((new_r21)*(sj4)))+(x150)+(((new_r01)*(x154)))+(((new_r11)*(x153))));
1948 evalcond[7]=((((IkReal(-1.00000000000000))*(x155)))+(((new_r10)*(x153)))+(((new_r00)*(x154)))+(((new_r20)*(sj4))));
1949 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 )
1950 {
1951 continue;
1952 }
1953 }
1954 
1955 {
1956 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
1957 vinfos[0].jointtype = 1;
1958 vinfos[0].foffset = j0;
1959 vinfos[0].indices[0] = _ij0[0];
1960 vinfos[0].indices[1] = _ij0[1];
1961 vinfos[0].maxsolutions = _nj0;
1962 vinfos[1].jointtype = 1;
1963 vinfos[1].foffset = j1;
1964 vinfos[1].indices[0] = _ij1[0];
1965 vinfos[1].indices[1] = _ij1[1];
1966 vinfos[1].maxsolutions = _nj1;
1967 vinfos[2].jointtype = 1;
1968 vinfos[2].foffset = j2;
1969 vinfos[2].indices[0] = _ij2[0];
1970 vinfos[2].indices[1] = _ij2[1];
1971 vinfos[2].maxsolutions = _nj2;
1972 vinfos[3].jointtype = 1;
1973 vinfos[3].foffset = j3;
1974 vinfos[3].indices[0] = _ij3[0];
1975 vinfos[3].indices[1] = _ij3[1];
1976 vinfos[3].maxsolutions = _nj3;
1977 vinfos[4].jointtype = 1;
1978 vinfos[4].foffset = j4;
1979 vinfos[4].indices[0] = _ij4[0];
1980 vinfos[4].indices[1] = _ij4[1];
1981 vinfos[4].maxsolutions = _nj4;
1982 vinfos[5].jointtype = 1;
1983 vinfos[5].foffset = j5;
1984 vinfos[5].indices[0] = _ij5[0];
1985 vinfos[5].indices[1] = _ij5[1];
1986 vinfos[5].maxsolutions = _nj5;
1987 std::vector<int> vfree(0);
1988 solutions.AddSolution(vinfos,vfree);
1989 }
1990 }
1991 }
1992 
1993 }
1994 
1995 }
1996 
1997 } else
1998 {
1999 {
2000 IkReal j5array[1], cj5array[1], sj5array[1];
2001 bool j5valid[1]={false};
2002 _nj5 = 1;
2003 if( IKabs(((IkReal(-1.00000000000000))*(gconst5)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
2004  continue;
2005 j5array[0]=IKatan2(((IkReal(-1.00000000000000))*(gconst5)*(new_r21)), ((gconst5)*(new_r20)));
2006 sj5array[0]=IKsin(j5array[0]);
2007 cj5array[0]=IKcos(j5array[0]);
2008 if( j5array[0] > IKPI )
2009 {
2010  j5array[0]-=IK2PI;
2011 }
2012 else if( j5array[0] < -IKPI )
2013 { j5array[0]+=IK2PI;
2014 }
2015 j5valid[0] = true;
2016 for(int ij5 = 0; ij5 < 1; ++ij5)
2017 {
2018 if( !j5valid[ij5] )
2019 {
2020  continue;
2021 }
2022 _ij5[0] = ij5; _ij5[1] = -1;
2023 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2024 {
2025 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2026 {
2027  j5valid[iij5]=false; _ij5[1] = iij5; break;
2028 }
2029 }
2030 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2031 {
2032 IkReal evalcond[8];
2033 IkReal x156=IKsin(j5);
2034 IkReal x157=IKcos(j5);
2035 IkReal x158=((IkReal(1.00000000000000))*(cj3));
2036 IkReal x159=((cj4)*(sj3));
2037 IkReal x160=((cj3)*(cj4));
2038 IkReal x161=((IkReal(1.00000000000000))*(x157));
2039 evalcond[0]=((new_r21)+(((sj4)*(x156))));
2040 evalcond[1]=((((IkReal(-1.00000000000000))*(sj4)*(x161)))+(new_r20));
2041 evalcond[2]=((x156)+(((IkReal(-1.00000000000000))*(new_r10)*(x158)))+(((new_r00)*(sj3))));
2042 evalcond[3]=((x157)+(((new_r01)*(sj3)))+(((IkReal(-1.00000000000000))*(new_r11)*(x158))));
2043 evalcond[4]=((((new_r11)*(sj3)))+(((cj4)*(x156)))+(((cj3)*(new_r01))));
2044 evalcond[5]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(cj4)*(x161)))+(((cj3)*(new_r00))));
2045 evalcond[6]=((((new_r21)*(sj4)))+(x156)+(((new_r11)*(x159)))+(((new_r01)*(x160))));
2046 evalcond[7]=((((new_r20)*(sj4)))+(((new_r00)*(x160)))+(((new_r10)*(x159)))+(((IkReal(-1.00000000000000))*(x161))));
2047 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 )
2048 {
2049 continue;
2050 }
2051 }
2052 
2053 {
2054 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2055 vinfos[0].jointtype = 1;
2056 vinfos[0].foffset = j0;
2057 vinfos[0].indices[0] = _ij0[0];
2058 vinfos[0].indices[1] = _ij0[1];
2059 vinfos[0].maxsolutions = _nj0;
2060 vinfos[1].jointtype = 1;
2061 vinfos[1].foffset = j1;
2062 vinfos[1].indices[0] = _ij1[0];
2063 vinfos[1].indices[1] = _ij1[1];
2064 vinfos[1].maxsolutions = _nj1;
2065 vinfos[2].jointtype = 1;
2066 vinfos[2].foffset = j2;
2067 vinfos[2].indices[0] = _ij2[0];
2068 vinfos[2].indices[1] = _ij2[1];
2069 vinfos[2].maxsolutions = _nj2;
2070 vinfos[3].jointtype = 1;
2071 vinfos[3].foffset = j3;
2072 vinfos[3].indices[0] = _ij3[0];
2073 vinfos[3].indices[1] = _ij3[1];
2074 vinfos[3].maxsolutions = _nj3;
2075 vinfos[4].jointtype = 1;
2076 vinfos[4].foffset = j4;
2077 vinfos[4].indices[0] = _ij4[0];
2078 vinfos[4].indices[1] = _ij4[1];
2079 vinfos[4].maxsolutions = _nj4;
2080 vinfos[5].jointtype = 1;
2081 vinfos[5].foffset = j5;
2082 vinfos[5].indices[0] = _ij5[0];
2083 vinfos[5].indices[1] = _ij5[1];
2084 vinfos[5].maxsolutions = _nj5;
2085 std::vector<int> vfree(0);
2086 solutions.AddSolution(vinfos,vfree);
2087 }
2088 }
2089 }
2090 
2091 }
2092 
2093 }
2094 }
2095 }
2096 
2097 }
2098 
2099 }
2100 
2101 } else
2102 {
2103 {
2104 IkReal j3array[1], cj3array[1], sj3array[1];
2105 bool j3valid[1]={false};
2106 _nj3 = 1;
2107 IkReal x162=((gconst2)*(sj4));
2108 if( IKabs(((new_r12)*(x162))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x162))) < IKFAST_ATAN2_MAGTHRESH )
2109  continue;
2110 j3array[0]=IKatan2(((new_r12)*(x162)), ((new_r02)*(x162)));
2111 sj3array[0]=IKsin(j3array[0]);
2112 cj3array[0]=IKcos(j3array[0]);
2113 if( j3array[0] > IKPI )
2114 {
2115  j3array[0]-=IK2PI;
2116 }
2117 else if( j3array[0] < -IKPI )
2118 { j3array[0]+=IK2PI;
2119 }
2120 j3valid[0] = true;
2121 for(int ij3 = 0; ij3 < 1; ++ij3)
2122 {
2123 if( !j3valid[ij3] )
2124 {
2125  continue;
2126 }
2127 _ij3[0] = ij3; _ij3[1] = -1;
2128 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2129 {
2130 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2131 {
2132  j3valid[iij3]=false; _ij3[1] = iij3; break;
2133 }
2134 }
2135 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2136 {
2137 IkReal evalcond[6];
2138 IkReal x163=IKsin(j3);
2139 IkReal x164=IKcos(j3);
2140 IkReal x165=((IkReal(1.00000000000000))*(cj4));
2141 IkReal x166=((new_r02)*(x164));
2142 IkReal x167=((sj4)*(x163));
2143 IkReal x168=((sj4)*(x164));
2144 IkReal x169=((new_r12)*(x163));
2145 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x163)))+(((new_r12)*(x164))));
2146 evalcond[1]=((sj4)+(x169)+(x166));
2147 evalcond[2]=((((new_r22)*(sj4)))+(((cj4)*(x169)))+(((cj4)*(x166))));
2148 evalcond[3]=((((new_r10)*(x167)))+(((IkReal(-1.00000000000000))*(new_r20)*(x165)))+(((new_r00)*(x168))));
2149 evalcond[4]=((((IkReal(-1.00000000000000))*(new_r21)*(x165)))+(((new_r01)*(x168)))+(((new_r11)*(x167))));
2150 evalcond[5]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)*(x165)))+(((new_r12)*(x167)))+(((sj4)*(x166))));
2151 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 )
2152 {
2153 continue;
2154 }
2155 }
2156 
2157 {
2158 IkReal dummyeval[1];
2159 IkReal gconst5;
2160 gconst5=IKsign(sj4);
2161 dummyeval[0]=sj4;
2162 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2163 {
2164 {
2165 IkReal dummyeval[1];
2166 dummyeval[0]=sj4;
2167 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2168 {
2169 {
2170 IkReal dummyeval[1];
2171 dummyeval[0]=sj4;
2172 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2173 {
2174 {
2175 IkReal evalcond[11];
2176 IkReal x170=((cj3)*(new_r12));
2177 IkReal x171=((new_r02)*(sj3));
2178 IkReal x172=((((new_r12)*(sj3)))+(((cj3)*(new_r02))));
2179 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(3.14159265358979))+(j4)), IkReal(6.28318530717959))));
2180 evalcond[1]=((IkReal(-1.00000000000000))+(new_r22));
2181 evalcond[2]=new_r21;
2182 evalcond[3]=new_r20;
2183 evalcond[4]=((x170)+(((IkReal(-1.00000000000000))*(x171))));
2184 evalcond[5]=((((IkReal(-1.00000000000000))*(x170)))+(x171));
2185 evalcond[6]=x172;
2186 evalcond[7]=x172;
2187 evalcond[8]=((IkReal(-1.00000000000000))*(new_r20));
2188 evalcond[9]=((IkReal(-1.00000000000000))*(new_r21));
2189 evalcond[10]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22))));
2190 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 )
2191 {
2192 {
2193 IkReal j5array[1], cj5array[1], sj5array[1];
2194 bool j5valid[1]={false};
2195 _nj5 = 1;
2196 IkReal x173=((IkReal(1.00000000000000))*(sj3));
2197 if( IKabs(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x173))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x173))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x173)))))+IKsqr(((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x173)))))-1) <= IKFAST_SINCOS_THRESH )
2198  continue;
2199 j5array[0]=IKatan2(((((cj3)*(new_r10)))+(((IkReal(-1.00000000000000))*(new_r00)*(x173)))), ((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x173)))));
2200 sj5array[0]=IKsin(j5array[0]);
2201 cj5array[0]=IKcos(j5array[0]);
2202 if( j5array[0] > IKPI )
2203 {
2204  j5array[0]-=IK2PI;
2205 }
2206 else if( j5array[0] < -IKPI )
2207 { j5array[0]+=IK2PI;
2208 }
2209 j5valid[0] = true;
2210 for(int ij5 = 0; ij5 < 1; ++ij5)
2211 {
2212 if( !j5valid[ij5] )
2213 {
2214  continue;
2215 }
2216 _ij5[0] = ij5; _ij5[1] = -1;
2217 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2218 {
2219 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2220 {
2221  j5valid[iij5]=false; _ij5[1] = iij5; break;
2222 }
2223 }
2224 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2225 {
2226 IkReal evalcond[4];
2227 IkReal x174=IKsin(j5);
2228 IkReal x175=IKcos(j5);
2229 IkReal x176=((IkReal(1.00000000000000))*(cj3));
2230 evalcond[0]=((x174)+(((IkReal(-1.00000000000000))*(new_r10)*(x176)))+(((new_r00)*(sj3))));
2231 evalcond[1]=((x175)+(((IkReal(-1.00000000000000))*(new_r11)*(x176)))+(((new_r01)*(sj3))));
2232 evalcond[2]=((x174)+(((new_r11)*(sj3)))+(((cj3)*(new_r01))));
2233 evalcond[3]=((((IkReal(-1.00000000000000))*(x175)))+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
2234 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2235 {
2236 continue;
2237 }
2238 }
2239 
2240 {
2241 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2242 vinfos[0].jointtype = 1;
2243 vinfos[0].foffset = j0;
2244 vinfos[0].indices[0] = _ij0[0];
2245 vinfos[0].indices[1] = _ij0[1];
2246 vinfos[0].maxsolutions = _nj0;
2247 vinfos[1].jointtype = 1;
2248 vinfos[1].foffset = j1;
2249 vinfos[1].indices[0] = _ij1[0];
2250 vinfos[1].indices[1] = _ij1[1];
2251 vinfos[1].maxsolutions = _nj1;
2252 vinfos[2].jointtype = 1;
2253 vinfos[2].foffset = j2;
2254 vinfos[2].indices[0] = _ij2[0];
2255 vinfos[2].indices[1] = _ij2[1];
2256 vinfos[2].maxsolutions = _nj2;
2257 vinfos[3].jointtype = 1;
2258 vinfos[3].foffset = j3;
2259 vinfos[3].indices[0] = _ij3[0];
2260 vinfos[3].indices[1] = _ij3[1];
2261 vinfos[3].maxsolutions = _nj3;
2262 vinfos[4].jointtype = 1;
2263 vinfos[4].foffset = j4;
2264 vinfos[4].indices[0] = _ij4[0];
2265 vinfos[4].indices[1] = _ij4[1];
2266 vinfos[4].maxsolutions = _nj4;
2267 vinfos[5].jointtype = 1;
2268 vinfos[5].foffset = j5;
2269 vinfos[5].indices[0] = _ij5[0];
2270 vinfos[5].indices[1] = _ij5[1];
2271 vinfos[5].maxsolutions = _nj5;
2272 std::vector<int> vfree(0);
2273 solutions.AddSolution(vinfos,vfree);
2274 }
2275 }
2276 }
2277 
2278 } else
2279 {
2280 IkReal x177=((IkReal(1.00000000000000))+(new_r22));
2281 IkReal x178=((cj3)*(new_r12));
2282 IkReal x179=((new_r12)*(sj3));
2283 IkReal x180=((new_r02)*(sj3));
2284 IkReal x181=((cj3)*(new_r02));
2285 evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(j4, IkReal(6.28318530717959))));
2286 evalcond[1]=x177;
2287 evalcond[2]=new_r21;
2288 evalcond[3]=new_r20;
2289 evalcond[4]=((x178)+(((IkReal(-1.00000000000000))*(x180))));
2290 evalcond[5]=((((IkReal(-1.00000000000000))*(x178)))+(x180));
2291 evalcond[6]=((x179)+(x181));
2292 evalcond[7]=((((IkReal(-1.00000000000000))*(x181)))+(((IkReal(-1.00000000000000))*(x179))));
2293 evalcond[8]=new_r20;
2294 evalcond[9]=new_r21;
2295 evalcond[10]=x177;
2296 if( IKabs(evalcond[0]) < 0.0000010000000000 && IKabs(evalcond[1]) < 0.0000010000000000 && IKabs(evalcond[2]) < 0.0000010000000000 && IKabs(evalcond[3]) < 0.0000010000000000 && IKabs(evalcond[4]) < 0.0000010000000000 && IKabs(evalcond[5]) < 0.0000010000000000 && IKabs(evalcond[6]) < 0.0000010000000000 && IKabs(evalcond[7]) < 0.0000010000000000 && IKabs(evalcond[8]) < 0.0000010000000000 && IKabs(evalcond[9]) < 0.0000010000000000 && IKabs(evalcond[10]) < 0.0000010000000000 )
2297 {
2298 {
2299 IkReal j5array[1], cj5array[1], sj5array[1];
2300 bool j5valid[1]={false};
2301 _nj5 = 1;
2302 IkReal x182=((IkReal(1.00000000000000))*(sj3));
2303 if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(x182)))+(((cj3)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x182))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(x182)))+(((cj3)*(new_r10)))))+IKsqr(((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x182)))))-1) <= IKFAST_SINCOS_THRESH )
2304  continue;
2305 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(x182)))+(((cj3)*(new_r10)))), ((((cj3)*(new_r11)))+(((IkReal(-1.00000000000000))*(new_r01)*(x182)))));
2306 sj5array[0]=IKsin(j5array[0]);
2307 cj5array[0]=IKcos(j5array[0]);
2308 if( j5array[0] > IKPI )
2309 {
2310  j5array[0]-=IK2PI;
2311 }
2312 else if( j5array[0] < -IKPI )
2313 { j5array[0]+=IK2PI;
2314 }
2315 j5valid[0] = true;
2316 for(int ij5 = 0; ij5 < 1; ++ij5)
2317 {
2318 if( !j5valid[ij5] )
2319 {
2320  continue;
2321 }
2322 _ij5[0] = ij5; _ij5[1] = -1;
2323 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2324 {
2325 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2326 {
2327  j5valid[iij5]=false; _ij5[1] = iij5; break;
2328 }
2329 }
2330 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2331 {
2332 IkReal evalcond[4];
2333 IkReal x183=IKsin(j5);
2334 IkReal x184=IKcos(j5);
2335 IkReal x185=((IkReal(1.00000000000000))*(cj3));
2336 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r10)*(x185)))+(x183)+(((new_r00)*(sj3))));
2337 evalcond[1]=((((IkReal(-1.00000000000000))*(new_r11)*(x185)))+(x184)+(((new_r01)*(sj3))));
2338 evalcond[2]=((((new_r11)*(sj3)))+(((IkReal(-1.00000000000000))*(x183)))+(((cj3)*(new_r01))));
2339 evalcond[3]=((x184)+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
2340 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 )
2341 {
2342 continue;
2343 }
2344 }
2345 
2346 {
2347 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2348 vinfos[0].jointtype = 1;
2349 vinfos[0].foffset = j0;
2350 vinfos[0].indices[0] = _ij0[0];
2351 vinfos[0].indices[1] = _ij0[1];
2352 vinfos[0].maxsolutions = _nj0;
2353 vinfos[1].jointtype = 1;
2354 vinfos[1].foffset = j1;
2355 vinfos[1].indices[0] = _ij1[0];
2356 vinfos[1].indices[1] = _ij1[1];
2357 vinfos[1].maxsolutions = _nj1;
2358 vinfos[2].jointtype = 1;
2359 vinfos[2].foffset = j2;
2360 vinfos[2].indices[0] = _ij2[0];
2361 vinfos[2].indices[1] = _ij2[1];
2362 vinfos[2].maxsolutions = _nj2;
2363 vinfos[3].jointtype = 1;
2364 vinfos[3].foffset = j3;
2365 vinfos[3].indices[0] = _ij3[0];
2366 vinfos[3].indices[1] = _ij3[1];
2367 vinfos[3].maxsolutions = _nj3;
2368 vinfos[4].jointtype = 1;
2369 vinfos[4].foffset = j4;
2370 vinfos[4].indices[0] = _ij4[0];
2371 vinfos[4].indices[1] = _ij4[1];
2372 vinfos[4].maxsolutions = _nj4;
2373 vinfos[5].jointtype = 1;
2374 vinfos[5].foffset = j5;
2375 vinfos[5].indices[0] = _ij5[0];
2376 vinfos[5].indices[1] = _ij5[1];
2377 vinfos[5].maxsolutions = _nj5;
2378 std::vector<int> vfree(0);
2379 solutions.AddSolution(vinfos,vfree);
2380 }
2381 }
2382 }
2383 
2384 } else
2385 {
2386 if( 1 )
2387 {
2388 continue;
2389 
2390 } else
2391 {
2392 }
2393 }
2394 }
2395 }
2396 
2397 } else
2398 {
2399 {
2400 IkReal j5array[1], cj5array[1], sj5array[1];
2401 bool j5valid[1]={false};
2402 _nj5 = 1;
2403 if( IKabs(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))+IKsqr(((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))))-1) <= IKFAST_SINCOS_THRESH )
2404  continue;
2405 j5array[0]=IKatan2(((IkReal(-1.00000000000000))*(new_r21)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))), ((((IkReal(-1.00000000000000))*(new_r01)*(sj3)))+(((cj3)*(new_r11)))));
2406 sj5array[0]=IKsin(j5array[0]);
2407 cj5array[0]=IKcos(j5array[0]);
2408 if( j5array[0] > IKPI )
2409 {
2410  j5array[0]-=IK2PI;
2411 }
2412 else if( j5array[0] < -IKPI )
2413 { j5array[0]+=IK2PI;
2414 }
2415 j5valid[0] = true;
2416 for(int ij5 = 0; ij5 < 1; ++ij5)
2417 {
2418 if( !j5valid[ij5] )
2419 {
2420  continue;
2421 }
2422 _ij5[0] = ij5; _ij5[1] = -1;
2423 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2424 {
2425 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2426 {
2427  j5valid[iij5]=false; _ij5[1] = iij5; break;
2428 }
2429 }
2430 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2431 {
2432 IkReal evalcond[8];
2433 IkReal x186=IKsin(j5);
2434 IkReal x187=IKcos(j5);
2435 IkReal x188=((IkReal(1.00000000000000))*(cj3));
2436 IkReal x189=((cj4)*(sj3));
2437 IkReal x190=((cj3)*(cj4));
2438 IkReal x191=((IkReal(1.00000000000000))*(x187));
2439 evalcond[0]=((new_r21)+(((sj4)*(x186))));
2440 evalcond[1]=((new_r20)+(((IkReal(-1.00000000000000))*(sj4)*(x191))));
2441 evalcond[2]=((x186)+(((IkReal(-1.00000000000000))*(new_r10)*(x188)))+(((new_r00)*(sj3))));
2442 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x188)))+(x187)+(((new_r01)*(sj3))));
2443 evalcond[4]=((((cj4)*(x186)))+(((new_r11)*(sj3)))+(((cj3)*(new_r01))));
2444 evalcond[5]=((((new_r10)*(sj3)))+(((IkReal(-1.00000000000000))*(cj4)*(x191)))+(((cj3)*(new_r00))));
2445 evalcond[6]=((((new_r21)*(sj4)))+(((new_r11)*(x189)))+(x186)+(((new_r01)*(x190))));
2446 evalcond[7]=((((IkReal(-1.00000000000000))*(x191)))+(((new_r20)*(sj4)))+(((new_r10)*(x189)))+(((new_r00)*(x190))));
2447 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 )
2448 {
2449 continue;
2450 }
2451 }
2452 
2453 {
2454 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2455 vinfos[0].jointtype = 1;
2456 vinfos[0].foffset = j0;
2457 vinfos[0].indices[0] = _ij0[0];
2458 vinfos[0].indices[1] = _ij0[1];
2459 vinfos[0].maxsolutions = _nj0;
2460 vinfos[1].jointtype = 1;
2461 vinfos[1].foffset = j1;
2462 vinfos[1].indices[0] = _ij1[0];
2463 vinfos[1].indices[1] = _ij1[1];
2464 vinfos[1].maxsolutions = _nj1;
2465 vinfos[2].jointtype = 1;
2466 vinfos[2].foffset = j2;
2467 vinfos[2].indices[0] = _ij2[0];
2468 vinfos[2].indices[1] = _ij2[1];
2469 vinfos[2].maxsolutions = _nj2;
2470 vinfos[3].jointtype = 1;
2471 vinfos[3].foffset = j3;
2472 vinfos[3].indices[0] = _ij3[0];
2473 vinfos[3].indices[1] = _ij3[1];
2474 vinfos[3].maxsolutions = _nj3;
2475 vinfos[4].jointtype = 1;
2476 vinfos[4].foffset = j4;
2477 vinfos[4].indices[0] = _ij4[0];
2478 vinfos[4].indices[1] = _ij4[1];
2479 vinfos[4].maxsolutions = _nj4;
2480 vinfos[5].jointtype = 1;
2481 vinfos[5].foffset = j5;
2482 vinfos[5].indices[0] = _ij5[0];
2483 vinfos[5].indices[1] = _ij5[1];
2484 vinfos[5].maxsolutions = _nj5;
2485 std::vector<int> vfree(0);
2486 solutions.AddSolution(vinfos,vfree);
2487 }
2488 }
2489 }
2490 
2491 }
2492 
2493 }
2494 
2495 } else
2496 {
2497 {
2498 IkReal j5array[1], cj5array[1], sj5array[1];
2499 bool j5valid[1]={false};
2500 _nj5 = 1;
2501 if( IKabs(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30)))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))))+IKsqr(((new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))))-1) <= IKFAST_SINCOS_THRESH )
2502  continue;
2503 j5array[0]=IKatan2(((((IkReal(-1.00000000000000))*(new_r00)*(sj3)))+(((cj3)*(new_r10)))), ((new_r20)*(((IKabs(sj4) != 0)?((IkReal)1/(sj4)):(IkReal)1.0e30))));
2504 sj5array[0]=IKsin(j5array[0]);
2505 cj5array[0]=IKcos(j5array[0]);
2506 if( j5array[0] > IKPI )
2507 {
2508  j5array[0]-=IK2PI;
2509 }
2510 else if( j5array[0] < -IKPI )
2511 { j5array[0]+=IK2PI;
2512 }
2513 j5valid[0] = true;
2514 for(int ij5 = 0; ij5 < 1; ++ij5)
2515 {
2516 if( !j5valid[ij5] )
2517 {
2518  continue;
2519 }
2520 _ij5[0] = ij5; _ij5[1] = -1;
2521 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2522 {
2523 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2524 {
2525  j5valid[iij5]=false; _ij5[1] = iij5; break;
2526 }
2527 }
2528 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2529 {
2530 IkReal evalcond[8];
2531 IkReal x192=IKsin(j5);
2532 IkReal x193=IKcos(j5);
2533 IkReal x194=((IkReal(1.00000000000000))*(cj3));
2534 IkReal x195=((cj4)*(sj3));
2535 IkReal x196=((cj3)*(cj4));
2536 IkReal x197=((IkReal(1.00000000000000))*(x193));
2537 evalcond[0]=((((sj4)*(x192)))+(new_r21));
2538 evalcond[1]=((new_r20)+(((IkReal(-1.00000000000000))*(sj4)*(x197))));
2539 evalcond[2]=((x192)+(((IkReal(-1.00000000000000))*(new_r10)*(x194)))+(((new_r00)*(sj3))));
2540 evalcond[3]=((x193)+(((IkReal(-1.00000000000000))*(new_r11)*(x194)))+(((new_r01)*(sj3))));
2541 evalcond[4]=((((new_r11)*(sj3)))+(((cj4)*(x192)))+(((cj3)*(new_r01))));
2542 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x197)))+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
2543 evalcond[6]=((((new_r11)*(x195)))+(((new_r21)*(sj4)))+(x192)+(((new_r01)*(x196))));
2544 evalcond[7]=((((new_r00)*(x196)))+(((new_r20)*(sj4)))+(((IkReal(-1.00000000000000))*(x197)))+(((new_r10)*(x195))));
2545 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 )
2546 {
2547 continue;
2548 }
2549 }
2550 
2551 {
2552 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2553 vinfos[0].jointtype = 1;
2554 vinfos[0].foffset = j0;
2555 vinfos[0].indices[0] = _ij0[0];
2556 vinfos[0].indices[1] = _ij0[1];
2557 vinfos[0].maxsolutions = _nj0;
2558 vinfos[1].jointtype = 1;
2559 vinfos[1].foffset = j1;
2560 vinfos[1].indices[0] = _ij1[0];
2561 vinfos[1].indices[1] = _ij1[1];
2562 vinfos[1].maxsolutions = _nj1;
2563 vinfos[2].jointtype = 1;
2564 vinfos[2].foffset = j2;
2565 vinfos[2].indices[0] = _ij2[0];
2566 vinfos[2].indices[1] = _ij2[1];
2567 vinfos[2].maxsolutions = _nj2;
2568 vinfos[3].jointtype = 1;
2569 vinfos[3].foffset = j3;
2570 vinfos[3].indices[0] = _ij3[0];
2571 vinfos[3].indices[1] = _ij3[1];
2572 vinfos[3].maxsolutions = _nj3;
2573 vinfos[4].jointtype = 1;
2574 vinfos[4].foffset = j4;
2575 vinfos[4].indices[0] = _ij4[0];
2576 vinfos[4].indices[1] = _ij4[1];
2577 vinfos[4].maxsolutions = _nj4;
2578 vinfos[5].jointtype = 1;
2579 vinfos[5].foffset = j5;
2580 vinfos[5].indices[0] = _ij5[0];
2581 vinfos[5].indices[1] = _ij5[1];
2582 vinfos[5].maxsolutions = _nj5;
2583 std::vector<int> vfree(0);
2584 solutions.AddSolution(vinfos,vfree);
2585 }
2586 }
2587 }
2588 
2589 }
2590 
2591 }
2592 
2593 } else
2594 {
2595 {
2596 IkReal j5array[1], cj5array[1], sj5array[1];
2597 bool j5valid[1]={false};
2598 _nj5 = 1;
2599 if( IKabs(((IkReal(-1.00000000000000))*(gconst5)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
2600  continue;
2601 j5array[0]=IKatan2(((IkReal(-1.00000000000000))*(gconst5)*(new_r21)), ((gconst5)*(new_r20)));
2602 sj5array[0]=IKsin(j5array[0]);
2603 cj5array[0]=IKcos(j5array[0]);
2604 if( j5array[0] > IKPI )
2605 {
2606  j5array[0]-=IK2PI;
2607 }
2608 else if( j5array[0] < -IKPI )
2609 { j5array[0]+=IK2PI;
2610 }
2611 j5valid[0] = true;
2612 for(int ij5 = 0; ij5 < 1; ++ij5)
2613 {
2614 if( !j5valid[ij5] )
2615 {
2616  continue;
2617 }
2618 _ij5[0] = ij5; _ij5[1] = -1;
2619 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2620 {
2621 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2622 {
2623  j5valid[iij5]=false; _ij5[1] = iij5; break;
2624 }
2625 }
2626 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2627 {
2628 IkReal evalcond[8];
2629 IkReal x198=IKsin(j5);
2630 IkReal x199=IKcos(j5);
2631 IkReal x200=((IkReal(1.00000000000000))*(cj3));
2632 IkReal x201=((cj4)*(sj3));
2633 IkReal x202=((cj3)*(cj4));
2634 IkReal x203=((IkReal(1.00000000000000))*(x199));
2635 evalcond[0]=((new_r21)+(((sj4)*(x198))));
2636 evalcond[1]=((new_r20)+(((IkReal(-1.00000000000000))*(sj4)*(x203))));
2637 evalcond[2]=((x198)+(((IkReal(-1.00000000000000))*(new_r10)*(x200)))+(((new_r00)*(sj3))));
2638 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x200)))+(x199)+(((new_r01)*(sj3))));
2639 evalcond[4]=((((new_r11)*(sj3)))+(((cj4)*(x198)))+(((cj3)*(new_r01))));
2640 evalcond[5]=((((IkReal(-1.00000000000000))*(cj4)*(x203)))+(((new_r10)*(sj3)))+(((cj3)*(new_r00))));
2641 evalcond[6]=((((new_r01)*(x202)))+(((new_r21)*(sj4)))+(x198)+(((new_r11)*(x201))));
2642 evalcond[7]=((((new_r00)*(x202)))+(((IkReal(-1.00000000000000))*(x203)))+(((new_r20)*(sj4)))+(((new_r10)*(x201))));
2643 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 )
2644 {
2645 continue;
2646 }
2647 }
2648 
2649 {
2650 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2651 vinfos[0].jointtype = 1;
2652 vinfos[0].foffset = j0;
2653 vinfos[0].indices[0] = _ij0[0];
2654 vinfos[0].indices[1] = _ij0[1];
2655 vinfos[0].maxsolutions = _nj0;
2656 vinfos[1].jointtype = 1;
2657 vinfos[1].foffset = j1;
2658 vinfos[1].indices[0] = _ij1[0];
2659 vinfos[1].indices[1] = _ij1[1];
2660 vinfos[1].maxsolutions = _nj1;
2661 vinfos[2].jointtype = 1;
2662 vinfos[2].foffset = j2;
2663 vinfos[2].indices[0] = _ij2[0];
2664 vinfos[2].indices[1] = _ij2[1];
2665 vinfos[2].maxsolutions = _nj2;
2666 vinfos[3].jointtype = 1;
2667 vinfos[3].foffset = j3;
2668 vinfos[3].indices[0] = _ij3[0];
2669 vinfos[3].indices[1] = _ij3[1];
2670 vinfos[3].maxsolutions = _nj3;
2671 vinfos[4].jointtype = 1;
2672 vinfos[4].foffset = j4;
2673 vinfos[4].indices[0] = _ij4[0];
2674 vinfos[4].indices[1] = _ij4[1];
2675 vinfos[4].maxsolutions = _nj4;
2676 vinfos[5].jointtype = 1;
2677 vinfos[5].foffset = j5;
2678 vinfos[5].indices[0] = _ij5[0];
2679 vinfos[5].indices[1] = _ij5[1];
2680 vinfos[5].maxsolutions = _nj5;
2681 std::vector<int> vfree(0);
2682 solutions.AddSolution(vinfos,vfree);
2683 }
2684 }
2685 }
2686 
2687 }
2688 
2689 }
2690 }
2691 }
2692 
2693 }
2694 
2695 }
2696 
2697 } else
2698 {
2699 {
2700 IkReal j5array[1], cj5array[1], sj5array[1];
2701 bool j5valid[1]={false};
2702 _nj5 = 1;
2703 if( IKabs(((IkReal(-1.00000000000000))*(gconst4)*(new_r21))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(new_r20))) < IKFAST_ATAN2_MAGTHRESH )
2704  continue;
2705 j5array[0]=IKatan2(((IkReal(-1.00000000000000))*(gconst4)*(new_r21)), ((gconst4)*(new_r20)));
2706 sj5array[0]=IKsin(j5array[0]);
2707 cj5array[0]=IKcos(j5array[0]);
2708 if( j5array[0] > IKPI )
2709 {
2710  j5array[0]-=IK2PI;
2711 }
2712 else if( j5array[0] < -IKPI )
2713 { j5array[0]+=IK2PI;
2714 }
2715 j5valid[0] = true;
2716 for(int ij5 = 0; ij5 < 1; ++ij5)
2717 {
2718 if( !j5valid[ij5] )
2719 {
2720  continue;
2721 }
2722 _ij5[0] = ij5; _ij5[1] = -1;
2723 for(int iij5 = ij5+1; iij5 < 1; ++iij5)
2724 {
2725 if( j5valid[iij5] && IKabs(cj5array[ij5]-cj5array[iij5]) < IKFAST_SOLUTION_THRESH && IKabs(sj5array[ij5]-sj5array[iij5]) < IKFAST_SOLUTION_THRESH )
2726 {
2727  j5valid[iij5]=false; _ij5[1] = iij5; break;
2728 }
2729 }
2730 j5 = j5array[ij5]; cj5 = cj5array[ij5]; sj5 = sj5array[ij5];
2731 {
2732 IkReal evalcond[2];
2733 evalcond[0]=((new_r21)+(((sj4)*(IKsin(j5)))));
2734 evalcond[1]=((new_r20)+(((IkReal(-1.00000000000000))*(sj4)*(IKcos(j5)))));
2735 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 )
2736 {
2737 continue;
2738 }
2739 }
2740 
2741 {
2742 IkReal dummyeval[1];
2743 IkReal gconst7;
2744 gconst7=IKsign(((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10)))));
2745 dummyeval[0]=((((IkReal(-1.00000000000000))*(new_r00)*(new_r12)))+(((new_r02)*(new_r10))));
2746 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2747 {
2748 {
2749 IkReal dummyeval[1];
2750 IkReal gconst6;
2751 gconst6=IKsign(((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12))))));
2752 dummyeval[0]=((((IkReal(-1.00000000000000))*((new_r02)*(new_r02))))+(((IkReal(-1.00000000000000))*((new_r12)*(new_r12)))));
2753 if( IKabs(dummyeval[0]) < 0.0000010000000000 )
2754 {
2755 continue;
2756 
2757 } else
2758 {
2759 {
2760 IkReal j3array[1], cj3array[1], sj3array[1];
2761 bool j3valid[1]={false};
2762 _nj3 = 1;
2763 IkReal x204=((gconst6)*(sj4));
2764 if( IKabs(((new_r12)*(x204))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x204))) < IKFAST_ATAN2_MAGTHRESH )
2765  continue;
2766 j3array[0]=IKatan2(((new_r12)*(x204)), ((new_r02)*(x204)));
2767 sj3array[0]=IKsin(j3array[0]);
2768 cj3array[0]=IKcos(j3array[0]);
2769 if( j3array[0] > IKPI )
2770 {
2771  j3array[0]-=IK2PI;
2772 }
2773 else if( j3array[0] < -IKPI )
2774 { j3array[0]+=IK2PI;
2775 }
2776 j3valid[0] = true;
2777 for(int ij3 = 0; ij3 < 1; ++ij3)
2778 {
2779 if( !j3valid[ij3] )
2780 {
2781  continue;
2782 }
2783 _ij3[0] = ij3; _ij3[1] = -1;
2784 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2785 {
2786 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2787 {
2788  j3valid[iij3]=false; _ij3[1] = iij3; break;
2789 }
2790 }
2791 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2792 {
2793 IkReal evalcond[12];
2794 IkReal x205=IKsin(j3);
2795 IkReal x206=IKcos(j3);
2796 IkReal x207=((IkReal(1.00000000000000))*(cj4));
2797 IkReal x208=((cj4)*(x206));
2798 IkReal x209=((IkReal(1.00000000000000))*(x206));
2799 IkReal x210=((sj4)*(x206));
2800 IkReal x211=((cj4)*(x205));
2801 IkReal x212=((new_r11)*(x205));
2802 IkReal x213=((sj4)*(x205));
2803 evalcond[0]=((((new_r12)*(x206)))+(((IkReal(-1.00000000000000))*(new_r02)*(x205))));
2804 evalcond[1]=((sj4)+(((new_r12)*(x205)))+(((new_r02)*(x206))));
2805 evalcond[2]=((((new_r00)*(x205)))+(((IkReal(-1.00000000000000))*(new_r10)*(x209)))+(sj5));
2806 evalcond[3]=((cj5)+(((IkReal(-1.00000000000000))*(new_r11)*(x209)))+(((new_r01)*(x205))));
2807 evalcond[4]=((x212)+(((new_r01)*(x206)))+(((cj4)*(sj5))));
2808 evalcond[5]=((((IkReal(-1.00000000000000))*(cj5)*(x207)))+(((new_r00)*(x206)))+(((new_r10)*(x205))));
2809 evalcond[6]=((((new_r22)*(sj4)))+(((new_r12)*(x211)))+(((new_r02)*(x208))));
2810 evalcond[7]=((((IkReal(-1.00000000000000))*(new_r20)*(x207)))+(((new_r00)*(x210)))+(((new_r10)*(x213))));
2811 evalcond[8]=((((IkReal(-1.00000000000000))*(new_r21)*(x207)))+(((sj4)*(x212)))+(((new_r01)*(x210))));
2812 evalcond[9]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(new_r22)*(x207)))+(((new_r12)*(x213)))+(((new_r02)*(x210))));
2813 evalcond[10]=((((new_r21)*(sj4)))+(((new_r01)*(x208)))+(sj5)+(((new_r11)*(x211))));
2814 evalcond[11]=((((new_r10)*(x211)))+(((new_r20)*(sj4)))+(((IkReal(-1.00000000000000))*(cj5)))+(((new_r00)*(x208))));
2815 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001 )
2816 {
2817 continue;
2818 }
2819 }
2820 
2821 {
2822 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2823 vinfos[0].jointtype = 1;
2824 vinfos[0].foffset = j0;
2825 vinfos[0].indices[0] = _ij0[0];
2826 vinfos[0].indices[1] = _ij0[1];
2827 vinfos[0].maxsolutions = _nj0;
2828 vinfos[1].jointtype = 1;
2829 vinfos[1].foffset = j1;
2830 vinfos[1].indices[0] = _ij1[0];
2831 vinfos[1].indices[1] = _ij1[1];
2832 vinfos[1].maxsolutions = _nj1;
2833 vinfos[2].jointtype = 1;
2834 vinfos[2].foffset = j2;
2835 vinfos[2].indices[0] = _ij2[0];
2836 vinfos[2].indices[1] = _ij2[1];
2837 vinfos[2].maxsolutions = _nj2;
2838 vinfos[3].jointtype = 1;
2839 vinfos[3].foffset = j3;
2840 vinfos[3].indices[0] = _ij3[0];
2841 vinfos[3].indices[1] = _ij3[1];
2842 vinfos[3].maxsolutions = _nj3;
2843 vinfos[4].jointtype = 1;
2844 vinfos[4].foffset = j4;
2845 vinfos[4].indices[0] = _ij4[0];
2846 vinfos[4].indices[1] = _ij4[1];
2847 vinfos[4].maxsolutions = _nj4;
2848 vinfos[5].jointtype = 1;
2849 vinfos[5].foffset = j5;
2850 vinfos[5].indices[0] = _ij5[0];
2851 vinfos[5].indices[1] = _ij5[1];
2852 vinfos[5].maxsolutions = _nj5;
2853 std::vector<int> vfree(0);
2854 solutions.AddSolution(vinfos,vfree);
2855 }
2856 }
2857 }
2858 
2859 }
2860 
2861 }
2862 
2863 } else
2864 {
2865 {
2866 IkReal j3array[1], cj3array[1], sj3array[1];
2867 bool j3valid[1]={false};
2868 _nj3 = 1;
2869 IkReal x214=((gconst7)*(sj5));
2870 if( IKabs(((new_r12)*(x214))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((new_r02)*(x214))) < IKFAST_ATAN2_MAGTHRESH )
2871  continue;
2872 j3array[0]=IKatan2(((new_r12)*(x214)), ((new_r02)*(x214)));
2873 sj3array[0]=IKsin(j3array[0]);
2874 cj3array[0]=IKcos(j3array[0]);
2875 if( j3array[0] > IKPI )
2876 {
2877  j3array[0]-=IK2PI;
2878 }
2879 else if( j3array[0] < -IKPI )
2880 { j3array[0]+=IK2PI;
2881 }
2882 j3valid[0] = true;
2883 for(int ij3 = 0; ij3 < 1; ++ij3)
2884 {
2885 if( !j3valid[ij3] )
2886 {
2887  continue;
2888 }
2889 _ij3[0] = ij3; _ij3[1] = -1;
2890 for(int iij3 = ij3+1; iij3 < 1; ++iij3)
2891 {
2892 if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
2893 {
2894  j3valid[iij3]=false; _ij3[1] = iij3; break;
2895 }
2896 }
2897 j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
2898 {
2899 IkReal evalcond[12];
2900 IkReal x215=IKsin(j3);
2901 IkReal x216=IKcos(j3);
2902 IkReal x217=((IkReal(1.00000000000000))*(cj4));
2903 IkReal x218=((cj4)*(x216));
2904 IkReal x219=((IkReal(1.00000000000000))*(x216));
2905 IkReal x220=((sj4)*(x216));
2906 IkReal x221=((cj4)*(x215));
2907 IkReal x222=((new_r11)*(x215));
2908 IkReal x223=((sj4)*(x215));
2909 evalcond[0]=((((IkReal(-1.00000000000000))*(new_r02)*(x215)))+(((new_r12)*(x216))));
2910 evalcond[1]=((sj4)+(((new_r02)*(x216)))+(((new_r12)*(x215))));
2911 evalcond[2]=((sj5)+(((new_r00)*(x215)))+(((IkReal(-1.00000000000000))*(new_r10)*(x219))));
2912 evalcond[3]=((((IkReal(-1.00000000000000))*(new_r11)*(x219)))+(cj5)+(((new_r01)*(x215))));
2913 evalcond[4]=((((new_r01)*(x216)))+(x222)+(((cj4)*(sj5))));
2914 evalcond[5]=((((new_r10)*(x215)))+(((IkReal(-1.00000000000000))*(cj5)*(x217)))+(((new_r00)*(x216))));
2915 evalcond[6]=((((new_r22)*(sj4)))+(((new_r02)*(x218)))+(((new_r12)*(x221))));
2916 evalcond[7]=((((IkReal(-1.00000000000000))*(new_r20)*(x217)))+(((new_r00)*(x220)))+(((new_r10)*(x223))));
2917 evalcond[8]=((((IkReal(-1.00000000000000))*(new_r21)*(x217)))+(((sj4)*(x222)))+(((new_r01)*(x220))));
2918 evalcond[9]=((IkReal(1.00000000000000))+(((new_r12)*(x223)))+(((new_r02)*(x220)))+(((IkReal(-1.00000000000000))*(new_r22)*(x217))));
2919 evalcond[10]=((((new_r21)*(sj4)))+(sj5)+(((new_r11)*(x221)))+(((new_r01)*(x218))));
2920 evalcond[11]=((((new_r20)*(sj4)))+(((IkReal(-1.00000000000000))*(cj5)))+(((new_r10)*(x221)))+(((new_r00)*(x218))));
2921 if( IKabs(evalcond[0]) > 0.000001 || IKabs(evalcond[1]) > 0.000001 || IKabs(evalcond[2]) > 0.000001 || IKabs(evalcond[3]) > 0.000001 || IKabs(evalcond[4]) > 0.000001 || IKabs(evalcond[5]) > 0.000001 || IKabs(evalcond[6]) > 0.000001 || IKabs(evalcond[7]) > 0.000001 || IKabs(evalcond[8]) > 0.000001 || IKabs(evalcond[9]) > 0.000001 || IKabs(evalcond[10]) > 0.000001 || IKabs(evalcond[11]) > 0.000001 )
2922 {
2923 continue;
2924 }
2925 }
2926 
2927 {
2928 std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(6);
2929 vinfos[0].jointtype = 1;
2930 vinfos[0].foffset = j0;
2931 vinfos[0].indices[0] = _ij0[0];
2932 vinfos[0].indices[1] = _ij0[1];
2933 vinfos[0].maxsolutions = _nj0;
2934 vinfos[1].jointtype = 1;
2935 vinfos[1].foffset = j1;
2936 vinfos[1].indices[0] = _ij1[0];
2937 vinfos[1].indices[1] = _ij1[1];
2938 vinfos[1].maxsolutions = _nj1;
2939 vinfos[2].jointtype = 1;
2940 vinfos[2].foffset = j2;
2941 vinfos[2].indices[0] = _ij2[0];
2942 vinfos[2].indices[1] = _ij2[1];
2943 vinfos[2].maxsolutions = _nj2;
2944 vinfos[3].jointtype = 1;
2945 vinfos[3].foffset = j3;
2946 vinfos[3].indices[0] = _ij3[0];
2947 vinfos[3].indices[1] = _ij3[1];
2948 vinfos[3].maxsolutions = _nj3;
2949 vinfos[4].jointtype = 1;
2950 vinfos[4].foffset = j4;
2951 vinfos[4].indices[0] = _ij4[0];
2952 vinfos[4].indices[1] = _ij4[1];
2953 vinfos[4].maxsolutions = _nj4;
2954 vinfos[5].jointtype = 1;
2955 vinfos[5].foffset = j5;
2956 vinfos[5].indices[0] = _ij5[0];
2957 vinfos[5].indices[1] = _ij5[1];
2958 vinfos[5].maxsolutions = _nj5;
2959 std::vector<int> vfree(0);
2960 solutions.AddSolution(vinfos,vfree);
2961 }
2962 }
2963 }
2964 
2965 }
2966 
2967 }
2968 }
2969 }
2970 
2971 }
2972 
2973 }
2974 }
2975 }
2976 }
2977 }};
2978 
2979 
2982 IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
2983 IKSolver solver;
2984 return solver.ComputeIk(eetrans,eerot,pfree,solutions);
2985 }
2986 
2987 IKFAST_API const char* GetKinematicsHash() { return "<robot:genericrobot - fanuc_m20ia10l (2b7303feff17886f97f9c038c72e1091)>"; }
2988 
2989 IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }
2990 
2991 #ifdef IKFAST_NAMESPACE
2992 } // end namespace
2993 #endif
2994 
2995 #ifndef IKFAST_NO_MAIN
2996 #include <stdio.h>
2997 #include <stdlib.h>
2998 #ifdef IKFAST_NAMESPACE
2999 using namespace IKFAST_NAMESPACE;
3000 #endif
3001 int main(int argc, char** argv)
3002 {
3003  if( argc != 12+GetNumFreeParameters()+1 ) {
3004  printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
3005  "Returns the ik solutions given the transformation of the end effector specified by\n"
3006  "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
3007  "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
3008  return 1;
3009  }
3010 
3011  IkSolutionList<IkReal> solutions;
3012  std::vector<IkReal> vfree(GetNumFreeParameters());
3013  IkReal eerot[9],eetrans[3];
3014  eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
3015  eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
3016  eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
3017  for(std::size_t i = 0; i < vfree.size(); ++i)
3018  vfree[i] = atof(argv[13+i]);
3019  bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
3020 
3021  if( !bSuccess ) {
3022  fprintf(stderr,"Failed to get ik solution\n");
3023  return -1;
3024  }
3025 
3026  printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
3027  std::vector<IkReal> solvalues(GetNumJoints());
3028  for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
3029  const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
3030  printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
3031  std::vector<IkReal> vsolfree(sol.GetFree().size());
3032  sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
3033  for( std::size_t j = 0; j < solvalues.size(); ++j)
3034  printf("%.15f, ", solvalues[j]);
3035  printf("\n");
3036  }
3037  return 0;
3038 }
3039 
3040 #endif
IKfmod
float IKfmod(float x, float y)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:128
ikfast::IkSolutionList::GetSolution
virtual const IkSolutionBase< T > & GetSolution(size_t index) const
returns the solution pointer
Definition: m20ia10l_kinematics/include/ikfast.h:239
IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:104
IKtan
float IKtan(float f)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:163
IKacos
float IKacos(float f)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:145
GetIkFastVersion
IKFAST_API const char * GetIkFastVersion()
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:2989
log
INLINE Rall1d< T, V, S > log(const Rall1d< T, V, S > &arg)
main
int main(int argc, char **argv)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:3001
GetNumJoints
IKFAST_API int GetNumJoints()
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:278
ikfast::IkSolutionList::GetNumSolutions
virtual size_t GetNumSolutions() const
returns the number of solutions stored
Definition: m20ia10l_kinematics/include/ikfast.h:249
ikfast::IkSolutionBase
The discrete solutions are returned in this structure.
Definition: m20ia10l_kinematics/include/ikfast.h:65
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...
IKSolver::rotationfunction0
void rotationfunction0(IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:597
IKsin
float IKsin(float f)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:159
IKcos
float IKcos(float f)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:161
ComputeFk
IKFAST_API void ComputeFk(const IkReal *j, IkReal *eetrans, IkReal *eerot)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:210
IKPI
#define IKPI
Definition: fanuc_m20ia10l_manipulator_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
IKsqr
float IKsqr(float f)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:91
IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:109
IKsqrt
float IKsqrt(float f)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:165
GetFreeParameters
IKFAST_API int * GetFreeParameters()
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:277
IKFAST_ASSERT
#define IKFAST_ASSERT(b)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:52
IKFAST_COMPILE_ASSERT
#define IKFAST_COMPILE_ASSERT(x)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:25
asin
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
GetNumFreeParameters
IKFAST_API int GetNumFreeParameters()
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:276
IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:99
ikfast::IkSolutionListBase::Clear
virtual void Clear()=0
clears all current solutions, note that any memory addresses returned from GetSolution will be invali...
dgetrf_
void dgetrf_(const int *m, const int *n, double *a, const int *lda, int *ipiv, int *info)
IK2PI
#define IK2PI
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:62
sqrt
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
IKlog
float IKlog(float f)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:94
ikfast::IkSolutionBase::GetSolution
virtual void GetSolution(T *solution, const T *freevalues) const =0
gets a concrete solution
IKSolver::ComputeIk
bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:289
ComputeIk
IKFAST_API bool ComputeIk(const IkReal *eetrans, const IkReal *eerot, const IkReal *pfree, IkSolutionListBase< IkReal > &solutions)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:2982
IKatan2
float IKatan2(float fy, float fx)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:167
acos
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
dgeev_
void dgeev_(const char *jobvl, const char *jobvr, const int *n, double *a, const int *lda, double *wr, double *wi, double *vl, const int *ldvl, double *vr, const int *ldvr, double *work, const int *lwork, int *info)
ikfast::IkSolutionList
Default implementation of IkSolutionListBase.
Definition: m20ia10l_kinematics/include/ikfast.h:229
atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
GetIkType
IKFAST_API int GetIkType()
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:282
IKsign
float IKsign(float f)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:188
dgetri_
void dgetri_(const int *n, const double *a, const int *lda, int *ipiv, double *work, const int *lwork, int *info)
dgesv_
void dgesv_(const int *n, const int *nrhs, double *a, const int *lda, int *ipiv, double *b, const int *ldb, int *info)
IKPI_2
#define IKPI_2
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:64
IKasin
float IKasin(float f)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:112
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)
GetIkRealSize
IKFAST_API int GetIkRealSize()
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:280
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)
IKabs
float IKabs(float f)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:88
IKFAST_STRINGIZE
#define IKFAST_STRINGIZE(s)
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:35
zgetrf_
void zgetrf_(const int *m, const int *n, std::complex< double > *a, const int *lda, int *ipiv, int *info)
sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
ikfast::IkSolutionListBase::GetNumSolutions
virtual size_t GetNumSolutions() const =0
returns the number of solutions stored
GetKinematicsHash
IKFAST_API const char * GetKinematicsHash()
Definition: fanuc_m20ia10l_manipulator_ikfast_solver.cpp:2987
IKFAST_VERSION
#define IKFAST_VERSION
Header file for all ikfast c++ files/shared objects.
Definition: m20ia10l_kinematics/include/ikfast.h:41

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