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

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