fanuc_m430ia2f_manipulator_ikfast_solver.cpp

Go to the documentation of this file.

#define IKFAST_HAS_LIBRARY
#include "ikfast.h" // found inside share/openrave-X.Y/python/ikfast.h
using namespace ikfast;

// check if the included ikfast version matches what this file was compiled with
#define IKFAST_COMPILE_ASSERT(x) extern int __dummy[(int)x]
IKFAST_COMPILE_ASSERT(IKFAST_VERSION==61);

#include <cmath>
#include <vector>
#include <limits>
#include <algorithm>
#include <complex>

#define IKFAST_STRINGIZE2(s) #s
#define IKFAST_STRINGIZE(s) IKFAST_STRINGIZE2(s)

#ifndef IKFAST_ASSERT
#include <stdexcept>
#include <sstream>
#include <iostream>

#ifdef _MSC_VER
#ifndef __PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__ __FUNCDNAME__
#endif
#endif

#ifndef __PRETTY_FUNCTION__
#define __PRETTY_FUNCTION__ __func__
#endif

#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()); } }

#endif

#if defined(_MSC_VER)
#define IKFAST_ALIGNED16(x) __declspec(align(16)) x
#else
#define IKFAST_ALIGNED16(x) x __attribute((aligned(16)))
#endif

#define IK2PI  ((IkReal)6.28318530717959)
#define IKPI  ((IkReal)3.14159265358979)
#define IKPI_2  ((IkReal)1.57079632679490)

#ifdef _MSC_VER
#ifndef isnan
#define isnan _isnan
#endif
#endif // _MSC_VER

// lapack routines
extern "C" {
  void dgetrf_ (const int* m, const int* n, double* a, const int* lda, int* ipiv, int* info);
  void zgetrf_ (const int* m, const int* n, std::complex<double>* a, const int* lda, int* ipiv, int* info);
  void dgetri_(const int* n, const double* a, const int* lda, int* ipiv, double* work, const int* lwork, int* info);
  void dgesv_ (const int* n, const int* nrhs, double* a, const int* lda, int* ipiv, double* b, const int* ldb, int* info);
  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);
  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);
}

using namespace std; // necessary to get std math routines

