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

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