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


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