#ifdef IKFAST_NAMESPACE
namespace IKFAST_NAMESPACE {
#endif

inline float IKabs(float f) { return fabsf(f); }
inline double IKabs(double f) { return fabs(f); }

inline float IKsqr(float f) { return f*f; }
inline double IKsqr(double f) { return f*f; }

inline float IKlog(float f) { return logf(f); }
inline double IKlog(double f) { return log(f); }

// allows asin and acos to exceed 1
#ifndef IKFAST_SINCOS_THRESH
#define IKFAST_SINCOS_THRESH ((IkReal)0.000001)
#endif

// used to check input to atan2 for degenerate cases
#ifndef IKFAST_ATAN2_MAGTHRESH
#define IKFAST_ATAN2_MAGTHRESH ((IkReal)2e-6)
#endif

// minimum distance of separate solutions
#ifndef IKFAST_SOLUTION_THRESH
#define IKFAST_SOLUTION_THRESH ((IkReal)1e-6)
#endif

inline float IKasin(float f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return float(-IKPI_2);
else if( f >= 1 ) return float(IKPI_2);
return asinf(f);
}
inline double IKasin(double f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return -IKPI_2;
else if( f >= 1 ) return IKPI_2;
return asin(f);
}

// return positive value in [0,y)
inline float IKfmod(float x, float y)
{
    while(x < 0) {
        x += y;
    }
    return fmodf(x,y);
}

// return positive value in [0,y)
inline double IKfmod(double x, double y)
{
    while(x < 0) {
        x += y;
    }
    return fmod(x,y);
}

inline float IKacos(float f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return float(IKPI);
else if( f >= 1 ) return float(0);
return acosf(f);
}
inline double IKacos(double f)
{
IKFAST_ASSERT( f > -1-IKFAST_SINCOS_THRESH && f < 1+IKFAST_SINCOS_THRESH ); // any more error implies something is wrong with the solver
if( f <= -1 ) return IKPI;
else if( f >= 1 ) return 0;
return acos(f);
}
inline float IKsin(float f) { return sinf(f); }
inline double IKsin(double f) { return sin(f); }
inline float IKcos(float f) { return cosf(f); }
inline double IKcos(double f) { return cos(f); }
inline float IKtan(float f) { return tanf(f); }
inline double IKtan(double f) { return tan(f); }
inline float IKsqrt(float f) { if( f <= 0.0f ) return 0.0f; return sqrtf(f); }
inline double IKsqrt(double f) { if( f <= 0.0 ) return 0.0; return sqrt(f); }
inline float IKatan2(float fy, float fx) {
    if( isnan(fy) ) {
        IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
        return float(IKPI_2);
    }
    else if( isnan(fx) ) {
        return 0;
    }
    return atan2f(fy,fx);
}
inline double IKatan2(double fy, double fx) {
    if( isnan(fy) ) {
        IKFAST_ASSERT(!isnan(fx)); // if both are nan, probably wrong value will be returned
        return IKPI_2;
    }
    else if( isnan(fx) ) {
        return 0;
    }
    return atan2(fy,fx);
}

inline float IKsign(float f) {
    if( f > 0 ) {
        return float(1);
    }
    else if( f < 0 ) {
        return float(-1);
    }
    return 0;
}

inline double IKsign(double f) {
    if( f > 0 ) {
        return 1.0;
    }
    else if( f < 0 ) {
        return -1.0;
    }
    return 0;
}

IKFAST_API void ComputeFk(const IkReal* j, IkReal* eetrans, IkReal* eerot) {
IkReal x0,x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11,x12,x13,x14,x15,x16,x17,x18,x19,x20;
x0=IKcos(j[0]);
x1=IKcos(j[2]);
x2=IKsin(j[1]);
x3=IKcos(j[1]);
x4=IKsin(j[2]);
x5=IKsin(j[0]);
x6=IKcos(j[3]);
x7=IKsin(j[3]);
x8=IKcos(j[4]);
x9=IKsin(j[4]);
x10=((IkReal(1.00000000000000))*(x2));
x11=((IkReal(0.550000000000000))*(x3));
x12=((IkReal(0.0650000000000000))*(x3));
x13=((IkReal(0.550000000000000))*(x2));
x14=((IkReal(1.00000000000000))*(x3));
x15=((IkReal(0.0650000000000000))*(x2));
x16=((IkReal(0.350000000000000))*(x2));
x17=((x4)*(x5));
x18=((x0)*(x4));
x19=((x0)*(x1));
x20=((x1)*(x5));
eetrans[0]=((((x0)*(x16)))+(((x11)*(x18)))+(((IkReal(0.0950000000000000))*(x5)))+(((x6)*(((((x12)*(x18)))+(((x15)*(x19)))))))+(((x13)*(x19)))+(((x7)*(((((x12)*(x19)))+(((IkReal(-1.00000000000000))*(x15)*(x18))))))));
eetrans[1]=((((x16)*(x5)))+(((x7)*(((((x12)*(x20)))+(((IkReal(-1.00000000000000))*(x15)*(x17)))))))+(((IkReal(-0.0950000000000000))*(x0)))+(((x11)*(x17)))+(((x6)*(((((x15)*(x20)))+(((x12)*(x17)))))))+(((x13)*(x20))));
IkReal x21=((IkReal(1.00000000000000))*(x4));
eetrans[2]=((IkReal(0.440000000000000))+(((IkReal(0.350000000000000))*(x3)))+(((x7)*(((((IkReal(-1.00000000000000))*(x1)*(x15)))+(((IkReal(-1.00000000000000))*(x12)*(x21)))))))+(((x1)*(x11)))+(((x6)*(((((x1)*(x12)))+(((IkReal(-1.00000000000000))*(x15)*(x21)))))))+(((IkReal(-1.00000000000000))*(x13)*(x21))));
IkReal x22=((IkReal(1.00000000000000))*(x10));
eerot[0]=((((IkReal(-1.00000000000000))*(x5)*(x9)))+(((x8)*(((((x7)*(((((IkReal(-1.00000000000000))*(x19)*(x22)))+(((IkReal(-1.00000000000000))*(x14)*(x18)))))))+(((x6)*(((((x19)*(x3)))+(((IkReal(-1.00000000000000))*(x18)*(x22))))))))))));
IkReal x23=((IkReal(1.00000000000000))*(x10));
eerot[1]=((((x0)*(x9)))+(((x8)*(((((x6)*(((((x20)*(x3)))+(((IkReal(-1.00000000000000))*(x17)*(x23)))))))+(((x7)*(((((IkReal(-1.00000000000000))*(x20)*(x23)))+(((IkReal(-1.00000000000000))*(x14)*(x17))))))))))));
IkReal x24=((IkReal(1.00000000000000))*(x1));
eerot[2]=((x8)*(((((x6)*(((((IkReal(-1.00000000000000))*(x14)*(x4)))+(((IkReal(-1.00000000000000))*(x10)*(x24)))))))+(((x7)*(((((x2)*(x4)))+(((IkReal(-1.00000000000000))*(x14)*(x24))))))))));
}

IKFAST_API int GetNumFreeParameters() { return 0; }
IKFAST_API int* GetFreeParameters() { return NULL; }
IKFAST_API int GetNumJoints() { return 5; }

IKFAST_API int GetIkRealSize() { return sizeof(IkReal); }

IKFAST_API int GetIkType() { return 0x56000007; }

class IKSolver {
public:
IkReal j0,cj0,sj0,htj0,j1,cj1,sj1,htj1,j2,cj2,sj2,htj2,j3,cj3,sj3,htj3,j4,cj4,sj4,htj4,new_r00,r00,rxp0_0,new_r01,r01,rxp0_1,new_r02,r02,rxp0_2,new_px,px,npx,new_py,py,npy,new_pz,pz,npz,pp;
unsigned char _ij0[2], _nj0,_ij1[2], _nj1,_ij2[2], _nj2,_ij3[2], _nj3,_ij4[2], _nj4;

bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
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; 
for(int dummyiter = 0; dummyiter < 1; ++dummyiter) {
    solutions.Clear();
px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];

r00 = eerot[0];
r01 = eerot[1];
r02 = eerot[2];
px = eetrans[0]; py = eetrans[1]; pz = eetrans[2];
new_r00=r00;
new_px=px;
new_r01=r01;
new_py=py;
new_r02=r02;
new_pz=((IkReal(-0.440000000000000))+(pz));
r00 = new_r00; r01 = new_r01; r02 = new_r02; px = new_px; py = new_py; pz = new_pz;

pp=(((px)*(px))+((pz)*(pz))+((py)*(py)));
{
IkReal dummyeval[1];
dummyeval[0]=(((px)*(px))+((py)*(py)));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IkReal j0array[2], cj0array[2], sj0array[2];
bool j0valid[2]={false};
_nj0 = 2;
if( IKabs(py) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-1.00000000000000))*(px))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
IkReal x25=((IkReal(1.00000000000000))*(IKatan2(py, ((IkReal(-1.00000000000000))*(px)))));
if( ((((px)*(px))+((py)*(py)))) < (IkReal)-0.00001 )
    continue;
if( (((IkReal(0.0950000000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30)))) < -1-IKFAST_SINCOS_THRESH || (((IkReal(0.0950000000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30)))) > 1+IKFAST_SINCOS_THRESH )
    continue;
IkReal x26=IKasin(((IkReal(0.0950000000000000))*(((IKabs(IKabs(IKsqrt((((px)*(px))+((py)*(py)))))) != 0)?((IkReal)1/(IKabs(IKsqrt((((px)*(px))+((py)*(py))))))):(IkReal)1.0e30))));
j0array[0]=((((IkReal(-1.00000000000000))*(x26)))+(((IkReal(-1.00000000000000))*(x25))));
sj0array[0]=IKsin(j0array[0]);
cj0array[0]=IKcos(j0array[0]);
j0array[1]=((IkReal(3.14159265358979))+(x26)+(((IkReal(-1.00000000000000))*(x25))));
sj0array[1]=IKsin(j0array[1]);
cj0array[1]=IKcos(j0array[1]);
if( j0array[0] > IKPI )
{
    j0array[0]-=IK2PI;
}
else if( j0array[0] < -IKPI )
{    j0array[0]+=IK2PI;
}
j0valid[0] = true;
if( j0array[1] > IKPI )
{
    j0array[1]-=IK2PI;
}
else if( j0array[1] < -IKPI )
{    j0array[1]+=IK2PI;
}
j0valid[1] = true;
for(int ij0 = 0; ij0 < 2; ++ij0)
{
if( !j0valid[ij0] )
{
    continue;
}
_ij0[0] = ij0; _ij0[1] = -1;
for(int iij0 = ij0+1; iij0 < 2; ++iij0)
{
if( j0valid[iij0] && IKabs(cj0array[ij0]-cj0array[iij0]) < IKFAST_SOLUTION_THRESH && IKabs(sj0array[ij0]-sj0array[iij0]) < IKFAST_SOLUTION_THRESH )
{
    j0valid[iij0]=false; _ij0[1] = iij0; break; 
}
}
j0 = j0array[ij0]; cj0 = cj0array[ij0]; sj0 = sj0array[ij0];

IkReal op[4+1], zeror[4];
int numroots;
op[0]=((((IkReal(-2.80000000000000))*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(0.163324000000000))*(px)*(sj0)*((r01)*(r01))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(0.266000000000000))*(cj0)*(r01)*(r02)*((py)*(py))))+(((IkReal(-0.369600000000000))*(pp)*((r02)*(r02))))+(((IkReal(0.133000000000000))*(cj0)*(pp)*(r01)*(r02)))+(((IkReal(-0.532000000000000))*(px)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(-0.240800000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(0.760000000000000))*(cj0)*(py)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.380000000000000))*(cj0)*(pp)*(px)*(r00)*(r01)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-0.0341510400000000))*((r02)*(r02))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.258720000000000))*(py)*(r01)*(r02)))+(((IkReal(0.279200000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.00442225000000000))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(1.71920000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.0361000000000000))*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0228760000000000))*(cj0)*(pz)*(r01)*(r02)))+(((IkReal(-0.380000000000000))*(pp)*(py)*(r00)*(r01)*(sj0)))+(((IkReal(-2.80000000000000))*(pz)*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-8.00000000000000))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.266000000000000))*(cj0)*(r01)*(r02)*((px)*(px))))+(((IkReal(0.760000000000000))*(cj0)*(py)*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.760000000000000))*(cj0)*(pz)*(r01)*(r02)*((py)*(py))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.0252700000000000))*(pz)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.266000000000000))*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.532000000000000))*(px)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(0.0931000000000000))*(px)*(sj0)*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.720000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(1.51220000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.760000000000000))*(cj0)*(py)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(cj0)*(py)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(cj0)*(pz)*(r01)*(r02)*((px)*(px))))+(((IkReal(2.80000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.720000000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.0722000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(2.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(-2.80000000000000))*(pz)*((py)*(py))*((r01)*(r01))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-1.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(-0.760000000000000))*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(1.71920000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.266000000000000))*(px)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-1.52000000000000))*(px)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.760000000000000))*(py)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.0702240000000000))*(px)*(sj0)*((r02)*(r02))))+(((IkReal(4.00000000000000))*(pp)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(1.71920000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(1.71920000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.0702240000000000))*(cj0)*(px)*(r00)*(r01)))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(0.0228760000000000))*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.380000000000000))*(cj0)*(pp)*(pz)*(r01)*(r02)))+(((IkReal(1.71920000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(0.760000000000000))*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.266000000000000))*(cj0)*(px)*(pz)*(r00)*(r01)))+(((IkReal(-0.0641615800000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(4.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.541100000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-0.0361000000000000))*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.0365030400000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-2.80000000000000))*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(2.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.245280000000000))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-5.60000000000000))*(px)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-2.80000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-16.0000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-4.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-2.80000000000000))*(cj0)*(pp)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(2.80000000000000))*(pz)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.380000000000000))*(pp)*(px)*(sj0)*((r01)*(r01))))+(((IkReal(1.40000000000000))*(pp)*(pz)*((r02)*(r02))))+(((IkReal(-0.270550000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-0.760000000000000))*(cj0)*(r00)*(r01)*((px)*(px)*(px))))+(((IkReal(-0.245280000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.240800000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(-0.380000000000000))*(cj0)*(pp)*(py)*((r00)*(r00))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(0.532000000000000))*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(0.720000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-5.60000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-0.859600000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0361000000000000))*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(0.279200000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(py)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(0.0252700000000000))*(px)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.490000000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(-5.60000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(1.40000000000000))*(pp)*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.249200000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.760000000000000))*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.0361000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.266000000000000))*(px)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-2.80000000000000))*((pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.245280000000000))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.207000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0361000000000000))*((py)*(py))*((r00)*(r00))))+(((IkReal(-1.40000000000000))*(pp)*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.207000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.266000000000000))*(py)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.258720000000000))*(pz)*((r02)*(r02))))+(((IkReal(0.760000000000000))*(px)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.258720000000000))*(px)*(r00)*(r02)))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.270550000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-0.907800000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(1.52000000000000))*(cj0)*(px)*(py)*(pz)*(r00)*(r02)))+(((IkReal(0.760000000000000))*(px)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(2.80000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-0.380000000000000))*(pp)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.760000000000000))*(cj0)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-0.163324000000000))*(cj0)*(py)*((r00)*(r00))))+(((IkReal(0.720000000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-2.80000000000000))*(pz)*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-5.60000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(cj0)*(py)*(pz)*((r01)*(r01))))+(((IkReal(-2.00000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pp)*(pp))))+(((IkReal(-0.859600000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(1.40000000000000))*(pp)*(py)*(r01)*(r02)))+(((IkReal(-1.71920000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(-0.133000000000000))*(pp)*(r00)*(r02)*(sj0)))+(((IkReal(2.80000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(2.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(2.80000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(2.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.760000000000000))*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(1.40000000000000))*(cj0)*(pp)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(0.0252700000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.490000000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(2.80000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-8.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.51220000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.760000000000000))*(px)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0931000000000000))*(cj0)*(py)*((r01)*(r01))))+(((IkReal(-2.80000000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.760000000000000))*(px)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.380000000000000))*(pp)*(px)*(sj0)*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.380000000000000))*(cj0)*(pp)*(py)*((r02)*(r02))))+(((IkReal(-0.760000000000000))*(cj0)*(px)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-5.60000000000000))*(px)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(0.532000000000000))*(cj0)*(px)*(py)*(r00)*(r02)))+(((IkReal(-0.266000000000000))*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(2.80000000000000))*(py)*(r01)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(0.245280000000000))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(0.266000000000000))*(cj0)*(py)*(pz)*((r02)*(r02))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.0722000000000000))*(px)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(1.40000000000000))*(cj0)*(pp)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(2.80000000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(-5.60000000000000))*(py)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-0.0365030400000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-1.40000000000000))*(pp)*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.0361000000000000))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.0252700000000000))*(pz)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-0.0702240000000000))*(cj0)*(py)*((r02)*(r02))))+(((IkReal(-2.80000000000000))*(px)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(0.0245784000000000))*(cj0)*(r01)*(r02)))+(((IkReal(-0.532000000000000))*(cj0)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-0.0702240000000000))*(py)*(r00)*(r01)*(sj0)))+(((IkReal(0.760000000000000))*(cj0)*(px)*(r00)*(r01)*((py)*(py))))+(((IkReal(-0.00442225000000000))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.266000000000000))*(cj0)*(py)*(pz)*((r00)*(r00))))+(((IkReal(1.40000000000000))*(pp)*(px)*(r00)*(r02)))+(((IkReal(-0.0245784000000000))*(r00)*(r02)*(sj0)))+(((IkReal(1.40000000000000))*(pp)*(px)*(r00)*(r02)*((cj0)*(cj0)))));
op[1]=((((IkReal(5.60000000000000))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(cj0)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(11.2000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.266000000000000))*(pp)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(5.60000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(5.60000000000000))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(0.532000000000000))*(px)*(py)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(0.186200000000000))*(cj0)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(5.60000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.186200000000000))*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.490560000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(0.532000000000000))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(5.60000000000000))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(1.96000000000000))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.186200000000000))*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.0491568000000000))*(cj0)*(sj0)*((r00)*(r00))))+(((IkReal(1.96000000000000))*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-0.186200000000000))*(cj0)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(1.96000000000000))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(2.80000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-1.96000000000000))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(2.80000000000000))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(-0.532000000000000))*(px)*(py)*((r00)*(r00))))+(((IkReal(5.60000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.186200000000000))*(cj0)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(0.532000000000000))*(px)*(py)*((cj0)*(cj0))*((r02)*(r02))))+(((IkReal(1.96000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-0.532000000000000))*(py)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-2.80000000000000))*(cj0)*(pp)*(py)*(r00)*(r01)))+(((IkReal(-0.532000000000000))*(px)*(py)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.532000000000000))*(px)*(pz)*(r01)*(r02)))+(((IkReal(5.60000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.0505400000000000))*(cj0)*(px)*((r01)*(r01))))+(((IkReal(-5.60000000000000))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.00940800000000000))*(r01)*(r02)*(sj0)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((py)*(py))))+(((IkReal(5.60000000000000))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(1.96000000000000))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(0.266000000000000))*(cj0)*(pp)*(sj0)*((r00)*(r00))))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((py)*(py))*((r02)*(r02))))+(((IkReal(0.0505400000000000))*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.0491568000000000))*(cj0)*(sj0)*((r01)*(r01))))+(((IkReal(1.96000000000000))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-0.186200000000000))*(py)*(r00)*(r02)))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.517440000000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(1.96000000000000))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-2.80000000000000))*(pp)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.567980000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(5.60000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.0491568000000000))*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.186200000000000))*(py)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-5.60000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-2.80000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(1.96000000000000))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(1.96000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(5.60000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.266000000000000))*(cj0)*(pp)*(sj0)*((r01)*(r01))))+(((IkReal(-0.490560000000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(0.186200000000000))*(cj0)*(px)*(r00)*(r02)*(sj0)))+(((IkReal(0.266000000000000))*(pp)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(5.60000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.186200000000000))*(px)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-1.96000000000000))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(1.96000000000000))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(-0.0268800000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(0.532000000000000))*(r00)*(r01)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.532000000000000))*(px)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(5.60000000000000))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-2.80000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((IkReal(0.532000000000000))*(px)*(py)*((r01)*(r01))))+(((IkReal(-0.0268800000000000))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(11.2000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(-0.00940800000000000))*(cj0)*(r00)*(r02)))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(py)*(pz)*(r00)*(r02)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.517440000000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-0.567980000000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(0.532000000000000))*(cj0)*(px)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(5.60000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.0491568000000000))*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.186200000000000))*(px)*(r01)*(r02)))+(((IkReal(1.96000000000000))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-0.532000000000000))*(px)*(py)*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.532000000000000))*(r00)*(r01)*((px)*(px))))+(((IkReal(5.60000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(1.96000000000000))*(cj0)*(px)*(py)*(r01)*(r02))));
op[2]=((((IkReal(-16.0000000000000))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.0722000000000000))*((py)*(py))*((r00)*(r00))))+(((IkReal(8.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-32.0000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-16.0000000000000))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-8.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(1.44000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.44000000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.0722000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.231952000000000))*(py)*(r00)*(r01)*(sj0)))+(((IkReal(-1.52000000000000))*(px)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0777100800000000))*((r02)*(r02))))+(((IkReal(-1.52000000000000))*(cj0)*(py)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(0.760000000000000))*(pp)*(px)*(sj0)*((r02)*(r02))))+(((IkReal(1.52000000000000))*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(8.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.760000000000000))*(pp)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.481600000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-16.0000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.760000000000000))*(pp)*(px)*(sj0)*((r01)*(r01))))+(((IkReal(-0.481600000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.52000000000000))*(cj0)*(r00)*(r01)*((px)*(px)*(px))))+(((IkReal(-1.52000000000000))*(cj0)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(3.43840000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.326648000000000))*(cj0)*(pz)*(r01)*(r02)))+(((IkReal(-2.00000000000000))*((pp)*(pp))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.326648000000000))*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0635980800000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(1.52000000000000))*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-2.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(0.240800000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.144400000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.481600000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-1.52000000000000))*(py)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-16.0000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.0635980800000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.760000000000000))*(pp)*(py)*(r00)*(r01)*(sj0)))+(((IkReal(8.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(8.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(8.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.481600000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(0.760000000000000))*(cj0)*(pp)*(px)*(r00)*(r01)))+(((IkReal(-32.0000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.481600000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(4.41840000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.52000000000000))*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(-0.00884450000000000))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-0.512848000000000))*(cj0)*(py)*((r02)*(r02))))+(((IkReal(-0.0722000000000000))*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-2.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(-0.109507160000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(1.52000000000000))*(cj0)*(px)*(r00)*(r01)*((py)*(py))))+(((IkReal(-0.00884450000000000))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-4.00000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pp)*(pp))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.186200000000000))*(cj0)*(py)*((r01)*(r01))))+(((IkReal(1.52000000000000))*(cj0)*(py)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.481600000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(3.02440000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-3.50600000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.980000000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(1.52000000000000))*(px)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-3.36160000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-16.0000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(1.52000000000000))*(py)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-1.52000000000000))*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.760000000000000))*(cj0)*(pp)*(py)*((r00)*(r00))))+(((IkReal(-0.0722000000000000))*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(-1.52000000000000))*(cj0)*(pz)*(r01)*(r02)*((px)*(px))))+(((IkReal(3.02440000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-1.81560000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(-1.52000000000000))*(px)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.0457520000000000))*(cj0)*(py)*((r00)*(r00))))+(((IkReal(0.186200000000000))*(px)*(sj0)*((r00)*(r00))))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.144400000000000))*(px)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.481600000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(3.04000000000000))*(cj0)*(px)*(py)*(pz)*(r00)*(r02)))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(-2.69920000000000))*(pp)*((r02)*(r02))))+(((IkReal(1.44000000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(3.43840000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-1.52000000000000))*(cj0)*(px)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(1.52000000000000))*(cj0)*(py)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-16.0000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(0.512848000000000))*(px)*(sj0)*((r02)*(r02))))+(((IkReal(-3.36160000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(1.52000000000000))*(cj0)*(pz)*(r01)*(r02)*((py)*(py))))+(((IkReal(8.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.980000000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.0457520000000000))*(px)*(sj0)*((r01)*(r01))))+(((IkReal(1.52000000000000))*(px)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(-32.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(1.44000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.481600000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.231952000000000))*(cj0)*(px)*(r00)*(r01)))+(((IkReal(-0.0722000000000000))*((px)*(px))*((r01)*(r01))))+(((IkReal(-8.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-0.481600000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(8.00000000000000))*(pp)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.240800000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.760000000000000))*(cj0)*(pp)*(pz)*(r01)*(r02)))+(((IkReal(-1.52000000000000))*(cj0)*(py)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(cj0)*(pp)*(py)*((r02)*(r02))))+(((IkReal(8.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-8.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-3.04000000000000))*(px)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(8.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-8.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0722000000000000))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-3.50600000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(8.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02))));
op[3]=((((IkReal(5.60000000000000))*(py)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(cj0)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(11.2000000000000))*(px)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-0.266000000000000))*(pp)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(5.60000000000000))*(cj0)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(5.60000000000000))*(py)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(0.532000000000000))*(px)*(py)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(5.60000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-0.490560000000000))*(py)*(sj0)*((r02)*(r02))))+(((IkReal(-0.186200000000000))*(px)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(0.186200000000000))*(cj0)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(-0.186200000000000))*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.532000000000000))*(r00)*(r01)*((cj0)*(cj0))*((pz)*(pz))))+(((IkReal(5.60000000000000))*(cj0)*(px)*((py)*(py))*((r01)*(r01))))+(((IkReal(0.0491568000000000))*(cj0)*(sj0)*((r00)*(r00))))+(((IkReal(2.80000000000000))*(pp)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(0.186200000000000))*(cj0)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(2.80000000000000))*(cj0)*(pp)*(px)*((r02)*(r02))))+(((IkReal(-0.532000000000000))*(px)*(py)*((r00)*(r00))))+(((IkReal(5.60000000000000))*(px)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.00940800000000000))*(cj0)*(r00)*(r02)))+(((IkReal(-1.96000000000000))*(cj0)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(0.532000000000000))*(px)*(py)*((cj0)*(cj0))*((r02)*(r02))))+(((IkReal(-0.532000000000000))*(py)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-2.80000000000000))*(cj0)*(pp)*(py)*(r00)*(r01)))+(((IkReal(1.96000000000000))*(cj0)*(pp)*(r00)*(r02)))+(((IkReal(-0.532000000000000))*(px)*(py)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-1.96000000000000))*(cj0)*(py)*(pz)*(r00)*(r01)))+(((IkReal(-0.186200000000000))*(cj0)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.532000000000000))*(px)*(pz)*(r01)*(r02)))+(((IkReal(5.60000000000000))*(cj0)*(px)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-1.96000000000000))*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.0505400000000000))*(cj0)*(px)*((r01)*(r01))))+(((IkReal(-5.60000000000000))*(pp)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-1.96000000000000))*(px)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((py)*(py))))+(((IkReal(5.60000000000000))*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(0.186200000000000))*(py)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.266000000000000))*(cj0)*(pp)*(sj0)*((r00)*(r00))))+(((IkReal(-1.96000000000000))*(px)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((py)*(py))*((r02)*(r02))))+(((IkReal(-1.96000000000000))*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.0505400000000000))*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.0491568000000000))*(cj0)*(sj0)*((r01)*(r01))))+(((IkReal(-1.96000000000000))*(cj0)*(px)*(py)*(r01)*(r02)))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.517440000000000))*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-2.80000000000000))*(pp)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.186200000000000))*(px)*(r01)*(r02)))+(((IkReal(-0.567980000000000))*(cj0)*(py)*(r00)*(r01)))+(((IkReal(5.60000000000000))*(cj0)*(px)*(py)*(pz)*(r01)*(r02)))+(((IkReal(0.0491568000000000))*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.532000000000000))*(cj0)*(sj0)*((px)*(px))*((r02)*(r02))))+(((IkReal(0.186200000000000))*(py)*(r00)*(r02)))+(((IkReal(-5.60000000000000))*(cj0)*(pp)*(pz)*(r00)*(r02)))+(((IkReal(-2.80000000000000))*(pp)*(px)*(r00)*(r01)*(sj0)))+(((IkReal(5.60000000000000))*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.266000000000000))*(cj0)*(pp)*(sj0)*((r01)*(r01))))+(((IkReal(-1.96000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-1.96000000000000))*(cj0)*(r00)*(r02)*((px)*(px))))+(((IkReal(-0.490560000000000))*(cj0)*(px)*((r02)*(r02))))+(((IkReal(-1.96000000000000))*(cj0)*(px)*(pz)*((r02)*(r02))))+(((IkReal(0.266000000000000))*(pp)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(5.60000000000000))*(cj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-0.186200000000000))*(cj0)*(px)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0268800000000000))*(cj0)*(pz)*(r00)*(r02)))+(((IkReal(1.96000000000000))*(pp)*(r01)*(r02)*(sj0)))+(((IkReal(0.532000000000000))*(r00)*(r01)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.532000000000000))*(px)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(5.60000000000000))*(px)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-2.80000000000000))*(cj0)*(pp)*(px)*((r00)*(r00))))+(((IkReal(0.532000000000000))*(px)*(py)*((r01)*(r01))))+(((IkReal(-0.0268800000000000))*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(11.2000000000000))*(cj0)*(py)*(r00)*(r01)*((px)*(px))))+(((IkReal(0.532000000000000))*(cj0)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(py)*(pz)*(r00)*(r02)))+(((IkReal(-0.532000000000000))*(r00)*(r01)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-0.517440000000000))*(cj0)*(px)*((r00)*(r00))))+(((IkReal(-0.567980000000000))*(px)*(r00)*(r01)*(sj0)))+(((IkReal(0.532000000000000))*(cj0)*(px)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(5.60000000000000))*(r01)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.0491568000000000))*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(0.00940800000000000))*(r01)*(r02)*(sj0)))+(((IkReal(-1.96000000000000))*(cj0)*(px)*(pz)*((r00)*(r00))))+(((IkReal(-0.532000000000000))*(px)*(py)*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(0.186200000000000))*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(0.532000000000000))*(r00)*(r01)*((px)*(px))))+(((IkReal(5.60000000000000))*(cj0)*(pz)*(r00)*(r02)*((px)*(px))))+(((IkReal(-1.96000000000000))*(py)*(pz)*(sj0)*((r02)*(r02)))));
op[4]=((((IkReal(0.163324000000000))*(px)*(sj0)*((r01)*(r01))))+(((IkReal(2.80000000000000))*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px)*(px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(pz)*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(-0.369600000000000))*(pp)*((r02)*(r02))))+(((IkReal(-2.80000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-2.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.240800000000000))*(px)*(pz)*(r00)*(r02)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(5.60000000000000))*(py)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(-0.532000000000000))*(cj0)*(px)*(py)*(r00)*(r02)))+(((IkReal(0.0252700000000000))*(pz)*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.760000000000000))*(cj0)*(py)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-2.80000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(0.380000000000000))*(cj0)*(pp)*(px)*(r00)*(r01)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-0.0341510400000000))*((r02)*(r02))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-0.258720000000000))*(px)*(r00)*(r02)))+(((IkReal(0.279200000000000))*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.00442225000000000))*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(1.71920000000000))*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.0361000000000000))*((px)*(px))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0252700000000000))*(px)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.0228760000000000))*(cj0)*(pz)*(r01)*(r02)))+(((IkReal(-0.380000000000000))*(pp)*(py)*(r00)*(r01)*(sj0)))+(((IkReal(-2.80000000000000))*(py)*(r01)*(r02)*((pz)*(pz))*((sj0)*(sj0))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r02)*(r02))))+(((IkReal(2.80000000000000))*(pz)*((py)*(py))*((r01)*(r01))))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(-8.00000000000000))*(py)*(r00)*(r01)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.760000000000000))*(cj0)*(py)*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(py)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.760000000000000))*(cj0)*(pz)*(r01)*(r02)*((py)*(py))))+(((IkReal(4.00000000000000))*(pp)*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(5.60000000000000))*(px)*(py)*(pz)*(r00)*(r01)))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(4.00000000000000))*(pp)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.0931000000000000))*(px)*(sj0)*((r00)*(r00))))+(((IkReal(0.270550000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(-4.00000000000000))*((py)*(py)*(py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.720000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(1.51220000000000))*(cj0)*(px)*(py)*(sj0)*((r02)*(r02))))+(((IkReal(0.532000000000000))*(px)*(py)*(r01)*(r02)*(sj0)))+(((IkReal(-0.760000000000000))*(cj0)*(py)*((pz)*(pz))*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(cj0)*(py)*((px)*(px))*((r01)*(r01))))+(((IkReal(-0.266000000000000))*(cj0)*(r01)*(r02)*((py)*(py))))+(((IkReal(-0.760000000000000))*(cj0)*(pz)*(r01)*(r02)*((px)*(px))))+(((IkReal(-0.258720000000000))*(pz)*((r02)*(r02))))+(((IkReal(-1.40000000000000))*(pp)*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(0.720000000000000))*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(4.00000000000000))*(pp)*(px)*(pz)*(r00)*(r02)))+(((IkReal(0.0722000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)))+(((IkReal(-2.80000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(-1.00000000000000))*((cj0)*(cj0))*((pp)*(pp))*((r00)*(r00))))+(((IkReal(-2.80000000000000))*(px)*(py)*(pz)*(r00)*(r01)*((cj0)*(cj0))))+(((IkReal(-0.760000000000000))*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(1.71920000000000))*(px)*(py)*(r00)*(r01)*((sj0)*(sj0))))+(((IkReal(-0.258720000000000))*(py)*(r01)*(r02)))+(((IkReal(-1.52000000000000))*(px)*(py)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(0.760000000000000))*(py)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(0.0702240000000000))*(px)*(sj0)*((r02)*(r02))))+(((IkReal(4.00000000000000))*(pp)*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((px)*(px))*((py)*(py))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*(cj0)*(px)*(py)*(sj0)*((r00)*(r00))))+(((IkReal(-0.266000000000000))*(cj0)*(py)*(pz)*((r02)*(r02))))+(((IkReal(1.71920000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-0.532000000000000))*(r00)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(1.71920000000000))*(cj0)*(r00)*(r01)*(sj0)*((py)*(py))))+(((IkReal(0.0702240000000000))*(cj0)*(px)*(r00)*(r01)))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((py)*(py))*((r01)*(r01))))+(((IkReal(0.0228760000000000))*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(0.380000000000000))*(cj0)*(pp)*(pz)*(r01)*(r02)))+(((IkReal(0.532000000000000))*(cj0)*(py)*(pz)*((r01)*(r01))))+(((IkReal(1.71920000000000))*((cj0)*(cj0))*((px)*(px))*((r00)*(r00))))+(((IkReal(-16.0000000000000))*(cj0)*(r00)*(r01)*(sj0)*((px)*(px))*((py)*(py))))+(((IkReal(0.760000000000000))*(r00)*(r02)*(sj0)*((pz)*(pz)*(pz))))+(((IkReal(-0.0641615800000000))*(cj0)*(r00)*(r01)*(sj0)))+(((IkReal(4.00000000000000))*(pp)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0361000000000000))*((px)*(px))*((r01)*(r01))))+(((IkReal(0.270550000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0365030400000000))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.40000000000000))*(pp)*(px)*(r00)*(r02)))+(((IkReal(-16.0000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-4.00000000000000))*((px)*(px))*((pz)*(pz))*((r00)*(r00))))+(((IkReal(0.0252700000000000))*(pz)*((cj0)*(cj0))*((r01)*(r01))))+(((IkReal(-8.00000000000000))*(py)*(pz)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(0.380000000000000))*(pp)*(px)*(sj0)*((r01)*(r01))))+(((IkReal(2.80000000000000))*(pz)*((py)*(py))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.760000000000000))*(cj0)*(r00)*(r01)*((px)*(px)*(px))))+(((IkReal(-0.240800000000000))*(py)*(pz)*(r01)*(r02)))+(((IkReal(-0.380000000000000))*(cj0)*(pp)*(py)*((r00)*(r00))))+(((IkReal(0.266000000000000))*(cj0)*(r01)*(r02)*((px)*(px))))+(((IkReal(4.00000000000000))*(pp)*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-8.00000000000000))*(pz)*(r01)*(r02)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(-0.266000000000000))*(px)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(0.532000000000000))*(cj0)*(r01)*(r02)*((pz)*(pz))))+(((IkReal(0.720000000000000))*((pz)*(pz))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-1.40000000000000))*(pp)*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.859600000000000))*(pp)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.245280000000000))*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-0.0361000000000000))*((cj0)*(cj0))*((py)*(py))*((r02)*(r02))))+(((IkReal(0.279200000000000))*(py)*(pz)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(1.71920000000000))*(cj0)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-0.760000000000000))*(py)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-0.541100000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(-8.00000000000000))*(px)*(r00)*(r01)*((py)*(py)*(py))*((sj0)*(sj0))))+(((IkReal(2.80000000000000))*(pz)*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-0.490000000000000))*((py)*(py))*((r01)*(r01))))+(((IkReal(-0.532000000000000))*(px)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(0.249200000000000))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(0.760000000000000))*(pz)*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.0361000000000000))*((cj0)*(cj0))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(1.40000000000000))*(pp)*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(2.80000000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))*((px)*(px))))+(((IkReal(-0.0252700000000000))*(py)*(r01)*(r02)*((cj0)*(cj0))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(px)*(py)*(sj0)*((r01)*(r01))))+(((IkReal(-8.00000000000000))*(cj0)*(py)*(sj0)*((px)*(px)*(px))*((r00)*(r00))))+(((IkReal(0.207000000000000))*(cj0)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.0361000000000000))*((py)*(py))*((r00)*(r00))))+(((IkReal(0.207000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)))+(((IkReal(-2.80000000000000))*(cj0)*(pz)*(r00)*(r01)*(sj0)*((px)*(px))))+(((IkReal(-2.80000000000000))*(pz)*((py)*(py))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.760000000000000))*(px)*(sj0)*((pz)*(pz))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r02)*(r02))*((sj0)*(sj0))))+(((IkReal(-0.266000000000000))*(r00)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-0.133000000000000))*(cj0)*(pp)*(r01)*(r02)))+(((IkReal(-2.80000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((pz)*(pz))))+(((IkReal(-0.907800000000000))*(px)*(py)*(r00)*(r01)))+(((IkReal(-1.40000000000000))*(cj0)*(pp)*(px)*(r01)*(r02)*(sj0)))+(((IkReal(2.80000000000000))*((pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(1.52000000000000))*(cj0)*(px)*(py)*(pz)*(r00)*(r02)))+(((IkReal(0.760000000000000))*(px)*(sj0)*((py)*(py))*((r00)*(r00))))+(((IkReal(-0.380000000000000))*(pp)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-0.760000000000000))*(cj0)*(r01)*(r02)*((pz)*(pz)*(pz))))+(((IkReal(0.245280000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))))+(((IkReal(-0.245280000000000))*(pz)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-0.0245784000000000))*(cj0)*(r01)*(r02)))+(((IkReal(-0.163324000000000))*(cj0)*(py)*((r00)*(r00))))+(((IkReal(0.720000000000000))*((cj0)*(cj0))*((px)*(px))*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(px)*(pz)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-4.00000000000000))*((py)*(py))*((pz)*(pz))*((r01)*(r01))))+(((IkReal(1.40000000000000))*(pp)*(pz)*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(-2.00000000000000))*(cj0)*(r00)*(r01)*(sj0)*((pp)*(pp))))+(((IkReal(-0.859600000000000))*(pp)*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(-4.00000000000000))*((pz)*(pz)*(pz)*(pz))*((r02)*(r02))))+(((IkReal(5.60000000000000))*(px)*(r00)*(r02)*((pz)*(pz))))+(((IkReal(-0.266000000000000))*(py)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.0245784000000000))*(r00)*(r02)*(sj0)))+(((IkReal(-1.71920000000000))*(cj0)*(pp)*(r00)*(r01)*(sj0)))+(((IkReal(0.760000000000000))*(r00)*(r01)*(sj0)*((py)*(py)*(py))))+(((IkReal(5.60000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(5.60000000000000))*(cj0)*(px)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(-1.40000000000000))*(pp)*(py)*(r01)*(r02)))+(((IkReal(-2.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r01)*(r01))))+(((IkReal(-0.490000000000000))*((px)*(px))*((r00)*(r00))))+(((IkReal(-8.00000000000000))*(px)*(py)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-1.40000000000000))*(cj0)*(pp)*(py)*(r00)*(r02)*(sj0)))+(((IkReal(2.80000000000000))*(px)*(r00)*(r02)*((py)*(py))*((sj0)*(sj0))))+(((IkReal(-4.00000000000000))*((cj0)*(cj0))*((px)*(px))*((pz)*(pz))*((r02)*(r02))))+(((IkReal(1.51220000000000))*(cj0)*(r00)*(r01)*(sj0)*((pz)*(pz))))+(((IkReal(-0.760000000000000))*(px)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-0.0931000000000000))*(cj0)*(py)*((r01)*(r01))))+(((IkReal(2.80000000000000))*(r00)*(r02)*((cj0)*(cj0))*((px)*(px)*(px))))+(((IkReal(0.266000000000000))*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(-0.760000000000000))*(px)*(sj0)*((py)*(py))*((r01)*(r01))))+(((IkReal(-2.80000000000000))*(cj0)*(px)*(py)*(pz)*(sj0)*((r00)*(r00))))+(((IkReal(2.80000000000000))*(cj0)*(pp)*(pz)*(r00)*(r01)*(sj0)))+(((IkReal(0.380000000000000))*(pp)*(px)*(sj0)*((r02)*(r02))))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(py)*(sj0)*((pz)*(pz))*((r02)*(r02))))+(((IkReal(-1.40000000000000))*(pp)*(pz)*((r02)*(r02))))+(((IkReal(2.80000000000000))*(pz)*((px)*(px))*((r00)*(r00))))+(((IkReal(-0.380000000000000))*(cj0)*(pp)*(py)*((r02)*(r02))))+(((IkReal(-0.760000000000000))*(cj0)*(px)*(r00)*(r01)*((pz)*(pz))))+(((IkReal(-1.00000000000000))*((pp)*(pp))*((r01)*(r01))*((sj0)*(sj0))))+(((IkReal(0.133000000000000))*(pp)*(r00)*(r02)*(sj0)))+(((IkReal(0.0722000000000000))*(px)*(pz)*(r00)*(r02)*((sj0)*(sj0))))+(((IkReal(0.266000000000000))*(cj0)*(py)*(pz)*((r00)*(r00))))+(((IkReal(5.60000000000000))*(cj0)*(py)*(r00)*(r02)*(sj0)*((px)*(px))))+(((IkReal(4.00000000000000))*(cj0)*(pp)*(py)*(pz)*(r00)*(r02)*(sj0)))+(((IkReal(-8.00000000000000))*(cj0)*(px)*(sj0)*((py)*(py)*(py))*((r01)*(r01))))+(((IkReal(-16.0000000000000))*(cj0)*(px)*(pz)*(r01)*(r02)*(sj0)*((py)*(py))))+(((IkReal(0.245280000000000))*(py)*(r01)*(r02)*((sj0)*(sj0))))+(((IkReal(-0.0365030400000000))*((cj0)*(cj0))*((r00)*(r00))))+(((IkReal(0.266000000000000))*(cj0)*(px)*(pz)*(r00)*(r01)))+(((IkReal(-0.0361000000000000))*((pz)*(pz))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(-0.0702240000000000))*(cj0)*(py)*((r02)*(r02))))+(((IkReal(-0.0702240000000000))*(py)*(r00)*(r01)*(sj0)))+(((IkReal(0.760000000000000))*(cj0)*(px)*(r00)*(r01)*((py)*(py))))+(((IkReal(-0.00442225000000000))*((r00)*(r00))*((sj0)*(sj0))))+(((IkReal(0.266000000000000))*(px)*(pz)*(sj0)*((r02)*(r02))))+(((IkReal(-2.80000000000000))*(px)*(r00)*(r02)*((cj0)*(cj0))*((pz)*(pz)))));
polyroots4(op,zeror,numroots);
IkReal j1array[4], cj1array[4], sj1array[4], tempj1array[1];
int numsolutions = 0;
for(int ij1 = 0; ij1 < numroots; ++ij1)
{
IkReal htj1 = zeror[ij1];
tempj1array[0]=((IkReal(2.00000000000000))*(atan(htj1)));
for(int kj1 = 0; kj1 < 1; ++kj1)
{
j1array[numsolutions] = tempj1array[kj1];
if( j1array[numsolutions] > IKPI )
{
    j1array[numsolutions]-=IK2PI;
}
else if( j1array[numsolutions] < -IKPI )
{
    j1array[numsolutions]+=IK2PI;
}
sj1array[numsolutions] = IKsin(j1array[numsolutions]);
cj1array[numsolutions] = IKcos(j1array[numsolutions]);
numsolutions++;
}
}
bool j1valid[4]={true,true,true,true};
_nj1 = 4;
for(int ij1 = 0; ij1 < numsolutions; ++ij1)
    {
if( !j1valid[ij1] )
{
    continue;
}
    j1 = j1array[ij1]; cj1 = cj1array[ij1]; sj1 = sj1array[ij1];
htj1 = IKtan(j1/2);

_ij1[0] = ij1; _ij1[1] = -1;
for(int iij1 = ij1+1; iij1 < numsolutions; ++iij1)
{
if( j1valid[iij1] && IKabs(cj1array[ij1]-cj1array[iij1]) < IKFAST_SOLUTION_THRESH && IKabs(sj1array[ij1]-sj1array[iij1]) < IKFAST_SOLUTION_THRESH )
{
    j1valid[iij1]=false; _ij1[1] = iij1; break; 
}
}
{
IkReal j4array[2], cj4array[2], sj4array[2];
bool j4valid[2]={false};
_nj4 = 2;
sj4array[0]=((((IkReal(-1.00000000000000))*(r00)*(sj0)))+(((cj0)*(r01))));
if( sj4array[0] >= -1-IKFAST_SINCOS_THRESH && sj4array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j4valid[0] = j4valid[1] = true;
    j4array[0] = IKasin(sj4array[0]);
    cj4array[0] = IKcos(j4array[0]);
    sj4array[1] = sj4array[0];
    j4array[1] = j4array[0] > 0 ? (IKPI-j4array[0]) : (-IKPI-j4array[0]);
    cj4array[1] = -cj4array[0];
}
else if( isnan(sj4array[0]) )
{
    // probably any value will work
    j4valid[0] = true;
    cj4array[0] = 1; sj4array[0] = 0; j4array[0] = 0;
}
for(int ij4 = 0; ij4 < 2; ++ij4)
{
if( !j4valid[ij4] )
{
    continue;
}
_ij4[0] = ij4; _ij4[1] = -1;
for(int iij4 = ij4+1; iij4 < 2; ++iij4)
{
if( j4valid[iij4] && IKabs(cj4array[ij4]-cj4array[iij4]) < IKFAST_SOLUTION_THRESH && IKabs(sj4array[ij4]-sj4array[iij4]) < IKFAST_SOLUTION_THRESH )
{
    j4valid[iij4]=false; _ij4[1] = iij4; break; 
}
}
j4 = j4array[ij4]; cj4 = cj4array[ij4]; sj4 = sj4array[ij4];

{
IkReal dummyeval[1];
IkReal gconst0;
gconst0=IKsign(cj4);
dummyeval[0]=cj4;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal dummyeval[1];
dummyeval[0]=cj4;
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal evalcond[9];
IkReal x27=(py)*(py);
IkReal x28=(px)*(px);
IkReal x29=(pz)*(pz);
IkReal x30=((cj1)*(r02));
IkReal x31=((IkReal(0.190000000000000))*(pz));
IkReal x32=((cj0)*(sj1));
IkReal x33=((cj1)*(r00));
IkReal x34=((IkReal(0.190000000000000))*(py));
IkReal x35=((px)*(sj0));
IkReal x36=((IkReal(2.00000000000000))*(r00));
IkReal x37=((r00)*(sj1));
IkReal x38=((px)*(r01));
IkReal x39=((IkReal(0.190000000000000))*(sj1));
IkReal x40=((IkReal(2.00000000000000))*(py));
IkReal x41=((py)*(r01));
IkReal x42=((IkReal(1.00000000000000))*(cj0));
IkReal x43=((r02)*(sj1));
IkReal x44=((r00)*(sj0));
IkReal x45=((IkReal(0.700000000000000))*(r00));
IkReal x46=((pz)*(r01));
IkReal x47=((IkReal(2.00000000000000))*(pz));
IkReal x48=((cj0)*(r01));
IkReal x49=((pz)*(r02));
IkReal x50=((cj0)*(pz));
IkReal x51=((r01)*(sj0)*(sj1));
IkReal x52=((cj0)*(px)*(r02));
IkReal x53=((sj1)*(x47));
IkReal x54=((py)*(r02)*(sj0));
IkReal x55=((cj1)*(r01)*(sj0));
IkReal x56=((IkReal(2.00000000000000))*(x27));
IkReal x57=((IkReal(2.00000000000000))*(x29));
IkReal x58=((sj0)*(x40)*(x49));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-0.0950000000000000))+(x35)+(((IkReal(-1.00000000000000))*(py)*(x42))));
evalcond[2]=((IkReal(1.00000000000000))+(x44)+(((IkReal(-1.00000000000000))*(r01)*(x42))));
evalcond[3]=((x51)+(x30)+(((r00)*(x32))));
evalcond[4]=((((IkReal(-1.00000000000000))*(x55)))+(x43)+(((IkReal(-1.00000000000000))*(x33)*(x42))));
evalcond[5]=((((IkReal(-0.0950000000000000))*(x48)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(0.350000000000000))*(x51)))+(((IkReal(-1.00000000000000))*(x41)))+(((IkReal(-1.00000000000000))*(x49)))+(((IkReal(0.350000000000000))*(r00)*(x32)))+(((IkReal(0.0950000000000000))*(x44)))+(((IkReal(0.350000000000000))*(x30))));
evalcond[6]=((((IkReal(0.350000000000000))*(x43)))+(((IkReal(-1.00000000000000))*(x54)))+(((sj0)*(x46)))+(((IkReal(-0.350000000000000))*(x55)))+(((r00)*(x50)))+(((IkReal(-0.350000000000000))*(cj0)*(x33)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x42))));
evalcond[7]=((((IkReal(-1.00000000000000))*(pz)*(sj0)*(x40)*(x43)))+(((pp)*(x51)))+(((IkReal(-1.00000000000000))*(x38)*(x39)))+(((sj0)*(x31)*(x33)))+(((IkReal(-0.113475000000000))*(x30)))+(((x34)*(x37)))+(((IkReal(0.700000000000000))*(x41)))+(((IkReal(-1.00000000000000))*(py)*(sj1)*(x35)*(x36)))+(((IkReal(0.0665000000000000))*(x48)))+(((IkReal(-1.00000000000000))*(cj1)*(x31)*(x48)))+(((IkReal(-1.00000000000000))*(x30)*(x57)))+(((IkReal(-0.190000000000000))*(x30)*(x35)))+(((IkReal(-0.0665000000000000))*(x44)))+(((IkReal(-0.113475000000000))*(r00)*(x32)))+(((IkReal(-1.00000000000000))*(x28)*(x32)*(x36)))+(((px)*(x45)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x32)*(x47)))+(((cj0)*(x30)*(x34)))+(((pp)*(x30)))+(((IkReal(-1.00000000000000))*(px)*(x33)*(x47)))+(((IkReal(0.700000000000000))*(x49)))+(((IkReal(-1.00000000000000))*(x32)*(x38)*(x40)))+(((IkReal(-1.00000000000000))*(x51)*(x56)))+(((IkReal(-0.113475000000000))*(x51)))+(((pp)*(r00)*(x32)))+(((IkReal(-1.00000000000000))*(cj1)*(x40)*(x46))));
evalcond[8]=((((IkReal(0.131525000000000))*(x43)))+(((IkReal(2.00000000000000))*(cj0)*(x28)*(x33)))+(((r02)*(x32)*(x34)))+(((cj0)*(px)*(x30)*(x47)))+(((IkReal(-0.131525000000000))*(cj0)*(x33)))+(((pp)*(x43)))+(((IkReal(0.700000000000000))*(sj0)*(x46)))+(((pz)*(sj0)*(x30)*(x40)))+(((cj0)*(cj1)*(x38)*(x40)))+(((IkReal(-1.00000000000000))*(r02)*(x35)*(x39)))+(((IkReal(0.190000000000000))*(cj1)*(x38)))+(((IkReal(-1.00000000000000))*(x33)*(x34)))+(((IkReal(-1.00000000000000))*(x43)*(x57)))+(((IkReal(-1.00000000000000))*(px)*(pz)*(sj1)*(x36)))+(((IkReal(-1.00000000000000))*(pp)*(x55)))+(((sj0)*(x31)*(x37)))+(((IkReal(-1.00000000000000))*(r01)*(x31)*(x32)))+(((IkReal(-1.00000000000000))*(sj1)*(x40)*(x46)))+(((IkReal(-0.131525000000000))*(x55)))+(((IkReal(-1.00000000000000))*(pp)*(x33)*(x42)))+(((IkReal(-0.700000000000000))*(x52)))+(((IkReal(-0.700000000000000))*(x54)))+(((x45)*(x50)))+(((x33)*(x35)*(x40)))+(((x55)*(x56))));
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  )
{
{
IkReal j3array[2], cj3array[2], sj3array[2];
bool j3valid[2]={false};
_nj3 = 2;
IkReal x59=((IkReal(9.79020979020979))*(sj1));
cj3array[0]=((IkReal(-2.45034965034965))+(((IkReal(2.65734265734266))*(cj0)*(py)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x59)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x59)))+(((IkReal(-2.65734265734266))*(px)*(sj0)))+(((IkReal(-9.79020979020979))*(cj1)*(pz)))+(((IkReal(13.9860139860140))*(pp))));
if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j3valid[0] = j3valid[1] = true;
    j3array[0] = IKacos(cj3array[0]);
    sj3array[0] = IKsin(j3array[0]);
    cj3array[1] = cj3array[0];
    j3array[1] = -j3array[0];
    sj3array[1] = -sj3array[0];
}
else if( isnan(cj3array[0]) )
{
    // probably any value will work
    j3valid[0] = true;
    cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
}
for(int ij3 = 0; ij3 < 2; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 2; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
{
IkReal evalcond[1];
IkReal x60=((IkReal(2.00000000000000))*(sj0));
IkReal x61=((px)*(r01));
IkReal x62=((r00)*(sj1));
IkReal x63=((IkReal(0.700000000000000))*(py));
IkReal x64=((r00)*(sj0));
IkReal x65=((IkReal(0.700000000000000))*(cj1));
IkReal x66=((px)*(r02));
IkReal x67=((IkReal(2.00000000000000))*(cj0));
IkReal x68=((px)*(r00));
IkReal x69=((cj0)*(r01));
IkReal x70=((pz)*(r02));
IkReal x71=((cj1)*(r02));
evalcond[0]=((IkReal(0.306725000000000))+(((IkReal(0.190000000000000))*(x70)))+(((IkReal(-1.00000000000000))*(pp)*(x69)))+(((IkReal(-1.00000000000000))*(pz)*(x60)*(x66)))+(((sj0)*(x65)*(x66)))+(((IkReal(-1.00000000000000))*(x62)*(x63)))+(((IkReal(0.0715000000000000))*(IKcos(j3))))+(((IkReal(0.190000000000000))*(py)*(r01)))+(((IkReal(-0.113475000000000))*(x69)))+(((IkReal(0.113475000000000))*(x64)))+(((r01)*(x67)*((py)*(py))))+(((IkReal(-0.0665000000000000))*(r01)*(sj0)*(sj1)))+(((pp)*(x64)))+(((IkReal(-1.00000000000000))*(py)*(x60)*(x61)))+(((IkReal(-1.00000000000000))*(pz)*(x64)*(x65)))+(((IkReal(-0.0665000000000000))*(cj0)*(x62)))+(((pz)*(x65)*(x69)))+(((IkReal(-1.00000000000000))*(px)*(x60)*(x68)))+(((py)*(x67)*(x68)))+(((IkReal(0.700000000000000))*(sj1)*(x61)))+(((py)*(x67)*(x70)))+(((IkReal(-0.0665000000000000))*(x71)))+(((IkReal(-1.00000000000000))*(cj0)*(x63)*(x71)))+(((IkReal(0.190000000000000))*(x68))));
if( IKabs(evalcond[0]) > 0.000001  )
{
continue;
}
}

{
IkReal dummyeval[1];
IkReal gconst9;
gconst9=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal dummyeval[1];
IkReal gconst10;
gconst10=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x72=((cj1)*(cj3));
IkReal x73=((sj0)*(sj1));
IkReal x74=((IkReal(22000.0000000000))*(pz));
IkReal x75=((IkReal(2600.00000000000))*(pz));
IkReal x76=((r02)*(sj1));
IkReal x77=((IkReal(2600.00000000000))*(sj3));
IkReal x78=((cj1)*(sj3));
IkReal x79=((r01)*(sj0));
IkReal x80=((cj0)*(sj1));
IkReal x81=((IkReal(2600.00000000000))*(cj3));
IkReal x82=((IkReal(2090.00000000000))*(cj1));
IkReal x83=((IkReal(22000.0000000000))*(px));
IkReal x84=((px)*(r02));
IkReal x85=((IkReal(22000.0000000000))*(py));
IkReal x86=((py)*(r00));
IkReal x87=((px)*(r01));
IkReal x88=((IkReal(247.000000000000))*(cj0)*(r00));
IkReal x89=((x77)*(x80));
if( IKabs(((gconst10)*(((((cj0)*(r00)*(x82)))+(((py)*(x73)*(x77)))+(((px)*(x89)))+(((cj1)*(r00)*(x85)))+(((IkReal(-1.00000000000000))*(cj0)*(x76)*(x85)))+(((x72)*(x88)))+(((IkReal(-247.000000000000))*(cj3)*(x76)))+(((r02)*(x73)*(x83)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x73)*(x75)))+(((IkReal(247.000000000000))*(x72)*(x79)))+(((IkReal(2600.00000000000))*(x72)*(x86)))+(((x79)*(x82)))+(((r01)*(x74)*(x80)))+(((IkReal(-910.000000000000))*(sj3)))+(((cj3)*(r01)*(x75)*(x80)))+(((IkReal(-2090.00000000000))*(x76)))+(((IkReal(-2600.00000000000))*(x72)*(x87)))+(((x75)*(x78)))+(((IkReal(-1.00000000000000))*(r00)*(x73)*(x74)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x83)))+(((x73)*(x81)*(x84)))+(((IkReal(-1.00000000000000))*(cj0)*(py)*(x76)*(x81))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst10)*(((IkReal(7700.00000000000))+(((x78)*(x88)))+(((IkReal(-1.00000000000000))*(cj0)*(py)*(x76)*(x77)))+(((IkReal(-1.00000000000000))*(x72)*(x75)))+(((r01)*(sj3)*(x75)*(x80)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x73)*(x75)))+(((cj1)*(x77)*(x86)))+(((IkReal(-1.00000000000000))*(px)*(x80)*(x81)))+(((IkReal(-1.00000000000000))*(x80)*(x83)))+(((IkReal(-1.00000000000000))*(py)*(x73)*(x81)))+(((x73)*(x77)*(x84)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x73)*(x85)))+(((IkReal(-247.000000000000))*(sj3)*(x76)))+(((IkReal(247.000000000000))*(x78)*(x79)))+(((IkReal(-1.00000000000000))*(cj1)*(x74)))+(((IkReal(-1.00000000000000))*(cj1)*(x77)*(x87))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst10)*(((((cj0)*(r00)*(x82)))+(((py)*(x73)*(x77)))+(((px)*(x89)))+(((cj1)*(r00)*(x85)))+(((IkReal(-1.00000000000000))*(cj0)*(x76)*(x85)))+(((x72)*(x88)))+(((IkReal(-247.000000000000))*(cj3)*(x76)))+(((r02)*(x73)*(x83)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x73)*(x75)))+(((IkReal(247.000000000000))*(x72)*(x79)))+(((IkReal(2600.00000000000))*(x72)*(x86)))+(((x79)*(x82)))+(((r01)*(x74)*(x80)))+(((IkReal(-910.000000000000))*(sj3)))+(((cj3)*(r01)*(x75)*(x80)))+(((IkReal(-2090.00000000000))*(x76)))+(((IkReal(-2600.00000000000))*(x72)*(x87)))+(((x75)*(x78)))+(((IkReal(-1.00000000000000))*(r00)*(x73)*(x74)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x83)))+(((x73)*(x81)*(x84)))+(((IkReal(-1.00000000000000))*(cj0)*(py)*(x76)*(x81)))))), ((gconst10)*(((IkReal(7700.00000000000))+(((x78)*(x88)))+(((IkReal(-1.00000000000000))*(cj0)*(py)*(x76)*(x77)))+(((IkReal(-1.00000000000000))*(x72)*(x75)))+(((r01)*(sj3)*(x75)*(x80)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x73)*(x75)))+(((cj1)*(x77)*(x86)))+(((IkReal(-1.00000000000000))*(px)*(x80)*(x81)))+(((IkReal(-1.00000000000000))*(x80)*(x83)))+(((IkReal(-1.00000000000000))*(py)*(x73)*(x81)))+(((x73)*(x77)*(x84)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(x73)*(x85)))+(((IkReal(-247.000000000000))*(sj3)*(x76)))+(((IkReal(247.000000000000))*(x78)*(x79)))+(((IkReal(-1.00000000000000))*(cj1)*(x74)))+(((IkReal(-1.00000000000000))*(cj1)*(x77)*(x87)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x90=IKcos(j2);
IkReal x91=IKsin(j2);
IkReal x92=((px)*(sj1));
IkReal x93=((py)*(sj1));
IkReal x94=((cj1)*(sj0));
IkReal x95=((IkReal(1.00000000000000))*(px));
IkReal x96=((r00)*(sj0));
IkReal x97=((IkReal(0.0950000000000000))*(r01));
IkReal x98=((IkReal(0.0650000000000000))*(cj3));
IkReal x99=((cj0)*(r01));
IkReal x100=((IkReal(0.0950000000000000))*(cj1));
IkReal x101=((pz)*(sj1));
IkReal x102=((cj0)*(r00));
IkReal x103=((IkReal(0.0950000000000000))*(sj1));
IkReal x104=((cj0)*(cj1));
IkReal x105=((cj1)*(pz));
IkReal x106=((IkReal(0.0650000000000000))*(sj3));
IkReal x107=((IkReal(0.550000000000000))*(x91));
IkReal x108=((IkReal(0.550000000000000))*(x90));
IkReal x109=((x91)*(x98));
IkReal x110=((x106)*(x90));
IkReal x111=((x90)*(x98));
IkReal x112=((x106)*(x91));
IkReal x113=((x108)+(x111));
IkReal x114=((x109)+(x107)+(x110));
evalcond[0]=((IkReal(-0.350000000000000))+(((sj0)*(x93)))+(((cj0)*(x92)))+(x105)+(x112)+(((IkReal(-1.00000000000000))*(x113))));
evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(x94)))+(x101)+(x114)+(((IkReal(-1.00000000000000))*(x104)*(x95))));
evalcond[2]=((((r02)*(sj0)*(x92)))+(((IkReal(-1.00000000000000))*(r02)*(x103)))+(((cj1)*(py)*(r00)))+(((x100)*(x102)))+(((IkReal(-1.00000000000000))*(x101)*(x96)))+(x114)+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x95)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x93)))+(((x101)*(x99)))+(((x94)*(x97))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r02)*(x94)*(x95)))+(((IkReal(-0.350000000000000))*(x96)))+(((sj0)*(sj1)*(x97)))+(((IkReal(-1.00000000000000))*(r01)*(x92)))+(((IkReal(-1.00000000000000))*(x105)*(x99)))+(((r00)*(x93)))+(((x102)*(x103)))+(((py)*(r02)*(x104)))+(((IkReal(0.350000000000000))*(x99)))+(((IkReal(-1.00000000000000))*(x112)))+(x113)+(((r02)*(x100)))+(((pz)*(r00)*(x94))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x115=((IkReal(2600.00000000000))*(sj3));
IkReal x116=((py)*(sj0));
IkReal x117=((pz)*(sj1));
IkReal x118=((IkReal(2600.00000000000))*(cj3));
IkReal x119=((cj1)*(pz));
IkReal x120=((IkReal(22000.0000000000))*(cj1));
IkReal x121=((cj0)*(px));
IkReal x122=((sj1)*(x121));
if( IKabs(((gconst9)*(((((IkReal(-1.00000000000000))*(x116)*(x120)))+(((x117)*(x118)))+(((IkReal(-1.00000000000000))*(cj1)*(x118)*(x121)))+(((x115)*(x122)))+(((IkReal(22000.0000000000))*(x117)))+(((IkReal(-1.00000000000000))*(cj1)*(x116)*(x118)))+(((IkReal(-910.000000000000))*(sj3)))+(((sj1)*(x115)*(x116)))+(((IkReal(-1.00000000000000))*(x120)*(x121)))+(((x115)*(x119))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst9)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x122)))+(((x115)*(x117)))+(((IkReal(-1.00000000000000))*(cj1)*(x115)*(x116)))+(((IkReal(-22000.0000000000))*(sj1)*(x116)))+(((IkReal(-1.00000000000000))*(x118)*(x119)))+(((IkReal(-1.00000000000000))*(cj1)*(x115)*(x121)))+(((IkReal(-1.00000000000000))*(x118)*(x122)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(x119)))+(((IkReal(-1.00000000000000))*(sj1)*(x116)*(x118))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst9)*(((((IkReal(-1.00000000000000))*(x116)*(x120)))+(((x117)*(x118)))+(((IkReal(-1.00000000000000))*(cj1)*(x118)*(x121)))+(((x115)*(x122)))+(((IkReal(22000.0000000000))*(x117)))+(((IkReal(-1.00000000000000))*(cj1)*(x116)*(x118)))+(((IkReal(-910.000000000000))*(sj3)))+(((sj1)*(x115)*(x116)))+(((IkReal(-1.00000000000000))*(x120)*(x121)))+(((x115)*(x119)))))), ((gconst9)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x122)))+(((x115)*(x117)))+(((IkReal(-1.00000000000000))*(cj1)*(x115)*(x116)))+(((IkReal(-22000.0000000000))*(sj1)*(x116)))+(((IkReal(-1.00000000000000))*(x118)*(x119)))+(((IkReal(-1.00000000000000))*(cj1)*(x115)*(x121)))+(((IkReal(-1.00000000000000))*(x118)*(x122)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(x119)))+(((IkReal(-1.00000000000000))*(sj1)*(x116)*(x118)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x123=IKcos(j2);
IkReal x124=IKsin(j2);
IkReal x125=((px)*(sj1));
IkReal x126=((py)*(sj1));
IkReal x127=((cj1)*(sj0));
IkReal x128=((IkReal(1.00000000000000))*(px));
IkReal x129=((r00)*(sj0));
IkReal x130=((IkReal(0.0950000000000000))*(r01));
IkReal x131=((IkReal(0.0650000000000000))*(cj3));
IkReal x132=((cj0)*(r01));
IkReal x133=((IkReal(0.0950000000000000))*(cj1));
IkReal x134=((pz)*(sj1));
IkReal x135=((cj0)*(r00));
IkReal x136=((IkReal(0.0950000000000000))*(sj1));
IkReal x137=((cj0)*(cj1));
IkReal x138=((cj1)*(pz));
IkReal x139=((IkReal(0.0650000000000000))*(sj3));
IkReal x140=((IkReal(0.550000000000000))*(x124));
IkReal x141=((IkReal(0.550000000000000))*(x123));
IkReal x142=((x124)*(x131));
IkReal x143=((x123)*(x139));
IkReal x144=((x123)*(x131));
IkReal x145=((x124)*(x139));
IkReal x146=((x141)+(x144));
IkReal x147=((x140)+(x142)+(x143));
evalcond[0]=((IkReal(-0.350000000000000))+(((sj0)*(x126)))+(((IkReal(-1.00000000000000))*(x146)))+(((cj0)*(x125)))+(x138)+(x145));
evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(x127)))+(x134)+(x147)+(((IkReal(-1.00000000000000))*(x128)*(x137))));
evalcond[2]=((((IkReal(-1.00000000000000))*(r02)*(x136)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x128)))+(((r02)*(sj0)*(x125)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x126)))+(((IkReal(-1.00000000000000))*(x129)*(x134)))+(((x133)*(x135)))+(((cj1)*(py)*(r00)))+(((x127)*(x130)))+(((x132)*(x134)))+(x147));
evalcond[3]=((((r00)*(x126)))+(((IkReal(-1.00000000000000))*(x132)*(x138)))+(((IkReal(-0.350000000000000))*(x129)))+(((IkReal(0.350000000000000))*(x132)))+(((IkReal(-1.00000000000000))*(x145)))+(((x135)*(x136)))+(((r02)*(x133)))+(((IkReal(-1.00000000000000))*(r01)*(x125)))+(((IkReal(-1.00000000000000))*(r02)*(x127)*(x128)))+(((py)*(r02)*(x137)))+(x146)+(((pz)*(r00)*(x127)))+(((sj0)*(sj1)*(x130))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}
}
}

} else
{
IkReal x148=(py)*(py);
IkReal x149=(px)*(px);
IkReal x150=(pz)*(pz);
IkReal x151=((cj1)*(r02));
IkReal x152=((IkReal(0.190000000000000))*(pz));
IkReal x153=((cj0)*(sj1));
IkReal x154=((cj1)*(r00));
IkReal x155=((IkReal(0.190000000000000))*(py));
IkReal x156=((px)*(sj0));
IkReal x157=((IkReal(2.00000000000000))*(r00));
IkReal x158=((r00)*(sj1));
IkReal x159=((px)*(r01));
IkReal x160=((IkReal(0.190000000000000))*(sj1));
IkReal x161=((IkReal(2.00000000000000))*(py));
IkReal x162=((py)*(r01));
IkReal x163=((IkReal(1.00000000000000))*(cj0));
IkReal x164=((r02)*(sj1));
IkReal x165=((r00)*(sj0));
IkReal x166=((IkReal(0.700000000000000))*(r00));
IkReal x167=((pz)*(r01));
IkReal x168=((IkReal(2.00000000000000))*(pz));
IkReal x169=((cj0)*(r01));
IkReal x170=((pz)*(r02));
IkReal x171=((cj0)*(pz));
IkReal x172=((r01)*(sj0)*(sj1));
IkReal x173=((cj0)*(px)*(r02));
IkReal x174=((sj1)*(x168));
IkReal x175=((py)*(r02)*(sj0));
IkReal x176=((cj1)*(r01)*(sj0));
IkReal x177=((IkReal(2.00000000000000))*(x148));
IkReal x178=((IkReal(2.00000000000000))*(x150));
IkReal x179=((sj0)*(x161)*(x170));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-0.0950000000000000))+(x156)+(((IkReal(-1.00000000000000))*(py)*(x163))));
evalcond[2]=((IkReal(-1.00000000000000))+(x165)+(((IkReal(-1.00000000000000))*(r01)*(x163))));
evalcond[3]=((x172)+(x151)+(((r00)*(x153))));
evalcond[4]=((((IkReal(-1.00000000000000))*(x154)*(x163)))+(x164)+(((IkReal(-1.00000000000000))*(x176))));
evalcond[5]=((((IkReal(-1.00000000000000))*(x170)))+(((IkReal(0.0950000000000000))*(x165)))+(((IkReal(0.350000000000000))*(x151)))+(((IkReal(-1.00000000000000))*(x162)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(0.350000000000000))*(r00)*(x153)))+(((IkReal(-0.0950000000000000))*(x169)))+(((IkReal(0.350000000000000))*(x172))));
evalcond[6]=((((sj0)*(x167)))+(((IkReal(-1.00000000000000))*(x175)))+(((IkReal(-0.350000000000000))*(x176)))+(((r00)*(x171)))+(((IkReal(-0.350000000000000))*(cj0)*(x154)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x163)))+(((IkReal(0.350000000000000))*(x164))));
evalcond[7]=((((IkReal(-1.00000000000000))*(x172)*(x177)))+(((sj0)*(x152)*(x154)))+(((IkReal(-0.113475000000000))*(x151)))+(((IkReal(-1.00000000000000))*(pz)*(sj0)*(x161)*(x164)))+(((pp)*(x151)))+(((IkReal(-1.00000000000000))*(x149)*(x153)*(x157)))+(((IkReal(0.700000000000000))*(x170)))+(((IkReal(-1.00000000000000))*(x159)*(x160)))+(((pp)*(x172)))+(((px)*(x166)))+(((x155)*(x158)))+(((IkReal(-1.00000000000000))*(cj1)*(x161)*(x167)))+(((IkReal(0.0665000000000000))*(x169)))+(((IkReal(-1.00000000000000))*(cj1)*(x152)*(x169)))+(((cj0)*(x151)*(x155)))+(((pp)*(r00)*(x153)))+(((IkReal(-1.00000000000000))*(x153)*(x159)*(x161)))+(((IkReal(-1.00000000000000))*(x151)*(x178)))+(((IkReal(0.700000000000000))*(x162)))+(((IkReal(-1.00000000000000))*(px)*(x154)*(x168)))+(((IkReal(-0.113475000000000))*(r00)*(x153)))+(((IkReal(-1.00000000000000))*(py)*(sj1)*(x156)*(x157)))+(((IkReal(-0.113475000000000))*(x172)))+(((IkReal(-1.00000000000000))*(px)*(r02)*(x153)*(x168)))+(((IkReal(-0.190000000000000))*(x151)*(x156)))+(((IkReal(-0.0665000000000000))*(x165))));
evalcond[8]=((((IkReal(0.700000000000000))*(sj0)*(x167)))+(((IkReal(2.00000000000000))*(cj0)*(x149)*(x154)))+(((IkReal(-1.00000000000000))*(px)*(pz)*(sj1)*(x157)))+(((IkReal(-1.00000000000000))*(x154)*(x155)))+(((cj0)*(cj1)*(x159)*(x161)))+(((x176)*(x177)))+(((IkReal(-1.00000000000000))*(r01)*(x152)*(x153)))+(((IkReal(-1.00000000000000))*(pp)*(x154)*(x163)))+(((IkReal(-1.00000000000000))*(r02)*(x156)*(x160)))+(((pz)*(sj0)*(x151)*(x161)))+(((IkReal(-0.700000000000000))*(x175)))+(((IkReal(0.190000000000000))*(cj1)*(x159)))+(((IkReal(-1.00000000000000))*(pp)*(x176)))+(((IkReal(0.131525000000000))*(x164)))+(((r02)*(x153)*(x155)))+(((IkReal(-1.00000000000000))*(sj1)*(x161)*(x167)))+(((IkReal(-0.131525000000000))*(x176)))+(((IkReal(-0.700000000000000))*(x173)))+(((sj0)*(x152)*(x158)))+(((IkReal(-0.131525000000000))*(cj0)*(x154)))+(((x166)*(x171)))+(((pp)*(x164)))+(((IkReal(-1.00000000000000))*(x164)*(x178)))+(((cj0)*(px)*(x151)*(x168)))+(((x154)*(x156)*(x161))));
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  )
{
{
IkReal j3array[2], cj3array[2], sj3array[2];
bool j3valid[2]={false};
_nj3 = 2;
IkReal x180=((IkReal(9.79020979020979))*(sj1));
cj3array[0]=((IkReal(-2.45034965034965))+(((IkReal(2.65734265734266))*(cj0)*(py)))+(((IkReal(-2.65734265734266))*(px)*(sj0)))+(((IkReal(-9.79020979020979))*(cj1)*(pz)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x180)))+(((IkReal(13.9860139860140))*(pp)))+(((IkReal(-1.00000000000000))*(py)*(sj0)*(x180))));
if( cj3array[0] >= -1-IKFAST_SINCOS_THRESH && cj3array[0] <= 1+IKFAST_SINCOS_THRESH )
{
    j3valid[0] = j3valid[1] = true;
    j3array[0] = IKacos(cj3array[0]);
    sj3array[0] = IKsin(j3array[0]);
    cj3array[1] = cj3array[0];
    j3array[1] = -j3array[0];
    sj3array[1] = -sj3array[0];
}
else if( isnan(cj3array[0]) )
{
    // probably any value will work
    j3valid[0] = true;
    cj3array[0] = 1; sj3array[0] = 0; j3array[0] = 0;
}
for(int ij3 = 0; ij3 < 2; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 2; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
{
IkReal evalcond[1];
IkReal x181=((IkReal(2.00000000000000))*(sj0));
IkReal x182=((px)*(r01));
IkReal x183=((r00)*(sj1));
IkReal x184=((IkReal(0.700000000000000))*(py));
IkReal x185=((r00)*(sj0));
IkReal x186=((IkReal(0.700000000000000))*(cj1));
IkReal x187=((px)*(r02));
IkReal x188=((IkReal(2.00000000000000))*(cj0));
IkReal x189=((px)*(r00));
IkReal x190=((cj0)*(r01));
IkReal x191=((pz)*(r02));
IkReal x192=((cj1)*(r02));
evalcond[0]=((IkReal(-0.306725000000000))+(((py)*(x188)*(x191)))+(((IkReal(-1.00000000000000))*(pz)*(x181)*(x187)))+(((IkReal(-0.0665000000000000))*(cj0)*(x183)))+(((IkReal(-1.00000000000000))*(cj0)*(x184)*(x192)))+(((IkReal(-0.113475000000000))*(x190)))+(((IkReal(-1.00000000000000))*(py)*(x181)*(x182)))+(((IkReal(0.190000000000000))*(py)*(r01)))+(((IkReal(0.700000000000000))*(sj1)*(x182)))+(((IkReal(0.190000000000000))*(x189)))+(((IkReal(-1.00000000000000))*(pp)*(x190)))+(((IkReal(0.190000000000000))*(x191)))+(((IkReal(-1.00000000000000))*(x183)*(x184)))+(((r01)*(x188)*((py)*(py))))+(((IkReal(-0.0665000000000000))*(r01)*(sj0)*(sj1)))+(((pz)*(x186)*(x190)))+(((IkReal(0.113475000000000))*(x185)))+(((IkReal(-1.00000000000000))*(pz)*(x185)*(x186)))+(((py)*(x188)*(x189)))+(((IkReal(-0.0665000000000000))*(x192)))+(((pp)*(x185)))+(((sj0)*(x186)*(x187)))+(((IkReal(-1.00000000000000))*(px)*(x181)*(x189)))+(((IkReal(-0.0715000000000000))*(IKcos(j3)))));
if( IKabs(evalcond[0]) > 0.000001  )
{
continue;
}
}

{
IkReal dummyeval[1];
IkReal gconst11;
gconst11=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal dummyeval[1];
IkReal gconst12;
gconst12=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x193=((cj1)*(cj3));
IkReal x194=((sj0)*(sj1));
IkReal x195=((IkReal(22000.0000000000))*(pz));
IkReal x196=((IkReal(2600.00000000000))*(pz));
IkReal x197=((r02)*(sj1));
IkReal x198=((IkReal(2600.00000000000))*(sj3));
IkReal x199=((cj1)*(sj3));
IkReal x200=((r01)*(sj0));
IkReal x201=((cj0)*(sj1));
IkReal x202=((IkReal(2600.00000000000))*(cj3));
IkReal x203=((IkReal(2090.00000000000))*(cj1));
IkReal x204=((IkReal(22000.0000000000))*(px));
IkReal x205=((px)*(r02));
IkReal x206=((IkReal(22000.0000000000))*(py));
IkReal x207=((py)*(r00));
IkReal x208=((px)*(r01));
IkReal x209=((IkReal(247.000000000000))*(cj0)*(r00));
IkReal x210=((x198)*(x201));
if( IKabs(((gconst12)*(((((px)*(x210)))+(((IkReal(-1.00000000000000))*(x193)*(x209)))+(((py)*(x194)*(x198)))+(((cj3)*(r00)*(x194)*(x196)))+(((x196)*(x199)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x206)))+(((IkReal(2600.00000000000))*(x193)*(x208)))+(((cj0)*(py)*(x197)*(x202)))+(((cj1)*(r01)*(x204)))+(((IkReal(-1.00000000000000))*(x194)*(x202)*(x205)))+(((IkReal(247.000000000000))*(cj3)*(x197)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-247.000000000000))*(x193)*(x200)))+(((IkReal(2090.00000000000))*(x197)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x203)))+(((IkReal(-1.00000000000000))*(x200)*(x203)))+(((IkReal(-1.00000000000000))*(r02)*(x194)*(x204)))+(((IkReal(-2600.00000000000))*(x193)*(x207)))+(((cj0)*(x197)*(x206)))+(((r00)*(x194)*(x195)))+(((IkReal(-1.00000000000000))*(r01)*(x195)*(x201)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x196)*(x201))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst12)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(x194)*(x198)*(x205)))+(((IkReal(-1.00000000000000))*(x193)*(x196)))+(((IkReal(-1.00000000000000))*(x199)*(x209)))+(((IkReal(-1.00000000000000))*(cj1)*(x195)))+(((cj0)*(py)*(x197)*(x198)))+(((IkReal(-1.00000000000000))*(px)*(x201)*(x202)))+(((r00)*(sj3)*(x194)*(x196)))+(((IkReal(-247.000000000000))*(x199)*(x200)))+(((cj1)*(x198)*(x208)))+(((IkReal(-1.00000000000000))*(cj1)*(x198)*(x207)))+(((IkReal(247.000000000000))*(sj3)*(x197)))+(((IkReal(-1.00000000000000))*(x194)*(x206)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(py)*(x194)*(x202)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x196)*(x201)))+(((IkReal(-1.00000000000000))*(x201)*(x204))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst12)*(((((px)*(x210)))+(((IkReal(-1.00000000000000))*(x193)*(x209)))+(((py)*(x194)*(x198)))+(((cj3)*(r00)*(x194)*(x196)))+(((x196)*(x199)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x206)))+(((IkReal(2600.00000000000))*(x193)*(x208)))+(((cj0)*(py)*(x197)*(x202)))+(((cj1)*(r01)*(x204)))+(((IkReal(-1.00000000000000))*(x194)*(x202)*(x205)))+(((IkReal(247.000000000000))*(cj3)*(x197)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-247.000000000000))*(x193)*(x200)))+(((IkReal(2090.00000000000))*(x197)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x203)))+(((IkReal(-1.00000000000000))*(x200)*(x203)))+(((IkReal(-1.00000000000000))*(r02)*(x194)*(x204)))+(((IkReal(-2600.00000000000))*(x193)*(x207)))+(((cj0)*(x197)*(x206)))+(((r00)*(x194)*(x195)))+(((IkReal(-1.00000000000000))*(r01)*(x195)*(x201)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x196)*(x201)))))), ((gconst12)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(x194)*(x198)*(x205)))+(((IkReal(-1.00000000000000))*(x193)*(x196)))+(((IkReal(-1.00000000000000))*(x199)*(x209)))+(((IkReal(-1.00000000000000))*(cj1)*(x195)))+(((cj0)*(py)*(x197)*(x198)))+(((IkReal(-1.00000000000000))*(px)*(x201)*(x202)))+(((r00)*(sj3)*(x194)*(x196)))+(((IkReal(-247.000000000000))*(x199)*(x200)))+(((cj1)*(x198)*(x208)))+(((IkReal(-1.00000000000000))*(cj1)*(x198)*(x207)))+(((IkReal(247.000000000000))*(sj3)*(x197)))+(((IkReal(-1.00000000000000))*(x194)*(x206)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(py)*(x194)*(x202)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x196)*(x201)))+(((IkReal(-1.00000000000000))*(x201)*(x204)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x211=IKcos(j2);
IkReal x212=IKsin(j2);
IkReal x213=((px)*(sj1));
IkReal x214=((py)*(sj1));
IkReal x215=((cj1)*(sj0));
IkReal x216=((IkReal(1.00000000000000))*(px));
IkReal x217=((r00)*(sj0));
IkReal x218=((IkReal(0.0950000000000000))*(r01));
IkReal x219=((IkReal(0.0650000000000000))*(cj3));
IkReal x220=((cj0)*(r01));
IkReal x221=((IkReal(0.0950000000000000))*(cj1));
IkReal x222=((pz)*(sj1));
IkReal x223=((cj0)*(r00));
IkReal x224=((IkReal(0.0950000000000000))*(sj1));
IkReal x225=((cj0)*(cj1));
IkReal x226=((cj1)*(pz));
IkReal x227=((IkReal(0.0650000000000000))*(sj3));
IkReal x228=((IkReal(0.550000000000000))*(x212));
IkReal x229=((IkReal(0.550000000000000))*(x211));
IkReal x230=((x212)*(x219));
IkReal x231=((x211)*(x227));
IkReal x232=((x212)*(x227));
IkReal x233=((x211)*(x219));
IkReal x234=((x233)+(x229));
IkReal x235=((x230)+(x231)+(x228));
evalcond[0]=((IkReal(-0.350000000000000))+(x232)+(((cj0)*(x213)))+(x226)+(((IkReal(-1.00000000000000))*(x234)))+(((sj0)*(x214))));
evalcond[1]=((x235)+(((IkReal(-1.00000000000000))*(py)*(x215)))+(((IkReal(-1.00000000000000))*(x216)*(x225)))+(x222));
evalcond[2]=((((IkReal(-1.00000000000000))*(x235)))+(((cj1)*(py)*(r00)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x216)))+(((r02)*(sj0)*(x213)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x214)))+(((IkReal(-1.00000000000000))*(r02)*(x224)))+(((x215)*(x218)))+(((x220)*(x222)))+(((x221)*(x223)))+(((IkReal(-1.00000000000000))*(x217)*(x222))));
evalcond[3]=((((IkReal(0.350000000000000))*(x220)))+(x232)+(((py)*(r02)*(x225)))+(((r02)*(x221)))+(((r00)*(x214)))+(((IkReal(-0.350000000000000))*(x217)))+(((x223)*(x224)))+(((IkReal(-1.00000000000000))*(x234)))+(((IkReal(-1.00000000000000))*(x220)*(x226)))+(((pz)*(r00)*(x215)))+(((IkReal(-1.00000000000000))*(r01)*(x213)))+(((IkReal(-1.00000000000000))*(r02)*(x215)*(x216)))+(((sj0)*(sj1)*(x218))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x236=((IkReal(2600.00000000000))*(sj3));
IkReal x237=((py)*(sj0));
IkReal x238=((pz)*(sj1));
IkReal x239=((IkReal(2600.00000000000))*(cj3));
IkReal x240=((cj1)*(pz));
IkReal x241=((IkReal(22000.0000000000))*(cj1));
IkReal x242=((cj0)*(px));
IkReal x243=((sj1)*(x242));
if( IKabs(((gconst11)*(((((IkReal(22000.0000000000))*(x238)))+(((x236)*(x243)))+(((IkReal(-1.00000000000000))*(cj1)*(x237)*(x239)))+(((IkReal(-1.00000000000000))*(x237)*(x241)))+(((IkReal(-1.00000000000000))*(x241)*(x242)))+(((x236)*(x240)))+(((IkReal(-1.00000000000000))*(cj1)*(x239)*(x242)))+(((IkReal(-910.000000000000))*(sj3)))+(((x238)*(x239)))+(((sj1)*(x236)*(x237))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst11)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(x239)*(x243)))+(((IkReal(-1.00000000000000))*(cj1)*(x236)*(x242)))+(((IkReal(-1.00000000000000))*(x239)*(x240)))+(((IkReal(-1.00000000000000))*(sj1)*(x237)*(x239)))+(((x236)*(x238)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x236)*(x237)))+(((IkReal(-22000.0000000000))*(x243)))+(((IkReal(-22000.0000000000))*(sj1)*(x237)))+(((IkReal(-22000.0000000000))*(x240))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst11)*(((((IkReal(22000.0000000000))*(x238)))+(((x236)*(x243)))+(((IkReal(-1.00000000000000))*(cj1)*(x237)*(x239)))+(((IkReal(-1.00000000000000))*(x237)*(x241)))+(((IkReal(-1.00000000000000))*(x241)*(x242)))+(((x236)*(x240)))+(((IkReal(-1.00000000000000))*(cj1)*(x239)*(x242)))+(((IkReal(-910.000000000000))*(sj3)))+(((x238)*(x239)))+(((sj1)*(x236)*(x237)))))), ((gconst11)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(x239)*(x243)))+(((IkReal(-1.00000000000000))*(cj1)*(x236)*(x242)))+(((IkReal(-1.00000000000000))*(x239)*(x240)))+(((IkReal(-1.00000000000000))*(sj1)*(x237)*(x239)))+(((x236)*(x238)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x236)*(x237)))+(((IkReal(-22000.0000000000))*(x243)))+(((IkReal(-22000.0000000000))*(sj1)*(x237)))+(((IkReal(-22000.0000000000))*(x240)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x244=IKcos(j2);
IkReal x245=IKsin(j2);
IkReal x246=((px)*(sj1));
IkReal x247=((py)*(sj1));
IkReal x248=((cj1)*(sj0));
IkReal x249=((IkReal(1.00000000000000))*(px));
IkReal x250=((r00)*(sj0));
IkReal x251=((IkReal(0.0950000000000000))*(r01));
IkReal x252=((IkReal(0.0650000000000000))*(cj3));
IkReal x253=((cj0)*(r01));
IkReal x254=((IkReal(0.0950000000000000))*(cj1));
IkReal x255=((pz)*(sj1));
IkReal x256=((cj0)*(r00));
IkReal x257=((IkReal(0.0950000000000000))*(sj1));
IkReal x258=((cj0)*(cj1));
IkReal x259=((cj1)*(pz));
IkReal x260=((IkReal(0.0650000000000000))*(sj3));
IkReal x261=((IkReal(0.550000000000000))*(x245));
IkReal x262=((IkReal(0.550000000000000))*(x244));
IkReal x263=((x245)*(x252));
IkReal x264=((x244)*(x260));
IkReal x265=((x245)*(x260));
IkReal x266=((x244)*(x252));
IkReal x267=((x266)+(x262));
IkReal x268=((x264)+(x261)+(x263));
evalcond[0]=((IkReal(-0.350000000000000))+(((IkReal(-1.00000000000000))*(x267)))+(x259)+(((cj0)*(x246)))+(x265)+(((sj0)*(x247))));
evalcond[1]=((x255)+(x268)+(((IkReal(-1.00000000000000))*(x249)*(x258)))+(((IkReal(-1.00000000000000))*(py)*(x248))));
evalcond[2]=((((IkReal(-1.00000000000000))*(cj0)*(r02)*(x247)))+(((x253)*(x255)))+(((r02)*(sj0)*(x246)))+(((IkReal(-1.00000000000000))*(x250)*(x255)))+(((cj1)*(py)*(r00)))+(((IkReal(-1.00000000000000))*(x268)))+(((x254)*(x256)))+(((x248)*(x251)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x249)))+(((IkReal(-1.00000000000000))*(r02)*(x257))));
evalcond[3]=((((r02)*(x254)))+(((py)*(r02)*(x258)))+(((IkReal(-1.00000000000000))*(x267)))+(((IkReal(-1.00000000000000))*(x253)*(x259)))+(((IkReal(-0.350000000000000))*(x250)))+(((IkReal(0.350000000000000))*(x253)))+(((x256)*(x257)))+(((pz)*(r00)*(x248)))+(((r00)*(x247)))+(((IkReal(-1.00000000000000))*(r01)*(x246)))+(x265)+(((IkReal(-1.00000000000000))*(r02)*(x248)*(x249)))+(((sj0)*(sj1)*(x251))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}

} else
{
{
IkReal j3array[1], cj3array[1], sj3array[1];
bool j3valid[1]={false};
_nj3 = 1;
IkReal x269=((sj0)*(sj1));
IkReal x270=((cj0)*(sj1));
if( IKabs(((IkReal(0.0909090909090909))*(((IKabs(cj4) != 0)?((IkReal)1/(cj4)):(IkReal)1.0e30))*(((((IkReal(-20.0000000000000))*(py)*(r01)))+(((IkReal(7.00000000000000))*(r00)*(x270)))+(((IkReal(-1.90000000000000))*(cj0)*(r01)))+(((IkReal(7.00000000000000))*(r01)*(x269)))+(((IkReal(-20.0000000000000))*(pz)*(r02)))+(((IkReal(-20.0000000000000))*(px)*(r00)))+(((IkReal(7.00000000000000))*(cj1)*(r02)))+(((IkReal(1.90000000000000))*(r00)*(sj0))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-2.45034965034965))+(((IkReal(2.65734265734266))*(cj0)*(py)))+(((IkReal(-2.65734265734266))*(px)*(sj0)))+(((IkReal(-9.79020979020979))*(py)*(x269)))+(((IkReal(-9.79020979020979))*(cj1)*(pz)))+(((IkReal(13.9860139860140))*(pp)))+(((IkReal(-9.79020979020979))*(px)*(x270))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.0909090909090909))*(((IKabs(cj4) != 0)?((IkReal)1/(cj4)):(IkReal)1.0e30))*(((((IkReal(-20.0000000000000))*(py)*(r01)))+(((IkReal(7.00000000000000))*(r00)*(x270)))+(((IkReal(-1.90000000000000))*(cj0)*(r01)))+(((IkReal(7.00000000000000))*(r01)*(x269)))+(((IkReal(-20.0000000000000))*(pz)*(r02)))+(((IkReal(-20.0000000000000))*(px)*(r00)))+(((IkReal(7.00000000000000))*(cj1)*(r02)))+(((IkReal(1.90000000000000))*(r00)*(sj0)))))))+IKsqr(((IkReal(-2.45034965034965))+(((IkReal(2.65734265734266))*(cj0)*(py)))+(((IkReal(-2.65734265734266))*(px)*(sj0)))+(((IkReal(-9.79020979020979))*(py)*(x269)))+(((IkReal(-9.79020979020979))*(cj1)*(pz)))+(((IkReal(13.9860139860140))*(pp)))+(((IkReal(-9.79020979020979))*(px)*(x270)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
j3array[0]=IKatan2(((IkReal(0.0909090909090909))*(((IKabs(cj4) != 0)?((IkReal)1/(cj4)):(IkReal)1.0e30))*(((((IkReal(-20.0000000000000))*(py)*(r01)))+(((IkReal(7.00000000000000))*(r00)*(x270)))+(((IkReal(-1.90000000000000))*(cj0)*(r01)))+(((IkReal(7.00000000000000))*(r01)*(x269)))+(((IkReal(-20.0000000000000))*(pz)*(r02)))+(((IkReal(-20.0000000000000))*(px)*(r00)))+(((IkReal(7.00000000000000))*(cj1)*(r02)))+(((IkReal(1.90000000000000))*(r00)*(sj0)))))), ((IkReal(-2.45034965034965))+(((IkReal(2.65734265734266))*(cj0)*(py)))+(((IkReal(-2.65734265734266))*(px)*(sj0)))+(((IkReal(-9.79020979020979))*(py)*(x269)))+(((IkReal(-9.79020979020979))*(cj1)*(pz)))+(((IkReal(13.9860139860140))*(pp)))+(((IkReal(-9.79020979020979))*(px)*(x270)))));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
for(int ij3 = 0; ij3 < 1; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 1; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
{
IkReal evalcond[4];
IkReal x271=IKcos(j3);
IkReal x272=((r01)*(sj1));
IkReal x273=((IkReal(0.350000000000000))*(sj0));
IkReal x274=((cj0)*(r00));
IkReal x275=((IkReal(0.350000000000000))*(cj1));
IkReal x276=((px)*(r02));
IkReal x277=((IkReal(1.00000000000000))*(cj0));
IkReal x278=((IkReal(0.190000000000000))*(px));
IkReal x279=((cj0)*(r01));
IkReal x280=((r00)*(sj0));
IkReal x281=((IkReal(0.700000000000000))*(sj1));
IkReal x282=((IkReal(0.550000000000000))*(cj4));
IkReal x283=((IkReal(0.700000000000000))*(cj1));
IkReal x284=((py)*(sj0));
IkReal x285=((IkReal(1.00000000000000))*(r02));
IkReal x286=((cj0)*(py));
IkReal x287=((IkReal(2.00000000000000))*(px));
IkReal x288=((pz)*(sj0));
IkReal x289=((pz)*(r02));
IkReal x290=((IkReal(0.350000000000000))*(sj1));
IkReal x291=((py)*(r01));
IkReal x292=((IkReal(0.0715000000000000))*(x271));
evalcond[0]=((IkReal(0.175200000000000))+(((x281)*(x284)))+(((sj0)*(x278)))+(((IkReal(-1.00000000000000))*(pp)))+(x292)+(((pz)*(x283)))+(((IkReal(-0.190000000000000))*(x286)))+(((cj0)*(px)*(x281))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x282)*(IKsin(j3))))+(((x272)*(x273)))+(((IkReal(0.0950000000000000))*(x280)))+(((x274)*(x290)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(-1.00000000000000))*(x291)))+(((r02)*(x275)))+(((IkReal(-1.00000000000000))*(pz)*(x285)))+(((IkReal(-0.0950000000000000))*(x279))));
evalcond[2]=((((r02)*(x290)))+(((r01)*(x288)))+(((IkReal(-1.00000000000000))*(x274)*(x275)))+(((IkReal(-1.00000000000000))*(x276)*(x277)))+(((IkReal(-0.0650000000000000))*(cj4)))+(((pz)*(x274)))+(((IkReal(-1.00000000000000))*(x284)*(x285)))+(((IkReal(-1.00000000000000))*(x271)*(x282)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x273))));
evalcond[3]=((((IkReal(-2.00000000000000))*(x276)*(x288)))+(((IkReal(0.306725000000000))*(sj4)))+(((pp)*(x280)))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x277)))+(((IkReal(-0.0665000000000000))*(sj1)*(x274)))+(((IkReal(0.190000000000000))*(x289)))+(((IkReal(-1.00000000000000))*(r01)*(x284)*(x287)))+(((IkReal(-0.113475000000000))*(x279)))+(((IkReal(0.700000000000000))*(px)*(x272)))+(((IkReal(2.00000000000000))*(x286)*(x289)))+(((IkReal(-1.00000000000000))*(pz)*(x280)*(x283)))+(((IkReal(0.113475000000000))*(x280)))+(((sj4)*(x292)))+(((IkReal(-1.00000000000000))*(px)*(x280)*(x287)))+(((IkReal(2.00000000000000))*(x279)*((py)*(py))))+(((r00)*(x278)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x281)))+(((IkReal(-0.0665000000000000))*(sj0)*(x272)))+(((sj0)*(x276)*(x283)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((pz)*(x279)*(x283)))+(((IkReal(-1.00000000000000))*(r02)*(x283)*(x286)))+(((py)*(x274)*(x287)))+(((IkReal(0.190000000000000))*(x291))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
IkReal dummyeval[1];
IkReal gconst1;
gconst1=IKsign(((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3))))));
dummyeval[0]=((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal dummyeval[1];
IkReal gconst2;
IkReal x293=((IkReal(13.0000000000000))*(cj4));
gconst2=IKsign(((((IkReal(-1.00000000000000))*(x293)*((sj3)*(sj3))))+(((IkReal(-110.000000000000))*(cj3)*(cj4)))+(((IkReal(-1.00000000000000))*(x293)*((cj3)*(cj3))))));
IkReal x294=((IkReal(1.00000000000000))*(cj4));
dummyeval[0]=((((IkReal(-1.00000000000000))*(x294)*((cj3)*(cj3))))+(((IkReal(-8.46153846153846))*(cj3)*(cj4)))+(((IkReal(-1.00000000000000))*(x294)*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal evalcond[11];
IkReal x295=((IkReal(0.0715000000000000))*(cj3));
IkReal x296=(py)*(py);
IkReal x297=(px)*(px);
IkReal x298=(pz)*(pz);
IkReal x299=((r01)*(sj0));
IkReal x300=((px)*(sj0));
IkReal x301=((cj1)*(r00));
IkReal x302=((IkReal(0.350000000000000))*(cj0));
IkReal x303=((IkReal(0.113475000000000))*(sj1));
IkReal x304=((r01)*(sj1));
IkReal x305=((IkReal(2.00000000000000))*(pz));
IkReal x306=((IkReal(0.190000000000000))*(px));
IkReal x307=((py)*(sj0));
IkReal x308=((IkReal(1.00000000000000))*(r02));
IkReal x309=((IkReal(0.190000000000000))*(py));
IkReal x310=((cj0)*(sj1));
IkReal x311=((px)*(r02));
IkReal x312=((px)*(r00));
IkReal x313=((r02)*(sj1));
IkReal x314=((IkReal(2.00000000000000))*(py));
IkReal x315=((pp)*(r00));
IkReal x316=((cj0)*(r01));
IkReal x317=((IkReal(0.700000000000000))*(sj1));
IkReal x318=((IkReal(0.190000000000000))*(pz));
IkReal x319=((cj1)*(r01));
IkReal x320=((cj0)*(r00));
IkReal x321=((IkReal(0.700000000000000))*(pz));
IkReal x322=((cj0)*(px));
IkReal x323=((r00)*(sj0));
IkReal x324=((cj1)*(r02));
IkReal x325=((IkReal(1.00000000000000))*(pp));
IkReal x326=((IkReal(1.00000000000000))*(py));
IkReal x327=((IkReal(0.700000000000000))*(cj0));
IkReal x328=((r00)*(sj1));
IkReal x329=((IkReal(0.700000000000000))*(px));
IkReal x330=((cj0)*(cj1));
IkReal x331=((cj1)*(x325));
IkReal x332=((IkReal(2.00000000000000))*(x298));
IkReal x333=((IkReal(2.00000000000000))*(x296));
IkReal x334=((IkReal(2.00000000000000))*(r00)*(x297));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-0.0950000000000000))+(x300)+(((IkReal(-1.00000000000000))*(cj0)*(x326))));
evalcond[2]=((IkReal(1.00000000000000))+(((IkReal(-1.00000000000000))*(x316)))+(x323));
evalcond[3]=((x324)+(((sj1)*(x299)))+(((r00)*(x310))));
evalcond[4]=((((IkReal(-1.00000000000000))*(cj1)*(x299)))+(((IkReal(-1.00000000000000))*(cj0)*(x301)))+(x313));
evalcond[5]=((IkReal(0.175200000000000))+(((IkReal(-1.00000000000000))*(x325)))+(((x307)*(x317)))+(((IkReal(-1.00000000000000))*(cj0)*(x309)))+(x295)+(((x310)*(x329)))+(((IkReal(0.190000000000000))*(x300)))+(((cj1)*(x321))));
evalcond[6]=((((x302)*(x328)))+(((IkReal(-1.00000000000000))*(pz)*(x308)))+(((IkReal(-1.00000000000000))*(x312)))+(((IkReal(0.350000000000000))*(sj1)*(x299)))+(((IkReal(0.350000000000000))*(x324)))+(((IkReal(0.0950000000000000))*(x323)))+(((IkReal(-0.0950000000000000))*(x316)))+(((IkReal(-1.00000000000000))*(r01)*(x326))));
evalcond[7]=((((IkReal(0.350000000000000))*(x313)))+(((IkReal(-1.00000000000000))*(x307)*(x308)))+(((IkReal(-1.00000000000000))*(x308)*(x322)))+(((pz)*(x320)))+(((IkReal(-1.00000000000000))*(x301)*(x302)))+(((pz)*(x299)))+(((IkReal(-0.350000000000000))*(cj1)*(x299))));
evalcond[8]=((IkReal(0.306725000000000))+(((x304)*(x329)))+(((IkReal(-1.00000000000000))*(x316)*(x325)))+(((r02)*(x318)))+(((IkReal(0.700000000000000))*(x300)*(x324)))+(((IkReal(-0.0665000000000000))*(sj1)*(x299)))+(((sj0)*(x315)))+(((r00)*(x306)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x317)))+(((IkReal(0.113475000000000))*(x323)))+(((cj0)*(x312)*(x314)))+(((x316)*(x333)))+(((IkReal(-2.00000000000000))*(x297)*(x323)))+(x295)+(((r01)*(x309)))+(((IkReal(-1.00000000000000))*(px)*(x299)*(x314)))+(((cj0)*(py)*(r02)*(x305)))+(((cj1)*(x316)*(x321)))+(((IkReal(-0.0665000000000000))*(x324)))+(((IkReal(-1.00000000000000))*(py)*(x324)*(x327)))+(((IkReal(-0.113475000000000))*(x316)))+(((IkReal(-1.00000000000000))*(r02)*(x300)*(x305)))+(((IkReal(-0.0665000000000000))*(r00)*(x310)))+(((IkReal(-1.00000000000000))*(sj0)*(x301)*(x321))));
evalcond[9]=((((IkReal(-1.00000000000000))*(cj1)*(x316)*(x318)))+(((IkReal(-1.00000000000000))*(py)*(x305)*(x319)))+(((IkReal(0.0665000000000000))*(x316)))+(((sj0)*(x301)*(x318)))+(((IkReal(-0.0665000000000000))*(x323)))+(((IkReal(-1.00000000000000))*(x324)*(x332)))+(((IkReal(0.700000000000000))*(x312)))+(((IkReal(-1.00000000000000))*(x304)*(x306)))+(((IkReal(-1.00000000000000))*(x310)*(x334)))+(((IkReal(-0.190000000000000))*(x300)*(x324)))+(((IkReal(-1.00000000000000))*(px)*(x301)*(x305)))+(((IkReal(-1.00000000000000))*(sj1)*(x299)*(x333)))+(((cj0)*(x309)*(x324)))+(((r02)*(x321)))+(((IkReal(-1.00000000000000))*(x304)*(x314)*(x322)))+(((IkReal(0.700000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(x303)*(x320)))+(((IkReal(-1.00000000000000))*(x305)*(x307)*(x313)))+(((x310)*(x315)))+(((pp)*(sj1)*(x299)))+(((IkReal(-1.00000000000000))*(x299)*(x303)))+(((IkReal(-1.00000000000000))*(x305)*(x310)*(x311)))+(((IkReal(-1.00000000000000))*(x300)*(x314)*(x328)))+(((IkReal(-0.113475000000000))*(x324)))+(((pp)*(x324)))+(((x309)*(x328))));
evalcond[10]=((((IkReal(-1.00000000000000))*(sj1)*(x305)*(x312)))+(((x320)*(x321)))+(((x306)*(x319)))+(((IkReal(-1.00000000000000))*(x301)*(x309)))+(((pp)*(x313)))+(((x299)*(x321)))+(((IkReal(-0.131525000000000))*(cj0)*(x301)))+(((x305)*(x311)*(x330)))+(((r02)*(x309)*(x310)))+(((IkReal(-1.00000000000000))*(cj0)*(x301)*(x325)))+(((IkReal(-0.190000000000000))*(x300)*(x313)))+(((IkReal(-0.131525000000000))*(cj1)*(x299)))+(((IkReal(0.131525000000000))*(x313)))+(((IkReal(-1.00000000000000))*(x299)*(x331)))+(((IkReal(-1.00000000000000))*(py)*(x304)*(x305)))+(((IkReal(-1.00000000000000))*(x311)*(x327)))+(((IkReal(-0.700000000000000))*(r02)*(x307)))+(((x300)*(x301)*(x314)))+(((IkReal(2.00000000000000))*(cj0)*(x297)*(x301)))+(((IkReal(-1.00000000000000))*(x313)*(x332)))+(((cj1)*(x299)*(x333)))+(((x305)*(x307)*(x324)))+(((IkReal(-1.00000000000000))*(cj0)*(x304)*(x318)))+(((sj1)*(x318)*(x323)))+(((cj1)*(px)*(x314)*(x316))));
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  )
{
{
IkReal dummyeval[1];
IkReal gconst3;
gconst3=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal dummyeval[1];
IkReal gconst4;
gconst4=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x335=((cj1)*(cj3));
IkReal x336=((sj0)*(sj1));
IkReal x337=((IkReal(22000.0000000000))*(pz));
IkReal x338=((IkReal(2600.00000000000))*(pz));
IkReal x339=((r02)*(sj1));
IkReal x340=((cj1)*(sj3));
IkReal x341=((cj1)*(r00));
IkReal x342=((r01)*(sj0));
IkReal x343=((cj0)*(sj1));
IkReal x344=((IkReal(2600.00000000000))*(px));
IkReal x345=((IkReal(22000.0000000000))*(px));
IkReal x346=((IkReal(22000.0000000000))*(py));
IkReal x347=((IkReal(2600.00000000000))*(py));
IkReal x348=((IkReal(247.000000000000))*(cj0)*(r00));
IkReal x349=((sj3)*(x347));
IkReal x350=((IkReal(2600.00000000000))*(sj3)*(x343));
if( IKabs(((gconst4)*(((((IkReal(2090.00000000000))*(cj1)*(x342)))+(((IkReal(-2090.00000000000))*(x339)))+(((IkReal(247.000000000000))*(x335)*(x342)))+(((r00)*(x335)*(x347)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x336)*(x338)))+(((IkReal(2090.00000000000))*(cj0)*(x341)))+(((x336)*(x349)))+(((cj3)*(r02)*(x336)*(x344)))+(((IkReal(-1.00000000000000))*(r00)*(x336)*(x337)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x345)))+(((r01)*(x337)*(x343)))+(((x338)*(x340)))+(((sj3)*(x343)*(x344)))+(((IkReal(-247.000000000000))*(cj3)*(x339)))+(((x341)*(x346)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(r01)*(x335)*(x344)))+(((IkReal(-1.00000000000000))*(cj0)*(x339)*(x346)))+(((r02)*(x336)*(x345)))+(((IkReal(-1.00000000000000))*(cj0)*(cj3)*(x339)*(x347)))+(((cj3)*(r01)*(x338)*(x343)))+(((x335)*(x348))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(cj1)*(x337)))+(((IkReal(-1.00000000000000))*(cj3)*(x343)*(x344)))+(((IkReal(-1.00000000000000))*(x343)*(x345)))+(((IkReal(-1.00000000000000))*(r01)*(x340)*(x344)))+(((IkReal(-1.00000000000000))*(x336)*(x346)))+(((IkReal(-1.00000000000000))*(cj3)*(x336)*(x347)))+(((x340)*(x348)))+(((r01)*(sj3)*(x338)*(x343)))+(((IkReal(-247.000000000000))*(sj3)*(x339)))+(((IkReal(-1.00000000000000))*(x335)*(x338)))+(((r02)*(sj3)*(x336)*(x344)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj0)*(x339)*(x349)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x336)*(x338)))+(((IkReal(247.000000000000))*(x340)*(x342)))+(((r00)*(x340)*(x347))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst4)*(((((IkReal(2090.00000000000))*(cj1)*(x342)))+(((IkReal(-2090.00000000000))*(x339)))+(((IkReal(247.000000000000))*(x335)*(x342)))+(((r00)*(x335)*(x347)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x336)*(x338)))+(((IkReal(2090.00000000000))*(cj0)*(x341)))+(((x336)*(x349)))+(((cj3)*(r02)*(x336)*(x344)))+(((IkReal(-1.00000000000000))*(r00)*(x336)*(x337)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x345)))+(((r01)*(x337)*(x343)))+(((x338)*(x340)))+(((sj3)*(x343)*(x344)))+(((IkReal(-247.000000000000))*(cj3)*(x339)))+(((x341)*(x346)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(r01)*(x335)*(x344)))+(((IkReal(-1.00000000000000))*(cj0)*(x339)*(x346)))+(((r02)*(x336)*(x345)))+(((IkReal(-1.00000000000000))*(cj0)*(cj3)*(x339)*(x347)))+(((cj3)*(r01)*(x338)*(x343)))+(((x335)*(x348)))))), ((gconst4)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(cj1)*(x337)))+(((IkReal(-1.00000000000000))*(cj3)*(x343)*(x344)))+(((IkReal(-1.00000000000000))*(x343)*(x345)))+(((IkReal(-1.00000000000000))*(r01)*(x340)*(x344)))+(((IkReal(-1.00000000000000))*(x336)*(x346)))+(((IkReal(-1.00000000000000))*(cj3)*(x336)*(x347)))+(((x340)*(x348)))+(((r01)*(sj3)*(x338)*(x343)))+(((IkReal(-247.000000000000))*(sj3)*(x339)))+(((IkReal(-1.00000000000000))*(x335)*(x338)))+(((r02)*(sj3)*(x336)*(x344)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj0)*(x339)*(x349)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x336)*(x338)))+(((IkReal(247.000000000000))*(x340)*(x342)))+(((r00)*(x340)*(x347)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x351=IKcos(j2);
IkReal x352=IKsin(j2);
IkReal x353=((px)*(sj1));
IkReal x354=((py)*(sj1));
IkReal x355=((cj1)*(sj0));
IkReal x356=((IkReal(1.00000000000000))*(px));
IkReal x357=((r00)*(sj0));
IkReal x358=((IkReal(0.0950000000000000))*(r01));
IkReal x359=((IkReal(0.0650000000000000))*(cj3));
IkReal x360=((cj0)*(r01));
IkReal x361=((IkReal(0.0950000000000000))*(cj1));
IkReal x362=((pz)*(sj1));
IkReal x363=((cj0)*(r00));
IkReal x364=((IkReal(0.0950000000000000))*(sj1));
IkReal x365=((cj0)*(cj1));
IkReal x366=((cj1)*(pz));
IkReal x367=((IkReal(0.0650000000000000))*(sj3));
IkReal x368=((IkReal(0.550000000000000))*(x352));
IkReal x369=((IkReal(0.550000000000000))*(x351));
IkReal x370=((x352)*(x359));
IkReal x371=((x351)*(x367));
IkReal x372=((x351)*(x359));
IkReal x373=((x352)*(x367));
IkReal x374=((x372)+(x369));
IkReal x375=((x371)+(x370)+(x368));
evalcond[0]=((IkReal(-0.350000000000000))+(x373)+(((IkReal(-1.00000000000000))*(x374)))+(x366)+(((cj0)*(x353)))+(((sj0)*(x354))));
evalcond[1]=((x375)+(((IkReal(-1.00000000000000))*(py)*(x355)))+(x362)+(((IkReal(-1.00000000000000))*(x356)*(x365))));
evalcond[2]=((x375)+(((r02)*(sj0)*(x353)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x356)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x354)))+(((cj1)*(py)*(r00)))+(((x360)*(x362)))+(((IkReal(-1.00000000000000))*(x357)*(x362)))+(((IkReal(-1.00000000000000))*(r02)*(x364)))+(((x355)*(x358)))+(((x361)*(x363))));
evalcond[3]=((((IkReal(-1.00000000000000))*(r02)*(x355)*(x356)))+(((r02)*(x361)))+(((IkReal(0.350000000000000))*(x360)))+(((py)*(r02)*(x365)))+(x374)+(((IkReal(-1.00000000000000))*(x360)*(x366)))+(((IkReal(-0.350000000000000))*(x357)))+(((IkReal(-1.00000000000000))*(r01)*(x353)))+(((x363)*(x364)))+(((r00)*(x354)))+(((sj0)*(sj1)*(x358)))+(((pz)*(r00)*(x355)))+(((IkReal(-1.00000000000000))*(x373))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x376=((IkReal(2600.00000000000))*(sj3));
IkReal x377=((py)*(sj0));
IkReal x378=((pz)*(sj1));
IkReal x379=((IkReal(2600.00000000000))*(cj3));
IkReal x380=((cj1)*(pz));
IkReal x381=((IkReal(22000.0000000000))*(cj1));
IkReal x382=((cj0)*(px));
IkReal x383=((sj1)*(x382));
if( IKabs(((gconst3)*(((((x376)*(x383)))+(((IkReal(22000.0000000000))*(x378)))+(((IkReal(-1.00000000000000))*(x381)*(x382)))+(((sj1)*(x376)*(x377)))+(((IkReal(-1.00000000000000))*(cj1)*(x379)*(x382)))+(((x378)*(x379)))+(((IkReal(-1.00000000000000))*(cj1)*(x377)*(x379)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x377)*(x381)))+(((x376)*(x380))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x377)*(x379)))+(((IkReal(-1.00000000000000))*(x379)*(x380)))+(((IkReal(-1.00000000000000))*(cj1)*(x376)*(x377)))+(((IkReal(-22000.0000000000))*(sj1)*(x377)))+(((IkReal(-22000.0000000000))*(x380)))+(((IkReal(-1.00000000000000))*(cj1)*(x376)*(x382)))+(((IkReal(-22000.0000000000))*(x383)))+(((IkReal(910.000000000000))*(cj3)))+(((x376)*(x378)))+(((IkReal(-1.00000000000000))*(x379)*(x383))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst3)*(((((x376)*(x383)))+(((IkReal(22000.0000000000))*(x378)))+(((IkReal(-1.00000000000000))*(x381)*(x382)))+(((sj1)*(x376)*(x377)))+(((IkReal(-1.00000000000000))*(cj1)*(x379)*(x382)))+(((x378)*(x379)))+(((IkReal(-1.00000000000000))*(cj1)*(x377)*(x379)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x377)*(x381)))+(((x376)*(x380)))))), ((gconst3)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x377)*(x379)))+(((IkReal(-1.00000000000000))*(x379)*(x380)))+(((IkReal(-1.00000000000000))*(cj1)*(x376)*(x377)))+(((IkReal(-22000.0000000000))*(sj1)*(x377)))+(((IkReal(-22000.0000000000))*(x380)))+(((IkReal(-1.00000000000000))*(cj1)*(x376)*(x382)))+(((IkReal(-22000.0000000000))*(x383)))+(((IkReal(910.000000000000))*(cj3)))+(((x376)*(x378)))+(((IkReal(-1.00000000000000))*(x379)*(x383)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x384=IKcos(j2);
IkReal x385=IKsin(j2);
IkReal x386=((px)*(sj1));
IkReal x387=((py)*(sj1));
IkReal x388=((cj1)*(sj0));
IkReal x389=((IkReal(1.00000000000000))*(px));
IkReal x390=((r00)*(sj0));
IkReal x391=((IkReal(0.0950000000000000))*(r01));
IkReal x392=((IkReal(0.0650000000000000))*(cj3));
IkReal x393=((cj0)*(r01));
IkReal x394=((IkReal(0.0950000000000000))*(cj1));
IkReal x395=((pz)*(sj1));
IkReal x396=((cj0)*(r00));
IkReal x397=((IkReal(0.0950000000000000))*(sj1));
IkReal x398=((cj0)*(cj1));
IkReal x399=((cj1)*(pz));
IkReal x400=((IkReal(0.0650000000000000))*(sj3));
IkReal x401=((IkReal(0.550000000000000))*(x385));
IkReal x402=((IkReal(0.550000000000000))*(x384));
IkReal x403=((x385)*(x392));
IkReal x404=((x384)*(x400));
IkReal x405=((x384)*(x392));
IkReal x406=((x385)*(x400));
IkReal x407=((x402)+(x405));
IkReal x408=((x401)+(x403)+(x404));
evalcond[0]=((IkReal(-0.350000000000000))+(((sj0)*(x387)))+(((IkReal(-1.00000000000000))*(x407)))+(((cj0)*(x386)))+(x406)+(x399));
evalcond[1]=((((IkReal(-1.00000000000000))*(x389)*(x398)))+(((IkReal(-1.00000000000000))*(py)*(x388)))+(x408)+(x395));
evalcond[2]=((((IkReal(-1.00000000000000))*(x390)*(x395)))+(((x388)*(x391)))+(((x393)*(x395)))+(((cj1)*(py)*(r00)))+(((r02)*(sj0)*(x386)))+(((x394)*(x396)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x389)))+(((IkReal(-1.00000000000000))*(r02)*(x397)))+(x408)+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x387))));
evalcond[3]=((((pz)*(r00)*(x388)))+(((IkReal(-1.00000000000000))*(r01)*(x386)))+(((IkReal(-1.00000000000000))*(r02)*(x388)*(x389)))+(((r00)*(x387)))+(((r02)*(x394)))+(((sj0)*(sj1)*(x391)))+(((IkReal(-0.350000000000000))*(x390)))+(((IkReal(-1.00000000000000))*(x406)))+(((IkReal(-1.00000000000000))*(x393)*(x399)))+(x407)+(((x396)*(x397)))+(((py)*(r02)*(x398)))+(((IkReal(0.350000000000000))*(x393))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
IkReal x409=((IkReal(0.0715000000000000))*(cj3));
IkReal x410=(py)*(py);
IkReal x411=(px)*(px);
IkReal x412=(pz)*(pz);
IkReal x413=((r01)*(sj0));
IkReal x414=((px)*(sj0));
IkReal x415=((cj1)*(r00));
IkReal x416=((IkReal(0.350000000000000))*(cj0));
IkReal x417=((IkReal(0.113475000000000))*(sj1));
IkReal x418=((r01)*(sj1));
IkReal x419=((IkReal(2.00000000000000))*(pz));
IkReal x420=((IkReal(0.190000000000000))*(px));
IkReal x421=((py)*(sj0));
IkReal x422=((IkReal(1.00000000000000))*(r02));
IkReal x423=((IkReal(0.190000000000000))*(py));
IkReal x424=((cj0)*(sj1));
IkReal x425=((px)*(r02));
IkReal x426=((px)*(r00));
IkReal x427=((r02)*(sj1));
IkReal x428=((IkReal(2.00000000000000))*(py));
IkReal x429=((pp)*(r00));
IkReal x430=((cj0)*(r01));
IkReal x431=((IkReal(0.700000000000000))*(sj1));
IkReal x432=((IkReal(0.190000000000000))*(pz));
IkReal x433=((cj1)*(r01));
IkReal x434=((cj0)*(r00));
IkReal x435=((IkReal(0.700000000000000))*(pz));
IkReal x436=((cj0)*(px));
IkReal x437=((r00)*(sj0));
IkReal x438=((cj1)*(r02));
IkReal x439=((IkReal(1.00000000000000))*(pp));
IkReal x440=((IkReal(1.00000000000000))*(py));
IkReal x441=((IkReal(0.700000000000000))*(cj0));
IkReal x442=((r00)*(sj1));
IkReal x443=((IkReal(0.700000000000000))*(px));
IkReal x444=((cj0)*(cj1));
IkReal x445=((cj1)*(x439));
IkReal x446=((IkReal(2.00000000000000))*(x412));
IkReal x447=((IkReal(2.00000000000000))*(x410));
IkReal x448=((IkReal(2.00000000000000))*(r00)*(x411));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(cj0)*(x440)))+(x414));
evalcond[2]=((IkReal(-1.00000000000000))+(((IkReal(-1.00000000000000))*(x430)))+(x437));
evalcond[3]=((x438)+(((sj1)*(x413)))+(((r00)*(x424))));
evalcond[4]=((((IkReal(-1.00000000000000))*(cj0)*(x415)))+(x427)+(((IkReal(-1.00000000000000))*(cj1)*(x413))));
evalcond[5]=((IkReal(0.175200000000000))+(((x424)*(x443)))+(((cj1)*(x435)))+(((x421)*(x431)))+(x409)+(((IkReal(-1.00000000000000))*(cj0)*(x423)))+(((IkReal(-1.00000000000000))*(x439)))+(((IkReal(0.190000000000000))*(x414))));
evalcond[6]=((((IkReal(-1.00000000000000))*(x426)))+(((IkReal(0.0950000000000000))*(x437)))+(((IkReal(0.350000000000000))*(x438)))+(((IkReal(-1.00000000000000))*(pz)*(x422)))+(((x416)*(x442)))+(((IkReal(-1.00000000000000))*(r01)*(x440)))+(((IkReal(-0.0950000000000000))*(x430)))+(((IkReal(0.350000000000000))*(sj1)*(x413))));
evalcond[7]=((((IkReal(-1.00000000000000))*(x415)*(x416)))+(((pz)*(x413)))+(((IkReal(0.350000000000000))*(x427)))+(((IkReal(-1.00000000000000))*(x421)*(x422)))+(((IkReal(-0.350000000000000))*(cj1)*(x413)))+(((pz)*(x434)))+(((IkReal(-1.00000000000000))*(x422)*(x436))));
evalcond[8]=((IkReal(-0.306725000000000))+(((IkReal(0.113475000000000))*(x437)))+(((IkReal(-0.113475000000000))*(x430)))+(((x418)*(x443)))+(((r02)*(x432)))+(((IkReal(-1.00000000000000))*(x430)*(x439)))+(((IkReal(-2.00000000000000))*(x411)*(x437)))+(((sj0)*(x429)))+(((IkReal(-0.0665000000000000))*(x438)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x431)))+(((r00)*(x420)))+(((IkReal(-1.00000000000000))*(x409)))+(((cj0)*(py)*(r02)*(x419)))+(((IkReal(0.700000000000000))*(x414)*(x438)))+(((cj1)*(x430)*(x435)))+(((IkReal(-1.00000000000000))*(sj0)*(x415)*(x435)))+(((IkReal(-1.00000000000000))*(r02)*(x414)*(x419)))+(((r01)*(x423)))+(((x430)*(x447)))+(((IkReal(-1.00000000000000))*(px)*(x413)*(x428)))+(((IkReal(-0.0665000000000000))*(sj1)*(x413)))+(((IkReal(-1.00000000000000))*(py)*(x438)*(x441)))+(((cj0)*(x426)*(x428)))+(((IkReal(-0.0665000000000000))*(r00)*(x424))));
evalcond[9]=((((IkReal(-1.00000000000000))*(x438)*(x446)))+(((IkReal(-1.00000000000000))*(py)*(x419)*(x433)))+(((IkReal(-0.190000000000000))*(x414)*(x438)))+(((IkReal(-1.00000000000000))*(sj1)*(x413)*(x447)))+(((IkReal(-1.00000000000000))*(x419)*(x421)*(x427)))+(((x423)*(x442)))+(((pp)*(x438)))+(((IkReal(-1.00000000000000))*(x418)*(x428)*(x436)))+(((x424)*(x429)))+(((IkReal(-1.00000000000000))*(px)*(x415)*(x419)))+(((IkReal(-1.00000000000000))*(x419)*(x424)*(x425)))+(((IkReal(-1.00000000000000))*(x414)*(x428)*(x442)))+(((IkReal(-0.113475000000000))*(x438)))+(((IkReal(0.0665000000000000))*(x430)))+(((cj0)*(x423)*(x438)))+(((IkReal(0.700000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(x424)*(x448)))+(((sj0)*(x415)*(x432)))+(((r02)*(x435)))+(((IkReal(-1.00000000000000))*(x413)*(x417)))+(((IkReal(0.700000000000000))*(x426)))+(((pp)*(sj1)*(x413)))+(((IkReal(-1.00000000000000))*(cj1)*(x430)*(x432)))+(((IkReal(-1.00000000000000))*(x417)*(x434)))+(((IkReal(-1.00000000000000))*(x418)*(x420)))+(((IkReal(-0.0665000000000000))*(x437))));
evalcond[10]=((((IkReal(-1.00000000000000))*(x427)*(x446)))+(((sj1)*(x432)*(x437)))+(((IkReal(-0.131525000000000))*(cj1)*(x413)))+(((x434)*(x435)))+(((IkReal(-1.00000000000000))*(x413)*(x445)))+(((cj1)*(px)*(x428)*(x430)))+(((IkReal(-1.00000000000000))*(sj1)*(x419)*(x426)))+(((r02)*(x423)*(x424)))+(((pp)*(x427)))+(((IkReal(-0.131525000000000))*(cj0)*(x415)))+(((x414)*(x415)*(x428)))+(((x420)*(x433)))+(((IkReal(-1.00000000000000))*(cj0)*(x415)*(x439)))+(((IkReal(-1.00000000000000))*(x425)*(x441)))+(((IkReal(-1.00000000000000))*(x415)*(x423)))+(((x413)*(x435)))+(((IkReal(-0.190000000000000))*(x414)*(x427)))+(((x419)*(x425)*(x444)))+(((IkReal(2.00000000000000))*(cj0)*(x411)*(x415)))+(((IkReal(-1.00000000000000))*(cj0)*(x418)*(x432)))+(((IkReal(-1.00000000000000))*(py)*(x418)*(x419)))+(((x419)*(x421)*(x438)))+(((IkReal(0.131525000000000))*(x427)))+(((IkReal(-0.700000000000000))*(r02)*(x421)))+(((cj1)*(x413)*(x447))));
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  )
{
{
IkReal dummyeval[1];
IkReal gconst5;
gconst5=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal dummyeval[1];
IkReal gconst6;
gconst6=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x449=((cj1)*(cj3));
IkReal x450=((sj0)*(sj1));
IkReal x451=((IkReal(22000.0000000000))*(pz));
IkReal x452=((IkReal(2600.00000000000))*(pz));
IkReal x453=((r02)*(sj1));
IkReal x454=((IkReal(2600.00000000000))*(sj3));
IkReal x455=((cj1)*(sj3));
IkReal x456=((r01)*(sj0));
IkReal x457=((cj0)*(sj1));
IkReal x458=((IkReal(2600.00000000000))*(cj3));
IkReal x459=((IkReal(2090.00000000000))*(cj1));
IkReal x460=((IkReal(22000.0000000000))*(px));
IkReal x461=((px)*(r02));
IkReal x462=((IkReal(22000.0000000000))*(py));
IkReal x463=((py)*(r00));
IkReal x464=((px)*(r01));
IkReal x465=((IkReal(247.000000000000))*(cj0)*(r00));
IkReal x466=((x454)*(x457));
if( IKabs(((gconst6)*(((((IkReal(-1.00000000000000))*(x450)*(x458)*(x461)))+(((cj3)*(r00)*(x450)*(x452)))+(((IkReal(-1.00000000000000))*(r02)*(x450)*(x460)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x462)))+(((IkReal(247.000000000000))*(cj3)*(x453)))+(((IkReal(-1.00000000000000))*(x456)*(x459)))+(((IkReal(2600.00000000000))*(x449)*(x464)))+(((px)*(x466)))+(((IkReal(2090.00000000000))*(x453)))+(((IkReal(-2600.00000000000))*(x449)*(x463)))+(((IkReal(-1.00000000000000))*(r01)*(x451)*(x457)))+(((IkReal(-1.00000000000000))*(x449)*(x465)))+(((cj0)*(x453)*(x462)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-247.000000000000))*(x449)*(x456)))+(((cj1)*(r01)*(x460)))+(((x452)*(x455)))+(((py)*(x450)*(x454)))+(((r00)*(x450)*(x451)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x459)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x452)*(x457)))+(((cj0)*(py)*(x453)*(x458))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(7700.00000000000))+(((cj1)*(x454)*(x464)))+(((IkReal(-1.00000000000000))*(x450)*(x454)*(x461)))+(((IkReal(-1.00000000000000))*(x449)*(x452)))+(((r00)*(sj3)*(x450)*(x452)))+(((IkReal(-1.00000000000000))*(py)*(x450)*(x458)))+(((IkReal(-247.000000000000))*(x455)*(x456)))+(((IkReal(-1.00000000000000))*(px)*(x457)*(x458)))+(((IkReal(247.000000000000))*(sj3)*(x453)))+(((IkReal(-1.00000000000000))*(x455)*(x465)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x452)*(x457)))+(((IkReal(-1.00000000000000))*(x457)*(x460)))+(((IkReal(-1.00000000000000))*(x450)*(x462)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x451)))+(((IkReal(-1.00000000000000))*(cj1)*(x454)*(x463)))+(((cj0)*(py)*(x453)*(x454))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst6)*(((((IkReal(-1.00000000000000))*(x450)*(x458)*(x461)))+(((cj3)*(r00)*(x450)*(x452)))+(((IkReal(-1.00000000000000))*(r02)*(x450)*(x460)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x462)))+(((IkReal(247.000000000000))*(cj3)*(x453)))+(((IkReal(-1.00000000000000))*(x456)*(x459)))+(((IkReal(2600.00000000000))*(x449)*(x464)))+(((px)*(x466)))+(((IkReal(2090.00000000000))*(x453)))+(((IkReal(-2600.00000000000))*(x449)*(x463)))+(((IkReal(-1.00000000000000))*(r01)*(x451)*(x457)))+(((IkReal(-1.00000000000000))*(x449)*(x465)))+(((cj0)*(x453)*(x462)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-247.000000000000))*(x449)*(x456)))+(((cj1)*(r01)*(x460)))+(((x452)*(x455)))+(((py)*(x450)*(x454)))+(((r00)*(x450)*(x451)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x459)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x452)*(x457)))+(((cj0)*(py)*(x453)*(x458)))))), ((gconst6)*(((IkReal(7700.00000000000))+(((cj1)*(x454)*(x464)))+(((IkReal(-1.00000000000000))*(x450)*(x454)*(x461)))+(((IkReal(-1.00000000000000))*(x449)*(x452)))+(((r00)*(sj3)*(x450)*(x452)))+(((IkReal(-1.00000000000000))*(py)*(x450)*(x458)))+(((IkReal(-247.000000000000))*(x455)*(x456)))+(((IkReal(-1.00000000000000))*(px)*(x457)*(x458)))+(((IkReal(247.000000000000))*(sj3)*(x453)))+(((IkReal(-1.00000000000000))*(x455)*(x465)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x452)*(x457)))+(((IkReal(-1.00000000000000))*(x457)*(x460)))+(((IkReal(-1.00000000000000))*(x450)*(x462)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x451)))+(((IkReal(-1.00000000000000))*(cj1)*(x454)*(x463)))+(((cj0)*(py)*(x453)*(x454)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x467=IKcos(j2);
IkReal x468=IKsin(j2);
IkReal x469=((px)*(sj1));
IkReal x470=((py)*(sj1));
IkReal x471=((cj1)*(sj0));
IkReal x472=((IkReal(1.00000000000000))*(px));
IkReal x473=((r00)*(sj0));
IkReal x474=((IkReal(0.0950000000000000))*(r01));
IkReal x475=((IkReal(0.0650000000000000))*(cj3));
IkReal x476=((cj0)*(r01));
IkReal x477=((IkReal(0.0950000000000000))*(cj1));
IkReal x478=((pz)*(sj1));
IkReal x479=((cj0)*(r00));
IkReal x480=((IkReal(0.0950000000000000))*(sj1));
IkReal x481=((cj0)*(cj1));
IkReal x482=((cj1)*(pz));
IkReal x483=((IkReal(0.0650000000000000))*(sj3));
IkReal x484=((IkReal(0.550000000000000))*(x468));
IkReal x485=((IkReal(0.550000000000000))*(x467));
IkReal x486=((x468)*(x475));
IkReal x487=((x467)*(x483));
IkReal x488=((x468)*(x483));
IkReal x489=((x467)*(x475));
IkReal x490=((x489)+(x485));
IkReal x491=((x484)+(x487)+(x486));
evalcond[0]=((IkReal(-0.350000000000000))+(((IkReal(-1.00000000000000))*(x490)))+(((sj0)*(x470)))+(x488)+(x482)+(((cj0)*(x469))));
evalcond[1]=((x478)+(((IkReal(-1.00000000000000))*(py)*(x471)))+(x491)+(((IkReal(-1.00000000000000))*(x472)*(x481))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x491)))+(((cj1)*(py)*(r00)))+(((x476)*(x478)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x470)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x472)))+(((IkReal(-1.00000000000000))*(r02)*(x480)))+(((IkReal(-1.00000000000000))*(x473)*(x478)))+(((x477)*(x479)))+(((x471)*(x474)))+(((r02)*(sj0)*(x469))));
evalcond[3]=((((x479)*(x480)))+(((IkReal(-1.00000000000000))*(r01)*(x469)))+(((pz)*(r00)*(x471)))+(((IkReal(-1.00000000000000))*(x490)))+(((IkReal(-1.00000000000000))*(x476)*(x482)))+(((r02)*(x477)))+(((py)*(r02)*(x481)))+(((r00)*(x470)))+(((IkReal(-0.350000000000000))*(x473)))+(x488)+(((IkReal(0.350000000000000))*(x476)))+(((IkReal(-1.00000000000000))*(r02)*(x471)*(x472)))+(((sj0)*(sj1)*(x474))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x492=((IkReal(2600.00000000000))*(sj3));
IkReal x493=((py)*(sj0));
IkReal x494=((pz)*(sj1));
IkReal x495=((IkReal(2600.00000000000))*(cj3));
IkReal x496=((cj1)*(pz));
IkReal x497=((IkReal(22000.0000000000))*(cj1));
IkReal x498=((cj0)*(px));
IkReal x499=((sj1)*(x498));
if( IKabs(((gconst5)*(((((x492)*(x499)))+(((sj1)*(x492)*(x493)))+(((x494)*(x495)))+(((x492)*(x496)))+(((IkReal(22000.0000000000))*(x494)))+(((IkReal(-1.00000000000000))*(cj1)*(x495)*(x498)))+(((IkReal(-1.00000000000000))*(cj1)*(x493)*(x495)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x497)*(x498)))+(((IkReal(-1.00000000000000))*(x493)*(x497))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x496)))+(((IkReal(-1.00000000000000))*(x495)*(x499)))+(((IkReal(-1.00000000000000))*(cj1)*(x492)*(x493)))+(((IkReal(-1.00000000000000))*(x495)*(x496)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(sj1)*(x493)))+(((IkReal(-1.00000000000000))*(sj1)*(x493)*(x495)))+(((IkReal(-22000.0000000000))*(x499)))+(((x492)*(x494)))+(((IkReal(-1.00000000000000))*(cj1)*(x492)*(x498))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst5)*(((((x492)*(x499)))+(((sj1)*(x492)*(x493)))+(((x494)*(x495)))+(((x492)*(x496)))+(((IkReal(22000.0000000000))*(x494)))+(((IkReal(-1.00000000000000))*(cj1)*(x495)*(x498)))+(((IkReal(-1.00000000000000))*(cj1)*(x493)*(x495)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(-1.00000000000000))*(x497)*(x498)))+(((IkReal(-1.00000000000000))*(x493)*(x497)))))), ((gconst5)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x496)))+(((IkReal(-1.00000000000000))*(x495)*(x499)))+(((IkReal(-1.00000000000000))*(cj1)*(x492)*(x493)))+(((IkReal(-1.00000000000000))*(x495)*(x496)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(sj1)*(x493)))+(((IkReal(-1.00000000000000))*(sj1)*(x493)*(x495)))+(((IkReal(-22000.0000000000))*(x499)))+(((x492)*(x494)))+(((IkReal(-1.00000000000000))*(cj1)*(x492)*(x498)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x500=IKcos(j2);
IkReal x501=IKsin(j2);
IkReal x502=((px)*(sj1));
IkReal x503=((py)*(sj1));
IkReal x504=((cj1)*(sj0));
IkReal x505=((IkReal(1.00000000000000))*(px));
IkReal x506=((r00)*(sj0));
IkReal x507=((IkReal(0.0950000000000000))*(r01));
IkReal x508=((IkReal(0.0650000000000000))*(cj3));
IkReal x509=((cj0)*(r01));
IkReal x510=((IkReal(0.0950000000000000))*(cj1));
IkReal x511=((pz)*(sj1));
IkReal x512=((cj0)*(r00));
IkReal x513=((IkReal(0.0950000000000000))*(sj1));
IkReal x514=((cj0)*(cj1));
IkReal x515=((cj1)*(pz));
IkReal x516=((IkReal(0.0650000000000000))*(sj3));
IkReal x517=((IkReal(0.550000000000000))*(x501));
IkReal x518=((IkReal(0.550000000000000))*(x500));
IkReal x519=((x501)*(x508));
IkReal x520=((x500)*(x516));
IkReal x521=((x501)*(x516));
IkReal x522=((x500)*(x508));
IkReal x523=((x518)+(x522));
IkReal x524=((x519)+(x517)+(x520));
evalcond[0]=((IkReal(-0.350000000000000))+(((sj0)*(x503)))+(x515)+(((cj0)*(x502)))+(((IkReal(-1.00000000000000))*(x523)))+(x521));
evalcond[1]=((x511)+(((IkReal(-1.00000000000000))*(x505)*(x514)))+(((IkReal(-1.00000000000000))*(py)*(x504)))+(x524));
evalcond[2]=((((x509)*(x511)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x505)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x503)))+(((IkReal(-1.00000000000000))*(x524)))+(((IkReal(-1.00000000000000))*(x506)*(x511)))+(((cj1)*(py)*(r00)))+(((x510)*(x512)))+(((x504)*(x507)))+(((IkReal(-1.00000000000000))*(r02)*(x513)))+(((r02)*(sj0)*(x502))));
evalcond[3]=((((sj0)*(sj1)*(x507)))+(((r02)*(x510)))+(((pz)*(r00)*(x504)))+(((r00)*(x503)))+(((py)*(r02)*(x514)))+(((x512)*(x513)))+(((IkReal(-1.00000000000000))*(x509)*(x515)))+(((IkReal(-0.350000000000000))*(x506)))+(((IkReal(-1.00000000000000))*(r02)*(x504)*(x505)))+(((IkReal(0.350000000000000))*(x509)))+(((IkReal(-1.00000000000000))*(x523)))+(x521)+(((IkReal(-1.00000000000000))*(r01)*(x502))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
IkReal x525=((r01)*(sj1));
IkReal x526=((IkReal(0.350000000000000))*(sj0));
IkReal x527=((px)*(r02));
IkReal x528=((IkReal(1.00000000000000))*(cj0));
IkReal x529=((cj0)*(r00));
IkReal x530=((IkReal(0.350000000000000))*(cj1));
IkReal x531=((IkReal(0.700000000000000))*(sj1));
IkReal x532=((IkReal(0.190000000000000))*(px));
IkReal x533=((r00)*(sj0));
IkReal x534=((px)*(sj0));
IkReal x535=((IkReal(0.700000000000000))*(cj1));
IkReal x536=((py)*(r02));
IkReal x537=((IkReal(0.190000000000000))*(py));
IkReal x538=((IkReal(2.00000000000000))*(py));
IkReal x539=((pz)*(r01));
IkReal x540=((IkReal(2.00000000000000))*(cj0));
IkReal x541=((IkReal(0.350000000000000))*(sj1));
IkReal x542=((pz)*(r02));
IkReal x543=((cj0)*(r01));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.45233765858574))+(j3)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(py)*(x528)))+(x534));
evalcond[2]=((sj4)+(((IkReal(-1.00000000000000))*(r01)*(x528)))+(x533));
evalcond[3]=((IkReal(0.166749999870000))+(((cj0)*(px)*(x531)))+(((sj0)*(x532)))+(((pz)*(x535)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(cj0)*(x537)))+(((py)*(sj0)*(x531))));
evalcond[4]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((IkReal(-0.546145583500000))*(cj4)))+(((r02)*(x530)))+(((IkReal(0.0950000000000000))*(x533)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(-1.00000000000000))*(x542)))+(((x529)*(x541)))+(((IkReal(-0.0950000000000000))*(x543)))+(((x525)*(x526))));
evalcond[5]=((((IkReal(-1.00000000000000))*(sj0)*(x536)))+(((IkReal(1.00000000000000e-9))*(cj4)))+(((IkReal(-1.00000000000000))*(x527)*(x528)))+(((pz)*(x529)))+(((sj0)*(x539)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x526)))+(((r02)*(x541)))+(((IkReal(-1.00000000000000))*(x529)*(x530))));
evalcond[6]=((((px)*(x529)*(x538)))+(((IkReal(-0.0665000000000000))*(sj0)*(x525)))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x528)))+(((pz)*(x536)*(x540)))+(((sj0)*(x527)*(x535)))+(((IkReal(-0.113475000000000))*(x543)))+(((pp)*(x533)))+(((IkReal(-2.00000000000000))*(x533)*((px)*(px))))+(((IkReal(-1.00000000000000))*(r01)*(x534)*(x538)))+(((IkReal(-0.0665000000000000))*(sj1)*(x529)))+(((IkReal(-1.00000000000000))*(cj0)*(x535)*(x536)))+(((IkReal(-1.00000000000000))*(pz)*(x533)*(x535)))+(((IkReal(0.298274999870000))*(sj4)))+(((r01)*(x537)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((IkReal(-2.00000000000000))*(pz)*(sj0)*(x527)))+(((IkReal(0.113475000000000))*(x533)))+(((IkReal(0.700000000000000))*(px)*(x525)))+(((cj0)*(x535)*(x539)))+(((r00)*(x532)))+(((py)*(x538)*(x543)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x531)))+(((IkReal(0.190000000000000))*(x542))));
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  )
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x544=((IkReal(1.81818181870518))*(sj1));
IkReal x545=((py)*(sj0));
IkReal x546=((IkReal(0.216392517249668))*(sj1));
IkReal x547=((cj0)*(px));
IkReal x548=((IkReal(0.216392517249668))*(cj1));
IkReal x549=((IkReal(1.81818181870518))*(cj1));
if( IKabs(((IkReal(0.0757373810373838))+(((x545)*(x549)))+(((IkReal(-1.00000000000000))*(x545)*(x546)))+(((IkReal(-1.00000000000000))*(pz)*(x544)))+(((IkReal(-1.00000000000000))*(pz)*(x548)))+(((x547)*(x549)))+(((IkReal(-1.00000000000000))*(x546)*(x547))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.636363636546814))+(((x545)*(x548)))+(((pz)*(x549)))+(((x544)*(x547)))+(((x547)*(x548)))+(((IkReal(-1.00000000000000))*(pz)*(x546)))+(((x544)*(x545))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.0757373810373838))+(((x545)*(x549)))+(((IkReal(-1.00000000000000))*(x545)*(x546)))+(((IkReal(-1.00000000000000))*(pz)*(x544)))+(((IkReal(-1.00000000000000))*(pz)*(x548)))+(((x547)*(x549)))+(((IkReal(-1.00000000000000))*(x546)*(x547)))))+IKsqr(((IkReal(-0.636363636546814))+(((x545)*(x548)))+(((pz)*(x549)))+(((x544)*(x547)))+(((x547)*(x548)))+(((IkReal(-1.00000000000000))*(pz)*(x546)))+(((x544)*(x545)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
j2array[0]=IKatan2(((IkReal(0.0757373810373838))+(((x545)*(x549)))+(((IkReal(-1.00000000000000))*(x545)*(x546)))+(((IkReal(-1.00000000000000))*(pz)*(x544)))+(((IkReal(-1.00000000000000))*(pz)*(x548)))+(((x547)*(x549)))+(((IkReal(-1.00000000000000))*(x546)*(x547)))), ((IkReal(-0.636363636546814))+(((x545)*(x548)))+(((pz)*(x549)))+(((x544)*(x547)))+(((x547)*(x548)))+(((IkReal(-1.00000000000000))*(pz)*(x546)))+(((x544)*(x545)))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[8];
IkReal x550=IKsin(j2);
IkReal x551=IKcos(j2);
IkReal x552=(py)*(py);
IkReal x553=(px)*(px);
IkReal x554=(pz)*(pz);
IkReal x555=((sj0)*(sj1));
IkReal x556=((py)*(r00));
IkReal x557=((IkReal(0.190000000000000))*(cj1));
IkReal x558=((cj0)*(r02));
IkReal x559=((py)*(r01));
IkReal x560=((px)*(r01));
IkReal x561=((IkReal(0.190000000000000))*(sj1));
IkReal x562=((cj1)*(r02));
IkReal x563=((pz)*(r00));
IkReal x564=((cj1)*(sj0));
IkReal x565=((cj0)*(r00));
IkReal x566=((IkReal(2.00000000000000))*(sj1));
IkReal x567=((IkReal(0.700000000000000))*(px));
IkReal x568=((IkReal(1.00000000000000))*(cj1));
IkReal x569=((IkReal(2.00000000000000))*(px));
IkReal x570=((pp)*(r01));
IkReal x571=((pp)*(sj1));
IkReal x572=((r02)*(sj1));
IkReal x573=((pz)*(r01));
IkReal x574=((px)*(r02));
IkReal x575=((IkReal(0.700000000000000))*(sj0));
IkReal x576=((cj0)*(sj1));
IkReal x577=((r00)*(sj0));
IkReal x578=((cj0)*(r01));
IkReal x579=((IkReal(1.00000000000000))*(py));
IkReal x580=((pz)*(r02));
IkReal x581=((IkReal(2.00000000000000))*(py));
IkReal x582=((cj1)*(pz));
IkReal x583=((IkReal(0.0950000000000000))*(r01));
IkReal x584=((cj0)*(px));
IkReal x585=((pz)*(x566));
IkReal x586=((cj4)*(x550));
IkReal x587=((cj4)*(x551));
IkReal x588=((sj4)*(x550));
IkReal x589=((IkReal(0.0645444780500000))*(x551));
IkReal x590=((IkReal(0.542318181700000))*(x551));
IkReal x591=((IkReal(2.00000000000000))*(r01)*(x552));
evalcond[0]=((x562)+(((r01)*(x555)))+(((sj1)*(x565)))+(((IkReal(0.992991970000000))*(x587)))+(((IkReal(-0.118181820000000))*(x586))));
evalcond[1]=((IkReal(-0.350000000000000))+(((IkReal(-1.00000000000000))*(x590)))+(((py)*(x555)))+(((px)*(x576)))+(((IkReal(0.0645444780500000))*(x550)))+(x582));
evalcond[2]=((x572)+(((IkReal(-0.118181820000000))*(x587)))+(((IkReal(-1.00000000000000))*(x565)*(x568)))+(((IkReal(-0.992991970000000))*(x586)))+(((IkReal(-1.00000000000000))*(r01)*(x564))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x564)*(x579)))+(((pz)*(sj1)))+(((IkReal(-1.00000000000000))*(x568)*(x584)))+(((IkReal(0.542318181700000))*(x550)))+(x589));
evalcond[4]=((((IkReal(-0.0950000000000000))*(x572)))+(((IkReal(-1.00000000000000))*(x560)*(x568)))+(((x573)*(x576)))+(((IkReal(-1.00000000000000))*(sj1)*(x558)*(x579)))+(((sj4)*(x589)))+(((x555)*(x574)))+(((IkReal(-1.00000000000000))*(x555)*(x563)))+(((IkReal(0.0950000000000000))*(cj1)*(x565)))+(((IkReal(0.542318181700000))*(x588)))+(((x564)*(x583)))+(((cj1)*(x556))));
evalcond[5]=((((IkReal(0.0950000000000000))*(x562)))+(((IkReal(0.350000000000000))*(x578)))+(((sj4)*(x590)))+(((sj1)*(x556)))+(((x563)*(x564)))+(((x555)*(x583)))+(((IkReal(-1.00000000000000))*(cj0)*(x568)*(x573)))+(((IkReal(-0.350000000000000))*(x577)))+(((IkReal(-0.0645444780500000))*(x588)))+(((IkReal(-1.00000000000000))*(px)*(sj0)*(x562)))+(((IkReal(-1.00000000000000))*(sj1)*(x560)))+(((cj1)*(py)*(x558)))+(((IkReal(0.0950000000000000))*(sj1)*(x565))));
evalcond[6]=((((IkReal(-1.00000000000000))*(cj1)*(x563)*(x569)))+(((IkReal(-0.0665000000000000))*(x577)))+(((py)*(x557)*(x558)))+(((IkReal(-0.113475000000000))*(sj1)*(x565)))+(((IkReal(-1.00000000000000))*(px)*(x558)*(x585)))+(((IkReal(-0.296184679851750))*(x587)))+(((x556)*(x561)))+(((IkReal(-0.113475000000000))*(x562)))+(((IkReal(-0.113475000000000))*(r01)*(x555)))+(((x565)*(x571)))+(((x555)*(x570)))+(((IkReal(-1.00000000000000))*(x555)*(x556)*(x569)))+(((sj0)*(x557)*(x563)))+(((IkReal(-1.00000000000000))*(cj0)*(x557)*(x573)))+(((IkReal(-2.00000000000000))*(x554)*(x562)))+(((IkReal(0.700000000000000))*(x559)))+(((IkReal(-1.00000000000000))*(x553)*(x565)*(x566)))+(((IkReal(-1.00000000000000))*(sj0)*(x557)*(x574)))+(((IkReal(-1.00000000000000))*(x555)*(x580)*(x581)))+(((IkReal(0.0352506812605000))*(x586)))+(((IkReal(-1.00000000000000))*(x560)*(x561)))+(((IkReal(0.0665000000000000))*(x578)))+(((IkReal(-1.00000000000000))*(x559)*(x566)*(x584)))+(((IkReal(-2.00000000000000))*(x559)*(x582)))+(((IkReal(0.700000000000000))*(x580)))+(((IkReal(-1.00000000000000))*(x555)*(x591)))+(((r00)*(x567)))+(((pp)*(x562))));
evalcond[7]=((((IkReal(-0.131525000000000))*(cj1)*(x565)))+(((py)*(x558)*(x561)))+(((r02)*(x571)))+(((IkReal(0.296184679851750))*(x586)))+(((IkReal(-1.00000000000000))*(x556)*(x557)))+(((IkReal(-1.00000000000000))*(x558)*(x567)))+(((x558)*(x569)*(x582)))+(((x556)*(x564)*(x569)))+(((IkReal(-1.00000000000000))*(x559)*(x585)))+(((IkReal(-1.00000000000000))*(py)*(r02)*(x575)))+(((x564)*(x591)))+(((IkReal(0.0352506812605000))*(x587)))+(((IkReal(-1.00000000000000))*(px)*(x563)*(x566)))+(((IkReal(-0.190000000000000))*(x555)*(x574)))+(((IkReal(2.00000000000000))*(cj1)*(x553)*(x565)))+(((x557)*(x560)))+(((IkReal(-1.00000000000000))*(x564)*(x570)))+(((IkReal(0.190000000000000))*(x555)*(x563)))+(((cj0)*(cj1)*(x559)*(x569)))+(((x573)*(x575)))+(((IkReal(-1.00000000000000))*(r02)*(x554)*(x566)))+(((IkReal(-0.131525000000000))*(r01)*(x564)))+(((IkReal(-1.00000000000000))*(pp)*(x565)*(x568)))+(((IkReal(-1.00000000000000))*(cj0)*(x561)*(x573)))+(((IkReal(0.700000000000000))*(cj0)*(x563)))+(((pz)*(sj0)*(x562)*(x581)))+(((IkReal(0.131525000000000))*(x572))));
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  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

} else
{
IkReal x592=((r01)*(sj1));
IkReal x593=((IkReal(0.350000000000000))*(sj0));
IkReal x594=((px)*(r02));
IkReal x595=((IkReal(1.00000000000000))*(cj0));
IkReal x596=((cj0)*(r00));
IkReal x597=((IkReal(0.350000000000000))*(cj1));
IkReal x598=((IkReal(0.700000000000000))*(sj1));
IkReal x599=((IkReal(0.190000000000000))*(px));
IkReal x600=((r00)*(sj0));
IkReal x601=((px)*(sj0));
IkReal x602=((IkReal(0.700000000000000))*(cj1));
IkReal x603=((py)*(r02));
IkReal x604=((IkReal(0.190000000000000))*(py));
IkReal x605=((IkReal(2.00000000000000))*(py));
IkReal x606=((pz)*(r01));
IkReal x607=((IkReal(2.00000000000000))*(cj0));
IkReal x608=((IkReal(0.350000000000000))*(sj1));
IkReal x609=((pz)*(r02));
IkReal x610=((cj0)*(r01));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.83084764859385))+(j3)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(py)*(x595)))+(x601));
evalcond[2]=((sj4)+(((IkReal(-1.00000000000000))*(r01)*(x595)))+(x600));
evalcond[3]=((IkReal(0.166749999870000))+(((pz)*(x602)))+(((IkReal(-1.00000000000000))*(cj0)*(x604)))+(((IkReal(-1.00000000000000))*(pp)))+(((cj0)*(px)*(x598)))+(((py)*(sj0)*(x598)))+(((sj0)*(x599))));
evalcond[4]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(x609)))+(((r02)*(x597)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(0.0950000000000000))*(x600)))+(((x592)*(x593)))+(((IkReal(-0.0950000000000000))*(x610)))+(((x596)*(x608)))+(((IkReal(0.546145583500000))*(cj4))));
evalcond[5]=((((sj0)*(x606)))+(((IkReal(1.00000000000000e-9))*(cj4)))+(((r02)*(x608)))+(((IkReal(-1.00000000000000))*(sj0)*(x603)))+(((IkReal(-1.00000000000000))*(x596)*(x597)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x593)))+(((pz)*(x596)))+(((IkReal(-1.00000000000000))*(x594)*(x595))));
evalcond[6]=((((IkReal(0.700000000000000))*(px)*(x592)))+(((pz)*(x603)*(x607)))+(((IkReal(0.190000000000000))*(x609)))+(((sj0)*(x594)*(x602)))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x595)))+(((IkReal(-0.0665000000000000))*(sj0)*(x592)))+(((r01)*(x604)))+(((IkReal(-1.00000000000000))*(r01)*(x601)*(x605)))+(((px)*(x596)*(x605)))+(((IkReal(0.113475000000000))*(x600)))+(((r00)*(x599)))+(((cj0)*(x602)*(x606)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x598)))+(((IkReal(-2.00000000000000))*(pz)*(sj0)*(x594)))+(((py)*(x605)*(x610)))+(((IkReal(-0.113475000000000))*(x610)))+(((pp)*(x600)))+(((IkReal(0.298274999870000))*(sj4)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((IkReal(-1.00000000000000))*(cj0)*(x602)*(x603)))+(((IkReal(-1.00000000000000))*(pz)*(x600)*(x602)))+(((IkReal(-0.0665000000000000))*(sj1)*(x596)))+(((IkReal(-2.00000000000000))*(x600)*((px)*(px)))));
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  )
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x611=((IkReal(1.81818181870518))*(sj1));
IkReal x612=((py)*(sj0));
IkReal x613=((IkReal(0.216392517249668))*(sj1));
IkReal x614=((cj0)*(px));
IkReal x615=((IkReal(0.216392517249668))*(cj1));
IkReal x616=((IkReal(1.81818181870518))*(cj1));
if( IKabs(((IkReal(-0.0757373810373838))+(((x614)*(x616)))+(((x612)*(x616)))+(((IkReal(-1.00000000000000))*(pz)*(x611)))+(((x612)*(x613)))+(((pz)*(x615)))+(((x613)*(x614))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.636363636546814))+(((x611)*(x614)))+(((pz)*(x613)))+(((pz)*(x616)))+(((IkReal(-1.00000000000000))*(x614)*(x615)))+(((IkReal(-1.00000000000000))*(x612)*(x615)))+(((x611)*(x612))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-0.0757373810373838))+(((x614)*(x616)))+(((x612)*(x616)))+(((IkReal(-1.00000000000000))*(pz)*(x611)))+(((x612)*(x613)))+(((pz)*(x615)))+(((x613)*(x614)))))+IKsqr(((IkReal(-0.636363636546814))+(((x611)*(x614)))+(((pz)*(x613)))+(((pz)*(x616)))+(((IkReal(-1.00000000000000))*(x614)*(x615)))+(((IkReal(-1.00000000000000))*(x612)*(x615)))+(((x611)*(x612)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
j2array[0]=IKatan2(((IkReal(-0.0757373810373838))+(((x614)*(x616)))+(((x612)*(x616)))+(((IkReal(-1.00000000000000))*(pz)*(x611)))+(((x612)*(x613)))+(((pz)*(x615)))+(((x613)*(x614)))), ((IkReal(-0.636363636546814))+(((x611)*(x614)))+(((pz)*(x613)))+(((pz)*(x616)))+(((IkReal(-1.00000000000000))*(x614)*(x615)))+(((IkReal(-1.00000000000000))*(x612)*(x615)))+(((x611)*(x612)))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[8];
IkReal x617=IKsin(j2);
IkReal x618=IKcos(j2);
IkReal x619=(py)*(py);
IkReal x620=(px)*(px);
IkReal x621=(pz)*(pz);
IkReal x622=((sj0)*(sj1));
IkReal x623=((py)*(r00));
IkReal x624=((IkReal(0.190000000000000))*(cj1));
IkReal x625=((cj0)*(r02));
IkReal x626=((py)*(r01));
IkReal x627=((px)*(r01));
IkReal x628=((IkReal(0.190000000000000))*(sj1));
IkReal x629=((cj1)*(r02));
IkReal x630=((pz)*(r00));
IkReal x631=((cj1)*(sj0));
IkReal x632=((cj0)*(r00));
IkReal x633=((IkReal(2.00000000000000))*(sj1));
IkReal x634=((IkReal(0.700000000000000))*(px));
IkReal x635=((IkReal(1.00000000000000))*(cj1));
IkReal x636=((IkReal(2.00000000000000))*(px));
IkReal x637=((pp)*(r01));
IkReal x638=((pp)*(sj1));
IkReal x639=((r02)*(sj1));
IkReal x640=((pz)*(r01));
IkReal x641=((px)*(r02));
IkReal x642=((IkReal(0.700000000000000))*(sj0));
IkReal x643=((cj0)*(sj1));
IkReal x644=((r00)*(sj0));
IkReal x645=((cj0)*(r01));
IkReal x646=((IkReal(1.00000000000000))*(py));
IkReal x647=((pz)*(r02));
IkReal x648=((IkReal(2.00000000000000))*(py));
IkReal x649=((cj1)*(pz));
IkReal x650=((IkReal(0.0950000000000000))*(r01));
IkReal x651=((cj0)*(px));
IkReal x652=((pz)*(x633));
IkReal x653=((cj4)*(x617));
IkReal x654=((cj4)*(x618));
IkReal x655=((sj4)*(x617));
IkReal x656=((IkReal(0.0645444780500000))*(x618));
IkReal x657=((IkReal(0.542318181700000))*(x618));
IkReal x658=((IkReal(2.00000000000000))*(r01)*(x619));
evalcond[0]=((((sj1)*(x632)))+(((r01)*(x622)))+(((IkReal(-0.118181820000000))*(x653)))+(x629)+(((IkReal(-0.992991970000000))*(x654))));
evalcond[1]=((IkReal(-0.350000000000000))+(x649)+(((py)*(x622)))+(((IkReal(-0.0645444780500000))*(x617)))+(((px)*(x643)))+(((IkReal(-1.00000000000000))*(x657))));
evalcond[2]=((((IkReal(-1.00000000000000))*(r01)*(x631)))+(((IkReal(-1.00000000000000))*(x632)*(x635)))+(((IkReal(0.992991970000000))*(x653)))+(x639)+(((IkReal(-0.118181820000000))*(x654))));
evalcond[3]=((((pz)*(sj1)))+(((IkReal(0.542318181700000))*(x617)))+(((IkReal(-1.00000000000000))*(x635)*(x651)))+(((IkReal(-1.00000000000000))*(x656)))+(((IkReal(-1.00000000000000))*(x631)*(x646))));
evalcond[4]=((((IkReal(0.542318181700000))*(x655)))+(((IkReal(-1.00000000000000))*(x627)*(x635)))+(((IkReal(-1.00000000000000))*(x622)*(x630)))+(((IkReal(-1.00000000000000))*(sj4)*(x656)))+(((IkReal(-1.00000000000000))*(sj1)*(x625)*(x646)))+(((x640)*(x643)))+(((x631)*(x650)))+(((IkReal(0.0950000000000000))*(cj1)*(x632)))+(((x622)*(x641)))+(((cj1)*(x623)))+(((IkReal(-0.0950000000000000))*(x639))));
evalcond[5]=((((IkReal(0.0950000000000000))*(x629)))+(((x630)*(x631)))+(((sj1)*(x623)))+(((IkReal(-1.00000000000000))*(px)*(sj0)*(x629)))+(((sj4)*(x657)))+(((x622)*(x650)))+(((IkReal(-0.350000000000000))*(x644)))+(((IkReal(0.0645444780500000))*(x655)))+(((cj1)*(py)*(x625)))+(((IkReal(0.0950000000000000))*(sj1)*(x632)))+(((IkReal(-1.00000000000000))*(sj1)*(x627)))+(((IkReal(0.350000000000000))*(x645)))+(((IkReal(-1.00000000000000))*(cj0)*(x635)*(x640))));
evalcond[6]=((((IkReal(-1.00000000000000))*(x627)*(x628)))+(((IkReal(-1.00000000000000))*(x626)*(x633)*(x651)))+(((IkReal(-0.113475000000000))*(x629)))+(((IkReal(0.296184679851750))*(x654)))+(((IkReal(-0.113475000000000))*(sj1)*(x632)))+(((IkReal(-1.00000000000000))*(x622)*(x623)*(x636)))+(((IkReal(0.700000000000000))*(x647)))+(((IkReal(-1.00000000000000))*(x622)*(x658)))+(((IkReal(-2.00000000000000))*(x621)*(x629)))+(((sj0)*(x624)*(x630)))+(((py)*(x624)*(x625)))+(((pp)*(x629)))+(((x622)*(x637)))+(((x632)*(x638)))+(((IkReal(-0.113475000000000))*(r01)*(x622)))+(((IkReal(-1.00000000000000))*(cj1)*(x630)*(x636)))+(((IkReal(-1.00000000000000))*(x622)*(x647)*(x648)))+(((IkReal(0.0665000000000000))*(x645)))+(((IkReal(-1.00000000000000))*(x620)*(x632)*(x633)))+(((IkReal(-1.00000000000000))*(cj0)*(x624)*(x640)))+(((IkReal(0.0352506812605000))*(x653)))+(((x623)*(x628)))+(((IkReal(-1.00000000000000))*(sj0)*(x624)*(x641)))+(((IkReal(-1.00000000000000))*(px)*(x625)*(x652)))+(((IkReal(-0.0665000000000000))*(x644)))+(((IkReal(0.700000000000000))*(x626)))+(((IkReal(-2.00000000000000))*(x626)*(x649)))+(((r00)*(x634))));
evalcond[7]=((((x623)*(x631)*(x636)))+(((x625)*(x636)*(x649)))+(((IkReal(-1.00000000000000))*(x626)*(x652)))+(((x631)*(x658)))+(((IkReal(-0.296184679851750))*(x653)))+(((IkReal(-0.131525000000000))*(r01)*(x631)))+(((r02)*(x638)))+(((IkReal(0.131525000000000))*(x639)))+(((IkReal(-0.131525000000000))*(cj1)*(x632)))+(((x640)*(x642)))+(((x624)*(x627)))+(((pz)*(sj0)*(x629)*(x648)))+(((IkReal(-1.00000000000000))*(r02)*(x621)*(x633)))+(((IkReal(-1.00000000000000))*(py)*(r02)*(x642)))+(((IkReal(-1.00000000000000))*(x631)*(x637)))+(((IkReal(0.190000000000000))*(x622)*(x630)))+(((IkReal(-1.00000000000000))*(pp)*(x632)*(x635)))+(((IkReal(-1.00000000000000))*(x623)*(x624)))+(((IkReal(-0.190000000000000))*(x622)*(x641)))+(((IkReal(0.700000000000000))*(cj0)*(x630)))+(((IkReal(-1.00000000000000))*(px)*(x630)*(x633)))+(((IkReal(0.0352506812605000))*(x654)))+(((IkReal(-1.00000000000000))*(cj0)*(x628)*(x640)))+(((IkReal(2.00000000000000))*(cj1)*(x620)*(x632)))+(((IkReal(-1.00000000000000))*(x625)*(x634)))+(((cj0)*(cj1)*(x626)*(x636)))+(((py)*(x625)*(x628))));
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  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}
}
}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x659=((cj0)*(sj1));
IkReal x660=((IkReal(13.0000000000000))*(r00));
IkReal x661=((cj1)*(pz));
IkReal x662=((sj0)*(sj1));
IkReal x663=((IkReal(13.0000000000000))*(sj3));
IkReal x664=((cj1)*(r02));
IkReal x665=((IkReal(70.0000000000000))*(cj4));
IkReal x666=((IkReal(13.0000000000000))*(cj3));
IkReal x667=((IkReal(200.000000000000))*(cj4)*(sj3));
IkReal x668=((IkReal(200.000000000000))*(cj3)*(cj4));
if( IKabs(((gconst2)*(((((r01)*(x662)*(x666)))+(((IkReal(110.000000000000))*(r00)*(x659)))+(((IkReal(110.000000000000))*(r01)*(x662)))+(((x661)*(x667)))+(((IkReal(110.000000000000))*(x664)))+(((IkReal(-1.00000000000000))*(sj3)*(x665)))+(((cj3)*(x659)*(x660)))+(((x664)*(x666)))+(((px)*(x659)*(x667)))+(((py)*(x662)*(x667))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((r01)*(x662)*(x663)))+(((cj3)*(x665)))+(((sj3)*(x659)*(x660)))+(((IkReal(-1.00000000000000))*(px)*(x659)*(x668)))+(((x663)*(x664)))+(((IkReal(-1.00000000000000))*(x661)*(x668)))+(((IkReal(-1.00000000000000))*(py)*(x662)*(x668))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst2)*(((((r01)*(x662)*(x666)))+(((IkReal(110.000000000000))*(r00)*(x659)))+(((IkReal(110.000000000000))*(r01)*(x662)))+(((x661)*(x667)))+(((IkReal(110.000000000000))*(x664)))+(((IkReal(-1.00000000000000))*(sj3)*(x665)))+(((cj3)*(x659)*(x660)))+(((x664)*(x666)))+(((px)*(x659)*(x667)))+(((py)*(x662)*(x667)))))), ((gconst2)*(((((r01)*(x662)*(x663)))+(((cj3)*(x665)))+(((sj3)*(x659)*(x660)))+(((IkReal(-1.00000000000000))*(px)*(x659)*(x668)))+(((x663)*(x664)))+(((IkReal(-1.00000000000000))*(x661)*(x668)))+(((IkReal(-1.00000000000000))*(py)*(x662)*(x668)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[8];
IkReal x669=IKsin(j2);
IkReal x670=IKcos(j2);
IkReal x671=(py)*(py);
IkReal x672=(px)*(px);
IkReal x673=(pz)*(pz);
IkReal x674=((sj0)*(sj1));
IkReal x675=((py)*(r00));
IkReal x676=((IkReal(0.190000000000000))*(cj1));
IkReal x677=((cj0)*(r02));
IkReal x678=((py)*(r01));
IkReal x679=((r01)*(sj1));
IkReal x680=((IkReal(0.190000000000000))*(px));
IkReal x681=((cj1)*(r02));
IkReal x682=((pz)*(r00));
IkReal x683=((cj1)*(sj0));
IkReal x684=((cj0)*(r00));
IkReal x685=((IkReal(2.00000000000000))*(sj1));
IkReal x686=((cj0)*(pz));
IkReal x687=((IkReal(1.00000000000000))*(r01));
IkReal x688=((cj1)*(px));
IkReal x689=((IkReal(2.00000000000000))*(px));
IkReal x690=((pp)*(sj1));
IkReal x691=((r02)*(sj1));
IkReal x692=((px)*(r02));
IkReal x693=((IkReal(0.700000000000000))*(pz));
IkReal x694=((cj0)*(px));
IkReal x695=((r00)*(sj0));
IkReal x696=((IkReal(1.00000000000000))*(px));
IkReal x697=((cj0)*(r01));
IkReal x698=((IkReal(1.00000000000000))*(cj1));
IkReal x699=((IkReal(1.00000000000000))*(py));
IkReal x700=((IkReal(0.700000000000000))*(px));
IkReal x701=((IkReal(0.190000000000000))*(sj1));
IkReal x702=((IkReal(0.298275000000000))*(sj3));
IkReal x703=((cj1)*(pz));
IkReal x704=((IkReal(0.0950000000000000))*(r01));
IkReal x705=((IkReal(0.306725000000000))*(cj3)*(cj4));
IkReal x706=((pz)*(x685));
IkReal x707=((IkReal(0.550000000000000))*(x670));
IkReal x708=((cj4)*(x670));
IkReal x709=((IkReal(0.550000000000000))*(x669));
IkReal x710=((IkReal(2.00000000000000))*(py)*(pz));
IkReal x711=((IkReal(0.0650000000000000))*(cj3)*(sj4));
IkReal x712=((IkReal(0.0650000000000000))*(x669));
IkReal x713=((cj4)*(x669));
IkReal x714=((IkReal(2.00000000000000))*(r01)*(x671));
IkReal x715=((IkReal(0.0650000000000000))*(sj3)*(x670));
evalcond[0]=((((r01)*(x674)))+(((sj1)*(x684)))+(((cj3)*(x713)))+(x681)+(((sj3)*(x708))));
evalcond[1]=((IkReal(-0.350000000000000))+(((sj1)*(x694)))+(((IkReal(-0.0650000000000000))*(cj3)*(x670)))+(((sj3)*(x712)))+(x703)+(((py)*(x674)))+(((IkReal(-1.00000000000000))*(x707))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x683)*(x687)))+(x691)+(((IkReal(-1.00000000000000))*(x684)*(x698)))+(((cj3)*(x708)))+(((IkReal(-1.00000000000000))*(sj3)*(x713))));
evalcond[3]=((((IkReal(-1.00000000000000))*(cj0)*(x688)))+(((pz)*(sj1)))+(x709)+(((cj3)*(x712)))+(x715)+(((IkReal(-1.00000000000000))*(x683)*(x699))));
evalcond[4]=((((x679)*(x686)))+(((IkReal(-0.0950000000000000))*(x691)))+(((IkReal(0.0950000000000000))*(cj1)*(x684)))+(((x674)*(x692)))+(((IkReal(-1.00000000000000))*(x674)*(x682)))+(((IkReal(-1.00000000000000))*(sj1)*(x677)*(x699)))+(((sj4)*(x709)))+(((x683)*(x704)))+(((sj4)*(x715)))+(((cj1)*(x675)))+(((IkReal(-1.00000000000000))*(x687)*(x688)))+(((x669)*(x711))));
evalcond[5]=((((sj1)*(x675)))+(((x670)*(x711)))+(((IkReal(-1.00000000000000))*(cj1)*(x686)*(x687)))+(((IkReal(0.0950000000000000))*(sj1)*(x684)))+(((IkReal(-1.00000000000000))*(x679)*(x696)))+(((x674)*(x704)))+(((IkReal(-0.350000000000000))*(x695)))+(((x682)*(x683)))+(((IkReal(-1.00000000000000))*(sj0)*(x681)*(x696)))+(((cj1)*(py)*(x677)))+(((IkReal(0.350000000000000))*(x697)))+(((IkReal(-1.00000000000000))*(sj3)*(sj4)*(x712)))+(((IkReal(0.0950000000000000))*(x681)))+(((sj4)*(x707))));
evalcond[6]=((((x684)*(x690)))+(((py)*(x676)*(x677)))+(((r00)*(x700)))+(((IkReal(-1.00000000000000))*(x674)*(x675)*(x689)))+(((IkReal(-1.00000000000000))*(x702)*(x708)))+(((x669)*(x705)))+(((IkReal(-0.113475000000000))*(r01)*(x674)))+(((IkReal(-2.00000000000000))*(x673)*(x681)))+(((IkReal(-1.00000000000000))*(r02)*(x674)*(x710)))+(((IkReal(-1.00000000000000))*(x674)*(x714)))+(((sj0)*(x676)*(x682)))+(((IkReal(-1.00000000000000))*(sj0)*(x676)*(x692)))+(((IkReal(-0.113475000000000))*(x681)))+(((IkReal(-2.00000000000000))*(x678)*(x703)))+(((IkReal(-2.00000000000000))*(x682)*(x688)))+(((IkReal(0.0665000000000000))*(x697)))+(((IkReal(-1.00000000000000))*(r01)*(x676)*(x686)))+(((IkReal(0.0715000000000000))*(x713)))+(((IkReal(-1.00000000000000))*(x672)*(x684)*(x685)))+(((r02)*(x693)))+(((pp)*(x681)))+(((IkReal(-1.00000000000000))*(px)*(x677)*(x706)))+(((IkReal(0.700000000000000))*(x678)))+(((IkReal(-1.00000000000000))*(x679)*(x680)))+(((x675)*(x701)))+(((IkReal(-1.00000000000000))*(x678)*(x685)*(x694)))+(((IkReal(-0.113475000000000))*(sj1)*(x684)))+(((pp)*(r01)*(x674)))+(((IkReal(-0.0665000000000000))*(x695))));
evalcond[7]=((((IkReal(-1.00000000000000))*(r02)*(x674)*(x680)))+(((IkReal(-1.00000000000000))*(x678)*(x706)))+(((x670)*(x705)))+(((IkReal(0.190000000000000))*(x674)*(x682)))+(((px)*(r01)*(x676)))+(((IkReal(2.00000000000000))*(cj1)*(x672)*(x684)))+(((IkReal(0.0715000000000000))*(x708)))+(((x683)*(x714)))+(((IkReal(0.700000000000000))*(cj0)*(x682)))+(((IkReal(-0.190000000000000))*(x679)*(x686)))+(((r01)*(sj0)*(x693)))+(((x702)*(x713)))+(((IkReal(-1.00000000000000))*(x675)*(x676)))+(((py)*(x677)*(x701)))+(((sj0)*(x681)*(x710)))+(((IkReal(-1.00000000000000))*(r02)*(x673)*(x685)))+(((IkReal(-1.00000000000000))*(px)*(x682)*(x685)))+(((IkReal(-1.00000000000000))*(x677)*(x700)))+(((IkReal(-0.131525000000000))*(r01)*(x683)))+(((IkReal(-0.700000000000000))*(py)*(r02)*(sj0)))+(((IkReal(2.00000000000000))*(pz)*(x677)*(x688)))+(((x675)*(x683)*(x689)))+(((IkReal(-1.00000000000000))*(pp)*(x683)*(x687)))+(((IkReal(-0.131525000000000))*(cj1)*(x684)))+(((r02)*(x690)))+(((IkReal(-1.00000000000000))*(pp)*(x684)*(x698)))+(((IkReal(2.00000000000000))*(cj0)*(x678)*(x688)))+(((IkReal(0.131525000000000))*(x691))));
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  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x716=((IkReal(1.00000000000000))*(sj1));
IkReal x717=((IkReal(1.00000000000000))*(sj3));
IkReal x718=((cj0)*(r00));
IkReal x719=((r01)*(sj0));
IkReal x720=((cj3)*(r02));
IkReal x721=((cj3)*(x719));
IkReal x722=((cj1)*(x718));
if( IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(cj3)*(x716)*(x718)))+(((IkReal(-1.00000000000000))*(x716)*(x721)))+(((r02)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x717)*(x719)))+(((IkReal(-1.00000000000000))*(cj1)*(x720)))+(((IkReal(-1.00000000000000))*(x717)*(x722))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((IkReal(-1.00000000000000))*(sj3)*(x716)*(x718)))+(((cj3)*(x722)))+(((cj1)*(x721)))+(((IkReal(-1.00000000000000))*(sj3)*(x716)*(x719)))+(((IkReal(-1.00000000000000))*(x716)*(x720)))+(((IkReal(-1.00000000000000))*(cj1)*(r02)*(x717))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst1)*(((((IkReal(-1.00000000000000))*(cj3)*(x716)*(x718)))+(((IkReal(-1.00000000000000))*(x716)*(x721)))+(((r02)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x717)*(x719)))+(((IkReal(-1.00000000000000))*(cj1)*(x720)))+(((IkReal(-1.00000000000000))*(x717)*(x722)))))), ((gconst1)*(((((IkReal(-1.00000000000000))*(sj3)*(x716)*(x718)))+(((cj3)*(x722)))+(((cj1)*(x721)))+(((IkReal(-1.00000000000000))*(sj3)*(x716)*(x719)))+(((IkReal(-1.00000000000000))*(x716)*(x720)))+(((IkReal(-1.00000000000000))*(cj1)*(r02)*(x717)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[8];
IkReal x723=IKsin(j2);
IkReal x724=IKcos(j2);
IkReal x725=(py)*(py);
IkReal x726=(px)*(px);
IkReal x727=(pz)*(pz);
IkReal x728=((sj0)*(sj1));
IkReal x729=((py)*(r00));
IkReal x730=((IkReal(0.190000000000000))*(cj1));
IkReal x731=((cj0)*(r02));
IkReal x732=((py)*(r01));
IkReal x733=((r01)*(sj1));
IkReal x734=((IkReal(0.190000000000000))*(px));
IkReal x735=((cj1)*(r02));
IkReal x736=((pz)*(r00));
IkReal x737=((cj1)*(sj0));
IkReal x738=((cj0)*(r00));
IkReal x739=((IkReal(2.00000000000000))*(sj1));
IkReal x740=((cj0)*(pz));
IkReal x741=((IkReal(1.00000000000000))*(r01));
IkReal x742=((cj1)*(px));
IkReal x743=((IkReal(2.00000000000000))*(px));
IkReal x744=((pp)*(sj1));
IkReal x745=((r02)*(sj1));
IkReal x746=((px)*(r02));
IkReal x747=((IkReal(0.700000000000000))*(pz));
IkReal x748=((cj0)*(px));
IkReal x749=((r00)*(sj0));
IkReal x750=((IkReal(1.00000000000000))*(px));
IkReal x751=((cj0)*(r01));
IkReal x752=((IkReal(1.00000000000000))*(cj1));
IkReal x753=((IkReal(1.00000000000000))*(py));
IkReal x754=((IkReal(0.700000000000000))*(px));
IkReal x755=((IkReal(0.190000000000000))*(sj1));
IkReal x756=((IkReal(0.298275000000000))*(sj3));
IkReal x757=((cj1)*(pz));
IkReal x758=((IkReal(0.0950000000000000))*(r01));
IkReal x759=((IkReal(0.306725000000000))*(cj3)*(cj4));
IkReal x760=((pz)*(x739));
IkReal x761=((IkReal(0.550000000000000))*(x724));
IkReal x762=((cj4)*(x724));
IkReal x763=((IkReal(0.550000000000000))*(x723));
IkReal x764=((IkReal(2.00000000000000))*(py)*(pz));
IkReal x765=((IkReal(0.0650000000000000))*(cj3)*(sj4));
IkReal x766=((IkReal(0.0650000000000000))*(x723));
IkReal x767=((cj4)*(x723));
IkReal x768=((IkReal(2.00000000000000))*(r01)*(x725));
IkReal x769=((IkReal(0.0650000000000000))*(sj3)*(x724));
evalcond[0]=((((sj1)*(x738)))+(x735)+(((cj3)*(x767)))+(((r01)*(x728)))+(((sj3)*(x762))));
evalcond[1]=((IkReal(-0.350000000000000))+(((sj1)*(x748)))+(((py)*(x728)))+(x757)+(((sj3)*(x766)))+(((IkReal(-0.0650000000000000))*(cj3)*(x724)))+(((IkReal(-1.00000000000000))*(x761))));
evalcond[2]=((((cj3)*(x762)))+(((IkReal(-1.00000000000000))*(sj3)*(x767)))+(((IkReal(-1.00000000000000))*(x738)*(x752)))+(x745)+(((IkReal(-1.00000000000000))*(x737)*(x741))));
evalcond[3]=((((IkReal(-1.00000000000000))*(cj0)*(x742)))+(((pz)*(sj1)))+(x769)+(x763)+(((IkReal(-1.00000000000000))*(x737)*(x753)))+(((cj3)*(x766))));
evalcond[4]=((((sj4)*(x769)))+(((IkReal(-1.00000000000000))*(sj1)*(x731)*(x753)))+(((x728)*(x746)))+(((x737)*(x758)))+(((x723)*(x765)))+(((IkReal(-1.00000000000000))*(x741)*(x742)))+(((cj1)*(x729)))+(((IkReal(0.0950000000000000))*(cj1)*(x738)))+(((IkReal(-1.00000000000000))*(x728)*(x736)))+(((sj4)*(x763)))+(((x733)*(x740)))+(((IkReal(-0.0950000000000000))*(x745))));
evalcond[5]=((((cj1)*(py)*(x731)))+(((IkReal(-0.350000000000000))*(x749)))+(((IkReal(-1.00000000000000))*(sj0)*(x735)*(x750)))+(((IkReal(0.0950000000000000))*(x735)))+(((x736)*(x737)))+(((IkReal(0.350000000000000))*(x751)))+(((sj1)*(x729)))+(((x724)*(x765)))+(((IkReal(-1.00000000000000))*(cj1)*(x740)*(x741)))+(((IkReal(0.0950000000000000))*(sj1)*(x738)))+(((IkReal(-1.00000000000000))*(x733)*(x750)))+(((x728)*(x758)))+(((IkReal(-1.00000000000000))*(sj3)*(sj4)*(x766)))+(((sj4)*(x761))));
evalcond[6]=((((x723)*(x759)))+(((sj0)*(x730)*(x736)))+(((pp)*(x735)))+(((IkReal(-1.00000000000000))*(r02)*(x728)*(x764)))+(((IkReal(-1.00000000000000))*(px)*(x731)*(x760)))+(((IkReal(-1.00000000000000))*(x728)*(x768)))+(((IkReal(-1.00000000000000))*(r01)*(x730)*(x740)))+(((IkReal(-1.00000000000000))*(sj0)*(x730)*(x746)))+(((IkReal(0.0715000000000000))*(x767)))+(((pp)*(r01)*(x728)))+(((py)*(x730)*(x731)))+(((IkReal(-2.00000000000000))*(x727)*(x735)))+(((IkReal(-1.00000000000000))*(x756)*(x762)))+(((r02)*(x747)))+(((IkReal(-1.00000000000000))*(x733)*(x734)))+(((IkReal(-1.00000000000000))*(x732)*(x739)*(x748)))+(((IkReal(-1.00000000000000))*(x726)*(x738)*(x739)))+(((IkReal(-0.113475000000000))*(x735)))+(((IkReal(-2.00000000000000))*(x732)*(x757)))+(((r00)*(x754)))+(((IkReal(-1.00000000000000))*(x728)*(x729)*(x743)))+(((IkReal(-0.0665000000000000))*(x749)))+(((IkReal(0.0665000000000000))*(x751)))+(((IkReal(-2.00000000000000))*(x736)*(x742)))+(((IkReal(-0.113475000000000))*(r01)*(x728)))+(((x738)*(x744)))+(((x729)*(x755)))+(((IkReal(-0.113475000000000))*(sj1)*(x738)))+(((IkReal(0.700000000000000))*(x732))));
evalcond[7]=((((IkReal(2.00000000000000))*(pz)*(x731)*(x742)))+(((sj0)*(x735)*(x764)))+(((IkReal(0.190000000000000))*(x728)*(x736)))+(((x756)*(x767)))+(((IkReal(-1.00000000000000))*(pp)*(x738)*(x752)))+(((py)*(x731)*(x755)))+(((IkReal(-0.131525000000000))*(r01)*(x737)))+(((IkReal(-0.190000000000000))*(x733)*(x740)))+(((IkReal(2.00000000000000))*(cj0)*(x732)*(x742)))+(((IkReal(-0.131525000000000))*(cj1)*(x738)))+(((IkReal(0.700000000000000))*(cj0)*(x736)))+(((IkReal(-1.00000000000000))*(x732)*(x760)))+(((IkReal(-1.00000000000000))*(pp)*(x737)*(x741)))+(((IkReal(-1.00000000000000))*(x731)*(x754)))+(((r01)*(sj0)*(x747)))+(((IkReal(-1.00000000000000))*(r02)*(x728)*(x734)))+(((IkReal(-1.00000000000000))*(r02)*(x727)*(x739)))+(((IkReal(-1.00000000000000))*(x729)*(x730)))+(((px)*(r01)*(x730)))+(((IkReal(-1.00000000000000))*(px)*(x736)*(x739)))+(((IkReal(0.131525000000000))*(x745)))+(((r02)*(x744)))+(((IkReal(-0.700000000000000))*(py)*(r02)*(sj0)))+(((x737)*(x768)))+(((IkReal(0.0715000000000000))*(x762)))+(((x729)*(x737)*(x743)))+(((IkReal(2.00000000000000))*(cj1)*(x726)*(x738)))+(((x724)*(x759))));
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  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}
}
}

}

}

} else
{
{
IkReal j3array[1], cj3array[1], sj3array[1];
bool j3valid[1]={false};
_nj3 = 1;
IkReal x770=((IkReal(20.0000000000000))*(r02));
IkReal x771=((IkReal(7.00000000000000))*(sj1));
IkReal x772=((r01)*(sj0));
IkReal x773=((cj0)*(r00));
IkReal x774=((IkReal(20.0000000000000))*(pz));
IkReal x775=((IkReal(7.00000000000000))*(cj1));
if( IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(pz)*(x770)))+(((r02)*(x775)))+(((x771)*(x773)))+(((IkReal(-20.0000000000000))*(py)*(r01)))+(((x771)*(x772)))+(((IkReal(-1.90000000000000))*(cj0)*(r01)))+(((IkReal(-20.0000000000000))*(px)*(r00)))+(((IkReal(1.90000000000000))*(r00)*(sj0))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst0)*(((((IkReal(-1.00000000000000))*(py)*(sj0)*(x770)))+(((r02)*(x771)))+(((x772)*(x774)))+(((x773)*(x774)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x770)))+(((IkReal(-1.00000000000000))*(x772)*(x775)))+(((IkReal(-1.00000000000000))*(x773)*(x775)))+(((IkReal(-1.30000000000000))*(cj4))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j3array[0]=IKatan2(((gconst0)*(((((IkReal(-1.00000000000000))*(pz)*(x770)))+(((r02)*(x775)))+(((x771)*(x773)))+(((IkReal(-20.0000000000000))*(py)*(r01)))+(((x771)*(x772)))+(((IkReal(-1.90000000000000))*(cj0)*(r01)))+(((IkReal(-20.0000000000000))*(px)*(r00)))+(((IkReal(1.90000000000000))*(r00)*(sj0)))))), ((gconst0)*(((((IkReal(-1.00000000000000))*(py)*(sj0)*(x770)))+(((r02)*(x771)))+(((x772)*(x774)))+(((x773)*(x774)))+(((IkReal(-1.00000000000000))*(cj0)*(px)*(x770)))+(((IkReal(-1.00000000000000))*(x772)*(x775)))+(((IkReal(-1.00000000000000))*(x773)*(x775)))+(((IkReal(-1.30000000000000))*(cj4)))))));
sj3array[0]=IKsin(j3array[0]);
cj3array[0]=IKcos(j3array[0]);
if( j3array[0] > IKPI )
{
    j3array[0]-=IK2PI;
}
else if( j3array[0] < -IKPI )
{    j3array[0]+=IK2PI;
}
j3valid[0] = true;
for(int ij3 = 0; ij3 < 1; ++ij3)
{
if( !j3valid[ij3] )
{
    continue;
}
_ij3[0] = ij3; _ij3[1] = -1;
for(int iij3 = ij3+1; iij3 < 1; ++iij3)
{
if( j3valid[iij3] && IKabs(cj3array[ij3]-cj3array[iij3]) < IKFAST_SOLUTION_THRESH && IKabs(sj3array[ij3]-sj3array[iij3]) < IKFAST_SOLUTION_THRESH )
{
    j3valid[iij3]=false; _ij3[1] = iij3; break; 
}
}
j3 = j3array[ij3]; cj3 = cj3array[ij3]; sj3 = sj3array[ij3];
{
IkReal evalcond[4];
IkReal x776=IKcos(j3);
IkReal x777=((r01)*(sj1));
IkReal x778=((IkReal(0.350000000000000))*(sj0));
IkReal x779=((cj0)*(r00));
IkReal x780=((IkReal(0.350000000000000))*(cj1));
IkReal x781=((px)*(r02));
IkReal x782=((IkReal(1.00000000000000))*(cj0));
IkReal x783=((IkReal(0.190000000000000))*(px));
IkReal x784=((cj0)*(r01));
IkReal x785=((r00)*(sj0));
IkReal x786=((IkReal(0.700000000000000))*(sj1));
IkReal x787=((IkReal(0.550000000000000))*(cj4));
IkReal x788=((IkReal(0.700000000000000))*(cj1));
IkReal x789=((py)*(sj0));
IkReal x790=((IkReal(1.00000000000000))*(r02));
IkReal x791=((cj0)*(py));
IkReal x792=((IkReal(2.00000000000000))*(px));
IkReal x793=((pz)*(sj0));
IkReal x794=((pz)*(r02));
IkReal x795=((IkReal(0.350000000000000))*(sj1));
IkReal x796=((py)*(r01));
IkReal x797=((IkReal(0.0715000000000000))*(x776));
evalcond[0]=((IkReal(0.175200000000000))+(((IkReal(-0.190000000000000))*(x791)))+(((pz)*(x788)))+(((IkReal(-1.00000000000000))*(pp)))+(x797)+(((cj0)*(px)*(x786)))+(((sj0)*(x783)))+(((x786)*(x789))));
evalcond[1]=((((r02)*(x780)))+(((x779)*(x795)))+(((x777)*(x778)))+(((IkReal(-1.00000000000000))*(x787)*(IKsin(j3))))+(((IkReal(0.0950000000000000))*(x785)))+(((IkReal(-1.00000000000000))*(pz)*(x790)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(-0.0950000000000000))*(x784)))+(((IkReal(-1.00000000000000))*(x796))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x789)*(x790)))+(((r02)*(x795)))+(((IkReal(-1.00000000000000))*(x781)*(x782)))+(((IkReal(-1.00000000000000))*(x779)*(x780)))+(((IkReal(-0.0650000000000000))*(cj4)))+(((IkReal(-1.00000000000000))*(x776)*(x787)))+(((pz)*(x779)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x778)))+(((r01)*(x793))));
evalcond[3]=((((sj0)*(x781)*(x788)))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x782)))+(((IkReal(0.306725000000000))*(sj4)))+(((IkReal(-1.00000000000000))*(r01)*(x789)*(x792)))+(((IkReal(0.700000000000000))*(px)*(x777)))+(((py)*(x779)*(x792)))+(((IkReal(2.00000000000000))*(x784)*((py)*(py))))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x786)))+(((IkReal(-1.00000000000000))*(px)*(x785)*(x792)))+(((pz)*(x784)*(x788)))+(((pp)*(x785)))+(((sj4)*(x797)))+(((r00)*(x783)))+(((IkReal(0.113475000000000))*(x785)))+(((IkReal(-1.00000000000000))*(pz)*(x785)*(x788)))+(((IkReal(-0.0665000000000000))*(sj1)*(x779)))+(((IkReal(-1.00000000000000))*(r02)*(x788)*(x791)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((IkReal(-0.113475000000000))*(x784)))+(((IkReal(-2.00000000000000))*(x781)*(x793)))+(((IkReal(0.190000000000000))*(x794)))+(((IkReal(-0.0665000000000000))*(sj0)*(x777)))+(((IkReal(0.190000000000000))*(x796)))+(((IkReal(2.00000000000000))*(x791)*(x794))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
IkReal dummyeval[1];
IkReal gconst1;
gconst1=IKsign(((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3))))));
dummyeval[0]=((((cj4)*((cj3)*(cj3))))+(((cj4)*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal dummyeval[1];
IkReal gconst2;
IkReal x798=((IkReal(13.0000000000000))*(cj4));
gconst2=IKsign(((((IkReal(-1.00000000000000))*(x798)*((sj3)*(sj3))))+(((IkReal(-1.00000000000000))*(x798)*((cj3)*(cj3))))+(((IkReal(-110.000000000000))*(cj3)*(cj4)))));
IkReal x799=((IkReal(1.00000000000000))*(cj4));
dummyeval[0]=((((IkReal(-1.00000000000000))*(x799)*((cj3)*(cj3))))+(((IkReal(-8.46153846153846))*(cj3)*(cj4)))+(((IkReal(-1.00000000000000))*(x799)*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal evalcond[11];
IkReal x800=((IkReal(0.0715000000000000))*(cj3));
IkReal x801=(py)*(py);
IkReal x802=(px)*(px);
IkReal x803=(pz)*(pz);
IkReal x804=((r01)*(sj0));
IkReal x805=((px)*(sj0));
IkReal x806=((cj1)*(r00));
IkReal x807=((IkReal(0.350000000000000))*(cj0));
IkReal x808=((IkReal(0.113475000000000))*(sj1));
IkReal x809=((r01)*(sj1));
IkReal x810=((IkReal(2.00000000000000))*(pz));
IkReal x811=((IkReal(0.190000000000000))*(px));
IkReal x812=((py)*(sj0));
IkReal x813=((IkReal(1.00000000000000))*(r02));
IkReal x814=((IkReal(0.190000000000000))*(py));
IkReal x815=((cj0)*(sj1));
IkReal x816=((px)*(r02));
IkReal x817=((px)*(r00));
IkReal x818=((r02)*(sj1));
IkReal x819=((IkReal(2.00000000000000))*(py));
IkReal x820=((pp)*(r00));
IkReal x821=((cj0)*(r01));
IkReal x822=((IkReal(0.700000000000000))*(sj1));
IkReal x823=((IkReal(0.190000000000000))*(pz));
IkReal x824=((cj1)*(r01));
IkReal x825=((cj0)*(r00));
IkReal x826=((IkReal(0.700000000000000))*(pz));
IkReal x827=((cj0)*(px));
IkReal x828=((r00)*(sj0));
IkReal x829=((cj1)*(r02));
IkReal x830=((IkReal(1.00000000000000))*(pp));
IkReal x831=((IkReal(1.00000000000000))*(py));
IkReal x832=((IkReal(0.700000000000000))*(cj0));
IkReal x833=((r00)*(sj1));
IkReal x834=((IkReal(0.700000000000000))*(px));
IkReal x835=((cj0)*(cj1));
IkReal x836=((cj1)*(x830));
IkReal x837=((IkReal(2.00000000000000))*(x803));
IkReal x838=((IkReal(2.00000000000000))*(x801));
IkReal x839=((IkReal(2.00000000000000))*(r00)*(x802));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.57079632679490))+(j4)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-0.0950000000000000))+(x805)+(((IkReal(-1.00000000000000))*(cj0)*(x831))));
evalcond[2]=((IkReal(1.00000000000000))+(x828)+(((IkReal(-1.00000000000000))*(x821))));
evalcond[3]=((x829)+(((r00)*(x815)))+(((sj1)*(x804))));
evalcond[4]=((((IkReal(-1.00000000000000))*(cj0)*(x806)))+(((IkReal(-1.00000000000000))*(cj1)*(x804)))+(x818));
evalcond[5]=((IkReal(0.175200000000000))+(x800)+(((IkReal(-1.00000000000000))*(x830)))+(((IkReal(-1.00000000000000))*(cj0)*(x814)))+(((x815)*(x834)))+(((cj1)*(x826)))+(((x812)*(x822)))+(((IkReal(0.190000000000000))*(x805))));
evalcond[6]=((((IkReal(-1.00000000000000))*(pz)*(x813)))+(((IkReal(0.350000000000000))*(sj1)*(x804)))+(((IkReal(-1.00000000000000))*(x817)))+(((IkReal(-1.00000000000000))*(r01)*(x831)))+(((IkReal(0.350000000000000))*(x829)))+(((IkReal(-0.0950000000000000))*(x821)))+(((IkReal(0.0950000000000000))*(x828)))+(((x807)*(x833))));
evalcond[7]=((((IkReal(-1.00000000000000))*(x806)*(x807)))+(((IkReal(-1.00000000000000))*(x813)*(x827)))+(((IkReal(-0.350000000000000))*(cj1)*(x804)))+(((IkReal(-1.00000000000000))*(x812)*(x813)))+(((IkReal(0.350000000000000))*(x818)))+(((pz)*(x825)))+(((pz)*(x804))));
evalcond[8]=((IkReal(0.306725000000000))+(((cj1)*(x821)*(x826)))+(((r02)*(x823)))+(x800)+(((IkReal(-1.00000000000000))*(py)*(x829)*(x832)))+(((IkReal(-1.00000000000000))*(r02)*(x805)*(x810)))+(((cj0)*(py)*(r02)*(x810)))+(((IkReal(0.700000000000000))*(x805)*(x829)))+(((cj0)*(x817)*(x819)))+(((r00)*(x811)))+(((r01)*(x814)))+(((IkReal(-0.0665000000000000))*(r00)*(x815)))+(((sj0)*(x820)))+(((IkReal(0.113475000000000))*(x828)))+(((IkReal(-0.0665000000000000))*(x829)))+(((x809)*(x834)))+(((IkReal(-1.00000000000000))*(x821)*(x830)))+(((IkReal(-1.00000000000000))*(sj0)*(x806)*(x826)))+(((IkReal(-2.00000000000000))*(x802)*(x828)))+(((IkReal(-0.113475000000000))*(x821)))+(((x821)*(x838)))+(((IkReal(-0.0665000000000000))*(sj1)*(x804)))+(((IkReal(-1.00000000000000))*(px)*(x804)*(x819)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x822))));
evalcond[9]=((((pp)*(sj1)*(x804)))+(((sj0)*(x806)*(x823)))+(((r02)*(x826)))+(((IkReal(-1.00000000000000))*(cj1)*(x821)*(x823)))+(((IkReal(-1.00000000000000))*(x810)*(x812)*(x818)))+(((IkReal(-1.00000000000000))*(x815)*(x839)))+(((IkReal(-1.00000000000000))*(x829)*(x837)))+(((pp)*(x829)))+(((x815)*(x820)))+(((IkReal(0.0665000000000000))*(x821)))+(((IkReal(-0.190000000000000))*(x805)*(x829)))+(((IkReal(-1.00000000000000))*(x810)*(x815)*(x816)))+(((cj0)*(x814)*(x829)))+(((IkReal(-1.00000000000000))*(x809)*(x819)*(x827)))+(((IkReal(-1.00000000000000))*(x804)*(x808)))+(((IkReal(0.700000000000000))*(x817)))+(((IkReal(0.700000000000000))*(py)*(r01)))+(((x814)*(x833)))+(((IkReal(-0.0665000000000000))*(x828)))+(((IkReal(-1.00000000000000))*(sj1)*(x804)*(x838)))+(((IkReal(-1.00000000000000))*(py)*(x810)*(x824)))+(((IkReal(-1.00000000000000))*(x809)*(x811)))+(((IkReal(-1.00000000000000))*(x808)*(x825)))+(((IkReal(-1.00000000000000))*(x805)*(x819)*(x833)))+(((IkReal(-0.113475000000000))*(x829)))+(((IkReal(-1.00000000000000))*(px)*(x806)*(x810))));
evalcond[10]=((((x825)*(x826)))+(((IkReal(-1.00000000000000))*(sj1)*(x810)*(x817)))+(((IkReal(-0.131525000000000))*(cj0)*(x806)))+(((pp)*(x818)))+(((cj1)*(px)*(x819)*(x821)))+(((x805)*(x806)*(x819)))+(((sj1)*(x823)*(x828)))+(((x811)*(x824)))+(((IkReal(0.131525000000000))*(x818)))+(((IkReal(-0.190000000000000))*(x805)*(x818)))+(((IkReal(-0.700000000000000))*(r02)*(x812)))+(((IkReal(-1.00000000000000))*(x804)*(x836)))+(((IkReal(-1.00000000000000))*(x806)*(x814)))+(((IkReal(-1.00000000000000))*(py)*(x809)*(x810)))+(((cj1)*(x804)*(x838)))+(((IkReal(-0.131525000000000))*(cj1)*(x804)))+(((x810)*(x816)*(x835)))+(((IkReal(-1.00000000000000))*(x818)*(x837)))+(((r02)*(x814)*(x815)))+(((IkReal(-1.00000000000000))*(x816)*(x832)))+(((IkReal(2.00000000000000))*(cj0)*(x802)*(x806)))+(((x804)*(x826)))+(((IkReal(-1.00000000000000))*(cj0)*(x806)*(x830)))+(((IkReal(-1.00000000000000))*(cj0)*(x809)*(x823)))+(((x810)*(x812)*(x829))));
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  )
{
{
IkReal dummyeval[1];
IkReal gconst3;
gconst3=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal dummyeval[1];
IkReal gconst4;
gconst4=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x840=((cj1)*(cj3));
IkReal x841=((sj0)*(sj1));
IkReal x842=((IkReal(22000.0000000000))*(pz));
IkReal x843=((IkReal(2600.00000000000))*(pz));
IkReal x844=((r02)*(sj1));
IkReal x845=((cj1)*(sj3));
IkReal x846=((cj1)*(r00));
IkReal x847=((r01)*(sj0));
IkReal x848=((cj0)*(sj1));
IkReal x849=((IkReal(2600.00000000000))*(px));
IkReal x850=((IkReal(22000.0000000000))*(px));
IkReal x851=((IkReal(22000.0000000000))*(py));
IkReal x852=((IkReal(2600.00000000000))*(py));
IkReal x853=((IkReal(247.000000000000))*(cj0)*(r00));
IkReal x854=((sj3)*(x852));
IkReal x855=((IkReal(2600.00000000000))*(sj3)*(x848));
if( IKabs(((gconst4)*(((((r02)*(x841)*(x850)))+(((x843)*(x845)))+(((r01)*(x842)*(x848)))+(((IkReal(-1.00000000000000))*(r01)*(x840)*(x849)))+(((IkReal(-247.000000000000))*(cj3)*(x844)))+(((IkReal(-1.00000000000000))*(r00)*(x841)*(x842)))+(((sj3)*(x848)*(x849)))+(((IkReal(2090.00000000000))*(cj1)*(x847)))+(((cj3)*(r02)*(x841)*(x849)))+(((IkReal(2090.00000000000))*(cj0)*(x846)))+(((IkReal(247.000000000000))*(x840)*(x847)))+(((r00)*(x840)*(x852)))+(((IkReal(-2090.00000000000))*(x844)))+(((IkReal(-1.00000000000000))*(cj0)*(cj3)*(x844)*(x852)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x850)))+(((cj3)*(r01)*(x843)*(x848)))+(((x846)*(x851)))+(((IkReal(-910.000000000000))*(sj3)))+(((x840)*(x853)))+(((IkReal(-1.00000000000000))*(cj0)*(x844)*(x851)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x841)*(x843)))+(((x841)*(x854))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst4)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(cj0)*(x844)*(x854)))+(((r00)*(x845)*(x852)))+(((IkReal(-1.00000000000000))*(cj3)*(x841)*(x852)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x841)*(x843)))+(((IkReal(-1.00000000000000))*(cj3)*(x848)*(x849)))+(((IkReal(-1.00000000000000))*(cj1)*(x842)))+(((IkReal(-1.00000000000000))*(x840)*(x843)))+(((r02)*(sj3)*(x841)*(x849)))+(((IkReal(-1.00000000000000))*(x841)*(x851)))+(((x845)*(x853)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(247.000000000000))*(x845)*(x847)))+(((IkReal(-1.00000000000000))*(r01)*(x845)*(x849)))+(((r01)*(sj3)*(x843)*(x848)))+(((IkReal(-1.00000000000000))*(x848)*(x850)))+(((IkReal(-247.000000000000))*(sj3)*(x844))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst4)*(((((r02)*(x841)*(x850)))+(((x843)*(x845)))+(((r01)*(x842)*(x848)))+(((IkReal(-1.00000000000000))*(r01)*(x840)*(x849)))+(((IkReal(-247.000000000000))*(cj3)*(x844)))+(((IkReal(-1.00000000000000))*(r00)*(x841)*(x842)))+(((sj3)*(x848)*(x849)))+(((IkReal(2090.00000000000))*(cj1)*(x847)))+(((cj3)*(r02)*(x841)*(x849)))+(((IkReal(2090.00000000000))*(cj0)*(x846)))+(((IkReal(247.000000000000))*(x840)*(x847)))+(((r00)*(x840)*(x852)))+(((IkReal(-2090.00000000000))*(x844)))+(((IkReal(-1.00000000000000))*(cj0)*(cj3)*(x844)*(x852)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x850)))+(((cj3)*(r01)*(x843)*(x848)))+(((x846)*(x851)))+(((IkReal(-910.000000000000))*(sj3)))+(((x840)*(x853)))+(((IkReal(-1.00000000000000))*(cj0)*(x844)*(x851)))+(((IkReal(-1.00000000000000))*(cj3)*(r00)*(x841)*(x843)))+(((x841)*(x854)))))), ((gconst4)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(cj0)*(x844)*(x854)))+(((r00)*(x845)*(x852)))+(((IkReal(-1.00000000000000))*(cj3)*(x841)*(x852)))+(((IkReal(-1.00000000000000))*(r00)*(sj3)*(x841)*(x843)))+(((IkReal(-1.00000000000000))*(cj3)*(x848)*(x849)))+(((IkReal(-1.00000000000000))*(cj1)*(x842)))+(((IkReal(-1.00000000000000))*(x840)*(x843)))+(((r02)*(sj3)*(x841)*(x849)))+(((IkReal(-1.00000000000000))*(x841)*(x851)))+(((x845)*(x853)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(247.000000000000))*(x845)*(x847)))+(((IkReal(-1.00000000000000))*(r01)*(x845)*(x849)))+(((r01)*(sj3)*(x843)*(x848)))+(((IkReal(-1.00000000000000))*(x848)*(x850)))+(((IkReal(-247.000000000000))*(sj3)*(x844)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x856=IKcos(j2);
IkReal x857=IKsin(j2);
IkReal x858=((px)*(sj1));
IkReal x859=((py)*(sj1));
IkReal x860=((cj1)*(sj0));
IkReal x861=((IkReal(1.00000000000000))*(px));
IkReal x862=((r00)*(sj0));
IkReal x863=((IkReal(0.0950000000000000))*(r01));
IkReal x864=((IkReal(0.0650000000000000))*(cj3));
IkReal x865=((cj0)*(r01));
IkReal x866=((IkReal(0.0950000000000000))*(cj1));
IkReal x867=((pz)*(sj1));
IkReal x868=((cj0)*(r00));
IkReal x869=((IkReal(0.0950000000000000))*(sj1));
IkReal x870=((cj0)*(cj1));
IkReal x871=((cj1)*(pz));
IkReal x872=((IkReal(0.0650000000000000))*(sj3));
IkReal x873=((IkReal(0.550000000000000))*(x857));
IkReal x874=((IkReal(0.550000000000000))*(x856));
IkReal x875=((x857)*(x864));
IkReal x876=((x856)*(x872));
IkReal x877=((x856)*(x864));
IkReal x878=((x857)*(x872));
IkReal x879=((x874)+(x877));
IkReal x880=((x873)+(x875)+(x876));
evalcond[0]=((IkReal(-0.350000000000000))+(((sj0)*(x859)))+(((cj0)*(x858)))+(x878)+(x871)+(((IkReal(-1.00000000000000))*(x879))));
evalcond[1]=((x880)+(((IkReal(-1.00000000000000))*(py)*(x860)))+(((IkReal(-1.00000000000000))*(x861)*(x870)))+(x867));
evalcond[2]=((x880)+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x861)))+(((x860)*(x863)))+(((IkReal(-1.00000000000000))*(x862)*(x867)))+(((cj1)*(py)*(r00)))+(((x866)*(x868)))+(((x865)*(x867)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x859)))+(((r02)*(sj0)*(x858)))+(((IkReal(-1.00000000000000))*(r02)*(x869))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x865)*(x871)))+(((IkReal(-0.350000000000000))*(x862)))+(((IkReal(-1.00000000000000))*(r01)*(x858)))+(((IkReal(-1.00000000000000))*(r02)*(x860)*(x861)))+(((sj0)*(sj1)*(x863)))+(((x868)*(x869)))+(((IkReal(0.350000000000000))*(x865)))+(((r00)*(x859)))+(((IkReal(-1.00000000000000))*(x878)))+(x879)+(((pz)*(r00)*(x860)))+(((r02)*(x866)))+(((py)*(r02)*(x870))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x881=((IkReal(2600.00000000000))*(sj3));
IkReal x882=((py)*(sj0));
IkReal x883=((pz)*(sj1));
IkReal x884=((IkReal(2600.00000000000))*(cj3));
IkReal x885=((cj1)*(pz));
IkReal x886=((IkReal(22000.0000000000))*(cj1));
IkReal x887=((cj0)*(px));
IkReal x888=((sj1)*(x887));
if( IKabs(((gconst3)*(((((IkReal(-1.00000000000000))*(x882)*(x886)))+(((IkReal(-1.00000000000000))*(cj1)*(x884)*(x887)))+(((IkReal(22000.0000000000))*(x883)))+(((sj1)*(x881)*(x882)))+(((IkReal(-1.00000000000000))*(cj1)*(x882)*(x884)))+(((IkReal(-1.00000000000000))*(x886)*(x887)))+(((IkReal(-910.000000000000))*(sj3)))+(((x881)*(x885)))+(((x881)*(x888)))+(((x883)*(x884))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst3)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x885)))+(((IkReal(-1.00000000000000))*(x884)*(x885)))+(((x881)*(x883)))+(((IkReal(-1.00000000000000))*(sj1)*(x882)*(x884)))+(((IkReal(-22000.0000000000))*(sj1)*(x882)))+(((IkReal(-1.00000000000000))*(x884)*(x888)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(x888)))+(((IkReal(-1.00000000000000))*(cj1)*(x881)*(x887)))+(((IkReal(-1.00000000000000))*(cj1)*(x881)*(x882))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst3)*(((((IkReal(-1.00000000000000))*(x882)*(x886)))+(((IkReal(-1.00000000000000))*(cj1)*(x884)*(x887)))+(((IkReal(22000.0000000000))*(x883)))+(((sj1)*(x881)*(x882)))+(((IkReal(-1.00000000000000))*(cj1)*(x882)*(x884)))+(((IkReal(-1.00000000000000))*(x886)*(x887)))+(((IkReal(-910.000000000000))*(sj3)))+(((x881)*(x885)))+(((x881)*(x888)))+(((x883)*(x884)))))), ((gconst3)*(((IkReal(7700.00000000000))+(((IkReal(-22000.0000000000))*(x885)))+(((IkReal(-1.00000000000000))*(x884)*(x885)))+(((x881)*(x883)))+(((IkReal(-1.00000000000000))*(sj1)*(x882)*(x884)))+(((IkReal(-22000.0000000000))*(sj1)*(x882)))+(((IkReal(-1.00000000000000))*(x884)*(x888)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-22000.0000000000))*(x888)))+(((IkReal(-1.00000000000000))*(cj1)*(x881)*(x887)))+(((IkReal(-1.00000000000000))*(cj1)*(x881)*(x882)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x889=IKcos(j2);
IkReal x890=IKsin(j2);
IkReal x891=((px)*(sj1));
IkReal x892=((py)*(sj1));
IkReal x893=((cj1)*(sj0));
IkReal x894=((IkReal(1.00000000000000))*(px));
IkReal x895=((r00)*(sj0));
IkReal x896=((IkReal(0.0950000000000000))*(r01));
IkReal x897=((IkReal(0.0650000000000000))*(cj3));
IkReal x898=((cj0)*(r01));
IkReal x899=((IkReal(0.0950000000000000))*(cj1));
IkReal x900=((pz)*(sj1));
IkReal x901=((cj0)*(r00));
IkReal x902=((IkReal(0.0950000000000000))*(sj1));
IkReal x903=((cj0)*(cj1));
IkReal x904=((cj1)*(pz));
IkReal x905=((IkReal(0.0650000000000000))*(sj3));
IkReal x906=((IkReal(0.550000000000000))*(x890));
IkReal x907=((IkReal(0.550000000000000))*(x889));
IkReal x908=((x890)*(x897));
IkReal x909=((x889)*(x905));
IkReal x910=((x889)*(x897));
IkReal x911=((x890)*(x905));
IkReal x912=((x907)+(x910));
IkReal x913=((x906)+(x908)+(x909));
evalcond[0]=((IkReal(-0.350000000000000))+(((IkReal(-1.00000000000000))*(x912)))+(x904)+(((cj0)*(x891)))+(x911)+(((sj0)*(x892))));
evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(x893)))+(x900)+(((IkReal(-1.00000000000000))*(x894)*(x903)))+(x913));
evalcond[2]=((((x893)*(x896)))+(((cj1)*(py)*(r00)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x894)))+(((r02)*(sj0)*(x891)))+(((IkReal(-1.00000000000000))*(x895)*(x900)))+(((IkReal(-1.00000000000000))*(r02)*(x902)))+(x913)+(((x898)*(x900)))+(((x899)*(x901)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x892))));
evalcond[3]=((((sj0)*(sj1)*(x896)))+(((IkReal(0.350000000000000))*(x898)))+(((x901)*(x902)))+(((IkReal(-1.00000000000000))*(x911)))+(((IkReal(-1.00000000000000))*(r01)*(x891)))+(((r02)*(x899)))+(((pz)*(r00)*(x893)))+(((r00)*(x892)))+(x912)+(((IkReal(-0.350000000000000))*(x895)))+(((IkReal(-1.00000000000000))*(x898)*(x904)))+(((IkReal(-1.00000000000000))*(r02)*(x893)*(x894)))+(((py)*(r02)*(x903))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
IkReal x914=((IkReal(0.0715000000000000))*(cj3));
IkReal x915=(py)*(py);
IkReal x916=(px)*(px);
IkReal x917=(pz)*(pz);
IkReal x918=((r01)*(sj0));
IkReal x919=((px)*(sj0));
IkReal x920=((cj1)*(r00));
IkReal x921=((IkReal(0.350000000000000))*(cj0));
IkReal x922=((IkReal(0.113475000000000))*(sj1));
IkReal x923=((r01)*(sj1));
IkReal x924=((IkReal(2.00000000000000))*(pz));
IkReal x925=((IkReal(0.190000000000000))*(px));
IkReal x926=((py)*(sj0));
IkReal x927=((IkReal(1.00000000000000))*(r02));
IkReal x928=((IkReal(0.190000000000000))*(py));
IkReal x929=((cj0)*(sj1));
IkReal x930=((px)*(r02));
IkReal x931=((px)*(r00));
IkReal x932=((r02)*(sj1));
IkReal x933=((IkReal(2.00000000000000))*(py));
IkReal x934=((pp)*(r00));
IkReal x935=((cj0)*(r01));
IkReal x936=((IkReal(0.700000000000000))*(sj1));
IkReal x937=((IkReal(0.190000000000000))*(pz));
IkReal x938=((cj1)*(r01));
IkReal x939=((cj0)*(r00));
IkReal x940=((IkReal(0.700000000000000))*(pz));
IkReal x941=((cj0)*(px));
IkReal x942=((r00)*(sj0));
IkReal x943=((cj1)*(r02));
IkReal x944=((IkReal(1.00000000000000))*(pp));
IkReal x945=((IkReal(1.00000000000000))*(py));
IkReal x946=((IkReal(0.700000000000000))*(cj0));
IkReal x947=((r00)*(sj1));
IkReal x948=((IkReal(0.700000000000000))*(px));
IkReal x949=((cj0)*(cj1));
IkReal x950=((cj1)*(x944));
IkReal x951=((IkReal(2.00000000000000))*(x917));
IkReal x952=((IkReal(2.00000000000000))*(x915));
IkReal x953=((IkReal(2.00000000000000))*(r00)*(x916));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.71238898038469))+(j4)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-0.0950000000000000))+(x919)+(((IkReal(-1.00000000000000))*(cj0)*(x945))));
evalcond[2]=((IkReal(-1.00000000000000))+(x942)+(((IkReal(-1.00000000000000))*(x935))));
evalcond[3]=((((r00)*(x929)))+(x943)+(((sj1)*(x918))));
evalcond[4]=((x932)+(((IkReal(-1.00000000000000))*(cj1)*(x918)))+(((IkReal(-1.00000000000000))*(cj0)*(x920))));
evalcond[5]=((IkReal(0.175200000000000))+(((x929)*(x948)))+(((cj1)*(x940)))+(((IkReal(-1.00000000000000))*(cj0)*(x928)))+(((IkReal(-1.00000000000000))*(x944)))+(((IkReal(0.190000000000000))*(x919)))+(x914)+(((x926)*(x936))));
evalcond[6]=((((IkReal(-1.00000000000000))*(pz)*(x927)))+(((IkReal(0.350000000000000))*(sj1)*(x918)))+(((IkReal(0.350000000000000))*(x943)))+(((IkReal(-1.00000000000000))*(x931)))+(((x921)*(x947)))+(((IkReal(-1.00000000000000))*(r01)*(x945)))+(((IkReal(0.0950000000000000))*(x942)))+(((IkReal(-0.0950000000000000))*(x935))));
evalcond[7]=((((IkReal(-1.00000000000000))*(x927)*(x941)))+(((IkReal(-1.00000000000000))*(x920)*(x921)))+(((pz)*(x918)))+(((IkReal(-0.350000000000000))*(cj1)*(x918)))+(((pz)*(x939)))+(((IkReal(-1.00000000000000))*(x926)*(x927)))+(((IkReal(0.350000000000000))*(x932))));
evalcond[8]=((IkReal(-0.306725000000000))+(((IkReal(-2.00000000000000))*(x916)*(x942)))+(((IkReal(-0.113475000000000))*(x935)))+(((IkReal(0.113475000000000))*(x942)))+(((IkReal(-1.00000000000000))*(py)*(x943)*(x946)))+(((r01)*(x928)))+(((r02)*(x937)))+(((x935)*(x952)))+(((cj0)*(py)*(r02)*(x924)))+(((IkReal(-1.00000000000000))*(r02)*(x919)*(x924)))+(((IkReal(-1.00000000000000))*(px)*(x918)*(x933)))+(((cj0)*(x931)*(x933)))+(((IkReal(-1.00000000000000))*(x935)*(x944)))+(((IkReal(-0.0665000000000000))*(sj1)*(x918)))+(((IkReal(-1.00000000000000))*(sj0)*(x920)*(x940)))+(((IkReal(0.700000000000000))*(x919)*(x943)))+(((x923)*(x948)))+(((IkReal(-1.00000000000000))*(x914)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x936)))+(((IkReal(-0.0665000000000000))*(x943)))+(((sj0)*(x934)))+(((r00)*(x925)))+(((cj1)*(x935)*(x940)))+(((IkReal(-0.0665000000000000))*(r00)*(x929))));
evalcond[9]=((((IkReal(-1.00000000000000))*(x943)*(x951)))+(((IkReal(-1.00000000000000))*(x924)*(x926)*(x932)))+(((pp)*(x943)))+(((IkReal(-1.00000000000000))*(py)*(x924)*(x938)))+(((IkReal(-1.00000000000000))*(x918)*(x922)))+(((sj0)*(x920)*(x937)))+(((IkReal(-0.190000000000000))*(x919)*(x943)))+(((x929)*(x934)))+(((pp)*(sj1)*(x918)))+(((IkReal(-1.00000000000000))*(sj1)*(x918)*(x952)))+(((IkReal(-0.113475000000000))*(x943)))+(((IkReal(-1.00000000000000))*(x923)*(x925)))+(((cj0)*(x928)*(x943)))+(((IkReal(-1.00000000000000))*(x929)*(x953)))+(((IkReal(-0.0665000000000000))*(x942)))+(((IkReal(0.700000000000000))*(x931)))+(((IkReal(-1.00000000000000))*(cj1)*(x935)*(x937)))+(((IkReal(0.700000000000000))*(py)*(r01)))+(((IkReal(-1.00000000000000))*(x919)*(x933)*(x947)))+(((IkReal(-1.00000000000000))*(x923)*(x933)*(x941)))+(((IkReal(-1.00000000000000))*(x924)*(x929)*(x930)))+(((r02)*(x940)))+(((IkReal(0.0665000000000000))*(x935)))+(((x928)*(x947)))+(((IkReal(-1.00000000000000))*(px)*(x920)*(x924)))+(((IkReal(-1.00000000000000))*(x922)*(x939))));
evalcond[10]=((((IkReal(-1.00000000000000))*(cj0)*(x920)*(x944)))+(((x925)*(x938)))+(((IkReal(-1.00000000000000))*(x932)*(x951)))+(((cj1)*(x918)*(x952)))+(((IkReal(-0.131525000000000))*(cj1)*(x918)))+(((IkReal(2.00000000000000))*(cj0)*(x916)*(x920)))+(((pp)*(x932)))+(((x919)*(x920)*(x933)))+(((x939)*(x940)))+(((x924)*(x930)*(x949)))+(((IkReal(-1.00000000000000))*(py)*(x923)*(x924)))+(((IkReal(-1.00000000000000))*(x930)*(x946)))+(((sj1)*(x937)*(x942)))+(((x924)*(x926)*(x943)))+(((IkReal(-1.00000000000000))*(cj0)*(x923)*(x937)))+(((cj1)*(px)*(x933)*(x935)))+(((IkReal(-0.700000000000000))*(r02)*(x926)))+(((x918)*(x940)))+(((IkReal(-0.131525000000000))*(cj0)*(x920)))+(((r02)*(x928)*(x929)))+(((IkReal(-1.00000000000000))*(sj1)*(x924)*(x931)))+(((IkReal(-1.00000000000000))*(x920)*(x928)))+(((IkReal(-0.190000000000000))*(x919)*(x932)))+(((IkReal(-1.00000000000000))*(x918)*(x950)))+(((IkReal(0.131525000000000))*(x932))));
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  )
{
{
IkReal dummyeval[1];
IkReal gconst5;
gconst5=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
{
IkReal dummyeval[1];
IkReal gconst6;
gconst6=IKsign(((IkReal(-12100.0000000000))+(((IkReal(-169.000000000000))*((cj3)*(cj3))))+(((IkReal(-2860.00000000000))*(cj3)))+(((IkReal(-169.000000000000))*((sj3)*(sj3))))));
dummyeval[0]=((IkReal(-71.5976331360947))+(((IkReal(-16.9230769230769))*(cj3)))+(((IkReal(-1.00000000000000))*((cj3)*(cj3))))+(((IkReal(-1.00000000000000))*((sj3)*(sj3)))));
if( IKabs(dummyeval[0]) < 0.0000010000000000  )
{
continue;

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x954=((cj1)*(cj3));
IkReal x955=((sj0)*(sj1));
IkReal x956=((IkReal(22000.0000000000))*(pz));
IkReal x957=((IkReal(2600.00000000000))*(pz));
IkReal x958=((r02)*(sj1));
IkReal x959=((IkReal(2600.00000000000))*(sj3));
IkReal x960=((cj1)*(sj3));
IkReal x961=((r01)*(sj0));
IkReal x962=((cj0)*(sj1));
IkReal x963=((IkReal(2600.00000000000))*(cj3));
IkReal x964=((IkReal(2090.00000000000))*(cj1));
IkReal x965=((IkReal(22000.0000000000))*(px));
IkReal x966=((px)*(r02));
IkReal x967=((IkReal(22000.0000000000))*(py));
IkReal x968=((py)*(r00));
IkReal x969=((px)*(r01));
IkReal x970=((IkReal(247.000000000000))*(cj0)*(r00));
IkReal x971=((x959)*(x962));
if( IKabs(((gconst6)*(((((cj0)*(py)*(x958)*(x963)))+(((cj3)*(r00)*(x955)*(x957)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x967)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x964)))+(((IkReal(2090.00000000000))*(x958)))+(((IkReal(-2600.00000000000))*(x954)*(x968)))+(((x957)*(x960)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x957)*(x962)))+(((IkReal(-1.00000000000000))*(x961)*(x964)))+(((IkReal(-1.00000000000000))*(x955)*(x963)*(x966)))+(((IkReal(-1.00000000000000))*(r02)*(x955)*(x965)))+(((cj0)*(x958)*(x967)))+(((IkReal(-1.00000000000000))*(x954)*(x970)))+(((px)*(x971)))+(((py)*(x955)*(x959)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(2600.00000000000))*(x954)*(x969)))+(((r00)*(x955)*(x956)))+(((IkReal(247.000000000000))*(cj3)*(x958)))+(((cj1)*(r01)*(x965)))+(((IkReal(-247.000000000000))*(x954)*(x961)))+(((IkReal(-1.00000000000000))*(r01)*(x956)*(x962))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst6)*(((IkReal(7700.00000000000))+(((cj1)*(x959)*(x969)))+(((IkReal(-1.00000000000000))*(x955)*(x959)*(x966)))+(((r00)*(sj3)*(x955)*(x957)))+(((IkReal(-1.00000000000000))*(x954)*(x957)))+(((IkReal(-1.00000000000000))*(cj1)*(x959)*(x968)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x957)*(x962)))+(((IkReal(-1.00000000000000))*(x962)*(x965)))+(((IkReal(-1.00000000000000))*(x955)*(x967)))+(((IkReal(-1.00000000000000))*(cj1)*(x956)))+(((IkReal(247.000000000000))*(sj3)*(x958)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(px)*(x962)*(x963)))+(((IkReal(-1.00000000000000))*(x960)*(x970)))+(((cj0)*(py)*(x958)*(x959)))+(((IkReal(-1.00000000000000))*(py)*(x955)*(x963)))+(((IkReal(-247.000000000000))*(x960)*(x961))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst6)*(((((cj0)*(py)*(x958)*(x963)))+(((cj3)*(r00)*(x955)*(x957)))+(((IkReal(-1.00000000000000))*(cj1)*(r00)*(x967)))+(((IkReal(-1.00000000000000))*(cj0)*(r00)*(x964)))+(((IkReal(2090.00000000000))*(x958)))+(((IkReal(-2600.00000000000))*(x954)*(x968)))+(((x957)*(x960)))+(((IkReal(-1.00000000000000))*(cj3)*(r01)*(x957)*(x962)))+(((IkReal(-1.00000000000000))*(x961)*(x964)))+(((IkReal(-1.00000000000000))*(x955)*(x963)*(x966)))+(((IkReal(-1.00000000000000))*(r02)*(x955)*(x965)))+(((cj0)*(x958)*(x967)))+(((IkReal(-1.00000000000000))*(x954)*(x970)))+(((px)*(x971)))+(((py)*(x955)*(x959)))+(((IkReal(-910.000000000000))*(sj3)))+(((IkReal(2600.00000000000))*(x954)*(x969)))+(((r00)*(x955)*(x956)))+(((IkReal(247.000000000000))*(cj3)*(x958)))+(((cj1)*(r01)*(x965)))+(((IkReal(-247.000000000000))*(x954)*(x961)))+(((IkReal(-1.00000000000000))*(r01)*(x956)*(x962)))))), ((gconst6)*(((IkReal(7700.00000000000))+(((cj1)*(x959)*(x969)))+(((IkReal(-1.00000000000000))*(x955)*(x959)*(x966)))+(((r00)*(sj3)*(x955)*(x957)))+(((IkReal(-1.00000000000000))*(x954)*(x957)))+(((IkReal(-1.00000000000000))*(cj1)*(x959)*(x968)))+(((IkReal(-1.00000000000000))*(r01)*(sj3)*(x957)*(x962)))+(((IkReal(-1.00000000000000))*(x962)*(x965)))+(((IkReal(-1.00000000000000))*(x955)*(x967)))+(((IkReal(-1.00000000000000))*(cj1)*(x956)))+(((IkReal(247.000000000000))*(sj3)*(x958)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(px)*(x962)*(x963)))+(((IkReal(-1.00000000000000))*(x960)*(x970)))+(((cj0)*(py)*(x958)*(x959)))+(((IkReal(-1.00000000000000))*(py)*(x955)*(x963)))+(((IkReal(-247.000000000000))*(x960)*(x961)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x972=IKcos(j2);
IkReal x973=IKsin(j2);
IkReal x974=((px)*(sj1));
IkReal x975=((py)*(sj1));
IkReal x976=((cj1)*(sj0));
IkReal x977=((IkReal(1.00000000000000))*(px));
IkReal x978=((r00)*(sj0));
IkReal x979=((IkReal(0.0950000000000000))*(r01));
IkReal x980=((IkReal(0.0650000000000000))*(cj3));
IkReal x981=((cj0)*(r01));
IkReal x982=((IkReal(0.0950000000000000))*(cj1));
IkReal x983=((pz)*(sj1));
IkReal x984=((cj0)*(r00));
IkReal x985=((IkReal(0.0950000000000000))*(sj1));
IkReal x986=((cj0)*(cj1));
IkReal x987=((cj1)*(pz));
IkReal x988=((IkReal(0.0650000000000000))*(sj3));
IkReal x989=((IkReal(0.550000000000000))*(x973));
IkReal x990=((IkReal(0.550000000000000))*(x972));
IkReal x991=((x973)*(x980));
IkReal x992=((x972)*(x988));
IkReal x993=((x973)*(x988));
IkReal x994=((x972)*(x980));
IkReal x995=((x990)+(x994));
IkReal x996=((x989)+(x992)+(x991));
evalcond[0]=((IkReal(-0.350000000000000))+(x987)+(((IkReal(-1.00000000000000))*(x995)))+(x993)+(((cj0)*(x974)))+(((sj0)*(x975))));
evalcond[1]=((((IkReal(-1.00000000000000))*(x977)*(x986)))+(x983)+(((IkReal(-1.00000000000000))*(py)*(x976)))+(x996));
evalcond[2]=((((x981)*(x983)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x977)))+(((x982)*(x984)))+(((IkReal(-1.00000000000000))*(x978)*(x983)))+(((cj1)*(py)*(r00)))+(((IkReal(-1.00000000000000))*(r02)*(x985)))+(((x976)*(x979)))+(((IkReal(-1.00000000000000))*(x996)))+(((r02)*(sj0)*(x974)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x975))));
evalcond[3]=((((r00)*(x975)))+(((IkReal(-0.350000000000000))*(x978)))+(((x984)*(x985)))+(((IkReal(-1.00000000000000))*(x981)*(x987)))+(((IkReal(0.350000000000000))*(x981)))+(((pz)*(r00)*(x976)))+(((py)*(r02)*(x986)))+(((IkReal(-1.00000000000000))*(r01)*(x974)))+(((sj0)*(sj1)*(x979)))+(((r02)*(x982)))+(((IkReal(-1.00000000000000))*(r02)*(x976)*(x977)))+(((IkReal(-1.00000000000000))*(x995)))+(x993));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x997=((IkReal(2600.00000000000))*(sj3));
IkReal x998=((py)*(sj0));
IkReal x999=((pz)*(sj1));
IkReal x1000=((IkReal(2600.00000000000))*(cj3));
IkReal x1001=((cj1)*(pz));
IkReal x1002=((IkReal(22000.0000000000))*(cj1));
IkReal x1003=((cj0)*(px));
IkReal x1004=((sj1)*(x1003));
if( IKabs(((gconst5)*(((((IkReal(-1.00000000000000))*(cj1)*(x1000)*(x1003)))+(((x1000)*(x999)))+(((IkReal(-1.00000000000000))*(x1002)*(x1003)))+(((sj1)*(x997)*(x998)))+(((IkReal(-1.00000000000000))*(cj1)*(x1000)*(x998)))+(((IkReal(-910.000000000000))*(sj3)))+(((x1001)*(x997)))+(((x1004)*(x997)))+(((IkReal(22000.0000000000))*(x999)))+(((IkReal(-1.00000000000000))*(x1002)*(x998))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst5)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x1000)*(x998)))+(((IkReal(-1.00000000000000))*(x1000)*(x1001)))+(((IkReal(-1.00000000000000))*(cj1)*(x1003)*(x997)))+(((IkReal(-1.00000000000000))*(x1000)*(x1004)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x997)*(x998)))+(((IkReal(-22000.0000000000))*(x1001)))+(((IkReal(-22000.0000000000))*(sj1)*(x998)))+(((IkReal(-22000.0000000000))*(x1004)))+(((x997)*(x999))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst5)*(((((IkReal(-1.00000000000000))*(cj1)*(x1000)*(x1003)))+(((x1000)*(x999)))+(((IkReal(-1.00000000000000))*(x1002)*(x1003)))+(((sj1)*(x997)*(x998)))+(((IkReal(-1.00000000000000))*(cj1)*(x1000)*(x998)))+(((IkReal(-910.000000000000))*(sj3)))+(((x1001)*(x997)))+(((x1004)*(x997)))+(((IkReal(22000.0000000000))*(x999)))+(((IkReal(-1.00000000000000))*(x1002)*(x998)))))), ((gconst5)*(((IkReal(7700.00000000000))+(((IkReal(-1.00000000000000))*(sj1)*(x1000)*(x998)))+(((IkReal(-1.00000000000000))*(x1000)*(x1001)))+(((IkReal(-1.00000000000000))*(cj1)*(x1003)*(x997)))+(((IkReal(-1.00000000000000))*(x1000)*(x1004)))+(((IkReal(910.000000000000))*(cj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x997)*(x998)))+(((IkReal(-22000.0000000000))*(x1001)))+(((IkReal(-22000.0000000000))*(sj1)*(x998)))+(((IkReal(-22000.0000000000))*(x1004)))+(((x997)*(x999)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[4];
IkReal x1005=IKcos(j2);
IkReal x1006=IKsin(j2);
IkReal x1007=((px)*(sj1));
IkReal x1008=((py)*(sj1));
IkReal x1009=((cj1)*(sj0));
IkReal x1010=((IkReal(1.00000000000000))*(px));
IkReal x1011=((r00)*(sj0));
IkReal x1012=((IkReal(0.0950000000000000))*(r01));
IkReal x1013=((IkReal(0.0650000000000000))*(cj3));
IkReal x1014=((cj0)*(r01));
IkReal x1015=((IkReal(0.0950000000000000))*(cj1));
IkReal x1016=((pz)*(sj1));
IkReal x1017=((cj0)*(r00));
IkReal x1018=((IkReal(0.0950000000000000))*(sj1));
IkReal x1019=((cj0)*(cj1));
IkReal x1020=((cj1)*(pz));
IkReal x1021=((IkReal(0.0650000000000000))*(sj3));
IkReal x1022=((IkReal(0.550000000000000))*(x1006));
IkReal x1023=((IkReal(0.550000000000000))*(x1005));
IkReal x1024=((x1006)*(x1013));
IkReal x1025=((x1005)*(x1021));
IkReal x1026=((x1006)*(x1021));
IkReal x1027=((x1005)*(x1013));
IkReal x1028=((x1027)+(x1023));
IkReal x1029=((x1025)+(x1024)+(x1022));
evalcond[0]=((IkReal(-0.350000000000000))+(((cj0)*(x1007)))+(((IkReal(-1.00000000000000))*(x1028)))+(((sj0)*(x1008)))+(x1026)+(x1020));
evalcond[1]=((((IkReal(-1.00000000000000))*(py)*(x1009)))+(((IkReal(-1.00000000000000))*(x1010)*(x1019)))+(x1016)+(x1029));
evalcond[2]=((((x1014)*(x1016)))+(((x1009)*(x1012)))+(((r02)*(sj0)*(x1007)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x1010)))+(((cj1)*(py)*(r00)))+(((IkReal(-1.00000000000000))*(cj0)*(r02)*(x1008)))+(((IkReal(-1.00000000000000))*(x1011)*(x1016)))+(((IkReal(-1.00000000000000))*(r02)*(x1018)))+(((IkReal(-1.00000000000000))*(x1029)))+(((x1015)*(x1017))));
evalcond[3]=((((r02)*(x1015)))+(((IkReal(-1.00000000000000))*(x1014)*(x1020)))+(((IkReal(-0.350000000000000))*(x1011)))+(((IkReal(-1.00000000000000))*(x1028)))+(((IkReal(0.350000000000000))*(x1014)))+(((IkReal(-1.00000000000000))*(r01)*(x1007)))+(((x1017)*(x1018)))+(((py)*(r02)*(x1019)))+(((pz)*(r00)*(x1009)))+(x1026)+(((sj0)*(sj1)*(x1012)))+(((r00)*(x1008)))+(((IkReal(-1.00000000000000))*(r02)*(x1009)*(x1010))));
if( IKabs(evalcond[0]) > 0.000001  || IKabs(evalcond[1]) > 0.000001  || IKabs(evalcond[2]) > 0.000001  || IKabs(evalcond[3]) > 0.000001  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
IkReal x1030=((r01)*(sj1));
IkReal x1031=((IkReal(0.350000000000000))*(sj0));
IkReal x1032=((px)*(r02));
IkReal x1033=((IkReal(1.00000000000000))*(cj0));
IkReal x1034=((cj0)*(r00));
IkReal x1035=((IkReal(0.350000000000000))*(cj1));
IkReal x1036=((IkReal(0.700000000000000))*(sj1));
IkReal x1037=((IkReal(0.190000000000000))*(px));
IkReal x1038=((r00)*(sj0));
IkReal x1039=((px)*(sj0));
IkReal x1040=((IkReal(0.700000000000000))*(cj1));
IkReal x1041=((py)*(r02));
IkReal x1042=((IkReal(0.190000000000000))*(py));
IkReal x1043=((IkReal(2.00000000000000))*(py));
IkReal x1044=((pz)*(r01));
IkReal x1045=((IkReal(2.00000000000000))*(cj0));
IkReal x1046=((IkReal(0.350000000000000))*(sj1));
IkReal x1047=((pz)*(r02));
IkReal x1048=((cj0)*(r01));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(1.45233765858574))+(j3)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(py)*(x1033)))+(x1039));
evalcond[2]=((sj4)+(x1038)+(((IkReal(-1.00000000000000))*(r01)*(x1033))));
evalcond[3]=((IkReal(0.166749999870000))+(((pz)*(x1040)))+(((py)*(sj0)*(x1036)))+(((cj0)*(px)*(x1036)))+(((IkReal(-1.00000000000000))*(pp)))+(((sj0)*(x1037)))+(((IkReal(-1.00000000000000))*(cj0)*(x1042))));
evalcond[4]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((IkReal(-0.546145583500000))*(cj4)))+(((x1034)*(x1046)))+(((IkReal(-0.0950000000000000))*(x1048)))+(((x1030)*(x1031)))+(((r02)*(x1035)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((IkReal(0.0950000000000000))*(x1038)))+(((IkReal(-1.00000000000000))*(x1047))));
evalcond[5]=((((r02)*(x1046)))+(((IkReal(1.00000000000000e-9))*(cj4)))+(((IkReal(-1.00000000000000))*(x1034)*(x1035)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x1031)))+(((sj0)*(x1044)))+(((IkReal(-1.00000000000000))*(x1032)*(x1033)))+(((pz)*(x1034)))+(((IkReal(-1.00000000000000))*(sj0)*(x1041))));
evalcond[6]=((((py)*(x1043)*(x1048)))+(((IkReal(-1.00000000000000))*(r01)*(x1039)*(x1043)))+(((IkReal(0.190000000000000))*(x1047)))+(((IkReal(-1.00000000000000))*(cj0)*(x1040)*(x1041)))+(((IkReal(0.700000000000000))*(px)*(x1030)))+(((pz)*(x1041)*(x1045)))+(((IkReal(-0.113475000000000))*(x1048)))+(((sj0)*(x1032)*(x1040)))+(((IkReal(-1.00000000000000))*(pz)*(x1038)*(x1040)))+(((IkReal(-2.00000000000000))*(x1038)*((px)*(px))))+(((IkReal(-2.00000000000000))*(pz)*(sj0)*(x1032)))+(((pp)*(x1038)))+(((IkReal(-0.0665000000000000))*(sj1)*(x1034)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x1036)))+(((r00)*(x1037)))+(((IkReal(0.113475000000000))*(x1038)))+(((px)*(x1034)*(x1043)))+(((IkReal(0.298274999870000))*(sj4)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x1033)))+(((IkReal(-0.0665000000000000))*(sj0)*(x1030)))+(((r01)*(x1042)))+(((cj0)*(x1040)*(x1044))));
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  )
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x1049=((IkReal(1.81818181870518))*(sj1));
IkReal x1050=((py)*(sj0));
IkReal x1051=((IkReal(0.216392517249668))*(sj1));
IkReal x1052=((cj0)*(px));
IkReal x1053=((IkReal(0.216392517249668))*(cj1));
IkReal x1054=((IkReal(1.81818181870518))*(cj1));
if( IKabs(((IkReal(0.0757373810373838))+(((IkReal(-1.00000000000000))*(pz)*(x1053)))+(((x1052)*(x1054)))+(((x1050)*(x1054)))+(((IkReal(-1.00000000000000))*(x1051)*(x1052)))+(((IkReal(-1.00000000000000))*(pz)*(x1049)))+(((IkReal(-1.00000000000000))*(x1050)*(x1051))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.636363636546814))+(((x1052)*(x1053)))+(((x1049)*(x1052)))+(((x1050)*(x1053)))+(((x1049)*(x1050)))+(((pz)*(x1054)))+(((IkReal(-1.00000000000000))*(pz)*(x1051))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(0.0757373810373838))+(((IkReal(-1.00000000000000))*(pz)*(x1053)))+(((x1052)*(x1054)))+(((x1050)*(x1054)))+(((IkReal(-1.00000000000000))*(x1051)*(x1052)))+(((IkReal(-1.00000000000000))*(pz)*(x1049)))+(((IkReal(-1.00000000000000))*(x1050)*(x1051)))))+IKsqr(((IkReal(-0.636363636546814))+(((x1052)*(x1053)))+(((x1049)*(x1052)))+(((x1050)*(x1053)))+(((x1049)*(x1050)))+(((pz)*(x1054)))+(((IkReal(-1.00000000000000))*(pz)*(x1051)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
j2array[0]=IKatan2(((IkReal(0.0757373810373838))+(((IkReal(-1.00000000000000))*(pz)*(x1053)))+(((x1052)*(x1054)))+(((x1050)*(x1054)))+(((IkReal(-1.00000000000000))*(x1051)*(x1052)))+(((IkReal(-1.00000000000000))*(pz)*(x1049)))+(((IkReal(-1.00000000000000))*(x1050)*(x1051)))), ((IkReal(-0.636363636546814))+(((x1052)*(x1053)))+(((x1049)*(x1052)))+(((x1050)*(x1053)))+(((x1049)*(x1050)))+(((pz)*(x1054)))+(((IkReal(-1.00000000000000))*(pz)*(x1051)))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[8];
IkReal x1055=IKsin(j2);
IkReal x1056=IKcos(j2);
IkReal x1057=(py)*(py);
IkReal x1058=(px)*(px);
IkReal x1059=(pz)*(pz);
IkReal x1060=((sj0)*(sj1));
IkReal x1061=((py)*(r00));
IkReal x1062=((IkReal(0.190000000000000))*(cj1));
IkReal x1063=((cj0)*(r02));
IkReal x1064=((py)*(r01));
IkReal x1065=((px)*(r01));
IkReal x1066=((IkReal(0.190000000000000))*(sj1));
IkReal x1067=((cj1)*(r02));
IkReal x1068=((pz)*(r00));
IkReal x1069=((cj1)*(sj0));
IkReal x1070=((cj0)*(r00));
IkReal x1071=((IkReal(2.00000000000000))*(sj1));
IkReal x1072=((IkReal(0.700000000000000))*(px));
IkReal x1073=((IkReal(1.00000000000000))*(cj1));
IkReal x1074=((IkReal(2.00000000000000))*(px));
IkReal x1075=((pp)*(r01));
IkReal x1076=((pp)*(sj1));
IkReal x1077=((r02)*(sj1));
IkReal x1078=((pz)*(r01));
IkReal x1079=((px)*(r02));
IkReal x1080=((IkReal(0.700000000000000))*(sj0));
IkReal x1081=((cj0)*(sj1));
IkReal x1082=((r00)*(sj0));
IkReal x1083=((cj0)*(r01));
IkReal x1084=((IkReal(1.00000000000000))*(py));
IkReal x1085=((pz)*(r02));
IkReal x1086=((IkReal(2.00000000000000))*(py));
IkReal x1087=((cj1)*(pz));
IkReal x1088=((IkReal(0.0950000000000000))*(r01));
IkReal x1089=((cj0)*(px));
IkReal x1090=((pz)*(x1071));
IkReal x1091=((cj4)*(x1055));
IkReal x1092=((cj4)*(x1056));
IkReal x1093=((sj4)*(x1055));
IkReal x1094=((IkReal(0.0645444780500000))*(x1056));
IkReal x1095=((IkReal(0.542318181700000))*(x1056));
IkReal x1096=((IkReal(2.00000000000000))*(r01)*(x1057));
evalcond[0]=((((sj1)*(x1070)))+(((IkReal(0.992991970000000))*(x1092)))+(x1067)+(((r01)*(x1060)))+(((IkReal(-0.118181820000000))*(x1091))));
evalcond[1]=((IkReal(-0.350000000000000))+(((py)*(x1060)))+(x1087)+(((px)*(x1081)))+(((IkReal(-1.00000000000000))*(x1095)))+(((IkReal(0.0645444780500000))*(x1055))));
evalcond[2]=((((IkReal(-1.00000000000000))*(x1070)*(x1073)))+(((IkReal(-0.992991970000000))*(x1091)))+(((IkReal(-0.118181820000000))*(x1092)))+(((IkReal(-1.00000000000000))*(r01)*(x1069)))+(x1077));
evalcond[3]=((((IkReal(0.542318181700000))*(x1055)))+(((IkReal(-1.00000000000000))*(x1069)*(x1084)))+(((pz)*(sj1)))+(((IkReal(-1.00000000000000))*(x1073)*(x1089)))+(x1094));
evalcond[4]=((((IkReal(-1.00000000000000))*(x1060)*(x1068)))+(((x1078)*(x1081)))+(((IkReal(0.0950000000000000))*(cj1)*(x1070)))+(((x1060)*(x1079)))+(((IkReal(-1.00000000000000))*(x1065)*(x1073)))+(((IkReal(-0.0950000000000000))*(x1077)))+(((sj4)*(x1094)))+(((cj1)*(x1061)))+(((IkReal(0.542318181700000))*(x1093)))+(((x1069)*(x1088)))+(((IkReal(-1.00000000000000))*(sj1)*(x1063)*(x1084))));
evalcond[5]=((((sj1)*(x1061)))+(((x1068)*(x1069)))+(((IkReal(-0.0645444780500000))*(x1093)))+(((IkReal(-1.00000000000000))*(px)*(sj0)*(x1067)))+(((IkReal(0.0950000000000000))*(x1067)))+(((IkReal(0.350000000000000))*(x1083)))+(((x1060)*(x1088)))+(((IkReal(-1.00000000000000))*(cj0)*(x1073)*(x1078)))+(((IkReal(-0.350000000000000))*(x1082)))+(((cj1)*(py)*(x1063)))+(((IkReal(-1.00000000000000))*(sj1)*(x1065)))+(((IkReal(0.0950000000000000))*(sj1)*(x1070)))+(((sj4)*(x1095))));
evalcond[6]=((((IkReal(-0.0665000000000000))*(x1082)))+(((IkReal(-1.00000000000000))*(x1058)*(x1070)*(x1071)))+(((py)*(x1062)*(x1063)))+(((IkReal(-0.113475000000000))*(sj1)*(x1070)))+(((IkReal(-1.00000000000000))*(px)*(x1063)*(x1090)))+(((IkReal(-0.296184679851750))*(x1092)))+(((IkReal(0.0665000000000000))*(x1083)))+(((IkReal(0.700000000000000))*(x1064)))+(((IkReal(-1.00000000000000))*(sj0)*(x1062)*(x1079)))+(((pp)*(x1067)))+(((sj0)*(x1062)*(x1068)))+(((x1061)*(x1066)))+(((IkReal(0.700000000000000))*(x1085)))+(((IkReal(-1.00000000000000))*(x1064)*(x1071)*(x1089)))+(((IkReal(-1.00000000000000))*(cj1)*(x1068)*(x1074)))+(((IkReal(-1.00000000000000))*(x1060)*(x1061)*(x1074)))+(((IkReal(-1.00000000000000))*(x1060)*(x1085)*(x1086)))+(((IkReal(-1.00000000000000))*(x1060)*(x1096)))+(((x1060)*(x1075)))+(((IkReal(0.0352506812605000))*(x1091)))+(((IkReal(-0.113475000000000))*(x1067)))+(((r00)*(x1072)))+(((IkReal(-2.00000000000000))*(x1064)*(x1087)))+(((IkReal(-2.00000000000000))*(x1059)*(x1067)))+(((IkReal(-1.00000000000000))*(x1065)*(x1066)))+(((IkReal(-0.113475000000000))*(r01)*(x1060)))+(((IkReal(-1.00000000000000))*(cj0)*(x1062)*(x1078)))+(((x1070)*(x1076))));
evalcond[7]=((((IkReal(-1.00000000000000))*(py)*(r02)*(x1080)))+(((IkReal(0.296184679851750))*(x1091)))+(((IkReal(0.700000000000000))*(cj0)*(x1068)))+(((IkReal(-1.00000000000000))*(x1064)*(x1090)))+(((IkReal(-0.131525000000000))*(cj1)*(x1070)))+(((py)*(x1063)*(x1066)))+(((IkReal(-1.00000000000000))*(px)*(x1068)*(x1071)))+(((cj0)*(cj1)*(x1064)*(x1074)))+(((x1061)*(x1069)*(x1074)))+(((x1078)*(x1080)))+(((IkReal(0.131525000000000))*(x1077)))+(((IkReal(-1.00000000000000))*(r02)*(x1059)*(x1071)))+(((x1069)*(x1096)))+(((IkReal(-1.00000000000000))*(pp)*(x1070)*(x1073)))+(((IkReal(-1.00000000000000))*(x1063)*(x1072)))+(((IkReal(-1.00000000000000))*(cj0)*(x1066)*(x1078)))+(((IkReal(-1.00000000000000))*(x1069)*(x1075)))+(((x1062)*(x1065)))+(((IkReal(0.190000000000000))*(x1060)*(x1068)))+(((IkReal(-1.00000000000000))*(x1061)*(x1062)))+(((x1063)*(x1074)*(x1087)))+(((IkReal(2.00000000000000))*(cj1)*(x1058)*(x1070)))+(((IkReal(-0.131525000000000))*(r01)*(x1069)))+(((r02)*(x1076)))+(((pz)*(sj0)*(x1067)*(x1086)))+(((IkReal(0.0352506812605000))*(x1092)))+(((IkReal(-0.190000000000000))*(x1060)*(x1079))));
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  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

} else
{
IkReal x1097=((r01)*(sj1));
IkReal x1098=((IkReal(0.350000000000000))*(sj0));
IkReal x1099=((px)*(r02));
IkReal x1100=((IkReal(1.00000000000000))*(cj0));
IkReal x1101=((cj0)*(r00));
IkReal x1102=((IkReal(0.350000000000000))*(cj1));
IkReal x1103=((IkReal(0.700000000000000))*(sj1));
IkReal x1104=((IkReal(0.190000000000000))*(px));
IkReal x1105=((r00)*(sj0));
IkReal x1106=((px)*(sj0));
IkReal x1107=((IkReal(0.700000000000000))*(cj1));
IkReal x1108=((py)*(r02));
IkReal x1109=((IkReal(0.190000000000000))*(py));
IkReal x1110=((IkReal(2.00000000000000))*(py));
IkReal x1111=((pz)*(r01));
IkReal x1112=((IkReal(2.00000000000000))*(cj0));
IkReal x1113=((IkReal(0.350000000000000))*(sj1));
IkReal x1114=((pz)*(r02));
IkReal x1115=((cj0)*(r01));
evalcond[0]=((IkReal(-3.14159265358979))+(IKfmod(((IkReal(4.83084764859385))+(j3)), IkReal(6.28318530717959))));
evalcond[1]=((IkReal(-0.0950000000000000))+(((IkReal(-1.00000000000000))*(py)*(x1100)))+(x1106));
evalcond[2]=((x1105)+(sj4)+(((IkReal(-1.00000000000000))*(r01)*(x1100))));
evalcond[3]=((IkReal(0.166749999870000))+(((py)*(sj0)*(x1103)))+(((IkReal(-1.00000000000000))*(pp)))+(((IkReal(-1.00000000000000))*(cj0)*(x1109)))+(((sj0)*(x1104)))+(((pz)*(x1107)))+(((cj0)*(px)*(x1103))));
evalcond[4]=((((IkReal(-1.00000000000000))*(py)*(r01)))+(((r02)*(x1102)))+(((IkReal(-1.00000000000000))*(px)*(r00)))+(((x1101)*(x1113)))+(((x1097)*(x1098)))+(((IkReal(-1.00000000000000))*(x1114)))+(((IkReal(0.546145583500000))*(cj4)))+(((IkReal(-0.0950000000000000))*(x1115)))+(((IkReal(0.0950000000000000))*(x1105))));
evalcond[5]=((((r02)*(x1113)))+(((IkReal(1.00000000000000e-9))*(cj4)))+(((IkReal(-1.00000000000000))*(sj0)*(x1108)))+(((pz)*(x1101)))+(((IkReal(-1.00000000000000))*(x1101)*(x1102)))+(((IkReal(-1.00000000000000))*(cj1)*(r01)*(x1098)))+(((IkReal(-1.00000000000000))*(x1099)*(x1100)))+(((sj0)*(x1111))));
evalcond[6]=((((IkReal(0.190000000000000))*(x1114)))+(((IkReal(-0.0665000000000000))*(sj0)*(x1097)))+(((pz)*(x1108)*(x1112)))+(((IkReal(-1.00000000000000))*(cj0)*(x1107)*(x1108)))+(((IkReal(-1.00000000000000))*(r01)*(x1106)*(x1110)))+(((IkReal(-1.00000000000000))*(py)*(r00)*(x1103)))+(((IkReal(-0.113475000000000))*(x1115)))+(((IkReal(-2.00000000000000))*(x1105)*((px)*(px))))+(((IkReal(-1.00000000000000))*(pp)*(r01)*(x1100)))+(((pp)*(x1105)))+(((IkReal(0.113475000000000))*(x1105)))+(((py)*(x1110)*(x1115)))+(((px)*(x1101)*(x1110)))+(((IkReal(-1.00000000000000))*(pz)*(x1105)*(x1107)))+(((IkReal(-2.00000000000000))*(pz)*(sj0)*(x1099)))+(((IkReal(0.298274999870000))*(sj4)))+(((IkReal(-0.0665000000000000))*(cj1)*(r02)))+(((cj0)*(x1107)*(x1111)))+(((IkReal(0.700000000000000))*(px)*(x1097)))+(((r01)*(x1109)))+(((r00)*(x1104)))+(((sj0)*(x1099)*(x1107)))+(((IkReal(-0.0665000000000000))*(sj1)*(x1101))));
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  )
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x1116=((IkReal(1.81818181870518))*(sj1));
IkReal x1117=((py)*(sj0));
IkReal x1118=((IkReal(0.216392517249668))*(sj1));
IkReal x1119=((cj0)*(px));
IkReal x1120=((IkReal(0.216392517249668))*(cj1));
IkReal x1121=((IkReal(1.81818181870518))*(cj1));
if( IKabs(((IkReal(-0.0757373810373838))+(((x1119)*(x1121)))+(((x1118)*(x1119)))+(((pz)*(x1120)))+(((x1117)*(x1118)))+(((IkReal(-1.00000000000000))*(pz)*(x1116)))+(((x1117)*(x1121))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((IkReal(-0.636363636546814))+(((pz)*(x1118)))+(((IkReal(-1.00000000000000))*(x1119)*(x1120)))+(((IkReal(-1.00000000000000))*(x1117)*(x1120)))+(((x1116)*(x1117)))+(((pz)*(x1121)))+(((x1116)*(x1119))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(IKsqr(((IkReal(-0.0757373810373838))+(((x1119)*(x1121)))+(((x1118)*(x1119)))+(((pz)*(x1120)))+(((x1117)*(x1118)))+(((IkReal(-1.00000000000000))*(pz)*(x1116)))+(((x1117)*(x1121)))))+IKsqr(((IkReal(-0.636363636546814))+(((pz)*(x1118)))+(((IkReal(-1.00000000000000))*(x1119)*(x1120)))+(((IkReal(-1.00000000000000))*(x1117)*(x1120)))+(((x1116)*(x1117)))+(((pz)*(x1121)))+(((x1116)*(x1119)))))-1) <= IKFAST_SINCOS_THRESH )
    continue;
j2array[0]=IKatan2(((IkReal(-0.0757373810373838))+(((x1119)*(x1121)))+(((x1118)*(x1119)))+(((pz)*(x1120)))+(((x1117)*(x1118)))+(((IkReal(-1.00000000000000))*(pz)*(x1116)))+(((x1117)*(x1121)))), ((IkReal(-0.636363636546814))+(((pz)*(x1118)))+(((IkReal(-1.00000000000000))*(x1119)*(x1120)))+(((IkReal(-1.00000000000000))*(x1117)*(x1120)))+(((x1116)*(x1117)))+(((pz)*(x1121)))+(((x1116)*(x1119)))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[8];
IkReal x1122=IKsin(j2);
IkReal x1123=IKcos(j2);
IkReal x1124=(py)*(py);
IkReal x1125=(px)*(px);
IkReal x1126=(pz)*(pz);
IkReal x1127=((sj0)*(sj1));
IkReal x1128=((py)*(r00));
IkReal x1129=((IkReal(0.190000000000000))*(cj1));
IkReal x1130=((cj0)*(r02));
IkReal x1131=((py)*(r01));
IkReal x1132=((px)*(r01));
IkReal x1133=((IkReal(0.190000000000000))*(sj1));
IkReal x1134=((cj1)*(r02));
IkReal x1135=((pz)*(r00));
IkReal x1136=((cj1)*(sj0));
IkReal x1137=((cj0)*(r00));
IkReal x1138=((IkReal(2.00000000000000))*(sj1));
IkReal x1139=((IkReal(0.700000000000000))*(px));
IkReal x1140=((IkReal(1.00000000000000))*(cj1));
IkReal x1141=((IkReal(2.00000000000000))*(px));
IkReal x1142=((pp)*(r01));
IkReal x1143=((pp)*(sj1));
IkReal x1144=((r02)*(sj1));
IkReal x1145=((pz)*(r01));
IkReal x1146=((px)*(r02));
IkReal x1147=((IkReal(0.700000000000000))*(sj0));
IkReal x1148=((cj0)*(sj1));
IkReal x1149=((r00)*(sj0));
IkReal x1150=((cj0)*(r01));
IkReal x1151=((IkReal(1.00000000000000))*(py));
IkReal x1152=((pz)*(r02));
IkReal x1153=((IkReal(2.00000000000000))*(py));
IkReal x1154=((cj1)*(pz));
IkReal x1155=((IkReal(0.0950000000000000))*(r01));
IkReal x1156=((cj0)*(px));
IkReal x1157=((pz)*(x1138));
IkReal x1158=((cj4)*(x1122));
IkReal x1159=((cj4)*(x1123));
IkReal x1160=((sj4)*(x1122));
IkReal x1161=((IkReal(0.0645444780500000))*(x1123));
IkReal x1162=((IkReal(0.542318181700000))*(x1123));
IkReal x1163=((IkReal(2.00000000000000))*(r01)*(x1124));
evalcond[0]=((x1134)+(((IkReal(-0.992991970000000))*(x1159)))+(((r01)*(x1127)))+(((IkReal(-0.118181820000000))*(x1158)))+(((sj1)*(x1137))));
evalcond[1]=((IkReal(-0.350000000000000))+(((px)*(x1148)))+(((IkReal(-0.0645444780500000))*(x1122)))+(((IkReal(-1.00000000000000))*(x1162)))+(((py)*(x1127)))+(x1154));
evalcond[2]=((x1144)+(((IkReal(0.992991970000000))*(x1158)))+(((IkReal(-0.118181820000000))*(x1159)))+(((IkReal(-1.00000000000000))*(r01)*(x1136)))+(((IkReal(-1.00000000000000))*(x1137)*(x1140))));
evalcond[3]=((((IkReal(-1.00000000000000))*(x1161)))+(((IkReal(0.542318181700000))*(x1122)))+(((pz)*(sj1)))+(((IkReal(-1.00000000000000))*(x1136)*(x1151)))+(((IkReal(-1.00000000000000))*(x1140)*(x1156))));
evalcond[4]=((((x1145)*(x1148)))+(((IkReal(-1.00000000000000))*(x1127)*(x1135)))+(((IkReal(0.0950000000000000))*(cj1)*(x1137)))+(((IkReal(-0.0950000000000000))*(x1144)))+(((x1136)*(x1155)))+(((IkReal(-1.00000000000000))*(sj4)*(x1161)))+(((IkReal(-1.00000000000000))*(x1132)*(x1140)))+(((x1127)*(x1146)))+(((cj1)*(x1128)))+(((IkReal(-1.00000000000000))*(sj1)*(x1130)*(x1151)))+(((IkReal(0.542318181700000))*(x1160))));
evalcond[5]=((((x1135)*(x1136)))+(((sj4)*(x1162)))+(((IkReal(-1.00000000000000))*(px)*(sj0)*(x1134)))+(((IkReal(-0.350000000000000))*(x1149)))+(((IkReal(0.350000000000000))*(x1150)))+(((x1127)*(x1155)))+(((IkReal(-1.00000000000000))*(sj1)*(x1132)))+(((IkReal(-1.00000000000000))*(cj0)*(x1140)*(x1145)))+(((IkReal(0.0950000000000000))*(sj1)*(x1137)))+(((sj1)*(x1128)))+(((cj1)*(py)*(x1130)))+(((IkReal(0.0950000000000000))*(x1134)))+(((IkReal(0.0645444780500000))*(x1160))));
evalcond[6]=((((IkReal(-0.113475000000000))*(x1134)))+(((IkReal(-0.113475000000000))*(sj1)*(x1137)))+(((IkReal(-2.00000000000000))*(x1131)*(x1154)))+(((IkReal(0.0665000000000000))*(x1150)))+(((IkReal(-1.00000000000000))*(cj0)*(x1129)*(x1145)))+(((IkReal(-1.00000000000000))*(x1127)*(x1163)))+(((IkReal(-1.00000000000000))*(px)*(x1130)*(x1157)))+(((IkReal(-1.00000000000000))*(x1127)*(x1152)*(x1153)))+(((IkReal(-1.00000000000000))*(sj0)*(x1129)*(x1146)))+(((sj0)*(x1129)*(x1135)))+(((IkReal(-1.00000000000000))*(cj1)*(x1135)*(x1141)))+(((IkReal(-1.00000000000000))*(x1131)*(x1138)*(x1156)))+(((IkReal(-0.0665000000000000))*(x1149)))+(((x1127)*(x1142)))+(((IkReal(0.700000000000000))*(x1152)))+(((pp)*(x1134)))+(((x1137)*(x1143)))+(((r00)*(x1139)))+(((IkReal(-0.113475000000000))*(r01)*(x1127)))+(((x1128)*(x1133)))+(((py)*(x1129)*(x1130)))+(((IkReal(0.296184679851750))*(x1159)))+(((IkReal(-1.00000000000000))*(x1125)*(x1137)*(x1138)))+(((IkReal(-2.00000000000000))*(x1126)*(x1134)))+(((IkReal(0.0352506812605000))*(x1158)))+(((IkReal(-1.00000000000000))*(x1132)*(x1133)))+(((IkReal(0.700000000000000))*(x1131)))+(((IkReal(-1.00000000000000))*(x1127)*(x1128)*(x1141))));
evalcond[7]=((((IkReal(-0.190000000000000))*(x1127)*(x1146)))+(((IkReal(0.190000000000000))*(x1127)*(x1135)))+(((py)*(x1130)*(x1133)))+(((IkReal(2.00000000000000))*(cj1)*(x1125)*(x1137)))+(((IkReal(-1.00000000000000))*(x1130)*(x1139)))+(((IkReal(-0.296184679851750))*(x1158)))+(((IkReal(-1.00000000000000))*(px)*(x1135)*(x1138)))+(((x1129)*(x1132)))+(((IkReal(-0.131525000000000))*(cj1)*(x1137)))+(((x1145)*(x1147)))+(((cj0)*(cj1)*(x1131)*(x1141)))+(((IkReal(-0.131525000000000))*(r01)*(x1136)))+(((x1136)*(x1163)))+(((x1128)*(x1136)*(x1141)))+(((IkReal(0.131525000000000))*(x1144)))+(((r02)*(x1143)))+(((IkReal(-1.00000000000000))*(x1131)*(x1157)))+(((IkReal(-1.00000000000000))*(cj0)*(x1133)*(x1145)))+(((x1130)*(x1141)*(x1154)))+(((IkReal(0.700000000000000))*(cj0)*(x1135)))+(((IkReal(-1.00000000000000))*(py)*(r02)*(x1147)))+(((IkReal(-1.00000000000000))*(pp)*(x1137)*(x1140)))+(((IkReal(0.0352506812605000))*(x1159)))+(((pz)*(sj0)*(x1134)*(x1153)))+(((IkReal(-1.00000000000000))*(x1136)*(x1142)))+(((IkReal(-1.00000000000000))*(x1128)*(x1129)))+(((IkReal(-1.00000000000000))*(r02)*(x1126)*(x1138))));
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  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

} else
{
if( 1 )
{
continue;

} else
{
}
}
}
}
}
}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x1164=((cj0)*(sj1));
IkReal x1165=((IkReal(13.0000000000000))*(r00));
IkReal x1166=((cj1)*(pz));
IkReal x1167=((sj0)*(sj1));
IkReal x1168=((IkReal(13.0000000000000))*(sj3));
IkReal x1169=((cj1)*(r02));
IkReal x1170=((IkReal(70.0000000000000))*(cj4));
IkReal x1171=((IkReal(13.0000000000000))*(cj3));
IkReal x1172=((IkReal(200.000000000000))*(cj4)*(sj3));
IkReal x1173=((IkReal(200.000000000000))*(cj3)*(cj4));
if( IKabs(((gconst2)*(((((cj3)*(x1164)*(x1165)))+(((py)*(x1167)*(x1172)))+(((IkReal(110.000000000000))*(r01)*(x1167)))+(((px)*(x1164)*(x1172)))+(((IkReal(-1.00000000000000))*(sj3)*(x1170)))+(((IkReal(110.000000000000))*(x1169)))+(((x1169)*(x1171)))+(((r01)*(x1167)*(x1171)))+(((x1166)*(x1172)))+(((IkReal(110.000000000000))*(r00)*(x1164))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst2)*(((((cj3)*(x1170)))+(((r01)*(x1167)*(x1168)))+(((x1168)*(x1169)))+(((IkReal(-1.00000000000000))*(px)*(x1164)*(x1173)))+(((IkReal(-1.00000000000000))*(x1166)*(x1173)))+(((IkReal(-1.00000000000000))*(py)*(x1167)*(x1173)))+(((sj3)*(x1164)*(x1165))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst2)*(((((cj3)*(x1164)*(x1165)))+(((py)*(x1167)*(x1172)))+(((IkReal(110.000000000000))*(r01)*(x1167)))+(((px)*(x1164)*(x1172)))+(((IkReal(-1.00000000000000))*(sj3)*(x1170)))+(((IkReal(110.000000000000))*(x1169)))+(((x1169)*(x1171)))+(((r01)*(x1167)*(x1171)))+(((x1166)*(x1172)))+(((IkReal(110.000000000000))*(r00)*(x1164)))))), ((gconst2)*(((((cj3)*(x1170)))+(((r01)*(x1167)*(x1168)))+(((x1168)*(x1169)))+(((IkReal(-1.00000000000000))*(px)*(x1164)*(x1173)))+(((IkReal(-1.00000000000000))*(x1166)*(x1173)))+(((IkReal(-1.00000000000000))*(py)*(x1167)*(x1173)))+(((sj3)*(x1164)*(x1165)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[8];
IkReal x1174=IKsin(j2);
IkReal x1175=IKcos(j2);
IkReal x1176=(py)*(py);
IkReal x1177=(px)*(px);
IkReal x1178=(pz)*(pz);
IkReal x1179=((sj0)*(sj1));
IkReal x1180=((py)*(r00));
IkReal x1181=((IkReal(0.190000000000000))*(cj1));
IkReal x1182=((cj0)*(r02));
IkReal x1183=((py)*(r01));
IkReal x1184=((r01)*(sj1));
IkReal x1185=((IkReal(0.190000000000000))*(px));
IkReal x1186=((cj1)*(r02));
IkReal x1187=((pz)*(r00));
IkReal x1188=((cj1)*(sj0));
IkReal x1189=((cj0)*(r00));
IkReal x1190=((IkReal(2.00000000000000))*(sj1));
IkReal x1191=((cj0)*(pz));
IkReal x1192=((IkReal(1.00000000000000))*(r01));
IkReal x1193=((cj1)*(px));
IkReal x1194=((IkReal(2.00000000000000))*(px));
IkReal x1195=((pp)*(sj1));
IkReal x1196=((r02)*(sj1));
IkReal x1197=((px)*(r02));
IkReal x1198=((IkReal(0.700000000000000))*(pz));
IkReal x1199=((cj0)*(px));
IkReal x1200=((r00)*(sj0));
IkReal x1201=((IkReal(1.00000000000000))*(px));
IkReal x1202=((cj0)*(r01));
IkReal x1203=((IkReal(1.00000000000000))*(cj1));
IkReal x1204=((IkReal(1.00000000000000))*(py));
IkReal x1205=((IkReal(0.700000000000000))*(px));
IkReal x1206=((IkReal(0.190000000000000))*(sj1));
IkReal x1207=((IkReal(0.298275000000000))*(sj3));
IkReal x1208=((cj1)*(pz));
IkReal x1209=((IkReal(0.0950000000000000))*(r01));
IkReal x1210=((IkReal(0.306725000000000))*(cj3)*(cj4));
IkReal x1211=((pz)*(x1190));
IkReal x1212=((IkReal(0.550000000000000))*(x1175));
IkReal x1213=((cj4)*(x1175));
IkReal x1214=((IkReal(0.550000000000000))*(x1174));
IkReal x1215=((IkReal(2.00000000000000))*(py)*(pz));
IkReal x1216=((IkReal(0.0650000000000000))*(cj3)*(sj4));
IkReal x1217=((IkReal(0.0650000000000000))*(x1174));
IkReal x1218=((cj4)*(x1174));
IkReal x1219=((IkReal(2.00000000000000))*(r01)*(x1176));
IkReal x1220=((IkReal(0.0650000000000000))*(sj3)*(x1175));
evalcond[0]=((((sj3)*(x1213)))+(((r01)*(x1179)))+(x1186)+(((cj3)*(x1218)))+(((sj1)*(x1189))));
evalcond[1]=((IkReal(-0.350000000000000))+(((py)*(x1179)))+(((IkReal(-0.0650000000000000))*(cj3)*(x1175)))+(((sj1)*(x1199)))+(((sj3)*(x1217)))+(((IkReal(-1.00000000000000))*(x1212)))+(x1208));
evalcond[2]=((x1196)+(((IkReal(-1.00000000000000))*(x1189)*(x1203)))+(((IkReal(-1.00000000000000))*(sj3)*(x1218)))+(((IkReal(-1.00000000000000))*(x1188)*(x1192)))+(((cj3)*(x1213))));
evalcond[3]=((((pz)*(sj1)))+(x1220)+(((IkReal(-1.00000000000000))*(x1188)*(x1204)))+(((cj3)*(x1217)))+(x1214)+(((IkReal(-1.00000000000000))*(cj0)*(x1193))));
evalcond[4]=((((IkReal(-0.0950000000000000))*(x1196)))+(((sj4)*(x1214)))+(((cj1)*(x1180)))+(((x1174)*(x1216)))+(((x1188)*(x1209)))+(((x1184)*(x1191)))+(((sj4)*(x1220)))+(((IkReal(-1.00000000000000))*(x1179)*(x1187)))+(((IkReal(-1.00000000000000))*(x1192)*(x1193)))+(((x1179)*(x1197)))+(((IkReal(0.0950000000000000))*(cj1)*(x1189)))+(((IkReal(-1.00000000000000))*(sj1)*(x1182)*(x1204))));
evalcond[5]=((((IkReal(0.0950000000000000))*(x1186)))+(((IkReal(-1.00000000000000))*(x1184)*(x1201)))+(((IkReal(0.350000000000000))*(x1202)))+(((sj1)*(x1180)))+(((x1179)*(x1209)))+(((IkReal(-0.350000000000000))*(x1200)))+(((sj4)*(x1212)))+(((cj1)*(py)*(x1182)))+(((IkReal(-1.00000000000000))*(cj1)*(x1191)*(x1192)))+(((x1175)*(x1216)))+(((x1187)*(x1188)))+(((IkReal(0.0950000000000000))*(sj1)*(x1189)))+(((IkReal(-1.00000000000000))*(sj0)*(x1186)*(x1201)))+(((IkReal(-1.00000000000000))*(sj3)*(sj4)*(x1217))));
evalcond[6]=((((IkReal(-0.113475000000000))*(sj1)*(x1189)))+(((IkReal(-1.00000000000000))*(x1179)*(x1180)*(x1194)))+(((IkReal(-2.00000000000000))*(x1183)*(x1208)))+(((pp)*(r01)*(x1179)))+(((r00)*(x1205)))+(((IkReal(-2.00000000000000))*(x1187)*(x1193)))+(((sj0)*(x1181)*(x1187)))+(((pp)*(x1186)))+(((IkReal(-1.00000000000000))*(x1184)*(x1185)))+(((x1174)*(x1210)))+(((IkReal(-0.113475000000000))*(x1186)))+(((IkReal(-1.00000000000000))*(x1177)*(x1189)*(x1190)))+(((IkReal(-1.00000000000000))*(x1179)*(x1219)))+(((IkReal(-1.00000000000000))*(x1207)*(x1213)))+(((IkReal(-1.00000000000000))*(r01)*(x1181)*(x1191)))+(((x1189)*(x1195)))+(((IkReal(-0.0665000000000000))*(x1200)))+(((r02)*(x1198)))+(((IkReal(-1.00000000000000))*(sj0)*(x1181)*(x1197)))+(((IkReal(0.700000000000000))*(x1183)))+(((IkReal(-1.00000000000000))*(x1183)*(x1190)*(x1199)))+(((IkReal(-1.00000000000000))*(px)*(x1182)*(x1211)))+(((IkReal(0.0715000000000000))*(x1218)))+(((IkReal(0.0665000000000000))*(x1202)))+(((x1180)*(x1206)))+(((py)*(x1181)*(x1182)))+(((IkReal(-2.00000000000000))*(x1178)*(x1186)))+(((IkReal(-0.113475000000000))*(r01)*(x1179)))+(((IkReal(-1.00000000000000))*(r02)*(x1179)*(x1215))));
evalcond[7]=((((IkReal(-1.00000000000000))*(r02)*(x1178)*(x1190)))+(((IkReal(-0.131525000000000))*(cj1)*(x1189)))+(((IkReal(0.0715000000000000))*(x1213)))+(((IkReal(-0.131525000000000))*(r01)*(x1188)))+(((IkReal(0.131525000000000))*(x1196)))+(((x1188)*(x1219)))+(((IkReal(-1.00000000000000))*(px)*(x1187)*(x1190)))+(((IkReal(-1.00000000000000))*(pp)*(x1188)*(x1192)))+(((IkReal(-1.00000000000000))*(r02)*(x1179)*(x1185)))+(((IkReal(-1.00000000000000))*(x1180)*(x1181)))+(((py)*(x1182)*(x1206)))+(((r02)*(x1195)))+(((IkReal(-1.00000000000000))*(pp)*(x1189)*(x1203)))+(((sj0)*(x1186)*(x1215)))+(((x1207)*(x1218)))+(((IkReal(-1.00000000000000))*(x1183)*(x1211)))+(((IkReal(-1.00000000000000))*(x1182)*(x1205)))+(((IkReal(2.00000000000000))*(cj1)*(x1177)*(x1189)))+(((IkReal(-0.700000000000000))*(py)*(r02)*(sj0)))+(((x1180)*(x1188)*(x1194)))+(((IkReal(2.00000000000000))*(pz)*(x1182)*(x1193)))+(((IkReal(0.700000000000000))*(cj0)*(x1187)))+(((IkReal(0.190000000000000))*(x1179)*(x1187)))+(((px)*(r01)*(x1181)))+(((IkReal(-0.190000000000000))*(x1184)*(x1191)))+(((x1175)*(x1210)))+(((r01)*(sj0)*(x1198)))+(((IkReal(2.00000000000000))*(cj0)*(x1183)*(x1193))));
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  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}

} else
{
{
IkReal j2array[1], cj2array[1], sj2array[1];
bool j2valid[1]={false};
_nj2 = 1;
IkReal x1221=((IkReal(1.00000000000000))*(sj1));
IkReal x1222=((IkReal(1.00000000000000))*(sj3));
IkReal x1223=((cj0)*(r00));
IkReal x1224=((r01)*(sj0));
IkReal x1225=((cj3)*(r02));
IkReal x1226=((cj3)*(x1224));
IkReal x1227=((cj1)*(x1223));
if( IKabs(((gconst1)*(((((r02)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x1225)))+(((IkReal(-1.00000000000000))*(x1222)*(x1227)))+(((IkReal(-1.00000000000000))*(cj1)*(x1222)*(x1224)))+(((IkReal(-1.00000000000000))*(x1221)*(x1226)))+(((IkReal(-1.00000000000000))*(cj3)*(x1221)*(x1223))))))) < IKFAST_ATAN2_MAGTHRESH && IKabs(((gconst1)*(((((cj3)*(x1227)))+(((IkReal(-1.00000000000000))*(cj1)*(r02)*(x1222)))+(((IkReal(-1.00000000000000))*(x1221)*(x1225)))+(((cj1)*(x1226)))+(((IkReal(-1.00000000000000))*(sj3)*(x1221)*(x1223)))+(((IkReal(-1.00000000000000))*(sj3)*(x1221)*(x1224))))))) < IKFAST_ATAN2_MAGTHRESH )
    continue;
j2array[0]=IKatan2(((gconst1)*(((((r02)*(sj1)*(sj3)))+(((IkReal(-1.00000000000000))*(cj1)*(x1225)))+(((IkReal(-1.00000000000000))*(x1222)*(x1227)))+(((IkReal(-1.00000000000000))*(cj1)*(x1222)*(x1224)))+(((IkReal(-1.00000000000000))*(x1221)*(x1226)))+(((IkReal(-1.00000000000000))*(cj3)*(x1221)*(x1223)))))), ((gconst1)*(((((cj3)*(x1227)))+(((IkReal(-1.00000000000000))*(cj1)*(r02)*(x1222)))+(((IkReal(-1.00000000000000))*(x1221)*(x1225)))+(((cj1)*(x1226)))+(((IkReal(-1.00000000000000))*(sj3)*(x1221)*(x1223)))+(((IkReal(-1.00000000000000))*(sj3)*(x1221)*(x1224)))))));
sj2array[0]=IKsin(j2array[0]);
cj2array[0]=IKcos(j2array[0]);
if( j2array[0] > IKPI )
{
    j2array[0]-=IK2PI;
}
else if( j2array[0] < -IKPI )
{    j2array[0]+=IK2PI;
}
j2valid[0] = true;
for(int ij2 = 0; ij2 < 1; ++ij2)
{
if( !j2valid[ij2] )
{
    continue;
}
_ij2[0] = ij2; _ij2[1] = -1;
for(int iij2 = ij2+1; iij2 < 1; ++iij2)
{
if( j2valid[iij2] && IKabs(cj2array[ij2]-cj2array[iij2]) < IKFAST_SOLUTION_THRESH && IKabs(sj2array[ij2]-sj2array[iij2]) < IKFAST_SOLUTION_THRESH )
{
    j2valid[iij2]=false; _ij2[1] = iij2; break; 
}
}
j2 = j2array[ij2]; cj2 = cj2array[ij2]; sj2 = sj2array[ij2];
{
IkReal evalcond[8];
IkReal x1228=IKsin(j2);
IkReal x1229=IKcos(j2);
IkReal x1230=(py)*(py);
IkReal x1231=(px)*(px);
IkReal x1232=(pz)*(pz);
IkReal x1233=((sj0)*(sj1));
IkReal x1234=((py)*(r00));
IkReal x1235=((IkReal(0.190000000000000))*(cj1));
IkReal x1236=((cj0)*(r02));
IkReal x1237=((py)*(r01));
IkReal x1238=((r01)*(sj1));
IkReal x1239=((IkReal(0.190000000000000))*(px));
IkReal x1240=((cj1)*(r02));
IkReal x1241=((pz)*(r00));
IkReal x1242=((cj1)*(sj0));
IkReal x1243=((cj0)*(r00));
IkReal x1244=((IkReal(2.00000000000000))*(sj1));
IkReal x1245=((cj0)*(pz));
IkReal x1246=((IkReal(1.00000000000000))*(r01));
IkReal x1247=((cj1)*(px));
IkReal x1248=((IkReal(2.00000000000000))*(px));
IkReal x1249=((pp)*(sj1));
IkReal x1250=((r02)*(sj1));
IkReal x1251=((px)*(r02));
IkReal x1252=((IkReal(0.700000000000000))*(pz));
IkReal x1253=((cj0)*(px));
IkReal x1254=((r00)*(sj0));
IkReal x1255=((IkReal(1.00000000000000))*(px));
IkReal x1256=((cj0)*(r01));
IkReal x1257=((IkReal(1.00000000000000))*(cj1));
IkReal x1258=((IkReal(1.00000000000000))*(py));
IkReal x1259=((IkReal(0.700000000000000))*(px));
IkReal x1260=((IkReal(0.190000000000000))*(sj1));
IkReal x1261=((IkReal(0.298275000000000))*(sj3));
IkReal x1262=((cj1)*(pz));
IkReal x1263=((IkReal(0.0950000000000000))*(r01));
IkReal x1264=((IkReal(0.306725000000000))*(cj3)*(cj4));
IkReal x1265=((pz)*(x1244));
IkReal x1266=((IkReal(0.550000000000000))*(x1229));
IkReal x1267=((cj4)*(x1229));
IkReal x1268=((IkReal(0.550000000000000))*(x1228));
IkReal x1269=((IkReal(2.00000000000000))*(py)*(pz));
IkReal x1270=((IkReal(0.0650000000000000))*(cj3)*(sj4));
IkReal x1271=((IkReal(0.0650000000000000))*(x1228));
IkReal x1272=((cj4)*(x1228));
IkReal x1273=((IkReal(2.00000000000000))*(r01)*(x1230));
IkReal x1274=((IkReal(0.0650000000000000))*(sj3)*(x1229));
evalcond[0]=((x1240)+(((r01)*(x1233)))+(((cj3)*(x1272)))+(((sj1)*(x1243)))+(((sj3)*(x1267))));
evalcond[1]=((IkReal(-0.350000000000000))+(((py)*(x1233)))+(((IkReal(-1.00000000000000))*(x1266)))+(((sj3)*(x1271)))+(((IkReal(-0.0650000000000000))*(cj3)*(x1229)))+(((sj1)*(x1253)))+(x1262));
evalcond[2]=((((IkReal(-1.00000000000000))*(x1243)*(x1257)))+(((IkReal(-1.00000000000000))*(sj3)*(x1272)))+(((cj3)*(x1267)))+(x1250)+(((IkReal(-1.00000000000000))*(x1242)*(x1246))));
evalcond[3]=((((cj3)*(x1271)))+(((pz)*(sj1)))+(((IkReal(-1.00000000000000))*(cj0)*(x1247)))+(x1274)+(((IkReal(-1.00000000000000))*(x1242)*(x1258)))+(x1268));
evalcond[4]=((((cj1)*(x1234)))+(((x1228)*(x1270)))+(((x1242)*(x1263)))+(((IkReal(-1.00000000000000))*(x1246)*(x1247)))+(((IkReal(-1.00000000000000))*(sj1)*(x1236)*(x1258)))+(((x1233)*(x1251)))+(((IkReal(-0.0950000000000000))*(x1250)))+(((IkReal(0.0950000000000000))*(cj1)*(x1243)))+(((x1238)*(x1245)))+(((IkReal(-1.00000000000000))*(x1233)*(x1241)))+(((sj4)*(x1268)))+(((sj4)*(x1274))));
evalcond[5]=((((IkReal(-1.00000000000000))*(cj1)*(x1245)*(x1246)))+(((x1229)*(x1270)))+(((IkReal(0.350000000000000))*(x1256)))+(((IkReal(0.0950000000000000))*(sj1)*(x1243)))+(((IkReal(-1.00000000000000))*(sj0)*(x1240)*(x1255)))+(((sj1)*(x1234)))+(((sj4)*(x1266)))+(((x1241)*(x1242)))+(((IkReal(0.0950000000000000))*(x1240)))+(((IkReal(-1.00000000000000))*(sj3)*(sj4)*(x1271)))+(((cj1)*(py)*(x1236)))+(((IkReal(-1.00000000000000))*(x1238)*(x1255)))+(((IkReal(-0.350000000000000))*(x1254)))+(((x1233)*(x1263))));
evalcond[6]=((((IkReal(-1.00000000000000))*(px)*(x1236)*(x1265)))+(((IkReal(-2.00000000000000))*(x1241)*(x1247)))+(((x1228)*(x1264)))+(((x1234)*(x1260)))+(((IkReal(-1.00000000000000))*(r01)*(x1235)*(x1245)))+(((IkReal(-1.00000000000000))*(x1231)*(x1243)*(x1244)))+(((r02)*(x1252)))+(((IkReal(-0.113475000000000))*(x1240)))+(((IkReal(-1.00000000000000))*(x1233)*(x1273)))+(((IkReal(-2.00000000000000))*(x1237)*(x1262)))+(((r00)*(x1259)))+(((IkReal(-0.113475000000000))*(r01)*(x1233)))+(((IkReal(0.0665000000000000))*(x1256)))+(((IkReal(0.0715000000000000))*(x1272)))+(((IkReal(-1.00000000000000))*(x1233)*(x1234)*(x1248)))+(((IkReal(-1.00000000000000))*(r02)*(x1233)*(x1269)))+(((IkReal(-1.00000000000000))*(x1238)*(x1239)))+(((pp)*(x1240)))+(((IkReal(0.700000000000000))*(x1237)))+(((IkReal(-1.00000000000000))*(x1261)*(x1267)))+(((sj0)*(x1235)*(x1241)))+(((pp)*(r01)*(x1233)))+(((IkReal(-0.113475000000000))*(sj1)*(x1243)))+(((IkReal(-2.00000000000000))*(x1232)*(x1240)))+(((IkReal(-1.00000000000000))*(x1237)*(x1244)*(x1253)))+(((IkReal(-0.0665000000000000))*(x1254)))+(((x1243)*(x1249)))+(((py)*(x1235)*(x1236)))+(((IkReal(-1.00000000000000))*(sj0)*(x1235)*(x1251))));
evalcond[7]=((((IkReal(0.190000000000000))*(x1233)*(x1241)))+(((r01)*(sj0)*(x1252)))+(((x1261)*(x1272)))+(((IkReal(0.131525000000000))*(x1250)))+(((r02)*(x1249)))+(((x1242)*(x1273)))+(((IkReal(-1.00000000000000))*(x1236)*(x1259)))+(((sj0)*(x1240)*(x1269)))+(((IkReal(2.00000000000000))*(cj0)*(x1237)*(x1247)))+(((IkReal(2.00000000000000))*(pz)*(x1236)*(x1247)))+(((IkReal(-1.00000000000000))*(r02)*(x1232)*(x1244)))+(((IkReal(0.700000000000000))*(cj0)*(x1241)))+(((IkReal(-1.00000000000000))*(pp)*(x1243)*(x1257)))+(((IkReal(-1.00000000000000))*(x1237)*(x1265)))+(((IkReal(-1.00000000000000))*(r02)*(x1233)*(x1239)))+(((IkReal(-0.190000000000000))*(x1238)*(x1245)))+(((IkReal(-0.131525000000000))*(r01)*(x1242)))+(((IkReal(0.0715000000000000))*(x1267)))+(((IkReal(-1.00000000000000))*(pp)*(x1242)*(x1246)))+(((IkReal(-0.131525000000000))*(cj1)*(x1243)))+(((IkReal(-0.700000000000000))*(py)*(r02)*(sj0)))+(((x1234)*(x1242)*(x1248)))+(((px)*(r01)*(x1235)))+(((IkReal(-1.00000000000000))*(x1234)*(x1235)))+(((py)*(x1236)*(x1260)))+(((IkReal(2.00000000000000))*(cj1)*(x1231)*(x1243)))+(((x1229)*(x1264)))+(((IkReal(-1.00000000000000))*(px)*(x1241)*(x1244))));
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  )
{
continue;
}
}

{
std::vector<IkSingleDOFSolutionBase<IkReal> > vinfos(5);
vinfos[0].jointtype = 1;
vinfos[0].foffset = j0;
vinfos[0].indices[0] = _ij0[0];
vinfos[0].indices[1] = _ij0[1];
vinfos[0].maxsolutions = _nj0;
vinfos[1].jointtype = 1;
vinfos[1].foffset = j1;
vinfos[1].indices[0] = _ij1[0];
vinfos[1].indices[1] = _ij1[1];
vinfos[1].maxsolutions = _nj1;
vinfos[2].jointtype = 1;
vinfos[2].foffset = j2;
vinfos[2].indices[0] = _ij2[0];
vinfos[2].indices[1] = _ij2[1];
vinfos[2].maxsolutions = _nj2;
vinfos[3].jointtype = 1;
vinfos[3].foffset = j3;
vinfos[3].indices[0] = _ij3[0];
vinfos[3].indices[1] = _ij3[1];
vinfos[3].maxsolutions = _nj3;
vinfos[4].jointtype = 1;
vinfos[4].foffset = j4;
vinfos[4].indices[0] = _ij4[0];
vinfos[4].indices[1] = _ij4[1];
vinfos[4].maxsolutions = _nj4;
std::vector<int> vfree(0);
solutions.AddSolution(vinfos,vfree);
}
}
}

}

}
}
}

}

}
}
}
    }
}
}

}

}
}
return solutions.GetNumSolutions()>0;
}
static inline void polyroots4(IkReal rawcoeffs[4+1], IkReal rawroots[4], int& numroots)
{
    using std::complex;
    IKFAST_ASSERT(rawcoeffs[0] != 0);
    const IkReal tol = 128.0*std::numeric_limits<IkReal>::epsilon();
    const IkReal tolsqrt = sqrt(std::numeric_limits<IkReal>::epsilon());
    complex<IkReal> coeffs[4];
    const int maxsteps = 110;
    for(int i = 0; i < 4; ++i) {
        coeffs[i] = complex<IkReal>(rawcoeffs[i+1]/rawcoeffs[0]);
    }
    complex<IkReal> roots[4];
    IkReal err[4];
    roots[0] = complex<IkReal>(1,0);
    roots[1] = complex<IkReal>(0.4,0.9); // any complex number not a root of unity works
    err[0] = 1.0;
    err[1] = 1.0;
    for(int i = 2; i < 4; ++i) {
        roots[i] = roots[i-1]*roots[1];
        err[i] = 1.0;
    }
    for(int step = 0; step < maxsteps; ++step) {
        bool changed = false;
        for(int i = 0; i < 4; ++i) {
            if ( err[i] >= tol ) {
                changed = true;
                // evaluate
                complex<IkReal> x = roots[i] + coeffs[0];
                for(int j = 1; j < 4; ++j) {
                    x = roots[i] * x + coeffs[j];
                }
                for(int j = 0; j < 4; ++j) {
                    if( i != j ) {
                        if( roots[i] != roots[j] ) {
                            x /= (roots[i] - roots[j]);
                        }
                    }
                }
                roots[i] -= x;
                err[i] = abs(x);
            }
        }
        if( !changed ) {
            break;
        }
    }

    numroots = 0;
    bool visited[4] = {false};
    for(int i = 0; i < 4; ++i) {
        if( !visited[i] ) {
            // might be a multiple root, in which case it will have more error than the other roots
            // find any neighboring roots, and take the average
            complex<IkReal> newroot=roots[i];
            int n = 1;
            for(int j = i+1; j < 4; ++j) {
                if( abs(roots[i]-roots[j]) < 8*tolsqrt ) {
                    newroot += roots[j];
                    n += 1;
                    visited[j] = true;
                }
            }
            if( n > 1 ) {
                newroot /= n;
            }
            // there are still cases where even the mean is not accurate enough, until a better multi-root algorithm is used, need to use the sqrt
            if( IKabs(imag(newroot)) < tolsqrt ) {
                rawroots[numroots++] = real(newroot);
            }
        }
    }
}
};


IKFAST_API bool ComputeIk(const IkReal* eetrans, const IkReal* eerot, const IkReal* pfree, IkSolutionListBase<IkReal>& solutions) {
IKSolver solver;
return solver.ComputeIk(eetrans,eerot,pfree,solutions);
}

IKFAST_API const char* GetKinematicsHash() { return "d2942246f1616a588c32a0fec55a0c4b"; }

IKFAST_API const char* GetIkFastVersion() { return IKFAST_STRINGIZE(IKFAST_VERSION); }

#ifdef IKFAST_NAMESPACE
} // end namespace
#endif

#ifndef IKFAST_NO_MAIN
#include <stdio.h>
#include <stdlib.h>
#ifdef IKFAST_NAMESPACE
using namespace IKFAST_NAMESPACE;
#endif
int main(int argc, char** argv)
{
    if( argc != 12+GetNumFreeParameters()+1 ) {
        printf("\nUsage: ./ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n"
               "Returns the ik solutions given the transformation of the end effector specified by\n"
               "a 3x3 rotation R (rXX), and a 3x1 translation (tX).\n"
               "There are %d free parameters that have to be specified.\n\n",GetNumFreeParameters());
        return 1;
    }

    IkSolutionList<IkReal> solutions;
    std::vector<IkReal> vfree(GetNumFreeParameters());
    IkReal eerot[9],eetrans[3];
    eerot[0] = atof(argv[1]); eerot[1] = atof(argv[2]); eerot[2] = atof(argv[3]); eetrans[0] = atof(argv[4]);
    eerot[3] = atof(argv[5]); eerot[4] = atof(argv[6]); eerot[5] = atof(argv[7]); eetrans[1] = atof(argv[8]);
    eerot[6] = atof(argv[9]); eerot[7] = atof(argv[10]); eerot[8] = atof(argv[11]); eetrans[2] = atof(argv[12]);
    for(std::size_t i = 0; i < vfree.size(); ++i)
        vfree[i] = atof(argv[13+i]);
    bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions);

    if( !bSuccess ) {
        fprintf(stderr,"Failed to get ik solution\n");
        return -1;
    }

    printf("Found %d ik solutions:\n", (int)solutions.GetNumSolutions());
    std::vector<IkReal> solvalues(GetNumJoints());
    for(std::size_t i = 0; i < solutions.GetNumSolutions(); ++i) {
        const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
        printf("sol%d (free=%d): ", (int)i, (int)sol.GetFree().size());
        std::vector<IkReal> vsolfree(sol.GetFree().size());
        sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL);
        for( std::size_t j = 0; j < solvalues.size(); ++j)
            printf("%.15f, ", solvalues[j]);
        printf("\n");
    }
    return 0;
}

#endif