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

fanuc_lrmate200id_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Tue Mar 23 2021 02:10:01