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

moveit_opw_kinematics_plugin
Author(s): Jeroen De Maeyer, Simon Schmeisser (isys vision)
autogenerated on Wed Jun 3 2020 03:17:14