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

staubli_rx160_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Sun Jan 19 2020 03:35:16