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

abb_irb2400_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute), Jeremy Zoss
autogenerated on Tue Jun 2 2020 03:19:55